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/features/include/opencv2/features.hpp b/modules/features/include/opencv2/features.hpp index e18198cff2..f9d14e5b51 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 476b93f6a5..8b0f0a46dc 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/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 7602feba97..a3e785b6a4 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -4,8 +4,8 @@ set(debug_modules "") if(DEBUG_opencv_ptcloud) list(APPEND debug_modules opencv_highgui) endif() -ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann ${debug_modules} - WRAP java objc python js +ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_features opencv_video ${debug_modules} + WRAP java objc python js ) ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES}) diff --git a/modules/ptcloud/include/opencv2/ptcloud.hpp b/modules/ptcloud/include/opencv2/ptcloud.hpp index d91e0da6bd..a29bed3527 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 Cloud Processing diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam.hpp new file mode 100644 index 0000000000..2941d0ef85 --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/slam.hpp @@ -0,0 +1,23 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_PTCLOUD_SLAM_HPP +#define OPENCV_PTCLOUD_SLAM_HPP + +/** +@defgroup slam SLAM and Visual Odometry + +Monocular visual odometry pipeline. Entry point is @ref cv::slam::VisualOdometry. +Bootstraps an initial map from two-view geometry, then tracks subsequent frames +with PnP, growing the map at keyframe promotions. +*/ + +#include "opencv2/ptcloud/slam/types.hpp" +#include "opencv2/ptcloud/slam/map.hpp" +#include "opencv2/ptcloud/slam/odometry_params.hpp" +#include "opencv2/ptcloud/slam/visual_odometry.hpp" + +#endif // OPENCV_PTCLOUD_SLAM_HPP diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp new file mode 100644 index 0000000000..01fe156d55 --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/map.hpp @@ -0,0 +1,78 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_MAP_HPP +#define OPENCV_SLAM_MAP_HPP + +#include "opencv2/ptcloud/slam/types.hpp" + +#include +#include + +namespace cv { +namespace slam { + +//! @addtogroup slam +//! @{ + +/** @brief Owns all persistent SLAM state (KeyFrames and MapPoints). +Pointers from addKeyframe / addMapPoint are valid until removeMapPoint / clear. */ +class CV_EXPORTS Map +{ +public: + Map(); + ~Map(); + + Map(const Map&) = delete; + Map& operator=(const Map&) = delete; + + // Keyframes + + KeyFrame* addKeyframe(KeyFrame* kf); //!< takes ownership; assigns id if < 0 + KeyFrame* getKeyframe(int id) const; + + const std::set& keyframes() const; + int numKeyframes() const; + + // Map points + + MapPoint* addMapPoint(MapPoint* mp); //!< takes ownership; assigns id if < 0 + MapPoint* getMapPoint(int id) const; + + const std::set& mapPoints() const; + int numMapPoints() const; + + void addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp); + void removeObservation(KeyFrame* kf, MapPoint* mp); + void removeMapPoint(MapPoint* mp); + + // Reference / current keyframes + + void setRefKeyframe(KeyFrame* kf); + KeyFrame* getRefKeyframe() const; + + void setCurrentKeyframe(KeyFrame* kf); + KeyFrame* getCurrentKeyframe() const; + + // Trajectory + + void appendPose(const Matx44d& T_cw); + const std::vector& trajectory() const; + + // Lifecycle + + void clear(); + +private: + struct Impl; + Ptr impl; +}; + +//! @} + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_MAP_HPP diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp new file mode 100644 index 0000000000..bca8687994 --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp @@ -0,0 +1,65 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_ODOMETRY_PARAMS_HPP +#define OPENCV_SLAM_ODOMETRY_PARAMS_HPP + +#include "opencv2/core.hpp" + +namespace cv { +namespace slam { + +//! @addtogroup slam +//! @{ + +/** @brief Tunable parameters for visual odometry: initialization, tracking, keyframe selection, and local-map refinement. */ +struct CV_EXPORTS_W_SIMPLE OdometryParams +{ + CV_WRAP OdometryParams() {} + + // Bootstrap (two-view map initialization) + CV_PROP_RW int minInitInliers = 40; //!< Minimum match/inlier count at each bootstrap stage. + CV_PROP_RW double minInitParallaxDeg = 1.5; //!< Minimum parallax (deg) to trigger initialization. + CV_PROP_RW int minInitPoints = 50; //!< Minimum triangulated points to seed the map. + CV_PROP_RW double hfRatioThresh = 0.45; //!< Homography/fundamental score ratio above which homography is chosen. + CV_PROP_RW double minGrowthParallaxDeg = 0.1; //!< Minimum parallax (deg) to triangulate new points during map growth. + CV_PROP_RW double essentialRansacThresh = 1.0; //!< RANSAC reprojection threshold (px) for essential-matrix/homography estimation. + CV_PROP_RW double essentialRansacConfidence = 0.999; //!< RANSAC confidence for essential-matrix estimation. + + // Tracking (PnP) + CV_PROP_RW double pnpReprojThresh = 4.0; //!< PnP RANSAC reprojection threshold (px). + CV_PROP_RW int pnpMinInliers = 6; //!< Minimum PnP inliers to accept a pose. + CV_PROP_RW int pnpRansacIters = 500; //!< Maximum PnP RANSAC iterations. + CV_PROP_RW double pnpConfidence = 0.99; //!< PnP RANSAC confidence. + + // Motion model (guided match search) + CV_PROP_RW double motionModelRadius = 15.0; //!< Guided-match search radius (px). + CV_PROP_RW double motionModelRadiusWide = 30.0; //!< Wider fallback search radius (px) when the narrow search finds too few. + CV_PROP_RW int motionModelMinMatches = 20; //!< Matches below which the wider search runs. + CV_PROP_RW double descProjThresh = 1.0; //!< Descriptor-distance cutoff for a projected map-point match. + + // Optical flow fallback + CV_PROP_RW int opticalFlowMinInliers = 10; //!< Minimum correspondences for the optical-flow fallback. + + // Keyframe promotion + CV_PROP_RW int kfMinFrames = 1; //!< Minimum frames since last keyframe before inserting one. + CV_PROP_RW int kfMaxFrames = 30; //!< Frames since last keyframe after which one is forced. + CV_PROP_RW double kfInlierRatio = 0.70; //!< Insert a keyframe when inliers drop below this fraction of the last keyframe's. + CV_PROP_RW int kfMinInliers = 40; //!< Absolute inlier floor: max(kfMinInliers, kfInlierRatio * lastKfInliers). + CV_PROP_RW double kfRotThreshDeg = 5.0; //!< Rotation (deg) from last keyframe that forces a new one. + CV_PROP_RW double kfTransThresh = 0.5; //!< Translation from last keyframe that forces a new one. + + // Local map refinement + CV_PROP_RW int localMapTopK = 10; //!< Top co-visible keyframes forming the local map. + CV_PROP_RW int localMapNeighborK = 5; //!< Covisibility neighbors expanded per local-map keyframe. + CV_PROP_RW double localMapRadius = 7.0; //!< Reprojection search radius (px) for local-map points. +}; + +//! @} + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_ODOMETRY_PARAMS_HPP diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/types.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/types.hpp new file mode 100644 index 0000000000..66481aae6f --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/types.hpp @@ -0,0 +1,71 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_TYPES_HPP +#define OPENCV_SLAM_TYPES_HPP + +#include "opencv2/core.hpp" +#include "opencv2/core/types.hpp" + +#include +#include + +namespace cv { +namespace slam { + +//! @addtogroup slam +//! @{ + +// Pipeline lifecycle state +enum OdometryState +{ + NOT_INITIALIZED = 0, + INITIALIZING = 1, + TRACKING = 2 +}; + +struct MapPoint; +struct KeyFrame; + +// 3D landmark, owned by Map +struct CV_EXPORTS MapPoint +{ + int id = -1; + Point3d pos { 0, 0, 0 }; + Mat refDesc; + + std::map observations; // kf -> keypoint index + + int visibleCount = 0; + int foundCount = 0; + bool bad = false; // soft-delete; check before use +}; + +// Keyframe: pose + keypoints + covisibility graph, owned by Map +struct CV_EXPORTS KeyFrame +{ + int id = -1; + Matx44d poseCw = Matx44d::eye(); // world->camera + + std::vector keypoints; + Mat descriptors; + std::vector undistKpts; // parallel to keypoints + Size imageSize; + + std::vector mapPoints; // parallel to keypoints; null = unmatched + + 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/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp new file mode 100644 index 0000000000..43338f83d3 --- /dev/null +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp @@ -0,0 +1,79 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_VISUAL_ODOMETRY_HPP +#define OPENCV_SLAM_VISUAL_ODOMETRY_HPP + +#include "opencv2/core.hpp" +#include "opencv2/features.hpp" + +#include "opencv2/ptcloud/slam/types.hpp" +#include "opencv2/ptcloud/slam/map.hpp" +#include "opencv2/ptcloud/slam/odometry_params.hpp" + +#include + +namespace cv { +namespace slam { + +//! @addtogroup slam +//! @{ + +/** @brief Monocular visual odometry pipeline. + +State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING +(per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING. + +Purely in-memory: feed images with @ref processFrame and read back the trajectory +and map with @ref getTrajectory / @ref getMap. See samples/slam for driving this +from a directory of images and writing the result to disk (e.g. COLMAP format). +*/ +class CV_EXPORTS_W VisualOdometry +{ +public: + virtual ~VisualOdometry(); + + CV_WRAP static Ptr create( + const Ptr& detector, + const Ptr& matcher, + InputArray cameraMatrix, + InputArray distCoeffs = noArray(), + const OdometryParams& params = OdometryParams()); + + /** @brief Feed one image. Returns true if a pose was emitted. */ + CV_WRAP virtual bool processFrame(InputArray image) = 0; + + /** @brief Reset to NOT_INITIALIZED, clearing map and trajectory. */ + CV_WRAP virtual void reset() = 0; + + CV_WRAP virtual OdometryState getState() const = 0; + CV_WRAP virtual Matx44d getLastPose() const = 0; + + //! @note Not exposed to Python: Map holds raw pointers / non-convertible containers. + virtual const Map& getMap() const = 0; + + //! @brief Number of keyframes in the map. Convenience for callers (e.g. Python) that + //! can't use @ref getMap directly. + CV_WRAP virtual int getNumKeyframes() const = 0; + + //! @brief Number of map points in the map. Convenience for callers (e.g. Python) that + //! can't use @ref getMap directly. + CV_WRAP virtual int getNumMapPoints() const = 0; + + CV_WRAP virtual const std::vector& getTrajectory() const = 0; + + CV_WRAP virtual const OdometryParams& getParams() const = 0; + CV_WRAP virtual void setParams(const OdometryParams& params) = 0; + +protected: + VisualOdometry(); +}; + +//! @} + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP diff --git a/modules/ptcloud/src/slam/map.cpp b/modules/ptcloud/src/slam/map.cpp new file mode 100644 index 0000000000..cabece130e --- /dev/null +++ b/modules/ptcloud/src/slam/map.cpp @@ -0,0 +1,148 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "precomp.hpp" + +#include +#include + +namespace cv { +namespace slam { + +struct Map::Impl +{ + std::set keyframes; + std::set mapPoints; + std::unordered_map kfIndex; + std::unordered_map mpIndex; + + KeyFrame* refKf = nullptr; + KeyFrame* currentKf = nullptr; + + std::vector trajectory; + + int nextKfId = 0; + int nextMpId = 0; +}; + +Map::Map() : impl(makePtr()) {} + +Map::~Map() +{ + for (KeyFrame* kf : impl->keyframes) delete kf; + for (MapPoint* mp : impl->mapPoints) delete mp; +} + +// Keyframes + +KeyFrame* Map::addKeyframe(KeyFrame* kf) +{ + CV_Assert(kf); + if (kf->id < 0) + kf->id = impl->nextKfId++; + else if (kf->id >= impl->nextKfId) + impl->nextKfId = kf->id + 1; + impl->keyframes.insert(kf); + impl->kfIndex[kf->id] = kf; + return kf; +} + +KeyFrame* Map::getKeyframe(int id) const +{ + auto it = impl->kfIndex.find(id); + return (it != impl->kfIndex.end()) ? it->second : nullptr; +} + +const std::set& Map::keyframes() const { return impl->keyframes; } +int Map::numKeyframes() const { return (int)impl->keyframes.size(); } + +// Map points + +MapPoint* Map::addMapPoint(MapPoint* mp) +{ + CV_Assert(mp); + if (mp->id < 0) + mp->id = impl->nextMpId++; + else if (mp->id >= impl->nextMpId) + impl->nextMpId = mp->id + 1; + impl->mapPoints.insert(mp); + impl->mpIndex[mp->id] = mp; + return mp; +} + +MapPoint* Map::getMapPoint(int id) const +{ + auto it = impl->mpIndex.find(id); + return (it != impl->mpIndex.end()) ? it->second : nullptr; +} + +const std::set& Map::mapPoints() const { return impl->mapPoints; } +int Map::numMapPoints() const { return (int)impl->mapPoints.size(); } + +// Observations + +void Map::addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp) +{ + CV_Assert(kf && mp); + CV_Assert(kpIdx < kf->mapPoints.size()); + if (kf->mapPoints[kpIdx] != nullptr) return; + kf->mapPoints[kpIdx] = mp; + mp->observations[kf] = kpIdx; +} + +void Map::removeObservation(KeyFrame* kf, MapPoint* mp) +{ + if (!kf || !mp) return; + auto it = mp->observations.find(kf); + if (it == mp->observations.end()) return; + size_t kpIdx = it->second; + if (kpIdx < kf->mapPoints.size()) + kf->mapPoints[kpIdx] = nullptr; + mp->observations.erase(it); +} + +void Map::removeMapPoint(MapPoint* mp) +{ + if (!mp) return; + for (auto& [kf, kpIdx] : mp->observations) + if (kpIdx < kf->mapPoints.size()) + kf->mapPoints[kpIdx] = nullptr; + impl->mapPoints.erase(mp); + impl->mpIndex.erase(mp->id); + delete mp; +} + +// Reference / current keyframes + +void Map::setRefKeyframe(KeyFrame* kf) { impl->refKf = kf; } +KeyFrame* Map::getRefKeyframe() const { return impl->refKf; } + +void Map::setCurrentKeyframe(KeyFrame* kf) { impl->currentKf = kf; } +KeyFrame* Map::getCurrentKeyframe() const { return impl->currentKf; } + +// Trajectory + +void Map::appendPose(const Matx44d& T_cw) { impl->trajectory.push_back(T_cw); } +const std::vector& Map::trajectory() const { return impl->trajectory; } + +// Lifecycle + +void Map::clear() +{ + for (KeyFrame* kf : impl->keyframes) delete kf; + for (MapPoint* mp : impl->mapPoints) delete mp; + impl->keyframes.clear(); + impl->mapPoints.clear(); + impl->kfIndex.clear(); + impl->mpIndex.clear(); + impl->refKf = nullptr; + impl->currentKf = nullptr; + impl->trajectory.clear(); + impl->nextKfId = 0; + impl->nextMpId = 0; +} + +}} // namespace cv::slam diff --git a/modules/ptcloud/src/slam/odometry/frame.hpp b/modules/ptcloud/src/slam/odometry/frame.hpp new file mode 100644 index 0000000000..bf7e255aa1 --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/frame.hpp @@ -0,0 +1,79 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_FRAME_HPP +#define OPENCV_SLAM_FRAME_HPP + +#include "../precomp.hpp" + +namespace cv { +namespace slam { + +/** @brief Per-frame scratch pad. Created per incoming image, never stored in the Map. */ +struct Frame +{ + Mat image; + std::vector 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/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp new file mode 100644 index 0000000000..d5e3a11034 --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -0,0 +1,146 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +// Factory + +VisualOdometry::VisualOdometry() = default; +VisualOdometry::~VisualOdometry() = default; + +Ptr VisualOdometry::create( + const Ptr& detector, + const Ptr& matcher, + InputArray cameraMatrix, + InputArray distCoeffs, + const OdometryParams& params) +{ + CV_Assert(detector && "VisualOdometry::create: detector must not be null"); + CV_Assert(matcher && "VisualOdometry::create: matcher must not be null"); + + Mat K = cameraMatrix.getMat(); + CV_Assert(!K.empty() && K.rows == 3 && K.cols == 3); + Mat dist = distCoeffs.empty() ? Mat() : distCoeffs.getMat(); + + return makePtr(detector, matcher, K, dist, params); +} + +// Constructor + +VisualOdometryImpl::VisualOdometryImpl( + const Ptr& detector_, + const Ptr& matcher_, + const Mat& cameraMatrix, + const Mat& distCoeffs, + const OdometryParams& params_) + : detector(detector_), matcher(matcher_), params(params_) +{ + cameraMatrix.convertTo(K, CV_64F); + if (!distCoeffs.empty()) + distCoeffs.convertTo(dist, CV_64F); +} + +// reset / processFrame + +void VisualOdometryImpl::reset() +{ + state = NOT_INITIALIZED; + lastPoseCw = Matx44d::eye(); + refFrame = Frame(); + lastKf = nullptr; + framesSinceKf = 0; + lastKfInliers = 0; + velocity = Matx44d::eye(); + hasVelocity = false; + prevFrame = Frame(); + hasPrevFrame = false; + lastEvent.clear(); + map.clear(); +} + +bool VisualOdometryImpl::processFrame(InputArray image) +{ + CV_INSTRUMENT_REGION(); + + if (image.empty()) return false; + lastEvent.clear(); + + Frame currentFrame; + extractFeatures(image, currentFrame); + if (currentFrame.keypoints.empty() || currentFrame.descriptors.empty()) return false; + + currentFrame.mapPoints.assign(currentFrame.keypoints.size(), nullptr); + currentFrame.outliers.assign(currentFrame.keypoints.size(), false); + currentFrame.buildGrid(); + + switch (state) + { + case NOT_INITIALIZED: + refFrame = currentFrame; + state = INITIALIZING; + return false; + + case INITIALIZING: + return bootstrap(currentFrame); + + case TRACKING: + return track(currentFrame); + } + return false; +} + +// Feature extraction + +void VisualOdometryImpl::extractFeatures(InputArray image, Frame& out) const +{ + Mat img = image.getMat(); + out.imageSize = img.size(); + out.keypoints.clear(); + + // Detect and compute on the original image (color/grey is up to the detector). + detector->detectAndCompute(img, noArray(), out.keypoints, out.descriptors); + + // Store a greyscale copy for the optical-flow fallback. + if (img.channels() > 1) + cvtColor(img, out.image, COLOR_BGR2GRAY); + else + out.image = img.clone(); + + // Pre-compute undistorted pixel coordinates used by every stage. + if (!out.keypoints.empty()) + { + std::vector 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; + + matcher->setImagePairInfo(qKp, tKp, qSz, tSz); + matcher->match(qDesc, tDesc, matches); +} + +}} // namespace cv::slam diff --git a/modules/ptcloud/src/slam/odometry/vo_bootstrap.cpp b/modules/ptcloud/src/slam/odometry/vo_bootstrap.cpp new file mode 100644 index 0000000000..a4b6afe4e8 --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/vo_bootstrap.cpp @@ -0,0 +1,252 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +// Returns the homography candidate index with the most cheirality-consistent triangulations. +int bestHomographyCandidate( + const std::vector& 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/ptcloud/src/slam/odometry/vo_impl.hpp b/modules/ptcloud/src/slam/odometry/vo_impl.hpp new file mode 100644 index 0000000000..cbd539737f --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/vo_impl.hpp @@ -0,0 +1,110 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_VO_IMPL_HPP +#define OPENCV_SLAM_VO_IMPL_HPP + +#include "../precomp.hpp" +#include "frame.hpp" +#include "../optimizer/pose_optimizer.hpp" + +namespace cv { +namespace slam { + +/** @brief Concrete VisualOdometry implementation (pimpl target). + +Stage logic is split across: + - vo_bootstrap.cpp : two-view H/F initialisation + - vo_tracking.cpp : per-frame localisation (motion model, fallback 1/2, local map) + - vo_keyframe.cpp : keyframe promotion decision + covisibility helpers + - vo_map_growth.cpp : triangulation of new map points at promotion time + - visual_odometry.cpp : factory, processFrame() +*/ +class VisualOdometryImpl CV_FINAL : public VisualOdometry +{ +public: + VisualOdometryImpl(const Ptr& detector, + const Ptr& matcher, + const Mat& cameraMatrix, + const Mat& distCoeffs, + const OdometryParams& params); + + // --- VisualOdometry interface ------------------------------------------- + + bool processFrame(InputArray image) CV_OVERRIDE; + void reset() CV_OVERRIDE; + + OdometryState getState() const CV_OVERRIDE { return state; } + Matx44d getLastPose() const CV_OVERRIDE { return lastPoseCw; } + const Map& getMap() const CV_OVERRIDE { return map; } + int getNumKeyframes() const CV_OVERRIDE { return map.numKeyframes(); } + int getNumMapPoints() const CV_OVERRIDE { return map.numMapPoints(); } + const std::vector& getTrajectory() const CV_OVERRIDE { return map.trajectory(); } + const OdometryParams& getParams() const CV_OVERRIDE { return params; } + void setParams(const OdometryParams& p) CV_OVERRIDE { params = p; } + + // --- Stage entry points ------------------------------------------------- + + bool bootstrap(Frame& cur); + bool track(Frame& cur); + + bool trackWithMotionModel(Frame& cur); // motion model + bool trackWithReferenceKF(Frame& cur); // fallback 1 + bool trackWithOpticalFlow(Frame& cur); // fallback 2 + void trackLocalMap(Frame& cur); + + bool shouldPromoteKeyframe(int nInliers, const Matx44d& T_cw, String& reason) const; + void promoteKeyframeAndGrowMap(Frame& cur); + + // --- Shared helpers (visual_odometry.cpp) -------------------------------- + + void extractFeatures(InputArray image, Frame& out) const; + + void matchFrames(const std::vector& qKp, const Mat& qDesc, Size qSz, + const std::vector& tKp, const Mat& tDesc, Size tSz, + std::vector& matches) const; + + // --- Owned state --------------------------------------------------------- + + Ptr detector; + Ptr matcher; + Mat K; // 3×3 CV_64F + Mat dist; // distortion coefficients (may be empty) + OdometryParams params; + + OdometryState state = NOT_INITIALIZED; + Matx44d lastPoseCw = Matx44d::eye(); + + Frame refFrame; + KeyFrame* lastKf = nullptr; + int framesSinceKf = 0; + int lastKfInliers = 0; + + Matx44d velocity = Matx44d::eye(); + bool hasVelocity = false; + + Frame prevFrame; + bool hasPrevFrame = false; + + String lastEvent; + + Map map; +}; + +namespace detail { + +double rotationAngleDeg(const Matx44d& A_cw, const Matx44d& B_cw); +double parallaxDeg(const Point3d& X_world, const Matx44d& A_cw, const Matx44d& B_cw); +Matx34d projectionFromPose(const Matx44d& T_cw); +Matx44d makePose(const Mat& R, const Mat& t); +Point3d cameraCenterWorld(const Matx44d& T_cw); +void updateCovisibility(KeyFrame* kf); + +} // namespace detail + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_VO_IMPL_HPP diff --git a/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp new file mode 100644 index 0000000000..cc5c806b37 --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/vo_keyframe.cpp @@ -0,0 +1,145 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace detail { + +double rotationAngleDeg(const Matx44d& A, const Matx44d& B) +{ + Matx33d Ra(A(0,0),A(0,1),A(0,2), A(1,0),A(1,1),A(1,2), A(2,0),A(2,1),A(2,2)); + Matx33d Rb(B(0,0),B(0,1),B(0,2), B(1,0),B(1,1),B(1,2), B(2,0),B(2,1),B(2,2)); + Matx33d D = Rb * Ra.t(); + double tr = D(0,0) + D(1,1) + D(2,2); + double c = std::max(-1.0, std::min(1.0, (tr - 1.0) * 0.5)); + return std::acos(c) * 180.0 / CV_PI; +} + +Point3d cameraCenterWorld(const Matx44d& T_cw) +{ + Matx33d R(T_cw(0,0),T_cw(0,1),T_cw(0,2), + T_cw(1,0),T_cw(1,1),T_cw(1,2), + T_cw(2,0),T_cw(2,1),T_cw(2,2)); + Matx31d t(T_cw(0,3),T_cw(1,3),T_cw(2,3)); + Matx31d C = -R.t() * t; + return Point3d(C(0),C(1),C(2)); +} + +double parallaxDeg(const Point3d& Xw, const Matx44d& A_cw, const Matx44d& B_cw) +{ + Point3d CA = cameraCenterWorld(A_cw), CB = cameraCenterWorld(B_cw); + Point3d vA = Xw - CA, vB = Xw - CB; + double nA = std::sqrt(vA.dot(vA)), nB = std::sqrt(vB.dot(vB)); + if (nA < 1e-9 || nB < 1e-9) return 0.0; + double c = std::max(-1.0, std::min(1.0, vA.dot(vB) / (nA * nB))); + return std::acos(c) * 180.0 / CV_PI; +} + +Matx34d projectionFromPose(const Matx44d& T) +{ + return Matx34d(T(0,0),T(0,1),T(0,2),T(0,3), + T(1,0),T(1,1),T(1,2),T(1,3), + T(2,0),T(2,1),T(2,2),T(2,3)); +} + +Matx44d makePose(const Mat& R, const Mat& t) +{ + CV_Assert(R.rows == 3 && R.cols == 3 && t.total() == 3); + Matx44d T = Matx44d::eye(); + Mat Rd, td; + R.convertTo(Rd, CV_64F); + t.reshape(1,3).convertTo(td, CV_64F); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) T(i,j) = Rd.at(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 transDist = std::sqrt(d.dot(d)); + if (transDist > params.kfTransThresh) + { + reason = format("trans=%.3f", transDist); + return true; + } + + return false; +} + +}} // namespace cv::slam diff --git a/modules/ptcloud/src/slam/odometry/vo_map_growth.cpp b/modules/ptcloud/src/slam/odometry/vo_map_growth.cpp new file mode 100644 index 0000000000..6e6dfbc590 --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/vo_map_growth.cpp @@ -0,0 +1,135 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +inline Point2d projectThrough(const Mat& P, double X, double Y, double Z) +{ + double u = P.at(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/ptcloud/src/slam/odometry/vo_tracking.cpp b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp new file mode 100644 index 0000000000..6ceb1e9933 --- /dev/null +++ b/modules/ptcloud/src/slam/odometry/vo_tracking.cpp @@ -0,0 +1,401 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "vo_impl.hpp" + +namespace cv { +namespace slam { + +namespace { + +bool projectPoint(const Matx44d& T_cw, const Mat& K, + double Xw, double Yw, double Zw, + double& u, double& v) +{ + const double Xc = T_cw(0,0)*Xw + T_cw(0,1)*Yw + T_cw(0,2)*Zw + T_cw(0,3); + const double Yc = T_cw(1,0)*Xw + T_cw(1,1)*Yw + T_cw(1,2)*Zw + T_cw(1,3); + const double Zc = T_cw(2,0)*Xw + T_cw(2,1)*Yw + T_cw(2,2)*Zw + T_cw(2,3); + if (Zc <= 0.0) return false; + u = K.at(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.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 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)); + + // 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()); + + for (size_t i = 0; i < prevPts.size(); ++i) + { + if (!status[i]) continue; + if (i >= prev.mapPoints.size()) continue; + MapPoint* mp = prev.mapPoints[i]; + if (!mp || mp->bad) continue; + obj.push_back(Point3f((float)mp->pos.x,(float)mp->pos.y,(float)mp->pos.z)); + img.push_back(curUndist[i]); + } + + if ((int)obj.size() < params.opticalFlowMinInliers) + { + lastEvent = format("optflow: corr=%d < %d", (int)obj.size(), params.opticalFlowMinInliers); + return false; + } + + int nInliers = runPnP(obj, img, cur, K, + params.pnpReprojThresh, params.pnpRansacIters, + params.pnpConfidence, params.opticalFlowMinInliers); + if (nInliers < 0) + { + lastEvent = "optflow: PnP failed"; + return false; + } + + return true; +} + +// Local map refinement: expand coverage + recheck inliers +void VisualOdometryImpl::trackLocalMap(Frame& cur) +{ + if (!lastKf) return; + + std::set 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/ptcloud/src/slam/optimizer/pose_optimizer.cpp b/modules/ptcloud/src/slam/optimizer/pose_optimizer.cpp new file mode 100644 index 0000000000..95b6a7faef --- /dev/null +++ b/modules/ptcloud/src/slam/optimizer/pose_optimizer.cpp @@ -0,0 +1,49 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "../precomp.hpp" +#include "pose_optimizer.hpp" + +namespace cv { namespace slam { + +// pose is not optimized yet, only inlier classification is done. +// adding bundle adjustment based pose optimization in a later commit. + +int poseInlierCheck(Frame& frame, const Mat& K, double reprojThresh) +{ + const double fx = K.at(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/ptcloud/src/slam/optimizer/pose_optimizer.hpp b/modules/ptcloud/src/slam/optimizer/pose_optimizer.hpp new file mode 100644 index 0000000000..69c7691284 --- /dev/null +++ b/modules/ptcloud/src/slam/optimizer/pose_optimizer.hpp @@ -0,0 +1,21 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_POSE_OPTIMIZER_HPP +#define OPENCV_SLAM_POSE_OPTIMIZER_HPP + +#include "../odometry/frame.hpp" + +namespace cv { +namespace slam { + +// Classify map point associations as inlier/outlier by reprojection distance. +// Returns inlier count. frame.poseCw is NOT modified. +int poseInlierCheck(Frame& frame, const Mat& K, double reprojThresh); + +}} // namespace cv::slam + +#endif // OPENCV_SLAM_POSE_OPTIMIZER_HPP diff --git a/modules/ptcloud/src/slam/precomp.hpp b/modules/ptcloud/src/slam/precomp.hpp new file mode 100644 index 0000000000..8bef8b8c6d --- /dev/null +++ b/modules/ptcloud/src/slam/precomp.hpp @@ -0,0 +1,29 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#ifndef OPENCV_SLAM_PRECOMP_HPP +#define OPENCV_SLAM_PRECOMP_HPP + +#include "opencv2/ptcloud/slam.hpp" + +#include "opencv2/core.hpp" +#include "opencv2/core/private.hpp" +#include "opencv2/imgproc.hpp" +#include "opencv2/features.hpp" +#include "opencv2/geometry.hpp" +#include "opencv2/video/tracking.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#endif // OPENCV_SLAM_PRECOMP_HPP diff --git a/modules/ptcloud/test/test_map.cpp b/modules/ptcloud/test/test_map.cpp new file mode 100644 index 0000000000..fdee7e9853 --- /dev/null +++ b/modules/ptcloud/test/test_map.cpp @@ -0,0 +1,79 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +// Allocates a keyframe with @p n keypoint slots (parallel mapPoints[] zeroed), +// as the pipeline expects before any observation is wired. +static slam::KeyFrame* newKeyFrame(int n) +{ + slam::KeyFrame* kf = new slam::KeyFrame(); + kf->keypoints.resize(n); + kf->mapPoints.assign(n, nullptr); + return kf; +} + +TEST(SLAM_Map, registers_and_clears) +{ + slam::Map map; + + // addKeyframe / addMapPoint assign sequential ids and the index round-trips. + slam::KeyFrame* kf0 = map.addKeyframe(newKeyFrame(0)); + slam::KeyFrame* kf1 = map.addKeyframe(newKeyFrame(0)); + slam::MapPoint* mp0 = map.addMapPoint(new slam::MapPoint()); + EXPECT_EQ(kf0->id, 0); + EXPECT_EQ(kf1->id, 1); + EXPECT_EQ(mp0->id, 0); + EXPECT_EQ(map.numKeyframes(), 2); + EXPECT_EQ(map.numMapPoints(), 1); + EXPECT_EQ(map.getKeyframe(1), kf1); + EXPECT_EQ(map.getMapPoint(0), mp0); + EXPECT_TRUE(map.getKeyframe(99) == nullptr); + + // An explicit id must advance the auto-id counter so it cannot collide. + slam::KeyFrame* kfExplicit = new slam::KeyFrame(); + kfExplicit->id = 5; + map.addKeyframe(kfExplicit); + EXPECT_EQ(map.addKeyframe(newKeyFrame(0))->id, 6); + + // clear() drops all state and restarts the id counters. + map.clear(); + EXPECT_EQ(map.numKeyframes(), 0); + EXPECT_EQ(map.numMapPoints(), 0); + EXPECT_EQ(map.addKeyframe(newKeyFrame(0))->id, 0); +} + +TEST(SLAM_Map, wires_and_removes_observations) +{ + slam::Map map; + slam::KeyFrame* kfA = map.addKeyframe(newKeyFrame(3)); + slam::KeyFrame* kfB = map.addKeyframe(newKeyFrame(3)); + slam::MapPoint* mp = map.addMapPoint(new slam::MapPoint()); + + // addObservation links both directions: keyframe slot <-> observation map. + map.addObservation(kfA, 2, mp); + map.addObservation(kfB, 0, mp); + EXPECT_EQ(kfA->mapPoints[2], mp); + EXPECT_EQ(mp->observations[kfA], 2u); + EXPECT_EQ(mp->observations.size(), 2u); + + // Adding into an already-occupied slot is a no-op. + slam::MapPoint* other = map.addMapPoint(new slam::MapPoint()); + map.addObservation(kfA, 2, other); + EXPECT_EQ(kfA->mapPoints[2], mp); + EXPECT_EQ(other->observations.count(kfA), 0u); + + // removeMapPoint unlinks every observing keyframe and drops the point. + const int mpId = mp->id; + map.removeMapPoint(mp); // deletes mp; must not be dereferenced afterwards + EXPECT_TRUE(map.getMapPoint(mpId) == nullptr); + EXPECT_TRUE(kfA->mapPoints[2] == nullptr); + EXPECT_TRUE(kfB->mapPoints[0] == nullptr); +} + +}} // namespace opencv_test 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/ptcloud/test/test_visual_odometry.cpp b/modules/ptcloud/test/test_visual_odometry.cpp new file mode 100644 index 0000000000..a1c9eeee2c --- /dev/null +++ b/modules/ptcloud/test/test_visual_odometry.cpp @@ -0,0 +1,251 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. +// Third party copyrights are property of their respective owners. + +#include "test_precomp.hpp" + +namespace opencv_test { namespace { + +// The slam module has no committed image data, so - as in the geometry module's +// tests - the pipeline is exercised on a synthetic scene: a fixed 3D point cloud +// (generated with theRNG()) projected through a pin-hole camera that translates +// along +X. A stub Feature2D replays the projected keypoints frame by frame and +// a brute-force matcher recovers the ground-truth correspondences, so +// VisualOdometry runs end-to-end without a real detector or image files. + +const int descDim = 8; +const int cloudSize = 400; +const Size imageSize(640, 480); + +// Camera centres along +X: a wide first baseline for good bootstrap parallax, +// then small uniform steps suited to per-frame tracking. +static const double camCenters[] = { 0.0, 0.8, 1.1, 1.4, 1.7, 2.0 }; + +static Matx33d cameraMatrix() +{ + return Matx33d(500, 0, 320, + 0, 500, 240, + 0, 0, 1); +} + +// Each landmark gets a globally-unique, view-invariant descriptor: identical for +// the same 3D point in every frame (so the matcher pairs them at distance 0) and +// >= sqrt(descDim) apart for distinct points (well above descProjThresh, so +// the projection search during tracking never mismatches). +static Mat makeDescriptor(int id) +{ + return Mat(1, descDim, CV_32F, Scalar((double)(id + 1))); +} + +// Stub Feature2D: replays pre-computed keypoints/descriptors, one frame per +// detectAndCompute() call (processFrame() invokes the detector exactly once). +class StubDetector CV_FINAL : public Feature2D +{ +public: + struct FrameFeatures { std::vector keypoints; Mat descriptors; }; + std::vector frames; + size_t next = 0; + + 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; + } +}; + +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; +} + +// 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) +{ + 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); + + 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; +} + +// 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()) +{ + std::vector cloud = makeCloud(); + Ptr detector = makePtr(); + for (int f = 0; f < nFrames; f++) + detector->frames.push_back(renderFrame(cloud, camCenters[f])); + + Ptr matcher = BFMatcher::create(NORM_L2, true); + return slam::VisualOdometry::create(detector, matcher, + Mat(cameraMatrix()), noArray(), params); +} + +static Mat blankImage() { return Mat::zeros(imageSize, CV_8UC1); } + +// Rotation magnitude (deg) of @p T's rotation block relative to identity. +static double rotationFromIdentityDeg(const Matx44d& T) +{ + const double trace = T(0,0) + T(1,1) + T(2,2); + const double c = std::max(-1.0, std::min(1.0, (trace - 1.0) * 0.5)); + return std::acos(c) * 180.0 / CV_PI; +} + +// Camera centre in world coordinates: C = -R^T t. +static Point3d cameraCenter(const Matx44d& T) +{ + Matx33d R(T(0,0),T(0,1),T(0,2), T(1,0),T(1,1),T(1,2), T(2,0),T(2,1),T(2,2)); + Matx31d t(T(0,3), T(1,3), T(2,3)); + Matx31d C = -R.t() * t; + return Point3d(C(0), C(1), C(2)); +} + +TEST(SLAM_VisualOdometry, create_validates_arguments) +{ + Ptr 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 1a8969449c..112aaf9c03 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) # HACK: CMake 4.x finds and links wrong OpenCL in 32-bit builds on Windows x64 @@ -127,6 +128,7 @@ if(WIN32) add_subdirectory(directx) endif() add_subdirectory(dnn) +add_subdirectory(slam) # add_subdirectory(gpu) # HACK: CMake 4.x finds and links wrong OpenCL in 32-bit builds on Windows x64 if(NOT (WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 4)) diff --git a/samples/slam/CMakeLists.txt b/samples/slam/CMakeLists.txt new file mode 100644 index 0000000000..2ce39a8df0 --- /dev/null +++ b/samples/slam/CMakeLists.txt @@ -0,0 +1,23 @@ +ocv_install_example_src(slam *.cpp *.hpp CMakeLists.txt) + +set(OPENCV_SLAM_SAMPLES_REQUIRED_DEPS + opencv_core + opencv_dnn + opencv_features + opencv_imgcodecs + opencv_ptcloud) +ocv_check_dependencies(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) + +if(NOT BUILD_EXAMPLES OR NOT OCV_DEPENDENCIES_FOUND) + return() +endif() + +project(slam_samples) +ocv_include_modules_recurse(${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) +include_directories("${CMAKE_CURRENT_SOURCE_DIR}/../dnn") + +file(GLOB_RECURSE slam_samples RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cpp) +foreach(sample_filename ${slam_samples}) + ocv_define_sample(tgt ${sample_filename} slam) + ocv_target_link_libraries(${tgt} PRIVATE ${OPENCV_LINKER_LIBS} ${OPENCV_SLAM_SAMPLES_REQUIRED_DEPS}) +endforeach() diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp new file mode 100644 index 0000000000..7f170139ee --- /dev/null +++ b/samples/slam/visual_odometry.cpp @@ -0,0 +1,320 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. +// Copyright (C) 2026, BigVision LLC, all rights reserved. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../dnn/common.hpp" + +using namespace cv; +using namespace std; + +namespace { + +// path helper: strips the directory part off a path +String basenameOf(const String& path) +{ + const size_t slash = path.find_last_of("/\\"); + return (slash == String::npos) ? path : path.substr(slash + 1); +} + +// reader: lists readable image files under a directory, sorted by filename +std::vector 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(cv::utils::fs::join(outputFolder, "camera.txt").c_str()); + int width = 0, height = 0; + if (!vo.getMap().keyframes().empty()) + { + const slam::KeyFrame* kf = *vo.getMap().keyframes().begin(); + width = kf->imageSize.width; + height = kf->imageSize.height; + } + f.setf(std::ios::fixed); f.precision(4); + f << "fx " << K.at(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(cv::utils::fs::join(outputFolder, "point3d.txt").c_str()); + f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; + f.setf(std::ios::scientific); f.precision(9); + for (slam::MapPoint* mp : vo.getMap().mapPoints()) + { + if (!mp || mp->bad) continue; + f << mp->id << " " + << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " " + << mp->observations.size() << "\n"; + } + } + + { + std::ofstream f(cv::utils::fs::join(outputFolder, "images.txt").c_str()); + const auto& traj = vo.getTrajectory(); + f << "# Image list with two lines of data per image:\n" + << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + << "# POINTS2D[] as (X, Y, POINT3D_ID)\n" + << "# Number of images: " << traj.size() << ", mean observations per image: 0.0\n"; + f.setf(std::ios::fixed); f.precision(6); + + for (size_t i = 0; i < traj.size(); ++i) + { + const Matx44d& T = traj[i]; + Matx33d R; + for (int r = 0; r < 3; ++r) + for (int c = 0; c < 3; ++c) R(r, c) = T(r, c); + + const Quatd q = Quatd::createFromRotMat(R); + const String name = (i < poseFilenames.size()) + ? basenameOf(poseFilenames[i]) + : (String("pose_") + std::to_string(i)); + + f << i << " " << q.w << " " << q.x << " " << q.y << " " << q.z << " " + << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n"; + } + } +} + +} // namespace + +const string about = + "Monocular visual odometry using ALIKED + LightGlue.\n\n" + "Runs feature detection (ALIKED) and matching (LightGlue) from ONNX models over a\n" + "directory of images, estimates the camera trajectory and writes it to --output.\n" + "To run:\n" + "\t ./example_slam_visual_odometry --aliked=aliked.onnx --lightglue=lightglue.onnx --images=./seq\n" + "Sample command (run on the GPU):\n" + "\t ./example_slam_visual_odometry --aliked=aliked.onnx --lightglue=lightglue.onnx --images=./seq --target=cuda\n"; + +const string param_keys = + "{ help h | | Print help message }" + "{ aliked | | 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 }" + "{ dist | | Lens distortion coeffs, comma-separated k1,k2,p1,p2[,k3,...] (default: none) }"; + +const string backend_keys = format( + "{ backend | default | Choose one of computation backends: " + "default: automatically (by default), " + "openvino: Intel's Deep Learning Inference Engine (https://software.intel.com/openvino-toolkit), " + "opencv: OpenCV implementation, " + "vkcom: VKCOM, " + "cuda: CUDA, " + "webnn: WebNN }"); + +const string target_keys = format( + "{ target | cpu | Choose one of target computation devices: " + "cpu: CPU target (by default), " + "opencl: OpenCL, " + "opencl_fp16: OpenCL fp16 (half-float precision), " + "vpu: VPU, " + "vulkan: Vulkan, " + "cuda: CUDA, " + "cuda_fp16: CUDA fp16 (half-float preprocess) }"); + +string keys = param_keys + backend_keys + target_keys; + +int main(int argc, char** argv) +{ + CommandLineParser parser(argc, argv, keys); + parser.about(about); + + if (parser.has("help")) + { + parser.printMessage(); + return 0; + } + + const String alikedPath = parser.get("aliked"); + const String lightgluePath = parser.get("lightglue"); + 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 == "") + { + 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.); + + // 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; + detParams.backend = backendId; + detParams.target = targetId; + auto detector = ALIKED::create(alikedPath, detParams); + + auto matcher = LightGlueMatcher::create(lightgluePath, 0.0f, + backendId, targetId); + + slam::OdometryParams voParams; + + voParams.minInitParallaxDeg = 1.5; + voParams.minInitPoints = 50; + voParams.pnpReprojThresh = 4.0; + voParams.kfMaxFrames = 30; + voParams.localMapTopK = 10; + + auto vo = slam::VisualOdometry::create( + detector, matcher, Mat(K), distCoeffs, voParams); + + const std::vector imgFiles = listImageFiles(imagesDir); + + if (imgFiles.empty()) + { + std::cerr << "no images found in " << imagesDir << "\n"; + return 1; + } + + std::cout << "images folder : " << imagesDir << "\n" + << "output folder : " << outputDir << "\n" + << "total frames : " << imgFiles.size() << "\n\n"; + + PoseFilenameTracker poseTracker; + int nEmitted = 0; + + const int nDigits = std::to_string(imgFiles.size()).size(); + const int64 t0 = getTickCount(); + for (size_t i = 0; i < imgFiles.size(); ++i) + { + const int64 tFrameStart = getTickCount(); + + Mat img = imread(imgFiles[i]); + if (img.empty()) + { + std::cerr << "Frame " << std::setw(nDigits) << (i + 1) << "/" << imgFiles.size() + << " failed to read: " << imgFiles[i] << "\n"; + continue; + } + + const slam::OdometryState before = vo->getState(); + const bool emitted = vo->processFrame(img); + const slam::OdometryState after = vo->getState(); + if (emitted) ++nEmitted; + + poseTracker.update(before, after, vo->getTrajectory().size(), imgFiles[i]); + + const double frameElapsed = (getTickCount() - tFrameStart) / getTickFrequency(); + const double fps = frameElapsed > 0. ? 1. / frameElapsed : 0.; + + std::cout << "Frame " << std::setw(nDigits) << (i + 1) << "/" << imgFiles.size() + << " processed | fps: " << std::fixed << std::setprecision(1) << std::setw(5) << fps + << " | emitted: " << (emitted ? "yes" : "no ") + << " | keyframes: " << vo->getMap().numKeyframes() + << " | map points: " << vo->getMap().numMapPoints() << "\n"; + } + const double elapsed = (getTickCount() - t0) / getTickFrequency(); + const double avgFps = elapsed > 0. ? imgFiles.size() / elapsed : 0.; + const bool ok = nEmitted > 0; + + if (ok && !outputDir.empty()) + writeColmapFiles(*vo, Mat(K), poseTracker.filenames(), outputDir); + + std::cout << "\n" + << "==================== Visual Odometry Result ====================\n" + << "status : " << (ok ? "OK" : "FAILED") << "\n" + << "total frames : " << imgFiles.size() << "\n" + << "camera poses : " << vo->getTrajectory().size() << "\n" + << "keyframes : " << vo->getMap().numKeyframes() << "\n" + << "map points : " << vo->getMap().numMapPoints() << "\n" + << "elapsed time : " << std::fixed << std::setprecision(2) << elapsed << " s\n" + << "average fps : " << std::fixed << std::setprecision(2) << avgFps << "\n" + << "output dir : " << outputDir << "\n" + << "====================================================================\n"; + return ok ? 0 : 1; +} diff --git a/samples/slam/visual_odometry.py b/samples/slam/visual_odometry.py new file mode 100644 index 0000000000..f3268c17d5 --- /dev/null +++ b/samples/slam/visual_odometry.py @@ -0,0 +1,192 @@ +''' +Monocular visual odometry with cv.slam.VisualOdometry (ALIKED + LightGlue). + +Example: + python visual_odometry.py --aliked aliked.onnx --lightglue lg.onnx --images ./seq +''' + +import argparse +import glob +import os +import sys +import time +import numpy as np +import cv2 as cv + + +def build_K(fx, fy, cx, cy): + return np.array([[fx, 0., cx], + [0., fy, cy], + [0., 0., 1.]], dtype=np.float64) + + +def list_image_files(images_dir): + files = [f for f in sorted(glob.glob(os.path.join(images_dir, '*'))) + if cv.haveImageReader(f)] + return files + + +def rotation_matrix_to_quaternion(R): + # Shepperd's method: numerically stable for all rotations. + trace = R[0, 0] + R[1, 1] + R[2, 2] + if trace > 0: + s = np.sqrt(trace + 1.0) * 2 + qw = 0.25 * s + qx = (R[2, 1] - R[1, 2]) / s + qy = (R[0, 2] - R[2, 0]) / s + qz = (R[1, 0] - R[0, 1]) / s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 + qw = (R[2, 1] - R[1, 2]) / s + qx = 0.25 * s + qy = (R[0, 1] + R[1, 0]) / s + qz = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 + qw = (R[0, 2] - R[2, 0]) / s + qx = (R[0, 1] + R[1, 0]) / s + qy = 0.25 * s + qz = (R[1, 2] + R[2, 1]) / s + else: + s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 + qw = (R[1, 0] - R[0, 1]) / s + qx = (R[0, 2] + R[2, 0]) / s + qy = (R[1, 2] + R[2, 1]) / s + qz = 0.25 * s + return qw, qx, qy, qz + + +def write_colmap_files(vo, K, image_size, pose_filenames, output_dir): + # Note: cv.slam.Map isn't wrapped for Python (only aggregate counts are, + # via getNumKeyframes/getNumMapPoints), so only camera intrinsics and the + # trajectory (images.txt) can be exported here; point3d.txt (map points) + # requires the C++ sample. + os.makedirs(output_dir, exist_ok=True) + + with open(os.path.join(output_dir, 'camera.txt'), 'w') as f: + f.write(f"fx {K[0, 0]:.4f}\n") + f.write(f"fy {K[1, 1]:.4f}\n") + f.write(f"cx {K[0, 2]:.4f}\n") + f.write(f"cy {K[1, 2]:.4f}\n") + f.write(f"width {image_size[0]}\n") + f.write(f"height {image_size[1]}\n") + + traj = vo.getTrajectory() + with open(os.path.join(output_dir, 'images.txt'), 'w') as f: + f.write("# Image list with two lines of data per image:\n") + f.write("# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n") + f.write("# POINTS2D[] as (X, Y, POINT3D_ID)\n") + f.write(f"# Number of images: {len(traj)}, mean observations per image: 0.0\n") + for i, T in enumerate(traj): + qw, qx, qy, qz = rotation_matrix_to_quaternion(T[:3, :3]) + name = (os.path.basename(pose_filenames[i]) if i < len(pose_filenames) + else f"pose_{i}") + f.write(f"{i} {qw:.6f} {qx:.6f} {qy:.6f} {qz:.6f} " + f"{T[0,3]:.6f} {T[1,3]:.6f} {T[2,3]:.6f} 1 {name}\n") + + +def main(): + parser = argparse.ArgumentParser( + description='Monocular visual odometry using ALIKED + LightGlue') + parser.add_argument('--aliked', required=True, + help='Path to ALIKED ONNX model') + parser.add_argument('--lightglue', required=True, + help='Path to LightGlue ONNX model') + parser.add_argument('--images', required=True, + help='Path to directory with input images') + parser.add_argument('--output', default='vo_out', + help='Output directory for trajectory and map (default: vo_out)') + parser.add_argument('--fx', type=float, default=718.856, + help='Camera focal length X (default: KITTI-00)') + parser.add_argument('--fy', type=float, default=718.856, + help='Camera focal length Y (default: KITTI-00)') + parser.add_argument('--cx', type=float, default=607.1928, + help='Camera principal point X (default: KITTI-00)') + parser.add_argument('--cy', type=float, default=185.2157, + help='Camera principal point Y (default: KITTI-00)') + parser.add_argument('--min-parallax', type=float, default=1.5, + help='Minimum initialisation parallax in degrees (default: 1.5)') + parser.add_argument('--min-points', type=int, default=50, + help='Minimum initialisation map points (default: 50)') + args = parser.parse_args() + + det_params = cv.ALIKED.Params() + det_params.inputSize = (640, 640) + det_params.engine = cv.dnn.ENGINE_NEW + detector = cv.ALIKED.create(args.aliked, det_params) + + matcher = cv.LightGlueMatcher.create( + args.lightglue, 0.0, + cv.dnn.DNN_BACKEND_DEFAULT, + cv.dnn.DNN_TARGET_CPU) + + vo_params = cv.slam.OdometryParams() + vo_params.minInitParallaxDeg = args.min_parallax + vo_params.minInitPoints = args.min_points + + K = build_K(args.fx, args.fy, args.cx, args.cy) + + vo = cv.slam.VisualOdometry.create( + detector, matcher, K, np.array([]), vo_params) + + image_files = list_image_files(args.images) + if not image_files: + print(f"no images found in {args.images}", file=sys.stderr) + return 1 + + print(f"images_folder = {args.images}") + print(f"output_folder = {args.output}") + print(f"found {len(image_files)} image(s)") + + # Tracks which input image each emitted trajectory pose came from, for images.txt. + pose_filenames = [] + prev_traj_len = 0 + ref_filename = None + image_size = (0, 0) + n_emitted = 0 + + t0 = time.perf_counter() + for i, path in enumerate(image_files): + img = cv.imread(path) + if img is None: + print(f"[FRAME {i}] file={path} imread failed", file=sys.stderr) + continue + image_size = (img.shape[1], img.shape[0]) + + before = vo.getState() + emitted = vo.processFrame(img) + after = vo.getState() + if emitted: + n_emitted += 1 + + # Track which input image maps to each trajectory pose. + if before == cv.slam.NOT_INITIALIZED or \ + (before == cv.slam.TRACKING and after == cv.slam.INITIALIZING): + ref_filename = path + + traj_len = len(vo.getTrajectory()) + added = traj_len - prev_traj_len + if added == 1: + pose_filenames.append(path) + elif added == 2: + pose_filenames.append(ref_filename) + pose_filenames.append(path) + prev_traj_len = traj_len + + print(f"[FRAME {i}] file={path}" + f" emitted={'yes' if emitted else 'no'}" + f" keyframes={vo.getNumKeyframes()}" + f" map_points={vo.getNumMapPoints()}") + elapsed = time.perf_counter() - t0 + ok = n_emitted > 0 + + if ok and args.output: + write_colmap_files(vo, K, image_size, pose_filenames, args.output) + + print(f"run={'ok' if ok else 'FAILED'} frames={len(vo.getTrajectory())} elapsed={elapsed:.2f}s") + print(f"output -> {args.output}") + return 0 if ok else 1 + + +if __name__ == '__main__': + sys.exit(main())