From 2c9a26520db92f91d1f1a333344ebc2e2300a739 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Aug 2021 17:07:08 -0400 Subject: [PATCH] linear triangulation solved! --- gtsam/geometry/SphericalCamera.h | 2 +- gtsam/geometry/tests/testTriangulation.cpp | 88 +++++++++++++++++++++- gtsam/geometry/triangulation.cpp | 27 ++++++- gtsam/geometry/triangulation.h | 8 ++ 4 files changed, 117 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index d819b5cfb..cb354f84b 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -212,7 +212,7 @@ class GTSAM_EXPORT SphericalCamera { /// for Linear Triangulation Matrix34 getCameraProjectionMatrix() const { - return Matrix::Zero(3,4); + return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4)); } private: diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 4f71a48da..dd5a74903 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -10,14 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * testTriangulation.cpp - * - * Created on: July 30th, 2013 - * Author: cbeall3 + * @file testTriangulation.cpp + * @brief triangulation utilities + * @date July 30th, 2013 + * @author Chris Beall (cbeall3) + * @author Luca Carlone */ #include #include +#include #include #include #include @@ -432,6 +434,84 @@ TEST( triangulation, StereotriangulateNonlinear ) { } } +//****************************************************************************** +// Simple test with a well-behaved two camera situation +TEST( triangulation, twoPoses_sphericalCamera) { + + vector poses; + std::vector measurements; + + // Project landmark into two cameras and triangulate + SphericalCamera cam1(pose1); + SphericalCamera cam2(pose2); + Unit3 u1 = cam1.project(landmark); + Unit3 u2 = cam2.project(landmark); + + poses += pose1, pose2; + measurements += u1, u2; + + CameraSet cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + double rank_tol = 1e-9; + + // construct projection matrices from poses & calibration + std::vector> projection_matrices = + getCameraProjectionMatrices(cameras); + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + EXPECT(assert_equal(landmark, point, 1e-7)); + + static const boost::shared_ptr canCal = // + boost::make_shared(1, 1, 0, 0, 0); + PinholeCamera canCam1(pose1, *canCal); + PinholeCamera canCam2(pose2, *canCal); + std::cout << "canCam1.project(landmark);" << canCam1.project(landmark) << std::endl; + std::cout << "canCam2.project(landmark);" << canCam2.project(landmark) << std::endl; + + CameraSet< PinholeCamera > canCameras; + canCameras.push_back(canCam1); + canCameras.push_back(canCam2); + + Point2Vector can_measurements; + can_measurements.push_back(canCam1.project(landmark)); + can_measurements.push_back(canCam2.project(landmark)); + + // construct projection matrices from poses & calibration + std::vector> can_projection_matrices = + getCameraProjectionMatrices< PinholeCamera >(canCameras); + std::cout <<"can_projection_matrices \n" << can_projection_matrices.at(0) < actual1 = // +// triangulatePoint3(cameras, measurements, rank_tol, optimize); +// EXPECT(assert_equal(landmark, *actual1, 1e-7)); +// +// // 2. test with optimization on, same answer +// optimize = true; +// boost::optional actual2 = // +// triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); +// EXPECT(assert_equal(landmark, *actual2, 1e-7)); +// +// // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) +// measurements.at(0) += Point2(0.1, 0.5); +// measurements.at(1) += Point2(-0.2, 0.3); +// optimize = false; +// boost::optional actual3 = // +// triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); +// EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); +// +// // 4. Now with optimization on +// optimize = true; +// boost::optional actual4 = // +// triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); +// EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a5d2e04cd..9f60916e3 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -53,15 +53,36 @@ Vector4 triangulateHomogeneousDLT( return v; } -Point3 triangulateDLT(const std::vector>& projection_matrices, +Point3 triangulateDLT( + const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol) { - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, 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) { + + Point2Vector measurements; + size_t i=0; + for (const Unit3& pu : unit3measurements) { // get canonical pixel projection from Unit3 + Point3 p = pu.point3(); + if (p.z() <= 0) // TODO: maybe we should handle this more delicately + throw(TriangulationCheiralityException()); + measurements.push_back(Point2(p.x() / p.z(), p.y() / p.z())); + std::cout << "px" << Point2(pu.point3().x() / pu.point3().z(), + pu.point3().y() / pu.point3().z())<< std::endl; + std::cout << "projection_matrices \n "<< projection_matrices.at(i)<< std::endl; + i++; + } + return triangulateDLT(projection_matrices, measurements, rank_tol); +} + /// /** * Optimize for triangulation @@ -71,7 +92,7 @@ Point3 triangulateDLT(const std::vector>& projection_matrices, + const std::vector& measurements, + double rank_tol = 1e-9); + /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses