#include "stdafx.h" #include "SysGeoCorrect.h" #include 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(0,0) = 1; m1.at(0,1) = 0; m1.at(0,2) = 0; m1.at(1,0) = 0; m1.at(1,1) = cos(ZH_PITCH); m1.at(1,2) = -sin(ZH_PITCH); m1.at(2,0) = 0; m1.at(2,1) = sin(ZH_PITCH); m1.at(2,2) = cos(ZH_PITCH); // 载荷方位角 cv::Mat m2(3,3,CV_64FC1); m2.at(0,0) = cos(ZH_YAW); m2.at(0,1) = sin(ZH_YAW); m2.at(0,2) = 0; m2.at(1,0) = -sin(ZH_YAW); m2.at(1,1) = cos(ZH_YAW); m2.at(1,2) = 0; m2.at(2,0) = 0; m2.at(2,1) = 0; m2.at(2,2) = 1; // 飞机俯仰角 cv::Mat m3(3,3,CV_64FC1); m3.at(0,0) = 1; m3.at(0,1) = 0; m3.at(0,2) = 0; m3.at(1,0) = 0; m3.at(1,1) = cos(UAV_PITCH); m3.at(1,2) = -sin(UAV_PITCH); m3.at(2,0) = 0; m3.at(2,1) = sin(UAV_PITCH); m3.at(2,2) = cos(UAV_PITCH); // 飞机滚转角 cv::Mat m4(3,3,CV_64FC1); m4.at(0,0) = cos(UAV_ROLL); m4.at(0,1) = 0; m4.at(0,2) = sin(UAV_ROLL); m4.at(1,0) = 0; m4.at(1,1) = 1; m4.at(1,2) = 0; m4.at(2,0) = -sin(UAV_ROLL); m4.at(2,1) = 0; m4.at(2,2) = cos(UAV_ROLL); // 飞机航向角 cv::Mat m5(3,3,CV_64FC1); m5.at(0,0) = cos(UAV_YAW); m5.at(0,1) = sin(UAV_YAW); m5.at(0,2) = 0; m5.at(1,0) = -sin(UAV_YAW); m5.at(1,1) = cos(UAV_YAW); m5.at(1,2) = 0; m5.at(2,0) = 0; m5.at(2,1) = 0; m5.at(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(0,0); double x2 = m54321.at(0,1); double x3 = m54321.at(0,2); double x4 = m54321.at(1,0); double x5 = m54321.at(1,1); double x6 = m54321.at(1,2); double x7 = m54321.at(2,0); double x8 = m54321.at(2,1); double x9 = m54321.at(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(0,0) = 1; m1.at(0,1) = 0; m1.at(0,2) = 0; m1.at(1,0) = 0; m1.at(1,1) = cos(ZH_PITCH); m1.at(1,2) = -sin(ZH_PITCH); m1.at(2,0) = 0; m1.at(2,1) = sin(ZH_PITCH); m1.at(2,2) = cos(ZH_PITCH); // 载荷方位角 cv::Mat m2(3,3,CV_64FC1); m2.at(0,0) = cos(ZH_YAW); m2.at(0,1) = sin(ZH_YAW); m2.at(0,2) = 0; m2.at(1,0) = -sin(ZH_YAW); m2.at(1,1) = cos(ZH_YAW); m2.at(1,2) = 0; m2.at(2,0) = 0; m2.at(2,1) = 0; m2.at(2,2) = 1; // 飞机俯仰角 cv::Mat m3(3,3,CV_64FC1); m3.at(0,0) = 1; m3.at(0,1) = 0; m3.at(0,2) = 0; m3.at(1,0) = 0; m3.at(1,1) = cos(UAV_PITCH); m3.at(1,2) = -sin(UAV_PITCH); m3.at(2,0) = 0; m3.at(2,1) = sin(UAV_PITCH); m3.at(2,2) = cos(UAV_PITCH); // 飞机滚转角 cv::Mat m4(3,3,CV_64FC1); m4.at(0,0) = cos(UAV_ROLL); m4.at(0,1) = 0; m4.at(0,2) = sin(UAV_ROLL); m4.at(1,0) = 0; m4.at(1,1) = 1; m4.at(1,2) = 0; m4.at(2,0) = -sin(UAV_ROLL); m4.at(2,1) = 0; m4.at(2,2) = cos(UAV_ROLL); // 飞机航向角 cv::Mat m5(3,3,CV_64FC1); m5.at(0,0) = cos(UAV_YAW); m5.at(0,1) = sin(UAV_YAW); m5.at(0,2) = 0; m5.at(1,0) = -sin(UAV_YAW); m5.at(1,1) = cos(UAV_YAW); m5.at(1,2) = 0; m5.at(2,0) = 0; m5.at(2,1) = 0; m5.at(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(0,0); double x2 = m54321.at(0,1); double x3 = m54321.at(0,2); double x4 = m54321.at(1,0); double x5 = m54321.at(1,1); double x6 = m54321.at(1,2); double x7 = m54321.at(2,0); double x8 = m54321.at(2,1); double x9 = m54321.at(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(0,0) = 1; m1.at(0,1) = 0; m1.at(0,2) = 0; m1.at(1,0) = 0; m1.at(1,1) = cos(ZH_PITCH); m1.at(1,2) = -sin(ZH_PITCH); m1.at(2,0) = 0; m1.at(2,1) = sin(ZH_PITCH); m1.at(2,2) = cos(ZH_PITCH); // 载荷方位角 cv::Mat m2(3,3,CV_64FC1); m2.at(0,0) = cos(ZH_YAW); m2.at(0,1) = sin(ZH_YAW); m2.at(0,2) = 0; m2.at(1,0) = -sin(ZH_YAW); m2.at(1,1) = cos(ZH_YAW); m2.at(1,2) = 0; m2.at(2,0) = 0; m2.at(2,1) = 0; m2.at(2,2) = 1; // 飞机俯仰角 cv::Mat m3(3,3,CV_64FC1); m3.at(0,0) = 1; m3.at(0,1) = 0; m3.at(0,2) = 0; m3.at(1,0) = 0; m3.at(1,1) = cos(UAV_PITCH); m3.at(1,2) = -sin(UAV_PITCH); m3.at(2,0) = 0; m3.at(2,1) = sin(UAV_PITCH); m3.at(2,2) = cos(UAV_PITCH); // 飞机滚转角 cv::Mat m4(3,3,CV_64FC1); m4.at(0,0) = cos(UAV_ROLL); m4.at(0,1) = 0; m4.at(0,2) = sin(UAV_ROLL); m4.at(1,0) = 0; m4.at(1,1) = 1; m4.at(1,2) = 0; m4.at(2,0) = -sin(UAV_ROLL); m4.at(2,1) = 0; m4.at(2,2) = cos(UAV_ROLL); // 飞机航向角 cv::Mat m5(3,3,CV_64FC1); m5.at(0,0) = cos(UAV_YAW); m5.at(0,1) = sin(UAV_YAW); m5.at(0,2) = 0; m5.at(1,0) = -sin(UAV_YAW); m5.at(1,1) = cos(UAV_YAW); m5.at(1,2) = 0; m5.at(2,0) = 0; m5.at(2,1) = 0; m5.at(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(0,0); double x2 = m54321.at(0,1); double x3 = m54321.at(0,2); double x4 = m54321.at(1,0); double x5 = m54321.at(1,1); double x6 = m54321.at(1,2); double x7 = m54321.at(2,0); double x8 = m54321.at(2,1); double x9 = m54321.at(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 GCP_XY; GCP_XY.push_back(cv::Point2d(0.0,0.0)); GCP_XY.push_back(cv::Point2d(static_cast(srcImg->ImgWidth - 1), 0.0)); GCP_XY.push_back(cv::Point2d(static_cast(0), static_cast(srcImg->ImgHeight - 1))); GCP_XY.push_back(cv::Point2d(static_cast(srcImg->ImgWidth - 1), static_cast(srcImg->ImgHeight - 1))); // 经纬度坐标 std::vector 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; }