#include "stdafx.h"
#include "SysGeoCorrect.h"

#include <opencv2\opencv.hpp>  
using namespace cv;


// 加载 地理信息计算dll 
#include "MapPrj.h"
#pragma comment( lib, "MapPrj.lib" )

// 基于GCP的几何精校正
#include "GCPGeoCorrect.h"

// 读取高程数据
#include "GetElevation.h"

double GetMaxValue(double* array, int count)
{
	try
	{
		// 数据有效性判断
		if (array == NULL || count <= 0)
		{
			return DBL_MAX;
		}

		// 取最大值
		double MaxValue = array[0];
		for (int i = 0; i < count; i++)
		{
			if (array[i] > MaxValue)
			{
				MaxValue = array[i];
			}
		}
		return MaxValue;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return DBL_MAX;
	}
	catch(...)
	{
		return DBL_MAX;
	}
}

double GetMinValue(double* array, int count)
{
	try
	{
		// 数据有效性判断
		if (array == NULL || count <= 0)
		{
			return DBL_MIN;
		}

		// 取最小值
		double MinValue = array[0];
		for (int i = 0; i < count; i++)
		{
			if (array[i] < MinValue)
			{
				MinValue = array[i];
			}
		}
		return MinValue;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return DBL_MIN;
	}
	catch(...)
	{
		return DBL_MIN;
	}
}


// 计算方位角
double getYaw(double XA, double YA)
{
	// 输出合成航向角 Yaw 
	double PayLoad_Yaw = 0;

	// 在坐标轴上
	if (XA > 0 && YA == 0)
	{
		PayLoad_Yaw = 90;
	}
	else if (XA < 0 && YA == 0)
	{
		PayLoad_Yaw = -90;
	}
	else if (XA == 0 && YA > 0)
	{
		PayLoad_Yaw = 0;
	}
	else if (XA == 0 && YA < 0)
	{
		PayLoad_Yaw = 180;
	}
	else
	{
		double tanYaw = 0.0; 
		if (YA > 0 && XA > 0)
		{
			// 第1象限
			tanYaw = XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
		}
		else if (YA < 0 && XA > 0)
		{
			// 第2象限
			tanYaw = -XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
			PayLoad_Yaw = 180 - PayLoad_Yaw;
		}
		else if (YA < 0 && XA < 0)
		{
			// 第3象限
			tanYaw = XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
			PayLoad_Yaw = -180 + PayLoad_Yaw;
		}
		else if (YA > 0 && XA < 0)
		{
			// 第4象限
			tanYaw = -XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
			PayLoad_Yaw = -PayLoad_Yaw;
		}
	}

	return PayLoad_Yaw;
}


