#include "StdAfx.h"
#include "GCPGeoCorrect.h"

#include "MapPrj.h"

#include <omp.h>

// 功能：空间分辨率估计（经纬度分辨率）
bool CalLonLatResolution(double &LonResolution, double &LatResolution, 
						 const std::vector<cv::Point2d>& GCP_XY, 
						 const std::vector<cv::Point2d>& GCP_LonLat)
{
	// 基本条件判断
	if (GCP_XY.size() < 4 || GCP_LonLat.size() < 4 || GCP_XY.size() != GCP_LonLat.size())
	{
		return false;
	}

	// 计算重心点坐标
	double Center_Lon = 0.0;
	double Center_Lat = 0.0;
	double Center_x = 0.0;
	double Center_y = 0.0;

	// 控制点数目
	int count = GCP_XY.size();

	for (int i = 0; i < count; i++ )
	{
		// 经纬度坐标
		Center_Lon += GCP_LonLat[i].x / count;
		Center_Lat += GCP_LonLat[i].y / count;

		// 像素坐标
		Center_x += GCP_XY[i].x / count;
		Center_y += GCP_XY[i].y / count;
	}


	// 计算各点到第一个点的距离
	double Resolution = 0;
	for (int i = 1; i < count; i++ )
	{
		double dis = 0;
		CalculateTwoPtsDistance(dis, 
			GCP_LonLat[0].x, GCP_LonLat[0].y, 
			GCP_LonLat[i].x, GCP_LonLat[i].y, 
			3);

		double L = cv::norm(GCP_XY[i] - GCP_XY[0]);

		Resolution += (dis / L);
	}

	Resolution /= (count - 1);

	//功能：计算目标点的经纬度
	double targetPtLon = Center_Lon;
	double targetPtLat = Center_Lat;

	// 分辨率
	CalculatePtCoordinate(targetPtLon, targetPtLat, Center_Lon, Center_Lat, 90, Resolution, 3);
	LonResolution = targetPtLon - Center_Lon;
	CalculatePtCoordinate(targetPtLon, targetPtLat, Center_Lon, Center_Lat, 0, Resolution, 3);
	LatResolution = targetPtLat - Center_Lat;

	return true;
}

