实现se3优化和H优化并存

CMakeSettings.json

@@ -0,0 +1,27 @@
+{
+  "configurations": [
+    {
+      "name": "x64-Debug",
+      "generator": "Ninja",
+      "configurationType": "Debug",
+      "inheritEnvironments": [ "msvc_x64_x64" ],
+      "buildRoot": "${projectDir}\\out\\build\\${name}",
+      "installRoot": "${projectDir}\\out\\install\\${name}",
+      "cmakeCommandArgs": "",
+      "buildCommandArgs": "",
+      "ctestCommandArgs": ""
+    },
+    {
+      "name": "x64-Release",
+      "generator": "Ninja",
+      "configurationType": "RelWithDebInfo",
+      "buildRoot": "${projectDir}\\out\\build\\${name}",
+      "installRoot": "${projectDir}\\out\\install\\${name}",
+      "cmakeCommandArgs": "",
+      "buildCommandArgs": "",
+      "ctestCommandArgs": "",
+      "inheritEnvironments": [ "msvc_x64_x64" ],
+      "variables": []
+    }
+  ]
+}

stitch/CMakeLists.txt

@@ -2,11 +2,15 @@
 string(TIMESTAMP COMPILE_TIME 20%y_%m_%d-%H.%M.%S)
 set(build_time ${COMPILE_TIME})
 
+
 # 获取当前版本信息
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/src/Version.h.in ${CMAKE_CURRENT_SOURCE_DIR}/src/Version.h)
 
 add_definitions(-D_SILENCE_EXPERIMENTAL_FILESYSTEM_DEPRECATION_WARNING)
 
+add_definitions(-DGLOG_USE_GLOG_EXPORT)
+add_definitions(-DGLOG_NO_ABBREVIATED_SEVERITIES)
+
 
 # cuda 加速支持
 SET(ENABLE_CUDA TRUE)

stitch/src/Arith_BATask.cpp

@@ -1,6 +1,6 @@
 #include "Arith_BATask.h"
-#define GLOG_USE_GLOG_EXPORT
 #include "ceres/ceres.h"
+#include <ceres/rotation.h>
 #include "Arith_GeoSolver.h"
 #include "math.h"
 #include "Arith_Utils.h"
@@ -120,6 +120,103 @@ private:
 
 };
 
