potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-29 15:23:05 +04:00
Code Packages Releases Wiki Activity

slam:VO fix part 1

Browse Source
This commit is contained in:
Agrim Rai
2026-07-07 15:06:18 +05:30
parent bf5224cfa8
commit 747a71fdfe
5 changed files with 51 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/ptcloud/include/opencv2/ptcloud/slam/odometry_params.hpp
+28 -27
View File
@@ -15,46 +15,47 @@ namespace slam {
//! @addtogroup slam
//! @{
/** @brief Tunable parameters for visual odometry: initialization, tracking, keyframe selection, and local-map refinement. */
struct CV_EXPORTS_W_SIMPLE OdometryParams
{
CV_WRAP OdometryParams() {}
// Bootstrap
CV_PROP_RW int minInitInliers = 40;
CV_PROP_RW double minInitParallaxDeg = 1.5;
CV_PROP_RW int minInitPoints = 50;
CV_PROP_RW double hfRatioThresh = 0.45;
CV_PROP_RW double minGrowthParallaxDeg = 0.1;
CV_PROP_RW double essentialRansacThresh = 1.0;
CV_PROP_RW double essentialRansacConfidence = 0.999;
// Bootstrap (two-view map initialization)
CV_PROP_RW int minInitInliers = 40; //!< Minimum match/inlier count at each bootstrap stage.
CV_PROP_RW double minInitParallaxDeg = 1.5; //!< Minimum parallax (deg) to trigger initialization.
CV_PROP_RW int minInitPoints = 50; //!< Minimum triangulated points to seed the map.
CV_PROP_RW double hfRatioThresh = 0.45; //!< Homography/fundamental score ratio above which homography is chosen.
CV_PROP_RW double minGrowthParallaxDeg = 0.1; //!< Minimum parallax (deg) to triangulate new points during map growth.
CV_PROP_RW double essentialRansacThresh = 1.0; //!< RANSAC reprojection threshold (px) for essential-matrix/homography estimation.
CV_PROP_RW double essentialRansacConfidence = 0.999; //!< RANSAC confidence for essential-matrix estimation.
// Tracking (PnP)
CV_PROP_RW double pnpReprojThresh = 4.0;
CV_PROP_RW int pnpMinInliers = 6;
CV_PROP_RW int pnpRansacIters = 500;
CV_PROP_RW double pnpConfidence = 0.99;
CV_PROP_RW double pnpReprojThresh = 4.0; //!< PnP RANSAC reprojection threshold (px).
CV_PROP_RW int pnpMinInliers = 6; //!< Minimum PnP inliers to accept a pose.
CV_PROP_RW int pnpRansacIters = 500; //!< Maximum PnP RANSAC iterations.
CV_PROP_RW double pnpConfidence = 0.99; //!< PnP RANSAC confidence.
// Motion model
CV_PROP_RW double motionModelRadius = 15.0;
CV_PROP_RW double motionModelRadiusWide = 30.0;
CV_PROP_RW int motionModelMinMatches = 20;
CV_PROP_RW double descProjThresh = 1.0;
// Motion model (guided match search)
CV_PROP_RW double motionModelRadius = 15.0; //!< Guided-match search radius (px).
CV_PROP_RW double motionModelRadiusWide = 30.0; //!< Wider fallback search radius (px) when the narrow search finds too few.
CV_PROP_RW int motionModelMinMatches = 20; //!< Matches below which the wider search runs.
CV_PROP_RW double descProjThresh = 1.0; //!< Descriptor-distance cutoff for a projected map-point match.
// Optical flow fallback
CV_PROP_RW int opticalFlowMinInliers = 10;
CV_PROP_RW int opticalFlowMinInliers = 10; //!< Minimum correspondences for the optical-flow fallback.
// Keyframe promotion
CV_PROP_RW int kfMinFrames = 1;
CV_PROP_RW int kfMaxFrames = 30;
CV_PROP_RW double kfInlierRatio = 0.70;
CV_PROP_RW int kfMinInliers = 40;
CV_PROP_RW double kfRotThreshDeg = 5.0;
CV_PROP_RW double kfTransThresh = 0.5;
CV_PROP_RW int kfMinFrames = 1; //!< Minimum frames since last keyframe before inserting one.
CV_PROP_RW int kfMaxFrames = 30; //!< Frames since last keyframe after which one is forced.
CV_PROP_RW double kfInlierRatio = 0.70; //!< Insert a keyframe when inliers drop below this fraction of the last keyframe's.
CV_PROP_RW int kfMinInliers = 40; //!< Absolute inlier floor: max(kfMinInliers, kfInlierRatio * lastKfInliers).
CV_PROP_RW double kfRotThreshDeg = 5.0; //!< Rotation (deg) from last keyframe that forces a new one.
CV_PROP_RW double kfTransThresh = 0.5; //!< Translation from last keyframe that forces a new one.
// Local map refinement
CV_PROP_RW int localMapTopK = 10;
CV_PROP_RW int localMapNeighborK = 5;
CV_PROP_RW double localMapRadius = 7.0;
CV_PROP_RW int localMapTopK = 10; //!< Top co-visible keyframes forming the local map.
CV_PROP_RW int localMapNeighborK = 5; //!< Covisibility neighbors expanded per local-map keyframe.
CV_PROP_RW double localMapRadius = 7.0; //!< Reprojection search radius (px) for local-map points.
};
//! @}
modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp
+1 -2
View File
@@ -27,8 +27,7 @@ namespace slam {
State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING
(per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING.
@ref run writes trajectory.txt, trajectory.bin, map_points.txt, keypoints.txt,
images.txt, and vo.log into `outputFolder`.
@ref run writes camera.txt, point3d.txt, and images.txt into `outputFolder`.
*/
class CV_EXPORTS_W VisualOdometry
{
modules/ptcloud/include/opencv2/slam.hpp
-14
View File
@@ -1,14 +0,0 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
// Copyright (C) 2026, BigVision LLC, all rights reserved.
// Third party copyrights are property of their respective owners.
#ifndef OPENCV_SLAM_HPP
#define OPENCV_SLAM_HPP
// Convenience umbrella header so SLAM / visual odometry can be included as
// <opencv2/slam.hpp>. The implementation currently lives in the ptcloud module.
#include "opencv2/ptcloud/slam.hpp"
#endif // OPENCV_SLAM_HPP
modules/ptcloud/src/slam/odometry/visual_odometry.cpp
+21 -57
View File
@@ -7,6 +7,8 @@
#include "../precomp.hpp"
#include "vo_impl.hpp"
#include <opencv2/core/quaternion.hpp>
#include <fstream>
#include <sstream>
@@ -105,26 +107,26 @@ bool VisualOdometryImpl::processFrame(InputArray image)
if (image.empty()) return false;
lastEvent.clear();
Frame cur;
extractFeatures(image, cur);
if (cur.keypoints.empty() || cur.descriptors.empty()) return false;
Frame currentFrame;
extractFeatures(image, currentFrame);
if (currentFrame.keypoints.empty() || currentFrame.descriptors.empty()) return false;
cur.mapPoints.assign(cur.keypoints.size(), nullptr);
cur.outliers.assign(cur.keypoints.size(), false);
cur.buildGrid();
currentFrame.mapPoints.assign(currentFrame.keypoints.size(), nullptr);
currentFrame.outliers.assign(currentFrame.keypoints.size(), false);
currentFrame.buildGrid();
switch (state)
{
case NOT_INITIALIZED:
refFrame = cur;
refFrame = currentFrame;
state = INITIALIZING;
return false;
case INITIALIZING:
return bootstrap(cur);
return bootstrap(currentFrame);
case TRACKING:
return track(cur);
return track(currentFrame);
}
return false;
}
@@ -223,16 +225,11 @@ bool VisualOdometryImpl::run()
if (!outputFolder.empty())
cv::utils::fs::createDirectories(outputFolder);
auto logln = [](const String&) {};
logln("[INFO] optimizer = reprojection inlier check");
logln(String("[INFO] images_folder = ") + imagesFolder);
logln(String("[INFO] output_folder = ") + outputFolder);
{
std::ostringstream ss;
ss << "[INFO] found " << imgFiles.size() << " image(s)";
logln(ss.str());
}
CV_LOG_INFO(NULL, "optimizer = reprojection inlier check");
CV_LOG_INFO(NULL, "images_folder = " << imagesFolder);
CV_LOG_INFO(NULL, "output_folder = " << outputFolder);
CV_LOG_INFO(NULL, "found " << imgFiles.size() << " image(s)");
reset();
@@ -245,9 +242,8 @@ bool VisualOdometryImpl::run()
Mat img = imread(imgFiles[i]);
if (img.empty())
{
std::ostringstream ss;
ss << "[FRAME " << i << "] file=" << imgFiles[i] << " ERROR: imread failed";
logln(ss.str()); continue;
CV_LOG_WARNING(NULL, "[FRAME " << i << "] file=" << imgFiles[i] << " imread failed");
continue;
}
OdometryState before = state;
@@ -283,7 +279,7 @@ bool VisualOdometryImpl::run()
Point3d C = detail::cameraCenterWorld(lastPoseCw);
ss << " C=(" << C.x << "," << C.y << "," << C.z << ")";
}
logln(ss.str());
CV_LOG_INFO(NULL, ss.str());
}
if (!outputFolder.empty())
@@ -340,39 +336,6 @@ void VisualOdometryImpl::writeMapPoints(const String& path) const
}
}
// Shepperd's method: numerically-stable R → unit quaternion (qw, qx, qy, qz).
static void rotMatToQuat(const Matx33d& R,
double& qw, double& qx, double& qy, double& qz)
{
const double tr = R(0,0) + R(1,1) + R(2,2);
if (tr > 0.0)
{
double s = std::sqrt(tr + 1.0) * 2.0;
qw = 0.25 * s;
qx = (R(2,1) - R(1,2)) / s;
qy = (R(0,2) - R(2,0)) / s;
qz = (R(1,0) - R(0,1)) / s;
}
else if (R(0,0) > R(1,1) && R(0,0) > R(2,2))
{
double s = std::sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2.0;
qw = (R(2,1) - R(1,2)) / s; qx = 0.25 * s;
qy = (R(0,1) + R(1,0)) / s; qz = (R(0,2) + R(2,0)) / s;
}
else if (R(1,1) > R(2,2))
{
double s = std::sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2.0;
qw = (R(0,2) - R(2,0)) / s; qx = (R(0,1) + R(1,0)) / s;
qy = 0.25 * s; qz = (R(1,2) + R(2,1)) / s;
}
else
{
double s = std::sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2.0;
qw = (R(1,0) - R(0,1)) / s; qx = (R(0,2) + R(2,0)) / s;
qy = (R(1,2) + R(2,1)) / s; qz = 0.25 * s;
}
}
static String basenameOf(const String& path)
{
const size_t slash = path.find_last_of("/\\");
@@ -397,8 +360,9 @@ void VisualOdometryImpl::writeImagesTxt(const String& path) const
Matx33d R;
for (int r = 0; r < 3; ++r)
for (int c = 0; c < 3; ++c) R(r,c) = T(r,c);
double qw, qx, qy, qz;
rotMatToQuat(R, qw, qx, qy, qz);
const Quatd q = Quatd::createFromRotMat(R);
const double qw = q.w, qx = q.x, qy = q.y, qz = q.z;
const String name = (i < poseFilenames.size())
? basenameOf(poseFilenames[i])
samples/slam/visual_odometry.cpp
+1 -1
View File
@@ -3,7 +3,7 @@
// of this distribution and at http://opencv.org/license.html.
// Copyright (C) 2026, BigVision LLC, all rights reserved.
#include <opencv2/slam.hpp>
#include <opencv2/ptcloud/slam.hpp>
#include <opencv2/features.hpp>
#include <opencv2/dnn.hpp>
#include <opencv2/core.hpp>