Integrating Modular SLAM Pipeline into OpenCV Contrib

modules/sfm/include/opencv2/sfm.hpp

@@ -42,7 +42,7 @@
 #include <opencv2/sfm/numeric.hpp>
 #include <opencv2/sfm/projection.hpp>
 #include <opencv2/sfm/triangulation.hpp>
-#if CERES_FOUND
+#if defined(CERES_FOUND) && CERES_FOUND
 #include <opencv2/sfm/reconstruct.hpp>
 #include <opencv2/sfm/simple_pipeline.hpp>
 #endif

modules/slam/CMakeLists.txt

@@ -0,0 +1,62 @@
+set(the_description "SLAM module: visual odometry / mapping / localization utilities")
+
+# Core dependencies used by the sources (see includes in src/ and include/)
+ocv_define_module(slam
+	opencv_core
+	opencv_imgproc
+	opencv_calib3d
+	opencv_features2d
+	OPTIONAL opencv_sfm
+	opencv_imgcodecs
+	opencv_highgui
+	opencv_video
+	WRAP python
+)
+
+# Detect optional dependencies and expose preprocessor macros so sources
+# can use #if defined(HAVE_SFM) / #if defined(HAVE_G2O)
+if(TARGET ${the_module})
+	# opencv_sfm is declared OPTIONAL above; if its target exists enable HAVE_SFM
+	if(TARGET opencv_sfm)
+		message(STATUS "slam: opencv_sfm target found — enabling HAVE_SFM")
+		target_compile_definitions(${the_module} PRIVATE HAVE_SFM=1)
+			# also add a global definition so header-parsers (python generator)
+			# running during build/config time can evaluate #if HAVE_SFM
+			add_definitions(-DHAVE_SFM=1)
+		target_link_libraries(${the_module} PRIVATE opencv_sfm)
+	else()
+		message(STATUS "slam: opencv_sfm not found — HAVE_SFM disabled")
+	endif()
+
+	# Try to detect g2o (header + library). This is a best-effort detection: we
+	# look for a common g2o header and try to find a library name. Users on
+	# Windows should install/build g2o and make it discoverable to CMake.
+	find_path(G2O_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h)
+	find_library(G2O_LIBRARY NAMES g2o_core g2o)
+	if(G2O_INCLUDE_DIR AND G2O_LIBRARY)
+		message(STATUS "slam: g2o found (headers and library) — enabling HAVE_G2O")
+		target_include_directories(${the_module} PRIVATE ${G2O_INCLUDE_DIR})
+		target_link_libraries(${the_module} PRIVATE ${G2O_LIBRARY})
+		target_compile_definitions(${the_module} PRIVATE HAVE_G2O=1)
+			# also expose globally so external header parsers can evaluate #if HAVE_G2O
+			add_definitions(-DHAVE_G2O=1)
+	else()
+		message(STATUS "slam: g2o not found — HAVE_G2O disabled")
+	endif()
+endif()
+
+# Ensure C++17 is enabled for this module (required for <filesystem>)
+if(TARGET ${the_module})
+	set_target_properties(${the_module} PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED ON)
+	# For older GCC/libstdc++ (<9) we may need to link stdc++fs
+	if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")
+		target_link_libraries(${the_module} PRIVATE stdc++fs)
+	endif()
+endif()
+
+# If you need to add special include directories, libraries or compile flags,
+# add them below using the OpenCV contrib module helper macros, for example:
+#
+# ocv_include_directories(${CMAKE_CURRENT_LIST_DIR}/include)
+# ocv_module_compile_options(-Wall)
+#

modules/slam/include/opencv2/slam.hpp

@@ -0,0 +1,27 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#ifndef __OPENCV_SLAM_HPP__
+#define __OPENCV_SLAM_HPP__
+
+#include "opencv2/slam/data_loader.hpp"
+#include "opencv2/slam/feature.hpp"
+#include "opencv2/slam/initializer.hpp"
+#include "opencv2/slam/keyframe.hpp"
+#include "opencv2/slam/localizer.hpp"
+#include "opencv2/slam/map.hpp"
+#include "opencv2/slam/matcher.hpp"
+#include "opencv2/slam/optimizer.hpp"
+#include "opencv2/slam/pose.hpp"
+#include "opencv2/slam/visualizer.hpp"
+#include "opencv2/slam/vo.hpp"
+
+
+/** @defgroup slam Simultaneous Localization and Mapping
+  @brief Simultaneous Localization and Mapping (SLAM) module
+*/
+
+#endif
+
+/* End of file. */

