Skip to content

Commit

Permalink
Return success status in low-level triangulation functions (colmap#2834)
Browse files Browse the repository at this point in the history 
More robustly handle degenerate configurations. Noticed a rare edge case
during some tests. Should barely make a difference in practice but
avoids issues when using the low-level functions in new places.
  • Loading branch information
@ahojnnes
ahojnnes authored Oct 16, 2024
1 parent 1f50839 commit 189478b
Show file tree
Hide file tree
Showing 7 changed files with 120 additions and 114 deletions.
18 changes: 11 additions & 7 deletions src/colmap/estimators/triangulation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,12 +63,13 @@ void TriangulationEstimator::Estimate(const std::vector<X_t>& point_data,
if (point_data.size() == 2) {
// Two-view triangulation.

const M_t xyz = TriangulatePoint(pose_data[0].cam_from_world,
pose_data[1].cam_from_world,
point_data[0].point_normalized,
point_data[1].point_normalized);

if (HasPointPositiveDepth(pose_data[0].cam_from_world, xyz) &&
M_t xyz;
if (TriangulatePoint(pose_data[0].cam_from_world,
pose_data[1].cam_from_world,
point_data[0].point_normalized,
point_data[1].point_normalized,
&xyz) &&
HasPointPositiveDepth(pose_data[0].cam_from_world, xyz) &&
HasPointPositiveDepth(pose_data[1].cam_from_world, xyz) &&
CalculateTriangulationAngle(pose_data[0].proj_center,
pose_data[1].proj_center,
Expand All @@ -89,7 +90,10 @@ void TriangulationEstimator::Estimate(const std::vector<X_t>& point_data,
points.push_back(point_data[i].point_normalized);
}

const M_t xyz = TriangulateMultiViewPoint(proj_matrices, points);
M_t xyz;
if (!TriangulateMultiViewPoint(proj_matrices, points, &xyz)) {
return;
}

// Check for cheirality constraint.
for (const auto& pose : pose_data) {
Expand Down
16 changes: 9 additions & 7 deletions src/colmap/geometry/essential_matrix.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,21 +102,21 @@ void FindOptimalImageObservations(const Eigen::Matrix3d& E,
const Eigen::Vector2d& point2,
Eigen::Vector2d* optimal_point1,
Eigen::Vector2d* optimal_point2) {
const Eigen::Vector3d& point1h = point1.homogeneous();
const Eigen::Vector3d& point2h = point2.homogeneous();
const Eigen::Vector3d& point1_homogeneous = point1.homogeneous();
const Eigen::Vector3d& point2_homogeneous = point2.homogeneous();

Eigen::Matrix<double, 2, 3> S;
S << 1, 0, 0, 0, 1, 0;

// Epipolar lines.
Eigen::Vector2d n1 = S * E * point2h;
Eigen::Vector2d n2 = S * E.transpose() * point1h;
Eigen::Vector2d n1 = S * E * point2_homogeneous;
Eigen::Vector2d n2 = S * E.transpose() * point1_homogeneous;

const Eigen::Matrix2d E_tilde = E.block<2, 2>(0, 0);

const double a = n1.transpose() * E_tilde * n2;
const double b = (n1.squaredNorm() + n2.squaredNorm()) / 2.0;
const double c = point1h.transpose() * E * point2h;
const double c = point1_homogeneous.transpose() * E * point2_homogeneous;
const double d = std::sqrt(b * b - a * c);
double lambda = c / (b + d);

Expand All @@ -128,8 +128,10 @@ void FindOptimalImageObservations(const Eigen::Matrix3d& E,

lambda *= (2.0 * d) / (n1.squaredNorm() + n2.squaredNorm());

*optimal_point1 = (point1h - S.transpose() * lambda * n1).hnormalized();
*optimal_point2 = (point2h - S.transpose() * lambda * n2).hnormalized();
*optimal_point1 =
(point1_homogeneous - S.transpose() * lambda * n1).hnormalized();
*optimal_point2 =
(point2_homogeneous - S.transpose() * lambda * n2).hnormalized();
}

Eigen::Vector3d EpipoleFromEssentialMatrix(const Eigen::Matrix3d& E,
Expand Down
22 changes: 15 additions & 7 deletions src/colmap/geometry/pose.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,15 +174,23 @@ bool CheckCheirality(const Rigid3d& cam2_from_cam1,
const double max_depth = 1000.0 * cam2_from_cam1.translation.norm();
points3D->clear();
for (size_t i = 0; i < points1.size(); ++i) {
const Eigen::Vector3d point3D = TriangulatePoint(
cam1_from_world, cam2_from_world, points1[i], points2[i]);
Eigen::Vector3d point3D;
if (!TriangulatePoint(cam1_from_world,
cam2_from_world,
points1[i],
points2[i],
&point3D)) {
continue;
}
const double depth1 = CalculateDepth(cam1_from_world, point3D);
if (depth1 > kMinDepth && depth1 < max_depth) {
const double depth2 = CalculateDepth(cam2_from_world, point3D);
if (depth2 > kMinDepth && depth2 < max_depth) {
points3D->push_back(point3D);
}
if (depth1 < kMinDepth || depth1 > max_depth) {
continue;
}
const double depth2 = CalculateDepth(cam2_from_world, point3D);
if (depth2 < kMinDepth || depth2 > max_depth) {
continue;
}
points3D->push_back(point3D);
}
return !points3D->empty();
}
Expand Down
87 changes: 38 additions & 49 deletions src/colmap/geometry/triangulation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,63 +37,64 @@

namespace colmap {

Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& cam1_from_world,
const Eigen::Matrix3x4d& cam2_from_world,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2) {
Eigen::Matrix4d A;
bool TriangulatePoint(const Eigen::Matrix3x4d& cam1_from_world,
const Eigen::Matrix3x4d& cam2_from_world,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2,
Eigen::Vector3d* xyz) {
THROW_CHECK_NOTNULL(xyz);

Eigen::Matrix4d A;
A.row(0) = point1(0) * cam1_from_world.row(2) - cam1_from_world.row(0);
A.row(1) = point1(1) * cam1_from_world.row(2) - cam1_from_world.row(1);
A.row(2) = point2(0) * cam2_from_world.row(2) - cam2_from_world.row(0);
A.row(3) = point2(1) * cam2_from_world.row(2) - cam2_from_world.row(1);

Eigen::JacobiSVD<Eigen::Matrix4d> svd(A, Eigen::ComputeFullV);

return svd.matrixV().col(3).hnormalized();
}

std::vector<Eigen::Vector3d> TriangulatePoints(
const Eigen::Matrix3x4d& cam1_from_world,
const Eigen::Matrix3x4d& cam2_from_world,
const std::vector<Eigen::Vector2d>& points1,
const std::vector<Eigen::Vector2d>& points2) {
THROW_CHECK_EQ(points1.size(), points2.size());

std::vector<Eigen::Vector3d> points3D(points1.size());
const Eigen::JacobiSVD<Eigen::Matrix4d> svd(A, Eigen::ComputeFullV);
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
if (svd.info() != Eigen::Success) {
return false;
}
#endif

for (size_t i = 0; i < points3D.size(); ++i) {
points3D[i] = TriangulatePoint(
cam1_from_world, cam2_from_world, points1[i], points2[i]);
if (svd.matrixV()(3, 3) == 0) {
return false;
}

return points3D;
*xyz = svd.matrixV().col(3).hnormalized();
return true;
}

Eigen::Vector3d TriangulateMultiViewPoint(
bool TriangulateMultiViewPoint(
const std::vector<Eigen::Matrix3x4d>& cams_from_world,
const std::vector<Eigen::Vector2d>& points) {
const std::vector<Eigen::Vector2d>& points,
Eigen::Vector3d* xyz) {
THROW_CHECK_EQ(cams_from_world.size(), points.size());
THROW_CHECK_NOTNULL(xyz);

Eigen::Matrix4d A = Eigen::Matrix4d::Zero();

for (size_t i = 0; i < points.size(); i++) {
const Eigen::Vector3d point = points[i].homogeneous().normalized();
const Eigen::Matrix3x4d term =
cams_from_world[i] - point * point.transpose() * cams_from_world[i];
A += term.transpose() * term;
}

Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigen_solver(A);
const Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigen_solver(A);
if (eigen_solver.info() != Eigen::Success ||
eigen_solver.eigenvectors()(3, 0) == 0) {
return false;
}

return eigen_solver.eigenvectors().col(0).hnormalized();
*xyz = eigen_solver.eigenvectors().col(0).hnormalized();
return true;
}

Eigen::Vector3d TriangulateOptimalPoint(
const Eigen::Matrix3x4d& cam1_from_world_mat,
const Eigen::Matrix3x4d& cam2_from_world_mat,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2) {
bool TriangulateOptimalPoint(const Eigen::Matrix3x4d& cam1_from_world_mat,
const Eigen::Matrix3x4d& cam2_from_world_mat,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2,
Eigen::Vector3d* xyz) {
const Rigid3d cam1_from_world(
Eigen::Quaterniond(cam1_from_world_mat.leftCols<3>()),
cam1_from_world_mat.col(3));
Expand All @@ -108,23 +109,11 @@ Eigen::Vector3d TriangulateOptimalPoint(
FindOptimalImageObservations(
E, point1, point2, &optimal_point1, &optimal_point2);

return TriangulatePoint(
cam1_from_world_mat, cam2_from_world_mat, optimal_point1, optimal_point2);
}

std::vector<Eigen::Vector3d> TriangulateOptimalPoints(
const Eigen::Matrix3x4d& cam1_from_world,
const Eigen::Matrix3x4d& cam2_from_world,
const std::vector<Eigen::Vector2d>& points1,
const std::vector<Eigen::Vector2d>& points2) {
std::vector<Eigen::Vector3d> points3D(points1.size());

for (size_t i = 0; i < points3D.size(); ++i) {
points3D[i] = TriangulateOptimalPoint(
cam1_from_world, cam2_from_world, points1[i], points2[i]);
}

return points3D;
return TriangulatePoint(cam1_from_world_mat,
cam2_from_world_mat,
optimal_point1,
optimal_point2,
xyz);
}

double CalculateTriangulationAngle(const Eigen::Vector3d& proj_center1,
Expand Down
59 changes: 25 additions & 34 deletions src/colmap/geometry/triangulation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,31 +47,28 @@ namespace colmap {
//
// @param cam_from_world1 Projection matrix of the first image as 3x4 matrix.
// @param cam_from_world2 Projection matrix of the second image as 3x4 matrix.
// @param point1 Corresponding 2D point in first image.
// @param point2 Corresponding 2D point in second image.
// @param point1 Corresponding 2D point in first image.
// @param point2 Corresponding 2D point in second image.
// @param point3D Triangulated 3D point.
//
// @return Triangulated 3D point.
Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& cam_from_world1,
const Eigen::Matrix3x4d& cam_from_world2,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2);

// Triangulate multiple 3D points from multiple image correspondences.
std::vector<Eigen::Vector3d> TriangulatePoints(
const Eigen::Matrix3x4d& cam_from_world1,
const Eigen::Matrix3x4d& cam_from_world2,
const std::vector<Eigen::Vector2d>& points1,
const std::vector<Eigen::Vector2d>& points2);
// @return Whether triangulation was successful.
bool TriangulatePoint(const Eigen::Matrix3x4d& cam_from_world1,
const Eigen::Matrix3x4d& cam_from_world2,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2,
Eigen::Vector3d* point3D);

// Triangulate point from multiple views minimizing the L2 error.
//
// @param cams_from_world Projection matrices of multi-view observations.
// @param points Image observations of multi-view observations.
// @param cams_from_world Projection matrices of multi-view observations.
// @param points Image observations of multi-view observations.
// @param point3D Triangulated 3D point.
//
// @return Estimated 3D point.
Eigen::Vector3d TriangulateMultiViewPoint(
// @return Whether triangulation was successful.
bool TriangulateMultiViewPoint(
const std::vector<Eigen::Matrix3x4d>& cams_from_world,
const std::vector<Eigen::Vector2d>& points);
const std::vector<Eigen::Vector2d>& points,
Eigen::Vector3d* point3D);

// Triangulate optimal 3D point from corresponding image point observations by
// finding the optimal image observations.
Expand All @@ -85,22 +82,16 @@ Eigen::Vector3d TriangulateMultiViewPoint(
//
// @param cam_from_world1 Projection matrix of the first image as 3x4 matrix.
// @param cam_from_world2 Projection matrix of the second image as 3x4 matrix.
// @param point1 Corresponding 2D point in first image.
// @param point2 Corresponding 2D point in second image.
// @param point1 Corresponding 2D point in first image.
// @param point2 Corresponding 2D point in second image.
// @param point3D Triangulated 3D point.
//
// @return Triangulated optimal 3D point.
Eigen::Vector3d TriangulateOptimalPoint(
const Eigen::Matrix3x4d& cam_from_world1,
const Eigen::Matrix3x4d& cam_from_world2,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2);

// Triangulate multiple optimal 3D points from multiple image correspondences.
std::vector<Eigen::Vector3d> TriangulateOptimalPoints(
const Eigen::Matrix3x4d& cam_from_world1,
const Eigen::Matrix3x4d& cam_from_world2,
const std::vector<Eigen::Vector2d>& points1,
const std::vector<Eigen::Vector2d>& points2);
// @return Whether triangulation was successful.
bool TriangulateOptimalPoint(const Eigen::Matrix3x4d& cam_from_world1,
const Eigen::Matrix3x4d& cam_from_world2,
const Eigen::Vector2d& point1,
const Eigen::Vector2d& point2,
Eigen::Vector3d* point3D);

// Calculate angle in radians between the two rays of a triangulated point.
double CalculateTriangulationAngle(const Eigen::Vector3d& proj_center1,
Expand Down
22 changes: 17 additions & 5 deletions src/colmap/geometry/triangulation_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,18 +60,30 @@ TEST(TriangulatePoint, Nominal) {
const Eigen::Vector3d point2D1 = cam_from_world1 * point3D;
const Eigen::Vector3d point2D2 = cam_from_world2 * point3D;

const Eigen::Vector3d tri_point3D =
TriangulatePoint(cam_from_world1.ToMatrix(),
cam_from_world2.ToMatrix(),
point2D1.hnormalized(),
point2D2.hnormalized());
Eigen::Vector3d tri_point3D;
EXPECT_TRUE(TriangulatePoint(cam_from_world1.ToMatrix(),
cam_from_world2.ToMatrix(),
point2D1.hnormalized(),
point2D2.hnormalized(),
&tri_point3D));

EXPECT_TRUE((point3D - tri_point3D).norm() < 1e-10);
}
}
}
}

TEST(TriangulatePoint, ParallelRays) {
Eigen::Vector3d xyz;
EXPECT_FALSE(TriangulatePoint(
Rigid3d().ToMatrix(),
Rigid3d(Eigen::Quaterniond::Identity(), Eigen::Vector3d(1, 0, 0))
.ToMatrix(),
Eigen::Vector2d(0, 0),
Eigen::Vector2d(0, 0),
&xyz));
}

TEST(CalculateTriangulationAngle, Nominal) {
const Eigen::Vector3d tvec1(0, 0, 0);
const Eigen::Vector3d tvec2(0, 1, 0);
Expand Down
10 changes: 5 additions & 5 deletions src/colmap/sfm/incremental_mapper.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -327,11 +327,11 @@ void IncrementalMapper::RegisterInitialImagePair(
camera1.CamFromImg(image1.Point2D(corr.point2D_idx1).xy);
const Eigen::Vector2d point2D2 =
camera2.CamFromImg(image2.Point2D(corr.point2D_idx2).xy);
const Eigen::Vector3d& xyz =
TriangulatePoint(cam_from_world1, cam_from_world2, point2D1, point2D2);
const double tri_angle =
CalculateTriangulationAngle(proj_center1, proj_center2, xyz);
if (tri_angle >= min_tri_angle_rad &&
Eigen::Vector3d xyz;
if (TriangulatePoint(
cam_from_world1, cam_from_world2, point2D1, point2D2, &xyz) &&
CalculateTriangulationAngle(proj_center1, proj_center2, xyz) >=
min_tri_angle_rad &&
HasPointPositiveDepth(cam_from_world1, xyz) &&
HasPointPositiveDepth(cam_from_world2, xyz)) {
track.Element(0).point2D_idx = corr.point2D_idx1;
Expand Down

0 comments on commit 189478b

Please sign in to comment.