From 4ef179b4bf8dcbc73970bc76c46b858e0ad9aac6 Mon Sep 17 00:00:00 2001 From: Agrim Rai Date: Wed, 8 Jul 2026 03:14:17 +0530 Subject: [PATCH] slam VO sample file refactor --- modules/ptcloud/CMakeLists.txt | 2 +- .../opencv2/ptcloud/slam/visual_odometry.hpp | 21 +- .../src/slam/odometry/visual_odometry.cpp | 221 +----------------- modules/ptcloud/src/slam/odometry/vo_impl.hpp | 23 +- modules/ptcloud/test/test_visual_odometry.cpp | 10 +- samples/slam/visual_odometry.cpp | 221 ++++++++++++++++-- samples/slam/visual_odometry.py | 127 +++++++++- 7 files changed, 351 insertions(+), 274 deletions(-) diff --git a/modules/ptcloud/CMakeLists.txt b/modules/ptcloud/CMakeLists.txt index 999181b3d7..a58b719f20 100644 --- a/modules/ptcloud/CMakeLists.txt +++ b/modules/ptcloud/CMakeLists.txt @@ -4,7 +4,7 @@ set(debug_modules "") if(DEBUG_opencv_ptcloud) list(APPEND debug_modules opencv_highgui) endif() -ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} +ocv_define_module(ptcloud opencv_geometry opencv_imgproc opencv_flann opencv_features opencv_imgcodecs opencv_video ${debug_modules} WRAP java objc python js ) ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES}) diff --git a/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp index e51d60e21c..43338f83d3 100644 --- a/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp +++ b/modules/ptcloud/include/opencv2/ptcloud/slam/visual_odometry.hpp @@ -27,7 +27,9 @@ namespace slam { State machine: NOT_INITIALIZED → INITIALIZING (H/F two-view bootstrap) → TRACKING (per-frame PnP + local-map refinement). Tracking failure rewinds to INITIALIZING. -@ref run writes camera.txt, point3d.txt, and images.txt into `outputFolder`. +Purely in-memory: feed images with @ref processFrame and read back the trajectory +and map with @ref getTrajectory / @ref getMap. See samples/slam for driving this +from a directory of images and writing the result to disk (e.g. COLMAP format). */ class CV_EXPORTS_W VisualOdometry { @@ -37,15 +39,10 @@ public: CV_WRAP static Ptr create( const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, InputArray cameraMatrix, InputArray distCoeffs = noArray(), const OdometryParams& params = OdometryParams()); - /** @brief Run the pipeline over every image in the configured folder. */ - CV_WRAP virtual bool run() = 0; - /** @brief Feed one image. Returns true if a pose was emitted. */ CV_WRAP virtual bool processFrame(InputArray image) = 0; @@ -58,15 +55,19 @@ public: //! @note Not exposed to Python: Map holds raw pointers / non-convertible containers. virtual const Map& getMap() const = 0; + //! @brief Number of keyframes in the map. Convenience for callers (e.g. Python) that + //! can't use @ref getMap directly. + CV_WRAP virtual int getNumKeyframes() const = 0; + + //! @brief Number of map points in the map. Convenience for callers (e.g. Python) that + //! can't use @ref getMap directly. + CV_WRAP virtual int getNumMapPoints() const = 0; + CV_WRAP virtual const std::vector& getTrajectory() const = 0; CV_WRAP virtual const OdometryParams& getParams() const = 0; CV_WRAP virtual void setParams(const OdometryParams& params) = 0; - CV_WRAP virtual const String& getImagesFolder() const = 0; - CV_WRAP virtual const String& getOutputFolder() const = 0; - CV_WRAP virtual void setOutputFolder(const String& outputFolder) = 0; - protected: VisualOdometry(); }; diff --git a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp index 5213ebf713..d8ae7fc470 100644 --- a/modules/ptcloud/src/slam/odometry/visual_odometry.cpp +++ b/modules/ptcloud/src/slam/odometry/visual_odometry.cpp @@ -7,37 +7,9 @@ #include "../precomp.hpp" #include "vo_impl.hpp" -#include - -#include -#include - namespace cv { namespace slam { -namespace { - -const char* stateName(OdometryState s) -{ - switch (s) - { - case NOT_INITIALIZED: return "NOT_INITIALIZED"; - case INITIALIZING: return "INITIALIZING"; - case TRACKING: return "TRACKING"; - } - return "NOT_INITIALIZED"; -} - -String joinPath(const String& dir, const String& name) -{ - if (dir.empty()) return name; - char last = dir.back(); - if (last == '/' || last == '\\') return dir + name; - return dir + "/" + name; -} - -} // anonymous namespace - // Factory VisualOdometry::VisualOdometry() = default; @@ -46,8 +18,6 @@ VisualOdometry::~VisualOdometry() = default; Ptr VisualOdometry::create( const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, InputArray cameraMatrix, InputArray distCoeffs, const OdometryParams& params) @@ -59,8 +29,7 @@ Ptr VisualOdometry::create( CV_Assert(!K.empty() && K.rows == 3 && K.cols == 3); Mat dist = distCoeffs.empty() ? Mat() : distCoeffs.getMat(); - return makePtr( - detector, matcher, imagesFolder, outputFolder, K, dist, params); + return makePtr(detector, matcher, K, dist, params); } // Constructor @@ -68,13 +37,10 @@ Ptr VisualOdometry::create( VisualOdometryImpl::VisualOdometryImpl( const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, const Mat& cameraMatrix, const Mat& distCoeffs, const OdometryParams& params) - : detector(detector), matcher(matcher), params(params), - imagesFolder(imagesFolder), outputFolder(outputFolder) + : detector(detector), matcher(matcher), params(params) { cameraMatrix.convertTo(K, CV_64F); if (!distCoeffs.empty()) @@ -96,7 +62,6 @@ void VisualOdometryImpl::reset() prevFrame = Frame(); hasPrevFrame = false; lastEvent.clear(); - poseFilenames.clear(); map.clear(); } @@ -178,186 +143,4 @@ void VisualOdometryImpl::matchFrames( matcher->match(qDesc, tDesc, matches); } -// Batch run() - -bool VisualOdometryImpl::run() -{ - CV_INSTRUMENT_REGION(); - - if (imagesFolder.empty()) - { - CV_LOG_ERROR(NULL, "VisualOdometry::run: imagesFolder is empty"); - return false; - } - - std::vector allFiles; - try { cv::glob(imagesFolder, allFiles, false); } - catch (const cv::Exception& e) - { - CV_LOG_ERROR(NULL, "VisualOdometry::run: glob failed: " << e.what()); - return false; - } - - std::vector imgFiles; - imgFiles.reserve(allFiles.size()); - for (const auto& f : allFiles) - if (cv::haveImageReader(f)) imgFiles.push_back(f); - std::sort(imgFiles.begin(), imgFiles.end()); - - if (imgFiles.empty()) - { - CV_LOG_WARNING(NULL, "VisualOdometry::run: no images in " << imagesFolder); - return false; - } - - if (!outputFolder.empty()) - cv::utils::fs::createDirectories(outputFolder); - - CV_LOG_INFO(NULL, "optimizer = reprojection inlier check"); - CV_LOG_INFO(NULL, "images_folder = " << imagesFolder); - CV_LOG_INFO(NULL, "output_folder = " << outputFolder); - CV_LOG_INFO(NULL, "found " << imgFiles.size() << " image(s)"); - - reset(); - - int nEmitted = 0; - size_t prevTrajLen = 0; - String refFilename; - - for (size_t i = 0; i < imgFiles.size(); ++i) - { - Mat img = imread(imgFiles[i]); - if (img.empty()) - { - CV_LOG_WARNING(NULL, "[FRAME " << i << "] file=" << imgFiles[i] << " imread failed"); - continue; - } - - OdometryState before = state; - bool emitted = processFrame(img); - OdometryState after = state; - if (emitted) ++nEmitted; - - // Track which input image maps to each trajectory pose. - if (before == NOT_INITIALIZED || - (before == TRACKING && after == INITIALIZING)) - refFilename = imgFiles[i]; - - const size_t added = map.trajectory().size() - prevTrajLen; - if (added == 1) - poseFilenames.push_back(imgFiles[i]); - else if (added == 2) - { - poseFilenames.push_back(refFilename); - poseFilenames.push_back(imgFiles[i]); - } - prevTrajLen = map.trajectory().size(); - - std::ostringstream ss; - ss << "[FRAME " << i << "] file=" << imgFiles[i] - << " state=" << stateName(before); - if (before != after) ss << "->" << stateName(after); - ss << " emitted=" << (emitted ? "yes" : "no") - << " keyframes=" << map.numKeyframes() - << " map_points=" << map.numMapPoints(); - if (!lastEvent.empty()) ss << " [" << lastEvent << "]"; - if (emitted) - { - Point3d C = detail::cameraCenterWorld(lastPoseCw); - ss << " C=(" << C.x << "," << C.y << "," << C.z << ")"; - } - CV_LOG_INFO(NULL, ss.str()); - } - - if (!outputFolder.empty()) - { - writeCameraIntrinsics(joinPath(outputFolder, "camera.txt")); - writeMapPoints (joinPath(outputFolder, "point3d.txt")); - writeImagesTxt (joinPath(outputFolder, "images.txt")); - } - - return nEmitted > 0; -} - -// IO helpers - -void VisualOdometryImpl::writeCameraIntrinsics(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeCameraIntrinsics: cannot open " << path); return; } - - const double fx = K.at(0, 0); - const double fy = K.at(1, 1); - const double cx = K.at(0, 2); - const double cy = K.at(1, 2); - - int width = 0, height = 0; - if (!map.keyframes().empty()) - { - const KeyFrame* kf = *map.keyframes().begin(); - width = kf->imageSize.width; - height = kf->imageSize.height; - } - - f.setf(std::ios::fixed); f.precision(4); - f << "fx " << fx << "\n" - << "fy " << fy << "\n" - << "cx " << cx << "\n" - << "cy " << cy << "\n" - << "width " << width << "\n" - << "height " << height << "\n"; -} - -void VisualOdometryImpl::writeMapPoints(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeMapPoints: cannot open " << path); return; } - f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; - f.setf(std::ios::scientific); f.precision(9); - for (MapPoint* mp : map.mapPoints()) - { - if (!mp || mp->bad) continue; - f << mp->id << " " - << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " " - << mp->observations.size() << "\n"; - } -} - -static String basenameOf(const String& path) -{ - const size_t slash = path.find_last_of("/\\"); - return (slash == String::npos) ? path : path.substr(slash + 1); -} - -void VisualOdometryImpl::writeImagesTxt(const String& path) const -{ - std::ofstream f(path.c_str()); - if (!f.is_open()) { CV_LOG_WARNING(NULL, "writeImagesTxt: cannot open " << path); return; } - - const auto& traj = map.trajectory(); - f << "# Image list with two lines of data per image:\n" - << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" - << "# POINTS2D[] as (X, Y, POINT3D_ID)\n" - << "# Number of images: " << traj.size() << ", mean observations per image: 0.0\n"; - f.setf(std::ios::fixed); f.precision(6); - - for (size_t i = 0; i < traj.size(); ++i) - { - const Matx44d& T = traj[i]; - Matx33d R; - for (int r = 0; r < 3; ++r) - for (int c = 0; c < 3; ++c) R(r,c) = T(r,c); - - const Quatd q = Quatd::createFromRotMat(R); - const double qw = q.w, qx = q.x, qy = q.y, qz = q.z; - - const String name = (i < poseFilenames.size()) - ? basenameOf(poseFilenames[i]) - : (String("pose_") + std::to_string(i)); - - f << i << " " << qw << " " << qx << " " << qy << " " << qz << " " - << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n"; - } -} - }} // namespace cv::slam diff --git a/modules/ptcloud/src/slam/odometry/vo_impl.hpp b/modules/ptcloud/src/slam/odometry/vo_impl.hpp index c809bf7294..cbd539737f 100644 --- a/modules/ptcloud/src/slam/odometry/vo_impl.hpp +++ b/modules/ptcloud/src/slam/odometry/vo_impl.hpp @@ -11,8 +11,6 @@ #include "frame.hpp" #include "../optimizer/pose_optimizer.hpp" -#include - namespace cv { namespace slam { @@ -23,36 +21,31 @@ Stage logic is split across: - vo_tracking.cpp : per-frame localisation (motion model, fallback 1/2, local map) - vo_keyframe.cpp : keyframe promotion decision + covisibility helpers - vo_map_growth.cpp : triangulation of new map points at promotion time - - visual_odometry.cpp : factory, run(), processFrame(), IO writers + - visual_odometry.cpp : factory, processFrame() */ class VisualOdometryImpl CV_FINAL : public VisualOdometry { public: VisualOdometryImpl(const Ptr& detector, const Ptr& matcher, - const String& imagesFolder, - const String& outputFolder, const Mat& cameraMatrix, const Mat& distCoeffs, const OdometryParams& params); // --- VisualOdometry interface ------------------------------------------- - bool run() CV_OVERRIDE; bool processFrame(InputArray image) CV_OVERRIDE; void reset() CV_OVERRIDE; OdometryState getState() const CV_OVERRIDE { return state; } Matx44d getLastPose() const CV_OVERRIDE { return lastPoseCw; } const Map& getMap() const CV_OVERRIDE { return map; } + int getNumKeyframes() const CV_OVERRIDE { return map.numKeyframes(); } + int getNumMapPoints() const CV_OVERRIDE { return map.numMapPoints(); } const std::vector& getTrajectory() const CV_OVERRIDE { return map.trajectory(); } const OdometryParams& getParams() const CV_OVERRIDE { return params; } void setParams(const OdometryParams& p) CV_OVERRIDE { params = p; } - const String& getImagesFolder() const CV_OVERRIDE { return imagesFolder; } - const String& getOutputFolder() const CV_OVERRIDE { return outputFolder; } - void setOutputFolder(const String& f) CV_OVERRIDE { outputFolder = f; } - // --- Stage entry points ------------------------------------------------- bool bootstrap(Frame& cur); @@ -74,12 +67,6 @@ public: const std::vector& tKp, const Mat& tDesc, Size tSz, std::vector& matches) const; - // --- IO helpers (visual_odometry.cpp) ------------------------------------ - - void writeCameraIntrinsics(const String& path) const; - void writeMapPoints(const String& path) const; - void writeImagesTxt(const String& path) const; - // --- Owned state --------------------------------------------------------- Ptr detector; @@ -88,9 +75,6 @@ public: Mat dist; // distortion coefficients (may be empty) OdometryParams params; - String imagesFolder; - String outputFolder; - OdometryState state = NOT_INITIALIZED; Matx44d lastPoseCw = Matx44d::eye(); @@ -106,7 +90,6 @@ public: bool hasPrevFrame = false; String lastEvent; - std::vector poseFilenames; Map map; }; diff --git a/modules/ptcloud/test/test_visual_odometry.cpp b/modules/ptcloud/test/test_visual_odometry.cpp index a3ea62136f..a1c9eeee2c 100644 --- a/modules/ptcloud/test/test_visual_odometry.cpp +++ b/modules/ptcloud/test/test_visual_odometry.cpp @@ -106,7 +106,7 @@ static Ptr makeOdometry(int nFrames, detector->frames.push_back(renderFrame(cloud, camCenters[f])); Ptr matcher = BFMatcher::create(NORM_L2, true); - return slam::VisualOdometry::create(detector, matcher, "", "", + return slam::VisualOdometry::create(detector, matcher, Mat(cameraMatrix()), noArray(), params); } @@ -135,13 +135,13 @@ TEST(SLAM_VisualOdometry, create_validates_arguments) Ptr matcher = BFMatcher::create(NORM_L2, true); Mat K(cameraMatrix()); - EXPECT_THROW(slam::VisualOdometry::create(Ptr(), matcher, "", "", K), + EXPECT_THROW(slam::VisualOdometry::create(Ptr(), matcher, K), cv::Exception); // null detector - EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr(), "", "", K), + EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr(), K), cv::Exception); // null matcher - EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat()), + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, Mat()), cv::Exception); // empty intrinsics - EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, "", "", Mat::eye(2, 2, CV_64F)), + EXPECT_THROW(slam::VisualOdometry::create(detector, matcher, Mat::eye(2, 2, CV_64F)), cv::Exception); // wrong-size intrinsics } diff --git a/samples/slam/visual_odometry.cpp b/samples/slam/visual_odometry.cpp index 450c2c434f..423aa6a8d2 100644 --- a/samples/slam/visual_odometry.cpp +++ b/samples/slam/visual_odometry.cpp @@ -7,6 +7,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include "../dnn/common.hpp" @@ -14,6 +19,139 @@ using namespace cv; using namespace std; +namespace { + +// path helper: joins a directory and filename, tolerating a dir with/without a trailing slash +String joinPath(const String& dir, const String& name) +{ + if (dir.empty()) return name; + char last = dir.back(); + if (last == '/' || last == '\\') return dir + name; + return dir + "/" + name; +} + +// path helper: strips the directory part off a path +String basenameOf(const String& path) +{ + const size_t slash = path.find_last_of("/\\"); + return (slash == String::npos) ? path : path.substr(slash + 1); +} + +// reader: lists readable image files under a directory, sorted by filename +std::vector listImageFiles(const String& imagesDir) +{ + std::vector allFiles; + glob(imagesDir, allFiles, false); + std::vector imgFiles; + imgFiles.reserve(allFiles.size()); + for (const auto& f : allFiles) + if (haveImageReader(f)) imgFiles.push_back(f); + std::sort(imgFiles.begin(), imgFiles.end()); + return imgFiles; +} + +// bookkeeping: maps each pose in vo->getTrajectory() back to its source image filename. +// +// processFrame() doesn't report which frame a pose came from, so this infers it: most frames +// add one pose, but the frame where two-view initialization completes adds two at once (the +// reference frame's pose, remembered when initialization started, plus the current frame's). +class PoseFilenameTracker +{ +public: + void update(slam::OdometryState before, slam::OdometryState after, + size_t trajectorySize, const String& filename) + { + if (before == slam::NOT_INITIALIZED || + (before == slam::TRACKING && after == slam::INITIALIZING)) + refFilename_ = filename; + + const size_t added = trajectorySize - prevTrajectorySize_; + if (added == 1) + filenames_.push_back(filename); + else if (added == 2) + { + filenames_.push_back(refFilename_); + filenames_.push_back(filename); + } + prevTrajectorySize_ = trajectorySize; + } + + const std::vector& filenames() const { return filenames_; } + +private: + std::vector filenames_; + size_t prevTrajectorySize_ = 0; + String refFilename_; +}; + +// writer: dumps camera.txt, point3d.txt and images.txt (COLMAP text format) for @p vo into +// @p outputFolder; @p poseFilenames[i] is the source image for trajectory pose i +void writeColmapFiles(const slam::VisualOdometry& vo, const Mat& K, + const std::vector& poseFilenames, + const String& outputFolder) +{ + cv::utils::fs::createDirectories(outputFolder); + + { + std::ofstream f(joinPath(outputFolder, "camera.txt").c_str()); + int width = 0, height = 0; + if (!vo.getMap().keyframes().empty()) + { + const slam::KeyFrame* kf = *vo.getMap().keyframes().begin(); + width = kf->imageSize.width; + height = kf->imageSize.height; + } + f.setf(std::ios::fixed); f.precision(4); + f << "fx " << K.at(0, 0) << "\n" + << "fy " << K.at(1, 1) << "\n" + << "cx " << K.at(0, 2) << "\n" + << "cy " << K.at(1, 2) << "\n" + << "width " << width << "\n" + << "height " << height << "\n"; + } + + { + std::ofstream f(joinPath(outputFolder, "point3d.txt").c_str()); + f << "# Map points in world coordinates.\n# Columns: id X Y Z n_observations\n"; + f.setf(std::ios::scientific); f.precision(9); + for (slam::MapPoint* mp : vo.getMap().mapPoints()) + { + if (!mp || mp->bad) continue; + f << mp->id << " " + << mp->pos.x << " " << mp->pos.y << " " << mp->pos.z << " " + << mp->observations.size() << "\n"; + } + } + + { + std::ofstream f(joinPath(outputFolder, "images.txt").c_str()); + const auto& traj = vo.getTrajectory(); + f << "# Image list with two lines of data per image:\n" + << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + << "# POINTS2D[] as (X, Y, POINT3D_ID)\n" + << "# Number of images: " << traj.size() << ", mean observations per image: 0.0\n"; + f.setf(std::ios::fixed); f.precision(6); + + for (size_t i = 0; i < traj.size(); ++i) + { + const Matx44d& T = traj[i]; + Matx33d R; + for (int r = 0; r < 3; ++r) + for (int c = 0; c < 3; ++c) R(r, c) = T(r, c); + + const Quatd q = Quatd::createFromRotMat(R); + const String name = (i < poseFilenames.size()) + ? basenameOf(poseFilenames[i]) + : (String("pose_") + std::to_string(i)); + + f << i << " " << q.w << " " << q.x << " " << q.y << " " << q.z << " " + << T(0,3) << " " << T(1,3) << " " << T(2,3) << " " << 1 << " " << name << "\n"; + } + } +} + +} // namespace + const string about = "Monocular visual odometry using ALIKED + LightGlue.\n\n" "Runs feature detection (ALIKED) and matching (LightGlue) from ONNX models over a\n" @@ -32,9 +170,7 @@ const string param_keys = "{ fx | 718.856 | Camera focal length X }" "{ fy | 718.856 | Camera focal length Y }" "{ cx | 607.1928 | Camera principal point X }" - "{ cy | 185.2157 | Camera principal point Y }" - "{ min-parallax | 1.5 | Minimum initialisation parallax in degrees }" - "{ min-points | 50 | Minimum initialisation map points }"; + "{ cy | 185.2157 | Camera principal point Y }"; const string backend_keys = format( "{ backend | default | Choose one of computation backends: " @@ -98,21 +234,78 @@ int main(int argc, char** argv) backendId, targetId); slam::OdometryParams voParams; - voParams.minInitParallaxDeg = parser.get("min-parallax"); - voParams.minInitPoints = parser.get("min-points"); + + voParams.minInitParallaxDeg = 1.5; + voParams.minInitPoints = 50; + voParams.pnpReprojThresh = 4.0; + voParams.kfMaxFrames = 30; + voParams.localMapTopK = 10; auto vo = slam::VisualOdometry::create( - detector, matcher, - imagesDir, outputDir, - Mat(K), Mat(), voParams); + detector, matcher, Mat(K), Mat(), voParams); - const int64 t0 = getTickCount(); - const bool ok = vo->run(); + const std::vector imgFiles = listImageFiles(imagesDir); + + if (imgFiles.empty()) + { + std::cerr << "no images found in " << imagesDir << "\n"; + return 1; + } + + std::cout << "images folder : " << imagesDir << "\n" + << "output folder : " << outputDir << "\n" + << "total frames : " << imgFiles.size() << "\n\n"; + + PoseFilenameTracker poseTracker; + int nEmitted = 0; + + const int nDigits = std::to_string(imgFiles.size()).size(); + const int64 t0 = getTickCount(); + for (size_t i = 0; i < imgFiles.size(); ++i) + { + const int64 tFrameStart = getTickCount(); + + Mat img = imread(imgFiles[i]); + if (img.empty()) + { + std::cerr << "Frame " << std::setw(nDigits) << (i + 1) << "/" << imgFiles.size() + << " failed to read: " << imgFiles[i] << "\n"; + continue; + } + + const slam::OdometryState before = vo->getState(); + const bool emitted = vo->processFrame(img); + const slam::OdometryState after = vo->getState(); + if (emitted) ++nEmitted; + + poseTracker.update(before, after, vo->getTrajectory().size(), imgFiles[i]); + + const double frameElapsed = (getTickCount() - tFrameStart) / getTickFrequency(); + const double fps = frameElapsed > 0. ? 1. / frameElapsed : 0.; + + std::cout << "Frame " << std::setw(nDigits) << (i + 1) << "/" << imgFiles.size() + << " processed | fps: " << std::fixed << std::setprecision(1) << std::setw(5) << fps + << " | emitted: " << (emitted ? "yes" : "no ") + << " | keyframes: " << vo->getMap().numKeyframes() + << " | map points: " << vo->getMap().numMapPoints() << "\n"; + } const double elapsed = (getTickCount() - t0) / getTickFrequency(); + const double avgFps = elapsed > 0. ? imgFiles.size() / elapsed : 0.; + const bool ok = nEmitted > 0; - std::cout << "run=" << (ok ? "ok" : "FAILED") - << " frames=" << vo->getTrajectory().size() - << " elapsed=" << elapsed << "s\n" - << "output -> " << outputDir << "\n"; + if (ok && !outputDir.empty()) + writeColmapFiles(*vo, Mat(K), poseTracker.filenames(), outputDir); + + std::cout << "\n" + << "==================== Visual Odometry Result ====================\n" + << "status : " << (ok ? "OK" : "FAILED") << "\n" + << "total frames : " << imgFiles.size() << "\n" + << "camera poses : " << vo->getTrajectory().size() << "\n" + << "keyframes : " << vo->getMap().numKeyframes() << "\n" + << "map points : " << vo->getMap().numMapPoints() << "\n" + << "elapsed time : " << std::fixed << std::setprecision(2) << elapsed << " s\n" + << "average fps : " << std::fixed << std::setprecision(2) << avgFps << "\n" + << "output dir : " << outputDir << "\n" + << "====================================================================\n"; return ok ? 0 : 1; } diff --git a/samples/slam/visual_odometry.py b/samples/slam/visual_odometry.py index c949fffa04..f3268c17d5 100644 --- a/samples/slam/visual_odometry.py +++ b/samples/slam/visual_odometry.py @@ -6,6 +6,9 @@ Example: ''' import argparse +import glob +import os +import sys import time import numpy as np import cv2 as cv @@ -17,6 +20,71 @@ def build_K(fx, fy, cx, cy): [0., 0., 1.]], dtype=np.float64) +def list_image_files(images_dir): + files = [f for f in sorted(glob.glob(os.path.join(images_dir, '*'))) + if cv.haveImageReader(f)] + return files + + +def rotation_matrix_to_quaternion(R): + # Shepperd's method: numerically stable for all rotations. + trace = R[0, 0] + R[1, 1] + R[2, 2] + if trace > 0: + s = np.sqrt(trace + 1.0) * 2 + qw = 0.25 * s + qx = (R[2, 1] - R[1, 2]) / s + qy = (R[0, 2] - R[2, 0]) / s + qz = (R[1, 0] - R[0, 1]) / s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 + qw = (R[2, 1] - R[1, 2]) / s + qx = 0.25 * s + qy = (R[0, 1] + R[1, 0]) / s + qz = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 + qw = (R[0, 2] - R[2, 0]) / s + qx = (R[0, 1] + R[1, 0]) / s + qy = 0.25 * s + qz = (R[1, 2] + R[2, 1]) / s + else: + s = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 + qw = (R[1, 0] - R[0, 1]) / s + qx = (R[0, 2] + R[2, 0]) / s + qy = (R[1, 2] + R[2, 1]) / s + qz = 0.25 * s + return qw, qx, qy, qz + + +def write_colmap_files(vo, K, image_size, pose_filenames, output_dir): + # Note: cv.slam.Map isn't wrapped for Python (only aggregate counts are, + # via getNumKeyframes/getNumMapPoints), so only camera intrinsics and the + # trajectory (images.txt) can be exported here; point3d.txt (map points) + # requires the C++ sample. + os.makedirs(output_dir, exist_ok=True) + + with open(os.path.join(output_dir, 'camera.txt'), 'w') as f: + f.write(f"fx {K[0, 0]:.4f}\n") + f.write(f"fy {K[1, 1]:.4f}\n") + f.write(f"cx {K[0, 2]:.4f}\n") + f.write(f"cy {K[1, 2]:.4f}\n") + f.write(f"width {image_size[0]}\n") + f.write(f"height {image_size[1]}\n") + + traj = vo.getTrajectory() + with open(os.path.join(output_dir, 'images.txt'), 'w') as f: + f.write("# Image list with two lines of data per image:\n") + f.write("# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n") + f.write("# POINTS2D[] as (X, Y, POINT3D_ID)\n") + f.write(f"# Number of images: {len(traj)}, mean observations per image: 0.0\n") + for i, T in enumerate(traj): + qw, qx, qy, qz = rotation_matrix_to_quaternion(T[:3, :3]) + name = (os.path.basename(pose_filenames[i]) if i < len(pose_filenames) + else f"pose_{i}") + f.write(f"{i} {qw:.6f} {qx:.6f} {qy:.6f} {qz:.6f} " + f"{T[0,3]:.6f} {T[1,3]:.6f} {T[2,3]:.6f} 1 {name}\n") + + def main(): parser = argparse.ArgumentParser( description='Monocular visual odometry using ALIKED + LightGlue') @@ -59,17 +127,66 @@ def main(): K = build_K(args.fx, args.fy, args.cx, args.cy) vo = cv.slam.VisualOdometry.create( - detector, matcher, - args.images, args.output, - K, np.array([]), vo_params) + detector, matcher, K, np.array([]), vo_params) + + image_files = list_image_files(args.images) + if not image_files: + print(f"no images found in {args.images}", file=sys.stderr) + return 1 + + print(f"images_folder = {args.images}") + print(f"output_folder = {args.output}") + print(f"found {len(image_files)} image(s)") + + # Tracks which input image each emitted trajectory pose came from, for images.txt. + pose_filenames = [] + prev_traj_len = 0 + ref_filename = None + image_size = (0, 0) + n_emitted = 0 t0 = time.perf_counter() - ok = vo.run() + for i, path in enumerate(image_files): + img = cv.imread(path) + if img is None: + print(f"[FRAME {i}] file={path} imread failed", file=sys.stderr) + continue + image_size = (img.shape[1], img.shape[0]) + + before = vo.getState() + emitted = vo.processFrame(img) + after = vo.getState() + if emitted: + n_emitted += 1 + + # Track which input image maps to each trajectory pose. + if before == cv.slam.NOT_INITIALIZED or \ + (before == cv.slam.TRACKING and after == cv.slam.INITIALIZING): + ref_filename = path + + traj_len = len(vo.getTrajectory()) + added = traj_len - prev_traj_len + if added == 1: + pose_filenames.append(path) + elif added == 2: + pose_filenames.append(ref_filename) + pose_filenames.append(path) + prev_traj_len = traj_len + + print(f"[FRAME {i}] file={path}" + f" emitted={'yes' if emitted else 'no'}" + f" keyframes={vo.getNumKeyframes()}" + f" map_points={vo.getNumMapPoints()}") elapsed = time.perf_counter() - t0 + ok = n_emitted > 0 + + if ok and args.output: + write_colmap_files(vo, K, image_size, pose_filenames, args.output) print(f"run={'ok' if ok else 'FAILED'} frames={len(vo.getTrajectory())} elapsed={elapsed:.2f}s") print(f"output -> {args.output}") + return 0 if ok else 1 if __name__ == '__main__': - main() + sys.exit(main())