|
|
#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;
|
|
|
} |