slam VO sample file refactor

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})

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<VisualOdometry> create(
         const Ptr<Feature2D>& detector,
         const Ptr<DescriptorMatcher>& 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<Matx44d>& 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();
 };

modules/ptcloud/src/slam/odometry/visual_odometry.cpp

@@ -7,37 +7,9 @@
 #include "../precomp.hpp"
 #include "vo_impl.hpp"
 
-#include <opencv2/core/quaternion.hpp>
-
-#include <fstream>
-#include <sstream>
-
 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> VisualOdometry::create(
     const Ptr<Feature2D>& detector,
     const Ptr<DescriptorMatcher>& matcher,
-    const String& imagesFolder,
-    const String& outputFolder,
     InputArray cameraMatrix,
     InputArray distCoeffs,
     const OdometryParams& params)
@@ -59,8 +29,7 @@ Ptr<VisualOdometry> VisualOdometry::create(
     CV_Assert(!K.empty() && K.rows == 3 && K.cols == 3);
     Mat dist = distCoeffs.empty() ? Mat() : distCoeffs.getMat();
 
-    return makePtr<VisualOdometryImpl>(
-        detector, matcher, imagesFolder, outputFolder, K, dist, params);
+    return makePtr<VisualOdometryImpl>(detector, matcher, K, dist, params);
 }
 
 // Constructor
@@ -68,13 +37,10 @@ Ptr<VisualOdometry> VisualOdometry::create(
 VisualOdometryImpl::VisualOdometryImpl(
     const Ptr<Feature2D>& detector,
     const Ptr<DescriptorMatcher>& 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<String> 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<String> 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<double>(0, 0);
-    const double fy = K.at<double>(1, 1);
-    const double cx = K.at<double>(0, 2);
-    const double cy = K.at<double>(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

modules/ptcloud/src/slam/odometry/vo_impl.hpp

@@ -11,8 +11,6 @@
 #include "frame.hpp"
 #include "../optimizer/pose_optimizer.hpp"
 
-#include <fstream>
-
 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<Feature2D>& detector,
                        const Ptr<DescriptorMatcher>& 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<Matx44d>& 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<KeyPoint>& tKp, const Mat& tDesc, Size tSz,
                      std::vector<DMatch>& 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<Feature2D> 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<String> poseFilenames;
 
     Map map;
 };

modules/ptcloud/test/test_visual_odometry.cpp

@@ -106,7 +106,7 @@ static Ptr<slam::VisualOdometry> makeOdometry(int nFrames,
         detector->frames.push_back(renderFrame(cloud, camCenters[f]));
 
     Ptr<DescriptorMatcher> 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<DescriptorMatcher> matcher = BFMatcher::create(NORM_L2, true);
     Mat K(cameraMatrix());
 
-    EXPECT_THROW(slam::VisualOdometry::create(Ptr<Feature2D>(), matcher, "", "", K),
+    EXPECT_THROW(slam::VisualOdometry::create(Ptr<Feature2D>(), matcher, K),
                  cv::Exception);                                  // null detector
-    EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr<DescriptorMatcher>(), "", "", K),
+    EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr<DescriptorMatcher>(), 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
 }