From 08077a00a182f71d72fd609bcc9f814ce370c605 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Wed, 17 Jun 2026 13:15:57 +0530 Subject: [PATCH] cleanupFinal --- modules/slam/CMakeLists.txt | 23 +- modules/slam/include/opencv2/slam.hpp | 23 +- modules/slam/include/opencv2/slam/map.hpp | 81 +-- .../include/opencv2/slam/odometry_params.hpp | 63 ++- modules/slam/include/opencv2/slam/slam.hpp | 13 + modules/slam/include/opencv2/slam/types.hpp | 78 +-- .../include/opencv2/slam/visual_odometry.hpp | 109 ++--- modules/slam/perf/perf_main.cpp | 3 +- modules/slam/perf/perf_precomp.hpp | 13 +- modules/slam/src/map.cpp | 183 ++++--- modules/slam/src/odometry/frame.hpp | 79 +++ modules/slam/src/odometry/visual_odometry.cpp | 461 ++++++++++++++++++ modules/slam/src/odometry/vo_bootstrap.cpp | 252 ++++++++++ modules/slam/src/odometry/vo_impl.hpp | 129 +++++ modules/slam/src/odometry/vo_keyframe.cpp | 145 ++++++ modules/slam/src/odometry/vo_map_growth.cpp | 135 +++++ modules/slam/src/odometry/vo_tracking.cpp | 390 +++++++++++++++ modules/slam/src/optimizer/pose_optimizer.cpp | 49 ++ modules/slam/src/optimizer/pose_optimizer.hpp | 21 + modules/slam/src/precomp.hpp | 34 +- .../{src/types.cpp => test/test_main.cpp} | 11 +- modules/slam/test/test_map.cpp | 79 +++ modules/slam/test/test_precomp.hpp | 15 +- modules/slam/test/test_visual_odometry.cpp | 321 ++++++++---- samples/CMakeLists.txt | 2 + samples/slam/CMakeLists.txt | 22 + samples/slam/visual_odometry.cpp | 63 +++ samples/slam/visual_odometry.py | 56 +++ 28 files changed, 2453 insertions(+), 400 deletions(-) create mode 100644 modules/slam/include/opencv2/slam/slam.hpp create mode 100644 modules/slam/src/odometry/frame.hpp create mode 100644 modules/slam/src/odometry/visual_odometry.cpp create mode 100644 modules/slam/src/odometry/vo_bootstrap.cpp create mode 100644 modules/slam/src/odometry/vo_impl.hpp create mode 100644 modules/slam/src/odometry/vo_keyframe.cpp create mode 100644 modules/slam/src/odometry/vo_map_growth.cpp create mode 100644 modules/slam/src/odometry/vo_tracking.cpp create mode 100644 modules/slam/src/optimizer/pose_optimizer.cpp create mode 100644 modules/slam/src/optimizer/pose_optimizer.hpp rename modules/slam/{src/types.cpp => test/test_main.cpp} (53%) create mode 100644 modules/slam/test/test_map.cpp create mode 100644 samples/slam/CMakeLists.txt create mode 100644 samples/slam/visual_odometry.cpp create mode 100644 samples/slam/visual_odometry.py diff --git a/modules/slam/CMakeLists.txt b/modules/slam/CMakeLists.txt index 91bcf7f368..6b6f62b2f8 100644 --- a/modules/slam/CMakeLists.txt +++ b/modules/slam/CMakeLists.txt @@ -1,14 +1,13 @@ -# 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") +set(the_description "SLAM and Visual Odometry") ocv_define_module(slam - opencv_core - opencv_imgproc - opencv_imgcodecs - opencv_features - opencv_3d - OPTIONAL opencv_dnn - WRAP python) + opencv_geometry + opencv_features + opencv_imgproc + opencv_imgcodecs + opencv_video + OPTIONAL + opencv_dnn + WRAP python) + +ocv_warnings_disable(CMAKE_CXX_FLAGS -Wshadow) diff --git a/modules/slam/include/opencv2/slam.hpp b/modules/slam/include/opencv2/slam.hpp index 2c4f84e058..c677e221b2 100644 --- a/modules/slam/include/opencv2/slam.hpp +++ b/modules/slam/include/opencv2/slam.hpp @@ -1,22 +1,23 @@ // 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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #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. -@} +@defgroup slam SLAM and Visual Odometry + +Monocular visual odometry pipeline. Entry point is @ref cv::slam::VisualOdometry. +Bootstraps an initial map from two-view geometry, then tracks subsequent frames +with PnP, growing the map at keyframe promotions. */ +#include "opencv2/slam/types.hpp" +#include "opencv2/slam/map.hpp" +#include "opencv2/slam/odometry_params.hpp" +#include "opencv2/slam/visual_odometry.hpp" + #endif // OPENCV_SLAM_HPP diff --git a/modules/slam/include/opencv2/slam/map.hpp b/modules/slam/include/opencv2/slam/map.hpp index 2da8af1633..ab8d1963bc 100644 --- a/modules/slam/include/opencv2/slam/map.hpp +++ b/modules/slam/include/opencv2/slam/map.hpp @@ -1,73 +1,78 @@ // 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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_MAP_HPP #define OPENCV_SLAM_MAP_HPP -#include "types.hpp" -#include +#include "opencv2/slam/types.hpp" + +#include #include -namespace cv { namespace slam { +namespace cv { +namespace slam { -/** @brief Container for keyframes, map points, and the per-frame trajectory. +//! @addtogroup slam +//! @{ - 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 +/** @brief Owns all persistent SLAM state (KeyFrames and MapPoints). +Pointers from addKeyframe / addMapPoint are valid until removeMapPoint / clear. */ +class CV_EXPORTS Map { public: - CV_WRAP Map(); + Map(); ~Map(); - /** @brief Add a keyframe; assigns id if kf.id < 0. Returns the assigned id. */ - CV_WRAP int addKeyframe(KeyFrame& kf); + Map(const Map&) = delete; + Map& operator=(const Map&) = delete; - /** @brief Return keyframe by id, or nullptr. */ - CV_WRAP KeyFrame* getKeyframe(int id); + // Keyframes - /** @brief All keyframes in insertion order. */ - CV_WRAP const std::vector& keyframes() const; + KeyFrame* addKeyframe(KeyFrame* kf); //!< takes ownership; assigns id if < 0 + KeyFrame* getKeyframe(int id) const; - CV_WRAP int numKeyframes() const; + const std::set& keyframes() const; + int numKeyframes() const; - /** @brief Add a map point; assigns id if mp.id < 0. Returns the assigned id. */ - CV_WRAP int addMapPoint(MapPoint& mp); + // Map points - /** @brief Return map point by id, or nullptr. */ - CV_WRAP MapPoint* getMapPoint(int id); + MapPoint* addMapPoint(MapPoint* mp); //!< takes ownership; assigns id if < 0 + MapPoint* getMapPoint(int id) const; - /** @brief All live (non-bad) map points. */ - CV_WRAP std::vector mapPoints() const; + const std::set& mapPoints() const; + int numMapPoints() const; - CV_WRAP int numMapPoints() const; + void addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp); + void removeObservation(KeyFrame* kf, MapPoint* mp); + void removeMapPoint(MapPoint* mp); - /** @brief Wire a bidirectional KF-MP observation. */ - CV_WRAP void addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp); + // Reference / current keyframes - /** @brief Mark a map point bad and remove all cross-references. */ - CV_WRAP void removeMapPoint(int mp_id); + void setRefKeyframe(KeyFrame* kf); + KeyFrame* getRefKeyframe() const; - /** @brief Append a world-to-camera pose to the trajectory. */ - CV_WRAP void appendPose(const Matx44d& T_cw); + void setCurrentKeyframe(KeyFrame* kf); + KeyFrame* getCurrentKeyframe() const; - /** @brief All emitted world-to-camera poses in order. */ - CV_WRAP const std::vector& trajectory() const; + // Trajectory - /** @brief Reset to empty state. */ - CV_WRAP void clear(); + void appendPose(const Matx44d& T_cw); + const std::vector& trajectory() const; + + // Lifecycle + + void clear(); private: struct Impl; - Ptr 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 index cff30cd48d..84194784ad 100644 --- a/modules/slam/include/opencv2/slam/odometry_params.hpp +++ b/modules/slam/include/opencv2/slam/odometry_params.hpp @@ -1,37 +1,64 @@ // 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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_ODOMETRY_PARAMS_HPP #define OPENCV_SLAM_ODOMETRY_PARAMS_HPP -#include +#include "opencv2/core.hpp" -namespace cv { namespace slam { +namespace cv { +namespace slam { -/** @brief Tunable parameters for the VisualOdometry pipeline. +//! @addtogroup slam +//! @{ - 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; + CV_WRAP OdometryParams() {} - // --- Bootstrap (two-view init) ------------------------------------------- + // Bootstrap + CV_PROP_RW int minInitInliers = 40; + CV_PROP_RW double minInitParallaxDeg = 1.5; + CV_PROP_RW int minInitPoints = 50; + CV_PROP_RW double hfRatioThresh = 0.45; + CV_PROP_RW double minGrowthParallaxDeg = 0.1; + CV_PROP_RW double essentialRansacThresh = 1.0; + CV_PROP_RW double essentialRansacConfidence = 0.999; - 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. + // Tracking (PnP) + CV_PROP_RW double pnpReprojThresh = 4.0; + CV_PROP_RW int pnpMinInliers = 6; + CV_PROP_RW int pnpRansacIters = 500; + CV_PROP_RW double pnpConfidence = 0.99; + + // Motion model + CV_PROP_RW double motionModelRadius = 15.0; + CV_PROP_RW double motionModelRadiusWide = 30.0; + CV_PROP_RW int motionModelMinMatches = 20; + CV_PROP_RW double descProjThresh = 1.0; + + // Optical flow fallback + CV_PROP_RW int opticalFlowMinInliers = 10; + + // Keyframe promotion + CV_PROP_RW int kfMinFrames = 1; + CV_PROP_RW int kfMaxFrames = 30; + CV_PROP_RW double kfInlierRatio = 0.70; + CV_PROP_RW int kfMinInliers = 40; + CV_PROP_RW double kfRotThreshDeg = 5.0; + CV_PROP_RW double kfTransThresh = 0.5; + + // Local map refinement + CV_PROP_RW int localMapTopK = 10; + CV_PROP_RW int localMapNeighborK = 5; + CV_PROP_RW double localMapRadius = 7.0; }; +//! @} + }} // namespace cv::slam #endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP diff --git a/modules/slam/include/opencv2/slam/slam.hpp b/modules/slam/include/opencv2/slam/slam.hpp new file mode 100644 index 0000000000..f296d9601e --- /dev/null +++ b/modules/slam/include/opencv2/slam/slam.hpp @@ -0,0 +1,13 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +// Compatibility shim: prefer #include . +#ifndef OPENCV_SLAM_SLAM_HPP +#define OPENCV_SLAM_SLAM_HPP + +#include "opencv2/slam.hpp" + +#endif diff --git a/modules/slam/include/opencv2/slam/types.hpp b/modules/slam/include/opencv2/slam/types.hpp index 4b54dbe7ee..66481aae6f 100644 --- a/modules/slam/include/opencv2/slam/types.hpp +++ b/modules/slam/include/opencv2/slam/types.hpp @@ -1,71 +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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_TYPES_HPP #define OPENCV_SLAM_TYPES_HPP -#include +#include "opencv2/core.hpp" +#include "opencv2/core/types.hpp" + #include #include -namespace cv { namespace slam { +namespace cv { +namespace slam { -/** @brief Current state of the VisualOdometry pipeline. - @ingroup slam_odometry -*/ +//! @addtogroup slam +//! @{ + +// Pipeline lifecycle state 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; + NOT_INITIALIZED = 0, + INITIALIZING = 1, + TRACKING = 2 }; struct MapPoint; struct KeyFrame; -/** @brief A triangulated 3-D landmark owned by Map. - @ingroup slam_odometry -*/ -struct CV_EXPORTS_W_SIMPLE MapPoint +// 3D landmark, owned by Map +struct CV_EXPORTS MapPoint { - int id = -1; - Point3d pos; - bool bad = false; + int id = -1; + Point3d pos { 0, 0, 0 }; + Mat refDesc; - std::map observations; //!< Observing keyframe → keypoint index. + std::map observations; // kf -> keypoint index + + int visibleCount = 0; + int foundCount = 0; + bool bad = false; // soft-delete; check before use }; -/** @brief A frame whose pose has been committed to the map. - @ingroup slam_odometry -*/ -struct CV_EXPORTS_W_SIMPLE KeyFrame +// Keyframe: pose + keypoints + covisibility graph, owned by Map +struct CV_EXPORTS KeyFrame { - int id = -1; - Matx44d pose_cw; //!< World-to-camera transform (row-major 4x4). + int id = -1; + Matx44d poseCw = Matx44d::eye(); // world->camera std::vector keypoints; - Mat descriptors; - std::vector undist_kpts; - Size imageSize; + Mat descriptors; + std::vector undistKpts; // parallel to keypoints + 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 mapPoints; // parallel to keypoints; null = unmatched - std::vector> ordered_covisibility; //!< Neighbours by shared point count. + std::map covisibility; + std::vector> orderedCovisibility; // sorted descending by count + + KeyFrame* parent = nullptr; + Mat globalDesc; }; +//! @} + }} // 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 index ec3e451383..95b55e89e3 100644 --- a/modules/slam/include/opencv2/slam/visual_odometry.hpp +++ b/modules/slam/include/opencv2/slam/visual_odometry.hpp @@ -1,102 +1,79 @@ // 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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_VISUAL_ODOMETRY_HPP #define OPENCV_SLAM_VISUAL_ODOMETRY_HPP -#include "types.hpp" -#include "map.hpp" -#include "odometry_params.hpp" -#include -#include +#include "opencv2/core.hpp" +#include "opencv2/features.hpp" -namespace cv { namespace slam { +#include "opencv2/slam/types.hpp" +#include "opencv2/slam/map.hpp" +#include "opencv2/slam/odometry_params.hpp" + +#include + +namespace cv { +namespace slam { + +//! @addtogroup slam +//! @{ /** @brief Monocular visual odometry pipeline. - Use in batch mode (process a whole folder) or frame-by-frame: +State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING +(per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING. - @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 +@ref run writes trajectory.txt, trajectory.bin, map_points.txt, keypoints.txt, +images.txt, and vo.log into `outputFolder`. */ -class CV_EXPORTS_W VisualOdometry : public Algorithm +class CV_EXPORTS_W VisualOdometry { 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& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, - InputArray cameraMatrix, - InputArray distCoeffs, - const OdometryParams& params = OdometryParams()); + const String& imagesFolder, + const String& outputFolder, + InputArray cameraMatrix, + InputArray distCoeffs = noArray(), + const OdometryParams& params = OdometryParams()); - /** @brief Process all images in imagesFolder and write output artifacts. - @return true if at least one pose was emitted. - */ + /** @brief Run the pipeline over every image in the configured folder. */ CV_WRAP virtual bool run() = 0; - /** @brief Process a single frame. - @return true if a pose was emitted. - */ + /** @brief Feed one image. Returns true if a pose was emitted. */ CV_WRAP virtual bool processFrame(InputArray image) = 0; - /** @brief Reset to NOT_INITIALIZED state and clear the map. */ + /** @brief Reset to NOT_INITIALIZED, clearing map and trajectory. */ 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 OdometryState getState() const = 0; + CV_WRAP virtual Matx44d getLastPose() const = 0; - CV_WRAP virtual OdometryParams getParams() const = 0; - CV_WRAP virtual void setParams(const OdometryParams& params) = 0; + //! @note Not exposed to Python: Map holds raw pointers / non-convertible containers. + virtual const Map& getMap() const = 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; + CV_WRAP virtual const std::vector& getTrajectory() 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 const OdometryParams& getParams() const = 0; + CV_WRAP virtual void setParams(const OdometryParams& params) = 0; - CV_WRAP virtual String getImagesFolder() const = 0; - CV_WRAP virtual void setImagesFolder(const String& path) = 0; + CV_WRAP virtual const String& getImagesFolder() const = 0; + CV_WRAP virtual const String& getOutputFolder() const = 0; + CV_WRAP virtual void setOutputFolder(const String& outputFolder) = 0; - CV_WRAP virtual String getOutputFolder() const = 0; - CV_WRAP virtual void setOutputFolder(const String& path) = 0; +protected: + VisualOdometry(); }; +//! @} + }} // 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 index 94d0aa3da3..95b14c1581 100644 --- a/modules/slam/perf/perf_main.cpp +++ b/modules/slam/perf/perf_main.cpp @@ -1,7 +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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #include "perf_precomp.hpp" diff --git a/modules/slam/perf/perf_precomp.hpp b/modules/slam/perf/perf_precomp.hpp index d562b26574..37fede9ca6 100644 --- a/modules/slam/perf/perf_precomp.hpp +++ b/modules/slam/perf/perf_precomp.hpp @@ -1,12 +1,13 @@ // 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#ifndef OPENCV_SLAM_PERF_PRECOMP_HPP +#define OPENCV_SLAM_PERF_PRECOMP_HPP -#ifndef __OPENCV_PERF_SLAM_PRECOMP_HPP__ -#define __OPENCV_PERF_SLAM_PRECOMP_HPP__ +#include "opencv2/ts.hpp" +#include "opencv2/slam.hpp" -#include -#include - -#endif // __OPENCV_PERF_SLAM_PRECOMP_HPP__ +#endif diff --git a/modules/slam/src/map.cpp b/modules/slam/src/map.cpp index 43ecd44246..fc958aa3d0 100644 --- a/modules/slam/src/map.cpp +++ b/modules/slam/src/map.cpp @@ -1,125 +1,150 @@ // 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. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #include "precomp.hpp" -namespace cv { namespace slam { +#include +#include +#include + +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; + std::set keyframes; + std::set mapPoints; + std::unordered_map kfIndex; + std::unordered_map mpIndex; + + KeyFrame* refKf = nullptr; + KeyFrame* currentKf = nullptr; + + std::vector trajectory; + std::mutex mutex; + + int nextKfId = 0; + int nextMpId = 0; }; -Map::Map() : impl_(makePtr()) {} +Map::Map() : impl(makePtr()) {} Map::~Map() { - clear(); + for (KeyFrame* kf : impl->keyframes) delete kf; + for (MapPoint* mp : impl->mapPoints) delete mp; } -int Map::addKeyframe(KeyFrame& kf) +// Keyframes + +KeyFrame* 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; + CV_Assert(kf); + if (kf->id < 0) + kf->id = impl->nextKfId++; + else if (kf->id >= impl->nextKfId) + impl->nextKfId = kf->id + 1; + impl->keyframes.insert(kf); + impl->kfIndex[kf->id] = kf; + return kf; } -KeyFrame* Map::getKeyframe(int id) +KeyFrame* Map::getKeyframe(int id) const { - auto it = impl_->kf_map.find(id); - return it != impl_->kf_map.end() ? it->second : nullptr; + auto it = impl->kfIndex.find(id); + return (it != impl->kfIndex.end()) ? it->second : nullptr; } -const std::vector& Map::keyframes() const { return impl_->kf_vec; } +const std::set& Map::keyframes() const { return impl->keyframes; } +int Map::numKeyframes() const { return (int)impl->keyframes.size(); } -int Map::numKeyframes() const { return (int)impl_->kf_vec.size(); } +// Map points -int Map::addMapPoint(MapPoint& mp) +MapPoint* 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; + CV_Assert(mp); + if (mp->id < 0) + mp->id = impl->nextMpId++; + else if (mp->id >= impl->nextMpId) + impl->nextMpId = mp->id + 1; + impl->mapPoints.insert(mp); + impl->mpIndex[mp->id] = mp; + return mp; } -MapPoint* Map::getMapPoint(int id) +MapPoint* Map::getMapPoint(int id) const { - auto it = impl_->mp_map.find(id); - return it != impl_->mp_map.end() ? it->second : nullptr; + auto it = impl->mpIndex.find(id); + return (it != impl->mpIndex.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; -} +const std::set& Map::mapPoints() const { return impl->mapPoints; } +int Map::numMapPoints() const { return (int)impl->mapPoints.size(); } -int Map::numMapPoints() const -{ - int n = 0; - for (const MapPoint* mp : impl_->mp_vec) - if (mp && !mp->bad) ++n; - return n; -} +// Observations -void Map::addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp) +void Map::addObservation(KeyFrame* kf, size_t kpIdx, 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; + CV_Assert(kpIdx < kf->mapPoints.size()); + if (kf->mapPoints[kpIdx] != nullptr) return; + kf->mapPoints[kpIdx] = mp; + mp->observations[kf] = kpIdx; } -void Map::removeMapPoint(int mp_id) +void Map::removeObservation(KeyFrame* kf, MapPoint* mp) { - 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; + if (!kf || !mp) return; + auto it = mp->observations.find(kf); + if (it == mp->observations.end()) return; + size_t kpIdx = it->second; + if (kpIdx < kf->mapPoints.size()) + kf->mapPoints[kpIdx] = nullptr; + mp->observations.erase(it); } -void Map::appendPose(const Matx44d& T_cw) { impl_->trajectory.push_back(T_cw); } +void Map::removeMapPoint(MapPoint* mp) +{ + if (!mp) return; + for (auto& [kf, kpIdx] : mp->observations) + if (kpIdx < kf->mapPoints.size()) + kf->mapPoints[kpIdx] = nullptr; + impl->mapPoints.erase(mp); + impl->mpIndex.erase(mp->id); + delete mp; +} -const std::vector& Map::trajectory() const { return impl_->trajectory; } +// Reference / current keyframes + +void Map::setRefKeyframe(KeyFrame* kf) { impl->refKf = kf; } +KeyFrame* Map::getRefKeyframe() const { return impl->refKf; } + +void Map::setCurrentKeyframe(KeyFrame* kf) { impl->currentKf = kf; } +KeyFrame* Map::getCurrentKeyframe() const { return impl->currentKf; } + +// Trajectory + +void Map::appendPose(const Matx44d& T_cw) { impl->trajectory.push_back(T_cw); } +const std::vector& Map::trajectory() const { return impl->trajectory; } + +// Lifecycle 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; + for (KeyFrame* kf : impl->keyframes) delete kf; + for (MapPoint* mp : impl->mapPoints) delete mp; + impl->keyframes.clear(); + impl->mapPoints.clear(); + impl->kfIndex.clear(); + impl->mpIndex.clear(); + impl->refKf = nullptr; + impl->currentKf = nullptr; + impl->trajectory.clear(); + impl->nextKfId = 0; + impl->nextMpId = 0; } }} // namespace cv::slam diff --git a/modules/slam/src/odometry/frame.hpp b/modules/slam/src/odometry/frame.hpp new file mode 100644 index 0000000000..bf7e255aa1 --- /dev/null +++ b/modules/slam/src/odometry/frame.hpp @@ -0,0 +1,79 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_FRAME_HPP +#define OPENCV_SLAM_FRAME_HPP + +#include "../precomp.hpp" + +namespace cv { +namespace slam { + +/** @brief Per-frame scratch pad. Created per incoming image, never stored in the Map. */ +struct Frame +{ + Mat image; + std::vector keypoints; + Mat descriptors; + std::vector undistKpts; //!< undistorted, parallel to keypoints + Size imageSize; + + Matx44d poseCw = Matx44d::eye(); + std::vector mapPoints; //!< parallel to keypoints; nullptr = unmatched + std::vector outliers; //!< parallel to keypoints; true = inlier check failed + + // Grid cell size in pixels — dimensions adapt to each image resolution. + static constexpr int CELL_SIZE_PX = 20; + + int gridRows = 0; + int gridCols = 0; + std::unordered_map> grid; //!< cell key -> keypoint indices + + int cellKey(int row, int col) const { return row * gridCols + col; } + + void buildGrid() + { + grid.clear(); + if (imageSize.width <= 0 || imageSize.height <= 0) return; + gridCols = std::max(1, (imageSize.width + CELL_SIZE_PX - 1) / CELL_SIZE_PX); + gridRows = std::max(1, (imageSize.height + CELL_SIZE_PX - 1) / CELL_SIZE_PX); + for (size_t i = 0; i < undistKpts.size(); ++i) + { + int col = std::min(gridCols - 1, std::max(0, (int)(undistKpts[i].x / CELL_SIZE_PX))); + int row = std::min(gridRows - 1, std::max(0, (int)(undistKpts[i].y / CELL_SIZE_PX))); + grid[cellKey(row, col)].push_back(i); + } + } + + std::vector getKeypointsInRadius(float x, float y, float r) const + { + std::vector result; + if (gridRows <= 0 || gridCols <= 0 || grid.empty()) return result; + const int colMin = std::max(0, (int)((x - r) / CELL_SIZE_PX)); + const int colMax = std::min(gridCols-1, (int)((x + r) / CELL_SIZE_PX)); + const int rowMin = std::max(0, (int)((y - r) / CELL_SIZE_PX)); + const int rowMax = std::min(gridRows-1, (int)((y + r) / CELL_SIZE_PX)); + const float r2 = r * r; + for (int row = rowMin; row <= rowMax; ++row) + for (int col = colMin; col <= colMax; ++col) + { + auto it = grid.find(cellKey(row, col)); + if (it == grid.end()) continue; + for (size_t idx : it->second) + { + float dx = undistKpts[idx].x - x; + float dy = undistKpts[idx].y - y; + if (dx * dx + dy * dy <= r2) + result.push_back(idx); + } + } + return result; + } +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_FRAME_HPP diff --git a/modules/slam/src/odometry/visual_odometry.cpp b/modules/slam/src/odometry/visual_odometry.cpp new file mode 100644 index 0000000000..0e65dfdaa5 --- /dev/null +++ b/modules/slam/src/odometry/visual_odometry.cpp @@ -0,0 +1,461 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +#include +#include + +namespace cv { +namespace slam { + +namespace { + +const char* stateName(OdometryState s) +{ + switch (s) + { + case NOT_INITIALIZED: return "NOT_INITIALIZED"; + case INITIALIZING: return "INITIALIZING"; + case TRACKING: return "TRACKING"; + } + return "NOT_INITIALIZED"; +} + +String joinPath(const String& dir, const String& name) +{ + if (dir.empty()) return name; + char last = dir.back(); + if (last == '/' || last == '\\') return dir + name; + return dir + "/" + name; +} + +} // anonymous namespace + +// Factory + +VisualOdometry::VisualOdometry() = default; +VisualOdometry::~VisualOdometry() = default; + +Ptr VisualOdometry::create( + const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + InputArray cameraMatrix, + InputArray distCoeffs, + const OdometryParams& params) +{ + CV_Assert(detector && "VisualOdometry::create: detector must not be null"); + CV_Assert(matcher && "VisualOdometry::create: matcher must not be null"); + + Mat K = cameraMatrix.getMat(); + CV_Assert(!K.empty() && K.rows == 3 && K.cols == 3); + Mat dist = distCoeffs.empty() ? Mat() : distCoeffs.getMat(); + + return makePtr( + detector, matcher, imagesFolder, outputFolder, K, dist, params); +} + +// Constructor + +VisualOdometryImpl::VisualOdometryImpl( + const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + const Mat& cameraMatrix, + const Mat& distCoeffs, + const OdometryParams& params) + : detector(detector), matcher(matcher), params(params), + imagesFolder(imagesFolder), outputFolder(outputFolder) +{ + cameraMatrix.convertTo(K, CV_64F); + if (!distCoeffs.empty()) + distCoeffs.convertTo(dist, CV_64F); +} + +// reset / processFrame + +void VisualOdometryImpl::reset() +{ + state = NOT_INITIALIZED; + lastPoseCw = Matx44d::eye(); + refFrame = Frame(); + lastKf = nullptr; + framesSinceKf = 0; + lastKfInliers = 0; + velocity = Matx44d::eye(); + hasVelocity = false; + prevFrame = Frame(); + hasPrevFrame = false; + lastEvent.clear(); + poseFilenames.clear(); + map.clear(); +} + +bool VisualOdometryImpl::processFrame(InputArray image) +{ + CV_INSTRUMENT_REGION(); + + if (image.empty()) return false; + lastEvent.clear(); + + Frame cur; + extractFeatures(image, cur); + if (cur.keypoints.empty() || cur.descriptors.empty()) return false; + + cur.mapPoints.assign(cur.keypoints.size(), nullptr); + cur.outliers.assign(cur.keypoints.size(), false); + cur.buildGrid(); + + switch (state) + { + case NOT_INITIALIZED: + refFrame = cur; + state = INITIALIZING; + return false; + + case INITIALIZING: + return bootstrap(cur); + + case TRACKING: + return track(cur); + } + return false; +} + +// Feature extraction + +void VisualOdometryImpl::extractFeatures(InputArray image, Frame& out) const +{ + Mat img = image.getMat(); + out.imageSize = img.size(); + out.keypoints.clear(); + + // Detect and compute on the original image (color/grey is up to the detector). + detector->detectAndCompute(img, noArray(), out.keypoints, out.descriptors); + + // Store a greyscale copy for the optical-flow fallback. + if (img.channels() > 1) + cvtColor(img, out.image, COLOR_BGR2GRAY); + else + out.image = img.clone(); + + // Pre-compute undistorted pixel coordinates used by every stage. + if (!out.keypoints.empty()) + { + std::vector raw; + raw.reserve(out.keypoints.size()); + for (const auto& kp : out.keypoints) + raw.push_back(kp.pt); + + if (!dist.empty()) + undistortPoints(raw, out.undistKpts, K, dist, noArray(), K); + else + out.undistKpts = raw; + } +} + +// Frame matching helper + +void VisualOdometryImpl::matchFrames( + const std::vector& qKp, const Mat& qDesc, Size qSz, + const std::vector& tKp, const Mat& tDesc, Size tSz, + std::vector& matches) const +{ + matches.clear(); + if (qDesc.empty() || tDesc.empty()) return; + if (qKp.empty() || tKp.empty()) return; + + LightGlueMatcher* lg = dynamic_cast(matcher.get()); + if (lg) + { + Mat qk((int)qKp.size(), 2, CV_32F); + for (size_t i = 0; i < qKp.size(); ++i) + { qk.at((int)i,0) = qKp[i].pt.x; qk.at((int)i,1) = qKp[i].pt.y; } + + Mat tk((int)tKp.size(), 2, CV_32F); + for (size_t i = 0; i < tKp.size(); ++i) + { tk.at((int)i,0) = tKp[i].pt.x; tk.at((int)i,1) = tKp[i].pt.y; } + + lg->setPairInfo(qk, tk, qSz, tSz); + } + + matcher->match(qDesc, tDesc, matches); +} + +// Batch run() + +bool VisualOdometryImpl::run() +{ + CV_INSTRUMENT_REGION(); + + if (imagesFolder.empty()) + { + CV_LOG_ERROR(NULL, "VisualOdometry::run: imagesFolder is empty"); + return false; + } + + std::vector allFiles; + try { cv::glob(imagesFolder, allFiles, false); } + catch (const cv::Exception& e) + { + CV_LOG_ERROR(NULL, "VisualOdometry::run: glob failed: " << e.what()); + return false; + } + + std::vector imgFiles; + imgFiles.reserve(allFiles.size()); + for (const auto& f : allFiles) + if (cv::haveImageReader(f)) imgFiles.push_back(f); + std::sort(imgFiles.begin(), imgFiles.end()); + + if (imgFiles.empty()) + { + CV_LOG_WARNING(NULL, "VisualOdometry::run: no images in " << imagesFolder); + return false; + } + + std::ofstream log; + if (!outputFolder.empty()) + { + cv::utils::fs::createDirectories(outputFolder); + log.open(joinPath(outputFolder, "vo.log").c_str()); + } + auto logln = [&](const String& s) { if (log.is_open()) log << s << "\n"; }; + + logln("[INFO] optimizer = reprojection inlier check"); + logln(String("[INFO] images_folder = ") + imagesFolder); + logln(String("[INFO] output_folder = ") + outputFolder); + { + std::ostringstream ss; + ss << "[INFO] found " << imgFiles.size() << " image(s)"; + logln(ss.str()); + } + + reset(); + + int nEmitted = 0; + size_t prevTrajLen = 0; + String refFilename; + + for (size_t i = 0; i < imgFiles.size(); ++i) + { + Mat img = imread(imgFiles[i]); + if (img.empty()) + { + std::ostringstream ss; + ss << "[FRAME " << i << "] file=" << imgFiles[i] << " ERROR: imread failed"; + logln(ss.str()); continue; + } + + OdometryState before = state; + bool emitted = processFrame(img); + OdometryState after = state; + if (emitted) ++nEmitted; + + // Track which input image maps to each trajectory pose. + if (before == NOT_INITIALIZED || + (before == TRACKING && after == INITIALIZING)) + refFilename = imgFiles[i]; + + const size_t added = map.trajectory().size() - prevTrajLen; + if (added == 1) + poseFilenames.push_back(imgFiles[i]); + else if (added == 2) + { + poseFilenames.push_back(refFilename); + poseFilenames.push_back(imgFiles[i]); + } + prevTrajLen = map.trajectory().size(); + + std::ostringstream ss; + ss << "[FRAME " << i << "] file=" << imgFiles[i] + << " state=" << stateName(before); + if (before != after) ss << "->" << stateName(after); + ss << " emitted=" << (emitted ? "yes" : "no") + << " keyframes=" << map.numKeyframes() + << " map_points=" << map.numMapPoints(); + if (!lastEvent.empty()) ss << " [" << lastEvent << "]"; + if (emitted) + { + Point3d C = detail::cameraCenterWorld(lastPoseCw); + ss << " C=(" << C.x << "," << C.y << "," << C.z << ")"; + } + logln(ss.str()); + } + + if (!outputFolder.empty()) + { + writeTrajectoryText(joinPath(outputFolder, "trajectory.txt")); + writeTrajectoryBin (joinPath(outputFolder, "trajectory.bin")); + writeMapPoints (joinPath(outputFolder, "map_points.txt")); + writeKeypoints (joinPath(outputFolder, "keypoints.txt")); + writeImagesTxt (joinPath(outputFolder, "images.txt")); + + std::ostringstream ss; + ss << "[INFO] run complete: frames=" << imgFiles.size() + << " emitted=" << nEmitted + << " keyframes=" << map.numKeyframes() + << " map_points=" << map.numMapPoints(); + logln(ss.str()); + logln("[INFO] wrote trajectory.txt, trajectory.bin, map_points.txt, " + "keypoints.txt, images.txt"); + } + + return nEmitted > 0; +} + +// IO helpers + +void VisualOdometryImpl::writeTrajectoryText(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeTrajectoryText: cannot open " << path); return; } + f << "# Per-frame camera center in world coordinates.\n# Columns: Cx Cy Cz\n"; + f.setf(std::ios::scientific); f.precision(9); + for (const auto& T : map.trajectory()) + { + Point3d C = detail::cameraCenterWorld(T); + f << C.x << " " << C.y << " " << C.z << "\n"; + } +} + +void VisualOdometryImpl::writeTrajectoryBin(const String& path) const +{ + std::ofstream f(path.c_str(), std::ios::binary); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeTrajectoryBin: cannot open " << path); return; } + const char magic[4] = {'V','O','T','R'}; + f.write(magic, 4); + int32_t version = 1; + f.write(reinterpret_cast(&version), sizeof(int32_t)); + int32_t n = (int32_t)map.trajectory().size(); + f.write(reinterpret_cast(&n), sizeof(int32_t)); + for (const auto& T : map.trajectory()) + { + double buf[16]; + for (int r = 0; r < 4; ++r) + for (int c = 0; c < 4; ++c) buf[r*4+c] = T(r,c); + f.write(reinterpret_cast(buf), sizeof(buf)); + } +} + +void VisualOdometryImpl::writeMapPoints(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeMapPoints: cannot open " << path); return; } + f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; + f.setf(std::ios::scientific); f.precision(9); + for (MapPoint* mp : map.mapPoints()) + { + if (!mp || mp->bad) continue; + f << mp->id << " " + << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " " + << mp->observations.size() << "\n"; + } +} + +void VisualOdometryImpl::writeKeypoints(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeKeypoints: cannot open " << path); return; } + f << "# Per-keyframe keypoints.\n" + << "# Block header: KF kf_id Cx Cy Cz n_keypoints\n" + << "# Followed by n_keypoints rows: kpIdx x y size angle response octave mpId\n" + << "# mpId = -1 if the keypoint has no map point.\n"; + f.setf(std::ios::fixed); f.precision(6); + + for (KeyFrame* kf : map.keyframes()) + { + if (!kf) continue; + Point3d C = detail::cameraCenterWorld(kf->poseCw); + f << "KF " << kf->id << " " << C.x << " " << C.y << " " << C.z + << " " << kf->keypoints.size() << "\n"; + for (size_t i = 0; i < kf->keypoints.size(); ++i) + { + const KeyPoint& kp = kf->keypoints[i]; + int mpId = -1; + if (i < kf->mapPoints.size() && kf->mapPoints[i]) + mpId = kf->mapPoints[i]->id; + f << i << " " << kp.pt.x << " " << kp.pt.y << " " + << kp.size << " " << kp.angle << " " << kp.response << " " + << kp.octave << " " << mpId << "\n"; + } + } +} + +// Shepperd's method: numerically-stable R → unit quaternion (qw, qx, qy, qz). +static void rotMatToQuat(const Matx33d& R, + double& qw, double& qx, double& qy, double& qz) +{ + const double tr = R(0,0) + R(1,1) + R(2,2); + if (tr > 0.0) + { + double s = std::sqrt(tr + 1.0) * 2.0; + qw = 0.25 * s; + qx = (R(2,1) - R(1,2)) / s; + qy = (R(0,2) - R(2,0)) / s; + qz = (R(1,0) - R(0,1)) / s; + } + else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) + { + double s = std::sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2.0; + qw = (R(2,1) - R(1,2)) / s; qx = 0.25 * s; + qy = (R(0,1) + R(1,0)) / s; qz = (R(0,2) + R(2,0)) / s; + } + else if (R(1,1) > R(2,2)) + { + double s = std::sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2.0; + qw = (R(0,2) - R(2,0)) / s; qx = (R(0,1) + R(1,0)) / s; + qy = 0.25 * s; qz = (R(1,2) + R(2,1)) / s; + } + else + { + double s = std::sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2.0; + qw = (R(1,0) - R(0,1)) / s; qx = (R(0,2) + R(2,0)) / s; + qy = (R(1,2) + R(2,1)) / s; qz = 0.25 * s; + } +} + +static String basenameOf(const String& path) +{ + const size_t slash = path.find_last_of("/\\"); + return (slash == String::npos) ? path : path.substr(slash + 1); +} + +void VisualOdometryImpl::writeImagesTxt(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeImagesTxt: cannot open " << path); return; } + + const auto& traj = map.trajectory(); + f << "# Image list with two lines of data per image:\n" + << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + << "# POINTS2D[] as (X, Y, POINT3D_ID)\n" + << "# Number of images: " << traj.size() << ", mean observations per image: 0.0\n"; + f.setf(std::ios::fixed); f.precision(6); + + for (size_t i = 0; i < traj.size(); ++i) + { + const Matx44d& T = traj[i]; + Matx33d R; + for (int r = 0; r < 3; ++r) + for (int c = 0; c < 3; ++c) R(r,c) = T(r,c); + double qw, qx, qy, qz; + rotMatToQuat(R, qw, qx, qy, qz); + + const String name = (i < poseFilenames.size()) + ? basenameOf(poseFilenames[i]) + : (String("pose_") + std::to_string(i)); + + f << i << " " << qw << " " << qx << " " << qy << " " << qz << " " + << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n\n"; + } +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_bootstrap.cpp b/modules/slam/src/odometry/vo_bootstrap.cpp new file mode 100644 index 0000000000..a4b6afe4e8 --- /dev/null +++ b/modules/slam/src/odometry/vo_bootstrap.cpp @@ -0,0 +1,252 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +// Returns the homography candidate index with the most cheirality-consistent triangulations. +int bestHomographyCandidate( + const std::vector& Rs, + const std::vector& ts, + const std::vector& pts1, + const std::vector& pts2, + const Mat& P1) +{ + int bestIdx = 0; + int bestCount = -1; + const int nSol = (int)Rs.size(); + + for (int s = 0; s < nSol; ++s) + { + Mat Rt(3, 4, CV_64F); + Rs[s].copyTo(Rt(Rect(0, 0, 3, 3))); + ts[s].reshape(1, 3).copyTo(Rt(Rect(3, 0, 1, 3))); + + Mat Klocal = P1(Rect(0, 0, 3, 3)).clone(); + Mat P2_s = Klocal * Rt; + + Mat pts4D; + triangulatePoints(P1, P2_s, pts1, pts2, pts4D); + + int count = 0; + for (int i = 0; i < pts4D.cols; ++i) + { + float w = pts4D.at(3, i); + if (std::abs(w) < 1e-9f) continue; + float X = pts4D.at(0, i) / w; + float Y = pts4D.at(1, i) / w; + float Z = pts4D.at(2, i) / w; + float Z2 = (float)(Rs[s].at(2,0)*X + Rs[s].at(2,1)*Y + + Rs[s].at(2,2)*Z + + ts[s].reshape(1,3).at(2,0)); + if (Z > 0 && Z2 > 0) ++count; + } + if (count > bestCount) { bestCount = count; bestIdx = s; } + } + return bestIdx; +} + +} // anonymous namespace + +bool VisualOdometryImpl::bootstrap(Frame& cur) +{ + std::vector matches; + matchFrames(refFrame.keypoints, refFrame.descriptors, refFrame.imageSize, + cur.keypoints, cur.descriptors, cur.imageSize, matches); + + if ((int)matches.size() < params.minInitInliers) + { + // slide ref forward so we don't keep trying against a frame that's too far + refFrame = std::move(cur); + return false; + } + + std::vector refU, curU; + refU.reserve(matches.size()); + curU.reserve(matches.size()); + for (const auto& m : matches) + { + refU.push_back(refFrame.undistKpts[m.queryIdx]); + curU.push_back(cur.undistKpts[m.trainIdx]); + } + + Mat maskE, maskH; + Mat E = findEssentialMat(refU, curU, K, RANSAC, + params.essentialRansacConfidence, + params.essentialRansacThresh, 1000, maskE); + Mat H = findHomography(refU, curU, RANSAC, + params.essentialRansacThresh, maskH); + + const int nE = (!E.empty() && !maskE.empty()) ? countNonZero(maskE) : 0; + const int nH = (!H.empty() && !maskH.empty()) ? countNonZero(maskH) : 0; + + if (nE < params.minInitInliers && nH < params.minInitInliers) + return false; + + const double RH = (double)nH / ((double)nH + (double)nE + 1e-9); + + Mat R, t, modelMask; + Mat P1 = K * Mat::eye(3, 4, CV_64F); + + if (RH > params.hfRatioThresh && !H.empty()) + { + std::vector Rs, ts, normals; + int nSol = decomposeHomographyMat(H, K, Rs, ts, normals); + if (nSol <= 0) return false; + + std::vector p1In, p2In; + for (size_t i = 0; i < matches.size(); ++i) + if (!maskH.empty() && maskH.at((int)i)) + { p1In.push_back(refU[i]); p2In.push_back(curU[i]); } + if (p1In.empty()) return false; + + int best = bestHomographyCandidate(Rs, ts, p1In, p2In, P1); + R = Rs[best].clone(); + t = ts[best].clone(); + modelMask = maskH; + } + else + { + if (E.empty() || nE < params.minInitInliers) return false; + Mat recoverMask = maskE.clone(); + int nPose = recoverPose(E, refU, curU, K, R, t, recoverMask); + if (nPose < params.minInitInliers) return false; + modelMask = recoverMask; + } + + std::vector refIn, curIn; + std::vector matchIn; + for (size_t i = 0; i < matches.size(); ++i) + if (!modelMask.empty() && modelMask.at((int)i)) + { + refIn.push_back(refU[i]); + curIn.push_back(curU[i]); + matchIn.push_back((int)i); + } + + if ((int)refIn.size() < params.minInitInliers) + return false; + + Mat Rt(3, 4, CV_64F); + R.copyTo(Rt(Rect(0, 0, 3, 3))); + t.reshape(1, 3).copyTo(Rt(Rect(3, 0, 1, 3))); + Mat P2 = K * Rt; + + Mat pts4D; + triangulatePoints(P1, P2, refIn, curIn, pts4D); + + Matx44d T_ref = Matx44d::eye(); + Matx44d T_cur = detail::makePose(R, t); + + std::vector goodPts; + std::vector goodMatch; + int nValid = 0; + int nPos = 0; + + for (int i = 0; i < pts4D.cols; ++i) + { + double w = pts4D.at(3, i); + if (std::abs(w) < 1e-9) continue; + ++nValid; + + double X = pts4D.at(0, i) / w; + double Y = pts4D.at(1, i) / w; + double Z = pts4D.at(2, i) / w; + + if (Z <= 0) continue; + double Z2 = R.at(2,0)*X + R.at(2,1)*Y + + R.at(2,2)*Z + t.reshape(1,3).at(2,0); + if (Z2 <= 0) continue; + ++nPos; + + Point3d Xw(X, Y, Z); + if (detail::parallaxDeg(Xw, T_ref, T_cur) < params.minInitParallaxDeg) + continue; + + goodPts.push_back(Xw); + goodMatch.push_back(matchIn[i]); + } + + // reject degenerate decompositions (< 90% positive-depth ratio) + if (nValid > 0 && (double)nPos / nValid < 0.9) + return false; + if ((int)goodPts.size() < params.minInitPoints) + return false; + + // normalize scale: set median scene depth to 1.0 + std::vector depths; + depths.reserve(goodPts.size()); + for (const auto& p : goodPts) depths.push_back(p.z); + std::nth_element(depths.begin(), depths.begin() + depths.size()/2, depths.end()); + double med = depths[depths.size()/2]; + if (med < 1e-9) return false; + + double scale = 1.0 / med; + for (auto& p : goodPts) { p.x *= scale; p.y *= scale; p.z *= scale; } + + Mat tSc; + t.reshape(1, 3).convertTo(tSc, CV_64F); + tSc = tSc * scale; + T_cur = detail::makePose(R, tSc); + + auto makeKF = [](const Frame& f) -> KeyFrame* { + KeyFrame* kf = new KeyFrame(); + kf->poseCw = Matx44d::eye(); + kf->keypoints = f.keypoints; + kf->descriptors = f.descriptors.clone(); + kf->undistKpts = f.undistKpts; + kf->imageSize = f.imageSize; + kf->mapPoints.assign(f.keypoints.size(), nullptr); + return kf; + }; + + KeyFrame* kfRef = makeKF(refFrame); + kfRef->poseCw = T_ref; + + KeyFrame* kfCur = makeKF(cur); + kfCur->poseCw = T_cur; + kfCur->parent = kfRef; + + map.addKeyframe(kfRef); + map.addKeyframe(kfCur); + + for (size_t i = 0; i < goodPts.size(); ++i) + { + MapPoint* mp = new MapPoint(); + mp->pos = goodPts[i]; + const DMatch& m = matches[goodMatch[i]]; + mp->refDesc = refFrame.descriptors.row(m.queryIdx).clone(); + + map.addMapPoint(mp); + map.addObservation(kfRef, (size_t)m.queryIdx, mp); + map.addObservation(kfCur, (size_t)m.trainIdx, mp); + } + + detail::updateCovisibility(kfRef); + + map.setRefKeyframe(kfRef); + map.setCurrentKeyframe(kfCur); + + lastKf = kfCur; + framesSinceKf = 0; + lastKfInliers = (int)goodPts.size(); + lastPoseCw = T_cur; + state = TRACKING; + hasVelocity = false; + + map.appendPose(T_ref); + map.appendPose(T_cur); + + refFrame = Frame(); + return true; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_impl.hpp b/modules/slam/src/odometry/vo_impl.hpp new file mode 100644 index 0000000000..deb673fe0d --- /dev/null +++ b/modules/slam/src/odometry/vo_impl.hpp @@ -0,0 +1,129 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_VO_IMPL_HPP +#define OPENCV_SLAM_VO_IMPL_HPP + +#include "../precomp.hpp" +#include "frame.hpp" +#include "../optimizer/pose_optimizer.hpp" + +#include + +namespace cv { +namespace slam { + +/** @brief Concrete VisualOdometry implementation (pimpl target). + +Stage logic is split across: + - vo_bootstrap.cpp : two-view H/F initialisation + - vo_tracking.cpp : per-frame localisation (motion model, fallback 1/2, local map) + - vo_keyframe.cpp : keyframe promotion decision + covisibility helpers + - vo_map_growth.cpp : triangulation of new map points at promotion time + - visual_odometry.cpp : factory, run(), processFrame(), IO writers +*/ +class VisualOdometryImpl CV_FINAL : public VisualOdometry +{ +public: + VisualOdometryImpl(const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + const Mat& cameraMatrix, + const Mat& distCoeffs, + const OdometryParams& params); + + // --- VisualOdometry interface ------------------------------------------- + + bool run() CV_OVERRIDE; + bool processFrame(InputArray image) CV_OVERRIDE; + void reset() CV_OVERRIDE; + + OdometryState getState() const CV_OVERRIDE { return state; } + Matx44d getLastPose() const CV_OVERRIDE { return lastPoseCw; } + const Map& getMap() const CV_OVERRIDE { return map; } + const std::vector& getTrajectory() const CV_OVERRIDE { return map.trajectory(); } + const OdometryParams& getParams() const CV_OVERRIDE { return params; } + void setParams(const OdometryParams& p) CV_OVERRIDE { params = p; } + + const String& getImagesFolder() const CV_OVERRIDE { return imagesFolder; } + const String& getOutputFolder() const CV_OVERRIDE { return outputFolder; } + void setOutputFolder(const String& f) CV_OVERRIDE { outputFolder = f; } + + // --- Stage entry points ------------------------------------------------- + + bool bootstrap(Frame& cur); + bool track(Frame& cur); + + bool trackWithMotionModel(Frame& cur); // motion model + bool trackWithReferenceKF(Frame& cur); // fallback 1 + bool trackWithOpticalFlow(Frame& cur); // fallback 2 + void trackLocalMap(Frame& cur); + + bool shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw, String& reason) const; + void promoteKeyframeAndGrowMap(Frame& cur); + + // --- Shared helpers (visual_odometry.cpp) -------------------------------- + + void extractFeatures(InputArray image, Frame& out) const; + + void matchFrames(const std::vector& qKp, const Mat& qDesc, Size qSz, + const std::vector& tKp, const Mat& tDesc, Size tSz, + std::vector& matches) const; + + // --- IO helpers (visual_odometry.cpp) ------------------------------------ + + void writeTrajectoryText(const String& path) const; + void writeTrajectoryBin(const String& path) const; + void writeMapPoints(const String& path) const; + void writeKeypoints(const String& path) const; + void writeImagesTxt(const String& path) const; + + // --- Owned state --------------------------------------------------------- + + Ptr detector; + Ptr matcher; + Mat K; // 3×3 CV_64F + Mat dist; // distortion coefficients (may be empty) + OdometryParams params; + + String imagesFolder; + String outputFolder; + + OdometryState state = NOT_INITIALIZED; + Matx44d lastPoseCw = Matx44d::eye(); + + Frame refFrame; + KeyFrame* lastKf = nullptr; + int framesSinceKf = 0; + int lastKfInliers = 0; + + Matx44d velocity = Matx44d::eye(); + bool hasVelocity = false; + + Frame prevFrame; + bool hasPrevFrame = false; + + String lastEvent; + std::vector poseFilenames; + + Map map; +}; + +namespace detail { + +double rotationAngleDeg(const Matx44d& A_cw, const Matx44d& B_cw); +double parallaxDeg(const Point3d& X_world, const Matx44d& A_cw, const Matx44d& B_cw); +Matx34d projectionFromPose(const Matx44d& T_cw); +Matx44d makePose(const Mat& R, const Mat& t); +Point3d cameraCenterWorld(const Matx44d& T_cw); +void updateCovisibility(KeyFrame* kf); + +} // namespace detail + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_VO_IMPL_HPP diff --git a/modules/slam/src/odometry/vo_keyframe.cpp b/modules/slam/src/odometry/vo_keyframe.cpp new file mode 100644 index 0000000000..8f6b0c29ee --- /dev/null +++ b/modules/slam/src/odometry/vo_keyframe.cpp @@ -0,0 +1,145 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace detail { + +double rotationAngleDeg(const Matx44d& A, const Matx44d& B) +{ + Matx33d Ra(A(0,0),A(0,1),A(0,2), A(1,0),A(1,1),A(1,2), A(2,0),A(2,1),A(2,2)); + Matx33d Rb(B(0,0),B(0,1),B(0,2), B(1,0),B(1,1),B(1,2), B(2,0),B(2,1),B(2,2)); + Matx33d D = Rb * Ra.t(); + double tr = D(0,0) + D(1,1) + D(2,2); + double c = std::max(-1.0, std::min(1.0, (tr - 1.0) * 0.5)); + return std::acos(c) * 180.0 / CV_PI; +} + +Point3d cameraCenterWorld(const Matx44d& T_cw) +{ + Matx33d R(T_cw(0,0),T_cw(0,1),T_cw(0,2), + T_cw(1,0),T_cw(1,1),T_cw(1,2), + T_cw(2,0),T_cw(2,1),T_cw(2,2)); + Matx31d t(T_cw(0,3),T_cw(1,3),T_cw(2,3)); + Matx31d C = -R.t() * t; + return Point3d(C(0),C(1),C(2)); +} + +double parallaxDeg(const Point3d& Xw, const Matx44d& A_cw, const Matx44d& B_cw) +{ + Point3d CA = cameraCenterWorld(A_cw), CB = cameraCenterWorld(B_cw); + Point3d vA = Xw - CA, vB = Xw - CB; + double nA = std::sqrt(vA.dot(vA)), nB = std::sqrt(vB.dot(vB)); + if (nA < 1e-9 || nB < 1e-9) return 0.0; + double c = std::max(-1.0, std::min(1.0, vA.dot(vB) / (nA * nB))); + return std::acos(c) * 180.0 / CV_PI; +} + +Matx34d projectionFromPose(const Matx44d& T) +{ + return Matx34d(T(0,0),T(0,1),T(0,2),T(0,3), + T(1,0),T(1,1),T(1,2),T(1,3), + T(2,0),T(2,1),T(2,2),T(2,3)); +} + +Matx44d makePose(const Mat& R, const Mat& t) +{ + CV_Assert(R.rows == 3 && R.cols == 3 && t.total() == 3); + Matx44d T = Matx44d::eye(); + Mat Rd, td; + R.convertTo(Rd, CV_64F); + t.reshape(1,3).convertTo(td, CV_64F); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) T(i,j) = Rd.at(i,j); + T(i,3) = td.at(i,0); + } + return T; +} + +static void rebuildOrderedCovisibility(KeyFrame* target) +{ + target->orderedCovisibility.clear(); + for (auto& [other, cnt] : target->covisibility) + target->orderedCovisibility.push_back({other, cnt}); + std::sort(target->orderedCovisibility.begin(), + target->orderedCovisibility.end(), + [](const auto& a, const auto& b){ return a.second > b.second; }); +} + +void updateCovisibility(KeyFrame* kf) +{ + if (!kf) return; + + kf->covisibility.clear(); + + for (MapPoint* mp : kf->mapPoints) + { + if (!mp || mp->bad) continue; + for (auto& [obsKf, kpIdx] : mp->observations) + { + if (obsKf == kf) continue; + kf->covisibility[obsKf]++; + obsKf->covisibility[kf]++; + } + } + + rebuildOrderedCovisibility(kf); + for (auto& [nb, cnt] : kf->covisibility) + rebuildOrderedCovisibility(nb); +} + +} // namespace detail + +bool VisualOdometryImpl::shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw, + String& reason) const +{ + if (framesSinceKf <= params.kfMinFrames) return false; + + if (framesSinceKf > params.kfMaxFrames) + { + reason = format("timeout(%d)", framesSinceKf); + return true; + } + + if (!lastKf) return false; + + // Motion-based triggers: rotation then translation (geometric, frame-count independent) + double rot = detail::rotationAngleDeg(lastKf->poseCw, T_cw); + if (rot > params.kfRotThreshDeg) + { + reason = format("rot=%.1fdeg", rot); + return true; + } + + // Tracking quality drop + int minInliers = (lastKfInliers > 0) + ? std::max(params.kfMinInliers, (int)(params.kfInlierRatio * lastKfInliers)) + : params.kfMinInliers; + if (nInliers < minInliers) + { + reason = format("inliers=%d<%d", nInliers, minInliers); + return true; + } + + Point3d cCur = detail::cameraCenterWorld(T_cw); + Point3d cLast = detail::cameraCenterWorld(lastKf->poseCw); + Point3d d = cCur - cLast; + double dist = std::sqrt(d.dot(d)); + if (dist > params.kfTransThresh) + { + reason = format("trans=%.3f", dist); + return true; + } + + return false; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_map_growth.cpp b/modules/slam/src/odometry/vo_map_growth.cpp new file mode 100644 index 0000000000..6e6dfbc590 --- /dev/null +++ b/modules/slam/src/odometry/vo_map_growth.cpp @@ -0,0 +1,135 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +inline Point2d projectThrough(const Mat& P, double X, double Y, double Z) +{ + double u = P.at(0,0)*X + P.at(0,1)*Y + P.at(0,2)*Z + P.at(0,3); + double v = P.at(1,0)*X + P.at(1,1)*Y + P.at(1,2)*Z + P.at(1,3); + double w = P.at(2,0)*X + P.at(2,1)*Y + P.at(2,2)*Z + P.at(2,3); + if (std::abs(w) < 1e-12) return Point2d(0, 0); + return Point2d(u / w, v / w); +} + +inline double cameraDepth(const Matx44d& T_cw, double X, double Y, double Z) +{ + return T_cw(2,0)*X + T_cw(2,1)*Y + T_cw(2,2)*Z + T_cw(2,3); +} + +} // anonymous namespace + +void VisualOdometryImpl::promoteKeyframeAndGrowMap(Frame& cur) +{ + KeyFrame* newKf = new KeyFrame(); + newKf->poseCw = cur.poseCw; + newKf->keypoints = cur.keypoints; + newKf->descriptors = cur.descriptors.clone(); + newKf->undistKpts = cur.undistKpts; + newKf->imageSize = cur.imageSize; + newKf->mapPoints.assign(cur.keypoints.size(), nullptr); + newKf->parent = lastKf; + + map.addKeyframe(newKf); + + for (size_t i = 0; i < cur.mapPoints.size(); ++i) + { + MapPoint* mp = cur.mapPoints[i]; + if (!mp || mp->bad || cur.outliers[i]) continue; + map.addObservation(newKf, i, mp); + } + + std::vector kfToCur; + matchFrames(lastKf->keypoints, lastKf->descriptors, lastKf->imageSize, + cur.keypoints, cur.descriptors, cur.imageSize, kfToCur); + + Mat Rt1(3, 4, CV_64F), Rt2(3, 4, CV_64F); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 4; ++j) + { + Rt1.at(i,j) = lastKf->poseCw(i,j); + Rt2.at(i,j) = cur.poseCw(i,j); + } + Mat P1 = K * Rt1; + Mat P2 = K * Rt2; + + std::vector pts1, pts2; + std::vector triMatchIdx; + for (size_t i = 0; i < kfToCur.size(); ++i) + { + const DMatch& m = kfToCur[i]; + if ((size_t)m.queryIdx >= lastKf->mapPoints.size()) continue; + if (lastKf->mapPoints[m.queryIdx] != nullptr) continue; + if ((size_t)m.trainIdx >= newKf->mapPoints.size()) continue; + if (newKf->mapPoints[m.trainIdx] != nullptr) continue; + pts1.push_back(lastKf->undistKpts[m.queryIdx]); + pts2.push_back(cur.undistKpts[m.trainIdx]); + triMatchIdx.push_back((int)i); + } + + if (pts1.empty()) + { + detail::updateCovisibility(newKf); + map.setCurrentKeyframe(newKf); + lastKf = newKf; + lastKfInliers = 0; + framesSinceKf = 0; + return; + } + + Mat pts4D; + triangulatePoints(P1, P2, pts1, pts2, pts4D); + + int nNew = 0; + for (int i = 0; i < pts4D.cols; ++i) + { + double w = pts4D.at(3, i); + if (std::abs(w) < 1e-9) continue; + double X = pts4D.at(0, i) / w; + double Y = pts4D.at(1, i) / w; + double Z = pts4D.at(2, i) / w; + + if (cameraDepth(lastKf->poseCw, X, Y, Z) <= 0) continue; + if (cameraDepth(cur.poseCw, X, Y, Z) <= 0) continue; + + Point2d p1p = projectThrough(P1, X, Y, Z); + Point2d p2p = projectThrough(P2, X, Y, Z); + double e1 = std::hypot(p1p.x - pts1[i].x, p1p.y - pts1[i].y); + double e2 = std::hypot(p2p.x - pts2[i].x, p2p.y - pts2[i].y); + if (e1 > params.pnpReprojThresh || e2 > params.pnpReprojThresh) continue; + + Point3d Xw(X, Y, Z); + if (detail::parallaxDeg(Xw, lastKf->poseCw, cur.poseCw) + < params.minGrowthParallaxDeg) continue; + + MapPoint* mp = new MapPoint(); + mp->pos = Xw; + const DMatch& dm = kfToCur[triMatchIdx[i]]; + mp->refDesc = cur.descriptors.row(dm.trainIdx).clone(); + + map.addMapPoint(mp); + map.addObservation(lastKf, (size_t)dm.queryIdx, mp); + map.addObservation(newKf, (size_t)dm.trainIdx, mp); + ++nNew; + } + + detail::updateCovisibility(newKf); + + map.setCurrentKeyframe(newKf); + lastKf = newKf; + lastKfInliers = 0; + framesSinceKf = 0; + + (void)nNew; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_tracking.cpp b/modules/slam/src/odometry/vo_tracking.cpp new file mode 100644 index 0000000000..f27147033a --- /dev/null +++ b/modules/slam/src/odometry/vo_tracking.cpp @@ -0,0 +1,390 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +bool projectPoint(const Matx44d& T_cw, const Mat& K, + double Xw, double Yw, double Zw, + double& u, double& v) +{ + const double Xc = T_cw(0,0)*Xw + T_cw(0,1)*Yw + T_cw(0,2)*Zw + T_cw(0,3); + const double Yc = T_cw(1,0)*Xw + T_cw(1,1)*Yw + T_cw(1,2)*Zw + T_cw(1,3); + const double Zc = T_cw(2,0)*Xw + T_cw(2,1)*Yw + T_cw(2,2)*Zw + T_cw(2,3); + if (Zc <= 0.0) return false; + u = K.at(0,0) * Xc / Zc + K.at(0,2); + v = K.at(1,1) * Yc / Zc + K.at(1,2); + return true; +} + +inline double descDist(const Mat& a, const Mat& b) +{ + if (a.empty() || b.empty()) return std::numeric_limits::max(); + int normType = (a.type() == CV_8U) ? NORM_HAMMING : NORM_L2; + return norm(a, b, normType); +} + +void buildLocalMapPoints(const KeyFrame* kf, int topK, + std::set& localMps) +{ + if (!kf) return; + for (MapPoint* mp : kf->mapPoints) + if (mp && !mp->bad) localMps.insert(mp); + + int k = 0; + for (const auto& [nbKf, cnt] : kf->orderedCovisibility) + { + if (k++ >= topK) break; + for (MapPoint* mp : nbKf->mapPoints) + if (mp && !mp->bad) localMps.insert(mp); + } +} + +int runPnP(const std::vector& obj, const std::vector& img, + Frame& frame, + const Mat& K, double reprojThresh, int maxIters, double confidence, + int minInliers) +{ + if ((int)obj.size() < minInliers) return -1; + + Mat rvec, tvec, inlierIdx; + bool ok = solvePnPRansac(obj, img, K, Mat(), + rvec, tvec, false, + maxIters, (float)reprojThresh, confidence, + inlierIdx, SOLVEPNP_AP3P); + if (!ok || inlierIdx.rows < minInliers) return -1; + + std::vector objIn; + std::vector imgIn; + objIn.reserve(inlierIdx.rows); imgIn.reserve(inlierIdx.rows); + for (int i = 0; i < inlierIdx.rows; ++i) + { + int idx = inlierIdx.at(i); + objIn.push_back(obj[idx]); + imgIn.push_back(img[idx]); + } + solvePnPRefineLM(objIn, imgIn, K, Mat(), rvec, tvec); + + Mat R; + Rodrigues(rvec, R); + frame.poseCw = detail::makePose(R, tvec); + return inlierIdx.rows; +} + +} // anonymous namespace + +// Motion model: constant-velocity pose prediction + projected map point search +bool VisualOdometryImpl::trackWithMotionModel(Frame& cur) +{ + if (!lastKf) return false; + + cur.poseCw = velocity * lastPoseCw; + + std::set localMps; + buildLocalMapPoints(lastKf, params.localMapTopK, localMps); + + std::fill(cur.mapPoints.begin(), cur.mapPoints.end(), nullptr); + std::fill(cur.outliers.begin(), cur.outliers.end(), false); + + auto doSearch = [&](float radius) -> int { + int n = 0; + for (MapPoint* mp : localMps) + { + double u, v; + if (!projectPoint(cur.poseCw, K, mp->pos.x, mp->pos.y, mp->pos.z, u, v)) + continue; + if (u < 0 || u >= cur.imageSize.width || + v < 0 || v >= cur.imageSize.height) continue; + + mp->visibleCount++; + + auto cands = cur.getKeypointsInRadius((float)u, (float)v, radius); + double bestD = std::numeric_limits::max(); + size_t bestI = std::numeric_limits::max(); + for (size_t idx : cands) + { + if (cur.mapPoints[idx]) continue; + double d = descDist(mp->refDesc, cur.descriptors.row((int)idx)); + if (d < bestD) { bestD = d; bestI = idx; } + } + if (bestI != std::numeric_limits::max() && + bestD < params.descProjThresh) + { + cur.mapPoints[bestI] = mp; + mp->foundCount++; + ++n; + } + } + return n; + }; + + int n = doSearch((float)params.motionModelRadius); + if (n < params.motionModelMinMatches) + { + std::fill(cur.mapPoints.begin(), cur.mapPoints.end(), nullptr); + n = doSearch((float)params.motionModelRadiusWide); + } + + if (n < params.pnpMinInliers) return false; + + std::vector obj; std::vector img; + obj.reserve(n); img.reserve(n); + for (size_t i = 0; i < cur.mapPoints.size(); ++i) + { + if (!cur.mapPoints[i]) continue; + const MapPoint* mp = cur.mapPoints[i]; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(cur.undistKpts[i]); + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, + params.pnpRansacIters, + params.pnpConfidence, + params.pnpMinInliers); + if (nInliers < 0) return false; + + int nOpt = poseInlierCheck(cur, K, params.pnpReprojThresh); + return nOpt >= params.pnpMinInliers; +} + +// Fallback 1: descriptor match against the reference keyframe +bool VisualOdometryImpl::trackWithReferenceKF(Frame& cur) +{ + if (!lastKf) return false; + + std::fill(cur.mapPoints.begin(), cur.mapPoints.end(), nullptr); + std::fill(cur.outliers.begin(), cur.outliers.end(), false); + + std::vector matches; + matchFrames(lastKf->keypoints, lastKf->descriptors, lastKf->imageSize, + cur.keypoints, cur.descriptors, cur.imageSize, matches); + + std::vector obj; + std::vector img; + std::vector corrMps; + std::vector corrKp; + obj.reserve(matches.size()); img.reserve(matches.size()); + corrMps.reserve(matches.size()); corrKp.reserve(matches.size()); + + for (const auto& m : matches) + { + if ((size_t)m.queryIdx >= lastKf->mapPoints.size()) continue; + MapPoint* mp = lastKf->mapPoints[m.queryIdx]; + if (!mp || mp->bad) continue; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(cur.undistKpts[m.trainIdx]); + corrMps.push_back(mp); + corrKp.push_back(m.trainIdx); + } + + if ((int)obj.size() < params.pnpMinInliers) + { + lastEvent = format("refKF: 2d3d=%d < %d", (int)obj.size(), params.pnpMinInliers); + return false; + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, params.pnpRansacIters, + params.pnpConfidence, params.pnpMinInliers); + if (nInliers < 0) + { + lastEvent = "refKF: PnP failed"; + return false; + } + + for (size_t k = 0; k < corrMps.size(); ++k) + { + int kpIdx = corrKp[k]; + if ((size_t)kpIdx < cur.mapPoints.size() && !cur.mapPoints[kpIdx]) + cur.mapPoints[kpIdx] = corrMps[k]; + } + + int nOpt = poseInlierCheck(cur, K, params.pnpReprojThresh); + return nOpt >= params.pnpMinInliers; +} + +// Fallback 2: optical flow when descriptor match also fails +bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) +{ + if (!hasPrevFrame || prevFrame.image.empty()) return false; + + const Frame& prev = prevFrame; + if (prev.undistKpts.empty()) return false; + + std::vector prevPts(prev.undistKpts.begin(), prev.undistKpts.end()); + std::vector curPts; + std::vector status; + std::vector err; + + calcOpticalFlowPyrLK(prev.image, cur.image, prevPts, curPts, + status, err, Size(21, 21), 3, + TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, 30, 0.01)); + + std::vector obj; std::vector img; + obj.reserve(prevPts.size()); img.reserve(prevPts.size()); + + for (size_t i = 0; i < prevPts.size(); ++i) + { + if (!status[i]) continue; + if (i >= prev.mapPoints.size()) continue; + MapPoint* mp = prev.mapPoints[i]; + if (!mp || mp->bad) continue; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(curPts[i]); + } + + if ((int)obj.size() < params.opticalFlowMinInliers) + { + lastEvent = format("optflow: corr=%d < %d", (int)obj.size(), params.opticalFlowMinInliers); + return false; + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, params.pnpRansacIters, + params.pnpConfidence, params.opticalFlowMinInliers); + if (nInliers < 0) + { + lastEvent = "optflow: PnP failed"; + return false; + } + + return true; +} + +// Local map refinement: expand coverage + recheck inliers +void VisualOdometryImpl::trackLocalMap(Frame& cur) +{ + if (!lastKf) return; + + std::set localMps; + buildLocalMapPoints(lastKf, params.localMapTopK, localMps); + + int nbK = 0; + for (const auto& [nbKf, cnt] : lastKf->orderedCovisibility) + { + if (nbK++ >= params.localMapTopK) break; + int nb2K = 0; + for (const auto& [nb2Kf, cnt2] : nbKf->orderedCovisibility) + { + if (nb2K++ >= params.localMapNeighborK) break; + for (MapPoint* mp : nb2Kf->mapPoints) + if (mp && !mp->bad) localMps.insert(mp); + } + } + + std::set alreadyMatched; + for (MapPoint* mp : cur.mapPoints) + if (mp) alreadyMatched.insert(mp); + + bool anyNew = false; + const float r = (float)params.localMapRadius; + + for (MapPoint* mp : localMps) + { + if (alreadyMatched.count(mp)) continue; + + double u, v; + if (!projectPoint(cur.poseCw, K, mp->pos.x, mp->pos.y, mp->pos.z, u, v)) + continue; + if (u < 0 || u >= cur.imageSize.width || + v < 0 || v >= cur.imageSize.height) continue; + + mp->visibleCount++; + + auto cands = cur.getKeypointsInRadius((float)u, (float)v, r); + double bestD = std::numeric_limits::max(); + size_t bestI = std::numeric_limits::max(); + for (size_t idx : cands) + { + if (cur.mapPoints[idx]) continue; + double d = descDist(mp->refDesc, cur.descriptors.row((int)idx)); + if (d < bestD) { bestD = d; bestI = idx; } + } + if (bestI != std::numeric_limits::max() && + bestD < params.descProjThresh) + { + cur.mapPoints[bestI] = mp; + cur.outliers[bestI] = false; + mp->foundCount++; + alreadyMatched.insert(mp); + anyNew = true; + } + } + + if (anyNew) + poseInlierCheck(cur, K, params.pnpReprojThresh); +} + +bool VisualOdometryImpl::track(Frame& cur) +{ + if (!lastKf) + { + refFrame = cur; + state = INITIALIZING; + return false; + } + + // motion model tracking + bool ok = false; + if (hasVelocity) + ok = trackWithMotionModel(cur); + + // fallback 1: descriptor match against reference keyframe + if (!ok) + ok = trackWithReferenceKF(cur); + + // fallback 2: optical flow + if (!ok) + ok = trackWithOpticalFlow(cur); + + if (!ok) + { + lastEvent = "track lost: all stages failed"; + refFrame = cur; + state = INITIALIZING; + return false; + } + + trackLocalMap(cur); + + int nInliers = 0; + for (size_t i = 0; i < cur.mapPoints.size(); ++i) + if (cur.mapPoints[i] && !cur.outliers[i]) ++nInliers; + + { + Matx44d Tcw_last_inv = lastPoseCw.inv(); + velocity = cur.poseCw * Tcw_last_inv; + hasVelocity = true; + } + + lastPoseCw = cur.poseCw; + map.appendPose(cur.poseCw); + ++framesSinceKf; + + prevFrame = cur; + hasPrevFrame = true; + + String kf_reason; + if (shouldPromoteKeyframe(nInliers, cur.poseCw, kf_reason)) + { + int mpBefore = map.numMapPoints(); + promoteKeyframeAndGrowMap(cur); + lastPoseCw = lastKf->poseCw; + lastEvent = format("keyframe: %s, +%d mp", + kf_reason.c_str(), + map.numMapPoints() - mpBefore); + } + + return true; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/optimizer/pose_optimizer.cpp b/modules/slam/src/optimizer/pose_optimizer.cpp new file mode 100644 index 0000000000..95b6a7faef --- /dev/null +++ b/modules/slam/src/optimizer/pose_optimizer.cpp @@ -0,0 +1,49 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "pose_optimizer.hpp" + +namespace cv { namespace slam { + +// pose is not optimized yet, only inlier classification is done. +// adding bundle adjustment based pose optimization in a later commit. + +int poseInlierCheck(Frame& frame, const Mat& K, double reprojThresh) +{ + const double fx = K.at(0, 0); + const double fy = K.at(1, 1); + const double cx = K.at(0, 2); + const double cy = K.at(1, 2); + const Matx44d& T = frame.poseCw; + + int nInliers = 0; + for (size_t i = 0; i < frame.mapPoints.size(); ++i) + { + frame.outliers[i] = true; + MapPoint* mp = frame.mapPoints[i]; + if (!mp || mp->bad) continue; + + const double Xc = T(0,0)*mp->pos.x + T(0,1)*mp->pos.y + T(0,2)*mp->pos.z + T(0,3); + const double Yc = T(1,0)*mp->pos.x + T(1,1)*mp->pos.y + T(1,2)*mp->pos.z + T(1,3); + const double Zc = T(2,0)*mp->pos.x + T(2,1)*mp->pos.y + T(2,2)*mp->pos.z + T(2,3); + if (Zc <= 0.0) continue; + + const double u = fx * Xc / Zc + cx; + const double v = fy * Yc / Zc + cy; + const double dx = u - static_cast(frame.undistKpts[i].x); + const double dy = v - static_cast(frame.undistKpts[i].y); + + if (std::sqrt(dx * dx + dy * dy) <= reprojThresh) + { + frame.outliers[i] = false; + ++nInliers; + } + } + return nInliers; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/optimizer/pose_optimizer.hpp b/modules/slam/src/optimizer/pose_optimizer.hpp new file mode 100644 index 0000000000..69c7691284 --- /dev/null +++ b/modules/slam/src/optimizer/pose_optimizer.hpp @@ -0,0 +1,21 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_POSE_OPTIMIZER_HPP +#define OPENCV_SLAM_POSE_OPTIMIZER_HPP + +#include "../odometry/frame.hpp" + +namespace cv { +namespace slam { + +// Classify map point associations as inlier/outlier by reprojection distance. +// Returns inlier count. frame.poseCw is NOT modified. +int poseInlierCheck(Frame& frame, const Mat& K, double reprojThresh); + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_POSE_OPTIMIZER_HPP diff --git a/modules/slam/src/precomp.hpp b/modules/slam/src/precomp.hpp index ecc564bffd..ad2246c4dd 100644 --- a/modules/slam/src/precomp.hpp +++ b/modules/slam/src/precomp.hpp @@ -1,28 +1,34 @@ // 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#ifndef OPENCV_SLAM_PRECOMP_HPP +#define OPENCV_SLAM_PRECOMP_HPP -#ifndef __OPENCV_SLAM_PRECOMP_HPP__ -#define __OPENCV_SLAM_PRECOMP_HPP__ - -#include -#include -#include -#include -#include -#include #include "opencv2/slam.hpp" +#include "opencv2/core.hpp" +#include "opencv2/core/utility.hpp" +#include "opencv2/core/utils/filesystem.hpp" +#include "opencv2/core/utils/logger.hpp" +#include "opencv2/core/private.hpp" +#include "opencv2/imgproc.hpp" +#include "opencv2/imgcodecs.hpp" +#include "opencv2/features.hpp" +#include "opencv2/geometry.hpp" +#include "opencv2/video/tracking.hpp" + #include #include -#include +#include +#include #include -#include +#include #include -#include -#include #include +#include #include -#endif // __OPENCV_SLAM_PRECOMP_HPP__ +#endif // OPENCV_SLAM_PRECOMP_HPP diff --git a/modules/slam/src/types.cpp b/modules/slam/test/test_main.cpp similarity index 53% rename from modules/slam/src/types.cpp rename to modules/slam/test/test_main.cpp index 80b2d0daef..1a7a912ed5 100644 --- a/modules/slam/src/types.cpp +++ b/modules/slam/test/test_main.cpp @@ -1,12 +1,9 @@ // 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#include "test_precomp.hpp" -#include "precomp.hpp" - -namespace cv { namespace slam { - -OdometryParams::OdometryParams() = default; - -}} // namespace cv::slam +CV_TEST_MAIN("cv") diff --git a/modules/slam/test/test_map.cpp b/modules/slam/test/test_map.cpp new file mode 100644 index 0000000000..fdee7e9853 --- /dev/null +++ b/modules/slam/test/test_map.cpp @@ -0,0 +1,79 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +// Allocates a keyframe with @p n keypoint slots (parallel mapPoints[] zeroed), +// as the pipeline expects before any observation is wired. +static slam::KeyFrame* newKeyFrame(int n) +{ + slam::KeyFrame* kf = new slam::KeyFrame(); + kf->keypoints.resize(n); + kf->mapPoints.assign(n, nullptr); + return kf; +} + +TEST(SLAM_Map, registers_and_clears) +{ + slam::Map map; + + // addKeyframe / addMapPoint assign sequential ids and the index round-trips. + slam::KeyFrame* kf0 = map.addKeyframe(newKeyFrame(0)); + slam::KeyFrame* kf1 = map.addKeyframe(newKeyFrame(0)); + slam::MapPoint* mp0 = map.addMapPoint(new slam::MapPoint()); + EXPECT_EQ(kf0->id, 0); + EXPECT_EQ(kf1->id, 1); + EXPECT_EQ(mp0->id, 0); + EXPECT_EQ(map.numKeyframes(), 2); + EXPECT_EQ(map.numMapPoints(), 1); + EXPECT_EQ(map.getKeyframe(1), kf1); + EXPECT_EQ(map.getMapPoint(0), mp0); + EXPECT_TRUE(map.getKeyframe(99) == nullptr); + + // An explicit id must advance the auto-id counter so it cannot collide. + slam::KeyFrame* kfExplicit = new slam::KeyFrame(); + kfExplicit->id = 5; + map.addKeyframe(kfExplicit); + EXPECT_EQ(map.addKeyframe(newKeyFrame(0))->id, 6); + + // clear() drops all state and restarts the id counters. + map.clear(); + EXPECT_EQ(map.numKeyframes(), 0); + EXPECT_EQ(map.numMapPoints(), 0); + EXPECT_EQ(map.addKeyframe(newKeyFrame(0))->id, 0); +} + +TEST(SLAM_Map, wires_and_removes_observations) +{ + slam::Map map; + slam::KeyFrame* kfA = map.addKeyframe(newKeyFrame(3)); + slam::KeyFrame* kfB = map.addKeyframe(newKeyFrame(3)); + slam::MapPoint* mp = map.addMapPoint(new slam::MapPoint()); + + // addObservation links both directions: keyframe slot <-> observation map. + map.addObservation(kfA, 2, mp); + map.addObservation(kfB, 0, mp); + EXPECT_EQ(kfA->mapPoints[2], mp); + EXPECT_EQ(mp->observations[kfA], 2u); + EXPECT_EQ(mp->observations.size(), 2u); + + // Adding into an already-occupied slot is a no-op. + slam::MapPoint* other = map.addMapPoint(new slam::MapPoint()); + map.addObservation(kfA, 2, other); + EXPECT_EQ(kfA->mapPoints[2], mp); + EXPECT_EQ(other->observations.count(kfA), 0u); + + // removeMapPoint unlinks every observing keyframe and drops the point. + const int mpId = mp->id; + map.removeMapPoint(mp); // deletes mp; must not be dereferenced afterwards + EXPECT_TRUE(map.getMapPoint(mpId) == nullptr); + EXPECT_TRUE(kfA->mapPoints[2] == nullptr); + EXPECT_TRUE(kfB->mapPoints[0] == nullptr); +} + +}} // namespace opencv_test diff --git a/modules/slam/test/test_precomp.hpp b/modules/slam/test/test_precomp.hpp index 9afe8de0be..735e4f4d22 100644 --- a/modules/slam/test/test_precomp.hpp +++ b/modules/slam/test/test_precomp.hpp @@ -1,12 +1,15 @@ // 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#ifndef OPENCV_SLAM_TEST_PRECOMP_HPP +#define OPENCV_SLAM_TEST_PRECOMP_HPP -#ifndef __OPENCV_TEST_SLAM_PRECOMP_HPP__ -#define __OPENCV_TEST_SLAM_PRECOMP_HPP__ +#include "opencv2/ts.hpp" +#include "opencv2/slam.hpp" +#include "opencv2/geometry.hpp" +#include "opencv2/features.hpp" -#include -#include - -#endif // __OPENCV_TEST_SLAM_PRECOMP_HPP__ +#endif diff --git a/modules/slam/test/test_visual_odometry.cpp b/modules/slam/test/test_visual_odometry.cpp index 4496908507..a3ea62136f 100644 --- a/modules/slam/test/test_visual_odometry.cpp +++ b/modules/slam/test/test_visual_odometry.cpp @@ -1,136 +1,251 @@ // 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #include "test_precomp.hpp" namespace opencv_test { namespace { -// ============================================================================= -// Map tests -// ============================================================================= +// The slam module has no committed image data, so - as in the geometry module's +// tests - the pipeline is exercised on a synthetic scene: a fixed 3D point cloud +// (generated with theRNG()) projected through a pin-hole camera that translates +// along +X. A stub Feature2D replays the projected keypoints frame by frame and +// a brute-force matcher recovers the ground-truth correspondences, so +// VisualOdometry runs end-to-end without a real detector or image files. -TEST(Map, AddAndRetrieveKeyframe) +const int descDim = 8; +const int cloudSize = 400; +const Size imageSize(640, 480); + +// Camera centres along +X: a wide first baseline for good bootstrap parallax, +// then small uniform steps suited to per-frame tracking. +static const double camCenters[] = { 0.0, 0.8, 1.1, 1.4, 1.7, 2.0 }; + +static Matx33d cameraMatrix() { - 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); + return Matx33d(500, 0, 320, + 0, 500, 240, + 0, 0, 1); } -TEST(Map, AddAndRetrieveMapPoint) +// Each landmark gets a globally-unique, view-invariant descriptor: identical for +// the same 3D point in every frame (so the matcher pairs them at distance 0) and +// >= sqrt(descDim) apart for distinct points (well above descProjThresh, so +// the projection search during tracking never mismatches). +static Mat makeDescriptor(int id) { - 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); + return Mat(1, descDim, CV_32F, Scalar((double)(id + 1))); } -TEST(Map, RemoveMapPointCleansObservations) +// Stub Feature2D: replays pre-computed keypoints/descriptors, one frame per +// detectAndCompute() call (processFrame() invokes the detector exactly once). +class StubDetector CV_FINAL : public Feature2D { - cv::slam::Map map; +public: + struct FrameFeatures { std::vector keypoints; Mat descriptors; }; + std::vector frames; + size_t next = 0; - // 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); + void detectAndCompute(InputArray, InputArray, + std::vector& keypoints, + OutputArray descriptors, bool) CV_OVERRIDE + { + CV_Assert(next < frames.size()); + keypoints = frames[next].keypoints; + frames[next].descriptors.copyTo(descriptors); + ++next; + } +}; - // 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); +static std::vector makeCloud() +{ + RNG& rng = theRNG(); // seeded per-test by the ts framework -> reproducible + std::vector pts(cloudSize); + for (int i = 0; i < cloudSize; i++) + pts[i] = Point3f(rng.uniform(-2.5f, 2.5f), + rng.uniform(-1.8f, 1.8f), + rng.uniform( 4.0f, 9.0f)); // varied depth -> non-planar + return pts; } -TEST(Map, TrajectoryAppendAndRetrieve) +// Projects the cloud through the camera at centre (camX, 0, 0), keeping the +// points that land inside the image. Ground-truth pose is pure translation +// (R = I), so projectPoints takes a zero rotation vector. +static StubDetector::FrameFeatures renderFrame(const std::vector& cloud, double camX) { - cv::slam::Map map; + Vec3d rvec(0, 0, 0), tvec(-camX, 0, 0); // t = -R*C, with R = I, C = (camX,0,0) + std::vector proj; + projectPoints(cloud, rvec, tvec, cameraMatrix(), noArray(), proj); - 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); + StubDetector::FrameFeatures ff; + std::vector ids; + for (int i = 0; i < (int)cloud.size(); i++) + { + const Point2f& p = proj[i]; + if (p.x < 0 || p.x >= imageSize.width || + p.y < 0 || p.y >= imageSize.height) continue; + ff.keypoints.push_back(KeyPoint(p, 7.f)); + ids.push_back(i); + } + ff.descriptors.create((int)ids.size(), descDim, CV_32F); + for (int r = 0; r < (int)ids.size(); r++) + makeDescriptor(ids[r]).copyTo(ff.descriptors.row(r)); + return ff; } -TEST(Map, ClearResetsState) +// Builds a VisualOdometry fed by a stub detector pre-loaded with @p nFrames of +// the synthetic sequence and a ground-truth brute-force matcher. +static Ptr makeOdometry(int nFrames, + const slam::OdometryParams& params = slam::OdometryParams()) { - cv::slam::Map map; + std::vector cloud = makeCloud(); + Ptr detector = makePtr(); + for (int f = 0; f < nFrames; f++) + detector->frames.push_back(renderFrame(cloud, camCenters[f])); - 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()); + Ptr matcher = BFMatcher::create(NORM_L2, true); + return slam::VisualOdometry::create(detector, matcher, "", "", + Mat(cameraMatrix()), noArray(), params); } -// ============================================================================= -// OdometryParams tests -// ============================================================================= +static Mat blankImage() { return Mat::zeros(imageSize, CV_8UC1); } -TEST(OdometryParams, DefaultValues) +// Rotation magnitude (deg) of @p T's rotation block relative to identity. +static double rotationFromIdentityDeg(const Matx44d& T) { - 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); + const double trace = T(0,0) + T(1,1) + T(2,2); + const double c = std::max(-1.0, std::min(1.0, (trace - 1.0) * 0.5)); + return std::acos(c) * 180.0 / CV_PI; +} + +// Camera centre in world coordinates: C = -R^T t. +static Point3d cameraCenter(const Matx44d& T) +{ + Matx33d R(T(0,0),T(0,1),T(0,2), T(1,0),T(1,1),T(1,2), T(2,0),T(2,1),T(2,2)); + Matx31d t(T(0,3), T(1,3), T(2,3)); + Matx31d C = -R.t() * t; + return Point3d(C(0), C(1), C(2)); +} + +TEST(SLAM_VisualOdometry, create_validates_arguments) +{ + Ptr detector = makePtr(); + Ptr matcher = BFMatcher::create(NORM_L2, true); + Mat K(cameraMatrix()); + + EXPECT_THROW(slam::VisualOdometry::create(Ptr(), matcher, "", "", K), + cv::Exception); // null detector + EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr(), "", "", K), + cv::Exception); // null matcher + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat()), + cv::Exception); // empty intrinsics + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat::eye(2, 2, CV_64F)), + cv::Exception); // wrong-size intrinsics +} + +TEST(SLAM_VisualOdometry, bootstrap_initializes_map) +{ + Ptr vo = makeOdometry(2); + Mat image = blankImage(); + + EXPECT_FALSE(vo->processFrame(image)); // frame 0 -> INITIALIZING + EXPECT_EQ(vo->getState(), slam::INITIALIZING); + + EXPECT_TRUE(vo->processFrame(image)); // frame 1 -> TRACKING + EXPECT_EQ(vo->getState(), slam::TRACKING); + + const slam::Map& map = vo->getMap(); + EXPECT_EQ(map.numKeyframes(), 2); + EXPECT_GE(map.numMapPoints(), 100); // OdometryParams::minInitPoints + EXPECT_EQ(vo->getTrajectory().size(), 2u); + + // Reference keyframe is pinned to the world origin. + const slam::KeyFrame* kfRef = map.getKeyframe(0); + ASSERT_TRUE(kfRef != nullptr); + EXPECT_LT(cv::norm(kfRef->poseCw - Matx44d::eye()), 1e-12); + + // Median scene depth is normalized to 1. + std::vector depths; + depths.reserve(map.mapPoints().size()); + for (slam::MapPoint* mp : map.mapPoints()) + depths.push_back(mp->pos.z); + ASSERT_FALSE(depths.empty()); + std::nth_element(depths.begin(), depths.begin() + depths.size() / 2, depths.end()); + EXPECT_NEAR(depths[depths.size() / 2], 1.0, 1e-2); + + // Second camera: ~no rotation; translation along +X up to scale. + const slam::KeyFrame* kfCur = map.getKeyframe(1); + ASSERT_TRUE(kfCur != nullptr); + EXPECT_LT(rotationFromIdentityDeg(kfCur->poseCw), 2.0); + Point3d C = cameraCenter(kfCur->poseCw); + EXPECT_GT(C.x, 0.0); + EXPECT_LT(std::abs(C.y), 0.2 * std::abs(C.x)); + EXPECT_LT(std::abs(C.z), 0.2 * std::abs(C.x)); +} + +TEST(SLAM_VisualOdometry, tracks_after_bootstrap) +{ + Ptr vo = makeOdometry(3); + Mat image = blankImage(); + + ASSERT_FALSE(vo->processFrame(image)); + ASSERT_TRUE(vo->processFrame(image)); + ASSERT_EQ(vo->getState(), slam::TRACKING); + const Point3d cBootstrap = cameraCenter(vo->getLastPose()); + + EXPECT_TRUE(vo->processFrame(image)); // frame 2 -> tracked by PnP + EXPECT_EQ(vo->getState(), slam::TRACKING); + EXPECT_EQ(vo->getTrajectory().size(), 3u); + + const Matx44d pose = vo->getLastPose(); + EXPECT_LT(rotationFromIdentityDeg(pose), 2.0); + const Point3d C = cameraCenter(pose); + EXPECT_GT(C.x, cBootstrap.x); // camera keeps advancing +X + EXPECT_LT(std::abs(C.y), 0.2 * std::abs(C.x)); + EXPECT_LT(std::abs(C.z), 0.2 * std::abs(C.x)); +} + +TEST(SLAM_VisualOdometry, promotes_keyframes_during_tracking) +{ + slam::OdometryParams params; + params.kfMaxFrames = 1; // force a keyframe promotion early in tracking + + Ptr vo = makeOdometry(6, params); + Mat image = blankImage(); + + ASSERT_FALSE(vo->processFrame(image)); // -> INITIALIZING + ASSERT_TRUE(vo->processFrame(image)); // -> TRACKING (2 keyframes) + ASSERT_EQ(vo->getMap().numKeyframes(), 2); + + for (int f = 2; f < 6; f++) + EXPECT_TRUE(vo->processFrame(image)); + + EXPECT_EQ(vo->getState(), slam::TRACKING); + EXPECT_GT(vo->getMap().numKeyframes(), 2); // new keyframes were promoted + + const slam::KeyFrame* current = vo->getMap().getCurrentKeyframe(); + ASSERT_TRUE(current != nullptr); + EXPECT_GT(current->id, 1); // current keyframe advanced +} + +TEST(SLAM_VisualOdometry, reset_clears_state) +{ + Ptr vo = makeOdometry(2); + Mat image = blankImage(); + + vo->processFrame(image); + vo->processFrame(image); + ASSERT_EQ(vo->getState(), slam::TRACKING); + ASSERT_GT(vo->getMap().numKeyframes(), 0); + + vo->reset(); + EXPECT_EQ(vo->getState(), slam::NOT_INITIALIZED); + EXPECT_EQ(vo->getMap().numKeyframes(), 0); + EXPECT_EQ(vo->getMap().numMapPoints(), 0); + EXPECT_TRUE(vo->getTrajectory().empty()); + EXPECT_LT(cv::norm(vo->getLastPose() - Matx44d::eye()), 1e-12); } }} // namespace opencv_test diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 933e2f9eff..e9ca4e196b 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -23,6 +23,7 @@ endif() add_subdirectory(cpp) add_subdirectory(java/tutorial_code) add_subdirectory(dnn) +add_subdirectory(slam) add_subdirectory(gpu) add_subdirectory(tapi) add_subdirectory(opencl) @@ -124,6 +125,7 @@ if(WIN32) add_subdirectory(directx) endif() add_subdirectory(dnn) +add_subdirectory(slam) # add_subdirectory(gpu) add_subdirectory(opencl) add_subdirectory(sycl) diff --git a/samples/slam/CMakeLists.txt b/samples/slam/CMakeLists.txt new file mode 100644 index 0000000000..4c6dd1707d --- /dev/null +++ b/samples/slam/CMakeLists.txt @@ -0,0 +1,22 @@ +ocv_install_example_src(slam *.cpp *.hpp CMakeLists.txt) + +set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS + opencv_core + opencv_dnn + opencv_features + opencv_slam) +ocv_check_dependencies(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) + +if(NOT BUILD_EXAMPLES OR NOT OCV_DEPENDENCIES_FOUND) + return() +endif() + +project(slam_samples) +ocv_include_modules_recurse(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) +include_directories("${CMAKE_CURRENT_SOURCE_DIR}/../dnn") + +file(GLOB_RECURSE slam_samples RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp) +foreach(sample_filename ${slam_samples}) + ocv_define_sample(tgt ${sample_filename} slam) + ocv_target_link_libraries(${tgt} PRIVATE ${OPENCV_LINKER_LIBS} ${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) +endforeach() diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp new file mode 100644 index 0000000000..e762686d37 --- /dev/null +++ b/samples/slam/visual_odometry.cpp @@ -0,0 +1,63 @@ +// 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. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include +#include +#include +#include +#include + +using namespace cv; + +static const char* ALIKED_MODEL = "/media/user/path/to/models/aliked-n16rot-top1k-640.onnx"; +static const char* LIGHTGLUE_MODEL = "/media/user/path/to/models/aliked_lightglue.onnx"; +static const char* IMAGES_DIR = "/media/user/path/to/dataset"; +static const char* OUTPUT_DIR = "vo_out"; + +// KITTI-00: fx, fy, cx, cy +static const Matx33d K(718.856, 0., 607.1928, + 0., 718.856, 185.2157, + 0., 0., 1.); + +// k1, k2, p1, p2, k3 +static const std::vector DIST = { -0.2811, 0.0723, -0.0003, 0.0001, 0.0 }; + +static Ptr makeDetector() +{ + ALIKED::Params p; + p.inputSize = Size(640, 640); + p.engine = dnn::ENGINE_NEW; + return ALIKED::create(ALIKED_MODEL, p); +} + +static Ptr makeMatcher() +{ + return LightGlueMatcher::create(LIGHTGLUE_MODEL, 0.0f, + dnn::DNN_BACKEND_DEFAULT, + dnn::DNN_TARGET_CPU); +} + +int main() +{ + slam::OdometryParams params; + params.minInitParallaxDeg = 1.5; + params.minInitPoints = 50; + + auto vo = slam::VisualOdometry::create( + makeDetector(), makeMatcher(), + IMAGES_DIR, OUTPUT_DIR, + Mat(K), Mat(DIST), params); + + const int64 t0 = getTickCount(); + const bool ok = vo->run(); + const double elapsed = (getTickCount() - t0) / getTickFrequency(); + + std::cout << "run=" << (ok ? "ok" : "FAILED") + << " frames=" << vo->getTrajectory().size() + << " elapsed=" << elapsed << "s\n" + << "output -> " << OUTPUT_DIR << "\n"; + return ok ? 0 : 1; +} diff --git a/samples/slam/visual_odometry.py b/samples/slam/visual_odometry.py new file mode 100644 index 0000000000..af4499da37 --- /dev/null +++ b/samples/slam/visual_odometry.py @@ -0,0 +1,56 @@ +''' +Monocular visual odometry with cv.slam.VisualOdometry (ALIKED + LightGlue). +''' + +import time +import numpy as np +import cv2 as cv + +ALIKED_MODEL = '/media/user/path/to/models/aliked-n16rot-top1k-640.onnx' +LIGHTGLUE_MODEL = '/media/user/path/to/models/aliked_lightglue.onnx' +IMAGES_DIR = '/media/user/path/to/dataset' +OUTPUT_DIR = 'vo_out' + +# KITTI-00: fx, fy, cx, cy +K = np.array([[718.856, 0., 607.1928], + [0., 718.856, 185.2157], + [0., 0., 1. ]], dtype=np.float64) + +# k1, k2, p1, p2, k3 +DIST = np.array([-0.2811, 0.0723, -0.0003, 0.0001, 0.0], dtype=np.float64) + + +def make_detector(): + p = cv.ALIKED.Params() + p.inputSize = (640, 640) + p.engine = cv.dnn.ENGINE_NEW + return cv.ALIKED.create(ALIKED_MODEL, p) + + +def make_matcher(): + return cv.LightGlueMatcher.create( + LIGHTGLUE_MODEL, 0.0, + cv.dnn.DNN_BACKEND_DEFAULT, + cv.dnn.DNN_TARGET_CPU) + + +def main(): + params = cv.slam.OdometryParams() + params.minInitParallaxDeg = 1.5 + params.minInitPoints = 50 + + vo = cv.slam.VisualOdometry.create( + make_detector(), make_matcher(), + IMAGES_DIR, OUTPUT_DIR, + K, DIST, params) + + t0 = time.perf_counter() + ok = vo.run() + elapsed = time.perf_counter() - t0 + + print(f"run={'ok' if ok else 'FAILED'} frames={len(vo.getTrajectory())} elapsed={elapsed:.2f}s") + print(f"output -> {OUTPUT_DIR}") + + +if __name__ == '__main__': + main()