i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixing ExtractXYZCorrespondencesRANSAC ci error

Browse Source
This commit is contained in:
matlabbe
2026-09-22 11:52:07 -07:00
parent 31c59d0977
commit 6a3171005b
5 changed files with 262 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/StereoCameraModel.h
+45
View File
@@ -427,6 +427,51 @@ public:
*/
float computeDisparity(unsigned short depth) const; // mm
/**
* @brief Reprojects a 3D point of the left camera frame into both image planes (floating-point).
*
* The point is given in the rectified left camera frame (/camera_link), the same frame used
* by CameraModel::reproject() of left(). The baseline is taken from the Tx of the rectified
* projection matrices, so the horizontal shift between uLeft and uRight is the disparity of
* that point. On a rectified stereo pair the rows are aligned, thus vRight equals vLeft.
*
* @note Unlike this function, CameraModel::reproject() ignores Tx, because a Tx set on a
* single camera model is also used to tag a left camera having stereo observations
* (see the stereo edges built by the BA optimizers).
*
* @param x X coordinate in the left camera space.
* @param y Y coordinate in the left camera space.
* @param z Z coordinate in the left camera space (must be non-zero).
* @param[out] uLeft Output horizontal image coordinate in the left image (float).
* @param[out] vLeft Output vertical image coordinate in the left image (float).
* @param[out] uRight Output horizontal image coordinate in the right image (float).
* @param[out] vRight Output vertical image coordinate in the right image (float).
*
* @pre `z != 0`
*
* @see CameraModel::reproject(), reproject(int&, int&, int&, int&)
*/
void reproject(float x, float y, float z, float & uLeft, float & vLeft, float & uRight, float & vRight) const;
/**
* @brief Reprojects a 3D point of the left camera frame into both image planes (rounded to int).
*
* This version of `reproject()` returns integer pixel indices, computed from the 3D position.
*
* @param x X coordinate in the left camera space.
* @param y Y coordinate in the left camera space.
* @param z Z coordinate in the left camera space (must be non-zero).
* @param[out] uLeft Output horizontal image coordinate in the left image (integer pixel).
* @param[out] vLeft Output vertical image coordinate in the left image (integer pixel).
* @param[out] uRight Output horizontal image coordinate in the right image (integer pixel).
* @param[out] vRight Output vertical image coordinate in the right image (integer pixel).
*
* @pre `z != 0`
*
* @see CameraModel::reproject(), reproject(float&, float&, float&, float&)
*/
void reproject(float x, float y, float z, int & uLeft, int & vLeft, int & uRight, int & vRight) const;
const cv::Mat & R() const {return R_;} ///< Stereo extrinsic rotation matrix.
const cv::Mat & T() const {return T_;} ///< Stereo extrinsic translation vector.
const cv::Mat & E() const {return E_;} ///< Essential matrix.
corelib/src/StereoCameraModel.cpp
+24
View File
@@ -598,6 +598,30 @@ float StereoCameraModel::computeDisparity(unsigned short depth) const
return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
}
void StereoCameraModel::reproject(float x, float y, float z, float & uLeft, float & vLeft, float & uRight, float & vRight) const
{
UASSERT(z!=0.0f);
float invZ = 1.0f/z;
// CameraModel::reproject() doesn't apply Tx, as a camera model with a Tx set is
// also used to tag a left camera having stereo observations (see the stereo edges
// of the BA optimizers). Here Tx is the baseline of the rectified projection
// matrices (0 for the left camera, -fx*baseline for the right one), so that
// (uLeft-uRight) is the disparity of the point.
uLeft = (left_.fx()*x + left_.Tx())*invZ + left_.cx();
vLeft = (left_.fy()*y)*invZ + left_.cy();
uRight = (right_.fx()*x + right_.Tx())*invZ + right_.cx();
vRight = (right_.fy()*y)*invZ + right_.cy();
}
void StereoCameraModel::reproject(float x, float y, float z, int & uLeft, int & vLeft, int & uRight, int & vRight) const
{
float uLeftF, vLeftF, uRightF, vRightF;
this->reproject(x, y, z, uLeftF, vLeftF, uRightF, vRightF);
uLeft = uLeftF;
vLeft = vLeftF;
uRight = uRightF;
vRight = vRightF;
}
Transform StereoCameraModel::stereoTransform() const
{
if(!R_.empty() && !T_.empty())
corelib/test/test_cameramodel.cpp
+39
View File
@@ -298,6 +298,45 @@ TEST_F(CameraModelTest, ReprojectInt)
EXPECT_NEAR(v, static_cast<int>(cy_), 1);
}
TEST_F(CameraModelTest, ReprojectIgnoresTx)
{
// A Tx set on a single camera model tags a left camera having stereo
// observations (the BA optimizers read the baseline from it to build their
// stereo edges), so reprojection stays that of the camera itself. Use
// StereoCameraModel::reproject() to get both images of a stereo pair.
double baseline = 0.12;
CameraModel withTx(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), -baseline*fx_, imageSize_);
CameraModel withoutTx(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
EXPECT_DOUBLE_EQ(withTx.Tx(), -baseline*fx_);
float x = 0.3f, y = -0.2f, z = 2.0f;
float u, v, uNoTx, vNoTx;
withTx.reproject(x, y, z, u, v);
withoutTx.reproject(x, y, z, uNoTx, vNoTx);
EXPECT_FLOAT_EQ(u, uNoTx);
EXPECT_FLOAT_EQ(v, vNoTx);
EXPECT_FLOAT_EQ(u, static_cast<float>(fx_*x/z + cx_));
EXPECT_FLOAT_EQ(v, static_cast<float>(fy_*y/z + cy_));
}
TEST_F(CameraModelTest, ReprojectProjectRoundTripNoTx)
{
CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
float x = 0.35f, y = -0.15f, z = 2.5f;
float u, v;
model.reproject(x, y, z, u, v);
float x2, y2, z2;
model.project(u, v, z, x2, y2, z2);
EXPECT_NEAR(x2, x, 0.001f);
EXPECT_NEAR(y2, y, 0.001f);
EXPECT_FLOAT_EQ(z2, z);
}
// Field of View Tests
TEST_F(CameraModelTest, FieldOfView)
corelib/test/test_stereocameramodel.cpp
+89
View File
@@ -7,6 +7,7 @@
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include <cmath>
#include <limits>
using namespace rtabmap;
@@ -236,6 +237,94 @@ TEST_F(StereoCameraModelTest, ComputeDisparityZeroDepth)
EXPECT_EQ(disparityMM, 0.0f);
}
// Reprojection Tests
TEST_F(StereoCameraModelTest, Reproject)
{
StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
// a point 2 m in front of the left camera, off its optical axis
float x = 0.3f, y = -0.2f, z = 2.0f;
float uLeft, vLeft, uRight, vRight;
model.reproject(x, y, z, uLeft, vLeft, uRight, vRight);
// the left camera has no Tx, so its image point is the one of the left model alone
float u, v;
model.left().reproject(x, y, z, u, v);
EXPECT_DOUBLE_EQ(model.left().Tx(), 0.0);
EXPECT_FLOAT_EQ(uLeft, u);
EXPECT_FLOAT_EQ(vLeft, v);
EXPECT_FLOAT_EQ(uLeft, static_cast<float>(fx_*x/z + cx_));
EXPECT_FLOAT_EQ(vLeft, static_cast<float>(fy_*y/z + cy_));
// rectified pair: same row in both images, right point shifted by the disparity
EXPECT_FLOAT_EQ(vRight, vLeft);
EXPECT_NEAR(uLeft - uRight, model.computeDisparity(z), 0.001f);
EXPECT_NEAR(uLeft - uRight, static_cast<float>(baseline_*fx_/z), 0.001f);
}
TEST_F(StereoCameraModelTest, ReprojectDisparityDecreasesWithDepth)
{
StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
float previousDisparity = std::numeric_limits<float>::max();
for(float z=1.0f; z<=10.0f; z+=1.0f)
{
float uLeft, vLeft, uRight, vRight;
model.reproject(0.0f, 0.0f, z, uLeft, vLeft, uRight, vRight);
// on the optical axis, the left point is the principal point
EXPECT_FLOAT_EQ(uLeft, static_cast<float>(cx_));
EXPECT_FLOAT_EQ(vLeft, static_cast<float>(cy_));
float disparity = uLeft - uRight;
EXPECT_GT(disparity, 0.0f); // right camera on the right of the left one
EXPECT_LT(disparity, previousDisparity);
EXPECT_NEAR(model.computeDepth(disparity), z, 0.001f);
previousDisparity = disparity;
}
}
TEST_F(StereoCameraModelTest, ReprojectInt)
{
StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
float x = 0.3f, y = -0.2f, z = 2.0f;
float uLeftF, vLeftF, uRightF, vRightF;
model.reproject(x, y, z, uLeftF, vLeftF, uRightF, vRightF);
int uLeft, vLeft, uRight, vRight;
model.reproject(x, y, z, uLeft, vLeft, uRight, vRight);
EXPECT_EQ(uLeft, static_cast<int>(uLeftF));
EXPECT_EQ(vLeft, static_cast<int>(vLeftF));
EXPECT_EQ(uRight, static_cast<int>(uRightF));
EXPECT_EQ(vRight, static_cast<int>(vRightF));
}
TEST_F(StereoCameraModelTest, ReprojectProjectRoundTrip)
{
StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
float x = -0.45f, y = 0.25f, z = 3.7f;
float uLeft, vLeft, uRight, vRight;
model.reproject(x, y, z, uLeft, vLeft, uRight, vRight);
// the disparity of the reprojected pair gives the depth back...
float depth = model.computeDepth(uLeft - uRight);
EXPECT_NEAR(depth, z, 0.001f);
// ... and the left image point gives the 3D point back
float x2, y2, z2;
model.left().project(uLeft, vLeft, depth, x2, y2, z2);
EXPECT_NEAR(x2, x, 0.001f);
EXPECT_NEAR(y2, y, 0.001f);
EXPECT_NEAR(z2, z, 0.001f);
}
// Getter Tests
TEST_F(StereoCameraModelTest, Baseline)
corelib/test/test_util3d_correspondences.cpp
+65 -15
View File
@@ -2,6 +2,7 @@
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/core/StereoCameraModel.h"
#include "rtabmap/utilite/UException.h"
#include "rtabmap/utilite/UConversion.h"
#include <pcl/io/pcd_io.h>
@@ -82,44 +83,93 @@ TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesNoCommonIDs) {
EXPECT_TRUE(cloud2.empty());
}
// Reprojects a fixed non-planar 3D scene in both images of a rectified stereo
// camera. The two-view geometry must be generic: with a planar scene or a pure
// image translation, all correspondences are related by a homography and the
// fundamental matrix is then only defined up to a 1-parameter family
// (F = [e']x * H for any epipole e'). RANSAC can pick a member of that family
// which also fits an outlier, making the inlier count depend on floating-point
// details of the platform and of the OpenCV version. Here the points span a
// range of depths, so their disparities differ and the geometry is well
// constrained.
static void reprojectStereoPair(int index, pcl::PointXYZ & left, pcl::PointXYZ & right)
{
static const float points3d[12][3] = {
{-0.50f, -0.40f, 2.0f}, { 0.40f, -0.30f, 3.5f}, {-0.20f, 0.50f, 2.8f},
{ 0.60f, 0.20f, 5.0f}, {-0.60f, 0.10f, 4.2f}, { 0.10f, -0.50f, 6.5f},
{ 0.30f, 0.45f, 3.0f}, {-0.35f, -0.15f, 7.5f}, { 0.50f, -0.05f, 2.2f},
{-0.10f, 0.30f, 5.8f}, { 0.25f, 0.35f, 4.6f}, {-0.45f, 0.20f, 3.3f}};
static const StereoCameraModel model(500.0, 500.0, 320.0, 240.0, 0.12);
float uLeft, vLeft, uRight, vRight;
model.reproject(points3d[index][0], points3d[index][1], points3d[index][2],
uLeft, vLeft, uRight, vRight);
// extractXYZCorrespondencesRANSAC() only uses x and y, as image coordinates
left = pcl::PointXYZ(uLeft, vLeft, 0.0f);
right = pcl::PointXYZ(uRight, vRight, 0.0f);
}
TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACAcceptsCleanMatches) {
std::multimap<int, pcl::PointXYZ> words1;
std::multimap<int, pcl::PointXYZ> words2;
// 10 consistent matches
for (int i = 0; i < 10; ++i) {
words1.insert({i, pcl::PointXYZ(i * 1.0f, exp2(i)/10.0f, 0.0f)});
words2.insert({i, pcl::PointXYZ(i * 1.0f + 1.1f, exp2(i)/10.0f + 1.1f, 0.0f)}); // Slight noise
// 12 consistent matches
for (int i = 0; i < 12; ++i) {
pcl::PointXYZ left, right;
reprojectStereoPair(i, left, right);
words1.insert({i, left});
words2.insert({i, right});
}
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
EXPECT_EQ(cloud1.size(), cloud2.size());
EXPECT_GE(cloud1.size(), 8); // At least 8 inliers from 10 consistent matches
EXPECT_EQ(cloud1.size(), 12); // every match is on its epipolar line
}
TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACRejectsOutliers) {
std::multimap<int, pcl::PointXYZ> words1;
std::multimap<int, pcl::PointXYZ> words2;
// 8 inliers
for (int i = 0; i < 8; ++i) {
words1.insert({i, pcl::PointXYZ(i * 1.0f, exp2(i)/10.0f, 0.0f)});
words2.insert({i, pcl::PointXYZ(i * 1.0f + 1.1f, exp2(i)/10.0f + 1.1f, 0.0f)}); // Slight noise
// 12 inliers
for (int i = 0; i < 12; ++i) {
pcl::PointXYZ left, right;
reprojectStereoPair(i, left, right);
words1.insert({i, left});
words2.insert({i, right});
}
// 2 outliers
words1.insert({100, pcl::PointXYZ(0.0f, 0.0f, 0.0f)});
words2.insert({100, pcl::PointXYZ(100.0f, 100.0f, 0.0f)});
words1.insert({101, pcl::PointXYZ(1.0f, 1.0f, 0.0f)});
words2.insert({101, pcl::PointXYZ(200.0f, -50.0f, 0.0f)});
// 3 outliers: correct point in the left image, right point moved far away from
// the corresponding epipolar line (horizontal on a rectified stereo camera)
const int outlierSources[3] = {0, 4, 8};
const float outlierOffsets[3][2] = {{0.0f, 120.0f}, {0.0f, -150.0f}, {40.0f, 90.0f}};
for (int i = 0; i < 3; ++i) {
pcl::PointXYZ left, right;
reprojectStereoPair(outlierSources[i], left, right);
right.x += outlierOffsets[i][0];
right.y += outlierOffsets[i][1];
words1.insert({100+i, left});
words2.insert({100+i, right});
}
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
util3d::extractXYZCorrespondencesRANSAC(words1, words2, cloud1, cloud2);
EXPECT_EQ(cloud1.size(), cloud2.size());
EXPECT_EQ(cloud1.size(), 8); // RANSAC should reject 2 outliers
EXPECT_EQ(cloud1.size(), 12); // RANSAC should reject the 3 outliers
// none of the outliers should have survived
for (unsigned int i = 0; i < cloud2.size(); ++i) {
for (int j = 0; j < 3; ++j) {
pcl::PointXYZ left, right;
reprojectStereoPair(outlierSources[j], left, right);
EXPECT_FALSE(cloud2[i].x == right.x + outlierOffsets[j][0] &&
cloud2[i].y == right.y + outlierOffsets[j][1]);
}
}
}
TEST(Util3dCorrespondencesTest, ExtractXYZCorrespondencesRANSACFailsGracefullyOnTooFewMatches) {