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

Faster 8-point fundamental matrix estimator #2613

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 3 commits into from
Jun 25, 2024
Merged

Faster 8-point fundamental matrix estimator #2613

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f3c02bd
Faster 8-point fundamental matrix estimator
ahojnnes Jun 24, 2024
21f137d
d
ahojnnes Jun 24, 2024
d7b11e1
d
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
39 changes: 23 additions & 16 deletions src/colmap/estimators/fundamental_matrix.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,7 @@ void FundamentalMatrixEightPointEstimator::Estimate(
const std::vector<Y_t>& points2,
std::vector<M_t>* models) {
THROW_CHECK_EQ(points1.size(), points2.size());
THROW_CHECK_GE(points1.size(), 8);
THROW_CHECK(models != nullptr);

models->clear();
Expand All @@ -139,30 +140,36 @@ void FundamentalMatrixEightPointEstimator::Estimate(
CenterAndNormalizeImagePoints(points2, &normed_points2, &normed_from_orig2);

// Setup homogeneous linear equation as x2' * F * x1 = 0.
Eigen::Matrix<double, Eigen::Dynamic, 9> cmatrix(points1.size(), 9);
Eigen::Matrix<double, Eigen::Dynamic, 9> A(points1.size(), 9);
for (size_t i = 0; i < points1.size(); ++i) {
cmatrix.block<1, 3>(i, 0) = normed_points1[i].homogeneous();
cmatrix.block<1, 3>(i, 0) *= normed_points2[i].x();
cmatrix.block<1, 3>(i, 3) = normed_points1[i].homogeneous();
cmatrix.block<1, 3>(i, 3) *= normed_points2[i].y();
cmatrix.block<1, 3>(i, 6) = normed_points1[i].homogeneous();
A.row(i) << normed_points2[i].x() *
normed_points1[i].transpose().homogeneous(),
normed_points2[i].y() * normed_points1[i].transpose().homogeneous(),
normed_points1[i].transpose().homogeneous();
}

// Solve for the nullspace of the constraint matrix.
Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> cmatrix_svd(
cmatrix, Eigen::ComputeFullV);
const Eigen::VectorXd cmatrix_nullspace = cmatrix_svd.matrixV().col(8);
const Eigen::Map<const Eigen::Matrix3d> ematrix_t(cmatrix_nullspace.data());
Eigen: 10000 :Matrix3d E;
if (points1.size() == 8) {
Eigen::Matrix<double, 9, 9> Q =
A.transpose().householderQr().householderQ();
E = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
Q.col(8).data());
} else {
Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd(
A, Eigen::ComputeFullV);
E = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
svd.matrixV().col(8).data());
}

// Enforcing the internal constraint that two singular values must non-zero
// and one must be zero.
Eigen::JacobiSVD<Eigen::Matrix3d> fmatrix_svd(
ematrix_t.transpose(), Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector3d singular_values = fmatrix_svd.singularValues();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(
E, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector3d singular_values = svd.singularValues();
singular_values(2) = 0.0;
const Eigen::Matrix3d F = fmatrix_svd.matrixU() *
singular_values.asDiagonal() *
fmatrix_svd.matrixV().transpose();
const Eigen::Matrix3d F =
svd.matrixU() * singular_values.asDiagonal() * svd.matrixV().transpose();

models->resize(1);
(*models)[0] = normed_from_orig2.transpose() * F * normed_from_orig1;
Expand Down
110 changes: 89 additions & 21 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 @@ -64,16 +64,31 @@ void RandomEpipolarCorrespondences(const Rigid3d& cam2_from_cam1,
}
}

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;
template <typename Estimator>
void ExpectAtLeastOneValidModel(const Estimator& estimator,
const std::vector<Eigen::Vector2d>& points1,
const std::vector<Eigen::Vector2d>& points2,
Eigen::Matrix3d& expected_F,
std::vector<Eigen::Matrix3d>& models,
double F_eps = 1e-6,
double r_eps = 1e-6) {
expected_F /= expected_F(2, 2);
for (size_t i = 0; i < models.size(); ++i) {
Eigen::Matrix3d F = models[i];
F /= F(2, 2);
if (!F.isApprox(expected_F, F_eps)) {
continue;
}

std::vector<double> residuals;
estimator.Residuals(points1, points2, F, &residuals);
for (size_t j = 0; j < points1.size(); ++j) {
EXPECT_LT(residuals[j], r_eps);
}

return;
}
FAIL() << "No fundamental matrix is equal up to scale.";
ADD_FAILURE() << "No fundamental matrix is equal up to scale.";
}

TEST(FundamentalSevenPointEstimator, Reference) {
Expand Down Expand Up @@ -152,7 +167,7 @@ TEST(FundamentalSevenPointEstimator, Nominal) {
std::vector<Eigen::Matrix3d> models;
estimator.Estimate(points1, points2, &models);

ExpectAtLeastOneEqualFundamentalUpToScale(expected_F, models);
ExpectAtLeastOneValidModel(estimator, points1, points2, expected_F, models);
}
}

Expand Down Expand Up @@ -206,18 +221,18 @@ TEST(FundamentalMatrixEightPointEstimator, Reference) {
estimator.Estimate(points1, points2, &models);

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

// Reference values obtained from Matlab.
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);
EXPECT_NEAR(F(0, 0), -9.85701, 1e-5);
EXPECT_NEAR(F(0, 1), 18.97038, 1e-5);
EXPECT_NEAR(F(0, 2), -1.55224, 1e-5);
EXPECT_NEAR(F(1, 0), -3.24832, 1e-5);
EXPECT_NEAR(F(1, 1), 2.04346, 1e-5);
EXPECT_NEAR(F(1, 2), 0.977619, 1e-5);
EXPECT_NEAR(F(2, 0), 11.22355, 1e-5);
EXPECT_NEAR(F(2, 1), -19.43171, 1e-5);
EXPECT_NEAR(F(2, 2), 1, 1e-5);
Copy link
Contributor Author

Choose a reason for hiding this comment

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

Same values, just rescaled.

}

class FundamentalMatrixEightPointEstimatorTests
Expand All @@ -240,13 +255,66 @@ TEST_P(FundamentalMatrixEightPointEstimatorTests, Nominal) {
std::vector<Eigen::Matrix3d> models;
estimator.Estimate(points1, points2, &models);

ExpectAtLeastOneEqualFundamentalUpToScale(expected_F, models);
ExpectAtLeastOneValidModel(estimator, points1, points2, expected_F, models);
}
}

TEST_P(FundamentalMatrixEightPointEstimatorTests, NumericalStability) {
const size_t kNumPoints = GetParam();
constexpr double kCoordinateScale = 1e3;
for (size_t k = 0; k < 100; ++k) {
Eigen::Matrix3d K = RandomCalibrationMatrix();
K(0, 0) *= kCoordinateScale;
K(1, 1) *= kCoordinateScale;
K(0, 2) *= kCoordinateScale;
K(1, 2) *= kCoordinateScale;
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);

ExpectAtLeastOneValidModel(
estimator, points1, points2, expected_F, models, 1e-4, 1e-4);
}
}

TEST_P(FundamentalMatrixEightPointEstimatorTests, NoiseStability) {
const size_t kNumPoints = GetParam();
constexpr double kNoise = 1e-4;
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);
for (size_t i = 0; i < kNumPoints; ++i) {
points2[i] += Eigen::Vector2d::Random() * kNoise;
}

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

ExpectAtLeastOneValidModel(
estimator, points1, points2, expected_F, models, 1e-3, 1e-2);
}
}

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

} // namespace
} // namespace colmap
Loading
0