//功能：计算图像中任意点经纬度坐标 无误差补偿
//输入：	1. frame  输入复接数据
//          2. pt_x： 列
//          3. pt_y： 行
//      
//输出：	1. Lon     经度
//          2. Lat：   纬度
//          3. 返回值：处理成功返回true，失败返回false
bool CalAnyPtCoordinateWithoutErrorCompensate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
	try
	{
		// 基础条件判断
		if (frame == NULL || frame->bValid == FALSE)
		{
			return false;
		}

		// 判断复接数据有效性
		double uavLon = frame->UAV_GPS_Lon;
		double uavLat = frame->UAV_GPS_Lat;
		double uavAlt = frame->UAV_GPS_Alt;

		double uavGroundHeight = frame->UAV_GroundHeight; 

		double uavYaw = frame->UAV_Yaw;
		double uavPitch = frame->UAV_Pitch;
		double uavRoll = frame->UAV_Roll;

		double zhAz = frame->ZH_Azimuth;
		double zhPitch = frame->ZH_Pitch;

		// 2. 越界判断
		if (abs(uavLon) > 180) // 经度
		{
			return false;
		}

		if (abs(uavLat) > 90)  // 纬度
		{
			return false;
		}

		if (uavAlt < 0 || uavAlt > 10000) // 高度
		{
			return false;
		}

		if (uavGroundHeight < 0 || (uavAlt-  uavGroundHeight <= 0)  )
		{
			return false;
		}

		if (abs(uavYaw) > 360) // 飞机航向角
		{
			return false;
		}

		if (abs(uavPitch) > 45) // 飞机俯仰角
		{
			return false;
		}

		if (abs(uavRoll) > 45)  // 飞机滚转角
		{
			return false;
		}

		if (abs(zhAz) > 180) // 载荷方位角
		{
			return false;
		}

		if (zhPitch > 20 || zhPitch < -120)  // if (zhPitch > -15 || zhPitch < -110) 
		{
			return false;
		}

		// 图像尺寸
		int imgWidth = frame->ZH_ImgWidth;
		int imgHeight = frame->ZH_ImgHeight;

		// 判断
		if (imgWidth <= 0 || imgHeight <= 0)
		{
			return false;
		}

		// 输入位置判断
		if (pt_x < 0 || pt_x >= imgWidth)
		{
			return false;
		}

		if (pt_y < 0 || pt_y >= imgHeight)
		{
			return false;
		}

		// 焦距
		double f = frame->ZH_FocalLen / 1000;  // 米
		if (f <= 0)
		{
			return false;
		}

		// 像元尺寸 米
		double pixelSize = frame->ZH_PixelSize;
		if (pixelSize <= 0)
		{
			return false;
		}

		// 飞机
		double UAV_ROLL  = uavRoll;  // 飞机滚转角  （角度）
		double UAV_PITCH = uavPitch; // 飞机俯仰角  （角度）
		double UAV_YAW   = uavYaw;   // 飞机航向角  （角度）

		// 载荷
		double ZH_PITCH = zhPitch; // 载荷高低角  （角度）
		double ZH_YAW   = zhAz;    // 载荷方位角  （角度）

		// 载荷俯仰角修正
		ZH_PITCH = ZH_PITCH + 90; // [0 90]

		// 各个角转换为弧度
		ZH_PITCH = ZH_PITCH / 180 * CV_PI;   // 1
		ZH_YAW   =   ZH_YAW / 180 * CV_PI;   // 2

		UAV_PITCH = UAV_PITCH / 180 * CV_PI; // 3
		UAV_ROLL  = UAV_ROLL  / 180 * CV_PI; // 4
		UAV_YAW   = UAV_YAW   / 180 * CV_PI; // 5


		double BaseLon = uavLon; // 经度
		double BaseLat = uavLat; // 经度

		double height = uavAlt - uavGroundHeight; // 相对高度 = 飞行高度 - 地面高程

		bool bLaser = frame->ZH_LaserOn;
		double LaserDistance = frame->ZH_LaserDist;

		// 激光测距有效性判断
		if (bLaser == true && LaserDistance < 100)
		{
			bLaser = false;
		}

		// 计算旋转矩阵 
		// x1  x2  x3
		// x4  x5  x6
		// x7  x8  x9

		// 载荷俯仰角
		cv::Mat m1(3,3,CV_64FC1);
		m1.at<double>(0,0) = 1;
		m1.at<double>(0,1) = 0;
		m1.at<double>(0,2) = 0;

		m1.at<double>(1,0) = 0;
		m1.at<double>(1,1) = cos(ZH_PITCH);
		m1.at<double>(1,2) = -sin(ZH_PITCH);

		m1.at<double>(2,0) = 0;
		m1.at<double>(2,1) = sin(ZH_PITCH);
		m1.at<double>(2,2) = cos(ZH_PITCH);

		// 载荷方位角
		cv::Mat m2(3,3,CV_64FC1);
		m2.at<double>(0,0) = cos(ZH_YAW);
		m2.at<double>(0,1) = sin(ZH_YAW);
		m2.at<double>(0,2) = 0;

		m2.at<double>(1,0) = -sin(ZH_YAW);
		m2.at<double>(1,1) = cos(ZH_YAW);
		m2.at<double>(1,2) = 0;

		m2.at<double>(2,0) = 0;
		m2.at<double>(2,1) = 0;
		m2.at<double>(2,2) = 1;

		// 飞机俯仰角
		cv::Mat m3(3,3,CV_64FC1);
		m3.at<double>(0,0) = 1;
		m3.at<double>(0,1) = 0;
		m3.at<double>(0,2) = 0;

		m3.at<double>(1,0) = 0;
		m3.at<double>(1,1) = cos(UAV_PITCH);
		m3.at<double>(1,2) = -sin(UAV_PITCH);

		m3.at<double>(2,0) = 0;
		m3.at<double>(2,1) = sin(UAV_PITCH);
		m3.at<double>(2,2) = cos(UAV_PITCH);

		// 飞机滚转角
		cv::Mat m4(3,3,CV_64FC1);
		m4.at<double>(0,0) = cos(UAV_ROLL);
		m4.at<double>(0,1) = 0;
		m4.at<double>(0,2) = sin(UAV_ROLL);

		m4.at<double>(1,0) = 0;
		m4.at<double>(1,1) = 1;
		m4.at<double>(1,2) = 0;

		m4.at<double>(2,0) = -sin(UAV_ROLL);
		m4.at<double>(2,1) = 0;
		m4.at<double>(2,2) = cos(UAV_ROLL);

		// 飞机航向角
		cv::Mat m5(3,3,CV_64FC1);
		m5.at<double>(0,0) = cos(UAV_YAW);
		m5.at<double>(0,1) = sin(UAV_YAW);
		m5.at<double>(0,2) = 0;

		m5.at<double>(1,0) = -sin(UAV_YAW);
		m5.at<double>(1,1) = cos(UAV_YAW);
		m5.at<double>(1,2) = 0;

		m5.at<double>(2,0) = 0;
		m5.at<double>(2,1) = 0;
		m5.at<double>(2,2) = 1;


		// 矩阵乘法
		cv::Mat m54321;
		cv::gemm(m2, m1, 1, cv::noArray(), 0, m54321);
		cv::gemm(m3, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m4, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m5, m54321, 1, cv::noArray(), 0, m54321);


		// 取出M54321中的值
		double x1 = m54321.at<double>(0,0);
		double x2 = m54321.at<double>(0,1);
		double x3 = m54321.at<double>(0,2);

		double x4 = m54321.at<double>(1,0);
		double x5 = m54321.at<double>(1,1);
		double x6 = m54321.at<double>(1,2);

		double x7 = m54321.at<double>(2,0);
		double x8 = m54321.at<double>(2,1);
		double x9 = m54321.at<double>(2,2);

		// 合成高低角不能在水平线之上
		if (x9 <= 0)
		{
			return false;
		}

		// 坐标转换
		double x = 0, y = 0;

		// 像平面坐标系
		x = +pt_x - imgWidth / 2;
		y = -pt_y + imgHeight / 2;  

		// 转换为物理坐标 单位 米
		x = x * pixelSize;
		y = y * pixelSize;

		// 分母零值判断
		if (x7 * x + x8 * y - x9 * f == 0)
		{
			return false;
		}

		// 共线方程模型
		double XA = -(x1 * x + x2 * y - x3 * f) / (x7 * x + x8 * y - x9 * f) * height;
		double YA = -(x4 * x + x5 * y - x6 * f) / (x7 * x + x8 * y - x9 * f) * height;

		// 计算方位角和距离
		double distance = sqrt(XA * XA + YA * YA);
		double yaw = getYaw(XA, YA);

		// 计算图像中心点经纬度
		CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84坐标系

		// 目标高度计算
		H = uavGroundHeight;

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

	return true;
}


