You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
62 lines
1.7 KiB
62 lines
1.7 KiB
#include "Arith_FrontProj.h"
|
|
#include "Arith_Utils.h"
|
|
#include "StitchStruct.h"
|
|
#include "Arith_GeoSolver.h"
|
|
Pole getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep)
|
|
{
|
|
// 投影到地面坐标，这一步可以利用前面优化成果
|
|
cv::Point2f grdPt = warpPointWithH(H, pt);
|
|
|
|
// 补深度信息并转为常用的NUE坐标系
|
|
PointXYZ grdPtXYZ = { 0 };
|
|
grdPtXYZ.X = grdPt.y;
|
|
grdPtXYZ.Y = -dep;
|
|
grdPtXYZ.Z = grdPt.x;
|
|
|
|
// 地面点转极坐标
|
|
Pole pole = getPoleFromXYZ(grdPtXYZ);
|
|
|
|
return pole;
|
|
|
|
}
|
|
|
|
cv::Point2f getImgPosFromPole(cv::Mat H_inv, Pole _pole, float dep)
|
|
{
|
|
PointXYZ virPt = getXYZFromPole(_pole);
|
|
|
|
//虚拟点投影到地面
|
|
float ratio = -dep / virPt.Y;
|
|
PointXYZ realPt = { 0 };
|
|
realPt.X = virPt.X * ratio;
|
|
realPt.Y = virPt.Y * ratio;
|
|
realPt.Z = virPt.Z * ratio;
|
|
|
|
|
|
// 转东北地
|
|
PointXYZ realPtGeo = { 0 };
|
|
realPtGeo.X = realPt.Z;
|
|
realPtGeo.Y = realPt.X;
|
|
realPtGeo.Z = -realPt.Y;
|
|
|
|
// 投影回像方
|
|
cv::Point2f px = warpPointWithH(H_inv, cv::Point2f(realPtGeo.X, realPtGeo.Y));
|
|
|
|
return px;
|
|
}
|
|
|
|
Pole getPoleFromFPan(cv::Point2f pt, FPanInfo _panPara)
|
|
{
|
|
Pole _pole = { 0 };
|
|
_pole.beta = DEGLIM(_panPara.center.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes);
|
|
_pole.alpha = DEGLIM(_panPara.center.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes);
|
|
return _pole;
|
|
}
|
|
|
|
cv::Point2f getFPanFromPole(Pole _pole, FPanInfo _panPara)
|
|
{
|
|
cv::Point2f pt = { 0 };
|
|
pt.x = DEGLIM(_pole.beta - _panPara.center.fAz) / _panPara.fAglRes + _panPara.m_pan_width / 2;
|
|
pt.y = DEGLIM(_panPara.center.fPt - _pole.alpha) / _panPara.fAglRes + _panPara.m_pan_height / 2;
|
|
return pt;
|
|
}
|