fix triangulatePoint3 for calibrations with distortion

gtsam/geometry/tests/testTriangulation.cpp

@@ -19,6 +19,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -38,7 +39,7 @@ using namespace boost::assign;
 // Some common constants
 
 static const boost::shared_ptr<Cal3_S2> sharedCal =  //
-    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
+    boost::make_shared<Cal3_S2>(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,123 @@ 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 Cal3DS2 calibration.
+TEST(triangulation, twoPosesCal3DS2) {
+  static const boost::shared_ptr<Cal3DS2> sharedDistortedCal =  //
+      boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
+                                  -0.0003);
+
+  PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal);
+
+  PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal);
+
+  // 0. Project two landmarks into two cameras and triangulate
+  Point2 z1Distorted = camera1Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(landmark);
+
+  vector<Pose3> 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<Point3> actual1 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+
+  // 2. test with optimization on, same answer
+  optimize = true;
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<Cal3DS2>(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<Point3> actual3 =  //
+      triangulatePoint3<Cal3DS2>(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<Point3> actual4 =  //
+      triangulatePoint3<Cal3DS2>(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<Calibration> sharedDistortedCal =  //
+      boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
+                                      0.0001, -0.0003);
+
+  PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal);
+
+  PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal);
+
+  // 0. Project two landmarks into two cameras and triangulate
+  Point2 z1Distorted = camera1Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(landmark);
+
+  vector<Pose3> 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<Point3> actual1 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+
+  // 2. test with optimization on, same answer
+  optimize = true;
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<Calibration>(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<Point3> actual3 =  //
+      triangulatePoint3<Calibration>(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<Point3> actual4 =  //
+      triangulatePoint3<Calibration>(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<Cal3Bundler> bundlerCal =  //
-      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
+      boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
   PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
   PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
 
@@ -117,16 +230,18 @@ TEST(triangulation, twoPosesBundler) {
   double rank_tol = 1e-9;
 
   boost::optional<Point3> actual =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
+                                     optimize);
   EXPECT(assert_equal(landmark, *actual, 1e-7));
 
   // Add some noise and try again
   measurements.at(0) += Point2(0.1, 0.5);
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
+                                     optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
 }
 
 //******************************************************************************
@@ -336,6 +451,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<Cal3DS2> 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<Cal3DS2> camera2(pose2, K2);
+
+  // 1. Project two landmarks into two cameras and triangulate
+  Point2 z1 = camera1.project(landmark);
+  Point2 z2 = camera2.project(landmark);
+
+  CameraSet<PinholeCamera<Cal3DS2>> cameras;
+  Point2Vector measurements;
+
+  cameras += camera1, camera2;
+  measurements += z1, z2;
+
+  boost::optional<Point3> actual =  //
+          triangulatePoint3<Cal3DS2>(cameras, measurements);
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
+}
+
 //******************************************************************************
 TEST(triangulation, outliersAndFarLandmarks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);

gtsam/geometry/triangulation.h

@@ -227,6 +227,109 @@ std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> 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 <class CALIBRATION>
+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 <class CALIBRATION, class MEASUREMENT>
+MEASUREMENT undistortMeasurementInternal(
+    const CALIBRATION& cal, const MEASUREMENT& measurement,
+    boost::optional<Cal3_S2> 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 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.
+ * @return measurements with the effect of the distortion of sharedCal removed.
+ */
+template <class CALIBRATION>
+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<CALIBRATION>(
+                       cal, measurement, pinholeCalibration);
+                 });
+  return undistortedMeasurements;
+}
+
+/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
+template <>
+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 <class CAMERA>
+typename CAMERA::MeasurementVector undistortMeasurements(
+    const CameraSet<CAMERA>& 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<typename CAMERA::CalibrationType>(
+            cameras[ii].calibration(), measurements[ii]);
+  }
+  return undistortedMeasurements;
+}
+
+/** Specialize for Cal3_S2 to do nothing. */
+template <class CAMERA = PinholeCamera<Cal3_S2>>
+inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
+    const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
+    const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
+  return measurements;
+}
+
+/** Specialize for SphericalCamera to do nothing. */
+template <class CAMERA = SphericalCamera>
+inline SphericalCamera::MeasurementVector undistortMeasurements(
+    const CameraSet<SphericalCamera>& 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
@@ -253,8 +356,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& 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<CALIBRATION>(*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)
@@ -300,7 +408,13 @@ 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<CAMERA>(cameras, measurements);
+
+  Point3 point =
+      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
 
   // The n refine using non-linear optimization
   if (optimize)