// 充分利用激光，提高单点定位精度
void CalLonLatOffset(double &LonOffset, double &LatOffset, const struQB_FJ *frame)
{
	try
	{
		// 置零
		LonOffset = 0;
		LatOffset = 0;

		// 基础条件判断
		if (frame == NULL || frame->bValid == false)
		{
			return;
		}

		// 如果无激光 直接返回
		if ((frame->ZH_LaserOn == true && frame->ZH_LaserDist > 0 && frame->ZH_LaserDist < 65535) == false)
		{
			return;
		}

		// 激光测量计算结果
		double LaserLon = 0.0;
		double LaserLat = 0.0;
		double LaserH = 0.0;

		// 无激光测量计算结果
		double Lon = 0.0;
		double Lat = 0.0;
		double H = 0.0;

		if (true == CalImgCenterCoordinate(LaserLon, LaserLat, LaserH, frame) && 
			true == CalAnyPtCoordinateWithoutErrorCompensate(Lon, Lat, H, frame, frame->ZH_ImgWidth / 2, frame->ZH_ImgWidth / 2))
		{
			LonOffset = LaserLon - Lon;
			LatOffset = LaserLat - Lat;
		}
	}
	catch(cv::Exception &e)
	{
		e.msg.c_str();
		return;
	}
	catch(...)
	{
		return;
	}
}


