From 72b3fea7ddef6d957d9938050daf1de83f665bfc Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Fri, 5 Jun 2026 18:00:13 +0530 Subject: [PATCH 01/10] slam-vo init --- modules/slam/CMakeLists.txt | 14 ++ modules/slam/include/opencv2/slam.hpp | 22 +++ modules/slam/include/opencv2/slam/map.hpp | 73 ++++++++++ .../include/opencv2/slam/odometry_params.hpp | 37 +++++ modules/slam/include/opencv2/slam/types.hpp | 71 +++++++++ .../include/opencv2/slam/visual_odometry.hpp | 102 +++++++++++++ modules/slam/perf/perf_main.cpp | 8 ++ modules/slam/perf/perf_precomp.hpp | 12 ++ modules/slam/src/map.cpp | 125 ++++++++++++++++ modules/slam/src/precomp.hpp | 28 ++++ modules/slam/src/types.cpp | 12 ++ modules/slam/test/test_precomp.hpp | 12 ++ modules/slam/test/test_visual_odometry.cpp | 136 ++++++++++++++++++ 13 files changed, 652 insertions(+) create mode 100644 modules/slam/CMakeLists.txt create mode 100644 modules/slam/include/opencv2/slam.hpp create mode 100644 modules/slam/include/opencv2/slam/map.hpp create mode 100644 modules/slam/include/opencv2/slam/odometry_params.hpp create mode 100644 modules/slam/include/opencv2/slam/types.hpp create mode 100644 modules/slam/include/opencv2/slam/visual_odometry.hpp create mode 100644 modules/slam/perf/perf_main.cpp create mode 100644 modules/slam/perf/perf_precomp.hpp create mode 100644 modules/slam/src/map.cpp create mode 100644 modules/slam/src/precomp.hpp create mode 100644 modules/slam/src/types.cpp create mode 100644 modules/slam/test/test_precomp.hpp create mode 100644 modules/slam/test/test_visual_odometry.cpp diff --git a/modules/slam/CMakeLists.txt b/modules/slam/CMakeLists.txt new file mode 100644 index 0000000000..91bcf7f368 --- /dev/null +++ b/modules/slam/CMakeLists.txt @@ -0,0 +1,14 @@ +# This file is part of OpenCV project. +# It is subject to the license terms in the LICENSE file found in the top-level directory +# of this distribution and at http://opencv.org/license.html. + +set(the_description "SLAM (Simultaneous Localization and Mapping) — visual odometry and mapping") + +ocv_define_module(slam + opencv_core + opencv_imgproc + opencv_imgcodecs + opencv_features + opencv_3d + OPTIONAL opencv_dnn + WRAP python) diff --git a/modules/slam/include/opencv2/slam.hpp b/modules/slam/include/opencv2/slam.hpp new file mode 100644 index 0000000000..2c4f84e058 --- /dev/null +++ b/modules/slam/include/opencv2/slam.hpp @@ -0,0 +1,22 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef OPENCV_SLAM_HPP +#define OPENCV_SLAM_HPP + +#include "slam/types.hpp" +#include "slam/map.hpp" +#include "slam/odometry_params.hpp" +#include "slam/visual_odometry.hpp" + +/** +@defgroup slam SLAM (Simultaneous Localization and Mapping) +@{ + @defgroup slam_odometry Visual Odometry + @brief Monocular visual odometry pipeline. +@} +*/ + +#endif // OPENCV_SLAM_HPP diff --git a/modules/slam/include/opencv2/slam/map.hpp b/modules/slam/include/opencv2/slam/map.hpp new file mode 100644 index 0000000000..2da8af1633 --- /dev/null +++ b/modules/slam/include/opencv2/slam/map.hpp @@ -0,0 +1,73 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef OPENCV_SLAM_MAP_HPP +#define OPENCV_SLAM_MAP_HPP + +#include "types.hpp" +#include +#include + +namespace cv { namespace slam { + +/** @brief Container for keyframes, map points, and the per-frame trajectory. + + Map owns all KeyFrame and MapPoint objects. External code holds only + raw non-owning pointers. Ids are assigned automatically when the + incoming id field is negative. + + @ingroup slam_odometry +*/ +class CV_EXPORTS_W Map +{ +public: + CV_WRAP Map(); + ~Map(); + + /** @brief Add a keyframe; assigns id if kf.id < 0. Returns the assigned id. */ + CV_WRAP int addKeyframe(KeyFrame& kf); + + /** @brief Return keyframe by id, or nullptr. */ + CV_WRAP KeyFrame* getKeyframe(int id); + + /** @brief All keyframes in insertion order. */ + CV_WRAP const std::vector& keyframes() const; + + CV_WRAP int numKeyframes() const; + + /** @brief Add a map point; assigns id if mp.id < 0. Returns the assigned id. */ + CV_WRAP int addMapPoint(MapPoint& mp); + + /** @brief Return map point by id, or nullptr. */ + CV_WRAP MapPoint* getMapPoint(int id); + + /** @brief All live (non-bad) map points. */ + CV_WRAP std::vector mapPoints() const; + + CV_WRAP int numMapPoints() const; + + /** @brief Wire a bidirectional KF-MP observation. */ + CV_WRAP void addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp); + + /** @brief Mark a map point bad and remove all cross-references. */ + CV_WRAP void removeMapPoint(int mp_id); + + /** @brief Append a world-to-camera pose to the trajectory. */ + CV_WRAP void appendPose(const Matx44d& T_cw); + + /** @brief All emitted world-to-camera poses in order. */ + CV_WRAP const std::vector& trajectory() const; + + /** @brief Reset to empty state. */ + CV_WRAP void clear(); + +private: + struct Impl; + Ptr impl_; +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_MAP_HPP diff --git a/modules/slam/include/opencv2/slam/odometry_params.hpp b/modules/slam/include/opencv2/slam/odometry_params.hpp new file mode 100644 index 0000000000..cff30cd48d --- /dev/null +++ b/modules/slam/include/opencv2/slam/odometry_params.hpp @@ -0,0 +1,37 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef OPENCV_SLAM_ODOMETRY_PARAMS_HPP +#define OPENCV_SLAM_ODOMETRY_PARAMS_HPP + +#include + +namespace cv { namespace slam { + +/** @brief Tunable parameters for the VisualOdometry pipeline. + + All fields have sensible defaults. Override any subset before + passing to VisualOdometry::create(). + + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE OdometryParams +{ + CV_WRAP OdometryParams() = default; + + // --- Bootstrap (two-view init) ------------------------------------------- + + CV_PROP_RW int min_init_inliers = 80; //!< Min essential-matrix RANSAC inliers. + CV_PROP_RW double min_init_parallax_deg = 3.0; //!< Min median parallax to trigger bootstrap. + CV_PROP_RW int min_init_points = 50; //!< Min triangulated points to accept init. + CV_PROP_RW double hf_ratio_thresh = 0.45; //!< H/(H+F) ratio; > thresh uses Homography. + CV_PROP_RW double min_growth_parallax_deg = 1.0; //!< Min parallax for a new map point to survive. + CV_PROP_RW double essential_ransac_thresh = 1.0; //!< RANSAC reprojection threshold (px). + CV_PROP_RW double essential_ransac_confidence = 0.999; //!< RANSAC confidence for findEssentialMat. +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP diff --git a/modules/slam/include/opencv2/slam/types.hpp b/modules/slam/include/opencv2/slam/types.hpp new file mode 100644 index 0000000000..4b54dbe7ee --- /dev/null +++ b/modules/slam/include/opencv2/slam/types.hpp @@ -0,0 +1,71 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef OPENCV_SLAM_TYPES_HPP +#define OPENCV_SLAM_TYPES_HPP + +#include +#include +#include + +namespace cv { namespace slam { + +/** @brief Current state of the VisualOdometry pipeline. + @ingroup slam_odometry +*/ +enum OdometryState +{ + NOT_INITIALIZED = 0, //!< No frames processed yet. + INITIALIZING = 1, //!< Reference frame set; waiting for bootstrap. + TRACKING = 2 //!< Map initialised; localising via PnP. +}; + +/** @brief Feature data for one image frame. + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE FrameFeatures +{ + std::vector keypoints; + Mat descriptors; + Size imageSize; +}; + +struct MapPoint; +struct KeyFrame; + +/** @brief A triangulated 3-D landmark owned by Map. + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE MapPoint +{ + int id = -1; + Point3d pos; + bool bad = false; + + std::map observations; //!< Observing keyframe → keypoint index. +}; + +/** @brief A frame whose pose has been committed to the map. + @ingroup slam_odometry +*/ +struct CV_EXPORTS_W_SIMPLE KeyFrame +{ + int id = -1; + Matx44d pose_cw; //!< World-to-camera transform (row-major 4x4). + + std::vector keypoints; + Mat descriptors; + std::vector undist_kpts; + Size imageSize; + + std::vector kpt_to_mp; //!< Map-point id per keypoint, -1 if unmatched. + std::vector mappoints; //!< Direct pointer per keypoint, nullptr if unmatched. + + std::vector> ordered_covisibility; //!< Neighbours by shared point count. +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_TYPES_HPP diff --git a/modules/slam/include/opencv2/slam/visual_odometry.hpp b/modules/slam/include/opencv2/slam/visual_odometry.hpp new file mode 100644 index 0000000000..ec3e451383 --- /dev/null +++ b/modules/slam/include/opencv2/slam/visual_odometry.hpp @@ -0,0 +1,102 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef OPENCV_SLAM_VISUAL_ODOMETRY_HPP +#define OPENCV_SLAM_VISUAL_ODOMETRY_HPP + +#include "types.hpp" +#include "map.hpp" +#include "odometry_params.hpp" +#include +#include + +namespace cv { namespace slam { + +/** @brief Monocular visual odometry pipeline. + + Use in batch mode (process a whole folder) or frame-by-frame: + + @code + auto vo = cv::slam::VisualOdometry::create( + detector, matcher, imagesFolder, outputFolder, K, dist, params); + vo->run(); // batch + vo->processFrame(img); // or incremental + @endcode + + Output files written by run() to outputFolder: + - trajectory.bin — binary T_cw pose stream + - trajectory.txt — camera centres in world coords + - images.txt — COLMAP-compatible pose file + - map_points.txt — 3-D point cloud + - keypoints.txt — per-keyframe observations + - vo.log — per-frame processing trace + + All poses are world-to-camera (T_cw). Camera centre = -R^T * t. + + @ingroup slam_odometry +*/ +class CV_EXPORTS_W VisualOdometry : public Algorithm +{ +public: + VisualOdometry(); + virtual ~VisualOdometry(); + + /** @brief Create a VisualOdometry instance. + @param detector Feature detector + descriptor extractor. + @param matcher Descriptor matcher. + @param imagesFolder Input image folder used by run(). May be empty. + @param outputFolder Artifact output folder. May be empty. + @param cameraMatrix 3x3 intrinsic matrix K. + @param distCoeffs Distortion coefficients. Pass empty Mat for rectified input. + @param params Odometry parameters. + */ + CV_WRAP static Ptr create( + const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + InputArray cameraMatrix, + InputArray distCoeffs, + const OdometryParams& params = OdometryParams()); + + /** @brief Process all images in imagesFolder and write output artifacts. + @return true if at least one pose was emitted. + */ + CV_WRAP virtual bool run() = 0; + + /** @brief Process a single frame. + @return true if a pose was emitted. + */ + CV_WRAP virtual bool processFrame(InputArray image) = 0; + + /** @brief Reset to NOT_INITIALIZED state and clear the map. */ + CV_WRAP virtual void reset() = 0; + + CV_WRAP virtual OdometryState getState() const = 0; + CV_WRAP virtual Matx44d getLastPose() const = 0; + CV_WRAP virtual Map& getMap() = 0; + CV_WRAP virtual const std::vector& getTrajectory() const = 0; + + CV_WRAP virtual OdometryParams getParams() const = 0; + CV_WRAP virtual void setParams(const OdometryParams& params) = 0; + + /** @brief Enable/disable pose-only BA after each PnP step. No-op without g2o. Default: true. */ + CV_WRAP virtual void setPoseOptimization(bool enable) = 0; + CV_WRAP virtual bool getPoseOptimization() const = 0; + + /** @brief Enable/disable local BA at each keyframe. No-op without g2o. Default: true. */ + CV_WRAP virtual void setLocalBA(bool enable) = 0; + CV_WRAP virtual bool getLocalBA() const = 0; + + CV_WRAP virtual String getImagesFolder() const = 0; + CV_WRAP virtual void setImagesFolder(const String& path) = 0; + + CV_WRAP virtual String getOutputFolder() const = 0; + CV_WRAP virtual void setOutputFolder(const String& path) = 0; +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP diff --git a/modules/slam/perf/perf_main.cpp b/modules/slam/perf/perf_main.cpp new file mode 100644 index 0000000000..94d0aa3da3 --- /dev/null +++ b/modules/slam/perf/perf_main.cpp @@ -0,0 +1,8 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#include "perf_precomp.hpp" + +CV_PERF_TEST_MAIN(slam) diff --git a/modules/slam/perf/perf_precomp.hpp b/modules/slam/perf/perf_precomp.hpp new file mode 100644 index 0000000000..d562b26574 --- /dev/null +++ b/modules/slam/perf/perf_precomp.hpp @@ -0,0 +1,12 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef __OPENCV_PERF_SLAM_PRECOMP_HPP__ +#define __OPENCV_PERF_SLAM_PRECOMP_HPP__ + +#include +#include + +#endif // __OPENCV_PERF_SLAM_PRECOMP_HPP__ diff --git a/modules/slam/src/map.cpp b/modules/slam/src/map.cpp new file mode 100644 index 0000000000..43ecd44246 --- /dev/null +++ b/modules/slam/src/map.cpp @@ -0,0 +1,125 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#include "precomp.hpp" + +namespace cv { namespace slam { + +struct Map::Impl +{ + std::vector kf_vec; + std::vector mp_vec; + std::unordered_map kf_map; + std::unordered_map mp_map; + std::vector trajectory; + int next_kf_id = 0; + int next_mp_id = 0; +}; + +Map::Map() : impl_(makePtr()) {} + +Map::~Map() +{ + clear(); +} + +int Map::addKeyframe(KeyFrame& kf) +{ + if (kf.id < 0) + kf.id = impl_->next_kf_id++; + KeyFrame* ptr = new KeyFrame(kf); + impl_->kf_vec.push_back(ptr); + impl_->kf_map[ptr->id] = ptr; + kf.id = ptr->id; + return ptr->id; +} + +KeyFrame* Map::getKeyframe(int id) +{ + auto it = impl_->kf_map.find(id); + return it != impl_->kf_map.end() ? it->second : nullptr; +} + +const std::vector& Map::keyframes() const { return impl_->kf_vec; } + +int Map::numKeyframes() const { return (int)impl_->kf_vec.size(); } + +int Map::addMapPoint(MapPoint& mp) +{ + if (mp.id < 0) + mp.id = impl_->next_mp_id++; + MapPoint* ptr = new MapPoint(mp); + impl_->mp_vec.push_back(ptr); + impl_->mp_map[ptr->id] = ptr; + mp.id = ptr->id; + return ptr->id; +} + +MapPoint* Map::getMapPoint(int id) +{ + auto it = impl_->mp_map.find(id); + return it != impl_->mp_map.end() ? it->second : nullptr; +} + +std::vector Map::mapPoints() const +{ + std::vector live; + live.reserve(impl_->mp_vec.size()); + for (MapPoint* mp : impl_->mp_vec) + if (mp && !mp->bad) + live.push_back(mp); + return live; +} + +int Map::numMapPoints() const +{ + int n = 0; + for (const MapPoint* mp : impl_->mp_vec) + if (mp && !mp->bad) ++n; + return n; +} + +void Map::addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp) +{ + CV_Assert(kf && mp); + mp->observations[kf] = kp_idx; + if (kp_idx < (int)kf->mappoints.size()) + kf->mappoints[kp_idx] = mp; + if (kp_idx < (int)kf->kpt_to_mp.size()) + kf->kpt_to_mp[kp_idx] = mp->id; +} + +void Map::removeMapPoint(int mp_id) +{ + MapPoint* mp = getMapPoint(mp_id); + if (!mp || mp->bad) return; + for (auto& [obs_kf, kp_idx] : mp->observations) + { + if (!obs_kf) continue; + if (kp_idx < (int)obs_kf->mappoints.size()) obs_kf->mappoints[kp_idx] = nullptr; + if (kp_idx < (int)obs_kf->kpt_to_mp.size()) obs_kf->kpt_to_mp[kp_idx] = -1; + } + mp->observations.clear(); + mp->bad = true; +} + +void Map::appendPose(const Matx44d& T_cw) { impl_->trajectory.push_back(T_cw); } + +const std::vector& Map::trajectory() const { return impl_->trajectory; } + +void Map::clear() +{ + for (KeyFrame* kf : impl_->kf_vec) delete kf; + for (MapPoint* mp : impl_->mp_vec) delete mp; + impl_->kf_vec.clear(); + impl_->mp_vec.clear(); + impl_->kf_map.clear(); + impl_->mp_map.clear(); + impl_->trajectory.clear(); + impl_->next_kf_id = 0; + impl_->next_mp_id = 0; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/precomp.hpp b/modules/slam/src/precomp.hpp new file mode 100644 index 0000000000..ecc564bffd --- /dev/null +++ b/modules/slam/src/precomp.hpp @@ -0,0 +1,28 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef __OPENCV_SLAM_PRECOMP_HPP__ +#define __OPENCV_SLAM_PRECOMP_HPP__ + +#include +#include +#include +#include +#include +#include +#include "opencv2/slam.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#endif // __OPENCV_SLAM_PRECOMP_HPP__ diff --git a/modules/slam/src/types.cpp b/modules/slam/src/types.cpp new file mode 100644 index 0000000000..80b2d0daef --- /dev/null +++ b/modules/slam/src/types.cpp @@ -0,0 +1,12 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#include "precomp.hpp" + +namespace cv { namespace slam { + +OdometryParams::OdometryParams() = default; + +}} // namespace cv::slam diff --git a/modules/slam/test/test_precomp.hpp b/modules/slam/test/test_precomp.hpp new file mode 100644 index 0000000000..9afe8de0be --- /dev/null +++ b/modules/slam/test/test_precomp.hpp @@ -0,0 +1,12 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + + +#ifndef __OPENCV_TEST_SLAM_PRECOMP_HPP__ +#define __OPENCV_TEST_SLAM_PRECOMP_HPP__ + +#include +#include + +#endif // __OPENCV_TEST_SLAM_PRECOMP_HPP__ diff --git a/modules/slam/test/test_visual_odometry.cpp b/modules/slam/test/test_visual_odometry.cpp new file mode 100644 index 0000000000..4496908507 --- /dev/null +++ b/modules/slam/test/test_visual_odometry.cpp @@ -0,0 +1,136 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +// ============================================================================= +// Map tests +// ============================================================================= + +TEST(Map, AddAndRetrieveKeyframe) +{ + cv::slam::Map map; + + cv::slam::KeyFrame kf; + kf.pose_cw = cv::Matx44d::eye(); + + int id = map.addKeyframe(kf); + EXPECT_GE(id, 0); + EXPECT_EQ(kf.id, id); + + cv::slam::KeyFrame* retrieved = map.getKeyframe(id); + ASSERT_NE(retrieved, nullptr); + EXPECT_EQ(retrieved->id, id); + EXPECT_EQ(map.numKeyframes(), 1); +} + +TEST(Map, AddAndRetrieveMapPoint) +{ + cv::slam::Map map; + + cv::slam::MapPoint mp; + mp.pos = cv::Point3d(1.0, 2.0, 3.0); + + int id = map.addMapPoint(mp); + EXPECT_GE(id, 0); + EXPECT_EQ(mp.id, id); + + cv::slam::MapPoint* retrieved = map.getMapPoint(id); + ASSERT_NE(retrieved, nullptr); + EXPECT_DOUBLE_EQ(retrieved->pos.x, 1.0); + EXPECT_DOUBLE_EQ(retrieved->pos.y, 2.0); + EXPECT_DOUBLE_EQ(retrieved->pos.z, 3.0); + EXPECT_FALSE(retrieved->bad); + EXPECT_EQ(map.numMapPoints(), 1); +} + +TEST(Map, RemoveMapPointCleansObservations) +{ + cv::slam::Map map; + + // Add a keyframe with one keypoint slot. + cv::slam::KeyFrame kf; + kf.pose_cw = cv::Matx44d::eye(); + kf.mappoints.assign(1, nullptr); + kf.kpt_to_mp.assign(1, -1); + int kf_id = map.addKeyframe(kf); + + // Add a map point. + cv::slam::MapPoint mp; + mp.pos = cv::Point3d(0, 0, 1); + int mp_id = map.addMapPoint(mp); + + cv::slam::KeyFrame* kf_ptr = map.getKeyframe(kf_id); + cv::slam::MapPoint* mp_ptr = map.getMapPoint(mp_id); + ASSERT_NE(kf_ptr, nullptr); + ASSERT_NE(mp_ptr, nullptr); + + // Wire the observation. + map.addObservation(kf_ptr, 0, mp_ptr); + EXPECT_EQ(mp_ptr->observations.size(), 1u); + EXPECT_EQ(kf_ptr->mappoints[0], mp_ptr); + + // Remove the map point and verify cleanup. + map.removeMapPoint(mp_id); + EXPECT_TRUE(mp_ptr->bad); + EXPECT_EQ(kf_ptr->mappoints[0], nullptr); + EXPECT_EQ(kf_ptr->kpt_to_mp[0], -1); + EXPECT_EQ(map.numMapPoints(), 0); +} + +TEST(Map, TrajectoryAppendAndRetrieve) +{ + cv::slam::Map map; + + cv::Matx44d T1 = cv::Matx44d::eye(); + cv::Matx44d T2 = cv::Matx44d::eye(); + T2(0, 3) = 1.0; // 1-unit translation + + map.appendPose(T1); + map.appendPose(T2); + + const auto& traj = map.trajectory(); + ASSERT_EQ(traj.size(), 2u); + EXPECT_DOUBLE_EQ(traj[1](0, 3), 1.0); +} + +TEST(Map, ClearResetsState) +{ + cv::slam::Map map; + + cv::slam::KeyFrame kf; + kf.pose_cw = cv::Matx44d::eye(); + map.addKeyframe(kf); + + cv::slam::MapPoint mp; + mp.pos = cv::Point3d(1, 2, 3); + map.addMapPoint(mp); + + map.appendPose(cv::Matx44d::eye()); + map.clear(); + + EXPECT_EQ(map.numKeyframes(), 0); + EXPECT_EQ(map.numMapPoints(), 0); + EXPECT_TRUE(map.trajectory().empty()); +} + +// ============================================================================= +// OdometryParams tests +// ============================================================================= + +TEST(OdometryParams, DefaultValues) +{ + cv::slam::OdometryParams p; + EXPECT_EQ(p.min_init_inliers, 80); + EXPECT_DOUBLE_EQ(p.min_init_parallax_deg, 3.0); + EXPECT_EQ(p.min_init_points, 50); + EXPECT_DOUBLE_EQ(p.hf_ratio_thresh, 0.45); + EXPECT_DOUBLE_EQ(p.min_growth_parallax_deg, 1.0); + EXPECT_DOUBLE_EQ(p.essential_ransac_thresh, 1.0); + EXPECT_DOUBLE_EQ(p.essential_ransac_confidence, 0.999); +} + +}} // namespace opencv_test From 08077a00a182f71d72fd609bcc9f814ce370c605 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Wed, 17 Jun 2026 13:15:57 +0530 Subject: [PATCH 02/10] cleanupFinal --- modules/slam/CMakeLists.txt | 23 +- modules/slam/include/opencv2/slam.hpp | 23 +- modules/slam/include/opencv2/slam/map.hpp | 81 +-- .../include/opencv2/slam/odometry_params.hpp | 63 ++- modules/slam/include/opencv2/slam/slam.hpp | 13 + modules/slam/include/opencv2/slam/types.hpp | 78 +-- .../include/opencv2/slam/visual_odometry.hpp | 109 ++--- modules/slam/perf/perf_main.cpp | 3 +- modules/slam/perf/perf_precomp.hpp | 13 +- modules/slam/src/map.cpp | 183 ++++--- modules/slam/src/odometry/frame.hpp | 79 +++ modules/slam/src/odometry/visual_odometry.cpp | 461 ++++++++++++++++++ modules/slam/src/odometry/vo_bootstrap.cpp | 252 ++++++++++ modules/slam/src/odometry/vo_impl.hpp | 129 +++++ modules/slam/src/odometry/vo_keyframe.cpp | 145 ++++++ modules/slam/src/odometry/vo_map_growth.cpp | 135 +++++ modules/slam/src/odometry/vo_tracking.cpp | 390 +++++++++++++++ modules/slam/src/optimizer/pose_optimizer.cpp | 49 ++ modules/slam/src/optimizer/pose_optimizer.hpp | 21 + modules/slam/src/precomp.hpp | 34 +- .../{src/types.cpp => test/test_main.cpp} | 11 +- modules/slam/test/test_map.cpp | 79 +++ modules/slam/test/test_precomp.hpp | 15 +- modules/slam/test/test_visual_odometry.cpp | 321 ++++++++---- samples/CMakeLists.txt | 2 + samples/slam/CMakeLists.txt | 22 + samples/slam/visual_odometry.cpp | 63 +++ samples/slam/visual_odometry.py | 56 +++ 28 files changed, 2453 insertions(+), 400 deletions(-) create mode 100644 modules/slam/include/opencv2/slam/slam.hpp create mode 100644 modules/slam/src/odometry/frame.hpp create mode 100644 modules/slam/src/odometry/visual_odometry.cpp create mode 100644 modules/slam/src/odometry/vo_bootstrap.cpp create mode 100644 modules/slam/src/odometry/vo_impl.hpp create mode 100644 modules/slam/src/odometry/vo_keyframe.cpp create mode 100644 modules/slam/src/odometry/vo_map_growth.cpp create mode 100644 modules/slam/src/odometry/vo_tracking.cpp create mode 100644 modules/slam/src/optimizer/pose_optimizer.cpp create mode 100644 modules/slam/src/optimizer/pose_optimizer.hpp rename modules/slam/{src/types.cpp => test/test_main.cpp} (53%) create mode 100644 modules/slam/test/test_map.cpp create mode 100644 samples/slam/CMakeLists.txt create mode 100644 samples/slam/visual_odometry.cpp create mode 100644 samples/slam/visual_odometry.py diff --git a/modules/slam/CMakeLists.txt b/modules/slam/CMakeLists.txt index 91bcf7f368..6b6f62b2f8 100644 --- a/modules/slam/CMakeLists.txt +++ b/modules/slam/CMakeLists.txt @@ -1,14 +1,13 @@ -# This file is part of OpenCV project. -# It is subject to the license terms in the LICENSE file found in the top-level directory -# of this distribution and at http://opencv.org/license.html. - -set(the_description "SLAM (Simultaneous Localization and Mapping) — visual odometry and mapping") +set(the_description "SLAM and Visual Odometry") ocv_define_module(slam - opencv_core - opencv_imgproc - opencv_imgcodecs - opencv_features - opencv_3d - OPTIONAL opencv_dnn - WRAP python) + opencv_geometry + opencv_features + opencv_imgproc + opencv_imgcodecs + opencv_video + OPTIONAL + opencv_dnn + WRAP python) + +ocv_warnings_disable(CMAKE_CXX_FLAGS -Wshadow) diff --git a/modules/slam/include/opencv2/slam.hpp b/modules/slam/include/opencv2/slam.hpp index 2c4f84e058..c677e221b2 100644 --- a/modules/slam/include/opencv2/slam.hpp +++ b/modules/slam/include/opencv2/slam.hpp @@ -1,22 +1,23 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_HPP #define OPENCV_SLAM_HPP -#include "slam/types.hpp" -#include "slam/map.hpp" -#include "slam/odometry_params.hpp" -#include "slam/visual_odometry.hpp" - /** -@defgroup slam SLAM (Simultaneous Localization and Mapping) -@{ - @defgroup slam_odometry Visual Odometry - @brief Monocular visual odometry pipeline. -@} +@defgroup slam SLAM and Visual Odometry + +Monocular visual odometry pipeline. Entry point is @ref cv::slam::VisualOdometry. +Bootstraps an initial map from two-view geometry, then tracks subsequent frames +with PnP, growing the map at keyframe promotions. */ +#include "opencv2/slam/types.hpp" +#include "opencv2/slam/map.hpp" +#include "opencv2/slam/odometry_params.hpp" +#include "opencv2/slam/visual_odometry.hpp" + #endif // OPENCV_SLAM_HPP diff --git a/modules/slam/include/opencv2/slam/map.hpp b/modules/slam/include/opencv2/slam/map.hpp index 2da8af1633..ab8d1963bc 100644 --- a/modules/slam/include/opencv2/slam/map.hpp +++ b/modules/slam/include/opencv2/slam/map.hpp @@ -1,73 +1,78 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_MAP_HPP #define OPENCV_SLAM_MAP_HPP -#include "types.hpp" -#include +#include "opencv2/slam/types.hpp" + +#include #include -namespace cv { namespace slam { +namespace cv { +namespace slam { -/** @brief Container for keyframes, map points, and the per-frame trajectory. +//! @addtogroup slam +//! @{ - Map owns all KeyFrame and MapPoint objects. External code holds only - raw non-owning pointers. Ids are assigned automatically when the - incoming id field is negative. - - @ingroup slam_odometry -*/ -class CV_EXPORTS_W Map +/** @brief Owns all persistent SLAM state (KeyFrames and MapPoints). +Pointers from addKeyframe / addMapPoint are valid until removeMapPoint / clear. */ +class CV_EXPORTS Map { public: - CV_WRAP Map(); + Map(); ~Map(); - /** @brief Add a keyframe; assigns id if kf.id < 0. Returns the assigned id. */ - CV_WRAP int addKeyframe(KeyFrame& kf); + Map(const Map&) = delete; + Map& operator=(const Map&) = delete; - /** @brief Return keyframe by id, or nullptr. */ - CV_WRAP KeyFrame* getKeyframe(int id); + // Keyframes - /** @brief All keyframes in insertion order. */ - CV_WRAP const std::vector& keyframes() const; + KeyFrame* addKeyframe(KeyFrame* kf); //!< takes ownership; assigns id if < 0 + KeyFrame* getKeyframe(int id) const; - CV_WRAP int numKeyframes() const; + const std::set& keyframes() const; + int numKeyframes() const; - /** @brief Add a map point; assigns id if mp.id < 0. Returns the assigned id. */ - CV_WRAP int addMapPoint(MapPoint& mp); + // Map points - /** @brief Return map point by id, or nullptr. */ - CV_WRAP MapPoint* getMapPoint(int id); + MapPoint* addMapPoint(MapPoint* mp); //!< takes ownership; assigns id if < 0 + MapPoint* getMapPoint(int id) const; - /** @brief All live (non-bad) map points. */ - CV_WRAP std::vector mapPoints() const; + const std::set& mapPoints() const; + int numMapPoints() const; - CV_WRAP int numMapPoints() const; + void addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp); + void removeObservation(KeyFrame* kf, MapPoint* mp); + void removeMapPoint(MapPoint* mp); - /** @brief Wire a bidirectional KF-MP observation. */ - CV_WRAP void addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp); + // Reference / current keyframes - /** @brief Mark a map point bad and remove all cross-references. */ - CV_WRAP void removeMapPoint(int mp_id); + void setRefKeyframe(KeyFrame* kf); + KeyFrame* getRefKeyframe() const; - /** @brief Append a world-to-camera pose to the trajectory. */ - CV_WRAP void appendPose(const Matx44d& T_cw); + void setCurrentKeyframe(KeyFrame* kf); + KeyFrame* getCurrentKeyframe() const; - /** @brief All emitted world-to-camera poses in order. */ - CV_WRAP const std::vector& trajectory() const; + // Trajectory - /** @brief Reset to empty state. */ - CV_WRAP void clear(); + void appendPose(const Matx44d& T_cw); + const std::vector& trajectory() const; + + // Lifecycle + + void clear(); private: struct Impl; - Ptr impl_; + Ptr impl; }; +//! @} + }} // namespace cv::slam #endif // OPENCV_SLAM_MAP_HPP diff --git a/modules/slam/include/opencv2/slam/odometry_params.hpp b/modules/slam/include/opencv2/slam/odometry_params.hpp index cff30cd48d..84194784ad 100644 --- a/modules/slam/include/opencv2/slam/odometry_params.hpp +++ b/modules/slam/include/opencv2/slam/odometry_params.hpp @@ -1,37 +1,64 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_ODOMETRY_PARAMS_HPP #define OPENCV_SLAM_ODOMETRY_PARAMS_HPP -#include +#include "opencv2/core.hpp" -namespace cv { namespace slam { +namespace cv { +namespace slam { -/** @brief Tunable parameters for the VisualOdometry pipeline. +//! @addtogroup slam +//! @{ - All fields have sensible defaults. Override any subset before - passing to VisualOdometry::create(). - - @ingroup slam_odometry -*/ struct CV_EXPORTS_W_SIMPLE OdometryParams { - CV_WRAP OdometryParams() = default; + CV_WRAP OdometryParams() {} - // --- Bootstrap (two-view init) ------------------------------------------- + // Bootstrap + CV_PROP_RW int minInitInliers = 40; + CV_PROP_RW double minInitParallaxDeg = 1.5; + CV_PROP_RW int minInitPoints = 50; + CV_PROP_RW double hfRatioThresh = 0.45; + CV_PROP_RW double minGrowthParallaxDeg = 0.1; + CV_PROP_RW double essentialRansacThresh = 1.0; + CV_PROP_RW double essentialRansacConfidence = 0.999; - CV_PROP_RW int min_init_inliers = 80; //!< Min essential-matrix RANSAC inliers. - CV_PROP_RW double min_init_parallax_deg = 3.0; //!< Min median parallax to trigger bootstrap. - CV_PROP_RW int min_init_points = 50; //!< Min triangulated points to accept init. - CV_PROP_RW double hf_ratio_thresh = 0.45; //!< H/(H+F) ratio; > thresh uses Homography. - CV_PROP_RW double min_growth_parallax_deg = 1.0; //!< Min parallax for a new map point to survive. - CV_PROP_RW double essential_ransac_thresh = 1.0; //!< RANSAC reprojection threshold (px). - CV_PROP_RW double essential_ransac_confidence = 0.999; //!< RANSAC confidence for findEssentialMat. + // Tracking (PnP) + CV_PROP_RW double pnpReprojThresh = 4.0; + CV_PROP_RW int pnpMinInliers = 6; + CV_PROP_RW int pnpRansacIters = 500; + CV_PROP_RW double pnpConfidence = 0.99; + + // Motion model + CV_PROP_RW double motionModelRadius = 15.0; + CV_PROP_RW double motionModelRadiusWide = 30.0; + CV_PROP_RW int motionModelMinMatches = 20; + CV_PROP_RW double descProjThresh = 1.0; + + // Optical flow fallback + CV_PROP_RW int opticalFlowMinInliers = 10; + + // Keyframe promotion + CV_PROP_RW int kfMinFrames = 1; + CV_PROP_RW int kfMaxFrames = 30; + CV_PROP_RW double kfInlierRatio = 0.70; + CV_PROP_RW int kfMinInliers = 40; + CV_PROP_RW double kfRotThreshDeg = 5.0; + CV_PROP_RW double kfTransThresh = 0.5; + + // Local map refinement + CV_PROP_RW int localMapTopK = 10; + CV_PROP_RW int localMapNeighborK = 5; + CV_PROP_RW double localMapRadius = 7.0; }; +//! @} + }} // namespace cv::slam #endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP diff --git a/modules/slam/include/opencv2/slam/slam.hpp b/modules/slam/include/opencv2/slam/slam.hpp new file mode 100644 index 0000000000..f296d9601e --- /dev/null +++ b/modules/slam/include/opencv2/slam/slam.hpp @@ -0,0 +1,13 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +// Compatibility shim: prefer #include . +#ifndef OPENCV_SLAM_SLAM_HPP +#define OPENCV_SLAM_SLAM_HPP + +#include "opencv2/slam.hpp" + +#endif diff --git a/modules/slam/include/opencv2/slam/types.hpp b/modules/slam/include/opencv2/slam/types.hpp index 4b54dbe7ee..66481aae6f 100644 --- a/modules/slam/include/opencv2/slam/types.hpp +++ b/modules/slam/include/opencv2/slam/types.hpp @@ -1,71 +1,71 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_TYPES_HPP #define OPENCV_SLAM_TYPES_HPP -#include +#include "opencv2/core.hpp" +#include "opencv2/core/types.hpp" + #include #include -namespace cv { namespace slam { +namespace cv { +namespace slam { -/** @brief Current state of the VisualOdometry pipeline. - @ingroup slam_odometry -*/ +//! @addtogroup slam +//! @{ + +// Pipeline lifecycle state enum OdometryState { - NOT_INITIALIZED = 0, //!< No frames processed yet. - INITIALIZING = 1, //!< Reference frame set; waiting for bootstrap. - TRACKING = 2 //!< Map initialised; localising via PnP. -}; - -/** @brief Feature data for one image frame. - @ingroup slam_odometry -*/ -struct CV_EXPORTS_W_SIMPLE FrameFeatures -{ - std::vector keypoints; - Mat descriptors; - Size imageSize; + NOT_INITIALIZED = 0, + INITIALIZING = 1, + TRACKING = 2 }; struct MapPoint; struct KeyFrame; -/** @brief A triangulated 3-D landmark owned by Map. - @ingroup slam_odometry -*/ -struct CV_EXPORTS_W_SIMPLE MapPoint +// 3D landmark, owned by Map +struct CV_EXPORTS MapPoint { - int id = -1; - Point3d pos; - bool bad = false; + int id = -1; + Point3d pos { 0, 0, 0 }; + Mat refDesc; - std::map observations; //!< Observing keyframe → keypoint index. + std::map observations; // kf -> keypoint index + + int visibleCount = 0; + int foundCount = 0; + bool bad = false; // soft-delete; check before use }; -/** @brief A frame whose pose has been committed to the map. - @ingroup slam_odometry -*/ -struct CV_EXPORTS_W_SIMPLE KeyFrame +// Keyframe: pose + keypoints + covisibility graph, owned by Map +struct CV_EXPORTS KeyFrame { - int id = -1; - Matx44d pose_cw; //!< World-to-camera transform (row-major 4x4). + int id = -1; + Matx44d poseCw = Matx44d::eye(); // world->camera std::vector keypoints; - Mat descriptors; - std::vector undist_kpts; - Size imageSize; + Mat descriptors; + std::vector undistKpts; // parallel to keypoints + Size imageSize; - std::vector kpt_to_mp; //!< Map-point id per keypoint, -1 if unmatched. - std::vector mappoints; //!< Direct pointer per keypoint, nullptr if unmatched. + std::vector mapPoints; // parallel to keypoints; null = unmatched - std::vector> ordered_covisibility; //!< Neighbours by shared point count. + std::map covisibility; + std::vector> orderedCovisibility; // sorted descending by count + + KeyFrame* parent = nullptr; + Mat globalDesc; }; +//! @} + }} // namespace cv::slam #endif // OPENCV_SLAM_TYPES_HPP diff --git a/modules/slam/include/opencv2/slam/visual_odometry.hpp b/modules/slam/include/opencv2/slam/visual_odometry.hpp index ec3e451383..95b55e89e3 100644 --- a/modules/slam/include/opencv2/slam/visual_odometry.hpp +++ b/modules/slam/include/opencv2/slam/visual_odometry.hpp @@ -1,102 +1,79 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #ifndef OPENCV_SLAM_VISUAL_ODOMETRY_HPP #define OPENCV_SLAM_VISUAL_ODOMETRY_HPP -#include "types.hpp" -#include "map.hpp" -#include "odometry_params.hpp" -#include -#include +#include "opencv2/core.hpp" +#include "opencv2/features.hpp" -namespace cv { namespace slam { +#include "opencv2/slam/types.hpp" +#include "opencv2/slam/map.hpp" +#include "opencv2/slam/odometry_params.hpp" + +#include + +namespace cv { +namespace slam { + +//! @addtogroup slam +//! @{ /** @brief Monocular visual odometry pipeline. - Use in batch mode (process a whole folder) or frame-by-frame: +State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING +(per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING. - @code - auto vo = cv::slam::VisualOdometry::create( - detector, matcher, imagesFolder, outputFolder, K, dist, params); - vo->run(); // batch - vo->processFrame(img); // or incremental - @endcode - - Output files written by run() to outputFolder: - - trajectory.bin — binary T_cw pose stream - - trajectory.txt — camera centres in world coords - - images.txt — COLMAP-compatible pose file - - map_points.txt — 3-D point cloud - - keypoints.txt — per-keyframe observations - - vo.log — per-frame processing trace - - All poses are world-to-camera (T_cw). Camera centre = -R^T * t. - - @ingroup slam_odometry +@ref run writes trajectory.txt, trajectory.bin, map_points.txt, keypoints.txt, +images.txt, and vo.log into `outputFolder`. */ -class CV_EXPORTS_W VisualOdometry : public Algorithm +class CV_EXPORTS_W VisualOdometry { public: - VisualOdometry(); virtual ~VisualOdometry(); - /** @brief Create a VisualOdometry instance. - @param detector Feature detector + descriptor extractor. - @param matcher Descriptor matcher. - @param imagesFolder Input image folder used by run(). May be empty. - @param outputFolder Artifact output folder. May be empty. - @param cameraMatrix 3x3 intrinsic matrix K. - @param distCoeffs Distortion coefficients. Pass empty Mat for rectified input. - @param params Odometry parameters. - */ CV_WRAP static Ptr create( - const Ptr& detector, + const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, - InputArray cameraMatrix, - InputArray distCoeffs, - const OdometryParams& params = OdometryParams()); + const String& imagesFolder, + const String& outputFolder, + InputArray cameraMatrix, + InputArray distCoeffs = noArray(), + const OdometryParams& params = OdometryParams()); - /** @brief Process all images in imagesFolder and write output artifacts. - @return true if at least one pose was emitted. - */ + /** @brief Run the pipeline over every image in the configured folder. */ CV_WRAP virtual bool run() = 0; - /** @brief Process a single frame. - @return true if a pose was emitted. - */ + /** @brief Feed one image. Returns true if a pose was emitted. */ CV_WRAP virtual bool processFrame(InputArray image) = 0; - /** @brief Reset to NOT_INITIALIZED state and clear the map. */ + /** @brief Reset to NOT_INITIALIZED, clearing map and trajectory. */ CV_WRAP virtual void reset() = 0; - CV_WRAP virtual OdometryState getState() const = 0; - CV_WRAP virtual Matx44d getLastPose() const = 0; - CV_WRAP virtual Map& getMap() = 0; - CV_WRAP virtual const std::vector& getTrajectory() const = 0; + CV_WRAP virtual OdometryState getState() const = 0; + CV_WRAP virtual Matx44d getLastPose() const = 0; - CV_WRAP virtual OdometryParams getParams() const = 0; - CV_WRAP virtual void setParams(const OdometryParams& params) = 0; + //! @note Not exposed to Python: Map holds raw pointers / non-convertible containers. + virtual const Map& getMap() const = 0; - /** @brief Enable/disable pose-only BA after each PnP step. No-op without g2o. Default: true. */ - CV_WRAP virtual void setPoseOptimization(bool enable) = 0; - CV_WRAP virtual bool getPoseOptimization() const = 0; + CV_WRAP virtual const std::vector& getTrajectory() const = 0; - /** @brief Enable/disable local BA at each keyframe. No-op without g2o. Default: true. */ - CV_WRAP virtual void setLocalBA(bool enable) = 0; - CV_WRAP virtual bool getLocalBA() const = 0; + CV_WRAP virtual const OdometryParams& getParams() const = 0; + CV_WRAP virtual void setParams(const OdometryParams& params) = 0; - CV_WRAP virtual String getImagesFolder() const = 0; - CV_WRAP virtual void setImagesFolder(const String& path) = 0; + CV_WRAP virtual const String& getImagesFolder() const = 0; + CV_WRAP virtual const String& getOutputFolder() const = 0; + CV_WRAP virtual void setOutputFolder(const String& outputFolder) = 0; - CV_WRAP virtual String getOutputFolder() const = 0; - CV_WRAP virtual void setOutputFolder(const String& path) = 0; +protected: + VisualOdometry(); }; +//! @} + }} // namespace cv::slam #endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP diff --git a/modules/slam/perf/perf_main.cpp b/modules/slam/perf/perf_main.cpp index 94d0aa3da3..95b14c1581 100644 --- a/modules/slam/perf/perf_main.cpp +++ b/modules/slam/perf/perf_main.cpp @@ -1,7 +1,8 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #include "perf_precomp.hpp" diff --git a/modules/slam/perf/perf_precomp.hpp b/modules/slam/perf/perf_precomp.hpp index d562b26574..37fede9ca6 100644 --- a/modules/slam/perf/perf_precomp.hpp +++ b/modules/slam/perf/perf_precomp.hpp @@ -1,12 +1,13 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#ifndef OPENCV_SLAM_PERF_PRECOMP_HPP +#define OPENCV_SLAM_PERF_PRECOMP_HPP -#ifndef __OPENCV_PERF_SLAM_PRECOMP_HPP__ -#define __OPENCV_PERF_SLAM_PRECOMP_HPP__ +#include "opencv2/ts.hpp" +#include "opencv2/slam.hpp" -#include -#include - -#endif // __OPENCV_PERF_SLAM_PRECOMP_HPP__ +#endif diff --git a/modules/slam/src/map.cpp b/modules/slam/src/map.cpp index 43ecd44246..fc958aa3d0 100644 --- a/modules/slam/src/map.cpp +++ b/modules/slam/src/map.cpp @@ -1,125 +1,150 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. - +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #include "precomp.hpp" -namespace cv { namespace slam { +#include +#include +#include + +namespace cv { +namespace slam { struct Map::Impl { - std::vector kf_vec; - std::vector mp_vec; - std::unordered_map kf_map; - std::unordered_map mp_map; - std::vector trajectory; - int next_kf_id = 0; - int next_mp_id = 0; + std::set keyframes; + std::set mapPoints; + std::unordered_map kfIndex; + std::unordered_map mpIndex; + + KeyFrame* refKf = nullptr; + KeyFrame* currentKf = nullptr; + + std::vector trajectory; + std::mutex mutex; + + int nextKfId = 0; + int nextMpId = 0; }; -Map::Map() : impl_(makePtr()) {} +Map::Map() : impl(makePtr()) {} Map::~Map() { - clear(); + for (KeyFrame* kf : impl->keyframes) delete kf; + for (MapPoint* mp : impl->mapPoints) delete mp; } -int Map::addKeyframe(KeyFrame& kf) +// Keyframes + +KeyFrame* Map::addKeyframe(KeyFrame* kf) { - if (kf.id < 0) - kf.id = impl_->next_kf_id++; - KeyFrame* ptr = new KeyFrame(kf); - impl_->kf_vec.push_back(ptr); - impl_->kf_map[ptr->id] = ptr; - kf.id = ptr->id; - return ptr->id; + CV_Assert(kf); + if (kf->id < 0) + kf->id = impl->nextKfId++; + else if (kf->id >= impl->nextKfId) + impl->nextKfId = kf->id + 1; + impl->keyframes.insert(kf); + impl->kfIndex[kf->id] = kf; + return kf; } -KeyFrame* Map::getKeyframe(int id) +KeyFrame* Map::getKeyframe(int id) const { - auto it = impl_->kf_map.find(id); - return it != impl_->kf_map.end() ? it->second : nullptr; + auto it = impl->kfIndex.find(id); + return (it != impl->kfIndex.end()) ? it->second : nullptr; } -const std::vector& Map::keyframes() const { return impl_->kf_vec; } +const std::set& Map::keyframes() const { return impl->keyframes; } +int Map::numKeyframes() const { return (int)impl->keyframes.size(); } -int Map::numKeyframes() const { return (int)impl_->kf_vec.size(); } +// Map points -int Map::addMapPoint(MapPoint& mp) +MapPoint* Map::addMapPoint(MapPoint* mp) { - if (mp.id < 0) - mp.id = impl_->next_mp_id++; - MapPoint* ptr = new MapPoint(mp); - impl_->mp_vec.push_back(ptr); - impl_->mp_map[ptr->id] = ptr; - mp.id = ptr->id; - return ptr->id; + CV_Assert(mp); + if (mp->id < 0) + mp->id = impl->nextMpId++; + else if (mp->id >= impl->nextMpId) + impl->nextMpId = mp->id + 1; + impl->mapPoints.insert(mp); + impl->mpIndex[mp->id] = mp; + return mp; } -MapPoint* Map::getMapPoint(int id) +MapPoint* Map::getMapPoint(int id) const { - auto it = impl_->mp_map.find(id); - return it != impl_->mp_map.end() ? it->second : nullptr; + auto it = impl->mpIndex.find(id); + return (it != impl->mpIndex.end()) ? it->second : nullptr; } -std::vector Map::mapPoints() const -{ - std::vector live; - live.reserve(impl_->mp_vec.size()); - for (MapPoint* mp : impl_->mp_vec) - if (mp && !mp->bad) - live.push_back(mp); - return live; -} +const std::set& Map::mapPoints() const { return impl->mapPoints; } +int Map::numMapPoints() const { return (int)impl->mapPoints.size(); } -int Map::numMapPoints() const -{ - int n = 0; - for (const MapPoint* mp : impl_->mp_vec) - if (mp && !mp->bad) ++n; - return n; -} +// Observations -void Map::addObservation(KeyFrame* kf, int kp_idx, MapPoint* mp) +void Map::addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp) { CV_Assert(kf && mp); - mp->observations[kf] = kp_idx; - if (kp_idx < (int)kf->mappoints.size()) - kf->mappoints[kp_idx] = mp; - if (kp_idx < (int)kf->kpt_to_mp.size()) - kf->kpt_to_mp[kp_idx] = mp->id; + CV_Assert(kpIdx < kf->mapPoints.size()); + if (kf->mapPoints[kpIdx] != nullptr) return; + kf->mapPoints[kpIdx] = mp; + mp->observations[kf] = kpIdx; } -void Map::removeMapPoint(int mp_id) +void Map::removeObservation(KeyFrame* kf, MapPoint* mp) { - MapPoint* mp = getMapPoint(mp_id); - if (!mp || mp->bad) return; - for (auto& [obs_kf, kp_idx] : mp->observations) - { - if (!obs_kf) continue; - if (kp_idx < (int)obs_kf->mappoints.size()) obs_kf->mappoints[kp_idx] = nullptr; - if (kp_idx < (int)obs_kf->kpt_to_mp.size()) obs_kf->kpt_to_mp[kp_idx] = -1; - } - mp->observations.clear(); - mp->bad = true; + if (!kf || !mp) return; + auto it = mp->observations.find(kf); + if (it == mp->observations.end()) return; + size_t kpIdx = it->second; + if (kpIdx < kf->mapPoints.size()) + kf->mapPoints[kpIdx] = nullptr; + mp->observations.erase(it); } -void Map::appendPose(const Matx44d& T_cw) { impl_->trajectory.push_back(T_cw); } +void Map::removeMapPoint(MapPoint* mp) +{ + if (!mp) return; + for (auto& [kf, kpIdx] : mp->observations) + if (kpIdx < kf->mapPoints.size()) + kf->mapPoints[kpIdx] = nullptr; + impl->mapPoints.erase(mp); + impl->mpIndex.erase(mp->id); + delete mp; +} -const std::vector& Map::trajectory() const { return impl_->trajectory; } +// Reference / current keyframes + +void Map::setRefKeyframe(KeyFrame* kf) { impl->refKf = kf; } +KeyFrame* Map::getRefKeyframe() const { return impl->refKf; } + +void Map::setCurrentKeyframe(KeyFrame* kf) { impl->currentKf = kf; } +KeyFrame* Map::getCurrentKeyframe() const { return impl->currentKf; } + +// Trajectory + +void Map::appendPose(const Matx44d& T_cw) { impl->trajectory.push_back(T_cw); } +const std::vector& Map::trajectory() const { return impl->trajectory; } + +// Lifecycle void Map::clear() { - for (KeyFrame* kf : impl_->kf_vec) delete kf; - for (MapPoint* mp : impl_->mp_vec) delete mp; - impl_->kf_vec.clear(); - impl_->mp_vec.clear(); - impl_->kf_map.clear(); - impl_->mp_map.clear(); - impl_->trajectory.clear(); - impl_->next_kf_id = 0; - impl_->next_mp_id = 0; + for (KeyFrame* kf : impl->keyframes) delete kf; + for (MapPoint* mp : impl->mapPoints) delete mp; + impl->keyframes.clear(); + impl->mapPoints.clear(); + impl->kfIndex.clear(); + impl->mpIndex.clear(); + impl->refKf = nullptr; + impl->currentKf = nullptr; + impl->trajectory.clear(); + impl->nextKfId = 0; + impl->nextMpId = 0; } }} // namespace cv::slam diff --git a/modules/slam/src/odometry/frame.hpp b/modules/slam/src/odometry/frame.hpp new file mode 100644 index 0000000000..bf7e255aa1 --- /dev/null +++ b/modules/slam/src/odometry/frame.hpp @@ -0,0 +1,79 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_FRAME_HPP +#define OPENCV_SLAM_FRAME_HPP + +#include "../precomp.hpp" + +namespace cv { +namespace slam { + +/** @brief Per-frame scratch pad. Created per incoming image, never stored in the Map. */ +struct Frame +{ + Mat image; + std::vector keypoints; + Mat descriptors; + std::vector undistKpts; //!< undistorted, parallel to keypoints + Size imageSize; + + Matx44d poseCw = Matx44d::eye(); + std::vector mapPoints; //!< parallel to keypoints; nullptr = unmatched + std::vector outliers; //!< parallel to keypoints; true = inlier check failed + + // Grid cell size in pixels — dimensions adapt to each image resolution. + static constexpr int CELL_SIZE_PX = 20; + + int gridRows = 0; + int gridCols = 0; + std::unordered_map> grid; //!< cell key -> keypoint indices + + int cellKey(int row, int col) const { return row * gridCols + col; } + + void buildGrid() + { + grid.clear(); + if (imageSize.width <= 0 || imageSize.height <= 0) return; + gridCols = std::max(1, (imageSize.width + CELL_SIZE_PX - 1) / CELL_SIZE_PX); + gridRows = std::max(1, (imageSize.height + CELL_SIZE_PX - 1) / CELL_SIZE_PX); + for (size_t i = 0; i < undistKpts.size(); ++i) + { + int col = std::min(gridCols - 1, std::max(0, (int)(undistKpts[i].x / CELL_SIZE_PX))); + int row = std::min(gridRows - 1, std::max(0, (int)(undistKpts[i].y / CELL_SIZE_PX))); + grid[cellKey(row, col)].push_back(i); + } + } + + std::vector getKeypointsInRadius(float x, float y, float r) const + { + std::vector result; + if (gridRows <= 0 || gridCols <= 0 || grid.empty()) return result; + const int colMin = std::max(0, (int)((x - r) / CELL_SIZE_PX)); + const int colMax = std::min(gridCols-1, (int)((x + r) / CELL_SIZE_PX)); + const int rowMin = std::max(0, (int)((y - r) / CELL_SIZE_PX)); + const int rowMax = std::min(gridRows-1, (int)((y + r) / CELL_SIZE_PX)); + const float r2 = r * r; + for (int row = rowMin; row <= rowMax; ++row) + for (int col = colMin; col <= colMax; ++col) + { + auto it = grid.find(cellKey(row, col)); + if (it == grid.end()) continue; + for (size_t idx : it->second) + { + float dx = undistKpts[idx].x - x; + float dy = undistKpts[idx].y - y; + if (dx * dx + dy * dy <= r2) + result.push_back(idx); + } + } + return result; + } +}; + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_FRAME_HPP diff --git a/modules/slam/src/odometry/visual_odometry.cpp b/modules/slam/src/odometry/visual_odometry.cpp new file mode 100644 index 0000000000..0e65dfdaa5 --- /dev/null +++ b/modules/slam/src/odometry/visual_odometry.cpp @@ -0,0 +1,461 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +#include +#include + +namespace cv { +namespace slam { + +namespace { + +const char* stateName(OdometryState s) +{ + switch (s) + { + case NOT_INITIALIZED: return "NOT_INITIALIZED"; + case INITIALIZING: return "INITIALIZING"; + case TRACKING: return "TRACKING"; + } + return "NOT_INITIALIZED"; +} + +String joinPath(const String& dir, const String& name) +{ + if (dir.empty()) return name; + char last = dir.back(); + if (last == '/' || last == '\\') return dir + name; + return dir + "/" + name; +} + +} // anonymous namespace + +// Factory + +VisualOdometry::VisualOdometry() = default; +VisualOdometry::~VisualOdometry() = default; + +Ptr VisualOdometry::create( + const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + InputArray cameraMatrix, + InputArray distCoeffs, + const OdometryParams& params) +{ + CV_Assert(detector && "VisualOdometry::create: detector must not be null"); + CV_Assert(matcher && "VisualOdometry::create: matcher must not be null"); + + Mat K = cameraMatrix.getMat(); + CV_Assert(!K.empty() && K.rows == 3 && K.cols == 3); + Mat dist = distCoeffs.empty() ? Mat() : distCoeffs.getMat(); + + return makePtr( + detector, matcher, imagesFolder, outputFolder, K, dist, params); +} + +// Constructor + +VisualOdometryImpl::VisualOdometryImpl( + const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + const Mat& cameraMatrix, + const Mat& distCoeffs, + const OdometryParams& params) + : detector(detector), matcher(matcher), params(params), + imagesFolder(imagesFolder), outputFolder(outputFolder) +{ + cameraMatrix.convertTo(K, CV_64F); + if (!distCoeffs.empty()) + distCoeffs.convertTo(dist, CV_64F); +} + +// reset / processFrame + +void VisualOdometryImpl::reset() +{ + state = NOT_INITIALIZED; + lastPoseCw = Matx44d::eye(); + refFrame = Frame(); + lastKf = nullptr; + framesSinceKf = 0; + lastKfInliers = 0; + velocity = Matx44d::eye(); + hasVelocity = false; + prevFrame = Frame(); + hasPrevFrame = false; + lastEvent.clear(); + poseFilenames.clear(); + map.clear(); +} + +bool VisualOdometryImpl::processFrame(InputArray image) +{ + CV_INSTRUMENT_REGION(); + + if (image.empty()) return false; + lastEvent.clear(); + + Frame cur; + extractFeatures(image, cur); + if (cur.keypoints.empty() || cur.descriptors.empty()) return false; + + cur.mapPoints.assign(cur.keypoints.size(), nullptr); + cur.outliers.assign(cur.keypoints.size(), false); + cur.buildGrid(); + + switch (state) + { + case NOT_INITIALIZED: + refFrame = cur; + state = INITIALIZING; + return false; + + case INITIALIZING: + return bootstrap(cur); + + case TRACKING: + return track(cur); + } + return false; +} + +// Feature extraction + +void VisualOdometryImpl::extractFeatures(InputArray image, Frame& out) const +{ + Mat img = image.getMat(); + out.imageSize = img.size(); + out.keypoints.clear(); + + // Detect and compute on the original image (color/grey is up to the detector). + detector->detectAndCompute(img, noArray(), out.keypoints, out.descriptors); + + // Store a greyscale copy for the optical-flow fallback. + if (img.channels() > 1) + cvtColor(img, out.image, COLOR_BGR2GRAY); + else + out.image = img.clone(); + + // Pre-compute undistorted pixel coordinates used by every stage. + if (!out.keypoints.empty()) + { + std::vector raw; + raw.reserve(out.keypoints.size()); + for (const auto& kp : out.keypoints) + raw.push_back(kp.pt); + + if (!dist.empty()) + undistortPoints(raw, out.undistKpts, K, dist, noArray(), K); + else + out.undistKpts = raw; + } +} + +// Frame matching helper + +void VisualOdometryImpl::matchFrames( + const std::vector& qKp, const Mat& qDesc, Size qSz, + const std::vector& tKp, const Mat& tDesc, Size tSz, + std::vector& matches) const +{ + matches.clear(); + if (qDesc.empty() || tDesc.empty()) return; + if (qKp.empty() || tKp.empty()) return; + + LightGlueMatcher* lg = dynamic_cast(matcher.get()); + if (lg) + { + Mat qk((int)qKp.size(), 2, CV_32F); + for (size_t i = 0; i < qKp.size(); ++i) + { qk.at((int)i,0) = qKp[i].pt.x; qk.at((int)i,1) = qKp[i].pt.y; } + + Mat tk((int)tKp.size(), 2, CV_32F); + for (size_t i = 0; i < tKp.size(); ++i) + { tk.at((int)i,0) = tKp[i].pt.x; tk.at((int)i,1) = tKp[i].pt.y; } + + lg->setPairInfo(qk, tk, qSz, tSz); + } + + matcher->match(qDesc, tDesc, matches); +} + +// Batch run() + +bool VisualOdometryImpl::run() +{ + CV_INSTRUMENT_REGION(); + + if (imagesFolder.empty()) + { + CV_LOG_ERROR(NULL, "VisualOdometry::run: imagesFolder is empty"); + return false; + } + + std::vector allFiles; + try { cv::glob(imagesFolder, allFiles, false); } + catch (const cv::Exception& e) + { + CV_LOG_ERROR(NULL, "VisualOdometry::run: glob failed: " << e.what()); + return false; + } + + std::vector imgFiles; + imgFiles.reserve(allFiles.size()); + for (const auto& f : allFiles) + if (cv::haveImageReader(f)) imgFiles.push_back(f); + std::sort(imgFiles.begin(), imgFiles.end()); + + if (imgFiles.empty()) + { + CV_LOG_WARNING(NULL, "VisualOdometry::run: no images in " << imagesFolder); + return false; + } + + std::ofstream log; + if (!outputFolder.empty()) + { + cv::utils::fs::createDirectories(outputFolder); + log.open(joinPath(outputFolder, "vo.log").c_str()); + } + auto logln = [&](const String& s) { if (log.is_open()) log << s << "\n"; }; + + logln("[INFO] optimizer = reprojection inlier check"); + logln(String("[INFO] images_folder = ") + imagesFolder); + logln(String("[INFO] output_folder = ") + outputFolder); + { + std::ostringstream ss; + ss << "[INFO] found " << imgFiles.size() << " image(s)"; + logln(ss.str()); + } + + reset(); + + int nEmitted = 0; + size_t prevTrajLen = 0; + String refFilename; + + for (size_t i = 0; i < imgFiles.size(); ++i) + { + Mat img = imread(imgFiles[i]); + if (img.empty()) + { + std::ostringstream ss; + ss << "[FRAME " << i << "] file=" << imgFiles[i] << " ERROR: imread failed"; + logln(ss.str()); continue; + } + + OdometryState before = state; + bool emitted = processFrame(img); + OdometryState after = state; + if (emitted) ++nEmitted; + + // Track which input image maps to each trajectory pose. + if (before == NOT_INITIALIZED || + (before == TRACKING && after == INITIALIZING)) + refFilename = imgFiles[i]; + + const size_t added = map.trajectory().size() - prevTrajLen; + if (added == 1) + poseFilenames.push_back(imgFiles[i]); + else if (added == 2) + { + poseFilenames.push_back(refFilename); + poseFilenames.push_back(imgFiles[i]); + } + prevTrajLen = map.trajectory().size(); + + std::ostringstream ss; + ss << "[FRAME " << i << "] file=" << imgFiles[i] + << " state=" << stateName(before); + if (before != after) ss << "->" << stateName(after); + ss << " emitted=" << (emitted ? "yes" : "no") + << " keyframes=" << map.numKeyframes() + << " map_points=" << map.numMapPoints(); + if (!lastEvent.empty()) ss << " [" << lastEvent << "]"; + if (emitted) + { + Point3d C = detail::cameraCenterWorld(lastPoseCw); + ss << " C=(" << C.x << "," << C.y << "," << C.z << ")"; + } + logln(ss.str()); + } + + if (!outputFolder.empty()) + { + writeTrajectoryText(joinPath(outputFolder, "trajectory.txt")); + writeTrajectoryBin (joinPath(outputFolder, "trajectory.bin")); + writeMapPoints (joinPath(outputFolder, "map_points.txt")); + writeKeypoints (joinPath(outputFolder, "keypoints.txt")); + writeImagesTxt (joinPath(outputFolder, "images.txt")); + + std::ostringstream ss; + ss << "[INFO] run complete: frames=" << imgFiles.size() + << " emitted=" << nEmitted + << " keyframes=" << map.numKeyframes() + << " map_points=" << map.numMapPoints(); + logln(ss.str()); + logln("[INFO] wrote trajectory.txt, trajectory.bin, map_points.txt, " + "keypoints.txt, images.txt"); + } + + return nEmitted > 0; +} + +// IO helpers + +void VisualOdometryImpl::writeTrajectoryText(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeTrajectoryText: cannot open " << path); return; } + f << "# Per-frame camera center in world coordinates.\n# Columns: Cx Cy Cz\n"; + f.setf(std::ios::scientific); f.precision(9); + for (const auto& T : map.trajectory()) + { + Point3d C = detail::cameraCenterWorld(T); + f << C.x << " " << C.y << " " << C.z << "\n"; + } +} + +void VisualOdometryImpl::writeTrajectoryBin(const String& path) const +{ + std::ofstream f(path.c_str(), std::ios::binary); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeTrajectoryBin: cannot open " << path); return; } + const char magic[4] = {'V','O','T','R'}; + f.write(magic, 4); + int32_t version = 1; + f.write(reinterpret_cast(&version), sizeof(int32_t)); + int32_t n = (int32_t)map.trajectory().size(); + f.write(reinterpret_cast(&n), sizeof(int32_t)); + for (const auto& T : map.trajectory()) + { + double buf[16]; + for (int r = 0; r < 4; ++r) + for (int c = 0; c < 4; ++c) buf[r*4+c] = T(r,c); + f.write(reinterpret_cast(buf), sizeof(buf)); + } +} + +void VisualOdometryImpl::writeMapPoints(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeMapPoints: cannot open " << path); return; } + f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; + f.setf(std::ios::scientific); f.precision(9); + for (MapPoint* mp : map.mapPoints()) + { + if (!mp || mp->bad) continue; + f << mp->id << " " + << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " " + << mp->observations.size() << "\n"; + } +} + +void VisualOdometryImpl::writeKeypoints(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeKeypoints: cannot open " << path); return; } + f << "# Per-keyframe keypoints.\n" + << "# Block header: KF kf_id Cx Cy Cz n_keypoints\n" + << "# Followed by n_keypoints rows: kpIdx x y size angle response octave mpId\n" + << "# mpId = -1 if the keypoint has no map point.\n"; + f.setf(std::ios::fixed); f.precision(6); + + for (KeyFrame* kf : map.keyframes()) + { + if (!kf) continue; + Point3d C = detail::cameraCenterWorld(kf->poseCw); + f << "KF " << kf->id << " " << C.x << " " << C.y << " " << C.z + << " " << kf->keypoints.size() << "\n"; + for (size_t i = 0; i < kf->keypoints.size(); ++i) + { + const KeyPoint& kp = kf->keypoints[i]; + int mpId = -1; + if (i < kf->mapPoints.size() && kf->mapPoints[i]) + mpId = kf->mapPoints[i]->id; + f << i << " " << kp.pt.x << " " << kp.pt.y << " " + << kp.size << " " << kp.angle << " " << kp.response << " " + << kp.octave << " " << mpId << "\n"; + } + } +} + +// Shepperd's method: numerically-stable R → unit quaternion (qw, qx, qy, qz). +static void rotMatToQuat(const Matx33d& R, + double& qw, double& qx, double& qy, double& qz) +{ + const double tr = R(0,0) + R(1,1) + R(2,2); + if (tr > 0.0) + { + double s = std::sqrt(tr + 1.0) * 2.0; + qw = 0.25 * s; + qx = (R(2,1) - R(1,2)) / s; + qy = (R(0,2) - R(2,0)) / s; + qz = (R(1,0) - R(0,1)) / s; + } + else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) + { + double s = std::sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2.0; + qw = (R(2,1) - R(1,2)) / s; qx = 0.25 * s; + qy = (R(0,1) + R(1,0)) / s; qz = (R(0,2) + R(2,0)) / s; + } + else if (R(1,1) > R(2,2)) + { + double s = std::sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2.0; + qw = (R(0,2) - R(2,0)) / s; qx = (R(0,1) + R(1,0)) / s; + qy = 0.25 * s; qz = (R(1,2) + R(2,1)) / s; + } + else + { + double s = std::sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2.0; + qw = (R(1,0) - R(0,1)) / s; qx = (R(0,2) + R(2,0)) / s; + qy = (R(1,2) + R(2,1)) / s; qz = 0.25 * s; + } +} + +static String basenameOf(const String& path) +{ + const size_t slash = path.find_last_of("/\\"); + return (slash == String::npos) ? path : path.substr(slash + 1); +} + +void VisualOdometryImpl::writeImagesTxt(const String& path) const +{ + std::ofstream f(path.c_str()); + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeImagesTxt: cannot open " << path); return; } + + const auto& traj = map.trajectory(); + f << "# Image list with two lines of data per image:\n" + << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + << "# POINTS2D[] as (X, Y, POINT3D_ID)\n" + << "# Number of images: " << traj.size() << ", mean observations per image: 0.0\n"; + f.setf(std::ios::fixed); f.precision(6); + + for (size_t i = 0; i < traj.size(); ++i) + { + const Matx44d& T = traj[i]; + Matx33d R; + for (int r = 0; r < 3; ++r) + for (int c = 0; c < 3; ++c) R(r,c) = T(r,c); + double qw, qx, qy, qz; + rotMatToQuat(R, qw, qx, qy, qz); + + const String name = (i < poseFilenames.size()) + ? basenameOf(poseFilenames[i]) + : (String("pose_") + std::to_string(i)); + + f << i << " " << qw << " " << qx << " " << qy << " " << qz << " " + << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n\n"; + } +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_bootstrap.cpp b/modules/slam/src/odometry/vo_bootstrap.cpp new file mode 100644 index 0000000000..a4b6afe4e8 --- /dev/null +++ b/modules/slam/src/odometry/vo_bootstrap.cpp @@ -0,0 +1,252 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +// Returns the homography candidate index with the most cheirality-consistent triangulations. +int bestHomographyCandidate( + const std::vector& Rs, + const std::vector& ts, + const std::vector& pts1, + const std::vector& pts2, + const Mat& P1) +{ + int bestIdx = 0; + int bestCount = -1; + const int nSol = (int)Rs.size(); + + for (int s = 0; s < nSol; ++s) + { + Mat Rt(3, 4, CV_64F); + Rs[s].copyTo(Rt(Rect(0, 0, 3, 3))); + ts[s].reshape(1, 3).copyTo(Rt(Rect(3, 0, 1, 3))); + + Mat Klocal = P1(Rect(0, 0, 3, 3)).clone(); + Mat P2_s = Klocal * Rt; + + Mat pts4D; + triangulatePoints(P1, P2_s, pts1, pts2, pts4D); + + int count = 0; + for (int i = 0; i < pts4D.cols; ++i) + { + float w = pts4D.at(3, i); + if (std::abs(w) < 1e-9f) continue; + float X = pts4D.at(0, i) / w; + float Y = pts4D.at(1, i) / w; + float Z = pts4D.at(2, i) / w; + float Z2 = (float)(Rs[s].at(2,0)*X + Rs[s].at(2,1)*Y + + Rs[s].at(2,2)*Z + + ts[s].reshape(1,3).at(2,0)); + if (Z > 0 && Z2 > 0) ++count; + } + if (count > bestCount) { bestCount = count; bestIdx = s; } + } + return bestIdx; +} + +} // anonymous namespace + +bool VisualOdometryImpl::bootstrap(Frame& cur) +{ + std::vector matches; + matchFrames(refFrame.keypoints, refFrame.descriptors, refFrame.imageSize, + cur.keypoints, cur.descriptors, cur.imageSize, matches); + + if ((int)matches.size() < params.minInitInliers) + { + // slide ref forward so we don't keep trying against a frame that's too far + refFrame = std::move(cur); + return false; + } + + std::vector refU, curU; + refU.reserve(matches.size()); + curU.reserve(matches.size()); + for (const auto& m : matches) + { + refU.push_back(refFrame.undistKpts[m.queryIdx]); + curU.push_back(cur.undistKpts[m.trainIdx]); + } + + Mat maskE, maskH; + Mat E = findEssentialMat(refU, curU, K, RANSAC, + params.essentialRansacConfidence, + params.essentialRansacThresh, 1000, maskE); + Mat H = findHomography(refU, curU, RANSAC, + params.essentialRansacThresh, maskH); + + const int nE = (!E.empty() && !maskE.empty()) ? countNonZero(maskE) : 0; + const int nH = (!H.empty() && !maskH.empty()) ? countNonZero(maskH) : 0; + + if (nE < params.minInitInliers && nH < params.minInitInliers) + return false; + + const double RH = (double)nH / ((double)nH + (double)nE + 1e-9); + + Mat R, t, modelMask; + Mat P1 = K * Mat::eye(3, 4, CV_64F); + + if (RH > params.hfRatioThresh && !H.empty()) + { + std::vector Rs, ts, normals; + int nSol = decomposeHomographyMat(H, K, Rs, ts, normals); + if (nSol <= 0) return false; + + std::vector p1In, p2In; + for (size_t i = 0; i < matches.size(); ++i) + if (!maskH.empty() && maskH.at((int)i)) + { p1In.push_back(refU[i]); p2In.push_back(curU[i]); } + if (p1In.empty()) return false; + + int best = bestHomographyCandidate(Rs, ts, p1In, p2In, P1); + R = Rs[best].clone(); + t = ts[best].clone(); + modelMask = maskH; + } + else + { + if (E.empty() || nE < params.minInitInliers) return false; + Mat recoverMask = maskE.clone(); + int nPose = recoverPose(E, refU, curU, K, R, t, recoverMask); + if (nPose < params.minInitInliers) return false; + modelMask = recoverMask; + } + + std::vector refIn, curIn; + std::vector matchIn; + for (size_t i = 0; i < matches.size(); ++i) + if (!modelMask.empty() && modelMask.at((int)i)) + { + refIn.push_back(refU[i]); + curIn.push_back(curU[i]); + matchIn.push_back((int)i); + } + + if ((int)refIn.size() < params.minInitInliers) + return false; + + Mat Rt(3, 4, CV_64F); + R.copyTo(Rt(Rect(0, 0, 3, 3))); + t.reshape(1, 3).copyTo(Rt(Rect(3, 0, 1, 3))); + Mat P2 = K * Rt; + + Mat pts4D; + triangulatePoints(P1, P2, refIn, curIn, pts4D); + + Matx44d T_ref = Matx44d::eye(); + Matx44d T_cur = detail::makePose(R, t); + + std::vector goodPts; + std::vector goodMatch; + int nValid = 0; + int nPos = 0; + + for (int i = 0; i < pts4D.cols; ++i) + { + double w = pts4D.at(3, i); + if (std::abs(w) < 1e-9) continue; + ++nValid; + + double X = pts4D.at(0, i) / w; + double Y = pts4D.at(1, i) / w; + double Z = pts4D.at(2, i) / w; + + if (Z <= 0) continue; + double Z2 = R.at(2,0)*X + R.at(2,1)*Y + + R.at(2,2)*Z + t.reshape(1,3).at(2,0); + if (Z2 <= 0) continue; + ++nPos; + + Point3d Xw(X, Y, Z); + if (detail::parallaxDeg(Xw, T_ref, T_cur) < params.minInitParallaxDeg) + continue; + + goodPts.push_back(Xw); + goodMatch.push_back(matchIn[i]); + } + + // reject degenerate decompositions (< 90% positive-depth ratio) + if (nValid > 0 && (double)nPos / nValid < 0.9) + return false; + if ((int)goodPts.size() < params.minInitPoints) + return false; + + // normalize scale: set median scene depth to 1.0 + std::vector depths; + depths.reserve(goodPts.size()); + for (const auto& p : goodPts) depths.push_back(p.z); + std::nth_element(depths.begin(), depths.begin() + depths.size()/2, depths.end()); + double med = depths[depths.size()/2]; + if (med < 1e-9) return false; + + double scale = 1.0 / med; + for (auto& p : goodPts) { p.x *= scale; p.y *= scale; p.z *= scale; } + + Mat tSc; + t.reshape(1, 3).convertTo(tSc, CV_64F); + tSc = tSc * scale; + T_cur = detail::makePose(R, tSc); + + auto makeKF = [](const Frame& f) -> KeyFrame* { + KeyFrame* kf = new KeyFrame(); + kf->poseCw = Matx44d::eye(); + kf->keypoints = f.keypoints; + kf->descriptors = f.descriptors.clone(); + kf->undistKpts = f.undistKpts; + kf->imageSize = f.imageSize; + kf->mapPoints.assign(f.keypoints.size(), nullptr); + return kf; + }; + + KeyFrame* kfRef = makeKF(refFrame); + kfRef->poseCw = T_ref; + + KeyFrame* kfCur = makeKF(cur); + kfCur->poseCw = T_cur; + kfCur->parent = kfRef; + + map.addKeyframe(kfRef); + map.addKeyframe(kfCur); + + for (size_t i = 0; i < goodPts.size(); ++i) + { + MapPoint* mp = new MapPoint(); + mp->pos = goodPts[i]; + const DMatch& m = matches[goodMatch[i]]; + mp->refDesc = refFrame.descriptors.row(m.queryIdx).clone(); + + map.addMapPoint(mp); + map.addObservation(kfRef, (size_t)m.queryIdx, mp); + map.addObservation(kfCur, (size_t)m.trainIdx, mp); + } + + detail::updateCovisibility(kfRef); + + map.setRefKeyframe(kfRef); + map.setCurrentKeyframe(kfCur); + + lastKf = kfCur; + framesSinceKf = 0; + lastKfInliers = (int)goodPts.size(); + lastPoseCw = T_cur; + state = TRACKING; + hasVelocity = false; + + map.appendPose(T_ref); + map.appendPose(T_cur); + + refFrame = Frame(); + return true; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_impl.hpp b/modules/slam/src/odometry/vo_impl.hpp new file mode 100644 index 0000000000..deb673fe0d --- /dev/null +++ b/modules/slam/src/odometry/vo_impl.hpp @@ -0,0 +1,129 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_VO_IMPL_HPP +#define OPENCV_SLAM_VO_IMPL_HPP + +#include "../precomp.hpp" +#include "frame.hpp" +#include "../optimizer/pose_optimizer.hpp" + +#include + +namespace cv { +namespace slam { + +/** @brief Concrete VisualOdometry implementation (pimpl target). + +Stage logic is split across: + - vo_bootstrap.cpp : two-view H/F initialisation + - vo_tracking.cpp : per-frame localisation (motion model, fallback 1/2, local map) + - vo_keyframe.cpp : keyframe promotion decision + covisibility helpers + - vo_map_growth.cpp : triangulation of new map points at promotion time + - visual_odometry.cpp : factory, run(), processFrame(), IO writers +*/ +class VisualOdometryImpl CV_FINAL : public VisualOdometry +{ +public: + VisualOdometryImpl(const Ptr& detector, + const Ptr& matcher, + const String& imagesFolder, + const String& outputFolder, + const Mat& cameraMatrix, + const Mat& distCoeffs, + const OdometryParams& params); + + // --- VisualOdometry interface ------------------------------------------- + + bool run() CV_OVERRIDE; + bool processFrame(InputArray image) CV_OVERRIDE; + void reset() CV_OVERRIDE; + + OdometryState getState() const CV_OVERRIDE { return state; } + Matx44d getLastPose() const CV_OVERRIDE { return lastPoseCw; } + const Map& getMap() const CV_OVERRIDE { return map; } + const std::vector& getTrajectory() const CV_OVERRIDE { return map.trajectory(); } + const OdometryParams& getParams() const CV_OVERRIDE { return params; } + void setParams(const OdometryParams& p) CV_OVERRIDE { params = p; } + + const String& getImagesFolder() const CV_OVERRIDE { return imagesFolder; } + const String& getOutputFolder() const CV_OVERRIDE { return outputFolder; } + void setOutputFolder(const String& f) CV_OVERRIDE { outputFolder = f; } + + // --- Stage entry points ------------------------------------------------- + + bool bootstrap(Frame& cur); + bool track(Frame& cur); + + bool trackWithMotionModel(Frame& cur); // motion model + bool trackWithReferenceKF(Frame& cur); // fallback 1 + bool trackWithOpticalFlow(Frame& cur); // fallback 2 + void trackLocalMap(Frame& cur); + + bool shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw, String& reason) const; + void promoteKeyframeAndGrowMap(Frame& cur); + + // --- Shared helpers (visual_odometry.cpp) -------------------------------- + + void extractFeatures(InputArray image, Frame& out) const; + + void matchFrames(const std::vector& qKp, const Mat& qDesc, Size qSz, + const std::vector& tKp, const Mat& tDesc, Size tSz, + std::vector& matches) const; + + // --- IO helpers (visual_odometry.cpp) ------------------------------------ + + void writeTrajectoryText(const String& path) const; + void writeTrajectoryBin(const String& path) const; + void writeMapPoints(const String& path) const; + void writeKeypoints(const String& path) const; + void writeImagesTxt(const String& path) const; + + // --- Owned state --------------------------------------------------------- + + Ptr detector; + Ptr matcher; + Mat K; // 3×3 CV_64F + Mat dist; // distortion coefficients (may be empty) + OdometryParams params; + + String imagesFolder; + String outputFolder; + + OdometryState state = NOT_INITIALIZED; + Matx44d lastPoseCw = Matx44d::eye(); + + Frame refFrame; + KeyFrame* lastKf = nullptr; + int framesSinceKf = 0; + int lastKfInliers = 0; + + Matx44d velocity = Matx44d::eye(); + bool hasVelocity = false; + + Frame prevFrame; + bool hasPrevFrame = false; + + String lastEvent; + std::vector poseFilenames; + + Map map; +}; + +namespace detail { + +double rotationAngleDeg(const Matx44d& A_cw, const Matx44d& B_cw); +double parallaxDeg(const Point3d& X_world, const Matx44d& A_cw, const Matx44d& B_cw); +Matx34d projectionFromPose(const Matx44d& T_cw); +Matx44d makePose(const Mat& R, const Mat& t); +Point3d cameraCenterWorld(const Matx44d& T_cw); +void updateCovisibility(KeyFrame* kf); + +} // namespace detail + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_VO_IMPL_HPP diff --git a/modules/slam/src/odometry/vo_keyframe.cpp b/modules/slam/src/odometry/vo_keyframe.cpp new file mode 100644 index 0000000000..8f6b0c29ee --- /dev/null +++ b/modules/slam/src/odometry/vo_keyframe.cpp @@ -0,0 +1,145 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace detail { + +double rotationAngleDeg(const Matx44d& A, const Matx44d& B) +{ + Matx33d Ra(A(0,0),A(0,1),A(0,2), A(1,0),A(1,1),A(1,2), A(2,0),A(2,1),A(2,2)); + Matx33d Rb(B(0,0),B(0,1),B(0,2), B(1,0),B(1,1),B(1,2), B(2,0),B(2,1),B(2,2)); + Matx33d D = Rb * Ra.t(); + double tr = D(0,0) + D(1,1) + D(2,2); + double c = std::max(-1.0, std::min(1.0, (tr - 1.0) * 0.5)); + return std::acos(c) * 180.0 / CV_PI; +} + +Point3d cameraCenterWorld(const Matx44d& T_cw) +{ + Matx33d R(T_cw(0,0),T_cw(0,1),T_cw(0,2), + T_cw(1,0),T_cw(1,1),T_cw(1,2), + T_cw(2,0),T_cw(2,1),T_cw(2,2)); + Matx31d t(T_cw(0,3),T_cw(1,3),T_cw(2,3)); + Matx31d C = -R.t() * t; + return Point3d(C(0),C(1),C(2)); +} + +double parallaxDeg(const Point3d& Xw, const Matx44d& A_cw, const Matx44d& B_cw) +{ + Point3d CA = cameraCenterWorld(A_cw), CB = cameraCenterWorld(B_cw); + Point3d vA = Xw - CA, vB = Xw - CB; + double nA = std::sqrt(vA.dot(vA)), nB = std::sqrt(vB.dot(vB)); + if (nA < 1e-9 || nB < 1e-9) return 0.0; + double c = std::max(-1.0, std::min(1.0, vA.dot(vB) / (nA * nB))); + return std::acos(c) * 180.0 / CV_PI; +} + +Matx34d projectionFromPose(const Matx44d& T) +{ + return Matx34d(T(0,0),T(0,1),T(0,2),T(0,3), + T(1,0),T(1,1),T(1,2),T(1,3), + T(2,0),T(2,1),T(2,2),T(2,3)); +} + +Matx44d makePose(const Mat& R, const Mat& t) +{ + CV_Assert(R.rows == 3 && R.cols == 3 && t.total() == 3); + Matx44d T = Matx44d::eye(); + Mat Rd, td; + R.convertTo(Rd, CV_64F); + t.reshape(1,3).convertTo(td, CV_64F); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) T(i,j) = Rd.at(i,j); + T(i,3) = td.at(i,0); + } + return T; +} + +static void rebuildOrderedCovisibility(KeyFrame* target) +{ + target->orderedCovisibility.clear(); + for (auto& [other, cnt] : target->covisibility) + target->orderedCovisibility.push_back({other, cnt}); + std::sort(target->orderedCovisibility.begin(), + target->orderedCovisibility.end(), + [](const auto& a, const auto& b){ return a.second > b.second; }); +} + +void updateCovisibility(KeyFrame* kf) +{ + if (!kf) return; + + kf->covisibility.clear(); + + for (MapPoint* mp : kf->mapPoints) + { + if (!mp || mp->bad) continue; + for (auto& [obsKf, kpIdx] : mp->observations) + { + if (obsKf == kf) continue; + kf->covisibility[obsKf]++; + obsKf->covisibility[kf]++; + } + } + + rebuildOrderedCovisibility(kf); + for (auto& [nb, cnt] : kf->covisibility) + rebuildOrderedCovisibility(nb); +} + +} // namespace detail + +bool VisualOdometryImpl::shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw, + String& reason) const +{ + if (framesSinceKf <= params.kfMinFrames) return false; + + if (framesSinceKf > params.kfMaxFrames) + { + reason = format("timeout(%d)", framesSinceKf); + return true; + } + + if (!lastKf) return false; + + // Motion-based triggers: rotation then translation (geometric, frame-count independent) + double rot = detail::rotationAngleDeg(lastKf->poseCw, T_cw); + if (rot > params.kfRotThreshDeg) + { + reason = format("rot=%.1fdeg", rot); + return true; + } + + // Tracking quality drop + int minInliers = (lastKfInliers > 0) + ? std::max(params.kfMinInliers, (int)(params.kfInlierRatio * lastKfInliers)) + : params.kfMinInliers; + if (nInliers < minInliers) + { + reason = format("inliers=%d<%d", nInliers, minInliers); + return true; + } + + Point3d cCur = detail::cameraCenterWorld(T_cw); + Point3d cLast = detail::cameraCenterWorld(lastKf->poseCw); + Point3d d = cCur - cLast; + double dist = std::sqrt(d.dot(d)); + if (dist > params.kfTransThresh) + { + reason = format("trans=%.3f", dist); + return true; + } + + return false; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_map_growth.cpp b/modules/slam/src/odometry/vo_map_growth.cpp new file mode 100644 index 0000000000..6e6dfbc590 --- /dev/null +++ b/modules/slam/src/odometry/vo_map_growth.cpp @@ -0,0 +1,135 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +inline Point2d projectThrough(const Mat& P, double X, double Y, double Z) +{ + double u = P.at(0,0)*X + P.at(0,1)*Y + P.at(0,2)*Z + P.at(0,3); + double v = P.at(1,0)*X + P.at(1,1)*Y + P.at(1,2)*Z + P.at(1,3); + double w = P.at(2,0)*X + P.at(2,1)*Y + P.at(2,2)*Z + P.at(2,3); + if (std::abs(w) < 1e-12) return Point2d(0, 0); + return Point2d(u / w, v / w); +} + +inline double cameraDepth(const Matx44d& T_cw, double X, double Y, double Z) +{ + return T_cw(2,0)*X + T_cw(2,1)*Y + T_cw(2,2)*Z + T_cw(2,3); +} + +} // anonymous namespace + +void VisualOdometryImpl::promoteKeyframeAndGrowMap(Frame& cur) +{ + KeyFrame* newKf = new KeyFrame(); + newKf->poseCw = cur.poseCw; + newKf->keypoints = cur.keypoints; + newKf->descriptors = cur.descriptors.clone(); + newKf->undistKpts = cur.undistKpts; + newKf->imageSize = cur.imageSize; + newKf->mapPoints.assign(cur.keypoints.size(), nullptr); + newKf->parent = lastKf; + + map.addKeyframe(newKf); + + for (size_t i = 0; i < cur.mapPoints.size(); ++i) + { + MapPoint* mp = cur.mapPoints[i]; + if (!mp || mp->bad || cur.outliers[i]) continue; + map.addObservation(newKf, i, mp); + } + + std::vector kfToCur; + matchFrames(lastKf->keypoints, lastKf->descriptors, lastKf->imageSize, + cur.keypoints, cur.descriptors, cur.imageSize, kfToCur); + + Mat Rt1(3, 4, CV_64F), Rt2(3, 4, CV_64F); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 4; ++j) + { + Rt1.at(i,j) = lastKf->poseCw(i,j); + Rt2.at(i,j) = cur.poseCw(i,j); + } + Mat P1 = K * Rt1; + Mat P2 = K * Rt2; + + std::vector pts1, pts2; + std::vector triMatchIdx; + for (size_t i = 0; i < kfToCur.size(); ++i) + { + const DMatch& m = kfToCur[i]; + if ((size_t)m.queryIdx >= lastKf->mapPoints.size()) continue; + if (lastKf->mapPoints[m.queryIdx] != nullptr) continue; + if ((size_t)m.trainIdx >= newKf->mapPoints.size()) continue; + if (newKf->mapPoints[m.trainIdx] != nullptr) continue; + pts1.push_back(lastKf->undistKpts[m.queryIdx]); + pts2.push_back(cur.undistKpts[m.trainIdx]); + triMatchIdx.push_back((int)i); + } + + if (pts1.empty()) + { + detail::updateCovisibility(newKf); + map.setCurrentKeyframe(newKf); + lastKf = newKf; + lastKfInliers = 0; + framesSinceKf = 0; + return; + } + + Mat pts4D; + triangulatePoints(P1, P2, pts1, pts2, pts4D); + + int nNew = 0; + for (int i = 0; i < pts4D.cols; ++i) + { + double w = pts4D.at(3, i); + if (std::abs(w) < 1e-9) continue; + double X = pts4D.at(0, i) / w; + double Y = pts4D.at(1, i) / w; + double Z = pts4D.at(2, i) / w; + + if (cameraDepth(lastKf->poseCw, X, Y, Z) <= 0) continue; + if (cameraDepth(cur.poseCw, X, Y, Z) <= 0) continue; + + Point2d p1p = projectThrough(P1, X, Y, Z); + Point2d p2p = projectThrough(P2, X, Y, Z); + double e1 = std::hypot(p1p.x - pts1[i].x, p1p.y - pts1[i].y); + double e2 = std::hypot(p2p.x - pts2[i].x, p2p.y - pts2[i].y); + if (e1 > params.pnpReprojThresh || e2 > params.pnpReprojThresh) continue; + + Point3d Xw(X, Y, Z); + if (detail::parallaxDeg(Xw, lastKf->poseCw, cur.poseCw) + < params.minGrowthParallaxDeg) continue; + + MapPoint* mp = new MapPoint(); + mp->pos = Xw; + const DMatch& dm = kfToCur[triMatchIdx[i]]; + mp->refDesc = cur.descriptors.row(dm.trainIdx).clone(); + + map.addMapPoint(mp); + map.addObservation(lastKf, (size_t)dm.queryIdx, mp); + map.addObservation(newKf, (size_t)dm.trainIdx, mp); + ++nNew; + } + + detail::updateCovisibility(newKf); + + map.setCurrentKeyframe(newKf); + lastKf = newKf; + lastKfInliers = 0; + framesSinceKf = 0; + + (void)nNew; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/odometry/vo_tracking.cpp b/modules/slam/src/odometry/vo_tracking.cpp new file mode 100644 index 0000000000..f27147033a --- /dev/null +++ b/modules/slam/src/odometry/vo_tracking.cpp @@ -0,0 +1,390 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +bool projectPoint(const Matx44d& T_cw, const Mat& K, + double Xw, double Yw, double Zw, + double& u, double& v) +{ + const double Xc = T_cw(0,0)*Xw + T_cw(0,1)*Yw + T_cw(0,2)*Zw + T_cw(0,3); + const double Yc = T_cw(1,0)*Xw + T_cw(1,1)*Yw + T_cw(1,2)*Zw + T_cw(1,3); + const double Zc = T_cw(2,0)*Xw + T_cw(2,1)*Yw + T_cw(2,2)*Zw + T_cw(2,3); + if (Zc <= 0.0) return false; + u = K.at(0,0) * Xc / Zc + K.at(0,2); + v = K.at(1,1) * Yc / Zc + K.at(1,2); + return true; +} + +inline double descDist(const Mat& a, const Mat& b) +{ + if (a.empty() || b.empty()) return std::numeric_limits::max(); + int normType = (a.type() == CV_8U) ? NORM_HAMMING : NORM_L2; + return norm(a, b, normType); +} + +void buildLocalMapPoints(const KeyFrame* kf, int topK, + std::set& localMps) +{ + if (!kf) return; + for (MapPoint* mp : kf->mapPoints) + if (mp && !mp->bad) localMps.insert(mp); + + int k = 0; + for (const auto& [nbKf, cnt] : kf->orderedCovisibility) + { + if (k++ >= topK) break; + for (MapPoint* mp : nbKf->mapPoints) + if (mp && !mp->bad) localMps.insert(mp); + } +} + +int runPnP(const std::vector& obj, const std::vector& img, + Frame& frame, + const Mat& K, double reprojThresh, int maxIters, double confidence, + int minInliers) +{ + if ((int)obj.size() < minInliers) return -1; + + Mat rvec, tvec, inlierIdx; + bool ok = solvePnPRansac(obj, img, K, Mat(), + rvec, tvec, false, + maxIters, (float)reprojThresh, confidence, + inlierIdx, SOLVEPNP_AP3P); + if (!ok || inlierIdx.rows < minInliers) return -1; + + std::vector objIn; + std::vector imgIn; + objIn.reserve(inlierIdx.rows); imgIn.reserve(inlierIdx.rows); + for (int i = 0; i < inlierIdx.rows; ++i) + { + int idx = inlierIdx.at(i); + objIn.push_back(obj[idx]); + imgIn.push_back(img[idx]); + } + solvePnPRefineLM(objIn, imgIn, K, Mat(), rvec, tvec); + + Mat R; + Rodrigues(rvec, R); + frame.poseCw = detail::makePose(R, tvec); + return inlierIdx.rows; +} + +} // anonymous namespace + +// Motion model: constant-velocity pose prediction + projected map point search +bool VisualOdometryImpl::trackWithMotionModel(Frame& cur) +{ + if (!lastKf) return false; + + cur.poseCw = velocity * lastPoseCw; + + std::set localMps; + buildLocalMapPoints(lastKf, params.localMapTopK, localMps); + + std::fill(cur.mapPoints.begin(), cur.mapPoints.end(), nullptr); + std::fill(cur.outliers.begin(), cur.outliers.end(), false); + + auto doSearch = [&](float radius) -> int { + int n = 0; + for (MapPoint* mp : localMps) + { + double u, v; + if (!projectPoint(cur.poseCw, K, mp->pos.x, mp->pos.y, mp->pos.z, u, v)) + continue; + if (u < 0 || u >= cur.imageSize.width || + v < 0 || v >= cur.imageSize.height) continue; + + mp->visibleCount++; + + auto cands = cur.getKeypointsInRadius((float)u, (float)v, radius); + double bestD = std::numeric_limits::max(); + size_t bestI = std::numeric_limits::max(); + for (size_t idx : cands) + { + if (cur.mapPoints[idx]) continue; + double d = descDist(mp->refDesc, cur.descriptors.row((int)idx)); + if (d < bestD) { bestD = d; bestI = idx; } + } + if (bestI != std::numeric_limits::max() && + bestD < params.descProjThresh) + { + cur.mapPoints[bestI] = mp; + mp->foundCount++; + ++n; + } + } + return n; + }; + + int n = doSearch((float)params.motionModelRadius); + if (n < params.motionModelMinMatches) + { + std::fill(cur.mapPoints.begin(), cur.mapPoints.end(), nullptr); + n = doSearch((float)params.motionModelRadiusWide); + } + + if (n < params.pnpMinInliers) return false; + + std::vector obj; std::vector img; + obj.reserve(n); img.reserve(n); + for (size_t i = 0; i < cur.mapPoints.size(); ++i) + { + if (!cur.mapPoints[i]) continue; + const MapPoint* mp = cur.mapPoints[i]; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(cur.undistKpts[i]); + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, + params.pnpRansacIters, + params.pnpConfidence, + params.pnpMinInliers); + if (nInliers < 0) return false; + + int nOpt = poseInlierCheck(cur, K, params.pnpReprojThresh); + return nOpt >= params.pnpMinInliers; +} + +// Fallback 1: descriptor match against the reference keyframe +bool VisualOdometryImpl::trackWithReferenceKF(Frame& cur) +{ + if (!lastKf) return false; + + std::fill(cur.mapPoints.begin(), cur.mapPoints.end(), nullptr); + std::fill(cur.outliers.begin(), cur.outliers.end(), false); + + std::vector matches; + matchFrames(lastKf->keypoints, lastKf->descriptors, lastKf->imageSize, + cur.keypoints, cur.descriptors, cur.imageSize, matches); + + std::vector obj; + std::vector img; + std::vector corrMps; + std::vector corrKp; + obj.reserve(matches.size()); img.reserve(matches.size()); + corrMps.reserve(matches.size()); corrKp.reserve(matches.size()); + + for (const auto& m : matches) + { + if ((size_t)m.queryIdx >= lastKf->mapPoints.size()) continue; + MapPoint* mp = lastKf->mapPoints[m.queryIdx]; + if (!mp || mp->bad) continue; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(cur.undistKpts[m.trainIdx]); + corrMps.push_back(mp); + corrKp.push_back(m.trainIdx); + } + + if ((int)obj.size() < params.pnpMinInliers) + { + lastEvent = format("refKF: 2d3d=%d < %d", (int)obj.size(), params.pnpMinInliers); + return false; + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, params.pnpRansacIters, + params.pnpConfidence, params.pnpMinInliers); + if (nInliers < 0) + { + lastEvent = "refKF: PnP failed"; + return false; + } + + for (size_t k = 0; k < corrMps.size(); ++k) + { + int kpIdx = corrKp[k]; + if ((size_t)kpIdx < cur.mapPoints.size() && !cur.mapPoints[kpIdx]) + cur.mapPoints[kpIdx] = corrMps[k]; + } + + int nOpt = poseInlierCheck(cur, K, params.pnpReprojThresh); + return nOpt >= params.pnpMinInliers; +} + +// Fallback 2: optical flow when descriptor match also fails +bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) +{ + if (!hasPrevFrame || prevFrame.image.empty()) return false; + + const Frame& prev = prevFrame; + if (prev.undistKpts.empty()) return false; + + std::vector prevPts(prev.undistKpts.begin(), prev.undistKpts.end()); + std::vector curPts; + std::vector status; + std::vector err; + + calcOpticalFlowPyrLK(prev.image, cur.image, prevPts, curPts, + status, err, Size(21, 21), 3, + TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, 30, 0.01)); + + std::vector obj; std::vector img; + obj.reserve(prevPts.size()); img.reserve(prevPts.size()); + + for (size_t i = 0; i < prevPts.size(); ++i) + { + if (!status[i]) continue; + if (i >= prev.mapPoints.size()) continue; + MapPoint* mp = prev.mapPoints[i]; + if (!mp || mp->bad) continue; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(curPts[i]); + } + + if ((int)obj.size() < params.opticalFlowMinInliers) + { + lastEvent = format("optflow: corr=%d < %d", (int)obj.size(), params.opticalFlowMinInliers); + return false; + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, params.pnpRansacIters, + params.pnpConfidence, params.opticalFlowMinInliers); + if (nInliers < 0) + { + lastEvent = "optflow: PnP failed"; + return false; + } + + return true; +} + +// Local map refinement: expand coverage + recheck inliers +void VisualOdometryImpl::trackLocalMap(Frame& cur) +{ + if (!lastKf) return; + + std::set localMps; + buildLocalMapPoints(lastKf, params.localMapTopK, localMps); + + int nbK = 0; + for (const auto& [nbKf, cnt] : lastKf->orderedCovisibility) + { + if (nbK++ >= params.localMapTopK) break; + int nb2K = 0; + for (const auto& [nb2Kf, cnt2] : nbKf->orderedCovisibility) + { + if (nb2K++ >= params.localMapNeighborK) break; + for (MapPoint* mp : nb2Kf->mapPoints) + if (mp && !mp->bad) localMps.insert(mp); + } + } + + std::set alreadyMatched; + for (MapPoint* mp : cur.mapPoints) + if (mp) alreadyMatched.insert(mp); + + bool anyNew = false; + const float r = (float)params.localMapRadius; + + for (MapPoint* mp : localMps) + { + if (alreadyMatched.count(mp)) continue; + + double u, v; + if (!projectPoint(cur.poseCw, K, mp->pos.x, mp->pos.y, mp->pos.z, u, v)) + continue; + if (u < 0 || u >= cur.imageSize.width || + v < 0 || v >= cur.imageSize.height) continue; + + mp->visibleCount++; + + auto cands = cur.getKeypointsInRadius((float)u, (float)v, r); + double bestD = std::numeric_limits::max(); + size_t bestI = std::numeric_limits::max(); + for (size_t idx : cands) + { + if (cur.mapPoints[idx]) continue; + double d = descDist(mp->refDesc, cur.descriptors.row((int)idx)); + if (d < bestD) { bestD = d; bestI = idx; } + } + if (bestI != std::numeric_limits::max() && + bestD < params.descProjThresh) + { + cur.mapPoints[bestI] = mp; + cur.outliers[bestI] = false; + mp->foundCount++; + alreadyMatched.insert(mp); + anyNew = true; + } + } + + if (anyNew) + poseInlierCheck(cur, K, params.pnpReprojThresh); +} + +bool VisualOdometryImpl::track(Frame& cur) +{ + if (!lastKf) + { + refFrame = cur; + state = INITIALIZING; + return false; + } + + // motion model tracking + bool ok = false; + if (hasVelocity) + ok = trackWithMotionModel(cur); + + // fallback 1: descriptor match against reference keyframe + if (!ok) + ok = trackWithReferenceKF(cur); + + // fallback 2: optical flow + if (!ok) + ok = trackWithOpticalFlow(cur); + + if (!ok) + { + lastEvent = "track lost: all stages failed"; + refFrame = cur; + state = INITIALIZING; + return false; + } + + trackLocalMap(cur); + + int nInliers = 0; + for (size_t i = 0; i < cur.mapPoints.size(); ++i) + if (cur.mapPoints[i] && !cur.outliers[i]) ++nInliers; + + { + Matx44d Tcw_last_inv = lastPoseCw.inv(); + velocity = cur.poseCw * Tcw_last_inv; + hasVelocity = true; + } + + lastPoseCw = cur.poseCw; + map.appendPose(cur.poseCw); + ++framesSinceKf; + + prevFrame = cur; + hasPrevFrame = true; + + String kf_reason; + if (shouldPromoteKeyframe(nInliers, cur.poseCw, kf_reason)) + { + int mpBefore = map.numMapPoints(); + promoteKeyframeAndGrowMap(cur); + lastPoseCw = lastKf->poseCw; + lastEvent = format("keyframe: %s, +%d mp", + kf_reason.c_str(), + map.numMapPoints() - mpBefore); + } + + return true; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/optimizer/pose_optimizer.cpp b/modules/slam/src/optimizer/pose_optimizer.cpp new file mode 100644 index 0000000000..95b6a7faef --- /dev/null +++ b/modules/slam/src/optimizer/pose_optimizer.cpp @@ -0,0 +1,49 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "pose_optimizer.hpp" + +namespace cv { namespace slam { + +// pose is not optimized yet, only inlier classification is done. +// adding bundle adjustment based pose optimization in a later commit. + +int poseInlierCheck(Frame& frame, const Mat& K, double reprojThresh) +{ + const double fx = K.at(0, 0); + const double fy = K.at(1, 1); + const double cx = K.at(0, 2); + const double cy = K.at(1, 2); + const Matx44d& T = frame.poseCw; + + int nInliers = 0; + for (size_t i = 0; i < frame.mapPoints.size(); ++i) + { + frame.outliers[i] = true; + MapPoint* mp = frame.mapPoints[i]; + if (!mp || mp->bad) continue; + + const double Xc = T(0,0)*mp->pos.x + T(0,1)*mp->pos.y + T(0,2)*mp->pos.z + T(0,3); + const double Yc = T(1,0)*mp->pos.x + T(1,1)*mp->pos.y + T(1,2)*mp->pos.z + T(1,3); + const double Zc = T(2,0)*mp->pos.x + T(2,1)*mp->pos.y + T(2,2)*mp->pos.z + T(2,3); + if (Zc <= 0.0) continue; + + const double u = fx * Xc / Zc + cx; + const double v = fy * Yc / Zc + cy; + const double dx = u - static_cast(frame.undistKpts[i].x); + const double dy = v - static_cast(frame.undistKpts[i].y); + + if (std::sqrt(dx * dx + dy * dy) <= reprojThresh) + { + frame.outliers[i] = false; + ++nInliers; + } + } + return nInliers; +} + +}} // namespace cv::slam diff --git a/modules/slam/src/optimizer/pose_optimizer.hpp b/modules/slam/src/optimizer/pose_optimizer.hpp new file mode 100644 index 0000000000..69c7691284 --- /dev/null +++ b/modules/slam/src/optimizer/pose_optimizer.hpp @@ -0,0 +1,21 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_POSE_OPTIMIZER_HPP +#define OPENCV_SLAM_POSE_OPTIMIZER_HPP + +#include "../odometry/frame.hpp" + +namespace cv { +namespace slam { + +// Classify map point associations as inlier/outlier by reprojection distance. +// Returns inlier count. frame.poseCw is NOT modified. +int poseInlierCheck(Frame& frame, const Mat& K, double reprojThresh); + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_POSE_OPTIMIZER_HPP diff --git a/modules/slam/src/precomp.hpp b/modules/slam/src/precomp.hpp index ecc564bffd..ad2246c4dd 100644 --- a/modules/slam/src/precomp.hpp +++ b/modules/slam/src/precomp.hpp @@ -1,28 +1,34 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#ifndef OPENCV_SLAM_PRECOMP_HPP +#define OPENCV_SLAM_PRECOMP_HPP -#ifndef __OPENCV_SLAM_PRECOMP_HPP__ -#define __OPENCV_SLAM_PRECOMP_HPP__ - -#include -#include -#include -#include -#include -#include #include "opencv2/slam.hpp" +#include "opencv2/core.hpp" +#include "opencv2/core/utility.hpp" +#include "opencv2/core/utils/filesystem.hpp" +#include "opencv2/core/utils/logger.hpp" +#include "opencv2/core/private.hpp" +#include "opencv2/imgproc.hpp" +#include "opencv2/imgcodecs.hpp" +#include "opencv2/features.hpp" +#include "opencv2/geometry.hpp" +#include "opencv2/video/tracking.hpp" + #include #include -#include +#include +#include #include -#include +#include #include -#include -#include #include +#include #include -#endif // __OPENCV_SLAM_PRECOMP_HPP__ +#endif // OPENCV_SLAM_PRECOMP_HPP diff --git a/modules/slam/src/types.cpp b/modules/slam/test/test_main.cpp similarity index 53% rename from modules/slam/src/types.cpp rename to modules/slam/test/test_main.cpp index 80b2d0daef..1a7a912ed5 100644 --- a/modules/slam/src/types.cpp +++ b/modules/slam/test/test_main.cpp @@ -1,12 +1,9 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#include "test_precomp.hpp" -#include "precomp.hpp" - -namespace cv { namespace slam { - -OdometryParams::OdometryParams() = default; - -}} // namespace cv::slam +CV_TEST_MAIN("cv") diff --git a/modules/slam/test/test_map.cpp b/modules/slam/test/test_map.cpp new file mode 100644 index 0000000000..fdee7e9853 --- /dev/null +++ b/modules/slam/test/test_map.cpp @@ -0,0 +1,79 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +// Allocates a keyframe with @p n keypoint slots (parallel mapPoints[] zeroed), +// as the pipeline expects before any observation is wired. +static slam::KeyFrame* newKeyFrame(int n) +{ + slam::KeyFrame* kf = new slam::KeyFrame(); + kf->keypoints.resize(n); + kf->mapPoints.assign(n, nullptr); + return kf; +} + +TEST(SLAM_Map, registers_and_clears) +{ + slam::Map map; + + // addKeyframe / addMapPoint assign sequential ids and the index round-trips. + slam::KeyFrame* kf0 = map.addKeyframe(newKeyFrame(0)); + slam::KeyFrame* kf1 = map.addKeyframe(newKeyFrame(0)); + slam::MapPoint* mp0 = map.addMapPoint(new slam::MapPoint()); + EXPECT_EQ(kf0->id, 0); + EXPECT_EQ(kf1->id, 1); + EXPECT_EQ(mp0->id, 0); + EXPECT_EQ(map.numKeyframes(), 2); + EXPECT_EQ(map.numMapPoints(), 1); + EXPECT_EQ(map.getKeyframe(1), kf1); + EXPECT_EQ(map.getMapPoint(0), mp0); + EXPECT_TRUE(map.getKeyframe(99) == nullptr); + + // An explicit id must advance the auto-id counter so it cannot collide. + slam::KeyFrame* kfExplicit = new slam::KeyFrame(); + kfExplicit->id = 5; + map.addKeyframe(kfExplicit); + EXPECT_EQ(map.addKeyframe(newKeyFrame(0))->id, 6); + + // clear() drops all state and restarts the id counters. + map.clear(); + EXPECT_EQ(map.numKeyframes(), 0); + EXPECT_EQ(map.numMapPoints(), 0); + EXPECT_EQ(map.addKeyframe(newKeyFrame(0))->id, 0); +} + +TEST(SLAM_Map, wires_and_removes_observations) +{ + slam::Map map; + slam::KeyFrame* kfA = map.addKeyframe(newKeyFrame(3)); + slam::KeyFrame* kfB = map.addKeyframe(newKeyFrame(3)); + slam::MapPoint* mp = map.addMapPoint(new slam::MapPoint()); + + // addObservation links both directions: keyframe slot <-> observation map. + map.addObservation(kfA, 2, mp); + map.addObservation(kfB, 0, mp); + EXPECT_EQ(kfA->mapPoints[2], mp); + EXPECT_EQ(mp->observations[kfA], 2u); + EXPECT_EQ(mp->observations.size(), 2u); + + // Adding into an already-occupied slot is a no-op. + slam::MapPoint* other = map.addMapPoint(new slam::MapPoint()); + map.addObservation(kfA, 2, other); + EXPECT_EQ(kfA->mapPoints[2], mp); + EXPECT_EQ(other->observations.count(kfA), 0u); + + // removeMapPoint unlinks every observing keyframe and drops the point. + const int mpId = mp->id; + map.removeMapPoint(mp); // deletes mp; must not be dereferenced afterwards + EXPECT_TRUE(map.getMapPoint(mpId) == nullptr); + EXPECT_TRUE(kfA->mapPoints[2] == nullptr); + EXPECT_TRUE(kfB->mapPoints[0] == nullptr); +} + +}} // namespace opencv_test diff --git a/modules/slam/test/test_precomp.hpp b/modules/slam/test/test_precomp.hpp index 9afe8de0be..735e4f4d22 100644 --- a/modules/slam/test/test_precomp.hpp +++ b/modules/slam/test/test_precomp.hpp @@ -1,12 +1,15 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. +#ifndef OPENCV_SLAM_TEST_PRECOMP_HPP +#define OPENCV_SLAM_TEST_PRECOMP_HPP -#ifndef __OPENCV_TEST_SLAM_PRECOMP_HPP__ -#define __OPENCV_TEST_SLAM_PRECOMP_HPP__ +#include "opencv2/ts.hpp" +#include "opencv2/slam.hpp" +#include "opencv2/geometry.hpp" +#include "opencv2/features.hpp" -#include -#include - -#endif // __OPENCV_TEST_SLAM_PRECOMP_HPP__ +#endif diff --git a/modules/slam/test/test_visual_odometry.cpp b/modules/slam/test/test_visual_odometry.cpp index 4496908507..a3ea62136f 100644 --- a/modules/slam/test/test_visual_odometry.cpp +++ b/modules/slam/test/test_visual_odometry.cpp @@ -1,136 +1,251 @@ // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. #include "test_precomp.hpp" namespace opencv_test { namespace { -// ============================================================================= -// Map tests -// ============================================================================= +// The slam module has no committed image data, so - as in the geometry module's +// tests - the pipeline is exercised on a synthetic scene: a fixed 3D point cloud +// (generated with theRNG()) projected through a pin-hole camera that translates +// along +X. A stub Feature2D replays the projected keypoints frame by frame and +// a brute-force matcher recovers the ground-truth correspondences, so +// VisualOdometry runs end-to-end without a real detector or image files. -TEST(Map, AddAndRetrieveKeyframe) +const int descDim = 8; +const int cloudSize = 400; +const Size imageSize(640, 480); + +// Camera centres along +X: a wide first baseline for good bootstrap parallax, +// then small uniform steps suited to per-frame tracking. +static const double camCenters[] = { 0.0, 0.8, 1.1, 1.4, 1.7, 2.0 }; + +static Matx33d cameraMatrix() { - cv::slam::Map map; - - cv::slam::KeyFrame kf; - kf.pose_cw = cv::Matx44d::eye(); - - int id = map.addKeyframe(kf); - EXPECT_GE(id, 0); - EXPECT_EQ(kf.id, id); - - cv::slam::KeyFrame* retrieved = map.getKeyframe(id); - ASSERT_NE(retrieved, nullptr); - EXPECT_EQ(retrieved->id, id); - EXPECT_EQ(map.numKeyframes(), 1); + return Matx33d(500, 0, 320, + 0, 500, 240, + 0, 0, 1); } -TEST(Map, AddAndRetrieveMapPoint) +// Each landmark gets a globally-unique, view-invariant descriptor: identical for +// the same 3D point in every frame (so the matcher pairs them at distance 0) and +// >= sqrt(descDim) apart for distinct points (well above descProjThresh, so +// the projection search during tracking never mismatches). +static Mat makeDescriptor(int id) { - cv::slam::Map map; - - cv::slam::MapPoint mp; - mp.pos = cv::Point3d(1.0, 2.0, 3.0); - - int id = map.addMapPoint(mp); - EXPECT_GE(id, 0); - EXPECT_EQ(mp.id, id); - - cv::slam::MapPoint* retrieved = map.getMapPoint(id); - ASSERT_NE(retrieved, nullptr); - EXPECT_DOUBLE_EQ(retrieved->pos.x, 1.0); - EXPECT_DOUBLE_EQ(retrieved->pos.y, 2.0); - EXPECT_DOUBLE_EQ(retrieved->pos.z, 3.0); - EXPECT_FALSE(retrieved->bad); - EXPECT_EQ(map.numMapPoints(), 1); + return Mat(1, descDim, CV_32F, Scalar((double)(id + 1))); } -TEST(Map, RemoveMapPointCleansObservations) +// Stub Feature2D: replays pre-computed keypoints/descriptors, one frame per +// detectAndCompute() call (processFrame() invokes the detector exactly once). +class StubDetector CV_FINAL : public Feature2D { - cv::slam::Map map; +public: + struct FrameFeatures { std::vector keypoints; Mat descriptors; }; + std::vector frames; + size_t next = 0; - // Add a keyframe with one keypoint slot. - cv::slam::KeyFrame kf; - kf.pose_cw = cv::Matx44d::eye(); - kf.mappoints.assign(1, nullptr); - kf.kpt_to_mp.assign(1, -1); - int kf_id = map.addKeyframe(kf); + void detectAndCompute(InputArray, InputArray, + std::vector& keypoints, + OutputArray descriptors, bool) CV_OVERRIDE + { + CV_Assert(next < frames.size()); + keypoints = frames[next].keypoints; + frames[next].descriptors.copyTo(descriptors); + ++next; + } +}; - // Add a map point. - cv::slam::MapPoint mp; - mp.pos = cv::Point3d(0, 0, 1); - int mp_id = map.addMapPoint(mp); - - cv::slam::KeyFrame* kf_ptr = map.getKeyframe(kf_id); - cv::slam::MapPoint* mp_ptr = map.getMapPoint(mp_id); - ASSERT_NE(kf_ptr, nullptr); - ASSERT_NE(mp_ptr, nullptr); - - // Wire the observation. - map.addObservation(kf_ptr, 0, mp_ptr); - EXPECT_EQ(mp_ptr->observations.size(), 1u); - EXPECT_EQ(kf_ptr->mappoints[0], mp_ptr); - - // Remove the map point and verify cleanup. - map.removeMapPoint(mp_id); - EXPECT_TRUE(mp_ptr->bad); - EXPECT_EQ(kf_ptr->mappoints[0], nullptr); - EXPECT_EQ(kf_ptr->kpt_to_mp[0], -1); - EXPECT_EQ(map.numMapPoints(), 0); +static std::vector makeCloud() +{ + RNG& rng = theRNG(); // seeded per-test by the ts framework -> reproducible + std::vector pts(cloudSize); + for (int i = 0; i < cloudSize; i++) + pts[i] = Point3f(rng.uniform(-2.5f, 2.5f), + rng.uniform(-1.8f, 1.8f), + rng.uniform( 4.0f, 9.0f)); // varied depth -> non-planar + return pts; } -TEST(Map, TrajectoryAppendAndRetrieve) +// Projects the cloud through the camera at centre (camX, 0, 0), keeping the +// points that land inside the image. Ground-truth pose is pure translation +// (R = I), so projectPoints takes a zero rotation vector. +static StubDetector::FrameFeatures renderFrame(const std::vector& cloud, double camX) { - cv::slam::Map map; + Vec3d rvec(0, 0, 0), tvec(-camX, 0, 0); // t = -R*C, with R = I, C = (camX,0,0) + std::vector proj; + projectPoints(cloud, rvec, tvec, cameraMatrix(), noArray(), proj); - cv::Matx44d T1 = cv::Matx44d::eye(); - cv::Matx44d T2 = cv::Matx44d::eye(); - T2(0, 3) = 1.0; // 1-unit translation - - map.appendPose(T1); - map.appendPose(T2); - - const auto& traj = map.trajectory(); - ASSERT_EQ(traj.size(), 2u); - EXPECT_DOUBLE_EQ(traj[1](0, 3), 1.0); + StubDetector::FrameFeatures ff; + std::vector ids; + for (int i = 0; i < (int)cloud.size(); i++) + { + const Point2f& p = proj[i]; + if (p.x < 0 || p.x >= imageSize.width || + p.y < 0 || p.y >= imageSize.height) continue; + ff.keypoints.push_back(KeyPoint(p, 7.f)); + ids.push_back(i); + } + ff.descriptors.create((int)ids.size(), descDim, CV_32F); + for (int r = 0; r < (int)ids.size(); r++) + makeDescriptor(ids[r]).copyTo(ff.descriptors.row(r)); + return ff; } -TEST(Map, ClearResetsState) +// Builds a VisualOdometry fed by a stub detector pre-loaded with @p nFrames of +// the synthetic sequence and a ground-truth brute-force matcher. +static Ptr makeOdometry(int nFrames, + const slam::OdometryParams& params = slam::OdometryParams()) { - cv::slam::Map map; + std::vector cloud = makeCloud(); + Ptr detector = makePtr(); + for (int f = 0; f < nFrames; f++) + detector->frames.push_back(renderFrame(cloud, camCenters[f])); - cv::slam::KeyFrame kf; - kf.pose_cw = cv::Matx44d::eye(); - map.addKeyframe(kf); - - cv::slam::MapPoint mp; - mp.pos = cv::Point3d(1, 2, 3); - map.addMapPoint(mp); - - map.appendPose(cv::Matx44d::eye()); - map.clear(); - - EXPECT_EQ(map.numKeyframes(), 0); - EXPECT_EQ(map.numMapPoints(), 0); - EXPECT_TRUE(map.trajectory().empty()); + Ptr matcher = BFMatcher::create(NORM_L2, true); + return slam::VisualOdometry::create(detector, matcher, "", "", + Mat(cameraMatrix()), noArray(), params); } -// ============================================================================= -// OdometryParams tests -// ============================================================================= +static Mat blankImage() { return Mat::zeros(imageSize, CV_8UC1); } -TEST(OdometryParams, DefaultValues) +// Rotation magnitude (deg) of @p T's rotation block relative to identity. +static double rotationFromIdentityDeg(const Matx44d& T) { - cv::slam::OdometryParams p; - EXPECT_EQ(p.min_init_inliers, 80); - EXPECT_DOUBLE_EQ(p.min_init_parallax_deg, 3.0); - EXPECT_EQ(p.min_init_points, 50); - EXPECT_DOUBLE_EQ(p.hf_ratio_thresh, 0.45); - EXPECT_DOUBLE_EQ(p.min_growth_parallax_deg, 1.0); - EXPECT_DOUBLE_EQ(p.essential_ransac_thresh, 1.0); - EXPECT_DOUBLE_EQ(p.essential_ransac_confidence, 0.999); + const double trace = T(0,0) + T(1,1) + T(2,2); + const double c = std::max(-1.0, std::min(1.0, (trace - 1.0) * 0.5)); + return std::acos(c) * 180.0 / CV_PI; +} + +// Camera centre in world coordinates: C = -R^T t. +static Point3d cameraCenter(const Matx44d& T) +{ + Matx33d R(T(0,0),T(0,1),T(0,2), T(1,0),T(1,1),T(1,2), T(2,0),T(2,1),T(2,2)); + Matx31d t(T(0,3), T(1,3), T(2,3)); + Matx31d C = -R.t() * t; + return Point3d(C(0), C(1), C(2)); +} + +TEST(SLAM_VisualOdometry, create_validates_arguments) +{ + Ptr detector = makePtr(); + Ptr matcher = BFMatcher::create(NORM_L2, true); + Mat K(cameraMatrix()); + + EXPECT_THROW(slam::VisualOdometry::create(Ptr(), matcher, "", "", K), + cv::Exception); // null detector + EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr(), "", "", K), + cv::Exception); // null matcher + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat()), + cv::Exception); // empty intrinsics + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat::eye(2, 2, CV_64F)), + cv::Exception); // wrong-size intrinsics +} + +TEST(SLAM_VisualOdometry, bootstrap_initializes_map) +{ + Ptr vo = makeOdometry(2); + Mat image = blankImage(); + + EXPECT_FALSE(vo->processFrame(image)); // frame 0 -> INITIALIZING + EXPECT_EQ(vo->getState(), slam::INITIALIZING); + + EXPECT_TRUE(vo->processFrame(image)); // frame 1 -> TRACKING + EXPECT_EQ(vo->getState(), slam::TRACKING); + + const slam::Map& map = vo->getMap(); + EXPECT_EQ(map.numKeyframes(), 2); + EXPECT_GE(map.numMapPoints(), 100); // OdometryParams::minInitPoints + EXPECT_EQ(vo->getTrajectory().size(), 2u); + + // Reference keyframe is pinned to the world origin. + const slam::KeyFrame* kfRef = map.getKeyframe(0); + ASSERT_TRUE(kfRef != nullptr); + EXPECT_LT(cv::norm(kfRef->poseCw - Matx44d::eye()), 1e-12); + + // Median scene depth is normalized to 1. + std::vector depths; + depths.reserve(map.mapPoints().size()); + for (slam::MapPoint* mp : map.mapPoints()) + depths.push_back(mp->pos.z); + ASSERT_FALSE(depths.empty()); + std::nth_element(depths.begin(), depths.begin() + depths.size() / 2, depths.end()); + EXPECT_NEAR(depths[depths.size() / 2], 1.0, 1e-2); + + // Second camera: ~no rotation; translation along +X up to scale. + const slam::KeyFrame* kfCur = map.getKeyframe(1); + ASSERT_TRUE(kfCur != nullptr); + EXPECT_LT(rotationFromIdentityDeg(kfCur->poseCw), 2.0); + Point3d C = cameraCenter(kfCur->poseCw); + EXPECT_GT(C.x, 0.0); + EXPECT_LT(std::abs(C.y), 0.2 * std::abs(C.x)); + EXPECT_LT(std::abs(C.z), 0.2 * std::abs(C.x)); +} + +TEST(SLAM_VisualOdometry, tracks_after_bootstrap) +{ + Ptr vo = makeOdometry(3); + Mat image = blankImage(); + + ASSERT_FALSE(vo->processFrame(image)); + ASSERT_TRUE(vo->processFrame(image)); + ASSERT_EQ(vo->getState(), slam::TRACKING); + const Point3d cBootstrap = cameraCenter(vo->getLastPose()); + + EXPECT_TRUE(vo->processFrame(image)); // frame 2 -> tracked by PnP + EXPECT_EQ(vo->getState(), slam::TRACKING); + EXPECT_EQ(vo->getTrajectory().size(), 3u); + + const Matx44d pose = vo->getLastPose(); + EXPECT_LT(rotationFromIdentityDeg(pose), 2.0); + const Point3d C = cameraCenter(pose); + EXPECT_GT(C.x, cBootstrap.x); // camera keeps advancing +X + EXPECT_LT(std::abs(C.y), 0.2 * std::abs(C.x)); + EXPECT_LT(std::abs(C.z), 0.2 * std::abs(C.x)); +} + +TEST(SLAM_VisualOdometry, promotes_keyframes_during_tracking) +{ + slam::OdometryParams params; + params.kfMaxFrames = 1; // force a keyframe promotion early in tracking + + Ptr vo = makeOdometry(6, params); + Mat image = blankImage(); + + ASSERT_FALSE(vo->processFrame(image)); // -> INITIALIZING + ASSERT_TRUE(vo->processFrame(image)); // -> TRACKING (2 keyframes) + ASSERT_EQ(vo->getMap().numKeyframes(), 2); + + for (int f = 2; f < 6; f++) + EXPECT_TRUE(vo->processFrame(image)); + + EXPECT_EQ(vo->getState(), slam::TRACKING); + EXPECT_GT(vo->getMap().numKeyframes(), 2); // new keyframes were promoted + + const slam::KeyFrame* current = vo->getMap().getCurrentKeyframe(); + ASSERT_TRUE(current != nullptr); + EXPECT_GT(current->id, 1); // current keyframe advanced +} + +TEST(SLAM_VisualOdometry, reset_clears_state) +{ + Ptr vo = makeOdometry(2); + Mat image = blankImage(); + + vo->processFrame(image); + vo->processFrame(image); + ASSERT_EQ(vo->getState(), slam::TRACKING); + ASSERT_GT(vo->getMap().numKeyframes(), 0); + + vo->reset(); + EXPECT_EQ(vo->getState(), slam::NOT_INITIALIZED); + EXPECT_EQ(vo->getMap().numKeyframes(), 0); + EXPECT_EQ(vo->getMap().numMapPoints(), 0); + EXPECT_TRUE(vo->getTrajectory().empty()); + EXPECT_LT(cv::norm(vo->getLastPose() - Matx44d::eye()), 1e-12); } }} // namespace opencv_test diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 933e2f9eff..e9ca4e196b 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -23,6 +23,7 @@ endif() add_subdirectory(cpp) add_subdirectory(java/tutorial_code) add_subdirectory(dnn) +add_subdirectory(slam) add_subdirectory(gpu) add_subdirectory(tapi) add_subdirectory(opencl) @@ -124,6 +125,7 @@ if(WIN32) add_subdirectory(directx) endif() add_subdirectory(dnn) +add_subdirectory(slam) # add_subdirectory(gpu) add_subdirectory(opencl) add_subdirectory(sycl) diff --git a/samples/slam/CMakeLists.txt b/samples/slam/CMakeLists.txt new file mode 100644 index 0000000000..4c6dd1707d --- /dev/null +++ b/samples/slam/CMakeLists.txt @@ -0,0 +1,22 @@ +ocv_install_example_src(slam *.cpp *.hpp CMakeLists.txt) + +set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS + opencv_core + opencv_dnn + opencv_features + opencv_slam) +ocv_check_dependencies(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) + +if(NOT BUILD_EXAMPLES OR NOT OCV_DEPENDENCIES_FOUND) + return() +endif() + +project(slam_samples) +ocv_include_modules_recurse(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) +include_directories("${CMAKE_CURRENT_SOURCE_DIR}/../dnn") + +file(GLOB_RECURSE slam_samples RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp) +foreach(sample_filename ${slam_samples}) + ocv_define_sample(tgt ${sample_filename} slam) + ocv_target_link_libraries(${tgt} PRIVATE ${OPENCV_LINKER_LIBS} ${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) +endforeach() diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp new file mode 100644 index 0000000000..e762686d37 --- /dev/null +++ b/samples/slam/visual_odometry.cpp @@ -0,0 +1,63 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include +#include +#include +#include +#include + +using namespace cv; + +static const char* ALIKED_MODEL = "/media/user/path/to/models/aliked-n16rot-top1k-640.onnx"; +static const char* LIGHTGLUE_MODEL = "/media/user/path/to/models/aliked_lightglue.onnx"; +static const char* IMAGES_DIR = "/media/user/path/to/dataset"; +static const char* OUTPUT_DIR = "vo_out"; + +// KITTI-00: fx, fy, cx, cy +static const Matx33d K(718.856, 0., 607.1928, + 0., 718.856, 185.2157, + 0., 0., 1.); + +// k1, k2, p1, p2, k3 +static const std::vector DIST = { -0.2811, 0.0723, -0.0003, 0.0001, 0.0 }; + +static Ptr makeDetector() +{ + ALIKED::Params p; + p.inputSize = Size(640, 640); + p.engine = dnn::ENGINE_NEW; + return ALIKED::create(ALIKED_MODEL, p); +} + +static Ptr makeMatcher() +{ + return LightGlueMatcher::create(LIGHTGLUE_MODEL, 0.0f, + dnn::DNN_BACKEND_DEFAULT, + dnn::DNN_TARGET_CPU); +} + +int main() +{ + slam::OdometryParams params; + params.minInitParallaxDeg = 1.5; + params.minInitPoints = 50; + + auto vo = slam::VisualOdometry::create( + makeDetector(), makeMatcher(), + IMAGES_DIR, OUTPUT_DIR, + Mat(K), Mat(DIST), params); + + const int64 t0 = getTickCount(); + const bool ok = vo->run(); + const double elapsed = (getTickCount() - t0) / getTickFrequency(); + + std::cout << "run=" << (ok ? "ok" : "FAILED") + << " frames=" << vo->getTrajectory().size() + << " elapsed=" << elapsed << "s\n" + << "output -> " << OUTPUT_DIR << "\n"; + return ok ? 0 : 1; +} diff --git a/samples/slam/visual_odometry.py b/samples/slam/visual_odometry.py new file mode 100644 index 0000000000..af4499da37 --- /dev/null +++ b/samples/slam/visual_odometry.py @@ -0,0 +1,56 @@ +''' +Monocular visual odometry with cv.slam.VisualOdometry (ALIKED + LightGlue). +''' + +import time +import numpy as np +import cv2 as cv + +ALIKED_MODEL = '/media/user/path/to/models/aliked-n16rot-top1k-640.onnx' +LIGHTGLUE_MODEL = '/media/user/path/to/models/aliked_lightglue.onnx' +IMAGES_DIR = '/media/user/path/to/dataset' +OUTPUT_DIR = 'vo_out' + +# KITTI-00: fx, fy, cx, cy +K = np.array([[718.856, 0., 607.1928], + [0., 718.856, 185.2157], + [0., 0., 1. ]], dtype=np.float64) + +# k1, k2, p1, p2, k3 +DIST = np.array([-0.2811, 0.0723, -0.0003, 0.0001, 0.0], dtype=np.float64) + + +def make_detector(): + p = cv.ALIKED.Params() + p.inputSize = (640, 640) + p.engine = cv.dnn.ENGINE_NEW + return cv.ALIKED.create(ALIKED_MODEL, p) + + +def make_matcher(): + return cv.LightGlueMatcher.create( + LIGHTGLUE_MODEL, 0.0, + cv.dnn.DNN_BACKEND_DEFAULT, + cv.dnn.DNN_TARGET_CPU) + + +def main(): + params = cv.slam.OdometryParams() + params.minInitParallaxDeg = 1.5 + params.minInitPoints = 50 + + vo = cv.slam.VisualOdometry.create( + make_detector(), make_matcher(), + IMAGES_DIR, OUTPUT_DIR, + K, DIST, params) + + t0 = time.perf_counter() + ok = vo.run() + elapsed = time.perf_counter() - t0 + + print(f"run={'ok' if ok else 'FAILED'} frames={len(vo.getTrajectory())} elapsed={elapsed:.2f}s") + print(f"output -> {OUTPUT_DIR}") + + +if __name__ == '__main__': + main() From 65c93e673d7a2d5ed8d537dd9de3c6dea76e22de Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Sun, 21 Jun 2026 07:20:25 +0530 Subject: [PATCH 03/10] slam files merge into ptcloud dir --- cmake/OpenCVModule.cmake | 2 ++ modules/ptcloud/CMakeLists.txt | 5 +++-- modules/ptcloud/include/opencv2/ptcloud.hpp | 1 + .../include/opencv2/ptcloud}/slam.hpp | 14 +++++++------- .../include/opencv2/ptcloud}/slam/map.hpp | 2 +- .../opencv2/ptcloud}/slam/odometry_params.hpp | 0 .../include/opencv2/ptcloud}/slam/types.hpp | 0 .../opencv2/ptcloud}/slam/visual_odometry.hpp | 6 +++--- .../slam => ptcloud/include/opencv2}/slam.hpp | 11 ++++++----- modules/{slam/src => ptcloud/src/slam}/map.cpp | 0 .../src => ptcloud/src/slam}/odometry/frame.hpp | 0 .../src/slam}/odometry/visual_odometry.cpp | 0 .../src/slam}/odometry/vo_bootstrap.cpp | 0 .../src => ptcloud/src/slam}/odometry/vo_impl.hpp | 0 .../src/slam}/odometry/vo_keyframe.cpp | 0 .../src/slam}/odometry/vo_map_growth.cpp | 0 .../src/slam}/odometry/vo_tracking.cpp | 0 .../src/slam}/optimizer/pose_optimizer.cpp | 0 .../src/slam}/optimizer/pose_optimizer.hpp | 0 .../{slam/src => ptcloud/src/slam}/precomp.hpp | 2 +- modules/{slam => ptcloud}/test/test_map.cpp | 0 modules/ptcloud/test/test_precomp.hpp | 2 ++ .../test/test_visual_odometry.cpp | 0 modules/slam/CMakeLists.txt | 13 ------------- modules/slam/perf/perf_main.cpp | 9 --------- modules/slam/perf/perf_precomp.hpp | 13 ------------- modules/slam/test/test_main.cpp | 9 --------- modules/slam/test/test_precomp.hpp | 15 --------------- samples/slam/CMakeLists.txt | 2 +- 29 files changed, 27 insertions(+), 79 deletions(-) rename modules/{slam/include/opencv2 => ptcloud/include/opencv2/ptcloud}/slam.hpp (67%) rename modules/{slam/include/opencv2 => ptcloud/include/opencv2/ptcloud}/slam/map.hpp (97%) rename modules/{slam/include/opencv2 => ptcloud/include/opencv2/ptcloud}/slam/odometry_params.hpp (100%) rename modules/{slam/include/opencv2 => ptcloud/include/opencv2/ptcloud}/slam/types.hpp (100%) rename modules/{slam/include/opencv2 => ptcloud/include/opencv2/ptcloud}/slam/visual_odometry.hpp (94%) rename modules/{slam/include/opencv2/slam => ptcloud/include/opencv2}/slam.hpp (54%) rename modules/{slam/src => ptcloud/src/slam}/map.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/frame.hpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/visual_odometry.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/vo_bootstrap.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/vo_impl.hpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/vo_keyframe.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/vo_map_growth.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/odometry/vo_tracking.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/optimizer/pose_optimizer.cpp (100%) rename modules/{slam/src => ptcloud/src/slam}/optimizer/pose_optimizer.hpp (100%) rename modules/{slam/src => ptcloud/src/slam}/precomp.hpp (96%) rename modules/{slam => ptcloud}/test/test_map.cpp (100%) rename modules/{slam => ptcloud}/test/test_visual_odometry.cpp (100%) delete mode 100644 modules/slam/CMakeLists.txt delete mode 100644 modules/slam/perf/perf_main.cpp delete mode 100644 modules/slam/perf/perf_precomp.hpp delete mode 100644 modules/slam/test/test_main.cpp delete mode 100644 modules/slam/test/test_precomp.hpp diff --git a/cmake/OpenCVModule.cmake b/cmake/OpenCVModule.cmake index a171ebfb5c..19117c1d6e 100644 --- a/cmake/OpenCVModule.cmake +++ b/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 diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 7602feba97..999181b3d7 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -4,10 +4,11 @@ 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_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} + WRAP java objc python js ) ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES}) +ocv_warnings_disable(CMAKE_CXX_FLAGS -Wshadow) if(NOT HAVE_EIGEN) message(STATUS "Geometry: Eigen support is disabled. Eigen is Required for Posegraph optimization") diff --git a/modules/ptcloud/include/opencv2/ptcloud.hpp b/modules/ptcloud/include/opencv2/ptcloud.hpp index ea1b09219f..ec4cdc85f9 100644 --- a/modules/ptcloud/include/opencv2/ptcloud.hpp +++ b/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 Clound Processing diff --git a/modules/slam/include/opencv2/slam.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam.hpp similarity index 67% rename from modules/slam/include/opencv2/slam.hpp rename to modules/ptcloud/include/opencv2/ptcloud/slam.hpp index c677e221b2..2941d0ef85 100644 --- a/modules/slam/include/opencv2/slam.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam.hpp @@ -4,8 +4,8 @@ // Copyright (C) 2026, BigVision LLC, all rights reserved. // Third party copyrights are property of their respective owners. -#ifndef OPENCV_SLAM_HPP -#define OPENCV_SLAM_HPP +#ifndef OPENCV_PTCLOUD_SLAM_HPP +#define OPENCV_PTCLOUD_SLAM_HPP /** @defgroup slam SLAM and Visual Odometry @@ -15,9 +15,9 @@ Bootstraps an initial map from two-view geometry, then tracks subsequent frames with PnP, growing the map at keyframe promotions. */ -#include "opencv2/slam/types.hpp" -#include "opencv2/slam/map.hpp" -#include "opencv2/slam/odometry_params.hpp" -#include "opencv2/slam/visual_odometry.hpp" +#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_SLAM_HPP +#endif // OPENCV_PTCLOUD_SLAM_HPP diff --git a/modules/slam/include/opencv2/slam/map.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp similarity index 97% rename from modules/slam/include/opencv2/slam/map.hpp rename to modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp index ab8d1963bc..01fe156d55 100644 --- a/modules/slam/include/opencv2/slam/map.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp @@ -7,7 +7,7 @@ #ifndef OPENCV_SLAM_MAP_HPP #define OPENCV_SLAM_MAP_HPP -#include "opencv2/slam/types.hpp" +#include "opencv2/ptcloud/slam/types.hpp" #include #include diff --git a/modules/slam/include/opencv2/slam/odometry_params.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp similarity index 100% rename from modules/slam/include/opencv2/slam/odometry_params.hpp rename to modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp diff --git a/modules/slam/include/opencv2/slam/types.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/types.hpp similarity index 100% rename from modules/slam/include/opencv2/slam/types.hpp rename to modules/ptcloud/include/opencv2/ptcloud/slam/types.hpp diff --git a/modules/slam/include/opencv2/slam/visual_odometry.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp similarity index 94% rename from modules/slam/include/opencv2/slam/visual_odometry.hpp rename to modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp index 95b55e89e3..65e3b79710 100644 --- a/modules/slam/include/opencv2/slam/visual_odometry.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp @@ -10,9 +10,9 @@ #include "opencv2/core.hpp" #include "opencv2/features.hpp" -#include "opencv2/slam/types.hpp" -#include "opencv2/slam/map.hpp" -#include "opencv2/slam/odometry_params.hpp" +#include "opencv2/ptcloud/slam/types.hpp" +#include "opencv2/ptcloud/slam/map.hpp" +#include "opencv2/ptcloud/slam/odometry_params.hpp" #include diff --git a/modules/slam/include/opencv2/slam/slam.hpp b/modules/ptcloud/include/opencv2/slam.hpp similarity index 54% rename from modules/slam/include/opencv2/slam/slam.hpp rename to modules/ptcloud/include/opencv2/slam.hpp index f296d9601e..71ff53c06e 100644 --- a/modules/slam/include/opencv2/slam/slam.hpp +++ b/modules/ptcloud/include/opencv2/slam.hpp @@ -4,10 +4,11 @@ // Copyright (C) 2026, BigVision LLC, all rights reserved. // Third party copyrights are property of their respective owners. -// Compatibility shim: prefer #include . -#ifndef OPENCV_SLAM_SLAM_HPP -#define OPENCV_SLAM_SLAM_HPP +#ifndef OPENCV_SLAM_HPP +#define OPENCV_SLAM_HPP -#include "opencv2/slam.hpp" +// Convenience umbrella header so SLAM / visual odometry can be included as +// . The implementation currently lives in the ptcloud module. +#include "opencv2/ptcloud/slam.hpp" -#endif +#endif // OPENCV_SLAM_HPP diff --git a/modules/slam/src/map.cpp b/modules/ptcloud/src/slam/map.cpp similarity index 100% rename from modules/slam/src/map.cpp rename to modules/ptcloud/src/slam/map.cpp diff --git a/modules/slam/src/odometry/frame.hpp b/modules/ptcloud/src/slam/odometry/frame.hpp similarity index 100% rename from modules/slam/src/odometry/frame.hpp rename to modules/ptcloud/src/slam/odometry/frame.hpp diff --git a/modules/slam/src/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp similarity index 100% rename from modules/slam/src/odometry/visual_odometry.cpp rename to modules/ptcloud/src/slam/odometry/visual_odometry.cpp diff --git a/modules/slam/src/odometry/vo_bootstrap.cpp b/modules/ptcloud/src/slam/odometry/vo_bootstrap.cpp similarity index 100% rename from modules/slam/src/odometry/vo_bootstrap.cpp rename to modules/ptcloud/src/slam/odometry/vo_bootstrap.cpp diff --git a/modules/slam/src/odometry/vo_impl.hpp b/modules/ptcloud/src/slam/odometry/vo_impl.hpp similarity index 100% rename from modules/slam/src/odometry/vo_impl.hpp rename to modules/ptcloud/src/slam/odometry/vo_impl.hpp diff --git a/modules/slam/src/odometry/vo_keyframe.cpp b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp similarity index 100% rename from modules/slam/src/odometry/vo_keyframe.cpp rename to modules/ptcloud/src/slam/odometry/vo_keyframe.cpp diff --git a/modules/slam/src/odometry/vo_map_growth.cpp b/modules/ptcloud/src/slam/odometry/vo_map_growth.cpp similarity index 100% rename from modules/slam/src/odometry/vo_map_growth.cpp rename to modules/ptcloud/src/slam/odometry/vo_map_growth.cpp diff --git a/modules/slam/src/odometry/vo_tracking.cpp b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp similarity index 100% rename from modules/slam/src/odometry/vo_tracking.cpp rename to modules/ptcloud/src/slam/odometry/vo_tracking.cpp diff --git a/modules/slam/src/optimizer/pose_optimizer.cpp b/modules/ptcloud/src/slam/optimizer/pose_optimizer.cpp similarity index 100% rename from modules/slam/src/optimizer/pose_optimizer.cpp rename to modules/ptcloud/src/slam/optimizer/pose_optimizer.cpp diff --git a/modules/slam/src/optimizer/pose_optimizer.hpp b/modules/ptcloud/src/slam/optimizer/pose_optimizer.hpp similarity index 100% rename from modules/slam/src/optimizer/pose_optimizer.hpp rename to modules/ptcloud/src/slam/optimizer/pose_optimizer.hpp diff --git a/modules/slam/src/precomp.hpp b/modules/ptcloud/src/slam/precomp.hpp similarity index 96% rename from modules/slam/src/precomp.hpp rename to modules/ptcloud/src/slam/precomp.hpp index ad2246c4dd..d489fa9c0d 100644 --- a/modules/slam/src/precomp.hpp +++ b/modules/ptcloud/src/slam/precomp.hpp @@ -7,7 +7,7 @@ #ifndef OPENCV_SLAM_PRECOMP_HPP #define OPENCV_SLAM_PRECOMP_HPP -#include "opencv2/slam.hpp" +#include "opencv2/ptcloud/slam.hpp" #include "opencv2/core.hpp" #include "opencv2/core/utility.hpp" diff --git a/modules/slam/test/test_map.cpp b/modules/ptcloud/test/test_map.cpp similarity index 100% rename from modules/slam/test/test_map.cpp rename to modules/ptcloud/test/test_map.cpp diff --git a/modules/ptcloud/test/test_precomp.hpp b/modules/ptcloud/test/test_precomp.hpp index c3fc4c8385..ace4e812c4 100644 --- a/modules/ptcloud/test/test_precomp.hpp +++ b/modules/ptcloud/test/test_precomp.hpp @@ -13,6 +13,8 @@ #include #include "opencv2/ptcloud/depth.hpp" #include "opencv2/ptcloud/odometry.hpp" +#include "opencv2/ptcloud/slam.hpp" +#include "opencv2/features.hpp" #ifdef HAVE_OPENCL #include diff --git a/modules/slam/test/test_visual_odometry.cpp b/modules/ptcloud/test/test_visual_odometry.cpp similarity index 100% rename from modules/slam/test/test_visual_odometry.cpp rename to modules/ptcloud/test/test_visual_odometry.cpp diff --git a/modules/slam/CMakeLists.txt b/modules/slam/CMakeLists.txt deleted file mode 100644 index 6b6f62b2f8..0000000000 --- a/modules/slam/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -set(the_description "SLAM and Visual Odometry") - -ocv_define_module(slam - opencv_geometry - opencv_features - opencv_imgproc - opencv_imgcodecs - opencv_video - OPTIONAL - opencv_dnn - WRAP python) - -ocv_warnings_disable(CMAKE_CXX_FLAGS -Wshadow) diff --git a/modules/slam/perf/perf_main.cpp b/modules/slam/perf/perf_main.cpp deleted file mode 100644 index 95b14c1581..0000000000 --- a/modules/slam/perf/perf_main.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html. -// Copyright (C) 2026, BigVision LLC, all rights reserved. -// Third party copyrights are property of their respective owners. - -#include "perf_precomp.hpp" - -CV_PERF_TEST_MAIN(slam) diff --git a/modules/slam/perf/perf_precomp.hpp b/modules/slam/perf/perf_precomp.hpp deleted file mode 100644 index 37fede9ca6..0000000000 --- a/modules/slam/perf/perf_precomp.hpp +++ /dev/null @@ -1,13 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html. -// Copyright (C) 2026, BigVision LLC, all rights reserved. -// Third party copyrights are property of their respective owners. - -#ifndef OPENCV_SLAM_PERF_PRECOMP_HPP -#define OPENCV_SLAM_PERF_PRECOMP_HPP - -#include "opencv2/ts.hpp" -#include "opencv2/slam.hpp" - -#endif diff --git a/modules/slam/test/test_main.cpp b/modules/slam/test/test_main.cpp deleted file mode 100644 index 1a7a912ed5..0000000000 --- a/modules/slam/test/test_main.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// 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" - -CV_TEST_MAIN("cv") diff --git a/modules/slam/test/test_precomp.hpp b/modules/slam/test/test_precomp.hpp deleted file mode 100644 index 735e4f4d22..0000000000 --- a/modules/slam/test/test_precomp.hpp +++ /dev/null @@ -1,15 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html. -// Copyright (C) 2026, BigVision LLC, all rights reserved. -// Third party copyrights are property of their respective owners. - -#ifndef OPENCV_SLAM_TEST_PRECOMP_HPP -#define OPENCV_SLAM_TEST_PRECOMP_HPP - -#include "opencv2/ts.hpp" -#include "opencv2/slam.hpp" -#include "opencv2/geometry.hpp" -#include "opencv2/features.hpp" - -#endif diff --git a/samples/slam/CMakeLists.txt b/samples/slam/CMakeLists.txt index 4c6dd1707d..f87637b768 100644 --- a/samples/slam/CMakeLists.txt +++ b/samples/slam/CMakeLists.txt @@ -4,7 +4,7 @@ set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS opencv_core opencv_dnn opencv_features - opencv_slam) + opencv_ptcloud) ocv_check_dependencies(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) if(NOT BUILD_EXAMPLES OR NOT OCV_DEPENDENCIES_FOUND) From a02eb6ad0f9b00a015bc3eafddc32dc570def6c2 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Tue, 23 Jun 2026 15:14:58 +0530 Subject: [PATCH 04/10] better vo samples --- samples/slam/visual_odometry.cpp | 94 +++++++++++++++++++------------- samples/slam/visual_odometry.py | 83 +++++++++++++++++----------- 2 files changed, 108 insertions(+), 69 deletions(-) diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index e762686d37..001b7fb958 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -2,7 +2,6 @@ // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html. // Copyright (C) 2026, BigVision LLC, all rights reserved. -// Third party copyrights are property of their respective owners. #include #include @@ -12,52 +11,73 @@ using namespace cv; -static const char* ALIKED_MODEL = "/media/user/path/to/models/aliked-n16rot-top1k-640.onnx"; -static const char* LIGHTGLUE_MODEL = "/media/user/path/to/models/aliked_lightglue.onnx"; -static const char* IMAGES_DIR = "/media/user/path/to/dataset"; -static const char* OUTPUT_DIR = "vo_out"; +static const char* keys = + "{ help h | | Print help message }" + "{ aliked | | Path to ALIKED ONNX model }" + "{ lightglue | | Path to LightGlue ONNX model }" + "{ images | | 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 }" + "{ min-parallax | 1.5 | Minimum initialisation parallax in degrees }" + "{ min-points | 50 | Minimum initialisation map points }"; -// KITTI-00: fx, fy, cx, cy -static const Matx33d K(718.856, 0., 607.1928, - 0., 718.856, 185.2157, - 0., 0., 1.); - -// k1, k2, p1, p2, k3 -static const std::vector DIST = { -0.2811, 0.0723, -0.0003, 0.0001, 0.0 }; - -static Ptr makeDetector() +int main(int argc, char** argv) { - ALIKED::Params p; - p.inputSize = Size(640, 640); - p.engine = dnn::ENGINE_NEW; - return ALIKED::create(ALIKED_MODEL, p); -} + CommandLineParser parser(argc, argv, keys); + parser.about("Monocular visual odometry using ALIKED + LightGlue\n" + " Example: visual_odometry --aliked=aliked.onnx --lightglue=lg.onnx --images=./seq\n"); -static Ptr makeMatcher() -{ - return LightGlueMatcher::create(LIGHTGLUE_MODEL, 0.0f, - dnn::DNN_BACKEND_DEFAULT, - dnn::DNN_TARGET_CPU); -} + if (parser.has("help")) + { + parser.printMessage(); + return 0; + } -int main() -{ - slam::OdometryParams params; - params.minInitParallaxDeg = 1.5; - params.minInitPoints = 50; + const String alikedPath = parser.get("aliked"); + const String lightgluePath = parser.get("lightglue"); + const String imagesDir = parser.get("images"); + + if (!parser.check() || alikedPath == "" || lightgluePath == "" || imagesDir == "") + { + parser.printErrors(); + parser.printMessage(); + return 1; + } + + const String outputDir = parser.get("output"); + + const Matx33d K(parser.get("fx"), 0., parser.get("cx"), + 0., parser.get("fy"), parser.get("cy"), + 0., 0., 1.); + + ALIKED::Params detParams; + detParams.inputSize = Size(640, 640); + detParams.engine = dnn::ENGINE_NEW; + auto detector = ALIKED::create(alikedPath, detParams); + + auto matcher = LightGlueMatcher::create(lightgluePath, 0.0f, + dnn::DNN_BACKEND_DEFAULT, + dnn::DNN_TARGET_CPU); + + slam::OdometryParams voParams; + voParams.minInitParallaxDeg = parser.get("min-parallax"); + voParams.minInitPoints = parser.get("min-points"); auto vo = slam::VisualOdometry::create( - makeDetector(), makeMatcher(), - IMAGES_DIR, OUTPUT_DIR, - Mat(K), Mat(DIST), params); + detector, matcher, + imagesDir, outputDir, + Mat(K), Mat(), voParams); - const int64 t0 = getTickCount(); - const bool ok = vo->run(); + const int64 t0 = getTickCount(); + const bool ok = vo->run(); const double elapsed = (getTickCount() - t0) / getTickFrequency(); - std::cout << "run=" << (ok ? "ok" : "FAILED") + std::cout << "run=" << (ok ? "ok" : "FAILED") << " frames=" << vo->getTrajectory().size() << " elapsed=" << elapsed << "s\n" - << "output -> " << OUTPUT_DIR << "\n"; + << "output -> " << outputDir << "\n"; return ok ? 0 : 1; } diff --git a/samples/slam/visual_odometry.py b/samples/slam/visual_odometry.py index af4499da37..c949fffa04 100644 --- a/samples/slam/visual_odometry.py +++ b/samples/slam/visual_odometry.py @@ -1,55 +1,74 @@ ''' 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 time import numpy as np import cv2 as cv -ALIKED_MODEL = '/media/user/path/to/models/aliked-n16rot-top1k-640.onnx' -LIGHTGLUE_MODEL = '/media/user/path/to/models/aliked_lightglue.onnx' -IMAGES_DIR = '/media/user/path/to/dataset' -OUTPUT_DIR = 'vo_out' -# KITTI-00: fx, fy, cx, cy -K = np.array([[718.856, 0., 607.1928], - [0., 718.856, 185.2157], - [0., 0., 1. ]], dtype=np.float64) - -# k1, k2, p1, p2, k3 -DIST = np.array([-0.2811, 0.0723, -0.0003, 0.0001, 0.0], dtype=np.float64) - - -def make_detector(): - p = cv.ALIKED.Params() - p.inputSize = (640, 640) - p.engine = cv.dnn.ENGINE_NEW - return cv.ALIKED.create(ALIKED_MODEL, p) - - -def make_matcher(): - return cv.LightGlueMatcher.create( - LIGHTGLUE_MODEL, 0.0, - cv.dnn.DNN_BACKEND_DEFAULT, - cv.dnn.DNN_TARGET_CPU) +def build_K(fx, fy, cx, cy): + return np.array([[fx, 0., cx], + [0., fy, cy], + [0., 0., 1.]], dtype=np.float64) def main(): - params = cv.slam.OdometryParams() - params.minInitParallaxDeg = 1.5 - params.minInitPoints = 50 + 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( - make_detector(), make_matcher(), - IMAGES_DIR, OUTPUT_DIR, - K, DIST, params) + detector, matcher, + args.images, args.output, + K, np.array([]), vo_params) t0 = time.perf_counter() ok = vo.run() elapsed = time.perf_counter() - t0 print(f"run={'ok' if ok else 'FAILED'} frames={len(vo.getTrajectory())} elapsed={elapsed:.2f}s") - print(f"output -> {OUTPUT_DIR}") + print(f"output -> {args.output}") if __name__ == '__main__': From bf5224cfa8e8ca9dbbe0cd2330e3675adde7d718 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Fri, 26 Jun 2026 19:36:50 +0530 Subject: [PATCH 05/10] cv::slam output files fixed --- .../src/slam/odometry/visual_odometry.cpp | 99 +++++-------------- modules/ptcloud/src/slam/odometry/vo_impl.hpp | 4 +- 2 files changed, 26 insertions(+), 77 deletions(-) diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index 0e65dfdaa5..c0d30a2767 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -221,13 +221,9 @@ bool VisualOdometryImpl::run() return false; } - std::ofstream log; if (!outputFolder.empty()) - { cv::utils::fs::createDirectories(outputFolder); - log.open(joinPath(outputFolder, "vo.log").c_str()); - } - auto logln = [&](const String& s) { if (log.is_open()) log << s << "\n"; }; + auto logln = [](const String&) {}; logln("[INFO] optimizer = reprojection inlier check"); logln(String("[INFO] images_folder = ") + imagesFolder); @@ -292,20 +288,9 @@ bool VisualOdometryImpl::run() if (!outputFolder.empty()) { - writeTrajectoryText(joinPath(outputFolder, "trajectory.txt")); - writeTrajectoryBin (joinPath(outputFolder, "trajectory.bin")); - writeMapPoints (joinPath(outputFolder, "map_points.txt")); - writeKeypoints (joinPath(outputFolder, "keypoints.txt")); - writeImagesTxt (joinPath(outputFolder, "images.txt")); - - std::ostringstream ss; - ss << "[INFO] run complete: frames=" << imgFiles.size() - << " emitted=" << nEmitted - << " keyframes=" << map.numKeyframes() - << " map_points=" << map.numMapPoints(); - logln(ss.str()); - logln("[INFO] wrote trajectory.txt, trajectory.bin, map_points.txt, " - "keypoints.txt, images.txt"); + writeCameraIntrinsics(joinPath(outputFolder, "camera.txt")); + writeMapPoints (joinPath(outputFolder, "point3d.txt")); + writeImagesTxt (joinPath(outputFolder, "images.txt")); } return nEmitted > 0; @@ -313,36 +298,31 @@ bool VisualOdometryImpl::run() // IO helpers -void VisualOdometryImpl::writeTrajectoryText(const String& path) const +void VisualOdometryImpl::writeCameraIntrinsics(const String& path) const { std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeTrajectoryText: cannot open " << path); return; } - f << "# Per-frame camera center in world coordinates.\n# Columns: Cx Cy Cz\n"; - f.setf(std::ios::scientific); f.precision(9); - for (const auto& T : map.trajectory()) - { - Point3d C = detail::cameraCenterWorld(T); - f << C.x << " " << C.y << " " << C.z << "\n"; - } -} + if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeCameraIntrinsics: cannot open " << path); return; } -void VisualOdometryImpl::writeTrajectoryBin(const String& path) const -{ - std::ofstream f(path.c_str(), std::ios::binary); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeTrajectoryBin: cannot open " << path); return; } - const char magic[4] = {'V','O','T','R'}; - f.write(magic, 4); - int32_t version = 1; - f.write(reinterpret_cast(&version), sizeof(int32_t)); - int32_t n = (int32_t)map.trajectory().size(); - f.write(reinterpret_cast(&n), sizeof(int32_t)); - for (const auto& T : map.trajectory()) + const double fx = K.at(0, 0); + const double fy = K.at(1, 1); + const double cx = K.at(0, 2); + const double cy = K.at(1, 2); + + int width = 0, height = 0; + if (!map.keyframes().empty()) { - double buf[16]; - for (int r = 0; r < 4; ++r) - for (int c = 0; c < 4; ++c) buf[r*4+c] = T(r,c); - f.write(reinterpret_cast(buf), sizeof(buf)); + const KeyFrame* kf = *map.keyframes().begin(); + width = kf->imageSize.width; + height = kf->imageSize.height; } + + f.setf(std::ios::fixed); f.precision(4); + f << "fx " << fx << "\n" + << "fy " << fy << "\n" + << "cx " << cx << "\n" + << "cy " << cy << "\n" + << "width " << width << "\n" + << "height " << height << "\n"; } void VisualOdometryImpl::writeMapPoints(const String& path) const @@ -360,35 +340,6 @@ void VisualOdometryImpl::writeMapPoints(const String& path) const } } -void VisualOdometryImpl::writeKeypoints(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeKeypoints: cannot open " << path); return; } - f << "# Per-keyframe keypoints.\n" - << "# Block header: KF kf_id Cx Cy Cz n_keypoints\n" - << "# Followed by n_keypoints rows: kpIdx x y size angle response octave mpId\n" - << "# mpId = -1 if the keypoint has no map point.\n"; - f.setf(std::ios::fixed); f.precision(6); - - for (KeyFrame* kf : map.keyframes()) - { - if (!kf) continue; - Point3d C = detail::cameraCenterWorld(kf->poseCw); - f << "KF " << kf->id << " " << C.x << " " << C.y << " " << C.z - << " " << kf->keypoints.size() << "\n"; - for (size_t i = 0; i < kf->keypoints.size(); ++i) - { - const KeyPoint& kp = kf->keypoints[i]; - int mpId = -1; - if (i < kf->mapPoints.size() && kf->mapPoints[i]) - mpId = kf->mapPoints[i]->id; - f << i << " " << kp.pt.x << " " << kp.pt.y << " " - << kp.size << " " << kp.angle << " " << kp.response << " " - << kp.octave << " " << mpId << "\n"; - } - } -} - // Shepperd's method: numerically-stable R → unit quaternion (qw, qx, qy, qz). static void rotMatToQuat(const Matx33d& R, double& qw, double& qx, double& qy, double& qz) @@ -454,7 +405,7 @@ void VisualOdometryImpl::writeImagesTxt(const String& path) const : (String("pose_") + std::to_string(i)); f << i << " " << qw << " " << qx << " " << qy << " " << qz << " " - << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n\n"; + << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n"; } } diff --git a/modules/ptcloud/src/slam/odometry/vo_impl.hpp b/modules/ptcloud/src/slam/odometry/vo_impl.hpp index deb673fe0d..c809bf7294 100644 --- a/modules/ptcloud/src/slam/odometry/vo_impl.hpp +++ b/modules/ptcloud/src/slam/odometry/vo_impl.hpp @@ -76,10 +76,8 @@ public: // --- IO helpers (visual_odometry.cpp) ------------------------------------ - void writeTrajectoryText(const String& path) const; - void writeTrajectoryBin(const String& path) const; + void writeCameraIntrinsics(const String& path) const; void writeMapPoints(const String& path) const; - void writeKeypoints(const String& path) const; void writeImagesTxt(const String& path) const; // --- Owned state --------------------------------------------------------- From 747a71fdfec68dba22e116d3b4ee890b4f7d930f Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Tue, 7 Jul 2026 15:06:18 +0530 Subject: [PATCH 06/10] slam:VO fix part 1 --- .../opencv2/ptcloud/slam/odometry_params.hpp | 55 ++++++------- .../opencv2/ptcloud/slam/visual_odometry.hpp | 3 +- modules/ptcloud/include/opencv2/slam.hpp | 14 ---- .../src/slam/odometry/visual_odometry.cpp | 78 +++++-------------- samples/slam/visual_odometry.cpp | 2 +- 5 files changed, 51 insertions(+), 101 deletions(-) delete mode 100644 modules/ptcloud/include/opencv2/slam.hpp diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp index 84194784ad..bca8687994 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp @@ -15,46 +15,47 @@ 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 - CV_PROP_RW int minInitInliers = 40; - CV_PROP_RW double minInitParallaxDeg = 1.5; - CV_PROP_RW int minInitPoints = 50; - CV_PROP_RW double hfRatioThresh = 0.45; - CV_PROP_RW double minGrowthParallaxDeg = 0.1; - CV_PROP_RW double essentialRansacThresh = 1.0; - CV_PROP_RW double essentialRansacConfidence = 0.999; + // 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; - CV_PROP_RW int pnpMinInliers = 6; - CV_PROP_RW int pnpRansacIters = 500; - CV_PROP_RW double pnpConfidence = 0.99; + 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 - CV_PROP_RW double motionModelRadius = 15.0; - CV_PROP_RW double motionModelRadiusWide = 30.0; - CV_PROP_RW int motionModelMinMatches = 20; - CV_PROP_RW double descProjThresh = 1.0; + // 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; + CV_PROP_RW int opticalFlowMinInliers = 10; //!< Minimum correspondences for the optical-flow fallback. // Keyframe promotion - CV_PROP_RW int kfMinFrames = 1; - CV_PROP_RW int kfMaxFrames = 30; - CV_PROP_RW double kfInlierRatio = 0.70; - CV_PROP_RW int kfMinInliers = 40; - CV_PROP_RW double kfRotThreshDeg = 5.0; - CV_PROP_RW double kfTransThresh = 0.5; + 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; - CV_PROP_RW int localMapNeighborK = 5; - CV_PROP_RW double localMapRadius = 7.0; + 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. }; //! @} diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp index 65e3b79710..e51d60e21c 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp @@ -27,8 +27,7 @@ namespace slam { State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING (per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING. -@ref run writes trajectory.txt, trajectory.bin, map_points.txt, keypoints.txt, -images.txt, and vo.log into `outputFolder`. +@ref run writes camera.txt, point3d.txt, and images.txt into `outputFolder`. */ class CV_EXPORTS_W VisualOdometry { diff --git a/modules/ptcloud/include/opencv2/slam.hpp b/modules/ptcloud/include/opencv2/slam.hpp deleted file mode 100644 index 71ff53c06e..0000000000 --- a/modules/ptcloud/include/opencv2/slam.hpp +++ /dev/null @@ -1,14 +0,0 @@ -// This file is part of OpenCV project. -// It is subject to the license terms in the LICENSE file found in the top-level directory -// of this distribution and at http://opencv.org/license.html. -// Copyright (C) 2026, BigVision LLC, all rights reserved. -// Third party copyrights are property of their respective owners. - -#ifndef OPENCV_SLAM_HPP -#define OPENCV_SLAM_HPP - -// Convenience umbrella header so SLAM / visual odometry can be included as -// . The implementation currently lives in the ptcloud module. -#include "opencv2/ptcloud/slam.hpp" - -#endif // OPENCV_SLAM_HPP diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index c0d30a2767..18b8bb1f3c 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -7,6 +7,8 @@ #include "../precomp.hpp" #include "vo_impl.hpp" +#include + #include #include @@ -105,26 +107,26 @@ bool VisualOdometryImpl::processFrame(InputArray image) if (image.empty()) return false; lastEvent.clear(); - Frame cur; - extractFeatures(image, cur); - if (cur.keypoints.empty() || cur.descriptors.empty()) return false; + Frame currentFrame; + extractFeatures(image, currentFrame); + if (currentFrame.keypoints.empty() || currentFrame.descriptors.empty()) return false; - cur.mapPoints.assign(cur.keypoints.size(), nullptr); - cur.outliers.assign(cur.keypoints.size(), false); - cur.buildGrid(); + currentFrame.mapPoints.assign(currentFrame.keypoints.size(), nullptr); + currentFrame.outliers.assign(currentFrame.keypoints.size(), false); + currentFrame.buildGrid(); switch (state) { case NOT_INITIALIZED: - refFrame = cur; + refFrame = currentFrame; state = INITIALIZING; return false; case INITIALIZING: - return bootstrap(cur); + return bootstrap(currentFrame); case TRACKING: - return track(cur); + return track(currentFrame); } return false; } @@ -223,16 +225,11 @@ bool VisualOdometryImpl::run() if (!outputFolder.empty()) cv::utils::fs::createDirectories(outputFolder); - auto logln = [](const String&) {}; - logln("[INFO] optimizer = reprojection inlier check"); - logln(String("[INFO] images_folder = ") + imagesFolder); - logln(String("[INFO] output_folder = ") + outputFolder); - { - std::ostringstream ss; - ss << "[INFO] found " << imgFiles.size() << " image(s)"; - logln(ss.str()); - } + CV_LOG_INFO(NULL, "optimizer = reprojection inlier check"); + CV_LOG_INFO(NULL, "images_folder = " << imagesFolder); + CV_LOG_INFO(NULL, "output_folder = " << outputFolder); + CV_LOG_INFO(NULL, "found " << imgFiles.size() << " image(s)"); reset(); @@ -245,9 +242,8 @@ bool VisualOdometryImpl::run() Mat img = imread(imgFiles[i]); if (img.empty()) { - std::ostringstream ss; - ss << "[FRAME " << i << "] file=" << imgFiles[i] << " ERROR: imread failed"; - logln(ss.str()); continue; + CV_LOG_WARNING(NULL, "[FRAME " << i << "] file=" << imgFiles[i] << " imread failed"); + continue; } OdometryState before = state; @@ -283,7 +279,7 @@ bool VisualOdometryImpl::run() Point3d C = detail::cameraCenterWorld(lastPoseCw); ss << " C=(" << C.x << "," << C.y << "," << C.z << ")"; } - logln(ss.str()); + CV_LOG_INFO(NULL, ss.str()); } if (!outputFolder.empty()) @@ -340,39 +336,6 @@ void VisualOdometryImpl::writeMapPoints(const String& path) const } } -// Shepperd's method: numerically-stable R → unit quaternion (qw, qx, qy, qz). -static void rotMatToQuat(const Matx33d& R, - double& qw, double& qx, double& qy, double& qz) -{ - const double tr = R(0,0) + R(1,1) + R(2,2); - if (tr > 0.0) - { - double s = std::sqrt(tr + 1.0) * 2.0; - qw = 0.25 * s; - qx = (R(2,1) - R(1,2)) / s; - qy = (R(0,2) - R(2,0)) / s; - qz = (R(1,0) - R(0,1)) / s; - } - else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) - { - double s = std::sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2.0; - qw = (R(2,1) - R(1,2)) / s; qx = 0.25 * s; - qy = (R(0,1) + R(1,0)) / s; qz = (R(0,2) + R(2,0)) / s; - } - else if (R(1,1) > R(2,2)) - { - double s = std::sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2.0; - qw = (R(0,2) - R(2,0)) / s; qx = (R(0,1) + R(1,0)) / s; - qy = 0.25 * s; qz = (R(1,2) + R(2,1)) / s; - } - else - { - double s = std::sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2.0; - qw = (R(1,0) - R(0,1)) / s; qx = (R(0,2) + R(2,0)) / s; - qy = (R(1,2) + R(2,1)) / s; qz = 0.25 * s; - } -} - static String basenameOf(const String& path) { const size_t slash = path.find_last_of("/\\"); @@ -397,8 +360,9 @@ void VisualOdometryImpl::writeImagesTxt(const String& path) const Matx33d R; for (int r = 0; r < 3; ++r) for (int c = 0; c < 3; ++c) R(r,c) = T(r,c); - double qw, qx, qy, qz; - rotMatToQuat(R, qw, qx, qy, qz); + + const Quatd q = Quatd::createFromRotMat(R); + const double qw = q.w, qx = q.x, qy = q.y, qz = q.z; const String name = (i < poseFilenames.size()) ? basenameOf(poseFilenames[i]) diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index 001b7fb958..2e1242fd32 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -3,7 +3,7 @@ // of this distribution and at http://opencv.org/license.html. // Copyright (C) 2026, BigVision LLC, all rights reserved. -#include +#include #include #include #include From f552e8ac820d165c9cc0f4ba1de6586f73514c5b Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Tue, 7 Jul 2026 21:12:37 +0530 Subject: [PATCH 07/10] slam VO modular matcher changes/fixes --- modules/features/include/opencv2/features.hpp | 17 ++++++ modules/features/src/matchers.cpp | 4 ++ modules/features/src/matchers_lightglue.cpp | 20 +++++++ .../src/slam/odometry/visual_odometry.cpp | 15 +----- samples/slam/visual_odometry.cpp | 53 +++++++++++++++---- 5 files changed, 86 insertions(+), 23 deletions(-) diff --git a/modules/features/include/opencv2/features.hpp b/modules/features/include/opencv2/features.hpp index 55519cca94..703b2f4c12 100644 --- a/modules/features/include/opencv2/features.hpp +++ b/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& queryKpts, const std::vector& 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& queryKpts, const std::vector& trainKpts, + Size queryImageSize = Size(), Size trainImageSize = Size()) CV_OVERRIDE; }; //! @} features_match diff --git a/modules/features/src/matchers.cpp b/modules/features/src/matchers.cpp index 2223d59400..d11876849d 100644 --- a/modules/features/src/matchers.cpp +++ b/modules/features/src/matchers.cpp @@ -580,6 +580,10 @@ bool DescriptorMatcher::empty() const void DescriptorMatcher::train() {} +void DescriptorMatcher::setImagePairInfo( const std::vector&, const std::vector&, + Size, Size ) +{} + void DescriptorMatcher::match( InputArray queryDescriptors, InputArray trainDescriptors, std::vector& matches, InputArray mask ) const { diff --git a/modules/features/src/matchers_lightglue.cpp b/modules/features/src/matchers_lightglue.cpp index 798c673362..3278f7fb94 100644 --- a/modules/features/src/matchers_lightglue.cpp +++ b/modules/features/src/matchers_lightglue.cpp @@ -15,6 +15,26 @@ namespace cv LightGlueMatcher::LightGlueMatcher() {} LightGlueMatcher::~LightGlueMatcher() {} +void LightGlueMatcher::setImagePairInfo(const std::vector& queryKpts, const std::vector& trainKpts, + Size queryImageSize, Size trainImageSize) +{ + Mat qk((int)queryKpts.size(), 2, CV_32F); + for (size_t i = 0; i < queryKpts.size(); ++i) + { + qk.at((int)i, 0) = queryKpts[i].pt.x; + qk.at((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((int)i, 0) = trainKpts[i].pt.x; + tk.at((int)i, 1) = trainKpts[i].pt.y; + } + + setPairInfo(qk, tk, queryImageSize, trainImageSize); +} + #ifdef HAVE_OPENCV_DNN struct LightGluePairContext diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index 18b8bb1f3c..5213ebf713 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -174,20 +174,7 @@ void VisualOdometryImpl::matchFrames( if (qDesc.empty() || tDesc.empty()) return; if (qKp.empty() || tKp.empty()) return; - LightGlueMatcher* lg = dynamic_cast(matcher.get()); - if (lg) - { - Mat qk((int)qKp.size(), 2, CV_32F); - for (size_t i = 0; i < qKp.size(); ++i) - { qk.at((int)i,0) = qKp[i].pt.x; qk.at((int)i,1) = qKp[i].pt.y; } - - Mat tk((int)tKp.size(), 2, CV_32F); - for (size_t i = 0; i < tKp.size(); ++i) - { tk.at((int)i,0) = tKp[i].pt.x; tk.at((int)i,1) = tKp[i].pt.y; } - - lg->setPairInfo(qk, tk, qSz, tSz); - } - + matcher->setImagePairInfo(qKp, tKp, qSz, tSz); matcher->match(qDesc, tDesc, matches); } diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index 2e1242fd32..450c2c434f 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -3,15 +3,27 @@ // of this distribution and at http://opencv.org/license.html. // Copyright (C) 2026, BigVision LLC, all rights reserved. -#include +#include #include #include #include #include -using namespace cv; +#include "../dnn/common.hpp" -static const char* keys = +using namespace cv; +using namespace std; + +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 | | Path to ALIKED ONNX model }" "{ lightglue | | Path to LightGlue ONNX model }" @@ -24,11 +36,31 @@ static const char* keys = "{ min-parallax | 1.5 | Minimum initialisation parallax in degrees }" "{ min-points | 50 | Minimum initialisation map points }"; +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("Monocular visual odometry using ALIKED + LightGlue\n" - " Example: visual_odometry --aliked=aliked.onnx --lightglue=lg.onnx --images=./seq\n"); + parser.about(about); if (parser.has("help")) { @@ -36,9 +68,11 @@ int main(int argc, char** argv) return 0; } - const String alikedPath = parser.get("aliked"); + const String alikedPath = parser.get("aliked"); const String lightgluePath = parser.get("lightglue"); - const String imagesDir = parser.get("images"); + const String imagesDir = parser.get("images"); + const int backendId = getBackendID(parser.get("backend")); + const int targetId = getTargetID(parser.get("target")); if (!parser.check() || alikedPath == "" || lightgluePath == "" || imagesDir == "") { @@ -56,11 +90,12 @@ int main(int argc, char** argv) 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, - dnn::DNN_BACKEND_DEFAULT, - dnn::DNN_TARGET_CPU); + backendId, targetId); slam::OdometryParams voParams; voParams.minInitParallaxDeg = parser.get("min-parallax"); From 4ef179b4bf8dcbc73970bc76c46b858e0ad9aac6 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Wed, 8 Jul 2026 03:14:17 +0530 Subject: [PATCH 08/10] slam VO sample file refactor --- modules/ptcloud/CMakeLists.txt | 2 +- .../opencv2/ptcloud/slam/visual_odometry.hpp | 21 +- .../src/slam/odometry/visual_odometry.cpp | 221 +----------------- modules/ptcloud/src/slam/odometry/vo_impl.hpp | 23 +- modules/ptcloud/test/test_visual_odometry.cpp | 10 +- samples/slam/visual_odometry.cpp | 221 ++++++++++++++++-- samples/slam/visual_odometry.py | 127 +++++++++- 7 files changed, 351 insertions(+), 274 deletions(-) diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 999181b3d7..a58b719f20 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -4,7 +4,7 @@ set(debug_modules "") if(DEBUG_opencv_ptcloud) list(APPEND debug_modules opencv_highgui) endif() -ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} +ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} WRAP java objc python js ) ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES}) diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp index e51d60e21c..43338f83d3 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp @@ -27,7 +27,9 @@ namespace slam { State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING (per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING. -@ref run writes camera.txt, point3d.txt, and images.txt into `outputFolder`. +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 { @@ -37,15 +39,10 @@ public: CV_WRAP static Ptr create( const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, InputArray cameraMatrix, InputArray distCoeffs = noArray(), const OdometryParams& params = OdometryParams()); - /** @brief Run the pipeline over every image in the configured folder. */ - CV_WRAP virtual bool run() = 0; - /** @brief Feed one image. Returns true if a pose was emitted. */ CV_WRAP virtual bool processFrame(InputArray image) = 0; @@ -58,15 +55,19 @@ public: //! @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& getTrajectory() const = 0; CV_WRAP virtual const OdometryParams& getParams() const = 0; CV_WRAP virtual void setParams(const OdometryParams& params) = 0; - CV_WRAP virtual const String& getImagesFolder() const = 0; - CV_WRAP virtual const String& getOutputFolder() const = 0; - CV_WRAP virtual void setOutputFolder(const String& outputFolder) = 0; - protected: VisualOdometry(); }; diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index 5213ebf713..d8ae7fc470 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -7,37 +7,9 @@ #include "../precomp.hpp" #include "vo_impl.hpp" -#include - -#include -#include - namespace cv { namespace slam { -namespace { - -const char* stateName(OdometryState s) -{ - switch (s) - { - case NOT_INITIALIZED: return "NOT_INITIALIZED"; - case INITIALIZING: return "INITIALIZING"; - case TRACKING: return "TRACKING"; - } - return "NOT_INITIALIZED"; -} - -String joinPath(const String& dir, const String& name) -{ - if (dir.empty()) return name; - char last = dir.back(); - if (last == '/' || last == '\\') return dir + name; - return dir + "/" + name; -} - -} // anonymous namespace - // Factory VisualOdometry::VisualOdometry() = default; @@ -46,8 +18,6 @@ VisualOdometry::~VisualOdometry() = default; Ptr VisualOdometry::create( const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, InputArray cameraMatrix, InputArray distCoeffs, const OdometryParams& params) @@ -59,8 +29,7 @@ Ptr VisualOdometry::create( CV_Assert(!K.empty() && K.rows == 3 && K.cols == 3); Mat dist = distCoeffs.empty() ? Mat() : distCoeffs.getMat(); - return makePtr( - detector, matcher, imagesFolder, outputFolder, K, dist, params); + return makePtr(detector, matcher, K, dist, params); } // Constructor @@ -68,13 +37,10 @@ Ptr VisualOdometry::create( VisualOdometryImpl::VisualOdometryImpl( const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, const Mat& cameraMatrix, const Mat& distCoeffs, const OdometryParams& params) - : detector(detector), matcher(matcher), params(params), - imagesFolder(imagesFolder), outputFolder(outputFolder) + : detector(detector), matcher(matcher), params(params) { cameraMatrix.convertTo(K, CV_64F); if (!distCoeffs.empty()) @@ -96,7 +62,6 @@ void VisualOdometryImpl::reset() prevFrame = Frame(); hasPrevFrame = false; lastEvent.clear(); - poseFilenames.clear(); map.clear(); } @@ -178,186 +143,4 @@ void VisualOdometryImpl::matchFrames( matcher->match(qDesc, tDesc, matches); } -// Batch run() - -bool VisualOdometryImpl::run() -{ - CV_INSTRUMENT_REGION(); - - if (imagesFolder.empty()) - { - CV_LOG_ERROR(NULL, "VisualOdometry::run: imagesFolder is empty"); - return false; - } - - std::vector allFiles; - try { cv::glob(imagesFolder, allFiles, false); } - catch (const cv::Exception& e) - { - CV_LOG_ERROR(NULL, "VisualOdometry::run: glob failed: " << e.what()); - return false; - } - - std::vector imgFiles; - imgFiles.reserve(allFiles.size()); - for (const auto& f : allFiles) - if (cv::haveImageReader(f)) imgFiles.push_back(f); - std::sort(imgFiles.begin(), imgFiles.end()); - - if (imgFiles.empty()) - { - CV_LOG_WARNING(NULL, "VisualOdometry::run: no images in " << imagesFolder); - return false; - } - - if (!outputFolder.empty()) - cv::utils::fs::createDirectories(outputFolder); - - CV_LOG_INFO(NULL, "optimizer = reprojection inlier check"); - CV_LOG_INFO(NULL, "images_folder = " << imagesFolder); - CV_LOG_INFO(NULL, "output_folder = " << outputFolder); - CV_LOG_INFO(NULL, "found " << imgFiles.size() << " image(s)"); - - reset(); - - int nEmitted = 0; - size_t prevTrajLen = 0; - String refFilename; - - for (size_t i = 0; i < imgFiles.size(); ++i) - { - Mat img = imread(imgFiles[i]); - if (img.empty()) - { - CV_LOG_WARNING(NULL, "[FRAME " << i << "] file=" << imgFiles[i] << " imread failed"); - continue; - } - - OdometryState before = state; - bool emitted = processFrame(img); - OdometryState after = state; - if (emitted) ++nEmitted; - - // Track which input image maps to each trajectory pose. - if (before == NOT_INITIALIZED || - (before == TRACKING && after == INITIALIZING)) - refFilename = imgFiles[i]; - - const size_t added = map.trajectory().size() - prevTrajLen; - if (added == 1) - poseFilenames.push_back(imgFiles[i]); - else if (added == 2) - { - poseFilenames.push_back(refFilename); - poseFilenames.push_back(imgFiles[i]); - } - prevTrajLen = map.trajectory().size(); - - std::ostringstream ss; - ss << "[FRAME " << i << "] file=" << imgFiles[i] - << " state=" << stateName(before); - if (before != after) ss << "->" << stateName(after); - ss << " emitted=" << (emitted ? "yes" : "no") - << " keyframes=" << map.numKeyframes() - << " map_points=" << map.numMapPoints(); - if (!lastEvent.empty()) ss << " [" << lastEvent << "]"; - if (emitted) - { - Point3d C = detail::cameraCenterWorld(lastPoseCw); - ss << " C=(" << C.x << "," << C.y << "," << C.z << ")"; - } - CV_LOG_INFO(NULL, ss.str()); - } - - if (!outputFolder.empty()) - { - writeCameraIntrinsics(joinPath(outputFolder, "camera.txt")); - writeMapPoints (joinPath(outputFolder, "point3d.txt")); - writeImagesTxt (joinPath(outputFolder, "images.txt")); - } - - return nEmitted > 0; -} - -// IO helpers - -void VisualOdometryImpl::writeCameraIntrinsics(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeCameraIntrinsics: cannot open " << path); return; } - - const double fx = K.at(0, 0); - const double fy = K.at(1, 1); - const double cx = K.at(0, 2); - const double cy = K.at(1, 2); - - int width = 0, height = 0; - if (!map.keyframes().empty()) - { - const KeyFrame* kf = *map.keyframes().begin(); - width = kf->imageSize.width; - height = kf->imageSize.height; - } - - f.setf(std::ios::fixed); f.precision(4); - f << "fx " << fx << "\n" - << "fy " << fy << "\n" - << "cx " << cx << "\n" - << "cy " << cy << "\n" - << "width " << width << "\n" - << "height " << height << "\n"; -} - -void VisualOdometryImpl::writeMapPoints(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeMapPoints: cannot open " << path); return; } - f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; - f.setf(std::ios::scientific); f.precision(9); - for (MapPoint* mp : map.mapPoints()) - { - if (!mp || mp->bad) continue; - f << mp->id << " " - << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " " - << mp->observations.size() << "\n"; - } -} - -static String basenameOf(const String& path) -{ - const size_t slash = path.find_last_of("/\\"); - return (slash == String::npos) ? path : path.substr(slash + 1); -} - -void VisualOdometryImpl::writeImagesTxt(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeImagesTxt: cannot open " << path); return; } - - const auto& traj = map.trajectory(); - f << "# Image list with two lines of data per image:\n" - << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" - << "# POINTS2D[] as (X, Y, POINT3D_ID)\n" - << "# Number of images: " << traj.size() << ", mean observations per image: 0.0\n"; - f.setf(std::ios::fixed); f.precision(6); - - for (size_t i = 0; i < traj.size(); ++i) - { - const Matx44d& T = traj[i]; - Matx33d R; - for (int r = 0; r < 3; ++r) - for (int c = 0; c < 3; ++c) R(r,c) = T(r,c); - - const Quatd q = Quatd::createFromRotMat(R); - const double qw = q.w, qx = q.x, qy = q.y, qz = q.z; - - const String name = (i < poseFilenames.size()) - ? basenameOf(poseFilenames[i]) - : (String("pose_") + std::to_string(i)); - - f << i << " " << qw << " " << qx << " " << qy << " " << qz << " " - << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n"; - } -} - }} // namespace cv::slam diff --git a/modules/ptcloud/src/slam/odometry/vo_impl.hpp b/modules/ptcloud/src/slam/odometry/vo_impl.hpp index c809bf7294..cbd539737f 100644 --- a/modules/ptcloud/src/slam/odometry/vo_impl.hpp +++ b/modules/ptcloud/src/slam/odometry/vo_impl.hpp @@ -11,8 +11,6 @@ #include "frame.hpp" #include "../optimizer/pose_optimizer.hpp" -#include - namespace cv { namespace slam { @@ -23,36 +21,31 @@ Stage logic is split across: - vo_tracking.cpp : per-frame localisation (motion model, fallback 1/2, local map) - vo_keyframe.cpp : keyframe promotion decision + covisibility helpers - vo_map_growth.cpp : triangulation of new map points at promotion time - - visual_odometry.cpp : factory, run(), processFrame(), IO writers + - visual_odometry.cpp : factory, processFrame() */ class VisualOdometryImpl CV_FINAL : public VisualOdometry { public: VisualOdometryImpl(const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, const Mat& cameraMatrix, const Mat& distCoeffs, const OdometryParams& params); // --- VisualOdometry interface ------------------------------------------- - bool run() CV_OVERRIDE; bool processFrame(InputArray image) CV_OVERRIDE; void reset() CV_OVERRIDE; OdometryState getState() const CV_OVERRIDE { return state; } Matx44d getLastPose() const CV_OVERRIDE { return lastPoseCw; } const Map& getMap() const CV_OVERRIDE { return map; } + int getNumKeyframes() const CV_OVERRIDE { return map.numKeyframes(); } + int getNumMapPoints() const CV_OVERRIDE { return map.numMapPoints(); } const std::vector& getTrajectory() const CV_OVERRIDE { return map.trajectory(); } const OdometryParams& getParams() const CV_OVERRIDE { return params; } void setParams(const OdometryParams& p) CV_OVERRIDE { params = p; } - const String& getImagesFolder() const CV_OVERRIDE { return imagesFolder; } - const String& getOutputFolder() const CV_OVERRIDE { return outputFolder; } - void setOutputFolder(const String& f) CV_OVERRIDE { outputFolder = f; } - // --- Stage entry points ------------------------------------------------- bool bootstrap(Frame& cur); @@ -74,12 +67,6 @@ public: const std::vector& tKp, const Mat& tDesc, Size tSz, std::vector& matches) const; - // --- IO helpers (visual_odometry.cpp) ------------------------------------ - - void writeCameraIntrinsics(const String& path) const; - void writeMapPoints(const String& path) const; - void writeImagesTxt(const String& path) const; - // --- Owned state --------------------------------------------------------- Ptr detector; @@ -88,9 +75,6 @@ public: Mat dist; // distortion coefficients (may be empty) OdometryParams params; - String imagesFolder; - String outputFolder; - OdometryState state = NOT_INITIALIZED; Matx44d lastPoseCw = Matx44d::eye(); @@ -106,7 +90,6 @@ public: bool hasPrevFrame = false; String lastEvent; - std::vector poseFilenames; Map map; }; diff --git a/modules/ptcloud/test/test_visual_odometry.cpp b/modules/ptcloud/test/test_visual_odometry.cpp index a3ea62136f..a1c9eeee2c 100644 --- a/modules/ptcloud/test/test_visual_odometry.cpp +++ b/modules/ptcloud/test/test_visual_odometry.cpp @@ -106,7 +106,7 @@ static Ptr makeOdometry(int nFrames, detector->frames.push_back(renderFrame(cloud, camCenters[f])); Ptr matcher = BFMatcher::create(NORM_L2, true); - return slam::VisualOdometry::create(detector, matcher, "", "", + return slam::VisualOdometry::create(detector, matcher, Mat(cameraMatrix()), noArray(), params); } @@ -135,13 +135,13 @@ TEST(SLAM_VisualOdometry, create_validates_arguments) Ptr matcher = BFMatcher::create(NORM_L2, true); Mat K(cameraMatrix()); - EXPECT_THROW(slam::VisualOdometry::create(Ptr(), matcher, "", "", K), + EXPECT_THROW(slam::VisualOdometry::create(Ptr(), matcher, K), cv::Exception); // null detector - EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr(), "", "", K), + EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr(), K), cv::Exception); // null matcher - EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat()), + 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)), + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, Mat::eye(2, 2, CV_64F)), cv::Exception); // wrong-size intrinsics } diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index 450c2c434f..423aa6a8d2 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -7,6 +7,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include "../dnn/common.hpp" @@ -14,6 +19,139 @@ using namespace cv; using namespace std; +namespace { + +// path helper: joins a directory and filename, tolerating a dir with/without a trailing slash +String joinPath(const String& dir, const String& name) +{ + if (dir.empty()) return name; + char last = dir.back(); + if (last == '/' || last == '\\') return dir + name; + return dir + "/" + name; +} + +// 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 listImageFiles(const String& imagesDir) +{ + std::vector allFiles; + glob(imagesDir, allFiles, false); + std::vector 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& filenames() const { return filenames_; } + +private: + std::vector 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& poseFilenames, + const String& outputFolder) +{ + cv::utils::fs::createDirectories(outputFolder); + + { + std::ofstream f(joinPath(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(0, 0) << "\n" + << "fy " << K.at(1, 1) << "\n" + << "cx " << K.at(0, 2) << "\n" + << "cy " << K.at(1, 2) << "\n" + << "width " << width << "\n" + << "height " << height << "\n"; + } + + { + std::ofstream f(joinPath(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(joinPath(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" @@ -32,9 +170,7 @@ const string param_keys = "{ 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 }" - "{ min-parallax | 1.5 | Minimum initialisation parallax in degrees }" - "{ min-points | 50 | Minimum initialisation map points }"; + "{ cy | 185.2157 | Camera principal point Y }"; const string backend_keys = format( "{ backend | default | Choose one of computation backends: " @@ -98,21 +234,78 @@ int main(int argc, char** argv) backendId, targetId); slam::OdometryParams voParams; - voParams.minInitParallaxDeg = parser.get("min-parallax"); - voParams.minInitPoints = parser.get("min-points"); + + voParams.minInitParallaxDeg = 1.5; + voParams.minInitPoints = 50; + voParams.pnpReprojThresh = 4.0; + voParams.kfMaxFrames = 30; + voParams.localMapTopK = 10; auto vo = slam::VisualOdometry::create( - detector, matcher, - imagesDir, outputDir, - Mat(K), Mat(), voParams); + detector, matcher, Mat(K), Mat(), voParams); - const int64 t0 = getTickCount(); - const bool ok = vo->run(); + const std::vector 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; - std::cout << "run=" << (ok ? "ok" : "FAILED") - << " frames=" << vo->getTrajectory().size() - << " elapsed=" << elapsed << "s\n" - << "output -> " << outputDir << "\n"; + 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; } diff --git a/samples/slam/visual_odometry.py b/samples/slam/visual_odometry.py index c949fffa04..f3268c17d5 100644 --- a/samples/slam/visual_odometry.py +++ b/samples/slam/visual_odometry.py @@ -6,6 +6,9 @@ Example: ''' import argparse +import glob +import os +import sys import time import numpy as np import cv2 as cv @@ -17,6 +20,71 @@ def build_K(fx, fy, cx, 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') @@ -59,17 +127,66 @@ def main(): K = build_K(args.fx, args.fy, args.cx, args.cy) vo = cv.slam.VisualOdometry.create( - detector, matcher, - args.images, args.output, - K, np.array([]), vo_params) + 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() - ok = vo.run() + 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__': - main() + sys.exit(main()) From ccbc31a0f18cd00917465a1c0cc0d5aa3e3bdb98 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Tue, 14 Jul 2026 03:48:16 +0530 Subject: [PATCH 09/10] slam VO review2 minor changes/fixes --- modules/ptcloud/CMakeLists.txt | 3 +- .../src/slam/odometry/visual_odometry.cpp | 8 ++-- .../ptcloud/src/slam/odometry/vo_keyframe.cpp | 6 +-- .../ptcloud/src/slam/odometry/vo_tracking.cpp | 17 ++++++-- modules/ptcloud/src/slam/precomp.hpp | 4 -- samples/slam/CMakeLists.txt | 1 + samples/slam/visual_odometry.cpp | 41 +++++++++++-------- 7 files changed, 48 insertions(+), 32 deletions(-) diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index a58b719f20..a3e785b6a4 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -4,11 +4,10 @@ set(debug_modules "") if(DEBUG_opencv_ptcloud) list(APPEND debug_modules opencv_highgui) endif() -ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} +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}) -ocv_warnings_disable(CMAKE_CXX_FLAGS -Wshadow) if(NOT HAVE_EIGEN) message(STATUS "Geometry: Eigen support is disabled. Eigen is Required for Posegraph optimization") diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index d8ae7fc470..d5e3a11034 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -35,12 +35,12 @@ Ptr VisualOdometry::create( // Constructor VisualOdometryImpl::VisualOdometryImpl( - const Ptr& detector, - const Ptr& matcher, + const Ptr& detector_, + const Ptr& matcher_, const Mat& cameraMatrix, const Mat& distCoeffs, - const OdometryParams& params) - : detector(detector), matcher(matcher), params(params) + const OdometryParams& params_) + : detector(detector_), matcher(matcher_), params(params_) { cameraMatrix.convertTo(K, CV_64F); if (!distCoeffs.empty()) diff --git a/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp index 8f6b0c29ee..cc5c806b37 100644 --- a/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp +++ b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp @@ -132,10 +132,10 @@ bool VisualOdometryImpl::shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw Point3d cCur = detail::cameraCenterWorld(T_cw); Point3d cLast = detail::cameraCenterWorld(lastKf->poseCw); Point3d d = cCur - cLast; - double dist = std::sqrt(d.dot(d)); - if (dist > params.kfTransThresh) + double transDist = std::sqrt(d.dot(d)); + if (transDist > params.kfTransThresh) { - reason = format("trans=%.3f", dist); + reason = format("trans=%.3f", transDist); return true; } diff --git a/modules/ptcloud/src/slam/odometry/vo_tracking.cpp b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp index f27147033a..6ceb1e9933 100644 --- a/modules/ptcloud/src/slam/odometry/vo_tracking.cpp +++ b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp @@ -218,9 +218,13 @@ bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) if (!hasPrevFrame || prevFrame.image.empty()) return false; const Frame& prev = prevFrame; - if (prev.undistKpts.empty()) return false; + if (prev.keypoints.empty()) return false; + + // LK runs on the raw imgs + std::vector prevPts; + prevPts.reserve(prev.keypoints.size()); + for (const auto& kp : prev.keypoints) prevPts.push_back(kp.pt); - std::vector prevPts(prev.undistKpts.begin(), prev.undistKpts.end()); std::vector curPts; std::vector status; std::vector err; @@ -229,6 +233,13 @@ bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) status, err, Size(21, 21), 3, TermCriteria(TermCriteria::COUNT | TermCriteria::EPS, 30, 0.01)); + // PnP uses K with no distortion + std::vector curUndist; + if (!dist.empty()) + undistortPoints(curPts, curUndist, K, dist, noArray(), K); + else + curUndist = curPts; + std::vector obj; std::vector img; obj.reserve(prevPts.size()); img.reserve(prevPts.size()); @@ -239,7 +250,7 @@ bool VisualOdometryImpl::trackWithOpticalFlow(Frame& cur) MapPoint* mp = prev.mapPoints[i]; if (!mp || mp->bad) continue; obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); - img.push_back(curPts[i]); + img.push_back(curUndist[i]); } if ((int)obj.size() < params.opticalFlowMinInliers) diff --git a/modules/ptcloud/src/slam/precomp.hpp b/modules/ptcloud/src/slam/precomp.hpp index d489fa9c0d..dc9fc8dff5 100644 --- a/modules/ptcloud/src/slam/precomp.hpp +++ b/modules/ptcloud/src/slam/precomp.hpp @@ -10,12 +10,8 @@ #include "opencv2/ptcloud/slam.hpp" #include "opencv2/core.hpp" -#include "opencv2/core/utility.hpp" -#include "opencv2/core/utils/filesystem.hpp" -#include "opencv2/core/utils/logger.hpp" #include "opencv2/core/private.hpp" #include "opencv2/imgproc.hpp" -#include "opencv2/imgcodecs.hpp" #include "opencv2/features.hpp" #include "opencv2/geometry.hpp" #include "opencv2/video/tracking.hpp" diff --git a/samples/slam/CMakeLists.txt b/samples/slam/CMakeLists.txt index f87637b768..2ce39a8df0 100644 --- a/samples/slam/CMakeLists.txt +++ b/samples/slam/CMakeLists.txt @@ -4,6 +4,7 @@ set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS opencv_core opencv_dnn opencv_features + opencv_imgcodecs opencv_ptcloud) ocv_check_dependencies(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index 423aa6a8d2..7f170139ee 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -5,7 +5,6 @@ #include #include -#include #include #include #include @@ -13,6 +12,7 @@ #include #include #include +#include #include "../dnn/common.hpp" @@ -21,15 +21,6 @@ using namespace std; namespace { -// path helper: joins a directory and filename, tolerating a dir with/without a trailing slash -String joinPath(const String& dir, const String& name) -{ - if (dir.empty()) return name; - char last = dir.back(); - if (last == '/' || last == '\\') return dir + name; - return dir + "/" + name; -} - // path helper: strips the directory part off a path String basenameOf(const String& path) { @@ -93,7 +84,7 @@ void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, cv::utils::fs::createDirectories(outputFolder); { - std::ofstream f(joinPath(outputFolder, "camera.txt").c_str()); + std::ofstream f(cv::utils::fs::join(outputFolder, "camera.txt").c_str()); int width = 0, height = 0; if (!vo.getMap().keyframes().empty()) { @@ -111,7 +102,7 @@ void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, } { - std::ofstream f(joinPath(outputFolder, "point3d.txt").c_str()); + 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()) @@ -124,7 +115,7 @@ void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, } { - std::ofstream f(joinPath(outputFolder, "images.txt").c_str()); + 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" @@ -170,7 +161,8 @@ const string param_keys = "{ 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 }"; + "{ 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: " @@ -223,6 +215,23 @@ int main(int argc, char** argv) 0., parser.get("fy"), parser.get("cy"), 0., 0., 1.); + // Optional lens distortion (k1,k2,p1,p2[,k3,...]); empty means no distortion. + Mat distCoeffs; + { + std::stringstream ss(parser.get("dist")); + std::vector 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; @@ -234,7 +243,7 @@ int main(int argc, char** argv) backendId, targetId); slam::OdometryParams voParams; - + voParams.minInitParallaxDeg = 1.5; voParams.minInitPoints = 50; voParams.pnpReprojThresh = 4.0; @@ -242,7 +251,7 @@ int main(int argc, char** argv) voParams.localMapTopK = 10; auto vo = slam::VisualOdometry::create( - detector, matcher, Mat(K), Mat(), voParams); + detector, matcher, Mat(K), distCoeffs, voParams); const std::vector imgFiles = listImageFiles(imagesDir); From aacac3d34923c04355cbca42d19b7d7b9f7a947c Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Fri, 24 Jul 2026 18:24:12 +0530 Subject: [PATCH 10/10] minor slamVO cleanup --- modules/ptcloud/src/slam/map.cpp | 2 -- modules/ptcloud/src/slam/precomp.hpp | 1 - 2 files changed, 3 deletions(-) diff --git a/modules/ptcloud/src/slam/map.cpp b/modules/ptcloud/src/slam/map.cpp index fc958aa3d0..cabece130e 100644 --- a/modules/ptcloud/src/slam/map.cpp +++ b/modules/ptcloud/src/slam/map.cpp @@ -6,7 +6,6 @@ #include "precomp.hpp" -#include #include #include @@ -24,7 +23,6 @@ struct Map::Impl KeyFrame* currentKf = nullptr; std::vector trajectory; - std::mutex mutex; int nextKfId = 0; int nextMpId = 0; diff --git a/modules/ptcloud/src/slam/precomp.hpp b/modules/ptcloud/src/slam/precomp.hpp index dc9fc8dff5..8bef8b8c6d 100644 --- a/modules/ptcloud/src/slam/precomp.hpp +++ b/modules/ptcloud/src/slam/precomp.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include