From 6a3171005b1483537624f65be771652043b9206a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 22 Sep 2026 11:52:07 -0700 Subject: [PATCH] fixing ExtractXYZCorrespondencesRANSAC ci error --- .../include/rtabmap/core/StereoCameraModel.h | 45 ++++++++++ corelib/src/StereoCameraModel.cpp | 24 +++++ corelib/test/test_cameramodel.cpp | 39 ++++++++ corelib/test/test_stereocameramodel.cpp | 89 +++++++++++++++++++ corelib/test/test_util3d_correspondences.cpp | 80 +++++++++++++---- 5 files changed, 262 insertions(+), 15 deletions(-) diff --git a/corelib/include/rtabmap/core/StereoCameraModel.h b/corelib/include/rtabmap/core/StereoCameraModel.h index 4edca15c..d01a4f54 100644 --- a/corelib/include/rtabmap/core/StereoCameraModel.h +++ b/corelib/include/rtabmap/core/StereoCameraModel.h @@ -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. diff --git a/corelib/src/StereoCameraModel.cpp b/corelib/src/StereoCameraModel.cpp index cae0437c..b2ca00b2 100644 --- a/corelib/src/StereoCameraModel.cpp +++ b/corelib/src/StereoCameraModel.cpp @@ -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()) diff --git a/corelib/test/test_cameramodel.cpp b/corelib/test/test_cameramodel.cpp index b7542eb0..300cdaf0 100644 --- a/corelib/test/test_cameramodel.cpp +++ b/corelib/test/test_cameramodel.cpp @@ -298,6 +298,45 @@ TEST_F(CameraModelTest, ReprojectInt) EXPECT_NEAR(v, static_cast(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(fx_*x/z + cx_)); + EXPECT_FLOAT_EQ(v, static_cast(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) diff --git a/corelib/test/test_stereocameramodel.cpp b/corelib/test/test_stereocameramodel.cpp index 3f40b82a..c6c448c5 100644 --- a/corelib/test/test_stereocameramodel.cpp +++ b/corelib/test/test_stereocameramodel.cpp @@ -7,6 +7,7 @@ #include "rtabmap/utilite/UDirectory.h" #include "rtabmap/utilite/UFile.h" #include +#include 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(fx_*x/z + cx_)); + EXPECT_FLOAT_EQ(vLeft, static_cast(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(baseline_*fx_/z), 0.001f); +} + +TEST_F(StereoCameraModelTest, ReprojectDisparityDecreasesWithDepth) +{ + StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_); + + float previousDisparity = std::numeric_limits::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(cx_)); + EXPECT_FLOAT_EQ(vLeft, static_cast(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(uLeftF)); + EXPECT_EQ(vLeft, static_cast(vLeftF)); + EXPECT_EQ(uRight, static_cast(uRightF)); + EXPECT_EQ(vRight, static_cast(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) diff --git a/corelib/test/test_util3d_correspondences.cpp b/corelib/test/test_util3d_correspondences.cpp index 7887d451..1f8f9b9b 100644 --- a/corelib/test/test_util3d_correspondences.cpp +++ b/corelib/test/test_util3d_correspondences.cpp @@ -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 @@ -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 words1; std::multimap 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 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 words1; std::multimap 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 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) {