一些拓扑结构理解

main.cpp

@@ -133,6 +133,15 @@ int main(int, char**)
         Map16BitTo8Bit(pFrameIR,IMAGE_WIDTH_IR*IMAGE_HEIGHT_IR,pImageIR);
 
         cv::Mat mat_src(IMAGE_HEIGHT_IR, IMAGE_WIDTH_IR, CV_8UC1, pImageIR);
+
+
+        //auto  detector_ = cv::ORB::create();
+        //std::vector<cv::KeyPoint> keypoints; 
+        //cv::Mat descriptors;
+        //detector_->detectAndCompute(mat_src, cv::noArray(), keypoints, descriptors);
+
+
+
 		frame.enPixelFormat = GD_PIXEL_FORMAT_GRAY_Y8;
 		frame.u32Width = IMAGE_WIDTH_IR;
 		frame.u32Height = IMAGE_HEIGHT_IR;

stitch/src/Arith_BAGraph.h

@@ -0,0 +1,64 @@
+#include <opencv2/opencv.hpp>
+#include <iostream>
+#include <map>
+#include <vector>
+
+// 定义相机节点
+struct CameraNode {
+    int cameraId;  // 相机 ID
+    cv::Mat R;     // 旋转矩阵
+    cv::Mat t;     // 平移向量
+    cv::Mat K;     // 内参矩阵
+};
+
+// 定义三维点节点
+struct PointNode {
+    int pointId;  // 三维点 ID
+    cv::Point3f X;  // 三维点坐标
+};
+
+// 定义观测边
+struct ObservationEdge {
+    int cameraId;  // 相机 ID
+    int pointId;   // 三维点 ID
+    cv::Point2f x;  // 观测到的二维点坐标
+};
+
+// 定义 Bundle Adjustment 图结构
+class BundleAdjustmentGraph {
+public:
+    // 添加相机节点
+    void addCameraNode(const CameraNode& camera) {
+        cameras[camera.cameraId] = camera;
+    }
+
+    // 添加三维点节点
+    void addPointNode(const PointNode& point) {
+        points[point.pointId] = point;
+    }
+
+    // 添加观测边
+    void addObservationEdge(const ObservationEdge& edge) {
+        observations.push_back(edge);
+    }
+
+    // 获取相机节点
+    CameraNode getCameraNode(int cameraId) {
+        return cameras[cameraId];
+    }
+
+    // 获取三维点节点
+    PointNode getPointNode(int pointId) {
+        return points[pointId];
+    }
+
+    // 获取观测边
+    std::vector<ObservationEdge> getObservationEdges() {
+        return observations;
+    }
+
+private:
+    std::map<int, CameraNode> cameras;  // 相机节点集合
+    std::map<int, PointNode> points;    // 三维点节点集合
+    std::vector<ObservationEdge> observations;  // 观测边集合
+};

stitch/src/Arith_ExtrinsicStitcher.cpp

@@ -63,7 +63,7 @@ cv::Point2f ExtrinsicStitcher::back_project(cv::Point2f pos_frame, Proj m_Proj,
 
 void ExtrinsicStitcher::SetOriginPoint(FrameInfo info)
 {
-	originPoint = info;
+	originPoint = getXYZFromBLH(info.craft.stPos);
 }
 
 cv::Point2f ExtrinsicStitcher::Trans_uv2Geo(cv::Point2f pos_frame, TForm form)
@@ -117,8 +117,7 @@ cv::Point2f ExtrinsicStitcher::Trans_Geo2pan(cv::Point2f pos_geo, PanInfo panPar
 Mat ExtrinsicStitcher::Mat_TransENGMove(FrameInfo info)
 {
 	PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos);
-	PointXYZ ptOrig = getXYZFromBLH(originPoint.craft.stPos);
+	PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, originPoint);
-	PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, ptOrig);
 
 	Mat move = Mat::zeros(3, 1, CV_64F);
 

