diff --git a/src/colmap/estimators/triangulation.cc b/src/colmap/estimators/triangulation.cc index f7e751990..217d45e46 100644 --- a/src/colmap/estimators/triangulation.cc +++ b/src/colmap/estimators/triangulation.cc @@ -63,12 +63,13 @@ void TriangulationEstimator::Estimate(const std::vector& 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, @@ -89,7 +90,10 @@ void TriangulationEstimator::Estimate(const std::vector& 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) { diff --git a/src/colmap/geometry/essential_matrix.cc b/src/colmap/geometry/essential_matrix.cc index 178a0004d..486153df6 100644 --- a/src/colmap/geometry/essential_matrix.cc +++ b/src/colmap/geometry/essential_matrix.cc @@ -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 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); @@ -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, diff --git a/src/colmap/geometry/pose.cc b/src/colmap/geometry/pose.cc index 588f3b7a9..a6093ff73 100644 --- a/src/colmap/geometry/pose.cc +++ b/src/colmap/geometry/pose.cc @@ -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(); } diff --git a/src/colmap/geometry/triangulation.cc b/src/colmap/geometry/triangulation.cc index e26317aea..cb6b9ab1b 100644 --- a/src/colmap/geometry/triangulation.cc +++ b/src/colmap/geometry/triangulation.cc @@ -37,46 +37,42 @@ 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 svd(A, Eigen::ComputeFullV); - - return svd.matrixV().col(3).hnormalized(); -} - -std::vector TriangulatePoints( - const Eigen::Matrix3x4d& cam1_from_world, - const Eigen::Matrix3x4d& cam2_from_world, - const std::vector& points1, - const std::vector& points2) { - THROW_CHECK_EQ(points1.size(), points2.size()); - - std::vector points3D(points1.size()); + const Eigen::JacobiSVD 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& cams_from_world, - const std::vector& points) { + const std::vector& 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 = @@ -84,16 +80,21 @@ Eigen::Vector3d TriangulateMultiViewPoint( A += term.transpose() * term; } - Eigen::SelfAdjointEigenSolver eigen_solver(A); + const Eigen::SelfAdjointEigenSolver 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)); @@ -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 TriangulateOptimalPoints( - const Eigen::Matrix3x4d& cam1_from_world, - const Eigen::Matrix3x4d& cam2_from_world, - const std::vector& points1, - const std::vector& points2) { - std::vector 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, diff --git a/src/colmap/geometry/triangulation.h b/src/colmap/geometry/triangulation.h index a35e9231c..3905481e7 100644 --- a/src/colmap/geometry/triangulation.h +++ b/src/colmap/geometry/triangulation.h @@ -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 TriangulatePoints( - const Eigen::Matrix3x4d& cam_from_world1, - const Eigen::Matrix3x4d& cam_from_world2, - const std::vector& points1, - const std::vector& 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& cams_from_world, - const std::vector& points); + const std::vector& points, + Eigen::Vector3d* point3D); // Triangulate optimal 3D point from corresponding image point observations by // finding the optimal image observations. @@ -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 TriangulateOptimalPoints( - const Eigen::Matrix3x4d& cam_from_world1, - const Eigen::Matrix3x4d& cam_from_world2, - const std::vector& points1, - const std::vector& 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, diff --git a/src/colmap/geometry/triangulation_test.cc b/src/colmap/geometry/triangulation_test.cc index aa81006a3..a89d77f88 100644 --- a/src/colmap/geometry/triangulation_test.cc +++ b/src/colmap/geometry/triangulation_test.cc @@ -60,11 +60,12 @@ 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); } @@ -72,6 +73,17 @@ TEST(TriangulatePoint, Nominal) { } } +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); diff --git a/src/colmap/sfm/incremental_mapper.cc b/src/colmap/sfm/incremental_mapper.cc index 2c30dd04f..46deb32ea 100644 --- a/src/colmap/sfm/incremental_mapper.cc +++ b/src/colmap/sfm/incremental_mapper.cc @@ -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;