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,134 @@
+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})
+	# Build-only shim for 3rd-party libs that include legacy <opencv2/core/core.hpp>
+	# (DBoW3 does this). The real header errors out during OpenCV's own build.
+	target_include_directories(${the_module} BEFORE PRIVATE ${CMAKE_CURRENT_LIST_DIR}/shim)
+
+	# 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()
+
+	# Optional DBoW3 for BoW-based loop detection
+	find_package(DBoW3 QUIET)
+	if(DBoW3_FOUND)
+		set(_dbow3_ok FALSE)
+		if(TARGET DBoW3::DBoW3)
+			target_link_libraries(${the_module} PRIVATE DBoW3::DBoW3)
+			set(_dbow3_ok TRUE)
+		else()
+			# Some DBoW3 installs do not export an imported target. In that case do
+			# NOT try to synthesize an imported target via IMPORTED_LOCATION because
+			# DBoW3_LIBRARIES is often a LIST (e.g. OpenCV targets + DBoW3), which
+			# would generate invalid Makefile rules.
+			if(DBoW3_INCLUDE_DIRS)
+				target_include_directories(${the_module} PRIVATE ${DBoW3_INCLUDE_DIRS})
+			elseif(DBoW3_INCLUDE_DIR)
+				target_include_directories(${the_module} PRIVATE ${DBoW3_INCLUDE_DIR})
+			endif()
+			# Link only the DBoW3 library itself. Some legacy FindDBoW3 scripts set
+			# DBoW3_LIBRARIES to include OpenCV libraries (or even opencv_slam), which
+			# would create circular/self links in an OpenCV superbuild.
+			set(_dbow3_libs "")
+			if(DBoW3_LIBRARIES)
+				set(_dbow3_libs ${DBoW3_LIBRARIES})
+			elseif(DBoW3_LIBRARY)
+				set(_dbow3_libs ${DBoW3_LIBRARY})
+			endif()
+			if(_dbow3_libs)
+				# Prefer entries that look like the DBoW3 library.
+				set(_dbow3_only ${_dbow3_libs})
+				list(FILTER _dbow3_only INCLUDE REGEX ".*[Dd][Bb][Oo][Ww]3.*")
+				if(_dbow3_only)
+					set(_dbow3_libs ${_dbow3_only})
+				else()
+					# Otherwise, remove OpenCV libs/targets to avoid circular dependencies.
+					list(FILTER _dbow3_libs EXCLUDE REGEX "^(opencv_.*|OpenCV::.*)$")
+					list(FILTER _dbow3_libs EXCLUDE REGEX "^opencv_slam$")
+				endif()
+				target_link_libraries(${the_module} PRIVATE ${_dbow3_libs})
+				set(_dbow3_ok TRUE)
+			endif()
+		endif()
+		if(_dbow3_ok)
+			message(STATUS "slam: DBoW3 found — enabling HAVE_DBOW3")
+			target_compile_definitions(${the_module} PRIVATE HAVE_DBOW3=1)
+			add_definitions(-DHAVE_DBOW3=1)
+		else()
+			message(WARNING "slam: DBoW3 found but no link information (DBoW3::DBoW3 / DBoW3_LIBRARIES / DBoW3_LIBRARY). BoW disabled.")
+		endif()
+	else()
+		message(STATUS "slam: DBoW3 not found — HAVE_DBOW3 disabled")
+	endif()
+
+	# Try to detect g2o (headers + required libs). We need core + stuff +
+	# types_sba for the SE3/pose graph edges used in this module. Optional libs
+	# are linked when available (slam3d/sim3/solvers) to improve compatibility
+	# with different g2o builds.
+	find_path(G2O_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h)
+	find_library(G2O_CORE_LIBRARY NAMES g2o_core g2o)
+	find_library(G2O_STUFF_LIBRARY NAMES g2o_stuff)
+	find_library(G2O_TYPES_SBA_LIBRARY NAMES g2o_types_sba)
+
+	set(_g2o_required_libs ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_TYPES_SBA_LIBRARY})
+	list(FILTER _g2o_required_libs EXCLUDE REGEX "NOTFOUND")
+
+	if(G2O_INCLUDE_DIR AND G2O_CORE_LIBRARY AND G2O_STUFF_LIBRARY AND G2O_TYPES_SBA_LIBRARY)
+		message(STATUS "slam: g2o found (core/stuff/types_sba) — enabling HAVE_G2O")
+		target_include_directories(${the_module} PRIVATE ${G2O_INCLUDE_DIR})
+
+		set(_g2o_libs ${_g2o_required_libs})
+		foreach(_g2o_candidate g2o_types_slam3d g2o_types_slam3d_addons g2o_types_sim3 g2o_types_data g2o_solver_csparse g2o_solver_cholmod g2o_solver_eigen g2o_solver_dense g2o_solver_pcg)
+			find_library(_g2o_lib NAMES ${_g2o_candidate})
+			if(_g2o_lib)
+				list(APPEND _g2o_libs ${_g2o_lib})
+			endif()
+		endforeach()
+		list(REMOVE_DUPLICATES _g2o_libs)
+		target_link_libraries(${the_module} PRIVATE ${_g2o_libs})
+		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 (need headers + core/stuff/types_sba)")
+	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,29 @@
+// 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/tracker.hpp"
+#include "opencv2/slam/visualizer.hpp"
+#include "opencv2/slam/visual_odometry.hpp"
+#include "opencv2/slam/slam_system.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,76 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <string>
+#include <vector>
+
+namespace cv{
+namespace vo{
+
+CV_EXPORTS bool ensureDirectoryExists(const std::string &dir);
+
+/**
+ * @brief Simple helper to iterate image sequences on disk.
+ *
+ * DataLoader enumerates image files in a directory and provides simple
+ * sequential access for samples and demos. This class does not perform
+ * any image decoding policy beyond forwarding files to OpenCV's IO.
+ */
+class CV_EXPORTS DataLoader {
+public:
+    /**
+     * @brief Construct a DataLoader for the given image directory.
+     * @param imageDir Directory containing image files (absolute or relative path).
+     */
+    DataLoader(const std::string &imageDir);
+
+    /**
+     * @brief Load the next image in the sequence.
+     * @param image Output image (decoded by OpenCV).
+     * @param imagePath Output path of the loaded image file.
+     * @return True if an image was loaded; false when the sequence has ended.
+     */
+    bool getNextImage(Mat &image, std::string &imagePath);
+
+    /**
+     * @brief Reset the internal iterator to the beginning of the sequence.
+     */
+    void reset();
+
+    /**
+     * @brief Check whether more images are available.
+     * @return True when there are remaining images to read.
+     */
+    bool hasNext() const;
+
+    /**
+     * @brief Get total number of images discovered in the directory.
+     * @return Number of image files.
+     */
+    size_t size() const;
+
+    /**
+     * @brief Try to load camera intrinsics from a YAML file.
+     * @param yamlPath Path to the YAML file containing camera parameters.
+     * @return True on success, false otherwise.
+     */
+    bool loadIntrinsics(const std::string &yamlPath);
+
+    /** @brief Focal length in x. */
+    double fx() const { return fx_; }
+    /** @brief Focal length in y. */
+    double fy() const { return fy_; }
+    /** @brief Principal point x-coordinate. */
+    double cx() const { return cx_; }
+    /** @brief Principal point y-coordinate. */
+    double cy() const { return cy_; }
+
+private:
+    std::vector<std::string> imageFiles;
+    size_t currentIndex;
+
+    // Camera intrinsics (fallback values when not loaded)
+    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