diff --git a/stitch/src/Arith_BATask.cpp b/stitch/src/Arith_BATask.cpp index 6f28c66..2184c57 100644 --- a/stitch/src/Arith_BATask.cpp +++ b/stitch/src/Arith_BATask.cpp @@ -19,6 +19,8 @@ using namespace ceres; #define STABLE_Y3 1080 + + //// H投影残差：将左图投影到右图，最小化同名点全景图上的投影误差。 //// 问题：缺少尺度约束，H矩阵优化后失去正交性 优点：形式简单 struct HomographyResidual @@ -106,8 +108,8 @@ private: // 问题：不是真正三角化，无深度优化。 优点：有尺度约束，标准做法，可对21个参数优化。 struct SE3Residual { - SE3Residual(cv::KeyPoint keypoint_i, cv::KeyPoint keypoint_j,cv::Mat _K, double _fEvHeight_i, double _fEvHeight_j) - : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),K(_K),EvHeight_i(_fEvHeight_i), EvHeight_j(_fEvHeight_j) + SE3Residual(cv::KeyPoint keypoint_i, cv::KeyPoint keypoint_j,cv::Mat _K, double _fEvHeight_i, double _fEvHeight_j, const cv::Mat H1, const cv::Mat H2) + : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),K(_K),EvHeight_i(_fEvHeight_i), EvHeight_j(_fEvHeight_j), Hi0_(H1), Hj0_(H2) { } @@ -115,18 +117,20 @@ struct SE3Residual template bool operator()(const T* const se3_i, const T* const se3_j, T* residual) const { + typedef Eigen::Matrix EPoint; + typedef Eigen::Matrix EMat; // 左图投影到物方 // T ptL[3] = {T(keypoint_i_.pt.x), T(keypoint_i_.pt.y)}; // 原始点 // 深度矩阵 - Eigen::Matrix M_het_inv; + EMat 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_i); // K-1 - Eigen::Matrix K_1; - Eigen::Matrix K_inv; + EMat K_1; + EMat K_inv; double fx = K.at(0, 0); double fy = K.at(1, 1); @@ -141,17 +145,18 @@ struct SE3Residual 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)); + EPoint ptL(T(keypoint_i_.pt.x), T(keypoint_i_.pt.y), T(1.0)); + EPoint ptR(T(keypoint_j_.pt.x), T(keypoint_j_.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; + EPoint pt = K_inv * ptL; + EPoint 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; + EPoint result = M_het_inv * LeftRota; // 恢复尺度 T GeoX = result[0] / result[2]; T GeoY = result[1] / result[2]; @@ -168,22 +173,20 @@ struct SE3Residual T rvec_j[3] = { se3_j[0], se3_j[1], se3_j[2] }; - Eigen::Matrix pGeoR(GeoX, GeoY, GeoZ); - Eigen::Matrix RightRota; + EPoint pGeoR(GeoX, GeoY, GeoZ); + EPoint 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]; + EPoint pUVR = K_1* RightRota; + EPoint pUVR_1 = pUVR / pUVR[2]; // 计算重投影误差 - residual[0] = uR - T(keypoint_j_.pt.x); - residual[1] = vR - T(keypoint_j_.pt.y); + residual[0] = (pUVR_1 - ptR).squaredNorm(); + + @@ -195,6 +198,9 @@ private: cv::Mat_ K; //内参 double EvHeight_i;//高差，用于三角化 double EvHeight_j;//高差，用于三角化 + + const cv::Mat Hi0_;//初始定位H，用于坐标控制 + const cv::Mat Hj0_;//初始定位H，用于坐标控制 }; @@ -332,8 +338,8 @@ void BA_Task::OptFrame(vector frameInd,cv::Mat H_map, std::unordered_ma #ifdef OPT_SE3 ceres::CostFunction* cost_function = - new ceres::AutoDiffCostFunction( - new SE3Residual(keypoint_i, keypoint_j, constK, _depth[i], _depth[j])); + new ceres::AutoDiffCostFunction( + new SE3Residual(keypoint_i, keypoint_j, constK, _depth[i], _depth[j], Hi0, Hj0)); problemSE3.AddResidualBlock(cost_function, scale_loss, se3list[i], se3list[j]); #endif diff --git a/stitch/src/Arith_UnderStitch.cpp b/stitch/src/Arith_UnderStitch.cpp index d3a11de..aac04e6 100644 --- a/stitch/src/Arith_UnderStitch.cpp +++ b/stitch/src/Arith_UnderStitch.cpp @@ -91,7 +91,7 @@ UPanInfo UnderStitch::InitMap(FrameInfo para) double yaw_angle = para.craft.stAtt.fYaw; // 根据飞行方向调整全景图尺寸 - int base_size = MIN(para.nWidth * 10, 10000); + int base_size = MIN(para.nWidth * 10, 12000); UPanInfo panPara = { 0 }; // 根据偏航角计算宽高比 @@ -649,7 +649,7 @@ bool UnderStitch::FilterFrame(FrameInfo para) bool UnderStitch::TriggerBA(FrameInfo para) { // 1. 固定帧间隔触发 - const int FIXED_FRAME_INTERVAL = 30; + const int FIXED_FRAME_INTERVAL = 15; if (_totalFrameCnt % FIXED_FRAME_INTERVAL == 0) { LOG_INFO("TriggerBA: Fixed frame interval {} reached",FIXED_FRAME_INTERVAL); @@ -876,13 +876,6 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para) 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(H_cur, 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); @@ -901,10 +894,6 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para) // 优化表初始化 _recvFrameOptFlag[para.nFrmID] = 0; - - - - _totalFrameCnt++; return _totalFrameCnt; diff --git a/stitch/src/Version.h b/stitch/src/Version.h index 0007ede..124ae60 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_11_14-15.38.58"; +std::string BUILD_TIME = "BUILD_TIME 2025_11_18-19.38.24"; std::string VERSION = "BUILD_VERSION 1.0.1"; diff --git a/tests/ProcDJ.cpp b/tests/ProcDJ.cpp index da4f5e4..645c6e0 100644 --- a/tests/ProcDJ.cpp +++ b/tests/ProcDJ.cpp @@ -89,7 +89,8 @@ void inferCameraParams_H30(FrameData& frame, const std::string& filename) frame.pixel_size_um = 2.4; } else if (filename.find("_Z") != std::string::npos) { - + frame.real_focal_mm = 6.72*2; + frame.pixel_size_um = 2.4; } else if (filename.find("_T") != std::string::npos) { frame.real_focal_mm = 24; @@ -216,7 +217,7 @@ void ProcDJVideo(vector videoPathList, vector srtPathL UPanConfig cfg = { 0 }; cfg.bOutFrameTile = 0; - cfg.bOutGoogleTile = 1; + cfg.bOutGoogleTile = 0; cfg.bUseBA = 1; stitcher->SetConfig(cfg); @@ -373,8 +374,8 @@ int main() string folder = "F:/K2D_data/"; - videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4"); - srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt"); + videoPathList.push_back(folder + "DJI_20251024144932_0001_Z.MP4"); + srtPathList.push_back(folder + "DJI_20251024144932_0001_Z.srt"); //videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4"); //srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt"); diff --git a/tests/stitch_S732.cpp b/tests/stitch_S732.cpp index b627ebe..6748e21 100644 --- a/tests/stitch_S732.cpp +++ b/tests/stitch_S732.cpp @@ -123,9 +123,9 @@ void ProcessVL(string filePath) stitcher->Init(info); UPanConfig cfg = { 0 }; - cfg.bOutGoogleTile = 1; + cfg.bOutGoogleTile = 0; cfg.bOutFrameTile = 0; - cfg.bUseBA = 1; + cfg.bUseBA = 0; stitcher->SetConfig(cfg); stitcher->SetOutput("baotou","F:/google_tiles"); @@ -205,6 +205,7 @@ void ProcessVL(string filePath) int main() { - ProcessVL("F:/包头/20251101/20251101_091550742_2.yuv"); + ProcessVL("F:/包头/20251101/20251101_092143724_3.yuv"); return 0; -} \ No newline at end of file +} +