From 64b520aea40dbbe7273e81ee092aaf4af6971214 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 28 Aug 2021 14:26:36 -0400 Subject: [PATCH] supertriangulation! with spherical camera --- gtsam/geometry/tests/testTriangulation.cpp | 56 ++++++++++++++++++- gtsam/geometry/triangulation.cpp | 48 ++++++++++++---- gtsam/geometry/triangulation.h | 12 ++++ .../slam/tests/testSmartProjectionFactorP.cpp | 2 +- 4 files changed, 103 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d0627c31a..8afae8c61 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -484,13 +484,65 @@ TEST( triangulation, twoPoses_sphericalCamera) { optimize = false; boost::optional actual3 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(5.94321,0.654327,1.48029), *actual3, 1e-4)); + EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3)); // 6. Now with optimization on optimize = true; boost::optional actual4 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(5.94321,0.654327,1.48029), *actual4, 1e-4)); + EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3)); +} + +//****************************************************************************** +TEST( triangulation, twoPoses_sphericalCamera_extremeFOV) { + + vector poses; + std::vector measurements; + + // Project landmark into two cameras and triangulate + Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame + Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(2.0,0.0,0.0)); // 2m in front of poseA + Point3 landmarkL(1.0,-1.0,0.0);// 1m to the right of both cameras, in front of poseA, behind poseB + SphericalCamera cam1(poseA); + SphericalCamera cam2(poseB); + Unit3 u1 = cam1.project(landmarkL); + Unit3 u2 = cam2.project(landmarkL); + + EXPECT(assert_equal(Unit3(Point3(1.0,0.0,1.0)), u1, 1e-7)); // in front and to the right of PoseA + EXPECT(assert_equal(Unit3(Point3(1.0,0.0,-1.0)), u2, 1e-7));// behind and to the right of PoseB + + poses += pose1, pose2; + measurements += u1, u2; + + CameraSet cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + double rank_tol = 1e-9; + + // 1. Test simple DLT, when 1 point is behind spherical camera + bool optimize = false; +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION( + triangulatePoint3(cameras, measurements, rank_tol, + optimize), TriangulationCheiralityException); +#else // otherwise project should not throw the exception + boost::optional actual1 = // + triangulatePoint3(cameras, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); +#endif + + // 2. test with optimization on, same answer + optimize = true; +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION( + triangulatePoint3(cameras, measurements, rank_tol, + optimize), TriangulationCheiralityException); +#else // otherwise project should not throw the exception + boost::optional actual1 = // + triangulatePoint3(cameras, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); +#endif } //****************************************************************************** diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 8bb8e4779..026afef24 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -53,31 +53,55 @@ Vector4 triangulateHomogeneousDLT( return v; } +Vector4 triangulateHomogeneousDLT( + const std::vector>& projection_matrices, + const std::vector& measurements, double rank_tol) { + + // number of cameras + size_t m = projection_matrices.size(); + + // Allocate DLT matrix + Matrix A = Matrix::Zero(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + size_t row = i * 2; + const Matrix34& projection = projection_matrices.at(i); + const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector + + // build system of equations + A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); + A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); + } + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A, rank_tol); + + if (rank < 3) + throw(TriangulationUnderconstrainedException()); + + return v; +} + Point3 triangulateDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol) { Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); - // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( const std::vector>& projection_matrices, - const std::vector& unit3measurements, double rank_tol) { + const std::vector& measurements, double rank_tol) { - Point2Vector measurements; - for (const Unit3& pu : unit3measurements) { // get canonical pixel projection from Unit3 - Point3 p = pu.point3(); -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (p.z() <= 0) // TODO: maybe we should handle this more delicately - throw(TriangulationCheiralityException()); -#endif - measurements.push_back(Point2(p.x() / p.z(), p.y() / p.z())); - } - return triangulateDLT(projection_matrices, measurements, rank_tol); + // contrary to previous triangulateDLT, this is now taking Unit3 inputs + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, + rank_tol); + // Create 3D point from homogeneous coordinates + return Point3(v.head<3>() / v[3]); } /// diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 2707f33c5..104846bdf 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -59,6 +59,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol = 1e-9); +/** + * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors + * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements Unit3 bearing measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated point, in homogeneous coordinates + */ +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector>& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 99fdd51c8..554ce7873 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -1148,7 +1148,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - DOUBLES_EQUAL(0.1584588987292, graph.error(values), 1e-9); + DOUBLES_EQUAL(0.15734109864597129, graph.error(values), 1e-9); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);