// 计算线性变换系数 :  A B  prePoints ---> nextPoints
bool CalLineTransformCoefficient(cv::Mat &A, cv::Mat &B, 
	                             const std::vector<cv::Point2d>& prePoints, 
	                             const std::vector<cv::Point2d>& nextPoints)
{
	try
	{
		// 最少需要4对点
		int size = 0;
		if (prePoints.size() < 4 || nextPoints.size() < 4 || prePoints.size() != nextPoints.size())
		{
			return false;
		}
		else
		{
			size = prePoints.size();
		}

		// 重新分配输出结果矩阵
		A.create(3, 1, CV_64FC1);
		A = cv::Scalar(0);

		B.create(3, 1, CV_64FC1);
		B = cv::Scalar(0);

		// 构造输入
		cv::Mat W, X, Y;
		W.create(size, 3, CV_64FC1);
		W = cv::Scalar(0);

		X.create(size, 1, CV_64FC1);
		X = cv::Scalar(0);

		Y.create(size, 1, CV_64FC1);
		Y = cv::Scalar(0);

		// 构造W X Y赋值
		for (int i = 0; i < size; i++)
		{
			W.at<double>(i, 0) = 1;
			W.at<double>(i, 1) = prePoints[i].x;
			W.at<double>(i, 2) = prePoints[i].y;

			X.at<double>(i, 0) = nextPoints[i].x;
			Y.at<double>(i, 0) = nextPoints[i].y;
		}


		// 计算A B 
		// W * A = X 
		// W * B = Y
		bool ret1 = cv::solve(W, X, A, CV_SVD); // 求A
		bool ret2 = cv::solve(W, Y, B, CV_SVD); // 求B

		if (ret1 == false || ret2 == false)
		{
			return false;
		}

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}

// 计算二次多项式变换系数 :  A B  prePoints ---> nextPoints
bool CalPolynomialCoefficient(cv::Mat &A, cv::Mat &B, 
	const std::vector<cv::Point2d>& prePoints, 
	const std::vector<cv::Point2d>& nextPoints)
{
	try
	{
		// 最少需要6对点
		int size = 0;
		if (prePoints.size() < 6 || nextPoints.size() < 6 || prePoints.size() != nextPoints.size())
		{
			return false;
		}
		else
		{
			size = prePoints.size();
		}

		// 重新分配输出结果矩阵
		A.create(6, 1, CV_64FC1);
		A = cv::Scalar(0);

		B.create(6, 1, CV_64FC1);
		B = cv::Scalar(0);

		// 构造输入
		cv::Mat W, X, Y;
		W.create(size, 6, CV_64FC1);
		W = cv::Scalar(0);

		X.create(size, 1, CV_64FC1);
		X = cv::Scalar(0);

		Y.create(size, 1, CV_64FC1);
		Y = cv::Scalar(0);

		// 构造W X Y赋值
		for (int i = 0; i < size; i++)
		{
			W.at<double>(i, 0) = 1;
			W.at<double>(i, 1) = prePoints[i].x;
			W.at<double>(i, 2) = prePoints[i].y;
			W.at<double>(i, 3) = prePoints[i].x * prePoints[i].y;
			W.at<double>(i, 4) = prePoints[i].x * prePoints[i].x;
			W.at<double>(i, 5) = prePoints[i].y * prePoints[i].y;

			X.at<double>(i, 0) = nextPoints[i].x;
			Y.at<double>(i, 0) = nextPoints[i].y;
		}


		// 计算A B 
		// W * A = X 
		// W * B = Y
		bool ret1 = cv::solve(W, X, A, CV_SVD); // 求A
		bool ret2 = cv::solve(W, Y, B, CV_SVD); // 求B

		if (ret1 == false || ret2 == false)
		{
			return false;
		}

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}

// 计算包含一幅图像有效区域的最小外接矩形(计算无效黑色区域)
RECT GetMinEncloseRect(const cv::Mat& image)
{
	RECT minRect;
	minRect.left   = -1L;
	minRect.right  = -1L;
	minRect.top    = -1L;
	minRect.bottom = -1L;

	if (image.empty() == true)
	{
		return minRect;
	}
	else
	{
		minRect.left   = 0L;
		minRect.right  = static_cast<LONG>(image.cols - 1);
		minRect.top    = 0L;
		minRect.bottom = static_cast<LONG>(image.rows - 1);
	}

	int type = image.type();

	int i = 0, j = 0;
	int temp = 0;

	if (type == 0) // CV_8UC1
	{
		// left
		for (i = 0; i < image.cols; ++i)
		{
			double count = 0;
			for (j = 0; j < image.rows; ++j)
			{
				temp = image.at<unsigned char>(j, i);
				if (temp > 0)
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.left++;
			} 
			else
			{
				break;
			}
		}

		// 全黑
		if (minRect.left == image.cols)
		{
			minRect.left   = -1L;
			minRect.right  = -1L;
			minRect.top    = -1L;
			minRect.bottom = -1L;

			return minRect;
		}

		// right
		temp = 0;
		for (i = image.cols - 1; i > -1; --i)
		{
			for (j = 0;j < image.rows; ++j)
			{
				temp = image.at<unsigned char>(j, i);

				if ( temp > 0)
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.right--;
			}
			else
			{
				break;
			}
		}

		// top
		temp = 0;
		for (j = 0; j < image.rows; ++j)
		{
			for (i = 0; i < image.cols; ++i)
			{
				temp = image.at<unsigned char>(j, i);

				if ( temp > 0 )
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.top++;
			}
			else
			{
				break;
			}
		}

		// bottom
		temp = 0;
		for ( j = image.rows - 1; j > -1; --j)
		{
			for (i = 0; i < image.cols; ++i)
			{
				temp = image.at<unsigned char>(j, i);

				if (temp > 0)
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.bottom--;
			}
			else
			{
				break;
			}
		}	
	} 
	else if(type == 16) // CV_8UC3
	{
		// left
		for (i = 0; i < image.cols; ++i)
		{
			for (j = 0; j < image.rows; ++j)
			{
				temp = image.at<cv::Vec3b>(j, i)[0] + image.at<cv::Vec3b>(j, i)[1] + image.at<cv::Vec3b>(j, i)[2];
				if (temp > 0)
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.left++;
			} 
			else
			{
				break;
			}
		}

		// 全黑
		if (minRect.left == image.cols)
		{
			minRect.left   = -1L;
			minRect.right  = -1L;
			minRect.top    = -1L;
			minRect.bottom = -1L;

			return minRect;
		}

		// right
		temp = 0;
		for (i = image.cols - 1; i > -1; --i)
		{
			for (j = 0;j < image.rows; ++j)
			{
				temp = image.at<cv::Vec3b>(j, i)[0] + image.at<cv::Vec3b>(j, i)[1] + image.at<cv::Vec3b>(j, i)[2];
				if ( temp > 0)
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.right--;
			}
			else
			{
				break;
			}
		}

		// top
		temp = 0;
		for (j = 0; j < image.rows; ++j)
		{
			for (i = 0; i < image.cols; ++i)
			{
				temp = image.at<cv::Vec3b>(j, i)[0] + image.at<cv::Vec3b>(j, i)[1] + image.at<cv::Vec3b>(j, i)[2];
				if ( temp > 0 )
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.top++;
			}
			else
			{
				break;
			}
		}

		// bottom
		temp = 0;
		for ( j = image.rows - 1; j > -1; --j)
		{
			for (i = 0; i < image.cols; ++i)
			{
				temp = image.at<cv::Vec3b>(j, i)[0] + image.at<cv::Vec3b>(j, i)[1] + image.at<cv::Vec3b>(j, i)[2];
				if (temp > 0)
				{
					break;
				}
			}

			if (0 == temp)
			{
				minRect.bottom--;
			}
			else
			{
				break;
			}
		}	
	}
	return minRect;
}


// 线性变换
bool GCPGeoCorrectImgLineTransform( cv::Mat& dstImg, GeoBoundingBox& LBbox, const cv::Mat& srcImg, const std::vector<cv::Point2d>& GCP_XY, const std::vector<cv::Point2d>& GCP_LonLat)
{
	try
	{
		// 指针空值判断
		if (srcImg.empty() == true || 
			GCP_XY.size() < 4 || GCP_LonLat.size() < 4 || GCP_XY.size() != GCP_LonLat.size())
		{
			return false;
		}

		// 类型判断
		if (srcImg.type() != CV_8UC1 && srcImg.type() != CV_8UC3)
		{
			return false;
		}

		// 地面控制点数目
		int count = GCP_XY.size();

		// 对每个GCP坐标验证
		for(int i = 0; i < count; i++)
		{
			if (GCP_XY[i].x < 0 || GCP_XY[i].x > srcImg.cols - 1)
			{
				return false;
			}

			if (GCP_XY[i].y < 0 || GCP_XY[i].y > srcImg.rows - 1)
			{
				return false;
			}

			if (GCP_LonLat[i].x < -180.0F || GCP_LonLat[i].x > 180.0F)
			{
				return false;
			}

			if (GCP_LonLat[i].y < -90.0F || GCP_LonLat[i].y > 90.0F)
			{
				return false;
			}
		}


		// 输入图像空间分辨率估计：一个像素的经纬度分辨率
		double deltaLon = 0.0;
		double deltaLat = 0.0;
		if ( false == CalLonLatResolution(deltaLon, deltaLat, GCP_XY, GCP_LonLat) )
		{
			return false;
		}


		// 计算系数矩阵：A、B
		cv::Mat A, B;
		if (false == CalLineTransformCoefficient(A, B, GCP_XY, GCP_LonLat))
		{
			return false;
		}

		// 图像四个顶点转换后的：经纬度坐标
		cv::Point2f LeftUp, RightUp, LeftDown, RightDown;
		LeftUp.x = static_cast<float>(A.at<double>(0, 0)); 
		LeftUp.y = static_cast<float>(B.at<double>(0, 0));

		RightUp.x = static_cast<float>(A.at<double>(0, 0) + srcImg.cols * A.at<double>(1, 0)); 
		RightUp.y = static_cast<float>(B.at<double>(0, 0) + srcImg.cols * B.at<double>(1, 0)); 

		LeftDown.x = static_cast<float>(A.at<double>(0, 0) + srcImg.rows * A.at<double>(2, 0)); 
		LeftDown.y = static_cast<float>(B.at<double>(0, 0) + srcImg.rows * B.at<double>(2, 0)); 

		RightDown.x =  static_cast<float>(A.at<double>(0, 0) + srcImg.cols * A.at<double>(1, 0) + srcImg.rows * A.at<double>(2, 0));
		RightDown.y = static_cast<float>(B.at<double>(0, 0) +  srcImg.cols * B.at<double>(1, 0) + srcImg.rows * B.at<double>(2, 0)); 


		// 目标图像的边界
		double Lon_min = 0.0;
		double Lon_max = 0.0;
		double Lat_min = 0.0;
		double Lat_max = 0.0;

		Lon_min = min(min(min(LeftUp.x, RightUp.x), RightDown.x), LeftDown.x);
		Lon_max = max(max(max(LeftUp.x, RightUp.x), RightDown.x), LeftDown.x);
		Lat_min = min(min(min(LeftUp.y, RightUp.y), RightDown.y), LeftDown.y);
		Lat_max = max(max(max(LeftUp.y, RightUp.y), RightDown.y), LeftDown.y);

		// 目标图像的尺寸
		int dstWidth  = 0;
		int dstHeight = 0;
		dstWidth =  static_cast<int>((Lon_max - Lon_min) / deltaLon);
		dstHeight = static_cast<int>((Lat_max - Lat_min) / deltaLat);

		if (dstWidth <= 0 || dstHeight <= 0)
		{
			return false;
		}

		// 逆变换
		if (false == CalLineTransformCoefficient(A, B, GCP_LonLat, GCP_XY))
		{
			return false;
		}

		// 计算输出图像
		dstImg = cv::Mat(dstHeight, dstWidth, srcImg.type());

		if (dstImg.empty() == true)
		{
			return false;
		}

		dstImg = cv::Scalar(0,0,0);

		// 单通道 
		if (dstImg.type() == 0)
		{
			#pragma omp parallel for
			for (int x = 0; x < dstImg.cols; x++)
			{
				for (int y = 0; y < dstImg.rows; y++)
				{
					double dst_Lon = 0;
					double dst_Lat = 0;

					double src_x = 0.0;
					double src_y = 0.0;

					dst_Lon = Lon_min + x * deltaLon;
					dst_Lat = Lat_max - y * deltaLat;

					src_x = A.at<double>(0, 0) + (dst_Lon) * A.at<double>(1, 0) + (dst_Lat) * A.at<double>(2, 0);
					src_y = B.at<double>(0, 0) + (dst_Lon) * B.at<double>(1, 0) + (dst_Lat) * B.at<double>(2, 0);


					// 最近邻点
					int nearest_x = int(src_x + 0.5);
					int nearest_y = int(src_y + 0.5);

					if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (srcImg.cols - 1)) && (nearest_y <= (srcImg.rows - 1)))
					{	
						dstImg.at<uchar>(y, x) = srcImg.at<uchar>(nearest_y, nearest_x);
					}
				}
			}
		} 
		else  // 三通道
		{
			#pragma omp parallel for
			for (int x = 0; x < dstImg.cols; x++)
			{
				for (int y = 0; y < dstImg.rows; y++)
				{
					double dst_Lon = 0;
					double dst_Lat = 0;

					double src_x = 0.0;
					double src_y = 0.0;

					dst_Lon = Lon_min + x * deltaLon;
					dst_Lat = Lat_max - y * deltaLat;

					src_x = A.at<double>(0, 0) + (dst_Lon) * A.at<double>(1, 0) + (dst_Lat) * A.at<double>(2, 0);
					src_y = B.at<double>(0, 0) + (dst_Lon) * B.at<double>(1, 0) + (dst_Lat) * B.at<double>(2, 0);

					// 最近邻点
					int nearest_x = int(src_x + 0.5);
					int nearest_y = int(src_y + 0.5);

					if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (srcImg.cols - 1)) && (nearest_y <= (srcImg.rows - 1)))
					{	
						dstImg.at<cv::Vec3b>(y, x) = srcImg.at<cv::Vec3b>(nearest_y, nearest_x);
					}
				}
			}
		}


		RECT ValidRect = GetMinEncloseRect(dstImg);

		if (ValidRect.left >= 0 && ValidRect.right >= 0 && ValidRect.right < dstImg.cols && ValidRect.right - ValidRect.left > 0 &&
			ValidRect.top >= 0 && ValidRect.bottom >= 0 && ValidRect.bottom < dstImg.rows && ValidRect.bottom - ValidRect.top > 0)
		{
			dstImg(cv::Range(ValidRect.top, ValidRect.bottom + 1), cv::Range(ValidRect.left, ValidRect.right + 1)).copyTo(dstImg);

			LBbox.WestLon = Lon_min + ValidRect.left * deltaLon;
			LBbox.EastLon = LBbox.WestLon + dstImg.cols * deltaLon;
			LBbox.NorthLat = Lat_max - ValidRect.top * deltaLat;
			LBbox.SouthLat = LBbox.NorthLat - dstImg.rows * deltaLat;	
		} 
		else
		{
			LBbox.WestLon = Lon_min;
			LBbox.EastLon = Lon_max;
			LBbox.NorthLat = Lat_max;
			LBbox.SouthLat = Lat_min;
		}

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}

