cleanupFinal

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)

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

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 <opencv2/core.hpp>
+#include "opencv2/slam/types.hpp"
+
+#include <set>
 #include <vector>
 
-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<KeyFrame*>& 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<KeyFrame*>& 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<MapPoint*> mapPoints() const;
+    const std::set<MapPoint*>& 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<Matx44d>& trajectory() const;
+    // Trajectory
 
-    /** @brief Reset to empty state. */
-    CV_WRAP void clear();
+    void appendPose(const Matx44d& T_cw);
+    const std::vector<Matx44d>& trajectory() const;
+
+    // Lifecycle
+
+    void clear();
 
 private:
     struct Impl;
-    Ptr<Impl> impl_;
+    Ptr<Impl> impl;
 };
 
+//! @}
+
 }} // namespace cv::slam
 
 #endif // OPENCV_SLAM_MAP_HPP

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 <opencv2/core.hpp>
+#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

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 <opencv2/slam.hpp>.
+#ifndef OPENCV_SLAM_SLAM_HPP
+#define OPENCV_SLAM_SLAM_HPP
+
+#include "opencv2/slam.hpp"
+
+#endif

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 <opencv2/core.hpp>
+#include "opencv2/core.hpp"
+#include "opencv2/core/types.hpp"
+
 #include <map>
 #include <vector>
 
-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<KeyPoint> 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<KeyFrame*, int> observations; //!< Observing keyframe → keypoint index.
+    std::map<KeyFrame*, size_t> 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<KeyPoint> keypoints;
-    Mat                   descriptors;
-    std::vector<Point2f>  undist_kpts;
-    Size                  imageSize;
+    Mat descriptors;
+    std::vector<Point2f> undistKpts; // parallel to keypoints
+    Size imageSize;
 
-    std::vector<int>       kpt_to_mp;  //!< Map-point id per keypoint, -1 if unmatched.
-    std::vector<MapPoint*> mappoints;  //!< Direct pointer per keypoint, nullptr if unmatched.
+    std::vector<MapPoint*> mapPoints; // parallel to keypoints; null = unmatched
 
-    std::vector<std::pair<KeyFrame*, int>> ordered_covisibility; //!< Neighbours by shared point count.
+    std::map<KeyFrame*, int> covisibility;
+    std::vector<std::pair<KeyFrame*, int>> orderedCovisibility; // sorted descending by count
+
+    KeyFrame* parent = nullptr;
+    Mat globalDesc;
 };
 