// 计算图像中心点经纬度坐标
bool CalImgCenterCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame)
{
	try
	{
		// 基础条件判断
		if (frame == NULL || frame->bValid == FALSE)
		{
			return false;
		}

		// 判断复接数据有效性
		double uavLon = frame->UAV_GPS_Lon;
		double uavLat = frame->UAV_GPS_Lat;
		double uavAlt = frame->UAV_GPS_Alt;

		double uavGroundHeight = frame->UAV_GroundHeight; 

		double uavYaw = frame->UAV_Yaw;
		double uavPitch = frame->UAV_Pitch;
		double uavRoll = frame->UAV_Roll;

		double zhAz = frame->ZH_Azimuth;
		double zhPitch = frame->ZH_Pitch;


		// 2. 越界判断
		if (abs(uavLon) > 180) // 经度
		{
			return false;
		}

		if (abs(uavLat) > 90)  // 纬度
		{
			return false;
		}

		if (uavAlt < 0 || uavAlt > 10000) // 高度
		{
			return false;
		}

		if (uavGroundHeight < 0 || (uavAlt-  uavGroundHeight <= 0))
		{
			return false;
		}

		if (abs(uavYaw) > 360) // 飞机航向角
		{
			return false;
		}

		if (abs(uavPitch) > 45) // 飞机俯仰角
		{
			return false;
		}

		if (abs(uavRoll) > 45)  // 飞机滚转角
		{
			return false;
		}

		if (abs(zhAz) > 180) // 载荷方位角
		{
			return false;
		}

		if (zhPitch > 20 || zhPitch < -120) 
		{
			return false;
		}

		// 飞机
		double UAV_ROLL  = uavRoll;  // 飞机滚转角  （角度）
		double UAV_PITCH = uavPitch; // 飞机俯仰角  （角度）
		double UAV_YAW   = uavYaw;   // 飞机航向角  （角度）

		// 载荷
		double ZH_PITCH = zhPitch; // 载荷高低角  （角度）
		double ZH_YAW   = zhAz;    // 载荷方位角  （角度）

		// 载荷俯仰角修正
		ZH_PITCH = ZH_PITCH + 90; // [0 90]

		// 各个角转换为弧度
		ZH_PITCH = ZH_PITCH / 180 * CV_PI;   // 1
		ZH_YAW   =   ZH_YAW / 180 * CV_PI;   // 2

		UAV_PITCH = UAV_PITCH / 180 * CV_PI; // 3
		UAV_ROLL  = UAV_ROLL  / 180 * CV_PI; // 4
		UAV_YAW   = UAV_YAW   / 180 * CV_PI; // 5


		double BaseLon = uavLon; // 经度
		double BaseLat = uavLat; // 经度

		double height = uavAlt - uavGroundHeight; // 相对高度 = 飞行高度 - 地面高程

		bool bLaser = frame->ZH_LaserOn;
		double LaserDistance = frame->ZH_LaserDist;

		// 激光测距有效性判断
		if (bLaser == true && LaserDistance < 100)
		{
			bLaser = false;
		}

		// 计算旋转矩阵 
		// x1  x2  x3
		// x4  x5  x6
		// x7  x8  x9

		// 载荷俯仰角
		cv::Mat m1(3,3,CV_64FC1);
		m1.at<double>(0,0) = 1;
		m1.at<double>(0,1) = 0;
		m1.at<double>(0,2) = 0;

		m1.at<double>(1,0) = 0;
		m1.at<double>(1,1) = cos(ZH_PITCH);
		m1.at<double>(1,2) = -sin(ZH_PITCH);

		m1.at<double>(2,0) = 0;
		m1.at<double>(2,1) = sin(ZH_PITCH);
		m1.at<double>(2,2) = cos(ZH_PITCH);

		// 载荷方位角
		cv::Mat m2(3,3,CV_64FC1);
		m2.at<double>(0,0) = cos(ZH_YAW);
		m2.at<double>(0,1) = sin(ZH_YAW);
		m2.at<double>(0,2) = 0;

		m2.at<double>(1,0) = -sin(ZH_YAW);
		m2.at<double>(1,1) = cos(ZH_YAW);
		m2.at<double>(1,2) = 0;

		m2.at<double>(2,0) = 0;
		m2.at<double>(2,1) = 0;
		m2.at<double>(2,2) = 1;

		// 飞机俯仰角
		cv::Mat m3(3,3,CV_64FC1);
		m3.at<double>(0,0) = 1;
		m3.at<double>(0,1) = 0;
		m3.at<double>(0,2) = 0;

		m3.at<double>(1,0) = 0;
		m3.at<double>(1,1) = cos(UAV_PITCH);
		m3.at<double>(1,2) = -sin(UAV_PITCH);

		m3.at<double>(2,0) = 0;
		m3.at<double>(2,1) = sin(UAV_PITCH);
		m3.at<double>(2,2) = cos(UAV_PITCH);

		// 飞机滚转角
		cv::Mat m4(3,3,CV_64FC1);
		m4.at<double>(0,0) = cos(UAV_ROLL);
		m4.at<double>(0,1) = 0;
		m4.at<double>(0,2) = sin(UAV_ROLL);

		m4.at<double>(1,0) = 0;
		m4.at<double>(1,1) = 1;
		m4.at<double>(1,2) = 0;

		m4.at<double>(2,0) = -sin(UAV_ROLL);
		m4.at<double>(2,1) = 0;
		m4.at<double>(2,2) = cos(UAV_ROLL);

		// 飞机航向角
		cv::Mat m5(3,3,CV_64FC1);
		m5.at<double>(0,0) = cos(UAV_YAW);
		m5.at<double>(0,1) = sin(UAV_YAW);
		m5.at<double>(0,2) = 0;

		m5.at<double>(1,0) = -sin(UAV_YAW);
		m5.at<double>(1,1) = cos(UAV_YAW);
		m5.at<double>(1,2) = 0;

		m5.at<double>(2,0) = 0;
		m5.at<double>(2,1) = 0;
		m5.at<double>(2,2) = 1;


		// 矩阵乘法
		cv::Mat m54321;
		cv::gemm(m2, m1, 1, cv::noArray(), 0, m54321);
		cv::gemm(m3, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m4, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m5, m54321, 1, cv::noArray(), 0, m54321);


		// 取出M54321中的值
		double x1 = m54321.at<double>(0,0);
		double x2 = m54321.at<double>(0,1);
		double x3 = m54321.at<double>(0,2);

		double x4 = m54321.at<double>(1,0);
		double x5 = m54321.at<double>(1,1);
		double x6 = m54321.at<double>(1,2);

		double x7 = m54321.at<double>(2,0);
		double x8 = m54321.at<double>(2,1);
		double x9 = m54321.at<double>(2,2);

		// 合成高低角不能位于水平线之上
		if (x9 <= 0)
		{
			return false;
		}

		// 如果有激光测距 以激光测距为准
		if (bLaser == true && LaserDistance < 65535) // 65535 = 0xFFFF 无效值
		{
			height = LaserDistance * x9;
		}

		double XA = -(x3) / (x9) * height;
		double YA = -(x6) / (x9) * height;

		// 输出合成航向角 Yaw 
		double PayLoad_Yaw = 0;
		PayLoad_Yaw = getYaw(XA, YA);

		// 高低角 合成
		double PayLoad_Pitch = acos(x9) * 180 / CV_PI;   

		// 三角形底边
		double distance = height * tan(PayLoad_Pitch / 180.0 * CV_PI);

		// 如果有激光测距 以激光测距为准
		if (bLaser == true && LaserDistance < 65535)
		{
			// 根据激光距离和合成角度 计算平面投影距离
			distance = LaserDistance * sin(PayLoad_Pitch / 180.0 * CV_PI);
		}

		//功能：计算目标点的经纬度
		CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, PayLoad_Yaw, distance, 3); // WGS84坐标系

		// 目标高度计算
		H = uavGroundHeight;  

		// 如果有激光测距 
		if (bLaser == true && LaserDistance < 65535)
		{
			H = uavAlt - LaserDistance * cos(PayLoad_Pitch * CV_PI / 180);
		}
	}
	catch(cv::Exception &e)
	{
		e.msg.c_str();
		return false;
	}
	catch(...)
	{
		return false;
	}

	return true;
}

