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

slam VO sample file refactor

Browse Source
This commit is contained in:
Agrim Rai
2026-07-08 03:14:17 +05:30
parent f552e8ac82
commit 4ef179b4bf
7 changed files with 351 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/ptcloud/CMakeLists.txt
+1 -1
View File
@@ -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
+11 -10
View File
@@ -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
+2 -219
View File
@@ -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
+3 -20
View File
@@ -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
+5 -5
View File
@@ -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
}
samples/slam/visual_odometry.cpp
+207 -14
View File
@@ -7,6 +7,11 @@
#include <opencv2/features.hpp>
#include <opencv2/dnn.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/quaternion.hpp>
#include <opencv2/imgcodecs.hpp>
#include <algorithm>
#include <fstream>
#include <iomanip>
#include <iostream>
#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<String> listImageFiles(const String& imagesDir)
{
std::vector<String> allFiles;
glob(imagesDir, allFiles, false);
std::vector<String> 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<String>& filenames() const { return filenames_; }
private:
std::vector<String> 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<String>& 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<double>(0, 0) << "\n"
<< "fy " << K.at<double>(1, 1) << "\n"
<< "cx " << K.at<double>(0, 2) << "\n"
<< "cy " << K.at<double>(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<double>("min-parallax");
voParams.minInitPoints = parser.get<int>("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<String> 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;
}
samples/slam/visual_odometry.py
+122 -5
View File
@@ -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())