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