stitch/src/Arith_ExtrinsicStitcher.h

@@ -46,7 +46,7 @@ public:
     // 反投影：从帧到全景图
     cv::Point2f back_project(cv::Point2f pos_frame, Proj m_Proj, PanInfo pan);
 
-    // 计算当前帧像方-成图坐标系R t（反投影关系）
+    // 计算当前帧像方-地理坐标系R t（反投影关系）
     Proj AnlayseTform(FrameInfo info);
 
 public:
@@ -75,5 +75,5 @@ private:
     // 设置起始拼接点外参
     void SetOriginPoint(FrameInfo info);
 
-    FrameInfo originPoint;//成图初始点，作为拼接参考
+    PointXYZ originPoint;//成图初始点的地心地固坐标，作为拼接参考
 };

stitch/src/Arith_FeaMatch.cpp

@@ -0,0 +1,92 @@
+#include "Arith_FeaMatch.h"
+
+// 构造函数
+FeatureMatcher::FeatureMatcher(DetectorType detectorType, MatcherType matcherType)
+    : detectorType_(detectorType), matcherType_(matcherType)
+{
+    initDetector();
+    initMatcher();
+}
+
+// 提取特征点和描述符
+void FeatureMatcher::extractFeatures(cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)
+{
+    detector_->detectAndCompute(image, cv::noArray(), keypoints, descriptors);
+}
+
+// 匹配特征点
+void FeatureMatcher::matchFeatures(cv::Mat& descriptors1, cv::Mat& descriptors2, std::vector<cv::DMatch>& matches)
+{
+    if (matcherType_ == FLANN)
+    {
+        flannMatcher_->match(descriptors1, descriptors2, matches);
+    }
+    else
+    {
+        bfMatcher_->match(descriptors1, descriptors2, matches);
+    }
+}
+
+// 计算单应性矩阵
+cv::Mat FeatureMatcher::computeHomography(std::vector<cv::KeyPoint>& keypoints1, std::vector<cv::KeyPoint>& keypoints2,
+    std::vector<cv::DMatch>& matches, double ransacReprojThreshold)
+{
+    std::vector<cv::Point2f> points1, points2;
+    for (const auto& match : matches)
+    {
+        points1.push_back(keypoints1[match.queryIdx].pt);
+        points2.push_back(keypoints2[match.trainIdx].pt);
+    }
+    return cv::findHomography(points1, points2, cv::RANSAC, ransacReprojThreshold);
+}
+
+// 初始化特征检测器
+void FeatureMatcher::initDetector()
+{
+    switch (detectorType_)
+    {
+    case SIFT:
+        detector_ = cv::SIFT::create(100);
+        break;
+    case SURF:
+        //detector_ = cv::xfeatures2d::SURF::create();
+#ifdef HAVE_OPENCV_XFEATURES2D
+       detector_ = cv::xfeatures2d::SURF::create();
+#else
+        throw std::runtime_error("SURF is not supported in this OpenCV build.");
+#endif
+        break;
+    case ORB:
+        detector_ = cv::ORB::create();
+        break;
+    default:
+        throw std::invalid_argument("Unsupported feature detector type");
+    }
+}
+
+// 初始化匹配器
+void FeatureMatcher::initMatcher()
+{
+    if (matcherType_ == FLANN)
+    {
+        if (detectorType_ == SIFT || detectorType_ == SURF)
+        {
+            flannMatcher_ = cv::FlannBasedMatcher::create();
+        }
+        else
+        {
+            flannMatcher_ = cv::makePtr<cv::FlannBasedMatcher>(cv::FlannBasedMatcher(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2)));
+        }
+    }
+    else
+    {
+        if (detectorType_ == SIFT || detectorType_ == SURF)
+        {
+            bfMatcher_ = cv::BFMatcher::create(cv::NORM_L2);
+        }
+        else
+        {
+            bfMatcher_ = cv::BFMatcher::create(cv::NORM_HAMMING);
+        }
+    }
+}

