完成BA，解决尺度一致性

stitch/src/Arith_BATask.cpp

@@ -1,11 +1,12 @@
 #include "Arith_BATask.h"
 #include "ceres/ceres.h"
+#include "math.h"
 using namespace ceres;
 // 定义残差结构体
 struct HomographyResidual 
 {
-    HomographyResidual(const cv::KeyPoint& keypoint_i, const cv::KeyPoint& keypoint_j)
-        : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j) {}
+    HomographyResidual(const cv::KeyPoint& keypoint_i, const cv::KeyPoint& keypoint_j,const cv::Mat H1, const cv::Mat H2)
+        : keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),H1_(H1),H2_(H2) {}
 
     template <typename T>
     bool operator()(const T* const h_i, const T* const h_j, T* residual) const 
@@ -33,7 +34,7 @@ struct HomographyResidual
                 P_j[row] += H_j[row * 3 + col] * p_j[col];
             }
         }
-
+         
         P_i[0] /= P_i[2];
         P_i[1] /= P_i[2];
         P_j[0] /= P_j[2];
@@ -42,6 +43,19 @@ struct HomographyResidual
         residual[0] = P_i[0] - P_j[0];
         residual[1] = P_i[1] - P_j[1];
 
+        // 计算单应性矩阵的范数（正则化项）
+        T norm_H_i = sqrt(h_i[0] * h_i[0] + h_i[1] * h_i[1] + h_i[2] * h_i[2] +
+            h_i[3] * h_i[3] + h_i[4] * h_i[4] + h_i[5] * h_i[5] +
+            h_i[6] * h_i[6] + h_i[7] * h_i[7] + T(1.0));
+
+        T norm_H_j = sqrt(h_j[0] * h_j[0] + h_j[1] * h_j[1] + h_j[2] * h_j[2] +
+            h_j[3] * h_j[3] + h_j[4] * h_j[4] + h_j[5] * h_j[5] +
+            h_j[6] * h_j[6] + h_j[7] * h_j[7] + T(1.0));
+
+        // 添加正则化项到残差
+        residual[2] = norm_H_i - T(cv::norm(H1_));
+        residual[3] = norm_H_j - T(cv::norm(H2_));
+
         return true;
     }
 
@@ -49,6 +63,9 @@ struct HomographyResidual
 private:
     const cv::KeyPoint keypoint_i_; // 第 i 帧图像中的特征点
     const cv::KeyPoint keypoint_j_; // 第 j 帧图像中的特征点
+
+    const cv::Mat H1_;
+	const cv::Mat H2_;
 };
 
 
@@ -110,7 +127,6 @@ void BA_Task::Test()
 //#define SHOW_MATCH
 void BA_Task::optimizeBA()
 {
-
     remap(_origMatrix, "_origMatrix");
 
     // 计算匹配性矩阵
@@ -121,7 +137,6 @@ void BA_Task::optimizeBA()
     // 将 cv::Mat 转换为 Ceres 需要的数组形式
     std::vector<double*> h_list(_currMatrix.size());
 
-
     for (int i = 0; i < _currMatrix.size(); i++)
     {
         h_list[i] = (double*)_currMatrix[i].data;
@@ -217,9 +232,12 @@ void BA_Task::optimizeBA()
 
                 ceres::LossFunction* loss_function = new ceres::HuberLoss(15);
 
+				cv::Mat H1 = _origMatrix[i];
+                cv::Mat H2 = _origMatrix[j];
+
                 ceres::CostFunction* cost_function =
-                    new ceres::AutoDiffCostFunction<HomographyResidual, 2, 8, 8>(
-                        new HomographyResidual(keypoint_i, keypoint_j));
+                    new ceres::AutoDiffCostFunction<HomographyResidual, 4, 8, 8>(
+                        new HomographyResidual(keypoint_i, keypoint_j,H1,H2));
                 problem.AddResidualBlock(cost_function, loss_function, h_list[i], h_list[j]);
             }
 
@@ -228,11 +246,9 @@ void BA_Task::optimizeBA()
         }
     }
 
-
-
     // 配置求解器
     ceres::Solver::Options options;
-    options.max_num_iterations = 2; // 增加最大迭代次数
+    options.max_num_iterations = 1; // 增加最大迭代次数
     options.function_tolerance = 1e-8; // 设置更严格的函数值容忍度
     options.gradient_tolerance = 1e-10; // 设置更严格的梯度容忍度
     options.parameter_tolerance = 1e-10; // 设置更严格的参数容忍度
@@ -321,6 +337,11 @@ SINT32 BA_Task::CalMatchMat(float fiou_thre)
 
 
 
+float F_Norm(cv::Mat H)
+{
+    return 0.0f;
+}
+
 // 函数：绘制多边形
 void drawPolygons(cv::Mat& image, const std::vector<std::vector<cv::Point2f>>& polygons) 
 {

stitch/src/Arith_BATask.h

@@ -68,6 +68,6 @@ private:
 
 };
 	
-
+float F_Norm(cv::Mat H);
 
 void drawPolygons(cv::Mat& image, const std::vector<std::vector<cv::Point2f>>& polygons);

stitch/src/Arith_FeaMatch.cpp

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