diff --git a/main.cpp b/main.cpp index f7f8e9e..decd1aa 100644 --- a/main.cpp +++ b/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 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; diff --git a/stitch/src/Arith_BAGraph.cpp b/stitch/src/Arith_BAGraph.cpp new file mode 100644 index 0000000..e69de29 diff --git a/stitch/src/Arith_BAGraph.h b/stitch/src/Arith_BAGraph.h new file mode 100644 index 0000000..3a3dd6e --- /dev/null +++ b/stitch/src/Arith_BAGraph.h @@ -0,0 +1,64 @@ +#include +#include +#include +#include + +// 定义相机节点 +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 getObservationEdges() { + return observations; + } + +private: + std::map cameras; // 相机节点集合 + std::map points; // 三维点节点集合 + std::vector observations; // 观测边集合 +}; \ No newline at end of file diff --git a/stitch/src/Arith_ExtrinsicStitcher.cpp b/stitch/src/Arith_ExtrinsicStitcher.cpp index 31c50cf..95eb9e4 100644 --- a/stitch/src/Arith_ExtrinsicStitcher.cpp +++ b/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, ptOrig); + PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, originPoint); Mat move = Mat::zeros(3, 1, CV_64F); diff --git a/stitch/src/Arith_ExtrinsicStitcher.h b/stitch/src/Arith_ExtrinsicStitcher.h index bf94780..318bf8f 100644 --- a/stitch/src/Arith_ExtrinsicStitcher.h +++ b/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;//成图初始点的地心地固坐标，作为拼接参考 }; \ No newline at end of file diff --git a/stitch/src/Arith_FeaMatch.cpp b/stitch/src/Arith_FeaMatch.cpp new file mode 100644 index 0000000..a8f683c --- /dev/null +++ b/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& keypoints, cv::Mat& descriptors) +{ + detector_->detectAndCompute(image, cv::noArray(), keypoints, descriptors); +} + +// 匹配特征点 +void FeatureMatcher::matchFeatures(cv::Mat& descriptors1, cv::Mat& descriptors2, std::vector& matches) +{ + if (matcherType_ == FLANN) + { + flannMatcher_->match(descriptors1, descriptors2, matches); + } + else + { + bfMatcher_->match(descriptors1, descriptors2, matches); + } +} + +// 计算单应性矩阵 +cv::Mat FeatureMatcher::computeHomography(std::vector& keypoints1, std::vector& keypoints2, + std::vector& matches, double ransacReprojThreshold) +{ + std::vector 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::makePtr(12, 20, 2))); + } + } + else + { + if (detectorType_ == SIFT || detectorType_ == SURF) + { + bfMatcher_ = cv::BFMatcher::create(cv::NORM_L2); + } + else + { + bfMatcher_ = cv::BFMatcher::create(cv::NORM_HAMMING); + } + } +} \ No newline at end of file diff --git a/stitch/src/Arith_FeaMatch.h b/stitch/src/Arith_FeaMatch.h new file mode 100644 index 0000000..365d230 --- /dev/null +++ b/stitch/src/Arith_FeaMatch.h @@ -0,0 +1,57 @@ +#ifndef FEATURE_MATCHER_H +#define FEATURE_MATCHER_H + +#include +#include +#include +#ifdef HAVE_OPENCV_XFEATURES2D +#include +#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& keypoints, cv::Mat& descriptors); + + // 匹配特征点 + void matchFeatures(cv::Mat& descriptors1, cv::Mat& descriptors2, std::vector& matches); + + // 计算单应性矩阵 + cv::Mat computeHomography(std::vector& keypoints1, std::vector& keypoints2, + std::vector& matches, double ransacReprojThreshold = 3.0); + +private: + // 初始化特征检测器 + void initDetector(); + + // 初始化匹配器 + void initMatcher(); + + DetectorType detectorType_; // 特征检测器类型 + MatcherType matcherType_; // 匹配器类型 + cv::Ptr detector_; // 特征检测器 + cv::Ptr flannMatcher_; // FLANN匹配器 + cv::Ptr bfMatcher_; // 暴力匹配器 +}; + +#endif // FEATURE_MATCHER_H \ No newline at end of file diff --git a/stitch/src/Arith_VideoStitch.cpp b/stitch/src/Arith_VideoStitch.cpp index abbe919..a2718c3 100644 --- a/stitch/src/Arith_VideoStitch.cpp +++ b/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 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); diff --git a/stitch/src/Arith_VideoStitch.h b/stitch/src/Arith_VideoStitch.h index 40e2693..50ded57 100644 --- a/stitch/src/Arith_VideoStitch.h +++ b/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; }; \ No newline at end of file