+// SE3群优化
+struct SE3Residual
+{
+    SE3Residual(cv::KeyPoint keypoint_i, cv::KeyPoint keypoint_j,cv::Mat _K, double _fEvHeight)
+        : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),K(_K),EvHeight(_fEvHeight)
+    {
+        int a = 0;
+    }
+
+    template <typename T>
+    bool operator()(const T* const se3_i, const T* const se3_j,  T* residual) const
+    {
+        // 左图投影到物方
+       // T ptL[3] = {T(keypoint_i_.pt.x), T(keypoint_i_.pt.y)}; // 原始点
+
+
+        // 深度矩阵
+        Eigen::Matrix<T, 3, 3> M_het_inv;
+        M_het_inv << T(1), T(0), T(0),
+            T(0), T(1), T(0),
+            T(0), T(0), T(1.0 / EvHeight);
+
+        // K-1
+        Eigen::Matrix<T, 3, 3> K_1;
+        Eigen::Matrix<T, 3, 3> K_inv;
+
+        double fx = K.at<double>(0, 0);
+        double fy = K.at<double>(1, 1);
+        double cx = K.at<double>(0, 2);
+        double cy = K.at<double>(1, 2);
+
+        K_1 << T(fx), T(0), T(cx),
+            T(0), T(fy), T(cy),
+            T(0), T(0), T(1);
+
+        K_inv << T(1) / T(fx), T(0), -T(cx) / T(fx),
+            T(0), T(1) / T(fy), -T(cy) / T(fy),
+            T(0), T(0), T(1);
+
+        Eigen::Matrix<T, 3, 1> ptL(T(keypoint_i_.pt.x), T(keypoint_i_.pt.y), T(1.0));
+
+
+        // 投影也可以用这个
+        //cv::Point2f warpPL = warpPointWithse3((const double*)se3_i, (const double)EvHeight, K, ptL);
+        Eigen::Matrix<T, 3, 1> pt = K_inv * ptL;
+        Eigen::Matrix<T, 3, 1> LeftRota;
+
+        T rvec_i[3] = { -se3_i[0], -se3_i[1], -se3_i[2] };
+        ceres::AngleAxisRotatePoint(rvec_i, pt.data(), LeftRota.data());
+        Eigen::Matrix<T, 3, 1> result = M_het_inv * LeftRota;
+        // 恢复尺度
+        T GeoX = result[0] / result[2];
+        T GeoY = result[1] / result[2];
+
+        // 平移，获得地理坐标
+        GeoX += se3_i[3];
+        GeoY += se3_i[4];
+
+        // 正算，投影到右图
+        GeoX += -se3_j[3];
+        GeoY += -se3_j[4];
+
+        T rvec_j[3] = { se3_j[0], se3_j[1], se3_j[2] };
+
+        Eigen::Matrix<T, 3, 1> pGeoR(GeoX, GeoY, T(EvHeight));
+        Eigen::Matrix<T, 3, 1> RightRota;
+
+        ceres::AngleAxisRotatePoint(rvec_j, pGeoR.data(), RightRota.data());
+
+        // 投影到右图像方
+        Eigen::Matrix<T, 3, 1> pUVR = K_1* RightRota;
+
+        // 压缩尺度
+        T uR = pUVR[0] / pUVR[2];
+        T vR = pUVR[1] / pUVR[2];
+
+
+         // 计算重投影误差
+         residual[0] = uR - T(keypoint_j_.pt.x);
+         residual[1] = vR - T(keypoint_j_.pt.y);
+
+
+        return true;
+    }
+private:
+    const cv::KeyPoint keypoint_i_; // 第 i 帧图像中的特征点
+    const cv::KeyPoint keypoint_j_; // 第 j 帧图像中的特征点
+    cv::Mat_<double> K;  //内参
+    double EvHeight;//高差，用于三角化
+};
+
+
+
+
+
+
+
 
 BA_Task::BA_Task(FileCache<FrameCache>* cache)
 {
@@ -144,6 +241,11 @@ void BA_Task::InitTask()
     _FeaPtVec.clear();
     _FeaDespVec.clear();
     _paraVec.clear();
+
+    _origRtK.clear();
+    _currRtK.clear();
+    _origse3.clear();
+    _currse3.clear();
 }
 
 
@@ -168,8 +270,20 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
     {
         h_list[i] = (double*)_currMatrix[i].data;
     }
+
+
+    // 整合为6D参数
+    std::vector<double*> se3_list(_currse3.size());
+    for (int i = 0; i < _currse3.size(); i++)
+    {
+        se3_list[i] = (double*)_currse3[i].data;
+    }
+
+    constK = cv::Mat(3, 3, CV_64FC1, ((double*)_origRtK[0].data) + 12);
+
     // 创建 Ceres 问题
-    ceres::Problem problem;
+    ceres::Problem problemH;
+    ceres::Problem problemSE3;
 
     // 添加残差块
     int nParaCnt = 0;//参数组数
@@ -221,11 +335,18 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
                 cv::Mat Hi0 = _origMatrix[i];
                 cv::Mat Hj0 = _origMatrix[j];
 
+#ifdef OPT_H
                 ceres::CostFunction* cost_function =
                     new ceres::AutoDiffCostFunction<HomographyResidual, 8, 8, 8>(
                         new HomographyResidual(keypoint_i, keypoint_j, Hi0, Hj0));
-                problem.AddResidualBlock(cost_function, scale_loss, h_list[i], h_list[j]);
+                problemH.AddResidualBlock(cost_function, scale_loss, h_list[i], h_list[j]);
 
