You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

1206 lines
24 KiB
C++

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

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