// 二次多项式变换
bool GCPGeoCorrectImgPolynomialTransform( cv::Mat& dstImg, GeoBoundingBox& LBbox, const cv::Mat& srcImg, const std::vector<cv::Point2d>& GCP_XY, const std::vector<cv::Point2d>& GCP_LonLat)
{
	try
	{
		// 指针空值判断
		if (srcImg.empty() == true || 
			GCP_XY.size() < 6 || GCP_LonLat.size() < 6 || GCP_XY.size() != GCP_LonLat.size())
		{
			return false;
		}

		// 类型判断
		if (srcImg.type() != CV_8UC1 && srcImg.type() != CV_8UC3)
		{
			return false;
		}


		// 地面控制点数目
		int count = GCP_XY.size();

		// 对每个GCP坐标验证
		for(int i = 0; i < count; i++)
		{
			if (GCP_XY[i].x < 0 || GCP_XY[i].x > srcImg.cols - 1)
			{
				return false;
			}

			if (GCP_XY[i].y < 0 || GCP_XY[i].y > srcImg.rows - 1)
			{
				return false;
			}

			if (GCP_LonLat[i].x < -180.0F || GCP_LonLat[i].x > 180.0F)
			{
				return false;
			}

			if (GCP_LonLat[i].y < -90.0F || GCP_LonLat[i].y > 90.0F)
			{
				return false;
			}
		}

		// 输入图像空间分辨率估计：一个像素的经纬度分辨率
		double deltaLon = 0.0;
		double deltaLat = 0.0;
		if ( false == CalLonLatResolution(deltaLon, deltaLat, GCP_XY, GCP_LonLat) )
		{
			return false;
		}


		// 计算系数矩阵：A、B
		cv::Mat A, B;
		if (!CalPolynomialCoefficient(A, B, GCP_XY, GCP_LonLat))
		{
			return false;
		}

		// 四个角点转换为经纬度后的坐标
		cv::Point2f LeftUp, RightUp, LeftDown, RightDown;

		LeftUp.x = static_cast<float>(A.at<double>(0, 0)); 
		LeftUp.y = static_cast<float>(B.at<double>(0, 0));

		RightUp.x = static_cast<float>(A.at<double>(0, 0) + 
			srcImg.cols * A.at<double>(1, 0) + 
			srcImg.cols * srcImg.cols * A.at<double>(4, 0)); 
		RightUp.y = static_cast<float>(B.at<double>(0, 0) + 
			srcImg.cols * B.at<double>(1, 0) + 
			srcImg.cols * srcImg.cols * B.at<double>(4, 0)); 

		LeftDown.x = static_cast<float>(A.at<double>(0, 0) + 
			srcImg.rows * A.at<double>(2, 0) + 
			srcImg.rows * srcImg.rows * A.at<double>(5, 0)); 
		LeftDown.y = static_cast<float>(B.at<double>(0, 0) + 
			srcImg.rows * B.at<double>(2, 0) + 
			srcImg.rows * srcImg.rows * B.at<double>(5, 0)); 

		RightDown.x =  static_cast<float>(A.at<double>(0, 0) + 
			srcImg.cols * A.at<double>(1, 0) + 
			srcImg.rows * A.at<double>(2, 0) + 
			srcImg.cols * srcImg.rows * A.at<double>(3, 0) + 
			srcImg.cols * srcImg.cols * A.at<double>(4, 0) + 
			srcImg.rows * srcImg.rows * A.at<double>(5, 0));
		RightDown.y = static_cast<float>(B.at<double>(0, 0) + 
			srcImg.cols * B.at<double>(1, 0) + 
			srcImg.rows * B.at<double>(2, 0) + 
			srcImg.cols * srcImg.rows * B.at<double>(3, 0) + 
			srcImg.cols * srcImg.cols * B.at<double>(4, 0) + 
			srcImg.rows * srcImg.rows * B.at<double>(5, 0));


		// 目标图像的边界
		double Lon_min = 0.0;
		double Lon_max = 0.0;
		double Lat_min = 0.0;
		double Lat_max = 0.0;

		Lon_min = min(min(min(LeftUp.x, RightUp.x), RightDown.x), LeftDown.x);
		Lon_max = max(max(max(LeftUp.x, RightUp.x), RightDown.x), LeftDown.x);
		Lat_min = min(min(min(LeftUp.y, RightUp.y), RightDown.y), LeftDown.y);
		Lat_max = max(max(max(LeftUp.y, RightUp.y), RightDown.y), LeftDown.y);

		// 目标图像的尺寸
		int dstWidth  = 0;
		int dstHeight = 0;
		dstWidth =  static_cast<int>((Lon_max - Lon_min) / deltaLon);
		dstHeight = static_cast<int>((Lat_max - Lat_min) / deltaLat);

		if (dstWidth <= 0 || dstHeight <= 0)
		{
			return false;
		}

		// 逆变换
		if (false == CalPolynomialCoefficient(A, B, GCP_LonLat, GCP_XY))
		{
			return false;
		}

		// 计算输出图像
		dstImg = cv::Mat(dstHeight, dstWidth, srcImg.type());

		if (dstImg.empty() == true)
		{
			return false;
		}

		dstImg = cv::Scalar(0,0,0);


		// 单通道 
		if (dstImg.type() == 0)
		{
			#pragma omp parallel for
			for (int x = 0; x < dstImg.cols; x++)
			{
				for (int y = 0; y < dstImg.rows; y++)
				{
					double dst_Lon = 0;
					double dst_Lat = 0;

					double src_x = 0.0;
					double src_y = 0.0;

					dst_Lon = Lon_min + x * deltaLon;
					dst_Lat = Lat_max - y * deltaLat;

					src_x = A.at<double>(0, 0) + 
						(dst_Lon) * A.at<double>(1, 0) + 
						(dst_Lat) * A.at<double>(2, 0) + 
						(dst_Lon) * (dst_Lat) * A.at<double>(3, 0) + 
						(dst_Lon) * (dst_Lon) * A.at<double>(4, 0) + 
						(dst_Lat) * (dst_Lat) * A.at<double>(5, 0);

					src_y = B.at<double>(0, 0) + 
						(dst_Lon) * B.at<double>(1, 0) + 
						(dst_Lat) * B.at<double>(2, 0) + 
						(dst_Lon) * (dst_Lat) * B.at<double>(3, 0) + 
						(dst_Lon) * (dst_Lon) * B.at<double>(4, 0) + 
						(dst_Lat) * (dst_Lat) * B.at<double>(5, 0);


					// 最近邻点
					int nearest_x = int(src_x + 0.5);
					int nearest_y = int(src_y + 0.5);

					if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (srcImg.cols - 1)) && (nearest_y <= (srcImg.rows - 1)))
					{	
						dstImg.at<uchar>(y, x) = srcImg.at<uchar>(nearest_y, nearest_x);
					}
				}
			}

		} 
		else  // 三通道
		{
			#pragma omp parallel for
			for (int x = 0; x < dstImg.cols; x++)
			{
				for (int y = 0; y < dstImg.rows; y++)
				{
					double dst_Lon = 0;
					double dst_Lat = 0;

					double src_x = 0.0;
					double src_y = 0.0;

					dst_Lon = Lon_min + x * deltaLon;
					dst_Lat = Lat_max - y * deltaLat;

					src_x = A.at<double>(0, 0) + 
						(dst_Lon) * A.at<double>(1, 0) + 
						(dst_Lat) * A.at<double>(2, 0) + 
						(dst_Lon) * (dst_Lat) * A.at<double>(3, 0) + 
						(dst_Lon) * (dst_Lon) * A.at<double>(4, 0) + 
						(dst_Lat) * (dst_Lat) * A.at<double>(5, 0);

					src_y = B.at<double>(0, 0) + 
						(dst_Lon) * B.at<double>(1, 0) + 
						(dst_Lat) * B.at<double>(2, 0) + 
						(dst_Lon) * (dst_Lat) * B.at<double>(3, 0) + 
						(dst_Lon) * (dst_Lon) * B.at<double>(4, 0) + 
						(dst_Lat) * (dst_Lat) * B.at<double>(5, 0);

					// 最近邻点
					int nearest_x = int(src_x + 0.5);
					int nearest_y = int(src_y + 0.5);

					if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (srcImg.cols - 1)) && (nearest_y <= (srcImg.rows - 1)))
					{	
						dstImg.at<cv::Vec3b>(y, x) = srcImg.at<cv::Vec3b>(nearest_y, nearest_x);
					}
				}
			}
		}


		RECT ValidRect = GetMinEncloseRect(dstImg);

		if (ValidRect.left >= 0 && ValidRect.right >= 0 && ValidRect.right < dstImg.cols && ValidRect.right - ValidRect.left > 0 &&
			ValidRect.top >= 0 && ValidRect.bottom >= 0 && ValidRect.bottom < dstImg.rows && ValidRect.bottom - ValidRect.top > 0)
		{
			dstImg(cv::Range(ValidRect.top, ValidRect.bottom + 1), cv::Range(ValidRect.left, ValidRect.right + 1)).copyTo(dstImg);

			LBbox.WestLon = Lon_min + ValidRect.left * deltaLon;
			LBbox.EastLon = LBbox.WestLon + dstImg.cols * deltaLon;
			LBbox.NorthLat = Lat_max - ValidRect.top * deltaLat;
			LBbox.SouthLat = LBbox.NorthLat - dstImg.rows * deltaLat;
		} 
		else
		{
			LBbox.WestLon = Lon_min;
			LBbox.EastLon = Lon_max;
			LBbox.NorthLat = Lat_max;
			LBbox.SouthLat = Lat_min;

			return false;
		}

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}