+#else
+                ceres::CostFunction* cost_function =
+                   new ceres::AutoDiffCostFunction<SE3Residual,2,6,6>(
+                       new SE3Residual(keypoint_i, keypoint_j, constK, constHeight));
+                problemSE3.AddResidualBlock(cost_function, scale_loss, se3_list[i],se3_list[j]);
+#endif
 
             }
 
@@ -236,32 +357,55 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
 
     // 配置求解器
     ceres::Solver::Options options;
-    options.max_num_iterations = 2; // 增加最大迭代次数
+    options.max_num_iterations = 6; // 增加最大迭代次数
     options.function_tolerance = 1e-5; // 设置更严格的函数值容忍度
     options.gradient_tolerance = 1e-5; // 设置更严格的梯度容忍度
     options.parameter_tolerance = 1e-5; // 设置更严格的参数容忍度
     options.minimizer_progress_to_stdout = true;
     //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 使用稀疏求解器
-    options.num_threads = 1; // 使用多线程
+    options.num_threads = 8; // 使用多线程
     
     ceres::Solver::Summary summary;
 
     // 求解
-    ceres::Solve(options, &problem, &summary);
+#ifdef OPT_H
+    ceres::Solve(options, &problemH, &summary);
+#else
+    ceres::Solve(options, &problemSE3, &summary);
+#endif
+
+    //for (int i = 0; i < _origse3.size(); i++)
+    //{
+    //    std::cout << "------------" << std::endl;
+    //    std::cout << _origse3[i] << std::endl;
+    //    std::cout << _currse3[i] << std::endl;
+    //    std::cout << "------------" << std::endl;
+    //}
 
-    //for (int i = 0; i < _currMatrix.size(); i++)
+
+
+    // 输出结果
+    std::cout << summary.FullReport() << std::endl;
+
+#ifndef OPT_H
+    // 将优化se3写入H
+    updateSe3();
+#endif
+
+    //for (int i = 0; i < _origMatrix.size(); i++)
     //{
     //    std::cout << "------------" << std::endl;
     //    std::cout << _origMatrix[i] << std::endl;
     //    std::cout << _currMatrix[i] << std::endl;
     //    std::cout << "------------" << std::endl;
     //}
+    // 
     
-    // 输出结果
-    std::cout << summary.BriefReport() << std::endl;
 
-    // 写入缓存
+    // 优化后H写入文件缓存
     writeFrameInfo();
+
+    
 }
 
 
