diff --git a/modules/slam/CMakeLists.txt b/modules/slam/CMakeLists.txt new file mode 100644 index 0000000000..91bcf7f368 --- /dev/null +++ b/modules/slam/CMakeLists.txt @@ -0,0 +1,14 @@ +# 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. + +set(the_description "SLAM (Simultaneous Localization and Mapping) — visual odometry and mapping") + +ocv_define_module(slam + opencv_core + opencv_imgproc + opencv_imgcodecs + opencv_features + opencv_3d + OPTIONAL opencv_dnn + WRAP python) diff --git a/modules/slam/include/opencv2/slam.hpp b/modules/slam/include/opencv2/slam.hpp new file mode 100644 index 0000000000..2c4f84e058 --- /dev/null +++ b/modules/slam/include/opencv2/slam.hpp @@ -0,0 +1,22 @@ +// 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 "slam/types.hpp" +#include "slam/map.hpp" +#include "slam/odometry_params.hpp" +#include "slam/visual_odometry.hpp" + +/** +@defgroup slam SLAM (Simultaneous Localization and Mapping) +@{ + @defgroup slam_odometry Visual Odometry + @brief Monocular visual odometry pipeline. +@} +*/ + +#endif // OPENCV_SLAM_HPP diff --git a/modules/slam/include/opencv2/slam/map.hpp b/modules/slam/include/opencv2/slam/map.hpp new file mode 100644 index 0000000000..2da8af1633 --- /dev/null +++ b/modules/slam/include/opencv2/slam/map.hpp @@ -0,0 +1,73 @@ +// 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_MAP_HPP +#define OPENCV_SLAM_MAP_HPP + +#include "types.hpp" +#include +#include + +namespace cv { namespace slam { + +/** @brief Container for keyframes, map points, and the per-frame trajectory. + + Map owns all KeyFrame and MapPoint objects. External code holds only + raw non-owning pointers. Ids are assigned automatically when the + incoming id field is negative. + + @ingroup slam_odometry +*/ +class CV_EXPORTS_W Map +{ +public: + CV_WRAP Map(); + ~Map(); + + /** @brief Add a keyframe; assigns id if kf.id < 0. Returns the assigned id. */ + CV_WRAP int addKeyframe(KeyFrame& kf); + + /** @brief Return keyframe by id, or nullptr. */ + CV_WRAP KeyFrame* getKeyframe(int id); + + /** @brief All keyframes in insertion order. */ + CV_WRAP const std::vector& keyframes() const; + + CV_WRAP int numKeyframes() const; + + /** @brief Add a map point; assigns id if mp.id < 0. Returns the assigned id. */ + CV_WRAP int addMapPoint(MapPoint& mp); + + /** @brief Return map point by id, or nullptr. */ + CV_WRAP MapPoint* getMapPoint(int id); + + /** @brief All live (non-bad) map points. */ + CV_WRAP std::vector mapPoints() const; + + CV_WRAP int numMapPoints() const; + + /** @brief Wire a bidirectional KF-MP observation. */ + CV_WRAP void addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp); + + /** @brief Mark a map point bad and remove all cross-references. */ + CV_WRAP void removeMapPoint(int mp_id); + + /** @brief Append a world-to-camera pose to the trajectory. */ + CV_WRAP void appendPose(const Matx44d& T_cw); + + /** @brief All emitted world-to-camera poses in order. */ + CV_WRAP const std::vector& trajectory() const; + + /** @brief Reset to empty state. */ + CV_WRAP void clear(); + +private: + struct Impl; + Ptr impl_; +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_MAP_HPP diff --git a/modules/slam/include/opencv2/slam/odometry_params.hpp b/modules/slam/include/opencv2/slam/odometry_params.hpp new file mode 100644 index 0000000000..cff30cd48d --- /dev/null +++ b/modules/slam/include/opencv2/slam/odometry_params.hpp @@ -0,0 +1,37 @@ +// 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_ODOMETRY_PARAMS_HPP +#define OPENCV_SLAM_ODOMETRY_PARAMS_HPP + +#include + +namespace cv { namespace slam { + +/** @brief Tunable parameters for the VisualOdometry pipeline. + + All fields have sensible defaults. Override any subset before + passing to VisualOdometry::create(). + + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE OdometryParams +{ + CV_WRAP OdometryParams() = default; + + // --- Bootstrap (two-view init) ------------------------------------------- + + CV_PROP_RW int min_init_inliers = 80; //!< Min essential-matrix RANSAC inliers. + CV_PROP_RW double min_init_parallax_deg = 3.0; //!< Min median parallax to trigger bootstrap. + CV_PROP_RW int min_init_points = 50; //!< Min triangulated points to accept init. + CV_PROP_RW double hf_ratio_thresh = 0.45; //!< H/(H+F) ratio; > thresh uses Homography. + CV_PROP_RW double min_growth_parallax_deg = 1.0; //!< Min parallax for a new map point to survive. + CV_PROP_RW double essential_ransac_thresh = 1.0; //!< RANSAC reprojection threshold (px). + CV_PROP_RW double essential_ransac_confidence = 0.999; //!< RANSAC confidence for findEssentialMat. +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP diff --git a/modules/slam/include/opencv2/slam/types.hpp b/modules/slam/include/opencv2/slam/types.hpp new file mode 100644 index 0000000000..4b54dbe7ee --- /dev/null +++ b/modules/slam/include/opencv2/slam/types.hpp @@ -0,0 +1,71 @@ +// 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_TYPES_HPP +#define OPENCV_SLAM_TYPES_HPP + +#include +#include +#include + +namespace cv { namespace slam { + +/** @brief Current state of the VisualOdometry pipeline. + @ingroup slam_odometry +*/ +enum OdometryState +{ + NOT_INITIALIZED = 0, //!< No frames processed yet. + INITIALIZING = 1, //!< Reference frame set; waiting for bootstrap. + TRACKING = 2 //!< Map initialised; localising via PnP. +}; + +/** @brief Feature data for one image frame. + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE FrameFeatures +{ + std::vector keypoints; + Mat descriptors; + Size imageSize; +}; + +struct MapPoint; +struct KeyFrame; + +/** @brief A triangulated 3-D landmark owned by Map. + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE MapPoint +{ + int id = -1; + Point3d pos; + bool bad = false; + + std::map observations; //!< Observing keyframe → keypoint index. +}; + +/** @brief A frame whose pose has been committed to the map. + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE KeyFrame +{ + int id = -1; + Matx44d pose_cw; //!< World-to-camera transform (row-major 4x4). + + std::vector keypoints; + Mat descriptors; + std::vector undist_kpts; + Size imageSize; + + std::vector kpt_to_mp; //!< Map-point id per keypoint, -1 if unmatched. + std::vector mappoints; //!< Direct pointer per keypoint, nullptr if unmatched. + + std::vector> ordered_covisibility; //!< Neighbours by shared point count. +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_TYPES_HPP diff --git a/modules/slam/include/opencv2/slam/visual_odometry.hpp b/modules/slam/include/opencv2/slam/visual_odometry.hpp new file mode 100644 index 0000000000..ec3e451383 --- /dev/null +++ b/modules/slam/include/opencv2/slam/visual_odometry.hpp @@ -0,0 +1,102 @@ +// 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_VISUAL_ODOMETRY_HPP +#define OPENCV_SLAM_VISUAL_ODOMETRY_HPP + +#include "types.hpp" +#include "map.hpp" +#include "odometry_params.hpp" +#include +#include + +namespace cv { namespace slam { + +/** @brief Monocular visual odometry pipeline. + + Use in batch mode (process a whole folder) or frame-by-frame: + + @code + auto vo = cv::slam::VisualOdometry::create( + detector, matcher, imagesFolder, outputFolder, K, dist, params); + vo->run(); // batch + vo->processFrame(img); // or incremental + @endcode + + Output files written by run() to outputFolder: + - trajectory.bin — binary T_cw pose stream + - trajectory.txt — camera centres in world coords + - images.txt — COLMAP-compatible pose file + - map_points.txt — 3-D point cloud + - keypoints.txt — per-keyframe observations + - vo.log — per-frame processing trace + + All poses are world-to-camera (T_cw). Camera centre = -R^T * t. + + @ingroup slam_odometry +*/ +class CV_EXPORTS_W VisualOdometry : public Algorithm +{ +public: + VisualOdometry(); + virtual ~VisualOdometry(); + + /** @brief Create a VisualOdometry instance. + @param detector Feature detector + descriptor extractor. + @param matcher Descriptor matcher. + @param imagesFolder Input image folder used by run(). May be empty. + @param outputFolder Artifact output folder. May be empty. + @param cameraMatrix 3x3 intrinsic matrix K. + @param distCoeffs Distortion coefficients. Pass empty Mat for rectified input. + @param params Odometry parameters. + */ + CV_WRAP static Ptr create( + const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + InputArray cameraMatrix, + InputArray distCoeffs, + const OdometryParams& params = OdometryParams()); + + /** @brief Process all images in imagesFolder and write output artifacts. + @return true if at least one pose was emitted. + */ + CV_WRAP virtual bool run() = 0; + + /** @brief Process a single frame. + @return true if a pose was emitted. + */ + CV_WRAP virtual bool processFrame(InputArray image) = 0; + + /** @brief Reset to NOT_INITIALIZED state and clear the map. */ + CV_WRAP virtual void reset() = 0; + + CV_WRAP virtual OdometryState getState() const = 0; + CV_WRAP virtual Matx44d getLastPose() const = 0; + CV_WRAP virtual Map& getMap() = 0; + CV_WRAP virtual const std::vector& getTrajectory() const = 0; + + CV_WRAP virtual OdometryParams getParams() const = 0; + CV_WRAP virtual void setParams(const OdometryParams& params) = 0; + + /** @brief Enable/disable pose-only BA after each PnP step. No-op without g2o. Default: true. */ + CV_WRAP virtual void setPoseOptimization(bool enable) = 0; + CV_WRAP virtual bool getPoseOptimization() const = 0; + + /** @brief Enable/disable local BA at each keyframe. No-op without g2o. Default: true. */ + CV_WRAP virtual void setLocalBA(bool enable) = 0; + CV_WRAP virtual bool getLocalBA() const = 0; + + CV_WRAP virtual String getImagesFolder() const = 0; + CV_WRAP virtual void setImagesFolder(const String& path) = 0; + + CV_WRAP virtual String getOutputFolder() const = 0; + CV_WRAP virtual void setOutputFolder(const String& path) = 0; +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP diff --git a/modules/slam/perf/perf_main.cpp b/modules/slam/perf/perf_main.cpp new file mode 100644 index 0000000000..94d0aa3da3 --- /dev/null +++ b/modules/slam/perf/perf_main.cpp @@ -0,0 +1,8 @@ +// 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. + + +#include "perf_precomp.hpp" + +CV_PERF_TEST_MAIN(slam) diff --git a/modules/slam/perf/perf_precomp.hpp b/modules/slam/perf/perf_precomp.hpp new file mode 100644 index 0000000000..d562b26574 --- /dev/null +++ b/modules/slam/perf/perf_precomp.hpp @@ -0,0 +1,12 @@ +// 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_PERF_SLAM_PRECOMP_HPP__ +#define __OPENCV_PERF_SLAM_PRECOMP_HPP__ + +#include +#include + +#endif // __OPENCV_PERF_SLAM_PRECOMP_HPP__ diff --git a/modules/slam/src/map.cpp b/modules/slam/src/map.cpp new file mode 100644 index 0000000000..43ecd44246 --- /dev/null +++ b/modules/slam/src/map.cpp @@ -0,0 +1,125 @@ +// 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. + + +#include "precomp.hpp" + +namespace cv { namespace slam { + +struct Map::Impl +{ + std::vector kf_vec; + std::vector mp_vec; + std::unordered_map kf_map; + std::unordered_map mp_map; + std::vector trajectory; + int next_kf_id = 0; + int next_mp_id = 0; +}; + +Map::Map() : impl_(makePtr()) {} + +Map::~Map() +{ + clear(); +} + +int Map::addKeyframe(KeyFrame& kf) +{ + if (kf.id < 0) + kf.id = impl_->next_kf_id++; + KeyFrame* ptr = new KeyFrame(kf); + impl_->kf_vec.push_back(ptr); + impl_->kf_map[ptr->id] = ptr; + kf.id = ptr->id; + return ptr->id; +} + +KeyFrame* Map::getKeyframe(int id) +{ + auto it = impl_->kf_map.find(id); + return it != impl_->kf_map.end() ? it->second : nullptr; +} + +const std::vector& Map::keyframes() const { return impl_->kf_vec; } + +int Map::numKeyframes() const { return (int)impl_->kf_vec.size(); } + +int Map::addMapPoint(MapPoint& mp) +{ + if (mp.id < 0) + mp.id = impl_->next_mp_id++; + MapPoint* ptr = new MapPoint(mp); + impl_->mp_vec.push_back(ptr); + impl_->mp_map[ptr->id] = ptr; + mp.id = ptr->id; + return ptr->id; +} + +MapPoint* Map::getMapPoint(int id) +{ + auto it = impl_->mp_map.find(id); + return it != impl_->mp_map.end() ? it->second : nullptr; +} + +std::vector Map::mapPoints() const +{ + std::vector live; + live.reserve(impl_->mp_vec.size()); + for (MapPoint* mp : impl_->mp_vec) + if (mp && !mp->bad) + live.push_back(mp); + return live; +} + +int Map::numMapPoints() const +{ + int n = 0; + for (const MapPoint* mp : impl_->mp_vec) + if (mp && !mp->bad) ++n; + return n; +} + +void Map::addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp) +{ + CV_Assert(kf && mp); + mp->observations[kf] = kp_idx; + if (kp_idx < (int)kf->mappoints.size()) + kf->mappoints[kp_idx] = mp; + if (kp_idx < (int)kf->kpt_to_mp.size()) + kf->kpt_to_mp[kp_idx] = mp->id; +} + +void Map::removeMapPoint(int mp_id) +{ + MapPoint* mp = getMapPoint(mp_id); + if (!mp || mp->bad) return; + for (auto& [obs_kf, kp_idx] : mp->observations) + { + if (!obs_kf) continue; + if (kp_idx < (int)obs_kf->mappoints.size()) obs_kf->mappoints[kp_idx] = nullptr; + if (kp_idx < (int)obs_kf->kpt_to_mp.size()) obs_kf->kpt_to_mp[kp_idx] = -1; + } + mp->observations.clear(); + mp->bad = true; +} + +void Map::appendPose(const Matx44d& T_cw) { impl_->trajectory.push_back(T_cw); } + +const std::vector& Map::trajectory() const { return impl_->trajectory; } + +void Map::clear() +{ + for (KeyFrame* kf : impl_->kf_vec) delete kf; + for (MapPoint* mp : impl_->mp_vec) delete mp; + impl_->kf_vec.clear(); + impl_->mp_vec.clear(); + impl_->kf_map.clear(); + impl_->mp_map.clear(); + impl_->trajectory.clear(); + impl_->next_kf_id = 0; + impl_->next_mp_id = 0; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/precomp.hpp b/modules/slam/src/precomp.hpp new file mode 100644 index 0000000000..ecc564bffd --- /dev/null +++ b/modules/slam/src/precomp.hpp @@ -0,0 +1,28 @@ +// 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_PRECOMP_HPP__ +#define __OPENCV_SLAM_PRECOMP_HPP__ + +#include +#include +#include +#include +#include +#include +#include "opencv2/slam.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#endif // __OPENCV_SLAM_PRECOMP_HPP__ diff --git a/modules/slam/src/types.cpp b/modules/slam/src/types.cpp new file mode 100644 index 0000000000..80b2d0daef --- /dev/null +++ b/modules/slam/src/types.cpp @@ -0,0 +1,12 @@ +// 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. + + +#include "precomp.hpp" + +namespace cv { namespace slam { + +OdometryParams::OdometryParams() = default; + +}} // namespace cv::slam diff --git a/modules/slam/test/test_precomp.hpp b/modules/slam/test/test_precomp.hpp new file mode 100644 index 0000000000..9afe8de0be --- /dev/null +++ b/modules/slam/test/test_precomp.hpp @@ -0,0 +1,12 @@ +// 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_TEST_SLAM_PRECOMP_HPP__ +#define __OPENCV_TEST_SLAM_PRECOMP_HPP__ + +#include +#include + +#endif // __OPENCV_TEST_SLAM_PRECOMP_HPP__ diff --git a/modules/slam/test/test_visual_odometry.cpp b/modules/slam/test/test_visual_odometry.cpp new file mode 100644 index 0000000000..4496908507 --- /dev/null +++ b/modules/slam/test/test_visual_odometry.cpp @@ -0,0 +1,136 @@ +// 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. + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +// ============================================================================= +// Map tests +// ============================================================================= + +TEST(Map, AddAndRetrieveKeyframe) +{ + cv::slam::Map map; + + cv::slam::KeyFrame kf; + kf.pose_cw = cv::Matx44d::eye(); + + int id = map.addKeyframe(kf); + EXPECT_GE(id, 0); + EXPECT_EQ(kf.id, id); + + cv::slam::KeyFrame* retrieved = map.getKeyframe(id); + ASSERT_NE(retrieved, nullptr); + EXPECT_EQ(retrieved->id, id); + EXPECT_EQ(map.numKeyframes(), 1); +} + +TEST(Map, AddAndRetrieveMapPoint) +{ + cv::slam::Map map; + + cv::slam::MapPoint mp; + mp.pos = cv::Point3d(1.0, 2.0, 3.0); + + int id = map.addMapPoint(mp); + EXPECT_GE(id, 0); + EXPECT_EQ(mp.id, id); + + cv::slam::MapPoint* retrieved = map.getMapPoint(id); + ASSERT_NE(retrieved, nullptr); + EXPECT_DOUBLE_EQ(retrieved->pos.x, 1.0); + EXPECT_DOUBLE_EQ(retrieved->pos.y, 2.0); + EXPECT_DOUBLE_EQ(retrieved->pos.z, 3.0); + EXPECT_FALSE(retrieved->bad); + EXPECT_EQ(map.numMapPoints(), 1); +} + +TEST(Map, RemoveMapPointCleansObservations) +{ + cv::slam::Map map; + + // Add a keyframe with one keypoint slot. + cv::slam::KeyFrame kf; + kf.pose_cw = cv::Matx44d::eye(); + kf.mappoints.assign(1, nullptr); + kf.kpt_to_mp.assign(1, -1); + int kf_id = map.addKeyframe(kf); + + // Add a map point. + cv::slam::MapPoint mp; + mp.pos = cv::Point3d(0, 0, 1); + int mp_id = map.addMapPoint(mp); + + cv::slam::KeyFrame* kf_ptr = map.getKeyframe(kf_id); + cv::slam::MapPoint* mp_ptr = map.getMapPoint(mp_id); + ASSERT_NE(kf_ptr, nullptr); + ASSERT_NE(mp_ptr, nullptr); + + // Wire the observation. + map.addObservation(kf_ptr, 0, mp_ptr); + EXPECT_EQ(mp_ptr->observations.size(), 1u); + EXPECT_EQ(kf_ptr->mappoints[0], mp_ptr); + + // Remove the map point and verify cleanup. + map.removeMapPoint(mp_id); + EXPECT_TRUE(mp_ptr->bad); + EXPECT_EQ(kf_ptr->mappoints[0], nullptr); + EXPECT_EQ(kf_ptr->kpt_to_mp[0], -1); + EXPECT_EQ(map.numMapPoints(), 0); +} + +TEST(Map, TrajectoryAppendAndRetrieve) +{ + cv::slam::Map map; + + cv::Matx44d T1 = cv::Matx44d::eye(); + cv::Matx44d T2 = cv::Matx44d::eye(); + T2(0, 3) = 1.0; // 1-unit translation + + map.appendPose(T1); + map.appendPose(T2); + + const auto& traj = map.trajectory(); + ASSERT_EQ(traj.size(), 2u); + EXPECT_DOUBLE_EQ(traj[1](0, 3), 1.0); +} + +TEST(Map, ClearResetsState) +{ + cv::slam::Map map; + + cv::slam::KeyFrame kf; + kf.pose_cw = cv::Matx44d::eye(); + map.addKeyframe(kf); + + cv::slam::MapPoint mp; + mp.pos = cv::Point3d(1, 2, 3); + map.addMapPoint(mp); + + map.appendPose(cv::Matx44d::eye()); + map.clear(); + + EXPECT_EQ(map.numKeyframes(), 0); + EXPECT_EQ(map.numMapPoints(), 0); + EXPECT_TRUE(map.trajectory().empty()); +} + +// ============================================================================= +// OdometryParams tests +// ============================================================================= + +TEST(OdometryParams, DefaultValues) +{ + cv::slam::OdometryParams p; + EXPECT_EQ(p.min_init_inliers, 80); + EXPECT_DOUBLE_EQ(p.min_init_parallax_deg, 3.0); + EXPECT_EQ(p.min_init_points, 50); + EXPECT_DOUBLE_EQ(p.hf_ratio_thresh, 0.45); + EXPECT_DOUBLE_EQ(p.min_growth_parallax_deg, 1.0); + EXPECT_DOUBLE_EQ(p.essential_ransac_thresh, 1.0); + EXPECT_DOUBLE_EQ(p.essential_ransac_confidence, 0.999); +} + +}} // namespace opencv_test