Merge pull request #29331 from agrim-rai/slamVO

[GSOC] Added Visual Odometry (VO) for SLAM

cmake/OpenCVModule.cmake

@@ -832,6 +832,8 @@ macro(ocv_glob_module_sources)
        "${CMAKE_CURRENT_LIST_DIR}/include/opencv2/${name}/hal/*.h"
        "${CMAKE_CURRENT_LIST_DIR}/include/opencv2/${name}/utils/*.hpp"
        "${CMAKE_CURRENT_LIST_DIR}/include/opencv2/${name}/utils/*.h"
+       "${CMAKE_CURRENT_LIST_DIR}/include/opencv2/${name}/slam/*.hpp"
+       "${CMAKE_CURRENT_LIST_DIR}/include/opencv2/${name}/slam/*.h"
        "${CMAKE_CURRENT_LIST_DIR}/include/opencv2/${name}/legacy/*.h"
   )
   file(GLOB lib_hdrs_detail

modules/features/include/opencv2/features.hpp

@@ -998,6 +998,19 @@ public:
      */
     CV_WRAP virtual bool isMaskSupported() const = 0;
 
+    /** @brief Provides keypoint and image-size context for matchers that need it (e.g. LightGlueMatcher).
+
+    Must be called before match()/knnMatch()/radiusMatch() for matchers that require this context.
+    Matchers that don't need it (e.g. BFMatcher, FlannBasedMatcher) ignore the call.
+
+    @param queryKpts Query image keypoints.
+    @param trainKpts Train image keypoints.
+    @param queryImageSize Size of the query image (width, height).
+    @param trainImageSize Size of the train image (width, height).
+    */
+    CV_WRAP virtual void setImagePairInfo(const std::vector<KeyPoint>& queryKpts, const std::vector<KeyPoint>& trainKpts,
+                                          Size queryImageSize = Size(), Size trainImageSize = Size());
+
     /** @brief Trains a descriptor matcher
 
     Trains a descriptor matcher (for example, the flann index). In all methods to match, the method
@@ -1349,6 +1362,10 @@ public:
     /** @brief Clears stored pair context information.
     */
     CV_WRAP virtual void clearPairInfo() = 0;
+
+    /** @brief Convenience overload of setPairInfo() taking keypoints directly. */
+    CV_WRAP void setImagePairInfo(const std::vector<KeyPoint>& queryKpts, const std::vector<KeyPoint>& trainKpts,
+                                  Size queryImageSize = Size(), Size trainImageSize = Size()) CV_OVERRIDE;
 };
 
 //! @} features_match

modules/features/src/matchers.cpp

@@ -580,6 +580,10 @@ bool DescriptorMatcher::empty() const
 void DescriptorMatcher::train()
 {}
 
+void DescriptorMatcher::setImagePairInfo( const std::vector<KeyPoint>&, const std::vector<KeyPoint>&,
+                                          Size, Size )
+{}
+
 void DescriptorMatcher::match( InputArray queryDescriptors, InputArray trainDescriptors,
                               std::vector<DMatch>& matches, InputArray mask ) const
 {

modules/features/src/matchers_lightglue.cpp

@@ -15,6 +15,26 @@ namespace cv
 LightGlueMatcher::LightGlueMatcher() {}
 LightGlueMatcher::~LightGlueMatcher() {}
 
+void LightGlueMatcher::setImagePairInfo(const std::vector<KeyPoint>& queryKpts, const std::vector<KeyPoint>& trainKpts,
+                                        Size queryImageSize, Size trainImageSize)
+{
+    Mat qk((int)queryKpts.size(), 2, CV_32F);
+    for (size_t i = 0; i < queryKpts.size(); ++i)
+    {
+        qk.at<float>((int)i, 0) = queryKpts[i].pt.x;
+        qk.at<float>((int)i, 1) = queryKpts[i].pt.y;
+    }
+
+    Mat tk((int)trainKpts.size(), 2, CV_32F);
+    for (size_t i = 0; i < trainKpts.size(); ++i)
+    {
+        tk.at<float>((int)i, 0) = trainKpts[i].pt.x;
+        tk.at<float>((int)i, 1) = trainKpts[i].pt.y;
+    }
+
+    setPairInfo(qk, tk, queryImageSize, trainImageSize);
+}
+
 #ifdef HAVE_OPENCV_DNN
 
 struct LightGluePairContext

modules/ptcloud/CMakeLists.txt

@@ -4,8 +4,8 @@ set(debug_modules "")
 if(DEBUG_opencv_ptcloud)
   list(APPEND debug_modules opencv_highgui)
 endif()
-ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann ${debug_modules}
-    WRAP java objc python js
+ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_features opencv_video ${debug_modules}
+  WRAP java objc python js
 )
 ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES})
 

modules/ptcloud/include/opencv2/ptcloud.hpp

@@ -11,6 +11,7 @@
 #include "opencv2/ptcloud/odometry.hpp"
 #include "opencv2/ptcloud/odometry_frame.hpp"
 #include "opencv2/ptcloud/odometry_settings.hpp"