+//! @}
+
 }} // namespace cv::slam
 
 #endif // OPENCV_SLAM_TYPES_HPP

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 <opencv2/core.hpp>
-#include <opencv2/features.hpp>
+#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 <vector>
+
+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<VisualOdometry> create(
-        const Ptr<Feature2D>&         detector,
+        const Ptr<Feature2D>& detector,
         const Ptr<DescriptorMatcher>& 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<Matx44d>&   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<Matx44d>& 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

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"
 

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 <opencv2/ts.hpp>
-#include <opencv2/slam.hpp>
-
-#endif // __OPENCV_PERF_SLAM_PRECOMP_HPP__
+#endif

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 <mutex>
+#include <set>
+#include <unordered_map>
+
+namespace cv {
+namespace slam {
 
 struct Map::Impl
 {
-    std::vector<KeyFrame*>             kf_vec;
-    std::vector<MapPoint*>             mp_vec;
-    std::unordered_map<int, KeyFrame*> kf_map;
-    std::unordered_map<int, MapPoint*> mp_map;
-    std::vector<Matx44d>               trajectory;
-    int next_kf_id = 0;
-    int next_mp_id = 0;
+    std::set<KeyFrame*> keyframes;
+    std::set<MapPoint*> mapPoints;
+    std::unordered_map<int, KeyFrame*> kfIndex;
+    std::unordered_map<int, MapPoint*> mpIndex;
+
+    KeyFrame* refKf = nullptr;
+    KeyFrame* currentKf = nullptr;
+
+    std::vector<Matx44d> trajectory;
+    std::mutex mutex;
+
+    int nextKfId = 0;
+    int nextMpId = 0;
 };
 
-Map::Map()  : impl_(makePtr<Impl>()) {}
+Map::Map() : impl(makePtr<Impl>()) {}
 
 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<KeyFrame*>& Map::keyframes() const { return impl_->kf_vec; }
+const std::set<KeyFrame*>& 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<MapPoint*> Map::mapPoints() const
-{
-    std::vector<MapPoint*> 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<MapPoint*>& 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<Matx44d>& 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<Matx44d>& 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

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<KeyPoint> keypoints;
+    Mat descriptors;
+    std::vector<Point2f> undistKpts; //!< undistorted, parallel to keypoints
+    Size imageSize;
+
+    Matx44d poseCw = Matx44d::eye();
+    std::vector<MapPoint*> mapPoints; //!< parallel to keypoints; nullptr = unmatched
+    std::vector<bool> 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<int, std::vector<size_t>> 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<size_t> getKeypointsInRadius(float x, float y, float r) const
+    {
+        std::vector<size_t> 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

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 <fstream>
+#include <sstream>
+
+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> VisualOdometry::create(
+    const Ptr<Feature2D>& detector,
+    const Ptr<DescriptorMatcher>& 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<VisualOdometryImpl>(
+        detector, matcher, imagesFolder, outputFolder, K, dist, params);
+}
+
+// Constructor
+
+VisualOdometryImpl::VisualOdometryImpl(
+    const Ptr<Feature2D>& detector,
+    const Ptr<DescriptorMatcher>& 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<Point2f> 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<KeyPoint>& qKp, const Mat& qDesc, Size qSz,
+    const std::vector<KeyPoint>& tKp, const Mat& tDesc, Size tSz,
+    std::vector<DMatch>& matches) const
+{
+    matches.clear();
+    if (qDesc.empty() || tDesc.empty()) return;
+    if (qKp.empty()   || tKp.empty())   return;
+
+    LightGlueMatcher* lg = dynamic_cast<LightGlueMatcher*>(matcher.get());
+    if (lg)
+    {
+        Mat qk((int)qKp.size(), 2, CV_32F);
+        for (size_t i = 0; i < qKp.size(); ++i)
+        { qk.at<float>((int)i,0) = qKp[i].pt.x; qk.at<float>((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<float>((int)i,0) = tKp[i].pt.x; tk.at<float>((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<String> 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<String> 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<const char*>(&version), sizeof(int32_t));
+    int32_t n = (int32_t)map.trajectory().size();
+    f.write(reinterpret_cast<const char*>(&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<const char*>(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

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<Mat>& Rs,
+    const std::vector<Mat>& ts,
+    const std::vector<Point2f>& pts1,
+    const std::vector<Point2f>& 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<float>(3, i);
+            if (std::abs(w) < 1e-9f) continue;
+            float X = pts4D.at<float>(0, i) / w;
+            float Y = pts4D.at<float>(1, i) / w;
+            float Z = pts4D.at<float>(2, i) / w;
+            float Z2 = (float)(Rs[s].at<double>(2,0)*X + Rs[s].at<double>(2,1)*Y
+                              + Rs[s].at<double>(2,2)*Z
+                              + ts[s].reshape(1,3).at<double>(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<DMatch> 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<Point2f> 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<Mat> Rs, ts, normals;
+        int nSol = decomposeHomographyMat(H, K, Rs, ts, normals);
+        if (nSol <= 0) return false;
+
+        std::vector<Point2f> p1In, p2In;
+        for (size_t i = 0; i < matches.size(); ++i)
+            if (!maskH.empty() && maskH.at<uchar>((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<Point2f> refIn, curIn;
+    std::vector<int> matchIn;
+    for (size_t i = 0; i < matches.size(); ++i)
+        if (!modelMask.empty() && modelMask.at<uchar>((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<Point3d> goodPts;
+    std::vector<int> goodMatch;
+    int nValid = 0;
+    int nPos = 0;
+
+    for (int i = 0; i < pts4D.cols; ++i)
+    {
+        double w = pts4D.at<float>(3, i);
+        if (std::abs(w) < 1e-9) continue;
+        ++nValid;
+
+        double X = pts4D.at<float>(0, i) / w;
+        double Y = pts4D.at<float>(1, i) / w;
+        double Z = pts4D.at<float>(2, i) / w;
+
+        if (Z <= 0) continue;
+        double Z2 = R.at<double>(2,0)*X + R.at<double>(2,1)*Y
+                  + R.at<double>(2,2)*Z + t.reshape(1,3).at<double>(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<double> 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

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 <fstream>
+
+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<Feature2D>& detector,
+                       const Ptr<DescriptorMatcher>& 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<Matx44d>& 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<KeyPoint>& qKp, const Mat& qDesc, Size qSz,
+                     const std::vector<KeyPoint>& tKp, const Mat& tDesc, Size tSz,
+                     std::vector<DMatch>& 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<Feature2D> detector;
+    Ptr<DescriptorMatcher> 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<String> 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

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<double>(i,j);
+        T(i,3) = td.at<double>(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

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<double>(0,0)*X + P.at<double>(0,1)*Y + P.at<double>(0,2)*Z + P.at<double>(0,3);
+    double v = P.at<double>(1,0)*X + P.at<double>(1,1)*Y + P.at<double>(1,2)*Z + P.at<double>(1,3);
+    double w = P.at<double>(2,0)*X + P.at<double>(2,1)*Y + P.at<double>(2,2)*Z + P.at<double>(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<DMatch> 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<double>(i,j) = lastKf->poseCw(i,j);
+            Rt2.at<double>(i,j) = cur.poseCw(i,j);
+        }
+    Mat P1 = K * Rt1;
+    Mat P2 = K * Rt2;
+
+    std::vector<Point2f> pts1, pts2;
+    std::vector<int> 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<float>(3, i);
+        if (std::abs(w) < 1e-9) continue;
+        double X = pts4D.at<float>(0, i) / w;
+        double Y = pts4D.at<float>(1, i) / w;
+        double Z = pts4D.at<float>(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

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<double>(0,0) * Xc / Zc + K.at<double>(0,2);
+    v = K.at<double>(1,1) * Yc / Zc + K.at<double>(1,2);
+    return true;
+}
+
+inline double descDist(const Mat& a, const Mat& b)
+{
+    if (a.empty() || b.empty()) return std::numeric_limits<double>::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<MapPoint*>& 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<Point3f>& obj, const std::vector<Point2f>& 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<Point3f> objIn;
+    std::vector<Point2f> imgIn;
+    objIn.reserve(inlierIdx.rows); imgIn.reserve(inlierIdx.rows);
+    for (int i = 0; i < inlierIdx.rows; ++i)
+    {
+        int idx = inlierIdx.at<int>(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<MapPoint*> 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<double>::max();
+            size_t bestI = std::numeric_limits<size_t>::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<size_t>::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<Point3f> obj; std::vector<Point2f> 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<DMatch> matches;
+    matchFrames(lastKf->keypoints, lastKf->descriptors, lastKf->imageSize,
+                cur.keypoints, cur.descriptors, cur.imageSize, matches);
+
+    std::vector<Point3f> obj;
+    std::vector<Point2f> img;
+    std::vector<MapPoint*> corrMps;
+    std::vector<int> 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<Point2f> prevPts(prev.undistKpts.begin(), prev.undistKpts.end());
+    std::vector<Point2f> curPts;
+    std::vector<uchar> status;
+    std::vector<float> err;
+
+    calcOpticalFlowPyrLK(prev.image, cur.image, prevPts, curPts,
+                         status, err, Size(21, 21), 3,
+                         TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, 30, 0.01));
+
+    std::vector<Point3f> obj; std::vector<Point2f> 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<MapPoint*> 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<MapPoint*> 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<double>::max();
+        size_t bestI = std::numeric_limits<size_t>::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<size_t>::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

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<double>(0, 0);
+    const double fy = K.at<double>(1, 1);
+    const double cx = K.at<double>(0, 2);
+    const double cy = K.at<double>(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<double>(frame.undistKpts[i].x);
+        const double dy = v - static_cast<double>(frame.undistKpts[i].y);
+
+        if (std::sqrt(dx * dx + dy * dy) <= reprojThresh)
+        {
+            frame.outliers[i] = false;
+            ++nInliers;
+        }
+    }
+    return nInliers;
+}
+
+}} // namespace cv::slam

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

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 <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/features.hpp>
-#include <opencv2/3d.hpp>
 #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 <algorithm>
 #include <cmath>
-#include <fstream>
+#include <cstdint>
+#include <limits>
 #include <map>
-#include <numeric>
+#include <mutex>
 #include <set>
-#include <sstream>
-#include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
-#endif // __OPENCV_SLAM_PRECOMP_HPP__
+#endif // OPENCV_SLAM_PRECOMP_HPP

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")

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

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 <opencv2/ts.hpp>
-#include <opencv2/slam.hpp>
-
-#endif // __OPENCV_TEST_SLAM_PRECOMP_HPP__
+#endif

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<KeyPoint> keypoints; Mat descriptors; };
+    std::vector<FrameFeatures> 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<KeyPoint>& 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<Point3f> makeCloud()
+{
+    RNG& rng = theRNG();   // seeded per-test by the ts framework -> reproducible
+    std::vector<Point3f> 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<Point3f>& 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<Point2f> 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<int> 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<slam::VisualOdometry> makeOdometry(int nFrames,
+                                              const slam::OdometryParams& params = slam::OdometryParams())
 {
-    cv::slam::Map map;
+    std::vector<Point3f> cloud = makeCloud();
+    Ptr<StubDetector> detector = makePtr<StubDetector>();
+    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<DescriptorMatcher> 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<Feature2D> detector = makePtr<StubDetector>();
+    Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_L2, true);
+    Mat K(cameraMatrix());
+
+    EXPECT_THROW(slam::VisualOdometry::create(Ptr<Feature2D>(), matcher, "", "", K),
+                 cv::Exception);                                  // null detector
+    EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr<DescriptorMatcher>(), "", "", 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<slam::VisualOdometry> 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<double> 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<slam::VisualOdometry> 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<slam::VisualOdometry> 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<slam::VisualOdometry> 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

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)

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()

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 <opencv2/slam.hpp>
+#include <opencv2/features.hpp>
+#include <opencv2/dnn.hpp>
+#include <opencv2/core.hpp>
+#include <iostream>
+
+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<double> DIST = { -0.2811, 0.0723, -0.0003, 0.0001, 0.0 };
+
+static Ptr<Feature2D> makeDetector()
+{
+    ALIKED::Params p;
+    p.inputSize = Size(640, 640);
+    p.engine    = dnn::ENGINE_NEW;
+    return ALIKED::create(ALIKED_MODEL, p);
+}
+
+static Ptr<DescriptorMatcher> 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;
+}

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()