@@ -284,6 +428,8 @@ bool BA_Task::updateCacheH(KeyType Key, cv::Mat H)
 int BA_Task::readFrameInfo(vector<KeyType> frameInd)
 {
     auto _t_frame_cache = std::make_shared<FrameCache>();
+    _cache->get(frameInd[0], _t_frame_cache);
+    constHeight = _t_frame_cache->_para.nEvHeight;
 
     for (size_t i = 0; i < frameInd.size(); i++)
     {
@@ -293,6 +439,8 @@ int BA_Task::readFrameInfo(vector<KeyType> frameInd)
             // 记录key
             _frameInd.push_back(key);
 
+            // 注意Mat 浅拷贝陷阱！！！！！！
+
             // 特征点
             vector<cv::KeyPoint> keypoints(_t_frame_cache->_pt, _t_frame_cache->_pt + _t_frame_cache->ptNum);
 			_FeaPtVec.push_back(keypoints);
@@ -308,6 +456,14 @@ int BA_Task::readFrameInfo(vector<KeyType> frameInd)
             _currMatrix.push_back(H.clone());
 
 
+            // 初始SE3
+            _origRtK.push_back(cv::Mat(21, 1, CV_64F, _t_frame_cache->RtK).clone());
+            _currRtK.push_back(cv::Mat(21, 1, CV_64F, _t_frame_cache->RtK).clone());
+
+            _origse3.push_back(cv::Mat(6, 1, CV_64F, _t_frame_cache->se3).clone());
+            _currse3.push_back(cv::Mat(6, 1, CV_64F, _t_frame_cache->se3).clone());
+
+
             // 缓存包围多边形
             _polygon.push_back(warpRectWithH(H, cv::Size(_t_frame_cache->_frame_info.u32Width, _t_frame_cache->_frame_info.u32Height)));
 
@@ -323,6 +479,15 @@ int BA_Task::readFrameInfo(vector<KeyType> frameInd)
     return 0;
 }
 
+#include <Arith_GeoSolver.h>
+void BA_Task::updateSe3()
+{
+    for (size_t i = 0; i < _origse3.size(); i++)
+    {
+        _currMatrix[i] = Transe3ToH((double*)_origse3[i].data,  constK, constHeight);
+    }
+}
+
 int BA_Task::writeFrameInfo()
 {
     for (size_t i = 0; i < _currMatrix.size(); i++)

stitch/src/Arith_BATask.h

@@ -17,6 +17,7 @@
 
 
 //#define SHOW_MATCH
+//#define OPT_H
 
 
 class  BA_Task
@@ -28,7 +29,6 @@ public:
 	// 初始化BA任务
 	void InitTask();
 
-
 	// 优化指定帧，并更新H到缓存中,H_map用于优化过程可视化
 	void OptFrame(vector<KeyType> frameInd, cv::Mat H_map);
 
@@ -40,6 +40,9 @@ private:
 	// 从缓存读取指定帧信息（不需要图像）
 	int readFrameInfo(vector<KeyType> frameInd);
 
+	// 更新se3到H
+	void updateSe3();
+
 	// 输出优化结果到cache
 	int writeFrameInfo();
 
@@ -59,15 +62,27 @@ private:
 	Mat_<int> _MatchMat;//配准点邻接表
 	Mat_<float> _IOUMat;//交汇邻接表
 
+	// H矩阵（Rt混合）
 	vector<cv::Mat_<double>> _origMatrix;//初始H矩阵
 	vector<cv::Mat_<double>> _currMatrix;//当前H矩阵
 
+	// 21维内外参完整参数
+	vector<cv::Mat_<double>> _origRtK;    // 初始RtK参数
+	vector<cv::Mat_<double>> _currRtK;	// 当前RtK参数
+
+	// 提取R->旋转向量 + t
+	vector<cv::Mat_<double>> _origse3;    // 初始旋转向量
+	vector<cv::Mat_<double>> _currse3;	// 当前旋转向量
+
 
 	vector<vector<cv::Point2f>> _polygon;//帧包围四边形
 	vector<vector<cv::KeyPoint>> _FeaPtVec;//特征点缓存
 	vector<cv::Mat> _FeaDespVec;//特征点描述子
 	vector<FrameInfo> _paraVec;
 
+	double constHeight;
+	cv::Mat constK;
+
 private:
 
 	// 调试：绘制匹配关系

stitch/src/Arith_GeoSolver.cpp

@@ -3,6 +3,9 @@
 #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"
 using namespace cv;
 
 GeoSolver::GeoSolver()
@@ -127,7 +130,6 @@ Mat GeoSolver::Mat_TransENGMove(FrameInfo info)
 Mat GeoSolver::Mat_TransENG2uv(FrameInfo info)
 {
 	//从地理坐标系转像素坐标
-
 	//[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]'
 	//[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH]
 
@@ -139,6 +141,21 @@ Mat GeoSolver::Mat_TransENG2uv(FrameInfo info)
 		);
 
 
+	// 内参
+	FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000;
+
+	Mat M_cam = (Mat_<double>(3, 3) << fd, 0, info.nWidth / 2,
+		0, -fd, info.nHeight / 2,
+		0, 0, 1
+		);
+
+	Mat M = M_cam * Mat_TransENG2Cam(info) * M_het;
+
+	return M;
+}
+
+cv::Mat GeoSolver::Mat_TransENG2Cam(FrameInfo info)
+{
 
 	float yaw = info.craft.stAtt.fYaw;
 	Mat M_yaw = (Mat_<double>(3, 3) << cosd(yaw), -sind(yaw), 0,
@@ -180,6 +197,13 @@ Mat GeoSolver::Mat_TransENG2uv(FrameInfo info)
 
 		);
 
+	Mat M =M_alaph * M_beta * M_roll * M_pitch * M_yaw;
+
+	return M;
+}
+
+cv::Mat GeoSolver::Mat_GetCamK(FrameInfo info)
+{
 	// 内参
 	FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000;
 
@@ -188,34 +212,65 @@ Mat GeoSolver::Mat_TransENG2uv(FrameInfo info)
 		0, 0, 1
 		);
 
-	Mat M = M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het;
+	return M_cam;
-	//cout << M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het;
+}
-	return M;
+
+RtK GeoSolver::AnlayseRtK(FrameInfo info)
+{
+	RtK rtk;
+	rtk.R = Mat_TransENG2Cam(info);
+	rtk.T = Mat_TransENGMove(info);
+	rtk.K = Mat_GetCamK(info);
+	return rtk;
 }
 