//功能：计算图像中任意点经纬度坐标
//输入：	1. frame  输入复接数据
//          2. pt_x： 列
//          3. pt_y： 行
//      
//输出：	1. Lon     经度
//          2. Lat：   纬度
//          3. 返回值：处理成功返回true，失败返回false
bool CalAnyPtCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
	try
	{
		// 基础条件判断
		if (frame == NULL || frame->bValid == FALSE)
		{
			return false;
		}

		// 判断复接数据有效性
		double uavLon = frame->UAV_GPS_Lon;
		double uavLat = frame->UAV_GPS_Lat;
		double uavAlt = frame->UAV_GPS_Alt;

		double uavGroundHeight = frame->UAV_GroundHeight; 

		double uavYaw = frame->UAV_Yaw;
		double uavPitch = frame->UAV_Pitch;
		double uavRoll = frame->UAV_Roll;

		double zhAz = frame->ZH_Azimuth;
		double zhPitch = frame->ZH_Pitch;

		// 2. 越界判断
		if (abs(uavLon) > 180) // 经度
		{
			return false;
		}

		if (abs(uavLat) > 90)  // 纬度
		{
			return false;
		}

		if (uavAlt < 0 || uavAlt > 10000) // 高度
		{
			return false;
		}

		if (uavGroundHeight < 0 || (uavAlt-  uavGroundHeight <= 0))
		{
			return false;
		}

		if (abs(uavYaw) > 360) // 飞机航向角
		{
			return false;
		}

		if (abs(uavPitch) > 45) // 飞机俯仰角
		{
			return false;
		}

		if (abs(uavRoll) > 45)  // 飞机滚转角
		{
			return false;
		}

		if (abs(zhAz) > 180) // 载荷方位角
		{
			return false;
		}

		if (zhPitch > 20 || zhPitch < -120) 
		{
			return false;
		}

		// 图像尺寸
		int imgWidth = frame->ZH_ImgWidth;
		int imgHeight = frame->ZH_ImgHeight;

		// 判断
		if (imgWidth <= 0 || imgHeight <= 0)
		{
			return false;
		}

		// 输入位置判断
		if (pt_x < 0 || pt_x >= imgWidth)
		{
			return false;
		}

		if (pt_y < 0 || pt_y >= imgHeight)
		{
			return false;
		}

		// 焦距
		double f = frame->ZH_FocalLen / 1000;  // 米
		if (f <= 0)
		{
			return false;
		}

		// 像元尺寸 米
		double pixelSize = frame->ZH_PixelSize;
		if (pixelSize <= 0)
		{
			return false;
		}

		// 飞机
		double UAV_ROLL  = uavRoll;  // 飞机滚转角  （角度）
		double UAV_PITCH = uavPitch; // 飞机俯仰角  （角度）
		double UAV_YAW   = uavYaw;   // 飞机航向角  （角度）

		// 载荷
		double ZH_PITCH = zhPitch; // 载荷高低角  （角度）
		double ZH_YAW   = zhAz;    // 载荷方位角  （角度）

		// 载荷俯仰角修正
		ZH_PITCH = ZH_PITCH + 90; // [0 90]

		// 各个角转换为弧度
		ZH_PITCH = ZH_PITCH / 180 * CV_PI;   // 1
		ZH_YAW   =   ZH_YAW / 180 * CV_PI;   // 2

		UAV_PITCH = UAV_PITCH / 180 * CV_PI; // 3
		UAV_ROLL  = UAV_ROLL  / 180 * CV_PI; // 4
		UAV_YAW   = UAV_YAW   / 180 * CV_PI; // 5


		double BaseLon = uavLon; // 经度
		double BaseLat = uavLat; // 经度

		double height = uavAlt - uavGroundHeight; // 相对高度 = 飞行高度 - 地面高程

		bool bLaser = frame->ZH_LaserOn;
		double LaserDistance = frame->ZH_LaserDist;

		// 激光测距有效性判断
		if (bLaser == true && LaserDistance < 100)
		{
			bLaser = false;
		}

		// 计算旋转矩阵 
		// x1  x2  x3
		// x4  x5  x6
		// x7  x8  x9

		// 载荷俯仰角
		cv::Mat m1(3,3,CV_64FC1);
		m1.at<double>(0,0) = 1;
		m1.at<double>(0,1) = 0;
		m1.at<double>(0,2) = 0;

		m1.at<double>(1,0) = 0;
		m1.at<double>(1,1) = cos(ZH_PITCH);
		m1.at<double>(1,2) = -sin(ZH_PITCH);

		m1.at<double>(2,0) = 0;
		m1.at<double>(2,1) = sin(ZH_PITCH);
		m1.at<double>(2,2) = cos(ZH_PITCH);

		// 载荷方位角
		cv::Mat m2(3,3,CV_64FC1);
		m2.at<double>(0,0) = cos(ZH_YAW);
		m2.at<double>(0,1) = sin(ZH_YAW);
		m2.at<double>(0,2) = 0;

		m2.at<double>(1,0) = -sin(ZH_YAW);
		m2.at<double>(1,1) = cos(ZH_YAW);
		m2.at<double>(1,2) = 0;

		m2.at<double>(2,0) = 0;
		m2.at<double>(2,1) = 0;
		m2.at<double>(2,2) = 1;

		// 飞机俯仰角
		cv::Mat m3(3,3,CV_64FC1);
		m3.at<double>(0,0) = 1;
		m3.at<double>(0,1) = 0;
		m3.at<double>(0,2) = 0;

		m3.at<double>(1,0) = 0;
		m3.at<double>(1,1) = cos(UAV_PITCH);
		m3.at<double>(1,2) = -sin(UAV_PITCH);

		m3.at<double>(2,0) = 0;
		m3.at<double>(2,1) = sin(UAV_PITCH);
		m3.at<double>(2,2) = cos(UAV_PITCH);

		// 飞机滚转角
		cv::Mat m4(3,3,CV_64FC1);
		m4.at<double>(0,0) = cos(UAV_ROLL);
		m4.at<double>(0,1) = 0;
		m4.at<double>(0,2) = sin(UAV_ROLL);

		m4.at<double>(1,0) = 0;
		m4.at<double>(1,1) = 1;
		m4.at<double>(1,2) = 0;

		m4.at<double>(2,0) = -sin(UAV_ROLL);
		m4.at<double>(2,1) = 0;
		m4.at<double>(2,2) = cos(UAV_ROLL);

		// 飞机航向角
		cv::Mat m5(3,3,CV_64FC1);
		m5.at<double>(0,0) = cos(UAV_YAW);
		m5.at<double>(0,1) = sin(UAV_YAW);
		m5.at<double>(0,2) = 0;

		m5.at<double>(1,0) = -sin(UAV_YAW);
		m5.at<double>(1,1) = cos(UAV_YAW);
		m5.at<double>(1,2) = 0;

		m5.at<double>(2,0) = 0;
		m5.at<double>(2,1) = 0;
		m5.at<double>(2,2) = 1;


		// 矩阵乘法
		cv::Mat m54321;
		cv::gemm(m2, m1, 1, cv::noArray(), 0, m54321);
		cv::gemm(m3, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m4, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m5, m54321, 1, cv::noArray(), 0, m54321);


		// 取出M54321中的值
		double x1 = m54321.at<double>(0,0);
		double x2 = m54321.at<double>(0,1);
		double x3 = m54321.at<double>(0,2);

		double x4 = m54321.at<double>(1,0);
		double x5 = m54321.at<double>(1,1);
		double x6 = m54321.at<double>(1,2);

		double x7 = m54321.at<double>(2,0);
		double x8 = m54321.at<double>(2,1);
		double x9 = m54321.at<double>(2,2);

		// 合成高低角不能在水平线之上
		if (x9 <= 0)
		{
			return false;
		}

		// 坐标转换
		double x = 0, y = 0;

		// 像平面坐标系
		x = +pt_x - imgWidth / 2;
		y = -pt_y + imgHeight / 2;  

		// 转换为物理坐标 单位 米
		x = x * pixelSize;
		y = y * pixelSize;

		// 分母零值判断
		if (x7 * x + x8 * y - x9 * f == 0)
		{
			return false;
		}

		// 共线方程模型
		double XA = -(x1 * x + x2 * y - x3 * f) / (x7 * x + x8 * y - x9 * f) * height;
		double YA = -(x4 * x + x5 * y - x6 * f) / (x7 * x + x8 * y - x9 * f) * height;

		// 计算方位角和距离
		double distance = sqrt(XA * XA + YA * YA);
		double yaw = getYaw(XA, YA);

		// 计算像素点经纬度
		CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84坐标系

		// 充分利用激光定位相对准确，计算偏移量，作补偿
		double Lonoffset = 0.0;
		double Latoffset = 0.0;
		CalLonLatOffset(Lonoffset, Latoffset, frame);

		// 修正偏移量
		Lon += Lonoffset;
		Lat += Latoffset;

		// 目标高度计算
		H = uavGroundHeight;

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

	return true;
}


