8000 Consistent interface/tests for rigid3d/sim3d/affine2d, pycolmap bindings for rigid3d by ahojnnes · Pull Request #3051 · colmap/colmap · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Consistent interface/tests for rigid3d/sim3d/affine2d, pycolmap bindings for rigid3d #3051

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 8 commits into from
Dec 17, 2024
Merged

Consistent interface/tests for rigid3d/sim3d/affine2d, pycolmap bindings for rigid3d #3051

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
e9ee86a
Consistent interface/tests for rigid3d/sim3d/affine2d, pycolmap bindi…
ahojnnes Dec 17, 2024
cfd89a1
d
ahojnnes Dec 17, 2024
9db13e1
d
ahojnnes Dec 17, 2024
11faa9e
d
ahojnnes Dec 17, 2024
0fb95c6
d
ahojnnes Dec 17, 2024
a5b72e7
Merge branch 'main' into user/jsch/rigid-sim-affine
ahojnnes Dec 17, 2024
f264b37
d
ahojnnes Dec 17, 2024
68caf9f
d
ahojnnes Dec 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/colmap/estimators/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
88 changes: 47 additions & 41 deletions src/colmap/estimators/affine_transform.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -37,27 +38,27 @@

namespace colmap {

void AffineTransformEstimator::Estimate(const std::vector<X_t>& points1,
const std::vector<Y_t>& points2,
std::vector<M_t>* models) {
const size_t num_points = points1.size();
THROW_CHECK_EQ(num_points, points2.size());
void AffineTransformEstimator::Estimate(const std::vector<X_t>& src,
const std::vector<Y_t>& tgt,
std::vector<M_t>* 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<double, Eigen::Dynamic, 6> 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;
Expand All @@ -75,43 +76,48 @@ void AffineTransformEstimator::Estimate(const std::vector<X_t>& points1,
sol = svd.solve(b);
}

models->resize(1);
(*models)[0] =
tgt_from_src->resize(1);
(*tgt_from_src)[0] =
Eigen::Map<const Eigen::Matrix<double, 3, 2>>(sol.data()).transpose();
}

void AffineTransformEstimator::Residuals(const std::vector<X_t>& points1,
const std::vector<Y_t>& points2,
const M_t& A,
void AffineTransformEstimator::Residuals(const std::vector<X_t>& src,
const std::vector<Y_t>& tgt,
const M_t& tgt_from_src,
std::vector<double>* residuals) {
THROW_CHECK_EQ(points1.size(), points2.size());

residuals->resize(points1.size());

// Note that this code might not be as nice as Eigen expressions,
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is not true anymore in recent profiling.

// 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<Eigen::Vector2d>& src,
const std::vector<Eigen::Vector2d>& tgt,
Eigen::Matrix2x3d& tgt_from_src) {
std::vector<Eigen::Matrix2x3d> models;
AffineTransformEstimator::Estimate(src, tgt, &models);
if (models.empty()) {
return false;
}
THROW_CHECK_EQ(models.size(), 1);
tgt_from_src = models[0];
return true;
}

(*residuals)[i] = dd_0 * dd_0 + dd_1 * dd_1;
typename RANSAC<AffineTransformEstimator>::Report EstimateAffine2dRobust(
const std::vector<Eigen::Vector2d>& src,
const std::vector<Eigen::Vector2d>& tgt,
const RANSACOptions& options,
Eigen::Matrix2x3d& tgt_from_src) {
LORANSAC<AffineTransformEstimator, AffineTransformEstimator> ransac(options);
auto report = ransac.Estimate(src, tgt);
if (report.success) {
tgt_from_src = report.model;
}
return report;
}

} // namespace colmap
25 changes: 18 additions & 7 deletions src/colmap/estimators/affine_transform.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#pragma once

#include "colmap/optim/ransac.h"
#include "colmap/util/eigen_alignment.h"
#include "colmap/util/types.h"

Expand All @@ -42,21 +43,31 @@ class AffineTransformEstimator {
public:
typedef Eigen::Vector2d X_t;
typedef Eigen::Vector2d Y_t;
typedef Eigen::Matrix<double, 2, 3> 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<X_t>& points1,
const std::vector<Y_t>& points2,
std::vector<M_t>* models);
static void Estimate(const std::vector<X_t>& src,
const std::vector<Y_t>& tgt,
std::vector<M_t>* tgt_from_src);

// Compute the squared transformation error.
static void Residuals(const std::vector<X_t>& points1,
const std::vector<Y_t>& points2,
const M_t& E,
static void Residuals(const std::vector<X_t>& src,
const std::vector<Y_t>& tgt,
const M_t& tgt_from_src,
std::vector<double>* residuals);
};

bool EstimateAffine2d(const std::vector<Eigen::Vector2d>& src,
const std::vector<Eigen::Vector2d>& tgt,
Eigen::Matrix2x3d& tgt_from_src);

typename RANSAC<AffineTransformEstimator>::Report EstimateAffine2dRobust(
const std::vector<Eigen::Vector2d>& src,
const std::vector<Eigen::Vector2d>& tgt,
const RANSACOptions& options,
Eigen::Matrix2x3d& tgt_from_src);

} // namespace colmap
172 changes: 75 additions & 97 deletions src/colmap/estimators/affine_transform_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <Eigen/Geometry>
Expand All @@ -38,117 +40,93 @@
namespace colmap {
namespace {

TEST(AffineTransform, Minimal) {
for (int x = 0; x < 10; ++x) {
Eigen::Matrix<double, 2, 3> A;
A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1;

std::vector<Eigen::Vector2d> src;
src.emplace_back(x, 10);
src.emplace_back(1, 0);
src.emplace_back(2, 1);

std::vector<Eigen::Vector2d> dst;
for (size_t i = 0; i < src.size(); ++i) {
dst.push_back(A * src[i].homogeneous());
}

AffineTransformEstimator estimator;
std::vector<Eigen::Matrix<double, 2, 3>> models;
estimator.Estimate(src, dst, &models);

ASSERT_EQ(models.size(), 1);

std::vector<double> 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<Eigen::Vector2d>, std::vector<Eigen::Vector2d>>
GenerateData(size_t num_inliers,
size_t num_outliers,
const Eigen::Matrix2x3d& tgt_from_src) {
std::vector<Eigen::Vector2d> src;
std::vector<Eigen::Vector2d> 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<double, 2, 3> A;
A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1;

std::vector<Eigen::Vector2d> 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<Eigen::Vector2d> dst;
for (size_t i = 0; i < src.size(); ++i) {
dst.push_back(A * src[i].homogeneous());
}

AffineTransformEstimator estimator;
std::vector<Eigen::Matrix<double, 2, 3>> models;
estimator.Estimate(src, dst, &models);

ASSERT_EQ(models.size(), 1);

std::vector<double> 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(i, std::sqrt(i) + 2);
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<double, 2, 3> A;
A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1;
return {std::move(src), std::move(tgt)};
}

std::vector<Eigen::Vector2d> 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<Eigen::Vector2d> 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<Eigen::Matrix<double, 2, 3>> 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<double, 2, 3> A;
A << x / 10.0, 0.2, 0.3, 30, 0.2, 0.1;
TEST(Affine2d, EstimateMinimalDegenerate) {
std::vector<Eigen::Vector2d> degenerate_src_tgt(3, Eigen::Vector2d::Zero());
Eigen::Matrix2x3d tgt_from_src;
EXPECT_FALSE(
EstimateAffine2d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src));
}

std::vector<Eigen::Vector2d> 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<Eigen::Vector2d> degenerate_src_tgt(5, Eigen::Vector2d::Zero());
Eigen::Matrix2x3d tgt_from_src;
EXPECT_FALSE(
EstimateAffine2d(degenerate_src_tgt, degenerate_src_tgt, tgt_from_src));
}

std::vector<Eigen::Vector2d> 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<Eigen::Matrix<double, 2, 3>> models;
estimator.Estimate(src, dst, &models);

ASSERT_TRUE(models.empty());
}

EXPECT_THAT(tgt_from_src, EigenMatrixNear(gt_tgt_from_src, 1e-6));
}

} // namespace
Expand Down
Loading
Loading
0