+cv::Mat TransRtKToH(RtK rtk, double Depth)
+{
+	// 深度矩阵
+	Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
+		0, 1, 0,
+		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;
+
+	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);
 
+	return H;
+}
 
-// find SE(3)
+cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth)
-Proj GeoSolver::AnlayseTform(FrameInfo info)
 {
-	Proj projection;
+	RtK rtk;
-	// 从像方->地理
+	cv::Rodrigues(cv::Mat(3, 1, CV_64FC1, se3), rtk.R);
-	projection.tf_p2g.R = Mat_TransENG2uv(info).inv();
+	rtk.T = cv::Mat(3, 1, CV_64FC1, se3 + 3);
-	projection.tf_p2g.T = Mat_TransENGMove(info);
 
-	// 从地理->像方
+	rtk.K = K;
-	projection.tf_g2p.R = Mat_TransENG2uv(info);
-	projection.tf_g2p.T = -projection.tf_p2g.T;
 
-	return projection;
+	return TransRtKToH(rtk, Depth);
 }
 
 
 // 检验H的计算
 cv::Mat GeoSolver::findHomography2(FrameInfo info)
 {
-	Proj _proj = AnlayseTform(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));
@@ -224,10 +279,10 @@ cv::Mat GeoSolver::findHomography2(FrameInfo info)
 	srcPoints.push_back(cv::Point2f(0, 1000));
 
 	// 同名点计算，从像方到全景
-	cv::Point2f leftTop_map = Trans_uv2Geo(srcPoints[0], _proj.tf_p2g);
+	cv::Point2f leftTop_map = Trans_uv2Geo(srcPoints[0], tf_p2g);
-	cv::Point2f rightTop_map = Trans_uv2Geo(srcPoints[1], _proj.tf_p2g);
+	cv::Point2f rightTop_map = Trans_uv2Geo(srcPoints[1], tf_p2g);
-	cv::Point2f rightBottom_map = Trans_uv2Geo(srcPoints[2], _proj.tf_p2g);
+	cv::Point2f rightBottom_map = Trans_uv2Geo(srcPoints[2], tf_p2g);
-	cv::Point2f leftBottom_map = Trans_uv2Geo(srcPoints[3], _proj.tf_p2g);
+	cv::Point2f leftBottom_map = Trans_uv2Geo(srcPoints[3], tf_p2g);
 
 	// 目标图像（全景图）的四个顶点坐标
 	std::vector<cv::Point2f> dstPoints;
@@ -245,16 +300,17 @@ cv::Mat GeoSolver::findHomography2(FrameInfo info)
 // 根据R和t，解析H
 cv::Mat GeoSolver::findHomography(FrameInfo info)
 {
-	Proj _proj = AnlayseTform(info);
+	TForm tf_p2g;
-	TForm tform = _proj.tf_p2g;
+	tf_p2g.R = Mat_TransENG2uv(info).inv();
+	tf_p2g.T = Mat_TransENGMove(info);
 
-	double R[9] = { 0 };
-	double T[3] = { 0 };
 
-	memcpy(R, tform.R.data, sizeof(double) * 9);
-	memcpy(T, tform.T.data, sizeof(double) * 3);
 
+	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);
 
 	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]),