//功能：OpenCV图像格式基于地面控制点的几何精校正
//输入：	1. srcImg 输入图像数据
//          2. GCP_XY 控制点（像素坐标）
//          3. GCP_LonLat 控制点（经纬度坐标）
//          4. type: 变换类型 -1：自适应 0：线性 1：二次多项式

//输出：	1. dstImg 输出图像数据
//          2. 返回值：处理成功返回true，失败返回false
bool GCPGeoCorrectImg( cv::Mat& dstImg, GeoBoundingBox& LBbox, const cv::Mat& srcImg, const std::vector<cv::Point2d>& GCP_XY, const std::vector<cv::Point2d>& GCP_LonLat, int type)
{
	try
	{
		// 指针空值判断
		if (srcImg.empty() == true || 
			GCP_XY.size() < 4 || GCP_LonLat.size() < 4 || GCP_XY.size() != GCP_LonLat.size())
		{
			return false;
		}

		// 变换类型判断
		if(type != -1 && type != 0 && type != 1)
		{
			return false;
		}

		// 类型判断
		if (srcImg.type() != CV_8UC1 && srcImg.type() != CV_8UC3)
		{
			return false;
		}

		if(type == 0)
		{
			// 线性变换
			return GCPGeoCorrectImgLineTransform(dstImg, LBbox, srcImg, GCP_XY, GCP_LonLat);
		} 
		else if(type == 1)  
		{
			 // 二次多项式
			return GCPGeoCorrectImgPolynomialTransform(dstImg, LBbox, srcImg, GCP_XY, GCP_LonLat);
		}
		else
		{
			// 自适应计算
			int count = GCP_XY.size();

			if (count >= 9) // 最小要求6个，设置为9个 提高稳定度
			{
				// 二次多项式
				return GCPGeoCorrectImgPolynomialTransform(dstImg, LBbox, srcImg, GCP_XY, GCP_LonLat);
			}
			else
			{
				// 线性变换
				return GCPGeoCorrectImgLineTransform(dstImg, LBbox, srcImg, GCP_XY, GCP_LonLat);
			}
		}
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}

//功能：基于控制点的几何校正
//输入：	1. srcImg 输入图像数据
//          2. GCP_xy 控制点（畸变像素坐标）
//          3. GCP_XY 控制点（参考图像像素坐标）

//输出：	1. dstImg 输出图像数据
//          2. 返回值：处理成功返回true，失败返回false
bool CPBasedImgCorrect( cv::Mat &dst, const cv::Mat &src, 
	const std::vector<cv::Point2d>& CP_xy, const std::vector<cv::Point2d>& CP_XY)
{
	try
	{
		// 输入有效性判断
		if (src.empty() == true)
		{
			return false;
		}

		if (src.type() != 0 && src.type() != 16)
		{
			return false;
		}

		// 输入图像宽和高
		int imgWidth = src.cols;
		int imgHeight = src.rows;

		// 控制点数目验证
		if (CP_xy.size() < 4 || CP_XY.size() < 4 || CP_xy.size() != CP_XY.size())
		{
			return false;
		}

		// 控制点数目
		int size = CP_xy.size();

		// 每个图像控制点验证
		for(int i = 0; i < size; i++)
		{
			// 畸变图像参考点
			if (CP_xy[i].x < 0 || CP_xy[i].x > imgWidth - 1)
			{
				return false;
			}

			if (CP_xy[i].y < 0 || CP_xy[i].y > imgHeight- 1)
			{
				return false;
			}

			// 参考图像控制点
			if (CP_XY[i].x < 0)
			{
				return false;
			}

			if (CP_XY[i].y < 0)
			{
				return false;
			}
		}


		// 计算系数矩阵：A、B
		cv::Mat A, B;

		if (size >= 6)
		{
			if (false == CalPolynomialCoefficient(A, B, CP_xy, CP_XY))
			{
				return false;
			}
		}
		else
		{
			if (false == CalLineTransformCoefficient(A, B, CP_xy, CP_XY))
			{
				return false;
			}
		}

		// 计算输入图像四个角点转换后的坐标
		cv::Point2f LeftUp, RightUp, LeftDown, RightDown;

		if (size >= 6)
		{
			LeftUp.x = static_cast<float>(A.at<double>(0, 0)); 
			LeftUp.y = static_cast<float>(B.at<double>(0, 0));

			RightUp.x = static_cast<float>(A.at<double>(0, 0) + 
				src.cols * A.at<double>(1, 0) + 
				src.cols * src.cols * A.at<double>(4, 0)); 
			RightUp.y = static_cast<float>(B.at<double>(0, 0) + 
				src.cols * B.at<double>(1, 0) + 
				src.cols * src.cols * B.at<double>(4, 0)); 

			LeftDown.x = static_cast<float>(A.at<double>(0, 0) + 
				src.rows * A.at<double>(2, 0) + 
				src.rows * src.rows * A.at<double>(5, 0)); 
			LeftDown.y = static_cast<float>(B.at<double>(0, 0) + 
				src.rows * B.at<double>(2, 0) + 
				src.rows * src.rows * B.at<double>(5, 0)); 

			RightDown.x =  static_cast<float>(A.at<double>(0, 0) + 
				src.cols * A.at<double>(1, 0) + 
				src.rows * A.at<double>(2, 0) + 
				src.cols * src.rows * A.at<double>(3, 0) + 
				src.cols * src.cols * A.at<double>(4, 0) + 
				src.rows * src.rows * A.at<double>(5, 0));
			RightDown.y = static_cast<float>(B.at<double>(0, 0) + 
				src.cols * B.at<double>(1, 0) + 
				src.rows * B.at<double>(2, 0) + 
				src.cols * src.rows * B.at<double>(3, 0) + 
				src.cols * src.cols * B.at<double>(4, 0) + 
				src.rows * src.rows * B.at<double>(5, 0));
		} 
		else
		{
			LeftUp.x = static_cast<float>(A.at<double>(0, 0)); 
			LeftUp.y = static_cast<float>(B.at<double>(0, 0));

			RightUp.x = static_cast<float>(A.at<double>(0, 0) + src.cols * A.at<double>(1, 0)); 
			RightUp.y = static_cast<float>(B.at<double>(0, 0) + src.cols * B.at<double>(1, 0)); 

			LeftDown.x = static_cast<float>(A.at<double>(0, 0) + src.rows * A.at<double>(2, 0)); 
			LeftDown.y = static_cast<float>(B.at<double>(0, 0) + src.rows * B.at<double>(2, 0)); 

			RightDown.x =  static_cast<float>(A.at<double>(0, 0) + src.cols * A.at<double>(1, 0) + src.rows * A.at<double>(2, 0));
			RightDown.y = static_cast<float>(B.at<double>(0, 0) +  src.cols * B.at<double>(1, 0) + src.rows * B.at<double>(2, 0));
		}

		// 目标图像的边界
		int min_X = 0;
		int max_X = 0;
		int min_Y = 0;
		int max_Y = 0;

		min_X = static_cast<int>(min(min(min(LeftUp.x, RightUp.x), RightDown.x), LeftDown.x));
		max_X = static_cast<int>(max(max(max(LeftUp.x, RightUp.x), RightDown.x), LeftDown.x));
		min_Y = static_cast<int>(min(min(min(LeftUp.y, RightUp.y), RightDown.y), LeftDown.y));
		max_Y = static_cast<int>(max(max(max(LeftUp.y, RightUp.y), RightDown.y), LeftDown.y));


		// 目标图像的尺寸
		int dstWidth  = 0;
		int dstHeight = 0;
		dstWidth =  static_cast<int>(max_X - min_X);
		dstHeight = static_cast<int>(max_Y - min_Y);

		// 逆变换
		if (size >= 6)
		{
			if (false == CalPolynomialCoefficient(A, B, CP_XY, CP_xy))
			{
				return false;
			}
		}
		else
		{
			if (false == CalLineTransformCoefficient(A, B, CP_XY, CP_xy))
			{
				return false;
			}
		}

		// 计算输出图像
		if (src.type() == CV_8UC1)
		{
			dst = cv::Mat(dstHeight, dstWidth, CV_8UC1);
		} 
		else
		{
			dst = cv::Mat(dstHeight, dstWidth, CV_8UC3);
		}

		if (dst.empty() == true)
		{
			return false;
		}

		dst = cv::Scalar(0,0,0);

		// 图像重采样
		if (size >= 6) // 二次多项式
		{
			if (dst.type() == 0)
			{
				for (int x = min_X; x < max_X; x++)
				{
					for (int y = min_Y; y < max_Y; y++)
					{
						double src_x = 0.0;
						double src_y = 0.0;

						src_x = A.at<double>(0, 0) + 
							x * A.at<double>(1, 0) + 
							y * A.at<double>(2, 0) + 
							x * y * A.at<double>(3, 0) + 
							x * x * A.at<double>(4, 0) + 
							y * y * A.at<double>(5, 0);

						src_y = B.at<double>(0, 0) + 
							x * B.at<double>(1, 0) + 
							y * B.at<double>(2, 0) + 
							x * y * B.at<double>(3, 0) + 
							x * x * B.at<double>(4, 0) + 
							y * y * B.at<double>(5, 0);


						// 最近邻点
						int nearest_x = int(src_x + 0.5);
						int nearest_y = int(src_y + 0.5);

						if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (src.cols - 1)) && (nearest_y <= (src.rows - 1)))
						{	
							dst.at<uchar>(y - min_Y, x - min_X) = src.at<uchar>(nearest_y, nearest_x);
						}
					}
				}

			} 
			else  
			{
				for (int x = min_X; x < max_X; x++)
				{
					for (int y = min_Y; y < max_Y; y++)
					{
						double src_x = 0.0;
						double src_y = 0.0;

						src_x = A.at<double>(0, 0) + 
							x * A.at<double>(1, 0) + 
							y * A.at<double>(2, 0) + 
							x * y * A.at<double>(3, 0) + 
							x * x * A.at<double>(4, 0) + 
							y * y * A.at<double>(5, 0);

						src_y = B.at<double>(0, 0) + 
							x * B.at<double>(1, 0) + 
							y * B.at<double>(2, 0) + 
							x * y * B.at<double>(3, 0) + 
							x * x * B.at<double>(4, 0) + 
							y * y * B.at<double>(5, 0);


						// 最近邻点
						int nearest_x = int(src_x + 0.5);
						int nearest_y = int(src_y + 0.5);

						if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (src.cols - 1)) && (nearest_y <= (src.rows - 1)))
						{	
							dst.at<cv::Vec3b>(y - min_Y, x - min_X) = src.at<cv::Vec3b>(nearest_y, nearest_x);
						}
					}
				}
			}
		}
		else  // 线性变换
		{
			if (dst.type() == 0)
			{
				for (int x = min_X; x < max_X; x++)
				{
					for (int y = min_Y; y < max_Y; y++)
					{
						double src_x = 0.0;
						double src_y = 0.0;

						src_x = A.at<double>(0, 0) + x * A.at<double>(1, 0) + y * A.at<double>(2, 0);
						src_y = B.at<double>(0, 0) + x * B.at<double>(1, 0) + y * B.at<double>(2, 0);

						// 最近邻点
						int nearest_x = int(src_x + 0.5);
						int nearest_y = int(src_y + 0.5);

						if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (src.cols - 1)) && (nearest_y <= (src.rows - 1)))
						{	
							dst.at<uchar>(y - min_Y, x - min_X) = src.at<uchar>(nearest_y, nearest_x);
						}
					}
				}
			} 
			else  
			{
				for (int x = min_X; x < max_X; x++)
				{
					for (int y = min_Y; y < max_Y; y++)
					{
						double src_x = 0.0;
						double src_y = 0.0;

						src_x = A.at<double>(0, 0) + x * A.at<double>(1, 0) + y * A.at<double>(2, 0);
						src_y = B.at<double>(0, 0) + x * B.at<double>(1, 0) + y * B.at<double>(2, 0);


						// 最近邻点
						int nearest_x = int(src_x + 0.5);
						int nearest_y = int(src_y + 0.5);

						if ((nearest_x >= 0) && (nearest_y >= 0) && (nearest_x <= (src.cols - 1)) && (nearest_y <= (src.rows - 1)))
						{	
							dst.at<cv::Vec3b>(y - min_Y, x - min_X) = src.at<cv::Vec3b>(nearest_y, nearest_x);
						}
					}
				}
			}
		}

		RECT ValidRect = GetMinEncloseRect(dst);

		if (ValidRect.left >= 0 && ValidRect.right >= 0 && ValidRect.right < dst.cols && ValidRect.right - ValidRect.left > 0 &&
			ValidRect.top >= 0 && ValidRect.bottom >= 0 && ValidRect.bottom < dst.rows && ValidRect.bottom - ValidRect.top > 0)
		{
			dst = dst(cv::Range(ValidRect.top, ValidRect.bottom), cv::Range(ValidRect.left, ValidRect.right));
		} 

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}


//功能：基于地面控制点的几何精校正
bool GCPGeoCorrectImg(	ImgStru *dstImg,       
	                    const ImgStru *srcImg,                           
	                    const std::vector<Point2d>& GCP_XY,        
	                    const std::vector<Point2d>& GCP_LonLat, 
						int type)
{
	try
	{
		// 指针空值判断
		if (srcImg == NULL || dstImg == NULL)
		{
			return false;
		}

		// 输入图像有效性判断
		if (srcImg->ImgWidth  <= 0 ||
			srcImg->ImgHeight <= 0 || 
			srcImg->buff == NULL)
		{
			return false;
		}

		if (srcImg->bitcount != 8 && srcImg->bitcount != 24)
		{
			return false;
		}

		// 创建OpenCV结构输入图像
		cv::Mat src;
		if (srcImg->bitcount == 8)
		{
			src = cv::Mat(srcImg->ImgHeight, srcImg->ImgWidth, CV_8UC1);
		} 
		else
		{
			src = cv::Mat(srcImg->ImgHeight, srcImg->ImgWidth, CV_8UC3);
		}

		if (src.empty() == true)
		{
			return false;
		}

		// 数据复制 
		int lineByte = srcImg->ImgWidth * srcImg->bitcount / 8;
		unsigned int imgBufSize = static_cast<unsigned int>(srcImg->ImgHeight * lineByte);
		memcpy(src.data, srcImg->buff, imgBufSize);  

		

		// 创建输出图像
		cv::Mat dst;
		if (GCPGeoCorrectImg(dst, dstImg->BoundingBox, src, GCP_XY, GCP_LonLat, type) == false)
		{
			return false;
		}

	
		// 数据复制
		SAFE_DELETE_ARRAY(dstImg->buff);

		// 属性赋值
		dstImg->ImgWidth  = dst.cols;
		dstImg->ImgHeight = dst.rows;
		dstImg->bitcount = srcImg->bitcount;

		lineByte = (dst.cols * (dstImg->bitcount / 8));
		imgBufSize = static_cast<unsigned int>(dst.rows * lineByte);
		dstImg->buff = new unsigned char[imgBufSize];

		if (dstImg->buff == NULL)
		{
			return false;
		}

		memcpy(dstImg->buff, dst.data, imgBufSize);

 		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}