|
|
|
|
@ -3,9 +3,8 @@
|
|
|
|
|
#include "Arith_Utils.h"
|
|
|
|
|
#include "Arith_CoordModule.h"
|
|
|
|
|
#include "Arith_SysStruct.h"
|
|
|
|
|
#define GLOG_USE_GLOG_EXPORT
|
|
|
|
|
#include "ceres/ceres.h"
|
|
|
|
|
#include "ceres/rotation.h"
|
|
|
|
|
//#define GLOG_USE_GLOG_EXPORT
|
|
|
|
|
|
|
|
|
|
using namespace cv;
|
|
|
|
|
|
|
|
|
|
GeoSolver::GeoSolver()
|
|
|
|
|
@ -82,36 +81,36 @@ cv::Point2f GeoSolver::getGeoFromBLH(PointBLH ptPos)
|
|
|
|
|
return ptInGeo;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
cv::Point2f GeoSolver::Trans_uv2Geo(cv::Point2f pos_frame, TForm form)
|
|
|
|
|
{
|
|
|
|
|
Mat point = (Mat_<double>(3, 1) << pos_frame.x, pos_frame.y, 1);
|
|
|
|
|
Mat result = form.R * point;
|
|
|
|
|
// 转局部地理系
|
|
|
|
|
double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
|
|
|
|
|
double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
|
|
|
|
|
// 平移到原点地理系
|
|
|
|
|
warpedX += form.T.at<double>(0, 0);
|
|
|
|
|
warpedY += form.T.at<double>(1, 0);
|
|
|
|
|
|
|
|
|
|
return cv::Point2f(warpedX, warpedY);
|
|
|
|
|
}
|
|
|
|
|
// cv::Point2f GeoSolver::Trans_uv2Geo(cv::Point2f pos_frame, TForm form)
|
|
|
|
|
// {
|
|
|
|
|
// Mat point = (Mat_<double>(3, 1) << pos_frame.x, pos_frame.y, 1);
|
|
|
|
|
// Mat result = form.R * point;
|
|
|
|
|
// // 转局部地理系
|
|
|
|
|
// double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
|
|
|
|
|
// double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
|
|
|
|
|
// // 平移到原点地理系
|
|
|
|
|
// warpedX += form.T.at<double>(0, 0);
|
|
|
|
|
// warpedY += form.T.at<double>(1, 0);
|
|
|
|
|
|
|
|
|
|
// return cv::Point2f(warpedX, warpedY);
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
cv::Point2f GeoSolver::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv)
|
|
|
|
|
{
|
|
|
|
|
// 先平移到当前相机位置
|
|
|
|
|
cv::Point2f pos_cam = pos_geo;
|
|
|
|
|
pos_cam.x = pos_geo.x + form_inv.T.at<double>(0, 0);
|
|
|
|
|
pos_cam.y = pos_geo.y + form_inv.T.at<double>(1, 0);
|
|
|
|
|
// cv::Point2f GeoSolver::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv)
|
|
|
|
|
// {
|
|
|
|
|
// // 先平移到当前相机位置
|
|
|
|
|
// cv::Point2f pos_cam = pos_geo;
|
|
|
|
|
// pos_cam.x = pos_geo.x + form_inv.T.at<double>(0, 0);
|
|
|
|
|
// pos_cam.y = pos_geo.y + form_inv.T.at<double>(1, 0);
|
|
|
|
|
|
|
|
|
|
Mat point = (Mat_<double>(3, 1) << pos_cam.x, pos_cam.y, 1);
|
|
|
|
|
Mat result = form_inv.R * point;
|
|
|
|
|
// Mat point = (Mat_<double>(3, 1) << pos_cam.x, pos_cam.y, 1);
|
|
|
|
|
// Mat result = form_inv.R * point;
|
|
|
|
|
|
|
|
|
|
// 转像方
|
|
|
|
|
double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
|
|
|
|
|
double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
|
|
|
|
|
// // 转像方
|
|
|
|
|
// double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
|
|
|
|
|
// double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
|
|
|
|
|
|
|
|
|
|
return cv::Point2f(warpedX, warpedY);
|
|
|
|
|
}
|
|
|
|
|
// return cv::Point2f(warpedX, warpedY);
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Mat GeoSolver::Mat_TransENGMove(FrameInfo info)
|
|
|
|
|
@ -180,6 +179,8 @@ cv::Mat GeoSolver::Mat_TransENG2Cam(FrameInfo info)
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float alaph = info.servoInfo.fServoPt;
|
|
|
|
|
|
|
|
|
|
Mat M_alaph = (Mat_<double>(3, 3) << 1, 0, 0,
|
|
|
|
|
@ -223,21 +224,43 @@ cv::Mat TransRtKToH(RtK rtk, double Depth)
|
|
|
|
|
0, 0, Depth
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
cv::Mat R_p2g = (rtk.K * rtk.R * M_het).inv();
|
|
|
|
|
//cv::Mat R_p2g = R_g2p.inv();
|
|
|
|
|
cv::Mat t_MAT = rtk.T;
|
|
|
|
|
// cv::Mat R_p2g = (rtk.K * rtk.R * M_het).inv();
|
|
|
|
|
// //cv::Mat R_p2g = R_g2p.inv();
|
|
|
|
|
// cv::Mat t_MAT = rtk.T;
|
|
|
|
|
|
|
|
|
|
// double R[9] = { 0 };
|
|
|
|
|
// double T[3] = { 0 };
|
|
|
|
|
|
|
|
|
|
// memcpy(R, R_p2g.data, sizeof(double) * 9);
|
|
|
|
|
// memcpy(T, t_MAT.data, sizeof(double) * 3);
|
|
|
|
|
|
|
|
|
|
// Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
|
|
|
|
|
// (R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
|
|
|
|
|
// R[6], R[7], R[8]);
|
|
|
|
|
|
|
|
|
|
// // 归一化
|
|
|
|
|
// H = H / H.at<double>(2, 2);
|
|
|
|
|
|
|
|
|
|
// 地面法向量 n_w(向上方向,Z轴正方向,在地理坐标系中)
|
|
|
|
|
cv::Mat n_w = (cv::Mat_<double>(3, 1) << 0, 0, 1);
|
|
|
|
|
|
|
|
|
|
// 平面到相机的距离(有效高度)
|
|
|
|
|
double d = Depth;
|
|
|
|
|
|
|
|
|
|
cv::Mat I = cv::Mat::eye(3, 3, CV_64F);
|
|
|
|
|
|
|
|
|
|
// 计算 (T_geo * n_w^T) / d
|
|
|
|
|
cv::Mat TnT = (rtk.R * rtk.T *n_w.t()) / d;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double R[9] = { 0 };
|
|
|
|
|
double T[3] = { 0 };
|
|
|
|
|
|
|
|
|
|
memcpy(R, R_p2g.data, sizeof(double) * 9);
|
|
|
|
|
memcpy(T, t_MAT.data, sizeof(double) * 3);
|
|
|
|
|
|
|
|
|
|
Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
|
|
|
|
|
(R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
|
|
|
|
|
R[6], R[7], R[8]);
|
|
|
|
|
// 计算从地理坐标到像素坐标的单应性矩阵
|
|
|
|
|
cv::Mat H_geo_to_pixel = rtk.K * (rtk.R - TnT) * M_het;
|
|
|
|
|
|
|
|
|
|
// 归一化
|
|
|
|
|
cv::Mat H = H_geo_to_pixel.inv();
|
|
|
|
|
|
|
|
|
|
// 归一化(使 H[2,2] = 1)
|
|
|
|
|
H = H / H.at<double>(2, 2);
|
|
|
|
|
|
|
|
|
|
return H.clone();
|
|
|
|
|
@ -264,83 +287,69 @@ cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 检验H的计算
|
|
|
|
|
cv::Mat GeoSolver::findHomography2(FrameInfo info)
|
|
|
|
|
{
|
|
|
|
|
TForm tf_p2g;
|
|
|
|
|
tf_p2g.R = Mat_TransENG2uv(info).inv();
|
|
|
|
|
tf_p2g.T = Mat_TransENGMove(info);
|
|
|
|
|
|
|
|
|
|
std::vector<cv::Point2f> srcPoints;
|
|
|
|
|
srcPoints.push_back(cv::Point2f(0, 0));
|
|
|
|
|
srcPoints.push_back(cv::Point2f(1000, 0));
|
|
|
|
|
srcPoints.push_back(cv::Point2f(1000, 1000));
|
|
|
|
|
srcPoints.push_back(cv::Point2f(0, 1000));
|
|
|
|
|
|
|
|
|
|
// 同名点计算,从像方到全景
|
|
|
|
|
cv::Point2f leftTop_map = Trans_uv2Geo(srcPoints[0], tf_p2g);
|
|
|
|
|
cv::Point2f rightTop_map = Trans_uv2Geo(srcPoints[1], tf_p2g);
|
|
|
|
|
cv::Point2f rightBottom_map = Trans_uv2Geo(srcPoints[2], tf_p2g);
|
|
|
|
|
cv::Point2f leftBottom_map = Trans_uv2Geo(srcPoints[3], tf_p2g);
|
|
|
|
|
|
|
|
|
|
// 目标图像(全景图)的四个顶点坐标
|
|
|
|
|
std::vector<cv::Point2f> dstPoints;
|
|
|
|
|
dstPoints.push_back(leftTop_map); // 左
|
|
|
|
|
dstPoints.push_back(rightTop_map); // 右上
|
|
|
|
|
dstPoints.push_back(rightBottom_map); // 右下
|
|
|
|
|
dstPoints.push_back(leftBottom_map); // 左下
|
|
|
|
|
|
|
|
|
|
// 计算单应性矩阵 H
|
|
|
|
|
cv::Mat H = cv::findHomography(srcPoints, dstPoints);
|
|
|
|
|
return H;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 根据R和t,解析H
|
|
|
|
|
cv::Mat GeoSolver::findHomography(FrameInfo info)
|
|
|
|
|
{
|
|
|
|
|
TForm tf_p2g;
|
|
|
|
|
tf_p2g.R = Mat_TransENG2uv(info).inv();
|
|
|
|
|
tf_p2g.T = Mat_TransENGMove(info);
|
|
|
|
|
// cv::Mat GeoSolver::findHomography(FrameInfo info)
|
|
|
|
|
// {
|
|
|
|
|
// cv::Mat Rmat = Mat_TransENG2uv(info).inv();
|
|
|
|
|
// cv::Mat Tmat = Mat_TransENGMove(info);
|
|
|
|
|
|
|
|
|
|
double R[9] = { 0 };
|
|
|
|
|
double T[3] = { 0 };
|
|
|
|
|
// double R[9] = { 0 };
|
|
|
|
|
// double T[3] = { 0 };
|
|
|
|
|
|
|
|
|
|
memcpy(R, tf_p2g.R.data, sizeof(double) * 9);
|
|
|
|
|
memcpy(T, tf_p2g.T.data, sizeof(double) * 3);
|
|
|
|
|
// memcpy(R, Rmat.data, sizeof(double) * 9);
|
|
|
|
|
// memcpy(T, Tmat.data, sizeof(double) * 3);
|
|
|
|
|
|
|
|
|
|
Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
|
|
|
|
|
(R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
|
|
|
|
|
R[6], R[7], R[8]);
|
|
|
|
|
// Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
|
|
|
|
|
// (R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
|
|
|
|
|
// R[6], R[7], R[8]);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 归一化
|
|
|
|
|
H = H / H.at<double>(2, 2);
|
|
|
|
|
// // 归一化
|
|
|
|
|
// H = H / H.at<double>(2, 2);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return H;
|
|
|
|
|
}
|
|
|
|
|
// return H;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
cv::Mat GeoSolver::findHomography_GPT(FrameInfo info)
|
|
|
|
|
cv::Mat GeoSolver::findHomography(FrameInfo info)
|
|
|
|
|
{
|
|
|
|
|
// world: Z=0 plane
|
|
|
|
|
cv::Mat R = Mat_TransENG2uv(info).inv(); // 3x3
|
|
|
|
|
cv::Mat t = Mat_TransENGMove(info); // 3x1
|
|
|
|
|
// 获取相机内参矩阵 K
|
|
|
|
|
cv::Mat Kmat = Mat_GetCamK(info);
|
|
|
|
|
|
|
|
|
|
// ground plane normal in world coord
|
|
|
|
|
cv::Mat n_w = (cv::Mat_<double>(3,1) << 0, 0, 1);
|
|
|
|
|
// 获取从地理坐标系到相机坐标系的旋转矩阵 R_geo_to_cam
|
|
|
|
|
cv::Mat R_geo_to_cam = Mat_TransENG2Cam(info);
|
|
|
|
|
|
|
|
|
|
// normal in camera coord
|
|
|
|
|
cv::Mat n = R * n_w; // 3x1
|
|
|
|
|
// 获取平移向量 T(从原点地理坐标系到当前相机位置地理坐标系)
|
|
|
|
|
cv::Mat T_geo = Mat_TransENGMove(info);
|
|
|
|
|
|
|
|
|
|
// distance to ground plane
|
|
|
|
|
// 深度矩阵
|
|
|
|
|
Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
|
|
|
|
|
0, 1, 0,
|
|
|
|
|
0, 0, info.nEvHeight
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
// 地面法向量 n_w(向上方向,Z轴正方向,在地理坐标系中)
|
|
|
|
|
cv::Mat n_w = (cv::Mat_<double>(3, 1) << 0, 0, 1);
|
|
|
|
|
|
|
|
|
|
// 平面到相机的距离(有效高度)
|
|
|
|
|
double d = info.nEvHeight;
|
|
|
|
|
|
|
|
|
|
cv::Mat I = cv::Mat::eye(3, 3, CV_64F);
|
|
|
|
|
|
|
|
|
|
cv::Mat H = R - (t * n.t()) / d;
|
|
|
|
|
// 计算 (T_geo * n_w^T) / d
|
|
|
|
|
cv::Mat TnT = (R_geo_to_cam * T_geo *n_w.t()) / d;
|
|
|
|
|
|
|
|
|
|
H /= H.at<double>(2,2);
|
|
|
|
|
return H;
|
|
|
|
|
|
|
|
|
|
// 计算从地理坐标到像素坐标的单应性矩阵
|
|
|
|
|
cv::Mat H_geo_to_pixel = Kmat * (R_geo_to_cam - TnT) * M_het;
|
|
|
|
|
|
|
|
|
|
cv::Mat H = H_geo_to_pixel.inv();
|
|
|
|
|
|
|
|
|
|
// 归一化(使 H[2,2] = 1)
|
|
|
|
|
H = H / H.at<double>(2, 2);
|
|
|
|
|
|
|
|
|
|
return H.clone();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|