From e9ee86a187de4c665b6471e036b114c3359f095e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 09:08:07 +0100 Subject: [PATCH 1/7] Consistent interface/tests for rigid3d/sim3d/affine2d, pycolmap bindings for rigid3d --- src/colmap/estimators/CMakeLists.txt | 2 +- src/colmap/estimators/affine_transform.cc | 87 ++++----- src/colmap/estimators/affine_transform.h | 25 ++- .../estimators/affine_transform_test.cc | 173 ++++++++---------- src/colmap/estimators/similarity_transform.cc | 113 ++++++++++++ src/colmap/estimators/similarity_transform.h | 95 +++++----- .../estimators/similarity_transform_test.cc | 118 ++++++++---- src/colmap/geometry/essential_matrix.cc | 2 +- src/colmap/geometry/rigid3.h | 7 + src/colmap/geometry/rigid3_test.cc | 11 +- src/colmap/retrieval/vote_and_verify.cc | 7 +- src/colmap/util/types.h | 1 + src/pycolmap/estimators/affine_transform.cc | 66 ++++--- .../estimators/similarity_transform.cc | 41 +++++ src/pycolmap/geometry/bindings.cc | 5 +- 15 files changed, 489 insertions(+), 264 deletions(-) create mode 100644 src/colmap/estimators/similarity_transform.cc diff --git a/src/colmap/estimators/CMakeLists.txt b/src/colmap/estimators/CMakeLists.txt index 952e7f68ea..015b14e667 100644 --- a/src/colmap/estimators/CMakeLists.txt +++ b/src/colmap/estimators/CMakeLists.txt @@ -49,7 +49,7 @@ COLMAP_ADD_LIBRARY( homography_matrix.h homography_matrix.cc pose.h pose.cc generalized_pose.h generalized_pose.cc - similarity_transform.h + similarity_transform.h similarity_transform.cc translation_transform.h triangulation.h triangulation.cc two_view_geometry.h two_view_geometry.cc diff --git a/src/colmap/estimators/affine_transform.cc b/src/colmap/estimators/affine_transform.cc index 955a536d60..35e28824d4 100644 --- a/src/colmap/estimators/affine_transform.cc +++ b/src/colmap/estimators/affine_transform.cc @@ -29,6 +29,7 @@ #include "colmap/estimators/affine_transform.h" +#include "colmap/optim/loransac.h" #include "colmap/util/eigen_alignment.h" #include "colmap/util/logging.h" @@ -37,27 +38,27 @@ namespace colmap { -void AffineTransformEstimator::Estimate(const std::vector& points1, - const std::vector& points2, - std::vector* models) { - const size_t num_points = points1.size(); - THROW_CHECK_EQ(num_points, points2.size()); +void AffineTransformEstimator::Estimate(const std::vector& src, + const std::vector& tgt, + std::vector* tgt_from_src) { + const size_t num_points = src.size(); + THROW_CHECK_EQ(num_points, tgt.size()); THROW_CHECK_GE(num_points, 3); - THROW_CHECK(models != nullptr); + THROW_CHECK(tgt_from_src != nullptr); - models->clear(); + tgt_from_src->clear(); // Sets up the linear system that we solve to obtain a least squared solution // for the affine transformation. Eigen::Matrix A(2 * num_points, 6); Eigen::VectorXd b(2 * num_points, 1); for (size_t i = 0; i < num_points; ++i) { - A.block<1, 3>(2 * i, 0) = points1[i].transpose().homogeneous(); + A.block<1, 3>(2 * i, 0) = src[i].transpose().homogeneous(); A.block<1, 3>(2 * i, 3).setZero(); - b(2 * i) = points2[i].x(); + b(2 * i) = tgt[i].x(); A.block<1, 3>(2 * i + 1, 0).setZero(); - A.block<1, 3>(2 * i + 1, 3) = points1[i].transpose().homogeneous(); - b(2 * i + 1) = points2[i].y(); + A.block<1, 3>(2 * i + 1, 3) = src[i].transpose().homogeneous(); + b(2 * i + 1) = tgt[i].y(); } Eigen::Vector6d sol; @@ -75,43 +76,47 @@ void AffineTransformEstimator::Estimate(const std::vector& points1, sol = svd.solve(b); } - models->resize(1); - (*models)[0] = + tgt_from_src->resize(1); + (*tgt_from_src)[0] = Eigen::Map>(sol.data()).transpose(); } -void AffineTransformEstimator::Residuals(const std::vector& points1, - const std::vector& points2, - const M_t& A, +void AffineTransformEstimator::Residuals(const std::vector& src, + const std::vector& tgt, + const M_t& tgt_from_src, std::vector* residuals) { - THROW_CHECK_EQ(points1.size(), points2.size()); - - residuals->resize(points1.size()); - - // Note that this code might not be as nice as Eigen expressions, - // but it is significantly faster in various tests. - - const double A_00 = A(0, 0); - const double A_01 = A(0, 1); - const double A_02 = A(0, 2); - const double A_10 = A(1, 0); - const double A_11 = A(1, 1); - const double A_12 = A(1, 2); - - for (size_t i = 0; i < points1.size(); ++i) { - const double s_0 = points1[i](0); - const double s_1 = points1[i](1); - const double d_0 = points2[i](0); - const double d_1 = points2[i](1); - - const double pd_0 = A_00 * s_0 + A_01 * s_1 + A_02; - const double pd_1 = A_10 * s_0 + A_11 * s_1 + A_12; + const size_t num_points = src.size(); + THROW_CHECK_EQ(num_points, tgt.size()); + residuals->resize(num_points); + for (size_t i = 0; i < num_points; ++i) { + (*residuals)[i] = + (tgt[i] - tgt_from_src * src[i].homogeneous()).squaredNorm(); + } +} - const double dd_0 = d_0 - pd_0; - const double dd_1 = d_1 - pd_1; +bool EstimateAffine2d(const std::vector& src, + const std::vector& tgt, + Eigen::Matrix2x3d& tgt_from_src) { + std::vector models; + AffineTransformEstimator::Estimate(src, tgt, &models); + if (models.empty()) { + return false; + } + tgt_from_src = models[0]; + return true; +} - (*residuals)[i] = dd_0 * dd_0 + dd_1 * dd_1; +typename RANSAC::Report EstimateAffine2dRobust( + const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options, + Eigen::Matrix2x3d& tgt_from_src) { + LORANSAC ransac(options); + auto report = ransac.Estimate(src, tgt); + if (report.success) { + tgt_from_src = report.model; } + return report; } } // namespace colmap diff --git a/src/colmap/estimators/affine_transform.h b/src/colmap/estimators/affine_transform.h index adbd82c16b..d600314440 100644 --- a/src/colmap/estimators/affine_transform.h +++ b/src/colmap/estimators/affine_transform.h @@ -29,6 +29,7 @@ #pragma once +#include "colmap/optim/ransac.h" #include "colmap/util/eigen_alignment.h" #include "colmap/util/types.h" @@ -42,21 +43,31 @@ class AffineTransformEstimator { public: typedef Eigen::Vector2d X_t; typedef Eigen::Vector2d Y_t; - typedef Eigen::Matrix M_t; + typedef Eigen::Matrix2x3d M_t; // The minimum number of samples needed to estimate a model. static const int kMinNumSamples = 3; // Estimate the affine transformation from at least 3 correspondences. - static void Estimate(const std::vector& points1, - const std::vector& points2, - std::vector* models); + static void Estimate(const std::vector& src, + const std::vector& tgt, + std::vector* tgt_from_src); // Compute the squared transformation error. - static void Residuals(const std::vector& points1, - const std::vector& points2, - const M_t& E, + static void Residuals(const std::vector& src, + const std::vector& tgt, + const M_t& tgt_from_src, std::vector* residuals); }; +bool EstimateAffine2d(const std::vector& src, + const std::vector& tgt, + Eigen::Matrix2x3d& tgt_from_src); + +typename RANSAC::Report EstimateAffine2dRobust( + const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options, + Eigen::Matrix2x3d& tgt_from_src); + } // namespace colmap diff --git a/src/colmap/estimators/affine_transform_test.cc b/src/colmap/estimators/affine_transform_test.cc index 26752a119e..c708f5ebb3 100644 --- a/src/colmap/estimators/affine_transform_test.cc +++ b/src/colmap/estimators/affine_transform_test.cc @@ -29,7 +29,9 @@ #include "colmap/estimators/affine_transform.h" +#include "colmap/math/random.h" #include "colmap/util/eigen_alignment.h" +#include "colmap/util/eigen_matchers.h" #include "colmap/util/logging.h" #include @@ -38,117 +40,94 @@ namespace colmap { namespace { -TEST(AffineTransform, Minimal) { - for (int x = 0; x < 10; ++x) { - Eigen::Matrix A; - A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1; - - std::vector src; - src.emplace_back(x, 10); - src.emplace_back(1, 0); - src.emplace_back(2, 1); - - std::vector dst; - for (size_t i = 0; i < src.size(); ++i) { - dst.push_back(A * src[i].homogeneous()); - } - - AffineTransformEstimator estimator; - std::vector> models; - estimator.Estimate(src, dst, &models); - - ASSERT_EQ(models.size(), 1); - - std::vector residuals; - estimator.Residuals(src, dst, models[0], &residuals); - - EXPECT_EQ(residuals.size(), src.size()); - - for (size_t i = 0; i < src.size(); ++i) { - EXPECT_LT(residuals[i], 1e-6); - } +std::pair, std::vector> +GenerateData(size_t num_inliers, + size_t num_outliers, + const Eigen::Matrix2x3d& tgt_from_src) { + std::vector src; + std::vector tgt; + + // Generate inlier data. + for (size_t i = 0; i < num_inliers; ++i) { + src.emplace_back(i, std::sqrt(i) + 2); + tgt.push_back(tgt_from_src * src.back().homogeneous()); } -} - -TEST(AffineTransform, NonMinimal) { - for (int x = 0; x < 10; ++x) { - Eigen::Matrix A; - A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1; - - std::vector src; - src.emplace_back(x, 10); - src.emplace_back(1, 0); - src.emplace_back(2, 1); - src.emplace_back(-10, 10); - src.emplace_back(-2, 5); - - std::vector dst; - for (size_t i = 0; i < src.size(); ++i) { - dst.push_back(A * src[i].homogeneous()); - } - - AffineTransformEstimator estimator; - std::vector> models; - estimator.Estimate(src, dst, &models); - - ASSERT_EQ(models.size(), 1); - std::vector residuals; - estimator.Residuals(src, dst, models[0], &residuals); - - EXPECT_EQ(residuals.size(), src.size()); - - for (size_t i = 0; i < src.size(); ++i) { - EXPECT_LT(residuals[i], 1e-6); - } + // Add some faulty data. + for (size_t i = 0; i < num_outliers; ++i) { + src.emplace_back(RandomUniformReal(-3000.0, -2000.0), + RandomUniformReal(-4000.0, -3000.0)); + tgt.emplace_back(RandomUniformReal(-3000.0, -2000.0), + RandomUniformReal(-4000.0, -3000.0)); } -} -TEST(AffineTransform, MinimalDegenerate) { - for (int x = 0; x < 10; ++x) { - Eigen::Matrix A; - A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1; + return {std::move(src), std::move(tgt)}; +} - std::vector src; - src.emplace_back(0, 0); - src.emplace_back(0, 0); - src.emplace_back(2, 1); +void TestEstimateAffine2dWithNumCoords(const size_t num_coords) { + const Eigen::Matrix2x3d gt_tgt_from_src = Eigen::Matrix2x3d::Random(); + const auto [src, tgt] = GenerateData( + /*num_inliers=*/num_coords, + /*num_outliers=*/0, + gt_tgt_from_src); - std::vector dst; - for (size_t i = 0; i < src.size(); ++i) { - dst.push_back(A * src[i].homogeneous()); - } + Eigen::Matrix2x3d tgt_from_src; + EXPECT_TRUE(EstimateAffine2d(src, tgt, tgt_from_src)); + EXPECT_THAT(tgt_from_src, EigenMatrixNear(gt_tgt_from_src, 1e-6)); +} - AffineTransformEstimator estimator; - std::vector> models; - estimator.Estimate(src, dst, &models); +TEST(Affine2d, EstimateMinimal) { TestEstimateAffine2dWithNumCoords(3); } - ASSERT_TRUE(models.empty()); - } +TEST(Affine2d, EstimateOverDetermined) { + TestEstimateAffine2dWithNumCoords(100); } -TEST(AffineTransform, NonMinimalDegenerate) { - for (int x = 0; x < 10; ++x) { - Eigen::Matrix A; - A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1; +TEST(Affine2d, EstimateMinimalDegenerate) { + std::vector invalid_src_dst(3, Eigen::Vector2d::Zero()); + Eigen::Matrix2x3d tgt_from_src; + EXPECT_FALSE( + EstimateAffine2d(invalid_src_dst, invalid_src_dst, tgt_from_src)); +} - std::vector src; - src.emplace_back(0, 0); - src.emplace_back(0, 0); - src.emplace_back(2, 1); - src.emplace_back(2, 1); +TEST(Affine2d, EstimateNonMinimalDegenerate) { + std::vector invalid_src_dst(5, Eigen::Vector2d::Zero()); + Eigen::Matrix2x3d tgt_from_src; + EXPECT_FALSE( + EstimateAffine2d(invalid_src_dst, invalid_src_dst, tgt_from_src)); +} - std::vector dst; - for (size_t i = 0; i < src.size(); ++i) { - dst.push_back(A * src[i].homogeneous()); +TEST(Affine2d, EstimateRobust) { + SetPRNGSeed(0); + + const size_t num_inliers = 1000; + const size_t num_outliers = 400; + + const Eigen::Matrix2x3d gt_tgt_from_src = Eigen::Matrix2x3d::Random(); + const auto [src, tgt] = GenerateData( + /*num_inliers=*/num_inliers, + /*num_outliers=*/num_outliers, + gt_tgt_from_src); + + // Robustly estimate transformation using RANSAC. + RANSACOptions options; + options.max_error = 10; + Eigen::Matrix2x3d tgt_from_src; + const auto report = EstimateAffine2dRobust(src, tgt, options, tgt_from_src); + + EXPECT_TRUE(report.success); + EXPECT_GT(report.num_trials, 0); + + // Make sure outliers were detected correctly. + EXPECT_EQ(report.support.num_inliers, num_inliers); + for (size_t i = 0; i < num_inliers + num_outliers; ++i) { + if (i >= num_inliers) { + EXPECT_FALSE(report.inlier_mask[i]); + } else { + EXPECT_TRUE(report.inlier_mask[i]); } - - AffineTransformEstimator estimator; - std::vector> models; - estimator.Estimate(src, dst, &models); - - ASSERT_TRUE(models.empty()); } + + EXPECT_THAT(tgt_from_src, EigenMatrixNear(gt_tgt_from_src, 1e-6)); } } // namespace diff --git a/src/colmap/estimators/similarity_transform.cc b/src/colmap/estimators/similarity_transform.cc new file mode 100644 index 0000000000..bbc9dbe0e8 --- /dev/null +++ b/src/colmap/estimators/similarity_transform.cc @@ -0,0 +1,113 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "colmap/estimators/similarity_transform.h" + +namespace colmap { +namespace { + +template +inline bool EstimateRigidOrSim3d(const std::vector& src, + const std::vector& tgt, + Eigen::Matrix3x4d& tgt_from_src) { + std::vector models; + SimilarityTransformEstimator<3, kEstimateScale>().Estimate(src, tgt, &models); + if (models.empty()) { + return false; + } + THROW_CHECK_EQ(models.size(), 1); + tgt_from_src = models[0]; + return true; +} + +template +inline typename RANSAC>::Report +EstimateRigidOrSim3dRobust(const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options, + Eigen::Matrix3x4d& tgt_from_src) { + LORANSAC, + SimilarityTransformEstimator<3, kEstimateScale>> + ransac(options); + auto report = ransac.Estimate(src, tgt); + if (report.success) { + tgt_from_src = report.model; + } + return report; +} + +} // namespace + +bool EstimateRigid3d(const std::vector& src, + const std::vector& tgt, + Rigid3d& tgt_from_src) { + Eigen::Matrix3x4d tgt_from_src_mat; + if (!EstimateRigidOrSim3d(src, tgt, tgt_from_src_mat)) { + return false; + } + tgt_from_src = Rigid3d::FromMatrix(tgt_from_src_mat); + return true; +} + +typename RANSAC>::Report +EstimateRigid3dRobust(const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options, + Rigid3d& tgt_from_src) { + Eigen::Matrix3x4d tgt_from_src_mat; + auto report = + EstimateRigidOrSim3dRobust(src, tgt, options, tgt_from_src_mat); + tgt_from_src = Rigid3d::FromMatrix(tgt_from_src_mat); + return report; +} + +bool EstimateSim3d(const std::vector& src, + const std::vector& tgt, + Sim3d& tgt_from_src) { + Eigen::Matrix3x4d tgt_from_src_mat; + if (!EstimateRigidOrSim3d(src, tgt, tgt_from_src_mat)) { + return false; + } + tgt_from_src = Sim3d::FromMatrix(tgt_from_src_mat); + return true; +} + +typename RANSAC>::Report +EstimateSim3dRobust(const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options, + Sim3d& tgt_from_src) { + Eigen::Matrix3x4d tgt_from_src_mat; + auto report = + EstimateRigidOrSim3dRobust(src, tgt, options, tgt_from_src_mat); + tgt_from_src = Sim3d::FromMatrix(tgt_from_src_mat); + return report; +} + +} // namespace colmap diff --git a/src/colmap/estimators/similarity_transform.h b/src/colmap/estimators/similarity_transform.h index c8c3527ce0..77307421ec 100644 --- a/src/colmap/estimators/similarity_transform.h +++ b/src/colmap/estimators/similarity_transform.h @@ -29,6 +29,7 @@ #pragma once +#include "colmap/geometry/rigid3.h" #include "colmap/geometry/sim3.h" #include "colmap/optim/loransac.h" #include "colmap/optim/ransac.h" @@ -69,58 +70,49 @@ class SimilarityTransformEstimator { // Estimate the similarity transform. // // @param src Set of corresponding source points. - // @param dst Set of corresponding destination points. + // @param tgt Set of corresponding destination points. // // @return 4x4 homogeneous transformation matrix. static void Estimate(const std::vector& src, - const std::vector& dst, - std::vector* models); + const std::vector& tgt, + std::vector* tgt_from_src); // Calculate the transformation error for each corresponding point pair. // // Residuals are defined as the squared transformation error when // transforming the source to the destination coordinates. // - // @param src Set of corresponding points in the source coordinate - // system as a Nx3 matrix. - // @param dst Set of corresponding points in the destination - // coordinate system as a Nx3 matrix. - // @param matrix 4x4 homogeneous transformation matrix. - // @param residuals Output vector of residuals for each point pair. + // @param src Set of corresponding points in the source coordinate + // system as a Nx3 matrix. + // @param tgt Set of corresponding points in the destination + // coordinate system as a Nx3 matrix. + // @param tgt_from_src 4x4 homogeneous transformation matrix. + // @param residuals Output vector of residuals for each point pair. static void Residuals(const std::vector& src, - const std::vector& dst, - const M_t& matrix, + const std::vector& tgt, + const M_t& tgt_from_src, std::vector* residuals); }; -inline bool EstimateSim3d(const std::vector& src, - const std::vector& tgt, - Sim3d& tgt_from_src) { - std::vector models; - SimilarityTransformEstimator<3, true>().Estimate(src, tgt, &models); - if (models.empty()) { - return false; - } - THROW_CHECK_EQ(models.size(), 1); - tgt_from_src = Sim3d::FromMatrix(models[0]); - return true; -} +bool EstimateRigid3d(const std::vector& src, + const std::vector& tgt, + Rigid3d& tgt_from_src); + +typename RANSAC>::Report +EstimateRigid3dRobust(const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options, + Rigid3d& tgt_from_src); -template -inline typename RANSAC>::Report +bool EstimateSim3d(const std::vector& src, + const std::vector& tgt, + Sim3d& tgt_from_src); + +typename RANSAC>::Report EstimateSim3dRobust(const std::vector& src, const std::vector& tgt, const RANSACOptions& options, - Sim3d& tgt_from_src) { - LORANSAC, - SimilarityTransformEstimator<3, kEstimateScale>> - ransac(options); - auto report = ransac.Estimate(src, tgt); - if (report.success) { - tgt_from_src = Sim3d::FromMatrix(report.model); - } - return report; -} + Sim3d& tgt_from_src); //////////////////////////////////////////////////////////////////////////////// // Implementation @@ -129,44 +121,43 @@ EstimateSim3dRobust(const std::vector& src, template void SimilarityTransformEstimator::Estimate( const std::vector& src, - const std::vector& dst, + const std::vector& tgt, std::vector* models) { - THROW_CHECK_EQ(src.size(), dst.size()); + THROW_CHECK_EQ(src.size(), tgt.size()); THROW_CHECK(models != nullptr); models->clear(); Eigen::Matrix src_mat(kDim, src.size()); - Eigen::Matrix dst_mat(kDim, dst.size()); + Eigen::Matrix dst_mat(kDim, tgt.size()); for (size_t i = 0; i < src.size(); ++i) { src_mat.col(i) = src[i]; - dst_mat.col(i) = dst[i]; + dst_mat.col(i) = tgt[i]; } - const M_t model = Eigen::umeyama(src_mat, dst_mat, kEstimateScale) - .template topLeftCorner(); + const M_t sol = Eigen::umeyama(src_mat, dst_mat, kEstimateScale) + .template topLeftCorner(); - if (model.array().isNaN().any()) { + if (sol.hasNaN()) { return; } models->resize(1); - (*models)[0] = model; + (*models)[0] = sol; } template void SimilarityTransformEstimator::Residuals( const std::vector& src, - const std::vector& dst, - const M_t& matrix, + const std::vector& tgt, + const M_t& tgt_from_src, std::vector* residuals) { - THROW_CHECK_EQ(src.size(), dst.size()); - - residuals->resize(src.size()); - - for (size_t i = 0; i < src.size(); ++i) { - const Y_t dst_transformed = matrix * src[i].homogeneous(); - (*residuals)[i] = (dst[i] - dst_transformed).squaredNorm(); + const size_t num_points = src.size(); + THROW_CHECK_EQ(num_points, tgt.size()); + residuals->resize(num_points); + for (size_t i = 0; i < num_points; ++i) { + (*residuals)[i] = + (tgt[i] - tgt_from_src * src[i].homogeneous()).squaredNorm(); } } diff --git a/src/colmap/estimators/similarity_transform_test.cc b/src/colmap/estimators/similarity_transform_test.cc index ab8b564cea..1209fcab54 100644 --- a/src/colmap/estimators/similarity_transform_test.cc +++ b/src/colmap/estimators/similarity_transform_test.cc @@ -32,6 +32,7 @@ #include "colmap/geometry/sim3.h" #include "colmap/math/random.h" #include "colmap/util/eigen_alignment.h" +#include "colmap/util/eigen_matchers.h" #include #include @@ -39,20 +40,43 @@ namespace colmap { namespace { +std::pair, std::vector > +GenerateData(size_t num_inliers, + size_t num_outliers, + const Eigen::Matrix3x4d& tgt_from_src) { + std::vector src; + std::vector tgt; + + // Generate inlier data. + for (size_t i = 0; i < num_inliers; ++i) { + src.emplace_back(i, std::sqrt(i) + 2, std::sqrt(2 * i + 2)); + tgt.push_back(tgt_from_src * src.back().homogeneous()); + } + + // Add some faulty data. + for (size_t i = 0; i < num_outliers; ++i) { + src.emplace_back(RandomUniformReal(-3000.0, -2000.0), + RandomUniformReal(-4000.0, -3000.0), + RandomUniformReal(-5000.0, -4000.0)); + tgt.emplace_back(RandomUniformReal(-3000.0, -2000.0), + RandomUniformReal(-4000.0, -3000.0), + RandomUniformReal(-5000.0, -4000.0)); + } + + return {std::move(src), std::move(tgt)}; +} + void TestEstimateSim3dWithNumCoords(const size_t num_coords) { const Sim3d gt_tgt_from_src(RandomUniformReal(0.1, 10), Eigen::Quaterniond::UnitRandom(), Eigen::Vector3d::Random()); - - std::vector src; - std::vector dst; - for (size_t i = 0; i < num_coords; ++i) { - src.emplace_back(i, i + 2, i * i); - dst.push_back(gt_tgt_from_src * src.back()); - } + const auto [src, tgt] = GenerateData( + /*num_inliers=*/num_coords, + /*num_outliers=*/0, + gt_tgt_from_src.ToMatrix()); Sim3d tgt_from_src; - EXPECT_TRUE(EstimateSim3d(src, dst, tgt_from_src)); + EXPECT_TRUE(EstimateSim3d(src, tgt, tgt_from_src)); EXPECT_NEAR(gt_tgt_from_src.scale, tgt_from_src.scale, 1e-6); EXPECT_LT(gt_tgt_from_src.rotation.angularDistance(tgt_from_src.rotation), 1e-6); @@ -64,37 +88,67 @@ TEST(Sim3d, EstimateMinimal) { TestEstimateSim3dWithNumCoords(3); } TEST(Sim3d, EstimateOverDetermined) { TestEstimateSim3dWithNumCoords(100); } -TEST(Sim3d, EstimateDegenerate) { +TEST(Sim3d, EstimateMinimalDegenerate) { std::vector invalid_src_dst(3, Eigen::Vector3d::Zero()); Sim3d tgt_from_src; EXPECT_FALSE(EstimateSim3d(invalid_src_dst, invalid_src_dst, tgt_from_src)); } -TEST(Sim3d, EstimateRobust) { +TEST(Sim3d, EstimateNonMinimalDegenerate) { + std::vector invalid_src_dst(5, Eigen::Vector3d::Zero()); + Sim3d tgt_from_src; + EXPECT_FALSE(EstimateSim3d(invalid_src_dst, invalid_src_dst, tgt_from_src)); +} + +TEST(Rigid3d, EstimateRobust) { SetPRNGSeed(0); - const size_t num_samples = 1000; + const size_t num_inliers = 1000; const size_t num_outliers = 400; - // Create some arbitrary transformation. - const Sim3d expectedTgtFromSrc( - 2, Eigen::Quaterniond::UnitRandom(), Eigen::Vector3d(100, 10, 10)); + const Rigid3d gt_tgt_from_src(Eigen::Quaterniond::UnitRandom(), + Eigen::Vector3d(100, 10, 10)); + const auto [src, tgt] = GenerateData( + /*num_inliers=*/num_inliers, + /*num_outliers=*/num_outliers, + gt_tgt_from_src.ToMatrix()); - // Generate exact data - std::vector src; - std::vector tgt; - for (size_t i = 0; i < num_samples; ++i) { - src.emplace_back(i, std::sqrt(i) + 2, std::sqrt(2 * i + 2)); - tgt.push_back(expectedTgtFromSrc * src.back()); - } + // Robustly estimate transformation using RANSAC. + RANSACOptions options; + options.max_error = 10; + Rigid3d tgt_from_src; + const auto report = EstimateRigid3dRobust(src, tgt, options, tgt_from_src); - // Add some faulty data. - for (size_t i = 0; i < num_outliers; ++i) { - tgt[i] = Eigen::Vector3d(RandomUniformReal(-3000.0, -2000.0), - RandomUniformReal(-4000.0, -3000.0), - RandomUniformReal(-5000.0, -4000.0)); + EXPECT_TRUE(report.success); + EXPECT_GT(report.num_trials, 0); + + // Make sure outliers were detected correctly. + EXPECT_EQ(report.support.num_inliers, num_inliers); + for (size_t i = 0; i < num_inliers + num_outliers; ++i) { + if (i >= num_inliers) { + EXPECT_FALSE(report.inlier_mask[i]); + } else { + EXPECT_TRUE(report.inlier_mask[i]); + } } + EXPECT_THAT(tgt_from_src.ToMatrix(), + EigenMatrixNear(gt_tgt_from_src.ToMatrix(), 1e-6)); +} + +TEST(Sim3d, EstimateRobust) { + SetPRNGSeed(0); + + const size_t num_inliers = 1000; + const size_t num_outliers = 400; + + const Sim3d gt_tgt_from_src( + 2, Eigen::Quaterniond::UnitRandom(), Eigen::Vector3d(100, 10, 10)); + const auto [src, tgt] = GenerateData( + /*num_inliers=*/num_inliers, + /*num_outliers=*/num_outliers, + gt_tgt_from_src.ToMatrix()); + // Robustly estimate transformation using RANSAC. RANSACOptions options; options.max_error = 10; @@ -105,19 +159,17 @@ TEST(Sim3d, EstimateRobust) { EXPECT_GT(report.num_trials, 0); // Make sure outliers were detected correctly. - EXPECT_EQ(report.support.num_inliers, num_samples - num_outliers); - for (size_t i = 0; i < num_samples; ++i) { - if (i < num_outliers) { + EXPECT_EQ(report.support.num_inliers, num_inliers); + for (size_t i = 0; i < num_inliers + num_outliers; ++i) { + if (i >= num_inliers) { EXPECT_FALSE(report.inlier_mask[i]); } else { EXPECT_TRUE(report.inlier_mask[i]); } } - // Make sure original transformation is estimated correctly. - const double matrix_diff = - (expectedTgtFromSrc.ToMatrix() - tgt_from_src.ToMatrix()).norm(); - EXPECT_LT(matrix_diff, 1e-6); + EXPECT_THAT(tgt_from_src.ToMatrix(), + EigenMatrixNear(gt_tgt_from_src.ToMatrix(), 1e-6)); } } // namespace diff --git a/src/colmap/geometry/essential_matrix.cc b/src/colmap/geometry/essential_matrix.cc index 486153df66..6769387ada 100644 --- a/src/colmap/geometry/essential_matrix.cc +++ b/src/colmap/geometry/essential_matrix.cc @@ -105,7 +105,7 @@ void FindOptimalImageObservations(const Eigen::Matrix3d& E, const Eigen::Vector3d& point1_homogeneous = point1.homogeneous(); const Eigen::Vector3d& point2_homogeneous = point2.homogeneous(); - Eigen::Matrix S; + Eigen::Matrix2x3d S; S << 1, 0, 0, 0, 1, 0; // Epipolar lines. diff --git a/src/colmap/geometry/rigid3.h b/src/colmap/geometry/rigid3.h index b807ffec23..bc3922246b 100644 --- a/src/colmap/geometry/rigid3.h +++ b/src/colmap/geometry/rigid3.h @@ -57,6 +57,13 @@ struct Rigid3d { return matrix; } + static inline Rigid3d FromMatrix(const Eigen::Matrix3x4d& matrix) { + Rigid3d t; + t.rotation = Eigen::Quaterniond(matrix.leftCols<3>()).normalized(); + t.translation = matrix.rightCols<1>(); + return t; + } + // Adjoint matrix to propagate uncertainty on Rigid3d // [Reference] https://gtsam.org/2021/02/23/uncertainties-part3.html inline Eigen::Matrix6d Adjoint() const { diff --git a/src/colmap/geometry/rigid3_test.cc b/src/colmap/geometry/rigid3_test.cc index 2c3c7aa39f..1176c310ae 100644 --- a/src/colmap/geometry/rigid3_test.cc +++ b/src/colmap/geometry/rigid3_test.cc @@ -74,7 +74,7 @@ TEST(Rigid3d, Inverse) { } } -TEST(Rigid3d, Matrix) { +TEST(Rigid3d, ToMatrix) { const Rigid3d b_from_a = TestRigid3d(); const Eigen::Matrix3x4d b_from_a_mat = b_from_a.ToMatrix(); for (int i = 0; i < 100; ++i) { @@ -84,6 +84,15 @@ TEST(Rigid3d, Matrix) { } } +TEST(Rigid3d, FromMatrix) { + const Rigid3d b1_from_a = TestRigid3d(); + const Rigid3d b2_from_a = Rigid3d::FromMatrix(b1_from_a.ToMatrix()); + for (int i = 0; i < 100; ++i) { + const Eigen::Vector3d x_in_a = Eigen::Vector3d::Random(); + EXPECT_LT((b1_from_a * x_in_a - b2_from_a * x_in_a).norm(), 1e-6); + } +} + TEST(Rigid3d, ApplyNoRotation) { const Rigid3d b_from_a(Eigen::Quaterniond::Identity(), Eigen::Vector3d(1, 2, 3)); diff --git a/src/colmap/retrieval/vote_and_verify.cc b/src/colmap/retrieval/vote_and_verify.cc index a6ca13070f..1f85fe4806 100644 --- a/src/colmap/retrieval/vote_and_verify.cc +++ b/src/colmap/retrieval/vote_and_verify.cc @@ -406,15 +406,14 @@ int VoteAndVerify(const VoteAndVerifyOptions& options, } // Local optimization on matching inlier points. - std::vector> models; + std::vector models; AffineTransformEstimator::Estimate( best_inlier_points1, best_inlier_points2, &models); THROW_CHECK_EQ(models.size(), 1); - const Eigen::Matrix& A = models[0]; + const Eigen::Matrix2x3d& A = models[0]; Eigen::Matrix3d A_homogeneous = Eigen::Matrix3d::Identity(); A_homogeneous.topRows<2>() = A; - const Eigen::Matrix inv_A = - A_homogeneous.inverse().topRows<2>(); + const Eigen::Matrix2x3d inv_A = A_homogeneous.inverse().topRows<2>(); TwoWayTransform local_tform; local_tform.A12 = A.leftCols<2>().cast(); diff --git a/src/colmap/util/types.h b/src/colmap/util/types.h index e8bb8e5b20..6274b3ae48 100644 --- a/src/colmap/util/types.h +++ b/src/colmap/util/types.h @@ -62,6 +62,7 @@ namespace Eigen { using Matrix3x4f = Matrix; using Matrix3x4d = Matrix; +using Matrix2x3d = Matrix; using Matrix6d = Matrix; using Vector3ub = Matrix; using Vector4ub = Matrix; diff --git a/src/pycolmap/estimators/affine_transform.cc b/src/pycolmap/estimators/affine_transform.cc index db3de7832d..1334119954 100644 --- a/src/pycolmap/estimators/affine_transform.cc +++ b/src/pycolmap/estimators/affine_transform.cc @@ -16,30 +16,50 @@ using namespace colmap; using namespace pybind11::literals; namespace py = pybind11; -py::typing::Optional PyEstimateAffineTransform2D( - const std::vector& points2D1, - const std::vector& points2D2, - const RANSACOptions& options) { - py::gil_scoped_release release; - THROW_CHECK_EQ(points2D1.size(), points2D2.size()); - LORANSAC ransac(options); - const auto report = ransac.Estimate(points2D1, points2D2); - py::gil_scoped_acquire acquire; - if (!report.success) { - return py::none(); - } - return py::dict("A"_a = report.model, - "num_inliers"_a = report.support.num_inliers, - "inlier_mask"_a = ToPythonMask(report.inlier_mask)); -} - void BindAffineTransformEstimator(py::module& m) { auto est_options = m.attr("RANSACOptions")().cast(); - m.def("estimate_affine_transform2D", - &PyEstimateAffineTransform2D, - "points2D1"_a, - "points2D2"_a, - py::arg_v("estimation_options", est_options, "RANSACOptions()"), - "Robustly estimate 2D affine transformation using LO-RANSAC."); + m.def( + "estimate_affine2d", + [](const std::vector& src, + const std::vector& tgt) + -> py::typing::Optional { + py::gil_scoped_release release; + Eigen::Matrix2x3d tgt_from_src; + const bool success = EstimateSim3d(src, tgt, tgt_from_src); + py::gil_scoped_acquire acquire; + if (success) { + return py::cast(tgt_from_src); + } else { + return py::none(); + } + }, + "src"_a, + "tgt"_a, + "Estimate the 2D affine transform tgt_from_src."); + + m.def( + "estimate_affine2d_robust", + [](const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options) + -> py::typing::Optional { + py::gil_scoped_release release; + Eigen::Matrix2x3d tgt_from_src; + const auto report = + EstimateSim3dRobust(src, tgt, options, tgt_from_src); + py::gil_scoped_acquire acquire; + if (!report.success) { + return py::none(); + } + return py::dict( + "tgt_from_src"_a = Eigen::Matrix2x3d::FromMatrix(report.model), + "num_inliers"_a = report.support.num_inliers, + "inlier_mask"_a = ToPythonMask(report.inlier_mask)); + }, + "src"_a, + "tgt"_a, + py::arg_v("estimation_options", ransac_options, "RANSACOptions()"), + "Robustly estimate the 2D affine transform tgt_from_src using " + "LO-RANSAC."); } diff --git a/src/pycolmap/estimators/similarity_transform.cc b/src/pycolmap/estimators/similarity_transform.cc index 734bb0213b..96d39744db 100644 --- a/src/pycolmap/estimators/similarity_transform.cc +++ b/src/pycolmap/estimators/similarity_transform.cc @@ -18,6 +18,47 @@ namespace py = pybind11; void BindSimilarityTransformEstimator(py::module& m) { auto ransac_options = m.attr("RANSACOptions")().cast(); + m.def( + "estimate_rigid3d", + [](const std::vector& src, + const std::vector& tgt) + -> py::typing::Optional { + py::gil_scoped_release release; + Rigid3d tgt_from_src; + const bool success = EstimateRigid3d(src, tgt, tgt_from_src); + py::gil_scoped_acquire acquire; + if (success) { + return py::cast(tgt_from_src); + } else { + return py::none(); + } + }, + "src"_a, + "tgt"_a, + "Estimate the 3D rigid transform tgt_from_src."); + + m.def( + "estimate_rigid3d_robust", + [](const std::vector& src, + const std::vector& tgt, + const RANSACOptions& options) -> py::typing::Optional { + py::gil_scoped_release release; + Rigid3d tgt_from_src; + const auto report = + EstimateRigid3dRobust(src, tgt, options, tgt_from_src); + py::gil_scoped_acquire acquire; + if (!report.success) { + return py::none(); + } + return py::dict("tgt_from_src"_a = Rigid3d::FromMatrix(report.model), + "num_inliers"_a = report.support.num_inliers, + "inlier_mask"_a = ToPythonMask(report.inlier_mask)); + }, + "src"_a, + "tgt"_a, + py::arg_v("estimation_options", ransac_options, "RANSACOptions()"), + "Robustly estimate the 3D rigid transform tgt_from_src using LO-RANSAC."); + m.def( "estimate_sim3d", [](const std::vector& src, diff --git a/src/pycolmap/geometry/bindings.cc b/src/pycolmap/geometry/bindings.cc index 04d2d010ec..53e915d1d0 100644 --- a/src/pycolmap/geometry/bindings.cc +++ b/src/pycolmap/geometry/bindings.cc @@ -78,10 +78,7 @@ void BindGeometry(py::module& m) { .def(py::init(), "rotation"_a, "translation"_a) - .def(py::init([](const Eigen::Matrix3x4d& matrix) { - return Rigid3d(Eigen::Quaterniond(matrix.leftCols<3>()), - matrix.col(3)); - }), + .def(py::init(&Rigid3d::FromMatrix), "matrix"_a, "3x4 transformation matrix.") .def_readwrite("rotation", &Rigid3d::rotation) From cfd89a1970493bd8519804be4439ed07764d8245 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 09:16:43 +0100 Subject: [PATCH 2/7] d --- src/colmap/estimators/affine_transform.cc | 1 + src/pycolmap/estimators/affine_transform.cc | 16 ++++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/colmap/estimators/affine_transform.cc b/src/colmap/estimators/affine_transform.cc index 35e28824d4..c96c821674 100644 --- a/src/colmap/estimators/affine_transform.cc +++ b/src/colmap/estimators/affine_transform.cc @@ -102,6 +102,7 @@ bool EstimateAffine2d(const std::vector& src, if (models.empty()) { return false; } + THROW_CHECK_EQ(models.size(), 1); tgt_from_src = models[0]; return true; } diff --git a/src/pycolmap/estimators/affine_transform.cc b/src/pycolmap/estimators/affine_transform.cc index 1334119954..8d98fa51ab 100644 --- a/src/pycolmap/estimators/affine_transform.cc +++ b/src/pycolmap/estimators/affine_transform.cc @@ -17,16 +17,16 @@ using namespace pybind11::literals; namespace py = pybind11; void BindAffineTransformEstimator(py::module& m) { - auto est_options = m.attr("RANSACOptions")().cast(); + auto ransac_options = m.attr("RANSACOptions")().cast(); m.def( "estimate_affine2d", - [](const std::vector& src, - const std::vector& tgt) + [](const std::vector& src, + const std::vector& tgt) -> py::typing::Optional { py::gil_scoped_release release; Eigen::Matrix2x3d tgt_from_src; - const bool success = EstimateSim3d(src, tgt, tgt_from_src); + const bool success = EstimateAffine2d(src, tgt, tgt_from_src); py::gil_scoped_acquire acquire; if (success) { return py::cast(tgt_from_src); @@ -40,20 +40,20 @@ void BindAffineTransformEstimator(py::module& m) { m.def( "estimate_affine2d_robust", - [](const std::vector& src, - const std::vector& tgt, + [](const std::vector& src, + const std::vector& tgt, const RANSACOptions& options) -> py::typing::Optional { py::gil_scoped_release release; Eigen::Matrix2x3d tgt_from_src; const auto report = - EstimateSim3dRobust(src, tgt, options, tgt_from_src); + EstimateAffine2dRobust(src, tgt, options, tgt_from_src); py::gil_scoped_acquire acquire; if (!report.success) { return py::none(); } return py::dict( - "tgt_from_src"_a = Eigen::Matrix2x3d::FromMatrix(report.model), + "tgt_from_src"_a = tgt_from_src, "num_inliers"_a = report.support.num_inliers, "inlier_mask"_a = ToPythonMask(report.inlier_mask)); }, From 9db13e13411106d8263190af89ea59bd775cd0f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 16:54:40 +0100 Subject: [PATCH 3/7] d --- src/pycolmap/estimators/affine_transform.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/pycolmap/estimators/affine_transform.cc b/src/pycolmap/estimators/affine_transform.cc index 8d98fa51ab..88bb460b06 100644 --- a/src/pycolmap/estimators/affine_transform.cc +++ b/src/pycolmap/estimators/affine_transform.cc @@ -52,10 +52,9 @@ void BindAffineTransformEstimator(py::module& m) { if (!report.success) { return py::none(); } - return py::dict( - "tgt_from_src"_a = tgt_from_src, - "num_inliers"_a = report.support.num_inliers, - "inlier_mask"_a = ToPythonMask(report.inlier_mask)); + return py::dict("tgt_from_src"_a = tgt_from_src, + "num_inliers"_a = report.support.num_inliers, + "inlier_mask"_a = ToPythonMask(report.inlier_mask)); }, "src"_a, "tgt"_a, From 11faa9e0e8bfecf574ddd7000a7c37ad388241fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 17:38:59 +0100 Subject: [PATCH 4/7] d --- .../estimators/affine_transform_test.cc | 11 ++- src/colmap/estimators/similarity_transform.h | 17 +++-- .../estimators/similarity_transform_test.cc | 74 ++++++++++++++----- src/colmap/geometry/normalization.cc | 2 +- 4 files changed, 71 insertions(+), 33 deletions(-) diff --git a/src/colmap/estimators/affine_transform_test.cc b/src/colmap/estimators/affine_transform_test.cc index c708f5ebb3..d4169b311b 100644 --- a/src/colmap/estimators/affine_transform_test.cc +++ b/src/colmap/estimators/affine_transform_test.cc @@ -55,8 +55,7 @@ GenerateData(size_t num_inliers, // Add some faulty data. for (size_t i = 0; i < num_outliers; ++i) { - src.emplace_back(RandomUniformReal(-3000.0, -2000.0), - RandomUniformReal(-4000.0, -3000.0)); + src.emplace_back(i, std::sqrt(i) + 2); tgt.emplace_back(RandomUniformReal(-3000.0, -2000.0), RandomUniformReal(-4000.0, -3000.0)); } @@ -83,17 +82,17 @@ TEST(Affine2d, EstimateOverDetermined) { } TEST(Affine2d, EstimateMinimalDegenerate) { - std::vector invalid_src_dst(3, Eigen::Vector2d::Zero()); + std::vector degenerate_src_tgt(3, Eigen::Vector2d::Zero()); Eigen::Matrix2x3d tgt_from_src; EXPECT_FALSE( - EstimateAffine2d(invalid_src_dst, invalid_src_dst, tgt_from_src)); + EstimateAffine2d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src)); } TEST(Affine2d, EstimateNonMinimalDegenerate) { - std::vector invalid_src_dst(5, Eigen::Vector2d::Zero()); + std::vector degenerate_src_tgt(5, Eigen::Vector2d::Zero()); Eigen::Matrix2x3d tgt_from_src; EXPECT_FALSE( - EstimateAffine2d(invalid_src_dst, invalid_src_dst, tgt_from_src)); + EstimateAffine2d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src)); } TEST(Affine2d, EstimateRobust) { diff --git a/src/colmap/estimators/similarity_transform.h b/src/colmap/estimators/similarity_transform.h index 77307421ec..5fc8b249b4 100644 --- a/src/colmap/estimators/similarity_transform.h +++ b/src/colmap/estimators/similarity_transform.h @@ -124,18 +124,23 @@ void SimilarityTransformEstimator::Estimate( const std::vector& tgt, std::vector* models) { THROW_CHECK_EQ(src.size(), tgt.size()); + THROW_CHECK_GE(src.size(), kMinNumSamples); THROW_CHECK(models != nullptr); models->clear(); - Eigen::Matrix src_mat(kDim, src.size()); - Eigen::Matrix dst_mat(kDim, tgt.size()); - for (size_t i = 0; i < src.size(); ++i) { - src_mat.col(i) = src[i]; - dst_mat.col(i) = tgt[i]; + using MatrixType = Eigen::Matrix; + const Eigen::Map src_mat( + reinterpret_cast(src.data()), 2, src.size()); + const Eigen::Map tgt_mat( + reinterpret_cast(tgt.data()), 2, tgt.size()); + + if (Eigen::FullPivLU(src_mat).rank() < kMinNumSamples || + Eigen::FullPivLU(tgt_mat).rank() < kMinNumSamples) { + return; } - const M_t sol = Eigen::umeyama(src_mat, dst_mat, kEstimateScale) + const M_t sol = Eigen::umeyama(src_mat, tgt_mat, kEstimateScale) .template topLeftCorner(); if (sol.hasNaN()) { diff --git a/src/colmap/estimators/similarity_transform_test.cc b/src/colmap/estimators/similarity_transform_test.cc index 1209fcab54..1dbd404673 100644 --- a/src/colmap/estimators/similarity_transform_test.cc +++ b/src/colmap/estimators/similarity_transform_test.cc @@ -55,9 +55,7 @@ GenerateData(size_t num_inliers, // Add some faulty data. for (size_t i = 0; i < num_outliers; ++i) { - src.emplace_back(RandomUniformReal(-3000.0, -2000.0), - RandomUniformReal(-4000.0, -3000.0), - RandomUniformReal(-5000.0, -4000.0)); + src.emplace_back(i, std::sqrt(i) + 2, std::sqrt(2 * i + 2)); tgt.emplace_back(RandomUniformReal(-3000.0, -2000.0), RandomUniformReal(-4000.0, -3000.0), RandomUniformReal(-5000.0, -4000.0)); @@ -66,38 +64,38 @@ GenerateData(size_t num_inliers, return {std::move(src), std::move(tgt)}; } -void TestEstimateSim3dWithNumCoords(const size_t num_coords) { - const Sim3d gt_tgt_from_src(RandomUniformReal(0.1, 10), - Eigen::Quaterniond::UnitRandom(), - Eigen::Vector3d::Random()); +void TestEstimateRigid3dWithNumCoords(const size_t num_coords) { + const Rigid3d gt_tgt_from_src(Eigen::Quaterniond::UnitRandom(), + Eigen::Vector3d::Random()); const auto [src, tgt] = GenerateData( /*num_inliers=*/num_coords, /*num_outliers=*/0, gt_tgt_from_src.ToMatrix()); - Sim3d tgt_from_src; - EXPECT_TRUE(EstimateSim3d(src, tgt, tgt_from_src)); - EXPECT_NEAR(gt_tgt_from_src.scale, tgt_from_src.scale, 1e-6); + Rigid3d tgt_from_src; + EXPECT_TRUE(EstimateRigid3d(src, tgt, tgt_from_src)); EXPECT_LT(gt_tgt_from_src.rotation.angularDistance(tgt_from_src.rotation), 1e-6); EXPECT_LT((gt_tgt_from_src.translation - tgt_from_src.translation).norm(), 1e-6); } -TEST(Sim3d, EstimateMinimal) { TestEstimateSim3dWithNumCoords(3); } +TEST(Rigid3d, EstimateMinimal) { TestEstimateRigid3dWithNumCoords(3); } -TEST(Sim3d, EstimateOverDetermined) { TestEstimateSim3dWithNumCoords(100); } +TEST(Rigid3d, EstimateOverDetermined) { TestEstimateRigid3dWithNumCoords(100); } -TEST(Sim3d, EstimateMinimalDegenerate) { - std::vector invalid_src_dst(3, Eigen::Vector3d::Zero()); - Sim3d tgt_from_src; - EXPECT_FALSE(EstimateSim3d(invalid_src_dst, invalid_src_dst, tgt_from_src)); +TEST(Rigid3d, EstimateMinimalDegenerate) { + std::vector degenerate_src_tgt(3, Eigen::Vector3d::Zero()); + Rigid3d tgt_from_src; + EXPECT_FALSE( + EstimateRigid3d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src)); } -TEST(Sim3d, EstimateNonMinimalDegenerate) { - std::vector invalid_src_dst(5, Eigen::Vector3d::Zero()); - Sim3d tgt_from_src; - EXPECT_FALSE(EstimateSim3d(invalid_src_dst, invalid_src_dst, tgt_from_src)); +TEST(Rigid3d, EstimateNonMinimalDegenerate) { + std::vector degenerate_src_tgt(5, Eigen::Vector3d::Zero()); + Rigid3d tgt_from_src; + EXPECT_FALSE( + EstimateRigid3d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src)); } TEST(Rigid3d, EstimateRobust) { @@ -136,6 +134,42 @@ TEST(Rigid3d, EstimateRobust) { EigenMatrixNear(gt_tgt_from_src.ToMatrix(), 1e-6)); } +void TestEstimateSim3dWithNumCoords(const size_t num_coords) { + const Sim3d gt_tgt_from_src(RandomUniformReal(0.1, 10), + Eigen::Quaterniond::UnitRandom(), + Eigen::Vector3d::Random()); + const auto [src, tgt] = GenerateData( + /*num_inliers=*/num_coords, + /*num_outliers=*/0, + gt_tgt_from_src.ToMatrix()); + + Sim3d tgt_from_src; + EXPECT_TRUE(EstimateSim3d(src, tgt, tgt_from_src)); + EXPECT_NEAR(gt_tgt_from_src.scale, tgt_from_src.scale, 1e-6); + EXPECT_LT(gt_tgt_from_src.rotation.angularDistance(tgt_from_src.rotation), + 1e-6); + EXPECT_LT((gt_tgt_from_src.translation - tgt_from_src.translation).norm(), + 1e-6); +} + +TEST(Sim3d, EstimateMinimal) { TestEstimateSim3dWithNumCoords(3); } + +TEST(Sim3d, EstimateOverDetermined) { TestEstimateSim3dWithNumCoords(100); } + +TEST(Sim3d, EstimateMinimalDegenerate) { + std::vector degenerate_src_tgt(3, Eigen::Vector3d::Zero()); + Sim3d tgt_from_src; + EXPECT_FALSE( + EstimateSim3d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src)); +} + +TEST(Sim3d, EstimateNonMinimalDegenerate) { + std::vector degenerate_src_tgt(5, Eigen::Vector3d::Zero()); + Sim3d tgt_from_src; + EXPECT_FALSE( + EstimateSim3d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src)); +} + TEST(Sim3d, EstimateRobust) { SetPRNGSeed(0); diff --git a/src/colmap/geometry/normalization.cc b/src/colmap/geometry/normalization.cc index db198ce6c0..bca67d73a6 100644 --- a/src/colmap/geometry/normalization.cc +++ b/src/colmap/geometry/normalization.cc @@ -79,7 +79,7 @@ std::pair ComputeBoundingBoxAndCentroid( Eigen::Vector3d centroid(0, 0, 0); const double normalization = 1.0 / (max_idx - min_idx + 1); - for (int i = min_idx; i <= max_idx; ++i) { + for (size_t i = min_idx; i <= max_idx; ++i) { centroid(0) += normalization * coords_x[i]; centroid(1) += normalization * coords_y[i]; centroid(2) += normalization * coords_z[i]; From 0fb95c64516c827917e7643db634b96487bf0e66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 17:55:42 +0100 Subject: [PATCH 5/7] d --- src/colmap/estimators/similarity_transform.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/colmap/estimators/similarity_transform.h b/src/colmap/estimators/similarity_transform.h index 5fc8b249b4..b098ec8436 100644 --- a/src/colmap/estimators/similarity_transform.h +++ b/src/colmap/estimators/similarity_transform.h @@ -130,9 +130,9 @@ void SimilarityTransformEstimator::Estimate( models->clear(); using MatrixType = Eigen::Matrix; - const Eigen::Map src_mat( + const Eigen::Map src_mat( reinterpret_cast(src.data()), 2, src.size()); - const Eigen::Map tgt_mat( + const Eigen::Map tgt_mat( reinterpret_cast(tgt.data()), 2, tgt.size()); if (Eigen::FullPivLU(src_mat).rank() < kMinNumSamples || From f264b37aa7b1e64412923ef2485fac5e6af0ba03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 20:18:55 +0100 Subject: [PATCH 6/7] d --- src/colmap/estimators/similarity_transform.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/colmap/estimators/similarity_transform.h b/src/colmap/estimators/similarity_transform.h index b098ec8436..7ea1dfb55e 100644 --- a/src/colmap/estimators/similarity_transform.h +++ b/src/colmap/estimators/similarity_transform.h @@ -131,9 +131,9 @@ void SimilarityTransformEstimator::Estimate( using MatrixType = Eigen::Matrix; const Eigen::Map src_mat( - reinterpret_cast(src.data()), 2, src.size()); + reinterpret_cast(src.data()), kDim, src.size()); const Eigen::Map tgt_mat( - reinterpret_cast(tgt.data()), 2, tgt.size()); + reinterpret_cast(tgt.data()), kDim, tgt.size()); if (Eigen::FullPivLU(src_mat).rank() < kMinNumSamples || Eigen::FullPivLU(tgt_mat).rank() < kMinNumSamples) { From 68caf9f45b2c4854cc833c98695fd57416cc241d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 17 Dec 2024 20:19:07 +0100 Subject: [PATCH 7/7] d --- src/colmap/estimators/similarity_transform.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/colmap/estimators/similarity_transform.h b/src/colmap/estimators/similarity_transform.h index 7ea1dfb55e..0c8ef86438 100644 --- a/src/colmap/estimators/similarity_transform.h +++ b/src/colmap/estimators/similarity_transform.h @@ -130,9 +130,9 @@ void SimilarityTransformEstimator::Estimate( models->clear(); using MatrixType = Eigen::Matrix; - const Eigen::Map src_mat( + const Eigen::Map src_mat( reinterpret_cast(src.data()), kDim, src.size()); - const Eigen::Map tgt_mat( + const Eigen::Map tgt_mat( reinterpret_cast(tgt.data()), kDim, tgt.size()); if (Eigen::FullPivLU(src_mat).rank() < kMinNumSamples ||