@@ -268,6 +324,15 @@ cv::Mat GeoSolver::findHomography(FrameInfo info)
 	return H;
 }
 
+cv::Mat GeoSolver::findHomography(FrameInfo info, RtK& rtk)
+{
+	rtk = AnlayseRtK(info);
+
+	cv::Mat H = TransRtKToH(rtk,info.nEvHeight);
+
+	return H;
+}
+
 
 // 计算多边形的面积
 double polygonArea(const vector<cv::Point2f>& points)
@@ -344,3 +409,41 @@ cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size)
 }
 
 
+cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt)
+{
+    Mat point = (Mat_<double>(3, 1) << srcPt.x, srcPt.y, 1.0);
+
+	// 轴角转R
+	cv::Mat rvec = (Mat_<double>(3, 1) << -se3[0], -se3[1], -se3[2]);
+	cv::Mat tvec = (Mat_<double>(3, 1) << se3[3], se3[4], se3[5]);
+	cv::Mat R;
+	cv::Rodrigues(rvec, R);
+
+
+	// 深度矩阵
+	Mat M_het_inv = (Mat_<double>(3, 3) << 1, 0, 0,
+		0, 1, 0,
+		0, 0, 1.0/depth
+		);
+
+	// 转地理系
+	Mat result = M_het_inv * R * K.inv() * point;
+
+
+	// 说明ceres::AngleAxisRotatePoint有效，结果等效于cv::Rodrigues
+	//double resP[3] = { 0 };
+	//cv::Mat pt = (K.inv() * point);
+	//ceres::AngleAxisRotatePoint((double*)rvec.data, (double*)pt.data, (double*)resP);
+	//result = M_het_inv * cv::Mat(3, 1, CV_64FC1, resP);
+
+
+	// 转局部地理系
+	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 += tvec.at<double>(0, 0);
+	warpedY += tvec.at<double>(1, 0);
+
+
+	return cv::Point2f(warpedX,warpedY);
+}

stitch/src/Arith_GeoSolver.h

@@ -13,7 +13,6 @@
 #include "StitchStruct.h"
 
 
-
 // 像方-物方转换关系
 struct TForm
 {
@@ -22,16 +21,14 @@ struct TForm
 };
 
 
-// 投影关系,描述反算过程
+// 李群+K
-struct Proj
+struct RtK
 {
-    TForm tf_p2g;//从帧到地理坐标系的Rt矩阵
+    cv::Mat R; // world->pix
-    TForm tf_g2p;//从地理坐标系到帧的Rt矩阵
+    cv::Mat T; // world
+    cv::Mat K; // cam
 };
 
-
-
-
 class GeoSolver 
 {
 public:
@@ -39,9 +36,12 @@ public:
     GeoSolver();
 	~GeoSolver();
 
-    // 建立全景图与归一化地理系的H矩阵，解析出H
+    // 建立像方与归一化地理系的H矩阵，解析出H
     cv::Mat findHomography(FrameInfo info);
 
+    // 计算标准李群,并返回H矩阵
+    cv::Mat findHomography(FrameInfo info, RtK& se3Para);
+
     // 测试
     cv::Mat findHomography2(FrameInfo info);
 
@@ -64,8 +64,11 @@ public:
     cv::Point2f getGeoFromBLH(PointBLH ptPos);
 
 private:
-    // 计算当前帧像方-地理坐标系R t（反投影关系）
+
-    Proj AnlayseTform(FrameInfo info);
+    // 计算当前帧李群（SE3）和投影K
+    RtK AnlayseRtK(FrameInfo info);
+
+
 
     // 帧像方-地理坐标
     cv::Point2f Trans_uv2Geo(cv::Point2f pos_frame, TForm form);
@@ -73,20 +76,33 @@ private:
     // 地理坐标-帧像方
     cv::Point2f Trans_Geo2uv(cv::Point2f pos_geo, TForm form);
 
+    // 机体ENG(东北地)到像方的 旋转矩阵
+    cv::Mat Mat_TransENG2uv(FrameInfo info);
+
+private:
 
     // 平移矩阵，以初始化点为基准，计算当前位置在初始点的地理坐标，那么当前帧所有点的坐标做此平移
     cv::Mat Mat_TransENGMove(FrameInfo info);
 
-    // 机体ENG(东北地)到像方的 旋转矩阵
+    // 机体ENG(东北地)到相机系 旋转矩阵
-    cv::Mat Mat_TransENG2uv(FrameInfo info);
+    cv::Mat Mat_TransENG2Cam(FrameInfo info);
 
-private:
+    // 内参
+    cv::Mat Mat_GetCamK(FrameInfo info);
 
     PointXYZ originCGCSPoint;//成图初始点的地心地固坐标，作为拼接参考
 
     FrameInfo origininfo;
 };
 