+#include "opencv2/ptcloud/slam.hpp"
 
 /**
 @defgroup ptcloud Point Cloud Processing

modules/ptcloud/include/opencv2/ptcloud/slam.hpp

@@ -0,0 +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_PTCLOUD_SLAM_HPP
+#define OPENCV_PTCLOUD_SLAM_HPP
+
+/**
+@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/ptcloud/slam/types.hpp"
+#include "opencv2/ptcloud/slam/map.hpp"
+#include "opencv2/ptcloud/slam/odometry_params.hpp"
+#include "opencv2/ptcloud/slam/visual_odometry.hpp"
+
+#endif // OPENCV_PTCLOUD_SLAM_HPP

modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp

@@ -0,0 +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 "opencv2/ptcloud/slam/types.hpp"
+
+#include <set>
+#include <vector>
+
+namespace cv {
+namespace slam {
+
+//! @addtogroup slam
+//! @{
+
+/** @brief Owns all persistent SLAM state (KeyFrames and MapPoints).
+Pointers from addKeyframe / addMapPoint are valid until removeMapPoint / clear. */
+class CV_EXPORTS Map
+{
+public:
+    Map();
+    ~Map();
+
+    Map(const Map&) = delete;
+    Map& operator=(const Map&) = delete;
+
+    // Keyframes
+
+    KeyFrame* addKeyframe(KeyFrame* kf); //!< takes ownership; assigns id if < 0
+    KeyFrame* getKeyframe(int id) const;
+
+    const std::set<KeyFrame*>& keyframes() const;
+    int numKeyframes() const;
+
+    // Map points
+
+    MapPoint* addMapPoint(MapPoint* mp); //!< takes ownership; assigns id if < 0
+    MapPoint* getMapPoint(int id) const;
+
+    const std::set<MapPoint*>& mapPoints() const;
+    int numMapPoints() const;
+
+    void addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp);
+    void removeObservation(KeyFrame* kf, MapPoint* mp);
+    void removeMapPoint(MapPoint* mp);
+
+    // Reference / current keyframes
+
+    void setRefKeyframe(KeyFrame* kf);
+    KeyFrame* getRefKeyframe() const;
+
+    void setCurrentKeyframe(KeyFrame* kf);
+    KeyFrame* getCurrentKeyframe() const;
+
+    // Trajectory
+
+    void appendPose(const Matx44d& T_cw);
+    const std::vector<Matx44d>& trajectory() const;
+
+    // Lifecycle
+
+    void clear();
+
+private:
+    struct Impl;
+    Ptr<Impl> impl;
+};
+
+//! @}
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_MAP_HPP

modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp

@@ -0,0 +1,65 @@
+// 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"
+
+namespace cv {
+namespace slam {
+
+//! @addtogroup slam
+//! @{
+
+/** @brief Tunable parameters for visual odometry: initialization, tracking, keyframe selection, and local-map refinement. */
+struct CV_EXPORTS_W_SIMPLE OdometryParams
+{
+    CV_WRAP OdometryParams() {}
+
+    // Bootstrap (two-view map initialization)
+    CV_PROP_RW int minInitInliers = 40;             //!< Minimum match/inlier count at each bootstrap stage.
+    CV_PROP_RW double minInitParallaxDeg = 1.5;     //!< Minimum parallax (deg) to trigger initialization.
+    CV_PROP_RW int minInitPoints = 50;              //!< Minimum triangulated points to seed the map.
+    CV_PROP_RW double hfRatioThresh = 0.45;         //!< Homography/fundamental score ratio above which homography is chosen.
+    CV_PROP_RW double minGrowthParallaxDeg = 0.1;   //!< Minimum parallax (deg) to triangulate new points during map growth.
+    CV_PROP_RW double essentialRansacThresh = 1.0;  //!< RANSAC reprojection threshold (px) for essential-matrix/homography estimation.
+    CV_PROP_RW double essentialRansacConfidence = 0.999; //!< RANSAC confidence for essential-matrix estimation.
+
+    // Tracking (PnP)
+    CV_PROP_RW double pnpReprojThresh = 4.0;        //!< PnP RANSAC reprojection threshold (px).
+    CV_PROP_RW int pnpMinInliers = 6;               //!< Minimum PnP inliers to accept a pose.
+    CV_PROP_RW int pnpRansacIters = 500;            //!< Maximum PnP RANSAC iterations.
+    CV_PROP_RW double pnpConfidence = 0.99;         //!< PnP RANSAC confidence.
+
+    // Motion model (guided match search)
+    CV_PROP_RW double motionModelRadius = 15.0;     //!< Guided-match search radius (px).
+    CV_PROP_RW double motionModelRadiusWide = 30.0; //!< Wider fallback search radius (px) when the narrow search finds too few.
+    CV_PROP_RW int motionModelMinMatches = 20;      //!< Matches below which the wider search runs.
+    CV_PROP_RW double descProjThresh = 1.0;         //!< Descriptor-distance cutoff for a projected map-point match.
+
+    // Optical flow fallback
+    CV_PROP_RW int opticalFlowMinInliers = 10;      //!< Minimum correspondences for the optical-flow fallback.
+
+    // Keyframe promotion
+    CV_PROP_RW int kfMinFrames = 1;                 //!< Minimum frames since last keyframe before inserting one.
+    CV_PROP_RW int kfMaxFrames = 30;                //!< Frames since last keyframe after which one is forced.
+    CV_PROP_RW double kfInlierRatio = 0.70;         //!< Insert a keyframe when inliers drop below this fraction of the last keyframe's.
+    CV_PROP_RW int kfMinInliers = 40;               //!< Absolute inlier floor: max(kfMinInliers, kfInlierRatio * lastKfInliers).
+    CV_PROP_RW double kfRotThreshDeg = 5.0;         //!< Rotation (deg) from last keyframe that forces a new one.
+    CV_PROP_RW double kfTransThresh = 0.5;          //!< Translation from last keyframe that forces a new one.
+
+    // Local map refinement
+    CV_PROP_RW int localMapTopK = 10;               //!< Top co-visible keyframes forming the local map.
+    CV_PROP_RW int localMapNeighborK = 5;           //!< Covisibility neighbors expanded per local-map keyframe.
+    CV_PROP_RW double localMapRadius = 7.0;         //!< Reprojection search radius (px) for local-map points.
+};
+
+//! @}
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP

modules/ptcloud/include/opencv2/ptcloud/slam/types.hpp

@@ -0,0 +1,71 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+// 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/types.hpp"
+
+#include <map>
+#include <vector>
+
+namespace cv {
+namespace slam {
+
+//! @addtogroup slam
+//! @{
+
+// Pipeline lifecycle state
+enum OdometryState
+{
+    NOT_INITIALIZED = 0,
+    INITIALIZING = 1,
+    TRACKING = 2
+};
+
+struct MapPoint;
+struct KeyFrame;
+
+// 3D landmark, owned by Map
+struct CV_EXPORTS MapPoint
+{
+    int id = -1;
+    Point3d pos { 0, 0, 0 };
+    Mat refDesc;
+
+    std::map<KeyFrame*, size_t> observations; // kf -> keypoint index
+
+    int visibleCount = 0;
+    int foundCount = 0;
+    bool bad = false; // soft-delete; check before use
+};
+
+// Keyframe: pose + keypoints + covisibility graph, owned by Map
+struct CV_EXPORTS KeyFrame
+{
+    int id = -1;
+    Matx44d poseCw = Matx44d::eye(); // world->camera
+
+    std::vector<KeyPoint> keypoints;
+    Mat descriptors;
+    std::vector<Point2f> undistKpts; // parallel to keypoints
+    Size imageSize;
+
+    std::vector<MapPoint*> mapPoints; // parallel to keypoints; null = unmatched
+
+    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/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.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_VISUAL_ODOMETRY_HPP
+#define OPENCV_SLAM_VISUAL_ODOMETRY_HPP
+
+#include "opencv2/core.hpp"
+#include "opencv2/features.hpp"
+
+#include "opencv2/ptcloud/slam/types.hpp"
+#include "opencv2/ptcloud/slam/map.hpp"
+#include "opencv2/ptcloud/slam/odometry_params.hpp"
+
+#include <vector>
+
+namespace cv {
+namespace slam {
+
+//! @addtogroup slam
+//! @{
+
+/** @brief Monocular visual odometry pipeline.
+
+State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING
+(per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING.
+
+Purely in-memory: feed images with @ref processFrame and read back the trajectory
+and map with @ref getTrajectory / @ref getMap. See samples/slam for driving this
+from a directory of images and writing the result to disk (e.g. COLMAP format).
+*/
+class CV_EXPORTS_W VisualOdometry
+{
+public:
+    virtual ~VisualOdometry();
+
+    CV_WRAP static Ptr<VisualOdometry> create(
+        const Ptr<Feature2D>& detector,
+        const Ptr<DescriptorMatcher>& matcher,
+        InputArray cameraMatrix,
+        InputArray distCoeffs = noArray(),
+        const OdometryParams& params = OdometryParams());
+
+    /** @brief Feed one image. Returns true if a pose was emitted. */
+    CV_WRAP virtual bool processFrame(InputArray image) = 0;
+
+    /** @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;
+
+    //! @note Not exposed to Python: Map holds raw pointers / non-convertible containers.
+    virtual const Map& getMap() const = 0;
+
+    //! @brief Number of keyframes in the map. Convenience for callers (e.g. Python) that
+    //! can't use @ref getMap directly.
+    CV_WRAP virtual int getNumKeyframes() const = 0;
+
+    //! @brief Number of map points in the map. Convenience for callers (e.g. Python) that
+    //! can't use @ref getMap directly.
+    CV_WRAP virtual int getNumMapPoints() const = 0;
+
+    CV_WRAP virtual const std::vector<Matx44d>& getTrajectory() const = 0;
+
+    CV_WRAP virtual const OdometryParams& getParams() const = 0;
+    CV_WRAP virtual void setParams(const OdometryParams& params) = 0;
+
+protected:
+    VisualOdometry();
+};
+
+//! @}
+
+}} // namespace cv::slam
+
+#endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP

modules/ptcloud/src/slam/map.cpp

@@ -0,0 +1,148 @@
+// 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 <set>
+#include <unordered_map>
+
+namespace cv {
+namespace slam {
+
+struct Map::Impl
+{
+    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;
+
+    int nextKfId = 0;
+    int nextMpId = 0;
+};
+
+Map::Map() : impl(makePtr<Impl>()) {}
+
+Map::~Map()
+{
+    for (KeyFrame* kf : impl->keyframes) delete kf;
+    for (MapPoint* mp : impl->mapPoints) delete mp;
+}
+
+// Keyframes
+
+KeyFrame* Map::addKeyframe(KeyFrame* kf)
+{
+    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) const
+{
+    auto it = impl->kfIndex.find(id);
+    return (it != impl->kfIndex.end()) ? it->second : nullptr;
+}
+
+const std::set<KeyFrame*>& Map::keyframes() const { return impl->keyframes; }
+int Map::numKeyframes() const { return (int)impl->keyframes.size(); }
+
+// Map points
+
+MapPoint* Map::addMapPoint(MapPoint* mp)
+{
+    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) const
+{
+    auto it = impl->mpIndex.find(id);
+    return (it != impl->mpIndex.end()) ? it->second : nullptr;
+}
+
+const std::set<MapPoint*>& Map::mapPoints() const { return impl->mapPoints; }
+int Map::numMapPoints() const { return (int)impl->mapPoints.size(); }
+
+// Observations
+
+void Map::addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp)
+{
+    CV_Assert(kf && mp);
+    CV_Assert(kpIdx < kf->mapPoints.size());
+    if (kf->mapPoints[kpIdx] != nullptr) return;
+    kf->mapPoints[kpIdx] = mp;
+    mp->observations[kf] = kpIdx;
+}
+
+void Map::removeObservation(KeyFrame* kf, MapPoint* mp)
+{
+    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::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;
+}
+
+// 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->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/ptcloud/src/slam/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/ptcloud/src/slam/odometry/visual_odometry.cpp

@@ -0,0 +1,146 @@
+// 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 {
+
+// Factory
+
+VisualOdometry::VisualOdometry() = default;
+VisualOdometry::~VisualOdometry() = default;
+
+Ptr<VisualOdometry> VisualOdometry::create(
+    const Ptr<Feature2D>& detector,
+    const Ptr<DescriptorMatcher>& matcher,
+    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, K, dist, params);
+}
+
+// Constructor
+
+VisualOdometryImpl::VisualOdometryImpl(
+    const Ptr<Feature2D>& detector_,
+    const Ptr<DescriptorMatcher>& matcher_,
+    const Mat& cameraMatrix,
+    const Mat& distCoeffs,
+    const OdometryParams& params_)
+    : detector(detector_), matcher(matcher_), params(params_)
+{
+    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();
+    map.clear();
+}
+
+bool VisualOdometryImpl::processFrame(InputArray image)
+{
+    CV_INSTRUMENT_REGION();
+
+    if (image.empty()) return false;
+    lastEvent.clear();
+
+    Frame currentFrame;
+    extractFeatures(image, currentFrame);
+    if (currentFrame.keypoints.empty() || currentFrame.descriptors.empty()) return false;
+
+    currentFrame.mapPoints.assign(currentFrame.keypoints.size(), nullptr);
+    currentFrame.outliers.assign(currentFrame.keypoints.size(), false);
+    currentFrame.buildGrid();
+
+    switch (state)
+    {
+    case NOT_INITIALIZED:
+        refFrame = currentFrame;
+        state = INITIALIZING;
+        return false;
+
+    case INITIALIZING:
+        return bootstrap(currentFrame);
+
+    case TRACKING:
+        return track(currentFrame);
+    }
+    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;
+
+    matcher->setImagePairInfo(qKp, tKp, qSz, tSz);
+    matcher->match(qDesc, tDesc, matches);
+}
+
+}} // namespace cv::slam

modules/ptcloud/src/slam/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/ptcloud/src/slam/odometry/vo_impl.hpp

@@ -0,0 +1,110 @@
+// 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"
+
+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, processFrame()
+*/
+class VisualOdometryImpl CV_FINAL : public VisualOdometry
+{
+public:
+    VisualOdometryImpl(const Ptr<Feature2D>& detector,
+                       const Ptr<DescriptorMatcher>& matcher,
+                       const Mat& cameraMatrix,
+                       const Mat& distCoeffs,
+                       const OdometryParams& params);
+
+    // --- VisualOdometry interface -------------------------------------------
+
+    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; }
+    int getNumKeyframes() const CV_OVERRIDE { return map.numKeyframes(); }
+    int getNumMapPoints() const CV_OVERRIDE { return map.numMapPoints(); }
+    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; }
+
+    // --- 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;
+
+    // --- Owned state ---------------------------------------------------------
+
+    Ptr<Feature2D> detector;
+    Ptr<DescriptorMatcher> matcher;
+    Mat K;    // 3×3 CV_64F
+    Mat dist; // distortion coefficients (may be empty)
+    OdometryParams params;
+
+    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;
+
+    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/ptcloud/src/slam/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 transDist = std::sqrt(d.dot(d));
+    if (transDist > params.kfTransThresh)
+    {
+        reason = format("trans=%.3f", transDist);
+        return true;
+    }
+
+    return false;
+}
+
+}} // namespace cv::slam

modules/ptcloud/src/slam/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/ptcloud/src/slam/odometry/vo_tracking.cpp

@@ -0,0 +1,401 @@
+// 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.keypoints.empty()) return false;
+
+    // LK runs on the raw imgs
+    std::vector<Point2f> prevPts;
+    prevPts.reserve(prev.keypoints.size());
+    for (const auto& kp : prev.keypoints) prevPts.push_back(kp.pt);
+
+    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));
+
+    // PnP uses K with no distortion
+    std::vector<Point2f> curUndist;
+    if (!dist.empty())
+        undistortPoints(curPts, curUndist, K, dist, noArray(), K);
+    else
+        curUndist = curPts;
+
+    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(curUndist[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/ptcloud/src/slam/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/ptcloud/src/slam/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/ptcloud/src/slam/precomp.hpp

@@ -0,0 +1,29 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+// 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
+
+#include "opencv2/ptcloud/slam.hpp"
+
+#include "opencv2/core.hpp"
+#include "opencv2/core/private.hpp"
+#include "opencv2/imgproc.hpp"
+#include "opencv2/features.hpp"
+#include "opencv2/geometry.hpp"
+#include "opencv2/video/tracking.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <limits>
+#include <map>
+#include <set>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#endif // OPENCV_SLAM_PRECOMP_HPP

modules/ptcloud/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/ptcloud/test/test_precomp.hpp

@@ -13,6 +13,8 @@
 #include <opencv2/core/utils/logger.hpp>
 #include "opencv2/ptcloud/depth.hpp"
 #include "opencv2/ptcloud/odometry.hpp"
+#include "opencv2/ptcloud/slam.hpp"
+#include "opencv2/features.hpp"
 
 #ifdef HAVE_OPENCL
 #include <opencv2/core/ocl.hpp>

modules/ptcloud/test/test_visual_odometry.cpp

@@ -0,0 +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 {
+
+// 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.
+
+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()
+{
+    return Matx33d(500, 0, 320,
+                   0, 500, 240,
+                   0,   0,   1);
+}
+
+// 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)
+{
+    return Mat(1, descDim, CV_32F, Scalar((double)(id + 1)));
+}
+
+// Stub Feature2D: replays pre-computed keypoints/descriptors, one frame per
+// detectAndCompute() call (processFrame() invokes the detector exactly once).
+class StubDetector CV_FINAL : public Feature2D
+{
+public:
+    struct FrameFeatures { std::vector<KeyPoint> keypoints; Mat descriptors; };
+    std::vector<FrameFeatures> frames;
+    size_t next = 0;
+
+    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;
+    }
+};
+
+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;
+}
+
+// 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)
+{
+    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);
+
+    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;
+}
+
+// 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())
+{
+    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]));
+
+    Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_L2, true);
+    return slam::VisualOdometry::create(detector, matcher,
+                                        Mat(cameraMatrix()), noArray(), params);
+}
+
+static Mat blankImage() { return Mat::zeros(imageSize, CV_8UC1); }
+
+// Rotation magnitude (deg) of @p T's rotation block relative to identity.
+static double rotationFromIdentityDeg(const Matx44d& T)
+{
+    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)
 # HACK: CMake 4.x finds and links wrong OpenCL in 32-bit builds on Windows x64
