初始BA完成，全局尺度约束存在问题

main.cpp

@@ -96,7 +96,7 @@ int main(int, char**)
     FILE* file = fopen("../20241219152917_4.xraw","rb");
     //FILE* file = fopen("/home/wang/data62/729dataset/ir_1280_1024_para40_y16/5.xraw","rb");
   
-    for (size_t i = 0; i < 900; i++)
+    for (size_t i = 0; i < 600; i++)
     {
 
         fread(pFrameIR,2,IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR + PARA_IR_LINE),file);
@@ -152,7 +152,7 @@ int main(int, char**)
         }
         else
         {
-            if(i % 20 != 0)
+            if(i % 10 != 0)
             {
                 continue;
             }

stitch/src/Arith_BATask.cpp

@@ -5,11 +5,11 @@ using namespace ceres;
 struct HomographyResidual 
 {
     HomographyResidual(const cv::KeyPoint& keypoint_i, const cv::KeyPoint& keypoint_j)
-        : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j)
+        : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j) {}
-    {
+
-    }
     template <typename T>
-    bool operator()(const T* const h_i, const T* const h_j, T* residual) const {
+    bool operator()(const T* const h_i, const T* const h_j, T* residual) const 
+    {
         // 残差计算逻辑
         T H_i[9] = { h_i[0], h_i[1], h_i[2],
                     h_i[3], h_i[4], h_i[5],
@@ -25,8 +25,10 @@ struct HomographyResidual
         T P_i[3] = { T(0), T(0), T(0) };
         T P_j[3] = { T(0), T(0), T(0) };
 
-        for (int row = 0; row < 3; row++) {
+        for (int row = 0; row < 3; row++) 
-            for (int col = 0; col < 3; col++) {
+        {
+            for (int col = 0; col < 3; col++) 
+            {
                 P_i[row] += H_i[row * 3 + col] * p_i[col];
                 P_j[row] += H_j[row * 3 + col] * p_j[col];
             }
@@ -77,8 +79,8 @@ SINT32 BA_Task::addFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
     // 缓存初始H
     Proj t_Proj = _GeoStitcher->AnlayseTform(para);
     auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
-    _origMatrix.push_back(H);
+    _origMatrix.push_back(H.clone());
-    _currMatrix.push_back(H);
+    _currMatrix.push_back(H.clone());
 
 
     // 提取特征点
@@ -102,43 +104,14 @@ void BA_Task::setPanPara(PanInfo info)
 
 void BA_Task::Test()
 {
-    // google::InitGoogleLogging("ceres");
-
-    // // The variable to solve for with its initial value. It will be
-    // // mutated in place by the solver.
-    // double x = 0.5;
-    // const double initial_x = x;
-
-    // // Build the problem.
-    // ceres::Problem problem;
-
-    // // Set up the only cost function (also known as residual). This uses
-    // // auto-differentiation to obtain the derivative (jacobian).
-    // ceres::CostFunction* cost_function =
-    //     new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
-    // problem.AddResidualBlock(cost_function, nullptr, &x);
-
-    // // Run the solver!
-    // ceres::Solver::Options options;
-    // options.minimizer_progress_to_stdout = true;
-    // ceres::Solver::Summary summary;
-    // ceres::Solve(options, &problem, &summary);
-
-    // std::cout << summary.BriefReport() << "\n";
-    // std::cout << "x : " << initial_x << " -> " << x << "\n";
-
     
 }
 
+//#define SHOW_MATCH
 void BA_Task::optimizeBA()
 {
-    //for (int i = 0; i < _currMatrix.size(); i++)
+
-    //{
+    remap(_origMatrix, "_origMatrix");
-    //    std::cout << "------------" << std::endl;
-    //    std::cout << _origMatrix[i] << std::endl;
-    //    std::cout << _currMatrix[i] << std::endl;
-    //    std::cout << "------------" << std::endl;
-    //}
 
     // 计算匹配性矩阵
     CalMatchMat(0.3);
@@ -146,137 +119,25 @@ void BA_Task::optimizeBA()
     google::InitGoogleLogging("ceres");
 
     // 将 cv::Mat 转换为 Ceres 需要的数组形式
-    std::vector<double*> h_list(_origMatrix.size());
+    std::vector<double*> h_list(_currMatrix.size());
 
 
-    for (int i = 0; i < _origMatrix.size(); i++)
+    for (int i = 0; i < _currMatrix.size(); i++)
     {
-        h_list[i] = (double*)_origMatrix[i].data;
+        h_list[i] = (double*)_currMatrix[i].data;
     }
     // 创建 Ceres 问题
     ceres::Problem problem;
 
     // 添加残差块
+    int nParaCnt = 0;//参数组数
+
     for (int i = 0; i < _MatchMat.cols; i++)
     {
         for (int j = i + 1; j < _MatchMat.rows; j++)
         {
-            int M = _MatchMat.at<int>(i, j); // 获取匹配点对的数量
-            if (M > 0) 
-            { 
-                // 添加匹配点对的残差块
-                for (int m = 0; m < M; m++) 
-                {
-                    // 注意：这里不对，应该找匹配点！！ todo 节后完成
-                    cv::KeyPoint keypoint_i = _FeaPtVec[i][m];
-                    cv::KeyPoint keypoint_j = _FeaPtVec[j][m];
-
-                    ceres::CostFunction* cost_function =
-                        new ceres::AutoDiffCostFunction<HomographyResidual, 2, 8, 8>(
-                            new HomographyResidual(keypoint_i, keypoint_j));
-                    problem.AddResidualBlock(cost_function, nullptr, h_list[i], h_list[j]);
-                }
-            }
-        }
-    }
-
-
-
-    // 配置求解器
-    ceres::Solver::Options options;
-    options.minimizer_progress_to_stdout = true;
-    //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 使用稀疏求解器
-    options.minimizer_progress_to_stdout = true;
-    options.num_threads = 1; // 使用多线程
-    ceres::Solver::Summary summary;
-
-    // 求解
-    ceres::Solve(options, &problem, &summary);
-
-    //// 将优化后的参数转换回 cv::Mat
-    for (int i = 0; i < _currMatrix.size(); i++) 
-    {
-        std::cout << "------------" << std::endl;
-        std::cout << _origMatrix[i] << std::endl;
-        std::cout << _currMatrix[i] << std::endl;
-        std::cout << "------------" << std::endl;
-        _currMatrix[i].at<double>(0, 0) = h_list[i][0];
-        _currMatrix[i].at<double>(0, 1) = h_list[i][1];
-        _currMatrix[i].at<double>(0, 2) = h_list[i][2];
-        _currMatrix[i].at<double>(1, 0) = h_list[i][3];
-        _currMatrix[i].at<double>(1, 1) = h_list[i][4];
-        _currMatrix[i].at<double>(1, 2) = h_list[i][5];
-        _currMatrix[i].at<double>(2, 0) = h_list[i][6];
-        _currMatrix[i].at<double>(2, 1) = h_list[i][7];
-        _currMatrix[i].at<double>(2, 2) = 1.0; // 固定 h_33 = 1
-    }
-
-    // 输出结果
-    std::cout << summary.BriefReport() << std::endl;
-
-    remap();
-}
-
-void BA_Task::remap()
-{
-    cv::Mat image(1000, 1000, CV_8UC3, cv::Scalar(0, 0, 0));
-
-    for (size_t i = 0; i < _imgVec.size(); i++)
-    {
-        cv::Mat imagetmp(1000, 1000, CV_8UC3, cv::Scalar(0, 0, 0));
-        cv::warpPerspective(_imgVec[i], imagetmp, _currMatrix[i], imagetmp.size());
-
-        cv::Mat mask = cv::Mat::ones(_imgVec[i].size(), CV_8UC1) * 255;
-        cv::Mat warped_mask;
-        cv::warpPerspective(mask, warped_mask, _currMatrix[i], image.size());
-        imagetmp.copyTo(image, warped_mask);
-    }
-
-    imshow("remap", image);
-    cv::waitKey(0);
-}
-
-//#define SHOW_MATCH
-SINT32 BA_Task::CalMatchMat(float fiou_thre)
-{
-    _IOUMat = cv::Mat::zeros(_polygon.size(),_polygon.size(),CV_32FC1);
-
-    _MatchMat = cv::Mat::zeros(_polygon.size(), _polygon.size(), CV_32SC1);
-
-
-    // 先计算IOU矩阵
-    for (size_t i = 0; i < _polygon.size(); i++)
-    {
-        vector<cv::Point2f> poly_i = _polygon[i];
-
-        for (size_t j = i + 1; j < _polygon.size(); j++)
-        {
-            if (i == j)
-            {
-                _IOUMat.at<float>(i,j) = 1; 
-                continue;
-            }
-            vector<cv::Point2f> poly_j = _polygon[j];
-
-            float fiou = computeQuadrilateralIOU(poly_i,poly_j);
-            _IOUMat.at<float>(i,j) = fiou;
-            _IOUMat.at<float>(j,i) = fiou;//是对称矩阵
-            
-        }
-    }
-
-    // IOU大于0.3以上才进行特征点匹配
-    for (size_t i = 0; i < _polygon.size(); i++)
-    {
-        for (size_t j = i+1; j < _polygon.size(); j++)
-        {
-            if (i == j)
-            {
-                continue;
-            }
-
             // IOU满足条件才匹配特征点
-            if (_IOUMat.at<float>(i,j) < fiou_thre)
+            if (_IOUMat.at<float>(i, j) < 0.3)
             {
                 continue;
             }
@@ -312,7 +173,7 @@ SINT32 BA_Task::CalMatchMat(float fiou_thre)
 #endif
             std::vector<cv::DMatch> matches;
             //_FeaMatcher->matchFeatures(_FeaDespVec[i],_FeaDespVec[j],matches);
-            _FeaMatcher->matchFeatures_WithH(_FeaPtVec[i],_FeaDespVec[i], _FeaPtVec[j], _FeaDespVec[j], _origMatrix[i], _origMatrix[j], matches);
+            _FeaMatcher->matchFeatures_WithH(_FeaPtVec[i], _FeaDespVec[i], _FeaPtVec[j], _FeaDespVec[j], _origMatrix[i], _origMatrix[j], matches);
 
             // 图像特征匹配点对超过N对才认为有效
             if (matches.size() > 50)
@@ -345,12 +206,115 @@ SINT32 BA_Task::CalMatchMat(float fiou_thre)
             cv::imshow("Feature Matches", img_matches);
             cv::waitKey(0);
 #endif
+
+            // 添加匹配点对的残差块
+            for (int m = 0; m < matches.size(); m++)
+            {
+                auto mc = matches[m];
+                // 注意：这里不对，应该找匹配点！！ todo 节后完成
+                cv::KeyPoint keypoint_i = _FeaPtVec[i][mc.queryIdx];
+                cv::KeyPoint keypoint_j = _FeaPtVec[j][mc.trainIdx];
+
+                ceres::LossFunction* loss_function = new ceres::HuberLoss(15);
+
+                ceres::CostFunction* cost_function =
+                    new ceres::AutoDiffCostFunction<HomographyResidual, 2, 8, 8>(
+                        new HomographyResidual(keypoint_i, keypoint_j));
+                problem.AddResidualBlock(cost_function, loss_function, h_list[i], h_list[j]);
+            }
+
+            nParaCnt += matches.size();
+            
         }
     }
-    
 
 
 
+    // 配置求解器
+    ceres::Solver::Options options;
+    options.max_num_iterations = 2; // 增加最大迭代次数
+    options.function_tolerance = 1e-8; // 设置更严格的函数值容忍度
+    options.gradient_tolerance = 1e-10; // 设置更严格的梯度容忍度
+    options.parameter_tolerance = 1e-10; // 设置更严格的参数容忍度
+    options.minimizer_progress_to_stdout = true;
+    //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 使用稀疏求解器
+    options.num_threads = 16; // 使用多线程
+    ceres::Solver::Summary summary;
+
+    // 求解
+    ceres::Solve(options, &problem, &summary);
+
+    //// 将优化后的参数转换回 cv::Mat
+    for (int i = 0; i < _currMatrix.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;
+
+
+    remap(_currMatrix, "_currMatrix");
+
+
+    cv::waitKey(0);
+}
+
+void BA_Task::remap(vector<cv::Mat_<double>> H_Vec, std::string winname)
+{
+    cv::Mat image(1500, 1500, CV_8UC3, cv::Scalar(0, 0, 0));
+
+    for (size_t i = 0; i < _imgVec.size(); i++)
+    {
+        cv::Mat imagetmp(1500, 1500, CV_8UC3, cv::Scalar(0, 0, 0));
+        cv::warpPerspective(_imgVec[i], imagetmp, H_Vec[i], imagetmp.size());
+
+        cv::Mat mask = cv::Mat::ones(_imgVec[i].size(), CV_8UC1) * 255;
+        cv::Mat warped_mask;
+        cv::warpPerspective(mask, warped_mask, H_Vec[i], image.size());
+        imagetmp.copyTo(image, warped_mask);
+    }
+
+    imshow(winname, image);
+    cv::waitKey(1);
+
+}
+
+
+
+//#define SHOW_MATCH
+SINT32 BA_Task::CalMatchMat(float fiou_thre)
+{
+    _IOUMat = cv::Mat::zeros(_polygon.size(),_polygon.size(),CV_32FC1);
+
+    _MatchMat = cv::Mat::zeros(_polygon.size(), _polygon.size(), CV_32SC1);
+
+
+    // 先计算IOU矩阵
+    for (size_t i = 0; i < _polygon.size(); i++)
+    {
+        vector<cv::Point2f> poly_i = _polygon[i];
+
+        for (size_t j = i + 1; j < _polygon.size(); j++)
+        {
+            if (i == j)
+            {
+                _IOUMat.at<float>(i,j) = 1; 
+                continue;
+            }
+            vector<cv::Point2f> poly_j = _polygon[j];
+
+            float fiou = computeQuadrilateralIOU(poly_i,poly_j);
+            _IOUMat.at<float>(i,j) = fiou;
+            _IOUMat.at<float>(j,i) = fiou;//是对称矩阵
+            
+        }
+    }
+
+
     return 0;
 }
 

stitch/src/Arith_BATask.h

@@ -39,7 +39,7 @@ public:
 	void optimizeBA();
 
 	// 使用H矩阵重投影所有帧
-	void remap();
+	void remap(vector<cv::Mat_<double>>H_Vec,std::string winname);
 
 private:
 	SINT32 CalMatchMat(float fiou_thre);//计算匹配性矩阵(以IOU约束)

stitch/src/Arith_FeaMatch.cpp

@@ -92,7 +92,7 @@ void FeatureMatcher::initDetector()
     switch (detectorType_)
     {
     case SIFT:
-        detector_ = cv::SIFT::create(500);
+        detector_ = cv::SIFT::create(2000);
         break;
     case SURF:
         //detector_ = cv::xfeatures2d::SURF::create();
@@ -103,7 +103,7 @@ void FeatureMatcher::initDetector()
 #endif
         break;
     case ORB:
-        detector_ = cv::ORB::create(500);
+        detector_ = cv::ORB::create(2000);
         break;
     default:
         throw std::invalid_argument("Unsupported feature detector type");

stitch/src/Arith_GeoStitcher.cpp

@@ -28,14 +28,14 @@ PanInfo GeoStitcher::InitMap(FrameInfo info)
 	PanInfo panPara = {0};
 	panPara.m_pan_width = 1000;//全景宽
 	panPara.m_pan_height = 1000;//全景高
-	panPara.scale = 0.3;//比例尺,1m = ?pix
+	panPara.scale = 0.5;//比例尺,1m = ?pix
 
 	// 直接无平移解算
 	auto cur = Trans_Geo2pan(ct_geo, panPara);
 
 	// 计算平移到全景图固定点的平移量，从此处开始拼接
-	int planX = 500;
+	int planX = 200;
-	int planY = 500;
+	int planY = 200;
 
 	panPara.map_shiftX = planX - (cur.x);//平移X
 	panPara.map_shiftY = planY - (cur.y);//平移Y