+
+
+// 李群转H矩阵，地平假设,利用深度
+cv::Mat TransRtKToH(RtK se3, double Depth);
+
+// 李代数转H矩阵，参见上面 李群转H矩阵
+cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth);
+
 // 多边形面积
 double polygonArea(const vector<cv::Point2f>& points);
 
@@ -101,3 +117,6 @@ vector<cv::Point2f> warpRectWithH(cv::Mat H,cv::Size size);
 
 // H映射多边形，转rect
 cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size);
+
+// 用SE3映射
+cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt);

stitch/src/Arith_UnderStitch.cpp

@@ -80,6 +80,7 @@ UnderStitch::~UnderStitch()
 
 UPanInfo UnderStitch::InitMap(FrameInfo info)
 {
+
 	cv::Mat H0 = _GeoSolver->findHomography(info);
 
 	// 计算视场中心的地理坐标
@@ -89,7 +90,7 @@ UPanInfo UnderStitch::InitMap(FrameInfo info)
 	double yaw_angle = info.craft.stAtt.fYaw;
 	
 	// 根据飞行方向调整全景图尺寸
-	int base_size = MIN(info.nWidth * 5, 25000);
+	int base_size = MIN(info.nWidth * 5, 8000);
 	UPanInfo panPara = { 0 };
 	
 	// 根据偏航角计算宽高比
@@ -121,7 +122,7 @@ UPanInfo UnderStitch::InitMap(FrameInfo info)
 	auto geo_pro_rect = warpRectWithH_2Rect(H0,cv::Size(info.nWidth,info.nHeight));
 	
 	// 计算目标比例（单帧投影后占全景图的0.25）
-	float target_ratio = 0.2;
+	float target_ratio = 0.25;
 	float current_ratio = MAX(geo_pro_rect.width / (target_ratio*panPara.m_pan_width), geo_pro_rect.height / (target_ratio*panPara.m_pan_height));
 
 	// 调整scale参数
@@ -303,7 +304,9 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
     LOG_INFO("Run frame {} ,_totalFrameCnt {}",para.nFrmID,_totalFrameCnt);
 
     // 获取地理投影H
-    cv::Mat H_geo = _GeoSolver->findHomography(para);
+	RtK rtk;
+    cv::Mat H_geo = _GeoSolver->findHomography(para, rtk);
+
 
     // 判断帧是否在全景图视场内
     if (!isFrameInPanView(cv::Size(para.nWidth, para.nHeight), _H_pan * H_geo, 
@@ -440,7 +443,7 @@ bool UnderStitch::FilterFrame(FrameInfo para)
 bool UnderStitch::TriggerBA(FrameInfo para)
 {
     // 1. 固定帧间隔触发
-    const int FIXED_FRAME_INTERVAL = 100;
+    const int FIXED_FRAME_INTERVAL = 10;
     if (_totalFrameCnt % FIXED_FRAME_INTERVAL == 0)
     {
         LOG_INFO("TriggerBA: Fixed frame interval {} reached",FIXED_FRAME_INTERVAL);
@@ -620,11 +623,29 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
 	// 保存描述子
 	memcpy(_t_frame_cache->_desp, descriptors.data, sizeof(float) * keyNum * FEA_DES_SIZE);
 
-	cv::Mat_<double> H0 = _GeoSolver->findHomography(para);
 
 	// 保存初始H
+	RtK rtk;
+	cv::Mat_<double> H0 = _GeoSolver->findHomography(para, rtk);
 	memcpy(_t_frame_cache->H, H0.data, sizeof(double) * 9);
 
+	memcpy(_t_frame_cache->RtK, rtk.R.data, sizeof(double) * 9);
+	memcpy(_t_frame_cache->RtK + 9, rtk.T.data, sizeof(double) * 3);
+	memcpy(_t_frame_cache->RtK + 12, rtk.K.data, sizeof(double) * 9);
+
+	
+	cv::Vec3d rvec;
+	cv::Rodrigues(rtk.R, rvec);
+	memcpy(_t_frame_cache->se3, rvec.val, sizeof(double) * 3);
+	memcpy(_t_frame_cache->se3 + 3, rtk.T.data, sizeof(double) * 3);
+
+//#define TEST
+#ifdef TEST
+	auto wpt1 = warpPointWithH(H0, cv::Point2f(100, 100));
+	auto wpt2 = warpPointWithse3(_t_frame_cache->se3, para.nEvHeight, rtk.K, cv::Point2f(100, 100));
+#endif
+
+
 	// 预处理结果加入文件缓存
 	_cache->set(para.nFrmID,_t_frame_cache);
 

stitch/src/StitchStruct.h

@@ -50,6 +50,9 @@ struct Match_Net
 };
 
 
+
+
+
 #define IMG_CACHE_SIZE  (1920 * 1080 * 3)   //图像缓存尺寸
 #define FEA_NUM_MAX   500                   // 单帧特征点数量
 #define FEA_DES_SIZE 128                    // 特征点描述子尺度
@@ -64,7 +67,13 @@ struct FrameCache
     SINT32 ptNum;
     cv::KeyPoint _pt[FEA_NUM_MAX];
     FLOAT32 _desp[FEA_NUM_MAX * FEA_DES_SIZE];
+
+
     DOUBLE64 H[9];// H矩阵
+
+    // SE3
+    DOUBLE64 RtK[21];  //R(9)+T(3)+K(9)
+    DOUBLE64 se3[6];     //旋转向量+平移向量，李代数（优化项）
 };
 
 

stitch/src/Version.h

@@ -2,5 +2,5 @@
 
 #pragma once
 #include <string>
-std::string BUILD_TIME = "BUILD_TIME 2025_10_17-08.55.27";
+std::string BUILD_TIME = "BUILD_TIME 2025_10_28-11.21.04";
 std::string VERSION = "BUILD_VERSION 1.0.1";

tests/ProcDJ.cpp

@@ -340,10 +340,10 @@ int main()
     vector<std::string> videoPathList;
     vector<std::string> srtPathList;
 
-    //string folder = "F:/DJI_202504181507_016/";
+    string folder = "F:/DJI_202504181507_016/";
 
 
-    string folder = "/media/wang/data/DJI_202504181507_016/";
+    //string folder = "/media/wang/data/DJI_202504181507_016/";
     //
     //videoPathList.push_back(folder + "DJI_20250418152649_0005_W.MP4");
 	//srtPathList.push_back(folder + "DJI_20250418152649_0005_W.srt");
@@ -354,8 +354,6 @@ int main()
    //videoPathList.push_back(folder + "DJI_20250418152649_0005_T.MP4");
    //srtPathList.push_back(folder + "DJI_20250418152649_0005_T.srt");
 
-
-
     ProcDJVideo(videoPathList, srtPathList);
     return 0;
 }