mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 07:43:03 +04:00
Merge pull request #29331 from agrim-rai/slamVO
[GSOC] Added Visual Odometry (VO) for SLAM
This commit is contained in:
Abhishek Gola
GitHub
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -0,0 +1,78 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
// Copyright (C) 2026, BigVision LLC, all rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
|
||||
#ifndef OPENCV_SLAM_MAP_HPP
|
||||
#define OPENCV_SLAM_MAP_HPP
|
||||
|
||||
#include "opencv2/ptcloud/slam/types.hpp"
|
||||
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace cv {
|
||||
namespace slam {
|
||||
|
||||
//! @addtogroup slam
|
||||
//! @{
|
||||
|
||||
/** @brief Owns all persistent SLAM state (KeyFrames and MapPoints).
|
||||
Pointers from addKeyframe / addMapPoint are valid until removeMapPoint / clear. */
|
||||
class CV_EXPORTS Map
|
||||
{
|
||||
public:
|
||||
Map();
|
||||
~Map();
|
||||
|
||||
Map(const Map&) = delete;
|
||||
Map& operator=(const Map&) = delete;
|
||||
|
||||
// Keyframes
|
||||
|
||||
KeyFrame* addKeyframe(KeyFrame* kf); //!< takes ownership; assigns id if < 0
|
||||
KeyFrame* getKeyframe(int id) const;
|
||||
|
||||
const std::set<KeyFrame*>& keyframes() const;
|
||||
int numKeyframes() const;
|
||||
|
||||
// Map points
|
||||
|
||||
MapPoint* addMapPoint(MapPoint* mp); //!< takes ownership; assigns id if < 0
|
||||
MapPoint* getMapPoint(int id) const;
|
||||
|
||||
const std::set<MapPoint*>& mapPoints() const;
|
||||
int numMapPoints() const;
|
||||
|
||||
void addObservation(KeyFrame* kf, size_t kpIdx, MapPoint* mp);
|
||||
void removeObservation(KeyFrame* kf, MapPoint* mp);
|
||||
void removeMapPoint(MapPoint* mp);
|
||||
|
||||
// Reference / current keyframes
|
||||
|
||||
void setRefKeyframe(KeyFrame* kf);
|
||||
KeyFrame* getRefKeyframe() const;
|
||||
|
||||
void setCurrentKeyframe(KeyFrame* kf);
|
||||
KeyFrame* getCurrentKeyframe() const;
|
||||
|
||||
// Trajectory
|
||||
|
||||
void appendPose(const Matx44d& T_cw);
|
||||
const std::vector<Matx44d>& trajectory() const;
|
||||
|
||||
// Lifecycle
|
||||
|
||||
void clear();
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
|
||||
//! @}
|
||||
|
||||
}} // namespace cv::slam
|
||||
|
||||
#endif // OPENCV_SLAM_MAP_HPP
|
||||
@@ -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
|
||||
@@ -0,0 +1,71 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
// Copyright (C) 2026, BigVision LLC, all rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
|
||||
#ifndef OPENCV_SLAM_TYPES_HPP
|
||||
#define OPENCV_SLAM_TYPES_HPP
|
||||
|
||||
#include "opencv2/core.hpp"
|
||||
#include "opencv2/core/types.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
namespace cv {
|
||||
namespace slam {
|
||||
|
||||
//! @addtogroup slam
|
||||
//! @{
|
||||
|
||||
// Pipeline lifecycle state
|
||||
enum OdometryState
|
||||
{
|
||||
NOT_INITIALIZED = 0,
|
||||
INITIALIZING = 1,
|
||||
TRACKING = 2
|
||||
};
|
||||
|
||||
struct MapPoint;
|
||||
struct KeyFrame;
|
||||
|
||||
// 3D landmark, owned by Map
|
||||
struct CV_EXPORTS MapPoint
|
||||
{
|
||||
int id = -1;
|
||||
Point3d pos { 0, 0, 0 };
|
||||
Mat refDesc;
|
||||
|
||||
std::map<KeyFrame*, size_t> observations; // kf -> keypoint index
|
||||
|
||||
int visibleCount = 0;
|
||||
int foundCount = 0;
|
||||
bool bad = false; // soft-delete; check before use
|
||||
};
|
||||
|
||||
// Keyframe: pose + keypoints + covisibility graph, owned by Map
|
||||
struct CV_EXPORTS KeyFrame
|
||||
{
|
||||
int id = -1;
|
||||
Matx44d poseCw = Matx44d::eye(); // world->camera
|
||||
|
||||
std::vector<KeyPoint> keypoints;
|
||||
Mat descriptors;
|
||||
std::vector<Point2f> undistKpts; // parallel to keypoints
|
||||
Size imageSize;
|
||||
|
||||
std::vector<MapPoint*> mapPoints; // parallel to keypoints; null = unmatched
|
||||
|
||||
std::map<KeyFrame*, int> covisibility;
|
||||
std::vector<std::pair<KeyFrame*, int>> orderedCovisibility; // sorted descending by count
|
||||
|
||||
KeyFrame* parent = nullptr;
|
||||
Mat globalDesc;
|
||||
};
|
||||
|
||||
//! @}
|
||||
|
||||
}} // namespace cv::slam
|
||||
|
||||
#endif // OPENCV_SLAM_TYPES_HPP
|
||||
@@ -0,0 +1,79 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
// Copyright (C) 2026, BigVision LLC, all rights reserved.
|
||||
// Third party copyrights are property of their respective owners.
|
||||
|
||||
#ifndef OPENCV_SLAM_VISUAL_ODOMETRY_HPP
|
||||
#define OPENCV_SLAM_VISUAL_ODOMETRY_HPP
|
||||
|
||||
#include "opencv2/core.hpp"
|
||||
#include "opencv2/features.hpp"
|
||||
|
||||
#include "opencv2/ptcloud/slam/types.hpp"
|
||||
#include "opencv2/ptcloud/slam/map.hpp"
|
||||
#include "opencv2/ptcloud/slam/odometry_params.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace cv {
|
||||
namespace slam {
|
||||
|
||||
//! @addtogroup slam
|
||||
//! @{
|
||||
|
||||
/** @brief Monocular visual odometry pipeline.
|
||||
|
||||
State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING
|
||||
(per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING.
|
||||
|
||||
Purely in-memory: feed images with @ref processFrame and read back the trajectory
|
||||
and map with @ref getTrajectory / @ref getMap. See samples/slam for driving this
|
||||
from a directory of images and writing the result to disk (e.g. COLMAP format).
|
||||
*/
|
||||
class CV_EXPORTS_W VisualOdometry
|
||||
{
|
||||
public:
|
||||
virtual ~VisualOdometry();
|
||||
|
||||
CV_WRAP static Ptr<VisualOdometry> create(
|
||||
const Ptr<Feature2D>& detector,
|
||||
const Ptr<DescriptorMatcher>& matcher,
|
||||
InputArray cameraMatrix,
|
||||
InputArray distCoeffs = noArray(),
|
||||
const OdometryParams& params = OdometryParams());
|
||||
|
||||
/** @brief Feed one image. Returns true if a pose was emitted. */
|
||||
CV_WRAP virtual bool processFrame(InputArray image) = 0;
|
||||
|
||||
/** @brief Reset to NOT_INITIALIZED, clearing map and trajectory. */
|
||||
CV_WRAP virtual void reset() = 0;
|
||||
|
||||
CV_WRAP virtual OdometryState getState() const = 0;
|
||||
CV_WRAP virtual Matx44d getLastPose() const = 0;
|
||||
|
||||
//! @note Not exposed to Python: Map holds raw pointers / non-convertible containers.
|
||||
virtual const Map& getMap() const = 0;
|
||||
|
||||
//! @brief Number of keyframes in the map. Convenience for callers (e.g. Python) that
|
||||
//! can't use @ref getMap directly.
|
||||
CV_WRAP virtual int getNumKeyframes() const = 0;
|
||||
|
||||
//! @brief Number of map points in the map. Convenience for callers (e.g. Python) that
|
||||
//! can't use @ref getMap directly.
|
||||
CV_WRAP virtual int getNumMapPoints() const = 0;
|
||||
|
||||
CV_WRAP virtual const std::vector<Matx44d>& getTrajectory() const = 0;
|
||||
|
||||
CV_WRAP virtual const OdometryParams& getParams() const = 0;
|
||||
CV_WRAP virtual void setParams(const OdometryParams& params) = 0;
|
||||
|
||||
protected:
|
||||
VisualOdometry();
|
||||
};
|
||||
|
||||
//! @}
|
||||
|
||||
}} // namespace cv::slam
|
||||
|
||||
#endif // OPENCV_SLAM_VISUAL_ODOMETRY_HPP
|
||||
Reference in New Issue
Block a user