From ba32a0de85c94eebae4d27116cec3885db9f66bc Mon Sep 17 00:00:00 2001 From: Thomas Sayre-McCord Date: Tue, 8 Mar 2022 16:34:09 +0100 Subject: [PATCH 1/3] fix triangulatePoint3 for calibrations with distortion Prior implementation only used the K() portion of all Cal3 calibrations for the linear triangulation of points with triangulatePoint3. - Added tests for triangulation with non-Cal3_S2 calibrations. - Added skew to the test Cal3_S2 calibration. - Added an undistortMeasurements step to triangulatePoint3 so that linear triangulation works for calibrations with distortion coefficients. --- gtsam/geometry/tests/testTriangulation.cpp | 109 ++++++++++++++++++++- gtsam/geometry/triangulation.h | 34 ++++++- 2 files changed, 139 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 3a09f49bc7..95c41fb99f 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -38,7 +39,7 @@ using namespace boost::assign; // Some common constants static const boost::shared_ptr sharedCal = // - boost::make_shared(1500, 1200, 0, 640, 480); + boost::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); @@ -95,11 +96,113 @@ TEST(triangulation, twoPoses) { EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } +//****************************************************************************** +// Simple test with a well-behaved two camera situation with Cal3S2 calibration. +TEST(triangulation, twoPosesCal3S2) { + static const boost::shared_ptr sharedDistortedCal = // + boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); + + PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + + PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + +// 1. Project two landmarks into two cameras and triangulate + Point2 z1Distorted = camera1Distorted.project(landmark); + Point2 z2Distorted = camera2Distorted.project(landmark); + + vector poses; + Point2Vector measurements; + + poses += pose1, pose2; + measurements += z1Distorted, z2Distorted; + + double rank_tol = 1e-9; + + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); + + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); +} + +//****************************************************************************** +// Simple test with a well-behaved two camera situation with Fisheye calibration. +TEST(triangulation, twoPosesFisheye) { + using Calibration = Cal3Fisheye; + static const boost::shared_ptr sharedDistortedCal = // + boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); + + PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + + PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + +// 1. Project two landmarks into two cameras and triangulate + Point2 z1Distorted = camera1Distorted.project(landmark); + Point2 z2Distorted = camera2Distorted.project(landmark); + + vector poses; + Point2Vector measurements; + + poses += pose1, pose2; + measurements += z1Distorted, z2Distorted; + + double rank_tol = 1e-9; + + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); + + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); +} + + //****************************************************************************** // Similar, but now with Bundler calibration TEST(triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // - boost::make_shared(1500, 0, 0, 640, 480); + boost::make_shared(1500, 0.1, 0.2, 640, 480); PinholeCamera camera1(pose1, *bundlerCal); PinholeCamera camera2(pose2, *bundlerCal); @@ -126,7 +229,7 @@ TEST(triangulation, twoPosesBundler) { boost::optional actual2 = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); } //****************************************************************************** diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index af01d3a364..fce7ca89b8 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -227,6 +227,35 @@ std::vector> projectionMatricesFrom return projection_matrices; } +/** Remove distortion for measurements so as if the measurements came from a pinhole camera. + * + * Removes distortion but maintains the K matrix of the initial sharedCal. Operates by calibrating using + * full calibration and uncalibrating with only the pinhole component of the calibration. + * @tparam CALIBRATION Calibration type to use. + * @param sharedCal Calibration with which measurements were taken. + * @param measurements Vector of measurements to undistort. + * @return measurements with the effect of the distortion of sharedCal removed. + */ +template +Point2Vector undistortMeasurements(boost::shared_ptr sharedCal, const Point2Vector& measurements) { + const auto& K = sharedCal->K(); + Cal3_S2 pinholeCalibration(K(0,0), K(1,1), K(0,1), K(0,2), K(1,2)); + Point2Vector undistortedMeasurements; + // Calibrate with sharedCal and uncalibrate with pinhole version of sharedCal so that measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), [&sharedCal, &pinholeCalibration](auto& measurement) { + return pinholeCalibration.uncalibrate(sharedCal->calibrate(measurement)); + }); + + return undistortedMeasurements; +} + +/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */ +template <> +Point2Vector undistortMeasurements(boost::shared_ptr sharedCal, const Point2Vector& measurements) { + return measurements; +} + + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -253,8 +282,11 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = undistortMeasurements(sharedCal, measurements); + // Triangulate linearly - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); // Then refine using non-linear optimization if (optimize) From afc406162b4b0a427c473de117ab380c1e9dd780 Mon Sep 17 00:00:00 2001 From: Thomas Sayre-McCord Date: Wed, 9 Mar 2022 11:39:07 +0100 Subject: [PATCH 2/3] add support for second version of triangulatePoint3 - added undistort for cameras version of triangulatePoint3 - changed to 2 space indent - changed to calibration from shared calibration --- gtsam/geometry/tests/testTriangulation.cpp | 171 ++++++++++++--------- gtsam/geometry/triangulation.h | 98 ++++++++++-- 2 files changed, 181 insertions(+), 88 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 95c41fb99f..0e30acaae2 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -99,102 +99,102 @@ TEST(triangulation, twoPoses) { //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3S2 calibration. TEST(triangulation, twoPosesCal3S2) { - static const boost::shared_ptr sharedDistortedCal = // - boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); + static const boost::shared_ptr sharedDistortedCal = // + boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(landmark); + Point2 z2Distorted = camera2Distorted.project(landmark); - vector poses; - Point2Vector measurements; + vector poses; + Point2Vector measurements; - poses += pose1, pose2; - measurements += z1Distorted, z2Distorted; + poses += pose1, pose2; + measurements += z1Distorted, z2Distorted; - double rank_tol = 1e-9; + double rank_tol = 1e-9; - // 1. Test simple DLT, perfect in no noise situation - bool optimize = false; - boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); - - // 4. Now with optimization on - optimize = true; - boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); + + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } //****************************************************************************** // Simple test with a well-behaved two camera situation with Fisheye calibration. TEST(triangulation, twoPosesFisheye) { - using Calibration = Cal3Fisheye; - static const boost::shared_ptr sharedDistortedCal = // - boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); + using Calibration = Cal3Fisheye; + static const boost::shared_ptr sharedDistortedCal = // + boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); - PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); + PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); - PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); + PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 z1Distorted = camera1Distorted.project(landmark); - Point2 z2Distorted = camera2Distorted.project(landmark); + Point2 z1Distorted = camera1Distorted.project(landmark); + Point2 z2Distorted = camera2Distorted.project(landmark); - vector poses; - Point2Vector measurements; + vector poses; + Point2Vector measurements; - poses += pose1, pose2; - measurements += z1Distorted, z2Distorted; + poses += pose1, pose2; + measurements += z1Distorted, z2Distorted; - double rank_tol = 1e-9; + double rank_tol = 1e-9; - // 1. Test simple DLT, perfect in no noise situation - bool optimize = false; - boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmark, *actual1, 1e-7)); + // 1. Test simple DLT, perfect in no noise situation + bool optimize = false; + boost::optional actual1 = // + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); - - // 4. Now with optimization on - optimize = true; - boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); - EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); + // 2. test with optimization on, same answer + optimize = true; + boost::optional actual2 = // + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); + + // 4. Now with optimization on + optimize = true; + boost::optional actual4 = // + triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } @@ -439,6 +439,31 @@ TEST(triangulation, fourPoses_distinct_Ks) { #endif } +//****************************************************************************** +TEST(triangulation, fourPoses_distinct_Ks_distortion) { + Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + PinholeCamera camera1(pose1, K1); + + // create second camera 1 meter to the right of first camera + Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); + PinholeCamera camera2(pose2, K2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); + + CameraSet> cameras; + Point2Vector measurements; + + cameras += camera1, camera2; + measurements += z1, z2; + + boost::optional actual = // + triangulatePoint3(cameras, measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); +} + //****************************************************************************** TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index fce7ca89b8..0df5696089 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -227,35 +227,99 @@ std::vector> projectionMatricesFrom return projection_matrices; } +/** Create a pinhole calibration from a different Cal3 object, removing distortion. + * + * @tparam CALIBRATION Original calibration object. + * @param cal Input calibration object. + * @return Cal3_S2 with only the pinhole elements of cal. + */ +template +Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { + const auto& K = cal.K(); + return Cal3_S2(K(0,0), K(1,1), K(0,1), K(0,2), K(1,2)); +} + +/** Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements */ +template +MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, + const MEASUREMENT& measurement, + boost::optional pinholeCal = boost::none) { + if (!pinholeCal) { + pinholeCal = createPinholeCalibration(cal); + } + return pinholeCal->uncalibrate(cal.calibrate(measurement)); +} + /** Remove distortion for measurements so as if the measurements came from a pinhole camera. * - * Removes distortion but maintains the K matrix of the initial sharedCal. Operates by calibrating using + * Removes distortion but maintains the K matrix of the initial cal. Operates by calibrating using * full calibration and uncalibrating with only the pinhole component of the calibration. * @tparam CALIBRATION Calibration type to use. - * @param sharedCal Calibration with which measurements were taken. + * @param cal Calibration with which measurements were taken. * @param measurements Vector of measurements to undistort. * @return measurements with the effect of the distortion of sharedCal removed. */ template -Point2Vector undistortMeasurements(boost::shared_ptr sharedCal, const Point2Vector& measurements) { - const auto& K = sharedCal->K(); - Cal3_S2 pinholeCalibration(K(0,0), K(1,1), K(0,1), K(0,2), K(1,2)); - Point2Vector undistortedMeasurements; - // Calibrate with sharedCal and uncalibrate with pinhole version of sharedCal so that measurements are undistorted. - std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), [&sharedCal, &pinholeCalibration](auto& measurement) { - return pinholeCalibration.uncalibrate(sharedCal->calibrate(measurement)); - }); - - return undistortedMeasurements; +Point2Vector undistortMeasurements(const CALIBRATION& cal, + const Point2Vector& measurements) { + Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); + Point2Vector undistortedMeasurements; + // Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), + [&cal, &pinholeCalibration](const Point2& measurement) { + return undistortMeasurementInternal(cal, measurement, pinholeCalibration); + }); + return undistortedMeasurements; } /** Specialization for Cal3_S2 as it doesn't need to be undistorted. */ template <> -Point2Vector undistortMeasurements(boost::shared_ptr sharedCal, const Point2Vector& measurements) { +inline Point2Vector undistortMeasurements(const Cal3_S2& cal, + const Point2Vector& measurements) { return measurements; } +/** Remove distortion for measurements so as if the measurements came from a pinhole camera. + * + * Removes distortion but maintains the K matrix of the initial calibrations. Operates by calibrating using + * full calibration and uncalibrating with only the pinhole component of the calibration. + * @tparam CAMERA Camera type to use. + * @param cameras Cameras corresponding to each measurement. + * @param measurements Vector of measurements to undistort. + * @return measurements with the effect of the distortion of the camera removed. + */ +template +typename CAMERA::MeasurementVector undistortMeasurements(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { + + const size_t num_meas = cameras.size(); + assert(num_meas == measurements.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); + for (size_t ii = 0; ii < num_meas; ++ii) { + // Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. + undistortedMeasurements[ii] = + undistortMeasurementInternal(cameras[ii].calibration(), measurements[ii]); + } + return undistortedMeasurements; +} + +/** Specialize for Cal3_S2 to do nothing. */ +template > +inline PinholeCamera::MeasurementVector undistortMeasurements( + const CameraSet>& cameras, + const PinholeCamera::MeasurementVector& measurements) { + return measurements; +} + +/** Specialize for SphericalCamera to do nothing. */ +template +inline SphericalCamera::MeasurementVector undistortMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { + return measurements; +} + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -283,7 +347,7 @@ Point3 triangulatePoint3(const std::vector& poses, auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = undistortMeasurements(sharedCal, measurements); + auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); @@ -332,7 +396,11 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromCameras(cameras); - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + + // Undistort the measurements, leaving only the pinhole elements in effect. + auto undistortedMeasurements = undistortMeasurements(cameras, measurements); + + Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); // The n refine using non-linear optimization if (optimize) From bcf49e6243b67d237aa14b9a52728ed674f2453e Mon Sep 17 00:00:00 2001 From: Thomas Sayre-McCord Date: Mon, 14 Mar 2022 09:19:19 +0100 Subject: [PATCH 3/3] set new code to google style and fix doc - new code in triangulation and testTriangulation - clean up doc number and typos --- gtsam/geometry/tests/testTriangulation.cpp | 48 +++++++++------ gtsam/geometry/triangulation.h | 70 +++++++++++++--------- 2 files changed, 72 insertions(+), 46 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0e30acaae2..fb66fb6a27 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -97,16 +97,17 @@ TEST(triangulation, twoPoses) { } //****************************************************************************** -// Simple test with a well-behaved two camera situation with Cal3S2 calibration. -TEST(triangulation, twoPosesCal3S2) { +// Simple test with a well-behaved two camera situation with Cal3DS2 calibration. +TEST(triangulation, twoPosesCal3DS2) { static const boost::shared_ptr sharedDistortedCal = // - boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); + boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, + -0.0003); PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); -// 1. Project two landmarks into two cameras and triangulate + // 0. Project two landmarks into two cameras and triangulate Point2 z1Distorted = camera1Distorted.project(landmark); Point2 z2Distorted = camera2Distorted.project(landmark); @@ -121,13 +122,15 @@ TEST(triangulation, twoPosesCal3S2) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); EXPECT(assert_equal(landmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -136,28 +139,32 @@ TEST(triangulation, twoPosesCal3S2) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } //****************************************************************************** -// Simple test with a well-behaved two camera situation with Fisheye calibration. +// Simple test with a well-behaved two camera situation with Fisheye +// calibration. TEST(triangulation, twoPosesFisheye) { using Calibration = Cal3Fisheye; static const boost::shared_ptr sharedDistortedCal = // - boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); + boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, + 0.0001, -0.0003); PinholeCamera camera1Distorted(pose1, *sharedDistortedCal); PinholeCamera camera2Distorted(pose2, *sharedDistortedCal); -// 1. Project two landmarks into two cameras and triangulate + // 0. Project two landmarks into two cameras and triangulate Point2 z1Distorted = camera1Distorted.project(landmark); Point2 z2Distorted = camera2Distorted.project(landmark); @@ -172,13 +179,15 @@ TEST(triangulation, twoPosesFisheye) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, 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, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); EXPECT(assert_equal(landmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -187,17 +196,18 @@ TEST(triangulation, twoPosesFisheye) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, sharedDistortedCal, measurements, + rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } - //****************************************************************************** // Similar, but now with Bundler calibration TEST(triangulation, twoPosesBundler) { @@ -220,7 +230,8 @@ TEST(triangulation, twoPosesBundler) { double rank_tol = 1e-9; boost::optional actual = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + optimize); EXPECT(assert_equal(landmark, *actual, 1e-7)); // Add some noise and try again @@ -228,7 +239,8 @@ TEST(triangulation, twoPosesBundler) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); + triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 0df5696089..49b5aef04b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -227,7 +227,8 @@ std::vector> projectionMatricesFrom return projection_matrices; } -/** Create a pinhole calibration from a different Cal3 object, removing distortion. +/** Create a pinhole calibration from a different Cal3 object, removing + * distortion. * * @tparam CALIBRATION Original calibration object. * @param cal Input calibration object. @@ -236,13 +237,14 @@ std::vector> projectionMatricesFrom template Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { const auto& K = cal.K(); - return Cal3_S2(K(0,0), K(1,1), K(0,1), K(0,2), K(1,2)); + return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)); } -/** Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements */ +/** Internal undistortMeasurement to be used by undistortMeasurement and + * undistortMeasurements */ template -MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, - const MEASUREMENT& measurement, +MEASUREMENT undistortMeasurementInternal( + const CALIBRATION& cal, const MEASUREMENT& measurement, boost::optional pinholeCal = boost::none) { if (!pinholeCal) { pinholeCal = createPinholeCalibration(cal); @@ -250,10 +252,12 @@ MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, return pinholeCal->uncalibrate(cal.calibrate(measurement)); } -/** Remove distortion for measurements so as if the measurements came from a pinhole camera. +/** Remove distortion for measurements so as if the measurements came from a + * pinhole camera. * - * Removes distortion but maintains the K matrix of the initial cal. Operates by calibrating using - * full calibration and uncalibrating with only the pinhole component of the calibration. + * Removes distortion but maintains the K matrix of the initial cal. Operates by + * calibrating using full calibration and uncalibrating with only the pinhole + * component of the calibration. * @tparam CALIBRATION Calibration type to use. * @param cal Calibration with which measurements were taken. * @param measurements Vector of measurements to undistort. @@ -261,14 +265,17 @@ MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, */ template Point2Vector undistortMeasurements(const CALIBRATION& cal, - const Point2Vector& measurements) { + const Point2Vector& measurements) { Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); Point2Vector undistortedMeasurements; - // Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. - std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. + std::transform(measurements.begin(), measurements.end(), + std::back_inserter(undistortedMeasurements), [&cal, &pinholeCalibration](const Point2& measurement) { - return undistortMeasurementInternal(cal, measurement, pinholeCalibration); - }); + return undistortMeasurementInternal( + cal, measurement, pinholeCalibration); + }); return undistortedMeasurements; } @@ -276,30 +283,33 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal, template <> inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) { - return measurements; + return measurements; } - -/** Remove distortion for measurements so as if the measurements came from a pinhole camera. +/** Remove distortion for measurements so as if the measurements came from a + * pinhole camera. * - * Removes distortion but maintains the K matrix of the initial calibrations. Operates by calibrating using - * full calibration and uncalibrating with only the pinhole component of the calibration. + * Removes distortion but maintains the K matrix of the initial calibrations. + * Operates by calibrating using full calibration and uncalibrating with only + * the pinhole component of the calibration. * @tparam CAMERA Camera type to use. * @param cameras Cameras corresponding to each measurement. * @param measurements Vector of measurements to undistort. * @return measurements with the effect of the distortion of the camera removed. */ template -typename CAMERA::MeasurementVector undistortMeasurements(const CameraSet& cameras, +typename CAMERA::MeasurementVector undistortMeasurements( + const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = cameras.size(); assert(num_meas == measurements.size()); typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); for (size_t ii = 0; ii < num_meas; ++ii) { - // Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. + // Calibrate with cal and uncalibrate with pinhole version of cal so that + // measurements are undistorted. undistortedMeasurements[ii] = - undistortMeasurementInternal(cameras[ii].calibration(), measurements[ii]); + undistortMeasurementInternal( + cameras[ii].calibration(), measurements[ii]); } return undistortedMeasurements; } @@ -315,8 +325,8 @@ inline PinholeCamera::MeasurementVector undistortMeasurements( /** Specialize for SphericalCamera to do nothing. */ template inline SphericalCamera::MeasurementVector undistortMeasurements( - const CameraSet& cameras, - const SphericalCamera::MeasurementVector& measurements) { + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { return measurements; } @@ -347,10 +357,12 @@ Point3 triangulatePoint3(const std::vector& poses, auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); // Triangulate linearly - Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + Point3 point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); // Then refine using non-linear optimization if (optimize) @@ -398,9 +410,11 @@ Point3 triangulatePoint3( auto projection_matrices = projectionMatricesFromCameras(cameras); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = undistortMeasurements(cameras, measurements); + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); - Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + Point3 point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); // The n refine using non-linear optimization if (optimize)