@@ -127,6 +128,7 @@ if(WIN32)
   add_subdirectory(directx)
 endif()
 add_subdirectory(dnn)
+add_subdirectory(slam)
 # add_subdirectory(gpu)
 # HACK: CMake 4.x finds and links wrong OpenCL in 32-bit builds on Windows x64
 if(NOT (WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 4))

samples/slam/CMakeLists.txt

@@ -0,0 +1,23 @@
+ocv_install_example_src(slam *.cpp *.hpp CMakeLists.txt)
+
+set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS
+  opencv_core
+  opencv_dnn
+  opencv_features
+  opencv_imgcodecs
+  opencv_ptcloud)
+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,320 @@
+// 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.
+
+#include <opencv2/ptcloud.hpp>
+#include <opencv2/features.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/core/quaternion.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <algorithm>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <sstream>
+
+#include "../dnn/common.hpp"
+
+using namespace cv;
+using namespace std;
+
+namespace {
+
+// path helper: strips the directory part off a path
+String basenameOf(const String& path)
+{
+    const size_t slash = path.find_last_of("/\\");
+    return (slash == String::npos) ? path : path.substr(slash + 1);
+}
+
+// reader: lists readable image files under a directory, sorted by filename
+std::vector<String> listImageFiles(const String& imagesDir)
+{
+    std::vector<String> allFiles;
+    glob(imagesDir, allFiles, false);
+    std::vector<String> imgFiles;
+    imgFiles.reserve(allFiles.size());
+    for (const auto& f : allFiles)
+        if (haveImageReader(f)) imgFiles.push_back(f);
+    std::sort(imgFiles.begin(), imgFiles.end());
+    return imgFiles;
+}
+
+// bookkeeping: maps each pose in vo->getTrajectory() back to its source image filename.
+//
+// processFrame() doesn't report which frame a pose came from, so this infers it: most frames
+// add one pose, but the frame where two-view initialization completes adds two at once (the
+// reference frame's pose, remembered when initialization started, plus the current frame's).
+class PoseFilenameTracker
+{
+public:
+    void update(slam::OdometryState before, slam::OdometryState after,
+                size_t trajectorySize, const String& filename)
+    {
+        if (before == slam::NOT_INITIALIZED ||
+            (before == slam::TRACKING && after == slam::INITIALIZING))
+            refFilename_ = filename;
+
+        const size_t added = trajectorySize - prevTrajectorySize_;
+        if (added == 1)
+            filenames_.push_back(filename);
+        else if (added == 2)
+        {
+            filenames_.push_back(refFilename_);
+            filenames_.push_back(filename);
+        }
+        prevTrajectorySize_ = trajectorySize;
+    }
+
+    const std::vector<String>& filenames() const { return filenames_; }
+
+private:
+    std::vector<String> filenames_;
+    size_t prevTrajectorySize_ = 0;
+    String refFilename_;
+};
+
+// writer: dumps camera.txt, point3d.txt and images.txt (COLMAP text format) for @p vo into
+// @p outputFolder; @p poseFilenames[i] is the source image for trajectory pose i
+void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K,
+                      const std::vector<String>& poseFilenames,
+                      const String& outputFolder)
+{
+    cv::utils::fs::createDirectories(outputFolder);
+
+    {
+        std::ofstream f(cv::utils::fs::join(outputFolder, "camera.txt").c_str());
+        int width = 0, height = 0;
+        if (!vo.getMap().keyframes().empty())
+        {
+            const slam::KeyFrame* kf = *vo.getMap().keyframes().begin();
+            width  = kf->imageSize.width;
+            height = kf->imageSize.height;
+        }
+        f.setf(std::ios::fixed); f.precision(4);
+        f << "fx " << K.at<double>(0, 0) << "\n"
+          << "fy " << K.at<double>(1, 1) << "\n"
+          << "cx " << K.at<double>(0, 2) << "\n"
+          << "cy " << K.at<double>(1, 2) << "\n"
+          << "width "  << width  << "\n"
+          << "height " << height << "\n";
+    }
+
+    {
+        std::ofstream f(cv::utils::fs::join(outputFolder, "point3d.txt").c_str());
+        f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n";
+        f.setf(std::ios::scientific); f.precision(9);
+        for (slam::MapPoint* mp : vo.getMap().mapPoints())
+        {
+            if (!mp || mp->bad) continue;
+            f << mp->id << " "
+              << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " "
+              << mp->observations.size() << "\n";
+        }
+    }
+
+    {
+        std::ofstream f(cv::utils::fs::join(outputFolder, "images.txt").c_str());
+        const auto& traj = vo.getTrajectory();
+        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);
+
+            const Quatd q = Quatd::createFromRotMat(R);
+            const String name = (i < poseFilenames.size())
+                ? basenameOf(poseFilenames[i])
+                : (String("pose_") + std::to_string(i));
+
+            f << i << " " << q.w << " " << q.x << " " << q.y << " " << q.z << " "
+              << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n";
+        }
+    }
+}
+
+} // namespace
+
+const string about =
+    "Monocular visual odometry using ALIKED + LightGlue.\n\n"
+    "Runs feature detection (ALIKED) and matching (LightGlue) from ONNX models over a\n"
+    "directory of images, estimates the camera trajectory and writes it to --output.\n"
+    "To run:\n"
+    "\t ./example_slam_visual_odometry --aliked=aliked.onnx --lightglue=lightglue.onnx --images=./seq\n"
+    "Sample command (run on the GPU):\n"
+    "\t ./example_slam_visual_odometry --aliked=aliked.onnx --lightglue=lightglue.onnx --images=./seq --target=cuda\n";
+
+const string param_keys =
+    "{ help h           |        | Print help message }"
+    "{ aliked           | <none> | Path to ALIKED ONNX model }"
+    "{ lightglue        | <none> | Path to LightGlue ONNX model }"
+    "{ images           | <none> | Path to directory with input images }"
+    "{ output           | vo_out | Output directory for trajectory and map }"
+    "{ fx               | 718.856  | Camera focal length X }"
+    "{ fy               | 718.856  | Camera focal length Y }"
+    "{ cx               | 607.1928 | Camera principal point X }"
+    "{ cy               | 185.2157 | Camera principal point Y }"
+    "{ dist             |        | Lens distortion coeffs, comma-separated k1,k2,p1,p2[,k3,...] (default: none) }";
+
+const string backend_keys = format(
+    "{ backend          | default | Choose one of computation backends: "
+                              "default: automatically (by default), "
+                              "openvino: Intel's Deep Learning Inference Engine (https://software.intel.com/openvino-toolkit), "
+                              "opencv: OpenCV implementation, "
+                              "vkcom: VKCOM, "
+                              "cuda: CUDA, "
+                              "webnn: WebNN }");
+
+const string target_keys = format(
+    "{ target           | cpu | Choose one of target computation devices: "
+                              "cpu: CPU target (by default), "
+                              "opencl: OpenCL, "
+                              "opencl_fp16: OpenCL fp16 (half-float precision), "
+                              "vpu: VPU, "
+                              "vulkan: Vulkan, "
+                              "cuda: CUDA, "
+                              "cuda_fp16: CUDA fp16 (half-float preprocess) }");
+
+string keys = param_keys + backend_keys + target_keys;
+
+int main(int argc, char** argv)
+{
+    CommandLineParser parser(argc, argv, keys);
+    parser.about(about);
+
+    if (parser.has("help"))
+    {
+        parser.printMessage();
+        return 0;
+    }
+
+    const String alikedPath = parser.get<String>("aliked");
+    const String lightgluePath = parser.get<String>("lightglue");
+    const String imagesDir = parser.get<String>("images");
+    const int backendId = getBackendID(parser.get<String>("backend"));
+    const int targetId = getTargetID(parser.get<String>("target"));
+
+    if (!parser.check() || alikedPath == "<none>" || lightgluePath == "<none>" || imagesDir == "<none>")
+    {
+        parser.printErrors();
+        parser.printMessage();
+        return 1;
+    }
+
+    const String outputDir = parser.get<String>("output");
+
+    const Matx33d K(parser.get<double>("fx"), 0., parser.get<double>("cx"),
+                    0., parser.get<double>("fy"), parser.get<double>("cy"),
+                    0., 0., 1.);
+
+    // Optional lens distortion (k1,k2,p1,p2[,k3,...]); empty means no distortion.
+    Mat distCoeffs;
+    {
+        std::stringstream ss(parser.get<String>("dist"));
+        std::vector<double> coeffs;
+        String tok;
+        while (std::getline(ss, tok, ','))
+        {
+            const size_t b = tok.find_first_not_of(" \t");
+            const size_t e = tok.find_last_not_of(" \t");
+            if (b == String::npos) continue;
+            coeffs.push_back(std::stod(tok.substr(b, e - b + 1)));
+        }
+        if (!coeffs.empty())
+            distCoeffs = Mat(coeffs, true).reshape(1, 1);
+    }
+
+    ALIKED::Params detParams;
+    detParams.inputSize = Size(640, 640);
+    detParams.engine    = dnn::ENGINE_NEW;
+    detParams.backend   = backendId;
+    detParams.target    = targetId;
+    auto detector = ALIKED::create(alikedPath, detParams);
+
+    auto matcher = LightGlueMatcher::create(lightgluePath, 0.0f,
+                                            backendId, targetId);
+
+    slam::OdometryParams voParams;
+
+    voParams.minInitParallaxDeg = 1.5;
+    voParams.minInitPoints      = 50;
+    voParams.pnpReprojThresh    = 4.0;
+    voParams.kfMaxFrames        = 30;
+    voParams.localMapTopK       = 10;
+
+    auto vo = slam::VisualOdometry::create(
+        detector, matcher, Mat(K), distCoeffs, voParams);
+
+    const std::vector<String> imgFiles = listImageFiles(imagesDir);
+
+    if (imgFiles.empty())
+    {
+        std::cerr << "no images found in " << imagesDir << "\n";
+        return 1;
+    }
+
+    std::cout << "images folder : " << imagesDir << "\n"
+              << "output folder : " << outputDir << "\n"
+              << "total frames  : " << imgFiles.size() << "\n\n";
+
+    PoseFilenameTracker poseTracker;
+    int nEmitted = 0;
+
+    const int nDigits = std::to_string(imgFiles.size()).size();
+    const int64 t0 = getTickCount();
+    for (size_t i = 0; i < imgFiles.size(); ++i)
+    {
+        const int64 tFrameStart = getTickCount();
+
+        Mat img = imread(imgFiles[i]);
+        if (img.empty())
+        {
+            std::cerr << "Frame " << std::setw(nDigits) << (i + 1) << "/" << imgFiles.size()
+                      << " failed to read: " << imgFiles[i] << "\n";
+            continue;
+        }
+
+        const slam::OdometryState before = vo->getState();
+        const bool emitted = vo->processFrame(img);
+        const slam::OdometryState after = vo->getState();
+        if (emitted) ++nEmitted;
+
+        poseTracker.update(before, after, vo->getTrajectory().size(), imgFiles[i]);
+
+        const double frameElapsed = (getTickCount() - tFrameStart) / getTickFrequency();
+        const double fps = frameElapsed > 0. ? 1. / frameElapsed : 0.;
+
+        std::cout << "Frame " << std::setw(nDigits) << (i + 1) << "/" << imgFiles.size()
+                  << " processed | fps: " << std::fixed << std::setprecision(1) << std::setw(5) << fps
+                  << " | emitted: " << (emitted ? "yes" : "no ")
+                  << " | keyframes: " << vo->getMap().numKeyframes()
+                  << " | map points: " << vo->getMap().numMapPoints() << "\n";
+    }
+    const double elapsed = (getTickCount() - t0) / getTickFrequency();
+    const double avgFps = elapsed > 0. ? imgFiles.size() / elapsed : 0.;
+    const bool ok = nEmitted > 0;
+
+    if (ok && !outputDir.empty())
+        writeColmapFiles(*vo, Mat(K), poseTracker.filenames(), outputDir);
+
+    std::cout << "\n"
+              << "==================== Visual Odometry Result ====================\n"
+              << "status        : " << (ok ? "OK" : "FAILED") << "\n"
+              << "total frames  : " << imgFiles.size() << "\n"
+              << "camera poses  : " << vo->getTrajectory().size() << "\n"
+              << "keyframes     : " << vo->getMap().numKeyframes() << "\n"
+              << "map points    : " << vo->getMap().numMapPoints() << "\n"
+              << "elapsed time  : " << std::fixed << std::setprecision(2) << elapsed << " s\n"
+              << "average fps   : " << std::fixed << std::setprecision(2) << avgFps << "\n"
+              << "output dir    : " << outputDir << "\n"
+              << "====================================================================\n";
+    return ok ? 0 : 1;
+}

