diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 3a09f49bc..95c41fb99 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 af01d3a36..fce7ca89b 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)