From 782cc531f119f936d7cebf4c9381f69d63637630 Mon Sep 17 00:00:00 2001 From: wangchongwu <759291707@qq.com> Date: Tue, 28 Oct 2025 16:37:42 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0se3=E4=BC=98=E5=8C=96?= =?UTF-8?q?=E5=92=8CH=E4=BC=98=E5=8C=96=E5=B9=B6=E5=AD=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeSettings.json | 27 +++++ stitch/CMakeLists.txt | 4 + stitch/src/Arith_BATask.cpp | 187 +++++++++++++++++++++++++++++-- stitch/src/Arith_BATask.h | 17 ++- stitch/src/Arith_GeoSolver.cpp | 153 ++++++++++++++++++++----- stitch/src/Arith_GeoSolver.h | 47 +++++--- stitch/src/Arith_UnderStitch.cpp | 31 ++++- stitch/src/StitchStruct.h | 9 ++ stitch/src/Version.h | 2 +- tests/ProcDJ.cpp | 6 +- 10 files changed, 422 insertions(+), 61 deletions(-) create mode 100644 CMakeSettings.json diff --git a/CMakeSettings.json b/CMakeSettings.json new file mode 100644 index 0000000..0c5fbf9 --- /dev/null +++ b/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": [] + } + ] +} \ No newline at end of file diff --git a/stitch/CMakeLists.txt b/stitch/CMakeLists.txt index f3249fa..7d4ed04 100644 --- a/stitch/CMakeLists.txt +++ b/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) diff --git a/stitch/src/Arith_BATask.cpp b/stitch/src/Arith_BATask.cpp index 22184aa..55229a5 100644 --- a/stitch/src/Arith_BATask.cpp +++ b/stitch/src/Arith_BATask.cpp @@ -1,6 +1,6 @@ #include "Arith_BATask.h" -#define GLOG_USE_GLOG_EXPORT #include "ceres/ceres.h" +#include #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 + 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 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 K_1; + Eigen::Matrix K_inv; + + double fx = K.at(0, 0); + double fy = K.at(1, 1); + double cx = K.at(0, 2); + double cy = K.at(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 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 pt = K_inv * ptL; + Eigen::Matrix LeftRota; + + T rvec_i[3] = { -se3_i[0], -se3_i[1], -se3_i[2] }; + ceres::AngleAxisRotatePoint(rvec_i, pt.data(), LeftRota.data()); + Eigen::Matrix 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 pGeoR(GeoX, GeoY, T(EvHeight)); + Eigen::Matrix RightRota; + + ceres::AngleAxisRotatePoint(rvec_j, pGeoR.data(), RightRota.data()); + + // 投影到右图像方 + Eigen::Matrix 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_ K; //内参 + double EvHeight;//高差，用于三角化 +}; + + + + + + + BA_Task::BA_Task(FileCache* 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 frameInd,cv::Mat H_map) { h_list[i] = (double*)_currMatrix[i].data; } + + + // 整合为6D参数 + std::vector 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 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( 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( + 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 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 < _currMatrix.size(); i++) + //for (int i = 0; i < _origse3.size(); i++) //{ // std::cout << "------------" << std::endl; - // std::cout << _origMatrix[i] << std::endl; - // std::cout << _currMatrix[i] << std::endl; + // std::cout << _origse3[i] << std::endl; + // std::cout << _currse3[i] << std::endl; // std::cout << "------------" << std::endl; //} + + // 输出结果 - std::cout << summary.BriefReport() << std::endl; + 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; + //} + // + + + // 优化后H写入文件缓存 writeFrameInfo(); + + } @@ -284,6 +428,8 @@ bool BA_Task::updateCacheH(KeyType Key, cv::Mat H) int BA_Task::readFrameInfo(vector frameInd) { auto _t_frame_cache = std::make_shared(); + _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 frameInd) // 记录key _frameInd.push_back(key); + // 注意Mat 浅拷贝陷阱！！！！！！ + // 特征点 vector 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 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 frameInd) return 0; } +#include +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++) diff --git a/stitch/src/Arith_BATask.h b/stitch/src/Arith_BATask.h index 97dcf10..1a377c8 100644 --- a/stitch/src/Arith_BATask.h +++ b/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 frameInd, cv::Mat H_map); @@ -40,6 +40,9 @@ private: // 从缓存读取指定帧信息（不需要图像） int readFrameInfo(vector frameInd); + // 更新se3到H + void updateSe3(); + // 输出优化结果到cache int writeFrameInfo(); @@ -59,15 +62,27 @@ private: Mat_ _MatchMat;//配准点邻接表 Mat_ _IOUMat;//交汇邻接表 + // H矩阵（Rt混合） vector> _origMatrix;//初始H矩阵 vector> _currMatrix;//当前H矩阵 + // 21维内外参完整参数 + vector> _origRtK; // 初始RtK参数 + vector> _currRtK; // 当前RtK参数 + + // 提取R->旋转向量 + t + vector> _origse3; // 初始旋转向量 + vector> _currse3; // 当前旋转向量 + vector> _polygon;//帧包围四边形 vector> _FeaPtVec;//特征点缓存 vector _FeaDespVec;//特征点描述子 vector _paraVec; + double constHeight; + cv::Mat constK; + private: // 调试：绘制匹配关系 diff --git a/stitch/src/Arith_GeoSolver.cpp b/stitch/src/Arith_GeoSolver.cpp index 0a44f8b..03264e2 100644 --- a/stitch/src/Arith_GeoSolver.cpp +++ b/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_(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_(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; - //cout << M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het; - return M; + return M_cam; +} + +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_(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_(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(2, 2); + return H; +} -// find SE(3) -Proj GeoSolver::AnlayseTform(FrameInfo info) +cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth) { - Proj projection; - // 从像方->地理 - projection.tf_p2g.R = Mat_TransENG2uv(info).inv(); - projection.tf_p2g.T = Mat_TransENGMove(info); + RtK rtk; + cv::Rodrigues(cv::Mat(3, 1, CV_64FC1, se3), rtk.R); + rtk.T = cv::Mat(3, 1, CV_64FC1, se3 + 3); - // 从地理->像方 - projection.tf_g2p.R = Mat_TransENG2uv(info); - projection.tf_g2p.T = -projection.tf_p2g.T; + rtk.K = K; - 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 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 rightTop_map = Trans_uv2Geo(srcPoints[1], _proj.tf_p2g); - cv::Point2f rightBottom_map = Trans_uv2Geo(srcPoints[2], _proj.tf_p2g); - cv::Point2f leftBottom_map = Trans_uv2Geo(srcPoints[3], _proj.tf_p2g); + 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 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 tform = _proj.tf_p2g; + TForm 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_(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& 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_(3, 1) << srcPt.x, srcPt.y, 1.0); + + // 轴角转R + cv::Mat rvec = (Mat_(3, 1) << -se3[0], -se3[1], -se3[2]); + cv::Mat tvec = (Mat_(3, 1) << se3[3], se3[4], se3[5]); + cv::Mat R; + cv::Rodrigues(rvec, R); + + + // 深度矩阵 + Mat M_het_inv = (Mat_(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(0, 0) / result.at(2, 0); + double warpedY = result.at(1, 0) / result.at(2, 0); + + warpedX += tvec.at(0, 0); + warpedY += tvec.at(1, 0); + + + return cv::Point2f(warpedX,warpedY); +} diff --git a/stitch/src/Arith_GeoSolver.h b/stitch/src/Arith_GeoSolver.h index ec7bd8a..f5fa420 100644 --- a/stitch/src/Arith_GeoSolver.h +++ b/stitch/src/Arith_GeoSolver.h @@ -13,7 +13,6 @@ #include "StitchStruct.h" - // 像方-物方转换关系 struct TForm { @@ -22,16 +21,14 @@ struct TForm }; -// 投影关系,描述反算过程 -struct Proj +// 李群+K +struct RtK { - TForm tf_p2g;//从帧到地理坐标系的Rt矩阵 - TForm tf_g2p;//从地理坐标系到帧的Rt矩阵 + cv::Mat R; // world->pix + 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(东北地)到像方的 旋转矩阵 - cv::Mat Mat_TransENG2uv(FrameInfo info); + // 机体ENG(东北地)到相机系 旋转矩阵 + 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& points); @@ -101,3 +117,6 @@ vector 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); diff --git a/stitch/src/Arith_UnderStitch.cpp b/stitch/src/Arith_UnderStitch.cpp index dc2863f..4820b2a 100644 --- a/stitch/src/Arith_UnderStitch.cpp +++ b/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_ H0 = _GeoSolver->findHomography(para); // 保存初始H + RtK rtk; + cv::Mat_ 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); diff --git a/stitch/src/StitchStruct.h b/stitch/src/StitchStruct.h index b140236..09e1184 100644 --- a/stitch/src/StitchStruct.h +++ b/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]; //旋转向量+平移向量，李代数（优化项） }; diff --git a/stitch/src/Version.h b/stitch/src/Version.h index 0d1c306..76a6773 100644 --- a/stitch/src/Version.h +++ b/stitch/src/Version.h @@ -2,5 +2,5 @@ #pragma once #include -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"; diff --git a/tests/ProcDJ.cpp b/tests/ProcDJ.cpp index 997f4b5..7bfeb98 100644 --- a/tests/ProcDJ.cpp +++ b/tests/ProcDJ.cpp @@ -340,10 +340,10 @@ int main() vector videoPathList; vector 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; }