modules/slam/include/opencv2/slam/data_loader.hpp

@@ -0,0 +1,46 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <string>
+#include <vector>
+
+namespace cv{
+namespace vo{
+
+bool ensureDirectoryExists(const std::string &dir);
+
+class DataLoader {
+public:
+    // 构造：传入图像目录（可以是相对或绝对路径）
+    DataLoader(const std::string &imageDir);
+
+    // 获取下一张图像，成功返回 true 并填充 image 与 imagePath；到末尾返回 false
+    bool getNextImage(Mat &image, std::string &imagePath);
+
+    // 重置到序列开始
+    void reset();
+
+    // 是否还有图像
+    bool hasNext() const;
+
+    // 图像总数
+    size_t size() const;
+
+    // 尝试加载并返回相机内参（fx, fy, cx, cy），返回是否成功
+    bool loadIntrinsics(const std::string &yamlPath);
+
+    // 内参访问
+    double fx() const { return fx_; }
+    double fy() const { return fy_; }
+    double cx() const { return cx_; }
+    double cy() const { return cy_; }
+
+private:
+    std::vector<std::string> imageFiles;
+    size_t currentIndex;
+
+    // 相机内参（若未加载则为回退值）
+    double fx_, fy_, cx_, cy_;
+};
+
+} // namespace vo
+} // namespace cv

modules/slam/include/opencv2/slam/feature.hpp

@@ -0,0 +1,38 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <opencv2/features2d.hpp>
+#include <vector>
+
+namespace cv {
+namespace vo {
+
+class FeatureExtractor {
+public:
+    explicit FeatureExtractor(int nfeatures = 2000);
+    // detectAndCompute: detect keypoints and compute descriptors.
+    // If previous-frame data (prevGray, prevKp) is provided, a flow-aware grid allocation
+    // will be used (score = response * (1 + flow_lambda * normalized_flow)). Otherwise a
+    // simpler ANMS selection is used. The prev arguments have defaults so this function
+    // replaces the two previous overloads.
+    void detectAndCompute(const Mat &image, std::vector<KeyPoint> &kps, Mat &desc,
+                          const Mat &prevGray = Mat(), const std::vector<KeyPoint> &prevKp = std::vector<KeyPoint>(),
+                          double flow_lambda = 5.0);
+private:
+    Ptr<ORB> orb_;
+    int nfeatures_;
+};
+
+// Function to detect and compute features in an image
+inline void detectAndComputeFeatures(const Mat &image,
+                                     std::vector<KeyPoint> &keypoints,
+                                     Mat &descriptors) {
+    // Create ORB detector and descriptor
+    auto orb = ORB::create();
+    // Detect keypoints
+    orb->detect(image, keypoints);
+    // Compute descriptors
+    orb->compute(image, keypoints, descriptors);
+}
+
+} // namespace vo
+} // namespace cv

modules/slam/include/opencv2/slam/initializer.hpp