// 王家星 算法
bool SysGeoCorrectImg(ImgStru *dstImg, const ImgStru *srcImg, const struQB_FJ *frameData)
{
	try
	{
		// 空值判断
		if (dstImg == NULL || srcImg == NULL || frameData == NULL)
		{
			return false;
		}

		// 图像坐标
		std::vector<cv::Point2d> GCP_XY;
		GCP_XY.push_back(cv::Point2d(0.0,0.0));
		GCP_XY.push_back(cv::Point2d(static_cast<double>(srcImg->ImgWidth - 1),    0.0));
		GCP_XY.push_back(cv::Point2d(static_cast<double>(0),                      static_cast<double>(srcImg->ImgHeight - 1)));
		GCP_XY.push_back(cv::Point2d(static_cast<double>(srcImg->ImgWidth - 1),   static_cast<double>(srcImg->ImgHeight - 1)));

		// 经纬度坐标
		std::vector<cv::Point2d> GCP_LonLat;

		double x = 0, y = 0;
		double targetPtLon;
		double targetPtLat;
		double targetPtH;
		cv::Point2d LonLat;

		if (true == CalAnyPtCoordinate(targetPtLon, targetPtLat, targetPtH, frameData, 0, 0))
		{
			LonLat.x = targetPtLon;
			LonLat.y = targetPtLat;
			GCP_LonLat.push_back(LonLat);
		}
		else
		{
			GCP_XY.clear();
			GCP_LonLat.clear();
			return false;
		}

		if (true == CalAnyPtCoordinate(targetPtLon, targetPtLat, targetPtH, frameData, int(srcImg->ImgWidth - 1), 0))
		{
			LonLat.x = targetPtLon;
			LonLat.y = targetPtLat;
			GCP_LonLat.push_back(LonLat);
		}
		else
		{
			GCP_XY.clear();
			GCP_LonLat.clear();
			return false;
		}

		if (true == CalAnyPtCoordinate(targetPtLon, targetPtLat, targetPtH, frameData, 0, srcImg->ImgHeight - 1))
		{
			LonLat.x = targetPtLon;
			LonLat.y = targetPtLat;
			GCP_LonLat.push_back(LonLat);
		}
		else
		{
			GCP_XY.clear();
			GCP_LonLat.clear();
			return false;
		}

		if (true == CalAnyPtCoordinate(targetPtLon, targetPtLat, targetPtH, frameData, srcImg->ImgWidth - 1, srcImg->ImgHeight - 1))
		{
			LonLat.x = targetPtLon;
			LonLat.y = targetPtLat;
			GCP_LonLat.push_back(LonLat);
		}
		else
		{
			GCP_XY.clear();
			GCP_LonLat.clear();
			return false;
		}


		// 基于地面控制点的几何精校正
		return GCPGeoCorrectImg(dstImg, srcImg, GCP_XY, GCP_LonLat, 0);
	}
	catch(cv::Exception &e)
	{
		string str = e.msg;

		return false;
	}
	catch(...)
	{
		return false;
	}
}

// 系统级几何校正
bool SysGeoCorrectImg(QBStru *qbData, bool bSrcFirst)
{
	try
	{
		// 基础条件判断
		if (qbData == NULL)
		{
			return false;
		}

		// 复接数据和图像数据标识位有效性判断
		if (false == qbData->framePart.bValid || false == qbData->framePart.bValid) 
		{
			return false;
		}

		// 优先级判断
		if (bSrcFirst == true)
		{
			if (false == SysGeoCorrectImg(&qbData->image.geoImg, &qbData->image.srcImg, &qbData->framePart))
			{
				return SysGeoCorrectImg(&qbData->image.geoImg, &qbData->image.dstImg, &qbData->framePart);
			}
			else
			{
				return true;
			}
		}
		else
		{
			if (false == SysGeoCorrectImg(&qbData->image.geoImg, &qbData->image.dstImg, &qbData->framePart))
			{
				return SysGeoCorrectImg(&qbData->image.geoImg, &qbData->image.srcImg, &qbData->framePart);
			}
			else
			{
				return true;
			}
		}
	}
	catch(cv::Exception &e)
	{
		string str = e.msg;

		return false;
	}
	catch(...)
	{
		return false;
	}

	return true;
}