stitch/src/Arith_FeaMatch.h

@@ -0,0 +1,57 @@
+#ifndef FEATURE_MATCHER_H
+#define FEATURE_MATCHER_H
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <stdexcept>
+#ifdef HAVE_OPENCV_XFEATURES2D
+#include <opencv2/xfeatures2d.hpp>
+#endif
+
+// 特征检测器类型
+enum DetectorType
+{
+    SIFT,
+    SURF,
+    ORB
+};
+
+// 匹配器类型
+enum MatcherType
+{
+    FLANN,
+    BF
+};
+class FeatureMatcher
+{
+public:
+ 
+
+    // 构造函数
+    FeatureMatcher(DetectorType detectorType, MatcherType matcherType);
+
+    // 提取特征点和描述符
+    void extractFeatures(cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors);
+
+    // 匹配特征点
+    void matchFeatures(cv::Mat& descriptors1, cv::Mat& descriptors2, std::vector<cv::DMatch>& matches);
+
+    // 计算单应性矩阵
+    cv::Mat computeHomography(std::vector<cv::KeyPoint>& keypoints1, std::vector<cv::KeyPoint>& keypoints2,
+        std::vector<cv::DMatch>& matches, double ransacReprojThreshold = 3.0);
+
+private:
+    // 初始化特征检测器
+    void initDetector();
+
+    // 初始化匹配器
+    void initMatcher();
+
+    DetectorType detectorType_;  // 特征检测器类型
+    MatcherType matcherType_;    // 匹配器类型
+    cv::Ptr<cv::Feature2D> detector_;  // 特征检测器
+    cv::Ptr<cv::DescriptorMatcher> flannMatcher_;  // FLANN匹配器
+    cv::Ptr<cv::DescriptorMatcher> bfMatcher_;     // 暴力匹配器
+};
+
+#endif // FEATURE_MATCHER_H

stitch/src/Arith_VideoStitch.cpp

@@ -23,6 +23,8 @@ void API_VideoStitch::Destroy(API_VideoStitch * obj)
 VideoStitch::VideoStitch(SINT32 nWidth, SINT32 nHeight)
 {
 	m_GeoStitcher = new ExtrinsicStitcher();
+
+	m_FeaMatcher = new FeatureMatcher(DetectorType::ORB, MatcherType::FLANN);
 	memset(&m_pan, 0, sizeof(GD_VIDEO_FRAME_S));
 }
 
@@ -82,6 +84,11 @@ BYTE8 VideoStitch::Updata(GD_VIDEO_FRAME_S img, FrameInfo para)
 {
 	Proj t_Proj = m_GeoStitcher->AnlayseTform(para);
 
+	cv::Mat frame(img.u32Height, img.u32Width, CV_8UC1, img.u64VirAddr[0]);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat descriptors;
+	m_FeaMatcher->extractFeatures(frame, keypoints, descriptors);
+
 	// 计算帧的map四至
 	cv::Point2f leftTop_map = m_GeoStitcher->back_project(cv::Point2f(0,0), t_Proj, m_panPara);
 	cv::Point2f rightTop_map = m_GeoStitcher->back_project(cv::Point2f(img.u32Width,0), t_Proj, m_panPara);

stitch/src/Arith_VideoStitch.h

@@ -1,7 +1,7 @@
 #include "API_VideoStitch.h"
 #include "opencv2/opencv.hpp"
 #include "Arith_ExtrinsicStitcher.h"
-
+#include "Arith_FeaMatch.h"
 
 class VideoStitch:public API_VideoStitch 
 {
@@ -25,6 +25,12 @@ private:
 private:
 
 	ExtrinsicStitcher* m_GeoStitcher;//外参计算
+
+    FeatureMatcher* m_FeaMatcher;//特征匹配
+
+
+
+
     PanInfo m_panPara;//全景图配置
     cv::Mat m_pan;
 };