8000 Faster 7-point fundamental matrix estimator by ahojnnes · Pull Request #2612 · colmap/colmap · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Faster 7-point fundamental matrix estimator #2612

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 6 commits into from
Jun 24, 2024
Merged

Faster 7-point fundamental matrix estimator #2612

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3120187
Faster 7-point fundamental matrix estimator
ahojnnes Jun 23, 2024
e7e2fb6
tests
ahojnnes Jun 23, 2024
ca9bf97
d
ahojnnes Jun 24, 2024
93d859e
d
ahojnnes Jun 24, 2024
fd48dfe
d
ahojnnes Jun 24, 2024
9a3e850
msvc
ahojnnes Jun 24, 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
69 changes: 20 additions & 49 deletions src/colmap/estimators/fundamental_matrix.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,37 +54,23 @@ void FundamentalMatrixSevenPointEstimator::Estimate(

models->clear();

// Note that no normalization of the points is necessary here.

// Setup system of equations: [points2(i,:), 1]' * F * [points1(i,:), 1]'.
Eigen::Matrix<double, 7, 9> A;
Eigen::Matrix<double, 9, 7> A;
for (size_t i = 0; i < 7; ++i) {
const double x0 = points1[i](0);
const double y0 = points1[i](1);
const double x1 = points2[i](0);
const double y1 = points2[i](1);
A(i, 0) = x1 * x0;
A(i, 1) = x1 * y0;
A(i, 2) = x1;
A(i, 3) = y1 * x0;
A(i, 4) = y1 * y0;
A(i, 5) = y1;
A(i, 6) = x0;
A(i, 7) = y0;
A(i, 8) = 1;
A.col(i) << points1[i].x() * points2[i].homogeneous(),
points1[i].y() * points2[i].homogeneous(), points2[i].homogeneous();
}

// 9 unknowns with 7 equations, so we have 2D null space.
Eigen::JacobiSVD<Eigen::Matrix<double, 7, 9>> svd(A, Eigen::ComputeFullV);
const Eigen::Matrix<double, 9, 9>& f = svd.matrixV();
Eigen::Matrix<double, 1, 9> f1 = f.col(7);
Eigen::Matrix<double, 1, 9> f2 = f.col(8);

f1 -= f2;
Eigen::Matrix<double, 9, 9> Q = A.fullPivHouseholderQr().matrixQ();

// Normalize, such that lambda + mu = 1
// and add constraint det(F) = det(lambda * f1 + (1 - lambda) * f2).

auto f1 = Q.col(7);
auto f2 = Q.col(8);
f1 -= f2;

const double t0 = f1(4) * f1(8) - f1(5) * f1(7);
const double t1 = f1(3) * f1(8) - f1(5) * f1(6);
const double t2 = f1(3) * f1(7) - f1(4) * f1(6);
Expand All @@ -94,6 +80,10 @@ void FundamentalMatrixSevenPointEstimator::Estimate(

Eigen::Vector4d coeffs;
coeffs(0) = f1(0) * t0 - f1(1) * t1 + f1(2) * t2;
if (std::abs(coeffs(0)) < 1e-16) {
return;
}

coeffs(1) = f2(0) * t0 - f2(1) * t1 + f2(2) * t2 -
f2(3) * (f1(1) * f1(8) - f1(2) * f1(7)) +
f2(4) * (f1(0) * f1(8) - f1(2) * f1(6)) -
Expand All @@ -110,35 +100,16 @@ void FundamentalMatrixSevenPointEstimator::Estimate(
f1(8) * (f2(0) * f2(4) - f2(1) * f2(3));
coeffs(3) = f2(0) * t3 - f2(1) * t4 + f2(2) * t5;

Eigen::VectorXd roots_real;
Eigen::VectorXd roots_imag;
if (!FindPolynomialRootsCompanionMatrix(coeffs, &roots_real, &roots_imag)) {
return;
}

models->reserve(roots_real.size());

for (Eigen::VectorXd::Index i = 0; i < roots_real.size(); ++i) {
const double kMaxRootImag = 1e-10;
if (std::abs(roots_imag(i)) > kMaxRootImag) {
continue;
}

const double lambda = roots_real(i);
const double mu = 1;

Eigen::MatrixXd F = lambda * f1 + mu * f2;

F.resize(3, 3);

const double kEps = 1e-10;
if (std::abs(F(2, 2)) < kEps) {
continue;
}
coeffs.tail<3>() /= coeffs(0);

F /= F(2, 2);
Eigen::Vector3d roots;
const int num_roots =
FindCubicPolynomialRoots(coeffs(1), coeffs(2), coeffs(3), &roots);

models->push_back(F.transpose());
models->reserve(num_roots);
for (int i = 0; i < num_roots; ++i) {
const Eigen::Matrix<double, 9, 1> F = (f1 * roots[i] + f2).normalized();
models->push_back(Eigen::Map<const Eigen::Matrix3d>(F.data()));
}
}

Expand Down
115 changes: 103 additions & 12 deletions src/colmap/estimators/fundamental_matrix_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,12 +29,54 @@

#include "colmap/estimators/fundamental_matrix.h"

#include "colmap/geometry/essential_matrix.h"
#include "colmap/math/random.h"

#include <gtest/gtest.h>

namespace colmap {
namespace {

TEST(FundamentalMatrix, SevenPoint) {
Eigen::Matrix3d RandomCalibrationMatrix() {
return (Eigen::Matrix3d() << RandomUniformReal<double>(800, 1200),
0,
RandomUniformReal<double>(400, 600),
0,
RandomUniformReal<double>(800, 1200),
RandomUniformReal<double>(400, 600),
0,
0,
1)
.finished();
}

void RandomEpipolarCorrespondences(const Rigid3d& cam2_from_cam1,
const Eigen::Matrix3d& K,
size_t num_points,
std::vector<Eigen::Vector2d>& points1,
std::vector<Eigen::Vector2d>& points2) {
for (size_t i = 0; i < num_points; ++i) {
points1.push_back(K.topRows<2>() * Eigen::Vector2d::Random().homogeneous());
const double random_depth = RandomUniformReal<double>(0.2, 2.0);
points2.push_back((K * (cam2_from_cam1 * (random_depth * K.inverse() *
points1.back().homogeneous())))
.hnormalized());
}
}

void ExpectAtLeastOneEqualFundamentalUpToScale(
Eigen::Matrix3d& F, std::vector<Eigen::Matrix3d>& Fs) {
F /= F(2, 2);
for (Eigen::Matrix3d& F_candidate : Fs) {
F_candidate /= F_candidate(2, 2);
if (F_candidate.isApprox(F, 1e-6)) {
return;
}
}
FAIL() << "No fundamental matrix is equal up to scale.";
}

TEST(FundamentalSevenPointEstimator, Reference) {
const double points1_raw[] = {0.4964,
1.0577,
0.3650,
Expand Down Expand Up @@ -79,7 +121,7 @@ TEST(FundamentalMatrix, SevenPoint) {
estimator.Estimate(points1, points2, &models);

ASSERT_EQ(models.size(), 1);
const auto& F = models[0];
const Eigen::Matrix3d F = models[0] / models[0](2, 2);

// Reference values obtained from Matlab.
EXPECT_NEAR(F(0, 0), 4.81441976, 1e-6);
Expand All @@ -93,7 +135,28 @@ TEST(FundamentalMatrix, SevenPoint) {
EXPECT_NEAR(F(2, 2), 1., 1e-6);
}

TEST(FundamentalMatrix, EightPoint) {
TEST(FundamentalSevenPointEstimator, Nominal) {
const size_t kNumPoints = 7;
for (size_t k = 0; k < 100; ++k) {
const Eigen::Matrix3d K = RandomCalibrationMatrix();
const Rigid3d cam2_from_cam1(Eigen::Quaterniond::UnitRandom(),
Eigen::Vector3d::Random());
Eigen::Matrix3d expected_F = FundamentalFromEssentialMatrix(
K, EssentialMatrixFromPose(cam2_from_cam1), K);
std::vector<Eigen::Vector2d> points1;
std::vector<Eigen::Vector2d> points2;
RandomEpipolarCorrespondences(
cam2_from_cam1, K, kNumPoints, points1, points2);

FundamentalMatrixSevenPointEstimator estimator;
std::vector<Eigen::Matrix3d> models;
estimator.Estimate(points1, points2, &models);

ExpectAtLeastOneEqualFundamentalUpToScale(expected_F, models);
}
}

TEST(FundamentalMatrixEightPointEstimator, Reference) {
const double points1_raw[] = {1.839035,
1.924743,
0.543582,
Expand Down Expand Up @@ -146,16 +209,44 @@ TEST(FundamentalMatrix, EightPoint) {
const auto& F = models[0];

// Reference values obtained from Matlab.
EXPECT_TRUE(std::abs(F(0, 0) - -0.217859) < 1e-5);
EXPECT_TRUE(std::abs(F(0, 1) - 0.419282) < 1e-5);
EXPECT_TRUE(std::abs(F(0, 2) - -0.0343075) < 1e-5);
EXPECT_TRUE(std::abs(F(1, 0) - -0.0717941) < 1e-5);
EXPECT_TRUE(std::abs(F(1, 1) - 0.0451643) < 1e-5);
EXPECT_TRUE(std::abs(F(1, 2) - 0.0216073) < 1e-5);
EXPECT_TRUE(std::abs(F(2, 0) - 0.248062) < 1e-5);
EXPECT_TRUE(std::abs(F(2, 1) - -0.429478) < 1e-5);
EXPECT_TRUE(std::abs(F(2, 2) - 0.0221019) < 1e-5);
EXPECT_NEAR(F(0, 0), -0.217859, 1e-5);
EXPECT_NEAR(F(0, 1), 0.419282, 1e-5);
EXPECT_NEAR(F(0, 2), -0.0343075, 1e-5);
EXPECT_NEAR(F(1, 0), -0.0717941, 1e-5);
EXPECT_NEAR(F(1, 1), 0.0451643, 1e-5);
EXPECT_NEAR(F(1, 2), 0.0216073, 1e-5);
EXPECT_NEAR(F(2, 0), 0.248062, 1e-5);
EXPECT_NEAR(F(2, 1), -0.429478, 1e-5);
EXPECT_NEAR(F(2, 2), 0.0221019, 1e-5);
}

class FundamentalMatrixEightPointEstimatorTests
: public ::testing::TestWithParam<size_t> {};

TEST_P(FundamentalMatrixEightPointEstimatorTests, Nominal) {
const size_t kNumPoints = GetParam();
for (size_t k = 0; k < 100; ++k) {
const Eigen::Matrix3d K = RandomCalibrationMatrix();
const Rigid3d cam2_from_cam1(Eigen::Quaterniond::UnitRandom(),
Eigen::Vector3d::Random());
Eigen::Matrix3d expected_F = FundamentalFromEssentialMatrix(
K, EssentialMatrixFromPose(cam2_from_cam1), K);
std::vector<Eigen::Vector2d> points1;
std::vector<Eigen::Vector2d> points2;
RandomEpipolarCorrespondences(
cam2_from_cam1, K, kNumPoints, points1, points2);

FundamentalMatrixEightPointEstimator estimator;
std::vector<Eigen::Matrix3d> models;
estimator.Estimate(points1, points2, &models);

ExpectAtLeastOneEqualFundamentalUpToScale(expected_F, models);
}
}

INSTANTIATE_TEST_SUITE_P(FundamentalMatrixEightPointEstimator,
FundamentalMatrixEightPointEstimatorTests,
::testing::Values(8, 16, 32));

} // namespace
} // namespace colmap
Loading
0