CaoYuhui
/
FH98ViewOnPlaneSoftware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
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.
NewDCCtrl
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'd09470095e'
${ noResults }
FH98ViewOnPlaneSoftware/Src/QB_ImgProcessFun/SysGeoCorrect.cpp

1206 lines
24 KiB
C++
Raw Normal View History Unescape Escape

first commit
2 years ago
#include "stdafx.h"
#include "SysGeoCorrect.h"
#include <opencv2\opencv.hpp>
using namespace cv;
// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>dll
#include "MapPrj.h"
#pragma comment( lib, "MapPrj.lib" )
// <20><><EFBFBD><EFBFBD>GCP<43>ļ<EFBFBD><C4BC>ξ<EFBFBD>У<EFBFBD><D0A3>
#include "GCPGeoCorrect.h"
// <20><>ȡ<EFBFBD>߳<EFBFBD><DFB3><EFBFBD><EFBFBD><EFBFBD>
#include "GetElevation.h"
double GetMaxValue(double* array, int count)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (array == NULL || count <= 0)
{
return DBL_MAX;
}
// ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
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
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (array == NULL || count <= 0)
{
return DBL_MIN;
}
// ȡ<><C8A1>Сֵ
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;
}
}
// <20><><EFBFBD>㷽λ<E3B7BD><CEBB>
double getYaw(double XA, double YA)
{
// <20><><EFBFBD><EFBFBD><EFBFBD>ϳɺ<CFB3><C9BA><EFBFBD><EFBFBD><EFBFBD> Yaw
double PayLoad_Yaw = 0;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{
// <20><>1<EFBFBD><31><EFBFBD><EFBFBD>
tanYaw = XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
}
else if (YA < 0 && XA > 0)
{
// <20><>2<EFBFBD><32><EFBFBD><EFBFBD>
tanYaw = -XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
PayLoad_Yaw = 180 - PayLoad_Yaw;
}
else if (YA < 0 && XA < 0)
{
// <20><>3<EFBFBD><33><EFBFBD><EFBFBD>
tanYaw = XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
PayLoad_Yaw = -180 + PayLoad_Yaw;
}
else if (YA > 0 && XA < 0)
{
// <20><>4<EFBFBD><34><EFBFBD><EFBFBD>
tanYaw = -XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
PayLoad_Yaw = -PayLoad_Yaw;
}
}
return PayLoad_Yaw;
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㾭γ<E3BEAD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEB2B9>
//<2F><><EFBFBD>룺 1. frame <20><><EFBFBD>븴<EFBFBD><EBB8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 2. pt_x<5F><78> <20><>
// 3. pt_y<5F><79> <20><>
//
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1. Lon <20><><EFBFBD><EFBFBD>
// 2. Lat<61><74> γ<><CEB3>
// 3. <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>true<75><65>ʧ<EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD>false
bool CalAnyPtCoordinateWithoutErrorCompensate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == FALSE)
{
return false;
}
// <20>жϸ<D0B6><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
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. Խ<><D4BD><EFBFBD>ж<EFBFBD>
if (abs(uavLon) > 180) // <20><><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavLat) > 90) // γ<><CEB3>
{
return false;
}
if (uavAlt < 0 || uavAlt > 10000) // <20>߶<EFBFBD>
{
return false;
}
if (uavGroundHeight < 0 || (uavAlt- uavGroundHeight <= 0) )
{
return false;
}
if (abs(uavYaw) > 360) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavPitch) > 45) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavRoll) > 45) // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
{
return false;
}
if (abs(zhAz) > 180) // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
{
return false;
}
if (zhPitch > 20 || zhPitch < -120) // if (zhPitch > -15 || zhPitch < -110)
{
return false;
}
// ͼ<><CDBC><EFBFBD>ߴ<EFBFBD>
int imgWidth = frame->ZH_ImgWidth;
int imgHeight = frame->ZH_ImgHeight;
// <20>ж<EFBFBD>
if (imgWidth <= 0 || imgHeight <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ж<EFBFBD>
if (pt_x < 0 || pt_x >= imgWidth)
{
return false;
}
if (pt_y < 0 || pt_y >= imgHeight)
{
return false;
}
// <20><><EFBFBD><EFBFBD>
double f = frame->ZH_FocalLen / 1000; // <20><>
if (f <= 0)
{
return false;
}
// <20><>Ԫ<EFBFBD>ߴ<EFBFBD> <20><>
double pixelSize = frame->ZH_PixelSize;
if (pixelSize <= 0)
{
return false;
}
// <20>ɻ<EFBFBD>
double UAV_ROLL = uavRoll; // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_PITCH = uavPitch; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_YAW = uavYaw; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غ<EFBFBD>
double ZH_PITCH = zhPitch; // <20>غɸߵͽ<DFB5> <20><><EFBFBD>Ƕȣ<C7B6>
double ZH_YAW = zhAz; // <20>غɷ<D8BA>λ<EFBFBD><CEBB> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZH_PITCH = ZH_PITCH + 90; // [0 90]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD>
double BaseLat = uavLat; // <20><><EFBFBD><EFBFBD>
double height = uavAlt - uavGroundHeight; // <20><><EFBFBD>Ը߶<D4B8> = <20><><EFBFBD>и߶<D0B8> - <20><><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
bool bLaser = frame->ZH_LaserOn;
double LaserDistance = frame->ZH_LaserDist;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (bLaser == true && LaserDistance < 100)
{
bLaser = false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
// x1 x2 x3
// x4 x5 x6
// x7 x8 x9
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// ȡ<><C8A1>M54321<32>е<EFBFBD>ֵ
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);
// <20>ϳɸߵͽDz<CDBD><C7B2><EFBFBD><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD>֮<EFBFBD><D6AE>
if (x9 <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
double x = 0, y = 0;
// <20><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
x = +pt_x - imgWidth / 2;
y = -pt_y + imgHeight / 2;
// ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>λ <20><>
x = x * pixelSize;
y = y * pixelSize;
// <20><>ĸ<EFBFBD><C4B8>ֵ<EFBFBD>ж<EFBFBD>
if (x7 * x + x8 * y - x9 * f == 0)
{
return false;
}
// <20><><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ģ<EFBFBD><C4A3>
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;
// <20><><EFBFBD>㷽λ<E3B7BD>Ǻ;<C7BA><CDBE><EFBFBD>
double distance = sqrt(XA * XA + YA * YA);
double yaw = getYaw(XA, YA);
// <20><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>ĵ㾭γ<E3BEAD><CEB3>
CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84<38><34><EFBFBD><EFBFBD>ϵ
// Ŀ<><C4BF><EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>
H = uavGroundHeight;
return true;
}
catch(cv::Exception &e)
{
e.msg.c_str();
return false;
}
catch(...)
{
return false;
}
return true;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><C3BC>⣬<EFBFBD><E2A3AC><EFBFBD>ߵ<EFBFBD><DFB5>㶨λ<E3B6A8><CEBB><EFBFBD><EFBFBD>
void CalLonLatOffset(double &LonOffset, double &LatOffset, const struQB_FJ *frame)
{
try
{
// <20><><EFBFBD><EFBFBD>
LonOffset = 0;
LatOffset = 0;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == false)
{
return;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>޼<EFBFBD><DEBC><EFBFBD> ֱ<>ӷ<EFBFBD><D3B7><EFBFBD>
if ((frame->ZH_LaserOn == true && frame->ZH_LaserDist > 0 && frame->ZH_LaserDist < 65535) == false)
{
return;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double LaserLon = 0.0;
double LaserLat = 0.0;
double LaserH = 0.0;
// <20>޼<EFBFBD><DEBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
}
// <20><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>ĵ㾭γ<E3BEAD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
bool CalImgCenterCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == FALSE)
{
return false;
}
// <20>жϸ<D0B6><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
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. Խ<><D4BD><EFBFBD>ж<EFBFBD>
if (abs(uavLon) > 180) // <20><><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavLat) > 90) // γ<><CEB3>
{
return false;
}
if (uavAlt < 0 || uavAlt > 10000) // <20>߶<EFBFBD>
{
return false;
}
if (uavGroundHeight < 0 || (uavAlt- uavGroundHeight <= 0))
{
return false;
}
if (abs(uavYaw) > 360) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavPitch) > 45) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavRoll) > 45) // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
{
return false;
}
if (abs(zhAz) > 180) // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
{
return false;
}
if (zhPitch > 20 || zhPitch < -120)
{
return false;
}
// <20>ɻ<EFBFBD>
double UAV_ROLL = uavRoll; // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_PITCH = uavPitch; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_YAW = uavYaw; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غ<EFBFBD>
double ZH_PITCH = zhPitch; // <20>غɸߵͽ<DFB5> <20><><EFBFBD>Ƕȣ<C7B6>
double ZH_YAW = zhAz; // <20>غɷ<D8BA>λ<EFBFBD><CEBB> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZH_PITCH = ZH_PITCH + 90; // [0 90]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD>
double BaseLat = uavLat; // <20><><EFBFBD><EFBFBD>
double height = uavAlt - uavGroundHeight; // <20><><EFBFBD>Ը߶<D4B8> = <20><><EFBFBD>и߶<D0B8> - <20><><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
bool bLaser = frame->ZH_LaserOn;
double LaserDistance = frame->ZH_LaserDist;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (bLaser == true && LaserDistance < 100)
{
bLaser = false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
// x1 x2 x3
// x4 x5 x6
// x7 x8 x9
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// ȡ<><C8A1>M54321<32>е<EFBFBD>ֵ
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);
// <20>ϳɸߵͽDz<CDBD><C7B2><EFBFBD>λ<EFBFBD><CEBB>ˮƽ<CBAE><C6BD>֮<EFBFBD><D6AE>
if (x9 <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ׼
if (bLaser == true && LaserDistance < 65535) // 65535 = 0xFFFF <20><>Чֵ
{
height = LaserDistance * x9;
}
double XA = -(x3) / (x9) * height;
double YA = -(x6) / (x9) * height;
// <20><><EFBFBD><EFBFBD><EFBFBD>ϳɺ<CFB3><C9BA><EFBFBD><EFBFBD><EFBFBD> Yaw
double PayLoad_Yaw = 0;
PayLoad_Yaw = getYaw(XA, YA);
// <20>ߵͽ<DFB5> <20>ϳ<EFBFBD>
double PayLoad_Pitch = acos(x9) * 180 / CV_PI;
// <20><><EFBFBD><EFBFBD><EFBFBD>εױ<CEB5>
double distance = height * tan(PayLoad_Pitch / 180.0 * CV_PI);
// <20><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ׼
if (bLaser == true && LaserDistance < 65535)
{
// <20><><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺϳɽǶ<C9BD> <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>
distance = LaserDistance * sin(PayLoad_Pitch / 180.0 * CV_PI);
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD>γ<EFBFBD><CEB3>
CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, PayLoad_Yaw, distance, 3); // WGS84<38><34><EFBFBD><EFBFBD>ϵ
// Ŀ<><C4BF><EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>
H = uavGroundHeight;
// <20><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㾭γ<E3BEAD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>룺 1. frame <20><><EFBFBD>븴<EFBFBD><EBB8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 2. pt_x<5F><78> <20><>
// 3. pt_y<5F><79> <20><>
//
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1. Lon <20><><EFBFBD><EFBFBD>
// 2. Lat<61><74> γ<><CEB3>
// 3. <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>true<75><65>ʧ<EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD>false
bool CalAnyPtCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == FALSE)
{
return false;
}
// <20>жϸ<D0B6><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
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. Խ<><D4BD><EFBFBD>ж<EFBFBD>
if (abs(uavLon) > 180) // <20><><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavLat) > 90) // γ<><CEB3>
{
return false;
}
if (uavAlt < 0 || uavAlt > 10000) // <20>߶<EFBFBD>
{
return false;
}
if (uavGroundHeight < 0 || (uavAlt- uavGroundHeight <= 0))
{
return false;
}
if (abs(uavYaw) > 360) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavPitch) > 45) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavRoll) > 45) // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
{
return false;
}
if (abs(zhAz) > 180) // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
{
return false;
}
if (zhPitch > 20 || zhPitch < -120)
{
return false;
}
// ͼ<><CDBC><EFBFBD>ߴ<EFBFBD>
int imgWidth = frame->ZH_ImgWidth;
int imgHeight = frame->ZH_ImgHeight;
// <20>ж<EFBFBD>
if (imgWidth <= 0 || imgHeight <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ж<EFBFBD>
if (pt_x < 0 || pt_x >= imgWidth)
{
return false;
}
if (pt_y < 0 || pt_y >= imgHeight)
{
return false;
}
// <20><><EFBFBD><EFBFBD>
double f = frame->ZH_FocalLen / 1000; // <20><>
if (f <= 0)
{
return false;
}
// <20><>Ԫ<EFBFBD>ߴ<EFBFBD> <20><>
double pixelSize = frame->ZH_PixelSize;
if (pixelSize <= 0)
{
return false;
}
// <20>ɻ<EFBFBD>
double UAV_ROLL = uavRoll; // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_PITCH = uavPitch; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_YAW = uavYaw; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غ<EFBFBD>
double ZH_PITCH = zhPitch; // <20>غɸߵͽ<DFB5> <20><><EFBFBD>Ƕȣ<C7B6>
double ZH_YAW = zhAz; // <20>غɷ<D8BA>λ<EFBFBD><CEBB> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZH_PITCH = ZH_PITCH + 90; // [0 90]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD>
double BaseLat = uavLat; // <20><><EFBFBD><EFBFBD>
double height = uavAlt - uavGroundHeight; // <20><><EFBFBD>Ը߶<D4B8> = <20><><EFBFBD>и߶<D0B8> - <20><><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
bool bLaser = frame->ZH_LaserOn;
double LaserDistance = frame->ZH_LaserDist;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (bLaser == true && LaserDistance < 100)
{
bLaser = false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
// x1 x2 x3
// x4 x5 x6
// x7 x8 x9
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// ȡ<><C8A1>M54321<32>е<EFBFBD>ֵ
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);
// <20>ϳɸߵͽDz<CDBD><C7B2><EFBFBD><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD>֮<EFBFBD><D6AE>
if (x9 <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
double x = 0, y = 0;
// <20><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
x = +pt_x - imgWidth / 2;
y = -pt_y + imgHeight / 2;
// ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>λ <20><>
x = x * pixelSize;
y = y * pixelSize;
// <20><>ĸ<EFBFBD><C4B8>ֵ<EFBFBD>ж<EFBFBD>
if (x7 * x + x8 * y - x9 * f == 0)
{
return false;
}
// <20><><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ģ<EFBFBD><C4A3>
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;
// <20><><EFBFBD>㷽λ<E3B7BD>Ǻ;<C7BA><CDBE><EFBFBD>
double distance = sqrt(XA * XA + YA * YA);
double yaw = getYaw(XA, YA);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص㾭γ<E3BEAD><CEB3>
CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84<38><34><EFBFBD><EFBFBD>ϵ
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><C3BC>ⶨλ<E2B6A8><CEBB><EFBFBD><EFBFBD>׼ȷ<D7BC><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double Lonoffset = 0.0;
double Latoffset = 0.0;
CalLonLatOffset(Lonoffset, Latoffset, frame);
// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
Lon += Lonoffset;
Lat += Latoffset;
// Ŀ<><C4BF><EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>
H = uavGroundHeight;
return true;
}
catch(cv::Exception &e)
{
e.msg.c_str();
return false;
}
catch(...)
{
return false;
}
return true;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>㷨
bool SysGeoCorrectImg(ImgStru *dstImg, const ImgStru *srcImg, const struQB_FJ *frameData)
{
try
{
// <20><>ֵ<EFBFBD>ж<EFBFBD>
if (dstImg == NULL || srcImg == NULL || frameData == NULL)
{
return false;
}
// ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)));
// <20><>γ<EFBFBD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
// <20><><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>ļ<EFBFBD><C4BC>ξ<EFBFBD>У<EFBFBD><D0A3>
return GCPGeoCorrectImg(dstImg, srcImg, GCP_XY, GCP_LonLat, 0);
}
catch(cv::Exception &e)
{
string str = e.msg;
return false;
}
catch(...)
{
return false;
}
}
// ϵͳ<CFB5><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3>
bool SysGeoCorrectImg(QBStru *qbData, bool bSrcFirst)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (qbData == NULL)
{
return false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݺ<EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD>ʶλ<CAB6><CEBB>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (false == qbData->framePart.bValid || false == qbData->framePart.bValid)
{
return false;
}
// <20><><EFBFBD>ȼ<EFBFBD><C8BC>ж<EFBFBD>
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;
}