@@ -0,0 +1,87 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <opencv2/features2d.hpp>
+#include <vector>
+#include "opencv2/slam/keyframe.hpp"
+#include "opencv2/slam/map.hpp"
+
+namespace cv {
+namespace vo {
+
+class Initializer {
+public:
+    Initializer();
+
+    // Attempt initialization with two frames
+    // Returns true if initialization successful
+    bool initialize(const std::vector<KeyPoint> &kps1,
+                   const std::vector<KeyPoint> &kps2,
+                   const std::vector<DMatch> &matches,
+                   double fx, double fy, double cx, double cy,
+                   Mat &R, Mat &t,
+                   std::vector<Point3d> &points3D,
+                   std::vector<bool> &isTriangulated);
+
+    // Check if frames have sufficient parallax for initialization
+    static bool checkParallax(const std::vector<KeyPoint> &kps1,
+                             const std::vector<KeyPoint> &kps2,
+                             const std::vector<DMatch> &matches,
+                             double minMedianParallax = 15.0);
+
+private:
+    // Reconstruct from Homography
+    bool reconstructH(const std::vector<Point2f> &pts1,
+                     const std::vector<Point2f> &pts2,
+                     const Mat &H,
+                     double fx, double fy, double cx, double cy,
+                     Mat &R, Mat &t,
+                     std::vector<Point3d> &points3D,
+                     std::vector<bool> &isTriangulated,
+                     float &parallax);
+
+    // Reconstruct from Fundamental/Essential
+    bool reconstructF(const std::vector<Point2f> &pts1,
+                     const std::vector<Point2f> &pts2,
+                     const Mat &F,
+                     double fx, double fy, double cx, double cy,
+                     Mat &R, Mat &t,
+                     std::vector<Point3d> &points3D,
+                     std::vector<bool> &isTriangulated,
+                     float &parallax);
+
+    // Check reconstructed points quality
+    int checkRT(const Mat &R, const Mat &t,
+               const std::vector<Point2f> &pts1,
+               const std::vector<Point2f> &pts2,
+               const std::vector<Point3d> &points3D,
+               std::vector<bool> &isGood,
+               double fx, double fy, double cx, double cy,
+               float &parallax);
+
+    // Triangulate points
+    void triangulate(const Mat &P1, const Mat &P2,
+                    const std::vector<Point2f> &pts1,
+                    const std::vector<Point2f> &pts2,
+                    std::vector<Point3d> &points3D);
+
+    // Decompose Homography
+    void decomposeH(const Mat &H, std::vector<Mat> &Rs,
+                   std::vector<Mat> &ts, std::vector<Mat> &normals);
+
+    // Compute homography score
+    float computeScore(const Mat &H21, const Mat &H12,
+                      const std::vector<Point2f> &pts1,
+                      const std::vector<Point2f> &pts2,
+                      std::vector<bool> &inliersH,
+                      float sigma = 1.0);
+
+    // Compute fundamental score
+    float computeScoreF(const Mat &F21,
+                       const std::vector<Point2f> &pts1,
+                       const std::vector<Point2f> &pts2,
+                       std::vector<bool> &inliersF,
+                       float sigma = 1.0);
+};
+
+} // namespace vo
+} // namespace cv

modules/slam/include/opencv2/slam/keyframe.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <opencv2/features2d.hpp>
+#include <vector>
+
+namespace cv {
+namespace vo {
+
+struct KeyFrame {
+    int id = -1;
+    Mat image; // optional
+    std::vector<KeyPoint> kps;
+    Mat desc;
+    // pose: rotation and translation to map coordinates
+    // X_world = R * X_cam + t
+    Mat R_w = Mat::eye(3,3,CV_64F);
+    Mat t_w = Mat::zeros(3,1,CV_64F);
+};
+
+} // namespace vo
+} // namespace cv

modules/slam/include/opencv2/slam/localizer.hpp

@@ -0,0 +1,32 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <vector>
+#include "opencv2/slam/data_loader.hpp"
+#include "opencv2/slam/map.hpp"
+
+namespace cv {
+namespace vo {
+
+class Localizer {
+public:
+    Localizer(float ratio = 0.7f);
+
+    // Try to solve PnP against map points. Returns true if solved and fills R_out/t_out and inliers.
+    // Provide camera intrinsics and image dimensions explicitly (DataLoader doesn't expose width/height).
+    // Enhanced tryPnP with optional diagnostics output (frame id, image and output directory)
+    // out_preMatches and out_postMatches can be nullptr. If outDir provided, diagnostics images/logs will be saved there.
+
+    bool tryPnP(const MapManager &map, const Mat &desc, const std::vector<KeyPoint> &kps,
+                double fx, double fy, double cx, double cy, int imgW, int imgH,
+                int min_inliers,
+                Mat &R_out, Mat &t_out, int &inliers_out,
+                int frame_id = -1, const Mat *image = nullptr, const std::string &outDir = "",
+                int *out_preMatches = nullptr, int *out_postMatches = nullptr, double *out_meanReproj = nullptr) const;
+
+    float ratio() const { return ratio_; }
+private:
+    float ratio_ = 0.7f;
+};
+
+} // namespace vo
+} // namespace cv