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

cleanupFinal

Browse Source
This commit is contained in:
Agrim Rai
2026-06-17 13:15:57 +05:30
parent 72b3fea7dd
commit 08077a00a1
28 changed files with 2453 additions and 400 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/slam/test/test_visual_odometry.cpp
+218 -103
View File
@@ -1,136 +1,251 @@
// 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.
#include "test_precomp.hpp"
namespace opencv_test { namespace {
// =============================================================================
// Map tests
// =============================================================================
// The slam module has no committed image data, so - as in the geometry module's
// tests - the pipeline is exercised on a synthetic scene: a fixed 3D point cloud
// (generated with theRNG()) projected through a pin-hole camera that translates
// along +X. A stub Feature2D replays the projected keypoints frame by frame and
// a brute-force matcher recovers the ground-truth correspondences, so
// VisualOdometry runs end-to-end without a real detector or image files.
TEST(Map, AddAndRetrieveKeyframe)
const int descDim = 8;
const int cloudSize = 400;
const Size imageSize(640, 480);
// Camera centres along +X: a wide first baseline for good bootstrap parallax,
// then small uniform steps suited to per-frame tracking.
static const double camCenters[] = { 0.0, 0.8, 1.1, 1.4, 1.7, 2.0 };
static Matx33d cameraMatrix()
{
cv::slam::Map map;
cv::slam::KeyFrame kf;
kf.pose_cw = cv::Matx44d::eye();
int id = map.addKeyframe(kf);
EXPECT_GE(id, 0);
EXPECT_EQ(kf.id, id);
cv::slam::KeyFrame* retrieved = map.getKeyframe(id);
ASSERT_NE(retrieved, nullptr);
EXPECT_EQ(retrieved->id, id);
EXPECT_EQ(map.numKeyframes(), 1);
return Matx33d(500, 0, 320,
0, 500, 240,
0, 0, 1);
}
TEST(Map, AddAndRetrieveMapPoint)
// Each landmark gets a globally-unique, view-invariant descriptor: identical for
// the same 3D point in every frame (so the matcher pairs them at distance 0) and
// >= sqrt(descDim) apart for distinct points (well above descProjThresh, so
// the projection search during tracking never mismatches).
static Mat makeDescriptor(int id)
{
cv::slam::Map map;
cv::slam::MapPoint mp;
mp.pos = cv::Point3d(1.0, 2.0, 3.0);
int id = map.addMapPoint(mp);
EXPECT_GE(id, 0);
EXPECT_EQ(mp.id, id);
cv::slam::MapPoint* retrieved = map.getMapPoint(id);
ASSERT_NE(retrieved, nullptr);
EXPECT_DOUBLE_EQ(retrieved->pos.x, 1.0);
EXPECT_DOUBLE_EQ(retrieved->pos.y, 2.0);
EXPECT_DOUBLE_EQ(retrieved->pos.z, 3.0);
EXPECT_FALSE(retrieved->bad);
EXPECT_EQ(map.numMapPoints(), 1);
return Mat(1, descDim, CV_32F, Scalar((double)(id + 1)));
}
TEST(Map, RemoveMapPointCleansObservations)
// Stub Feature2D: replays pre-computed keypoints/descriptors, one frame per
// detectAndCompute() call (processFrame() invokes the detector exactly once).
class StubDetector CV_FINAL : public Feature2D
{
cv::slam::Map map;
public:
struct FrameFeatures { std::vector<KeyPoint> keypoints; Mat descriptors; };
std::vector<FrameFeatures> frames;
size_t next = 0;
// Add a keyframe with one keypoint slot.
cv::slam::KeyFrame kf;
kf.pose_cw = cv::Matx44d::eye();
kf.mappoints.assign(1, nullptr);
kf.kpt_to_mp.assign(1, -1);
int kf_id = map.addKeyframe(kf);
void detectAndCompute(InputArray, InputArray,
std::vector<KeyPoint>& keypoints,
OutputArray descriptors, bool) CV_OVERRIDE
{
CV_Assert(next < frames.size());
keypoints = frames[next].keypoints;
frames[next].descriptors.copyTo(descriptors);
++next;
}
};
// Add a map point.
cv::slam::MapPoint mp;
mp.pos = cv::Point3d(0, 0, 1);
int mp_id = map.addMapPoint(mp);
cv::slam::KeyFrame* kf_ptr = map.getKeyframe(kf_id);
cv::slam::MapPoint* mp_ptr = map.getMapPoint(mp_id);
ASSERT_NE(kf_ptr, nullptr);
ASSERT_NE(mp_ptr, nullptr);
// Wire the observation.
map.addObservation(kf_ptr, 0, mp_ptr);
EXPECT_EQ(mp_ptr->observations.size(), 1u);
EXPECT_EQ(kf_ptr->mappoints[0], mp_ptr);
// Remove the map point and verify cleanup.
map.removeMapPoint(mp_id);
EXPECT_TRUE(mp_ptr->bad);
EXPECT_EQ(kf_ptr->mappoints[0], nullptr);
EXPECT_EQ(kf_ptr->kpt_to_mp[0], -1);
EXPECT_EQ(map.numMapPoints(), 0);
static std::vector<Point3f> makeCloud()
{
RNG& rng = theRNG(); // seeded per-test by the ts framework -> reproducible
std::vector<Point3f> pts(cloudSize);
for (int i = 0; i < cloudSize; i++)
pts[i] = Point3f(rng.uniform(-2.5f, 2.5f),
rng.uniform(-1.8f, 1.8f),
rng.uniform( 4.0f, 9.0f)); // varied depth -> non-planar
return pts;
}
TEST(Map, TrajectoryAppendAndRetrieve)
// Projects the cloud through the camera at centre (camX, 0, 0), keeping the
// points that land inside the image. Ground-truth pose is pure translation
// (R = I), so projectPoints takes a zero rotation vector.
static StubDetector::FrameFeatures renderFrame(const std::vector<Point3f>& cloud, double camX)
{
cv::slam::Map map;
Vec3d rvec(0, 0, 0), tvec(-camX, 0, 0); // t = -R*C, with R = I, C = (camX,0,0)
std::vector<Point2f> proj;
projectPoints(cloud, rvec, tvec, cameraMatrix(), noArray(), proj);
cv::Matx44d T1 = cv::Matx44d::eye();
cv::Matx44d T2 = cv::Matx44d::eye();
T2(0, 3) = 1.0; // 1-unit translation
map.appendPose(T1);
map.appendPose(T2);
const auto& traj = map.trajectory();
ASSERT_EQ(traj.size(), 2u);
EXPECT_DOUBLE_EQ(traj[1](0, 3), 1.0);
StubDetector::FrameFeatures ff;
std::vector<int> ids;
for (int i = 0; i < (int)cloud.size(); i++)
{
const Point2f& p = proj[i];
if (p.x < 0 || p.x >= imageSize.width ||
p.y < 0 || p.y >= imageSize.height) continue;
ff.keypoints.push_back(KeyPoint(p, 7.f));
ids.push_back(i);
}
ff.descriptors.create((int)ids.size(), descDim, CV_32F);
for (int r = 0; r < (int)ids.size(); r++)
makeDescriptor(ids[r]).copyTo(ff.descriptors.row(r));
return ff;
}
TEST(Map, ClearResetsState)
// Builds a VisualOdometry fed by a stub detector pre-loaded with @p nFrames of
// the synthetic sequence and a ground-truth brute-force matcher.
static Ptr<slam::VisualOdometry> makeOdometry(int nFrames,
const slam::OdometryParams& params = slam::OdometryParams())
{
cv::slam::Map map;
std::vector<Point3f> cloud = makeCloud();
Ptr<StubDetector> detector = makePtr<StubDetector>();
for (int f = 0; f < nFrames; f++)
detector->frames.push_back(renderFrame(cloud, camCenters[f]));
cv::slam::KeyFrame kf;
kf.pose_cw = cv::Matx44d::eye();
map.addKeyframe(kf);
cv::slam::MapPoint mp;
mp.pos = cv::Point3d(1, 2, 3);
map.addMapPoint(mp);
map.appendPose(cv::Matx44d::eye());
map.clear();
EXPECT_EQ(map.numKeyframes(), 0);
EXPECT_EQ(map.numMapPoints(), 0);
EXPECT_TRUE(map.trajectory().empty());
Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_L2, true);
return slam::VisualOdometry::create(detector, matcher, "", "",
Mat(cameraMatrix()), noArray(), params);
}
// =============================================================================
// OdometryParams tests
// =============================================================================
static Mat blankImage() { return Mat::zeros(imageSize, CV_8UC1); }
TEST(OdometryParams, DefaultValues)
// Rotation magnitude (deg) of @p T's rotation block relative to identity.
static double rotationFromIdentityDeg(const Matx44d& T)
{
cv::slam::OdometryParams p;
EXPECT_EQ(p.min_init_inliers, 80);
EXPECT_DOUBLE_EQ(p.min_init_parallax_deg, 3.0);
EXPECT_EQ(p.min_init_points, 50);
EXPECT_DOUBLE_EQ(p.hf_ratio_thresh, 0.45);
EXPECT_DOUBLE_EQ(p.min_growth_parallax_deg, 1.0);
EXPECT_DOUBLE_EQ(p.essential_ransac_thresh, 1.0);
EXPECT_DOUBLE_EQ(p.essential_ransac_confidence, 0.999);
const double trace = T(0,0) + T(1,1) + T(2,2);
const double c = std::max(-1.0, std::min(1.0, (trace - 1.0) * 0.5));
return std::acos(c) * 180.0 / CV_PI;
}
// Camera centre in world coordinates: C = -R^T t.
static Point3d cameraCenter(const Matx44d& T)
{
Matx33d R(T(0,0),T(0,1),T(0,2), T(1,0),T(1,1),T(1,2), T(2,0),T(2,1),T(2,2));
Matx31d t(T(0,3), T(1,3), T(2,3));
Matx31d C = -R.t() * t;
return Point3d(C(0), C(1), C(2));
}
TEST(SLAM_VisualOdometry, create_validates_arguments)
{
Ptr<Feature2D> detector = makePtr<StubDetector>();
Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_L2, true);
Mat K(cameraMatrix());
EXPECT_THROW(slam::VisualOdometry::create(Ptr<Feature2D>(), matcher, "", "", K),
cv::Exception); // null detector
EXPECT_THROW(slam::VisualOdometry::create(detector, Ptr<DescriptorMatcher>(), "", "", K),
cv::Exception); // null matcher
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)),
cv::Exception); // wrong-size intrinsics
}
TEST(SLAM_VisualOdometry, bootstrap_initializes_map)
{
Ptr<slam::VisualOdometry> vo = makeOdometry(2);
Mat image = blankImage();
EXPECT_FALSE(vo->processFrame(image)); // frame 0 -> INITIALIZING
EXPECT_EQ(vo->getState(), slam::INITIALIZING);
EXPECT_TRUE(vo->processFrame(image)); // frame 1 -> TRACKING
EXPECT_EQ(vo->getState(), slam::TRACKING);
const slam::Map& map = vo->getMap();
EXPECT_EQ(map.numKeyframes(), 2);
EXPECT_GE(map.numMapPoints(), 100); // OdometryParams::minInitPoints
EXPECT_EQ(vo->getTrajectory().size(), 2u);
// Reference keyframe is pinned to the world origin.
const slam::KeyFrame* kfRef = map.getKeyframe(0);
ASSERT_TRUE(kfRef != nullptr);
EXPECT_LT(cv::norm(kfRef->poseCw - Matx44d::eye()), 1e-12);
// Median scene depth is normalized to 1.
std::vector<double> depths;
depths.reserve(map.mapPoints().size());
for (slam::MapPoint* mp : map.mapPoints())
depths.push_back(mp->pos.z);
ASSERT_FALSE(depths.empty());
std::nth_element(depths.begin(), depths.begin() + depths.size() / 2, depths.end());
EXPECT_NEAR(depths[depths.size() / 2], 1.0, 1e-2);
// Second camera: ~no rotation; translation along +X up to scale.
const slam::KeyFrame* kfCur = map.getKeyframe(1);
ASSERT_TRUE(kfCur != nullptr);
EXPECT_LT(rotationFromIdentityDeg(kfCur->poseCw), 2.0);
Point3d C = cameraCenter(kfCur->poseCw);
EXPECT_GT(C.x, 0.0);
EXPECT_LT(std::abs(C.y), 0.2 * std::abs(C.x));
EXPECT_LT(std::abs(C.z), 0.2 * std::abs(C.x));
}
TEST(SLAM_VisualOdometry, tracks_after_bootstrap)
{
Ptr<slam::VisualOdometry> vo = makeOdometry(3);
Mat image = blankImage();
ASSERT_FALSE(vo->processFrame(image));
ASSERT_TRUE(vo->processFrame(image));
ASSERT_EQ(vo->getState(), slam::TRACKING);
const Point3d cBootstrap = cameraCenter(vo->getLastPose());
EXPECT_TRUE(vo->processFrame(image)); // frame 2 -> tracked by PnP
EXPECT_EQ(vo->getState(), slam::TRACKING);
EXPECT_EQ(vo->getTrajectory().size(), 3u);
const Matx44d pose = vo->getLastPose();
EXPECT_LT(rotationFromIdentityDeg(pose), 2.0);
const Point3d C = cameraCenter(pose);
EXPECT_GT(C.x, cBootstrap.x); // camera keeps advancing +X
EXPECT_LT(std::abs(C.y), 0.2 * std::abs(C.x));
EXPECT_LT(std::abs(C.z), 0.2 * std::abs(C.x));
}
TEST(SLAM_VisualOdometry, promotes_keyframes_during_tracking)
{
slam::OdometryParams params;
params.kfMaxFrames = 1; // force a keyframe promotion early in tracking
Ptr<slam::VisualOdometry> vo = makeOdometry(6, params);
Mat image = blankImage();
ASSERT_FALSE(vo->processFrame(image)); // -> INITIALIZING
ASSERT_TRUE(vo->processFrame(image)); // -> TRACKING (2 keyframes)
ASSERT_EQ(vo->getMap().numKeyframes(), 2);
for (int f = 2; f < 6; f++)
EXPECT_TRUE(vo->processFrame(image));
EXPECT_EQ(vo->getState(), slam::TRACKING);
EXPECT_GT(vo->getMap().numKeyframes(), 2); // new keyframes were promoted
const slam::KeyFrame* current = vo->getMap().getCurrentKeyframe();
ASSERT_TRUE(current != nullptr);
EXPECT_GT(current->id, 1); // current keyframe advanced
}
TEST(SLAM_VisualOdometry, reset_clears_state)
{
Ptr<slam::VisualOdometry> vo = makeOdometry(2);
Mat image = blankImage();
vo->processFrame(image);
vo->processFrame(image);
ASSERT_EQ(vo->getState(), slam::TRACKING);
ASSERT_GT(vo->getMap().numKeyframes(), 0);
vo->reset();
EXPECT_EQ(vo->getState(), slam::NOT_INITIALIZED);
EXPECT_EQ(vo->getMap().numKeyframes(), 0);
EXPECT_EQ(vo->getMap().numMapPoints(), 0);
EXPECT_TRUE(vo->getTrajectory().empty());
EXPECT_LT(cv::norm(vo->getLastPose() - Matx44d::eye()), 1e-12);
}
}} // namespace opencv_test