samples/slam/visual_odometry.py

@@ -0,0 +1,192 @@
+'''
+Monocular visual odometry with cv.slam.VisualOdometry (ALIKED + LightGlue).
+
+Example:
+    python visual_odometry.py --aliked aliked.onnx --lightglue lg.onnx --images ./seq
+'''
+
+import argparse
+import glob
+import os
+import sys
+import time
+import numpy as np
+import cv2 as cv
+
+
+def build_K(fx, fy, cx, cy):
+    return np.array([[fx, 0., cx],
+                     [0., fy, cy],
+                     [0., 0., 1.]], dtype=np.float64)
+
+
+def list_image_files(images_dir):
+    files = [f for f in sorted(glob.glob(os.path.join(images_dir, '*')))
+             if cv.haveImageReader(f)]
+    return files
+
+
+def rotation_matrix_to_quaternion(R):
+    # Shepperd's method: numerically stable for all rotations.
+    trace = R[0, 0] + R[1, 1] + R[2, 2]
+    if trace > 0:
+        s = np.sqrt(trace + 1.0) * 2
+        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
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2
+        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
+    elif R[1, 1] > R[2, 2]:
+        s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2
+        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:
+        s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2
+        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
+    return qw, qx, qy, qz
+
+
+def write_colmap_files(vo, K, image_size, pose_filenames, output_dir):
+    # Note: cv.slam.Map isn't wrapped for Python (only aggregate counts are,
+    # via getNumKeyframes/getNumMapPoints), so only camera intrinsics and the
+    # trajectory (images.txt) can be exported here; point3d.txt (map points)
+    # requires the C++ sample.
+    os.makedirs(output_dir, exist_ok=True)
+
+    with open(os.path.join(output_dir, 'camera.txt'), 'w') as f:
+        f.write(f"fx {K[0, 0]:.4f}\n")
+        f.write(f"fy {K[1, 1]:.4f}\n")
+        f.write(f"cx {K[0, 2]:.4f}\n")
+        f.write(f"cy {K[1, 2]:.4f}\n")
+        f.write(f"width {image_size[0]}\n")
+        f.write(f"height {image_size[1]}\n")
+
+    traj = vo.getTrajectory()
+    with open(os.path.join(output_dir, 'images.txt'), 'w') as f:
+        f.write("# Image list with two lines of data per image:\n")
+        f.write("#   IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n")
+        f.write("#   POINTS2D[] as (X, Y, POINT3D_ID)\n")
+        f.write(f"# Number of images: {len(traj)}, mean observations per image: 0.0\n")
+        for i, T in enumerate(traj):
+            qw, qx, qy, qz = rotation_matrix_to_quaternion(T[:3, :3])
+            name = (os.path.basename(pose_filenames[i]) if i < len(pose_filenames)
+                    else f"pose_{i}")
+            f.write(f"{i} {qw:.6f} {qx:.6f} {qy:.6f} {qz:.6f} "
+                    f"{T[0,3]:.6f} {T[1,3]:.6f} {T[2,3]:.6f} 1 {name}\n")
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Monocular visual odometry using ALIKED + LightGlue')
+    parser.add_argument('--aliked',    required=True,
+                        help='Path to ALIKED ONNX model')
+    parser.add_argument('--lightglue', required=True,
+                        help='Path to LightGlue ONNX model')
+    parser.add_argument('--images',    required=True,
+                        help='Path to directory with input images')
+    parser.add_argument('--output',    default='vo_out',
+                        help='Output directory for trajectory and map (default: vo_out)')
+    parser.add_argument('--fx', type=float, default=718.856,
+                        help='Camera focal length X (default: KITTI-00)')
+    parser.add_argument('--fy', type=float, default=718.856,
+                        help='Camera focal length Y (default: KITTI-00)')
+    parser.add_argument('--cx', type=float, default=607.1928,
+                        help='Camera principal point X (default: KITTI-00)')
+    parser.add_argument('--cy', type=float, default=185.2157,
+                        help='Camera principal point Y (default: KITTI-00)')
+    parser.add_argument('--min-parallax', type=float, default=1.5,
+                        help='Minimum initialisation parallax in degrees (default: 1.5)')
+    parser.add_argument('--min-points', type=int, default=50,
+                        help='Minimum initialisation map points (default: 50)')
+    args = parser.parse_args()
+
+    det_params = cv.ALIKED.Params()
+    det_params.inputSize = (640, 640)
+    det_params.engine    = cv.dnn.ENGINE_NEW
+    detector = cv.ALIKED.create(args.aliked, det_params)
+
+    matcher = cv.LightGlueMatcher.create(
+        args.lightglue, 0.0,
+        cv.dnn.DNN_BACKEND_DEFAULT,
+        cv.dnn.DNN_TARGET_CPU)
+
+    vo_params = cv.slam.OdometryParams()
+    vo_params.minInitParallaxDeg = args.min_parallax
+    vo_params.minInitPoints      = args.min_points
+
+    K = build_K(args.fx, args.fy, args.cx, args.cy)
+
+    vo = cv.slam.VisualOdometry.create(
+        detector, matcher, K, np.array([]), vo_params)
+
+    image_files = list_image_files(args.images)
+    if not image_files:
+        print(f"no images found in {args.images}", file=sys.stderr)
+        return 1
+
+    print(f"images_folder = {args.images}")
+    print(f"output_folder = {args.output}")
+    print(f"found {len(image_files)} image(s)")
+
+    # Tracks which input image each emitted trajectory pose came from, for images.txt.
+    pose_filenames = []
+    prev_traj_len = 0
+    ref_filename = None
+    image_size = (0, 0)
+    n_emitted = 0
+
+    t0 = time.perf_counter()
+    for i, path in enumerate(image_files):
+        img = cv.imread(path)
+        if img is None:
+            print(f"[FRAME {i}] file={path} imread failed", file=sys.stderr)
+            continue
+        image_size = (img.shape[1], img.shape[0])
+
+        before = vo.getState()
+        emitted = vo.processFrame(img)
+        after = vo.getState()
+        if emitted:
+            n_emitted += 1
+
+        # Track which input image maps to each trajectory pose.
+        if before == cv.slam.NOT_INITIALIZED or \
+           (before == cv.slam.TRACKING and after == cv.slam.INITIALIZING):
+            ref_filename = path
+
+        traj_len = len(vo.getTrajectory())
+        added = traj_len - prev_traj_len
+        if added == 1:
+            pose_filenames.append(path)
+        elif added == 2:
+            pose_filenames.append(ref_filename)
+            pose_filenames.append(path)
+        prev_traj_len = traj_len
+
+        print(f"[FRAME {i}] file={path}"
+              f" emitted={'yes' if emitted else 'no'}"
+              f" keyframes={vo.getNumKeyframes()}"
+              f" map_points={vo.getNumMapPoints()}")
+    elapsed = time.perf_counter() - t0
+    ok = n_emitted > 0
+
+    if ok and args.output:
+        write_colmap_files(vo, K, image_size, pose_filenames, args.output)
+
+    print(f"run={'ok' if ok else 'FAILED'}  frames={len(vo.getTrajectory())}  elapsed={elapsed:.2f}s")
+    print(f"output -> {args.output}")
+    return 0 if ok else 1
+
+
+if __name__ == '__main__':
+    sys.exit(main())