added robustness to triangulation

gtsam/geometry/tests/testTriangulation.cpp

@@ -182,6 +182,94 @@ TEST(triangulation, fourPoses) {
 #endif
 }
 
+//******************************************************************************
+TEST(triangulation, threePoses_robustNoiseModel) {
+
+  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
+  Point2 z3 = camera3.project(landmark);
+
+  vector<Pose3> poses;
+  Point2Vector measurements;
+  poses += pose1, pose2, pose3;
+  measurements += z1, z2, z3;
+
+  // noise free, so should give exactly the landmark
+  boost::optional<Point3> actual =
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
+
+  // Add outlier
+  measurements.at(0) += Point2(100, 120); // very large pixel noise!
+
+  // now estimate does not match landmark
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+  // DLT is surprisingly robust, but still off (actual error is around 0.26m):
+  EXPECT( (landmark - *actual2).norm() >= 0.2);
+  EXPECT( (landmark - *actual2).norm() <= 0.5);
+
+  // Again with nonlinear optimization
+  boost::optional<Point3> actual3 =
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
+  // result from nonlinear (but non-robust optimization) is close to DLT and still off
+  EXPECT(assert_equal(*actual2, *actual3, 0.1));
+
+  // Again with nonlinear optimization, this time with robust loss
+  auto model = noiseModel::Robust::Create(
+        noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
+  boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
+      poses, sharedCal, measurements, 1e-9, true, model);
+  // using the Huber loss we now have a quite small error!! nice!
+  EXPECT(assert_equal(landmark, *actual4, 0.05));
+}
+
+//******************************************************************************
+TEST(triangulation, fourPoses_robustNoiseModel) {
+
+  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
+  Point2 z3 = camera3.project(landmark);
+
+  vector<Pose3> poses;
+  Point2Vector measurements;
+  poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1
+  measurements += z1, z1, z2, z3;
+
+  // noise free, so should give exactly the landmark
+  boost::optional<Point3> actual =
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
+
+  // Add outlier
+  measurements.at(0) += Point2(100, 120); // very large pixel noise!
+  // add noise on other measurements:
+  measurements.at(1) += Point2(0.1, 0.2); // small noise
+  measurements.at(2) += Point2(0.2, 0.2);
+  measurements.at(3) += Point2(0.3, 0.1);
+
+  // now estimate does not match landmark
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+  // DLT is surprisingly robust, but still off (actual error is around 0.17m):
+  EXPECT( (landmark - *actual2).norm() >= 0.1);
+  EXPECT( (landmark - *actual2).norm() <= 0.5);
+
+  // Again with nonlinear optimization
+  boost::optional<Point3> actual3 =
+      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
+  // result from nonlinear (but non-robust optimization) is close to DLT and still off
+  EXPECT(assert_equal(*actual2, *actual3, 0.1));
+
+  // Again with nonlinear optimization, this time with robust loss
+  auto model = noiseModel::Robust::Create(
+        noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
+  boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
+      poses, sharedCal, measurements, 1e-9, true, model);
+  // using the Huber loss we now have a quite small error!! nice!
+  EXPECT(assert_equal(landmark, *actual4, 0.05));
+}
+
 //******************************************************************************
 TEST(triangulation, fourPoses_distinct_Ks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);

gtsam/geometry/triangulation.h

@@ -14,6 +14,7 @@
  * @brief Functions for triangulation
  * @date July 31, 2013
  * @author Chris Beall
+ * @author Luca Carlone
  */
 
 #pragma once
@@ -105,18 +106,18 @@ template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
     const Point2Vector& measurements, Key landmarkKey,
-    const Point3& initialEstimate) {
+    const Point3& initialEstimate,
+    const SharedNoiseModel& model = nullptr) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
   static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
-  static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
   for (size_t i = 0; i < measurements.size(); i++) {
     const Pose3& pose_i = poses[i];
     typedef PinholePose<CALIBRATION> Camera;
     Camera camera_i(pose_i, sharedCal);
     graph.emplace_shared<TriangulationFactor<Camera> > //
-        (camera_i, measurements[i], unit2, landmarkKey);
+        (camera_i, measurements[i], model? model : unit2, landmarkKey);
   }
   return std::make_pair(graph, values);
 }
@@ -134,7 +135,8 @@ template<class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
-    const Point3& initialEstimate) {
+    const Point3& initialEstimate,
+    const SharedNoiseModel& model = nullptr) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
@@ -143,7 +145,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
   for (size_t i = 0; i < measurements.size(); i++) {
     const CAMERA& camera_i = cameras[i];
     graph.emplace_shared<TriangulationFactor<CAMERA> > //
-        (camera_i, measurements[i], unit, landmarkKey);
+        (camera_i, measurements[i], model? model : unit, landmarkKey);
   }
   return std::make_pair(graph, values);
 }
@@ -169,13 +171,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
 template<class CALIBRATION>
 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
     boost::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements, const Point3& initialEstimate) {
+    const Point2Vector& measurements, const Point3& initialEstimate,
+    const SharedNoiseModel& model = nullptr) {
 
   // Create a factor graph and initial values
   Values values;
   NonlinearFactorGraph graph;
   boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
-      (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
+      (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
 
   return optimize(graph, values, Symbol('p', 0));
 }
@@ -190,13 +193,14 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
 template<class CAMERA>
 Point3 triangulateNonlinear(
     const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
+    const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
+    const SharedNoiseModel& model = nullptr) {
 
   // Create a factor graph and initial values
   Values values;
   NonlinearFactorGraph graph;
   boost::tie(graph, values) = triangulationGraph<CAMERA> //
-      (cameras, measurements, Symbol('p', 0), initialEstimate);
+      (cameras, measurements, Symbol('p', 0), initialEstimate, model);
 
   return optimize(graph, values, Symbol('p', 0));
 }
@@ -239,7 +243,8 @@ template<class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     boost::shared_ptr<CALIBRATION> sharedCal,
     const Point2Vector& measurements, double rank_tol = 1e-9,
-    bool optimize = false) {
+    bool optimize = false,
+    const SharedNoiseModel& model = nullptr) {
 
   assert(poses.size() == measurements.size());
   if (poses.size() < 2)
@@ -254,7 +259,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // Then refine using non-linear optimization
   if (optimize)
     point = triangulateNonlinear<CALIBRATION> //
-        (poses, sharedCal, measurements, point);
+        (poses, sharedCal, measurements, point, model);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
@@ -284,7 +289,8 @@ template<class CAMERA>
 Point3 triangulatePoint3(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
-    bool optimize = false) {
+    bool optimize = false,
+    const SharedNoiseModel& model = nullptr) {
 
   size_t m = cameras.size();
   assert(measurements.size() == m);
@@ -298,7 +304,7 @@ Point3 triangulatePoint3(
 
   // The n refine using non-linear optimization
   if (optimize)
-    point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
+    point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
@@ -317,9 +323,10 @@ template<class CALIBRATION>
 Point3 triangulatePoint3(
     const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
     const Point2Vector& measurements, double rank_tol = 1e-9,
-    bool optimize = false) {
+    bool optimize = false,
+    const SharedNoiseModel& model = nullptr) {
   return triangulatePoint3<PinholeCamera<CALIBRATION> > //
-  (cameras, measurements, rank_tol, optimize);
+  (cameras, measurements, rank_tol, optimize, model);
 }
 
 struct GTSAM_EXPORT TriangulationParameters {
@@ -341,20 +348,29 @@ struct GTSAM_EXPORT TriangulationParameters {
    */
   double dynamicOutlierRejectionThreshold;
 
+  SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
+
+  bool invDepthLinearTriangulation; ///< if set to true, we use an inverse depth alternative to DL
+
   /**
    * Constructor
    * @param rankTol tolerance used to check if point triangulation is degenerate
    * @param enableEPI if true refine triangulation with embedded LM iterations
    * @param landmarkDistanceThreshold flag as degenerate if point further than this
    * @param dynamicOutlierRejectionThreshold or if average error larger than this
+   * @param invDepthLinearTriangulation use inverse depth approach to linear triangulation
    *
    */
   TriangulationParameters(const double _rankTolerance = 1.0,
       const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
-      double _dynamicOutlierRejectionThreshold = -1) :
+      double _dynamicOutlierRejectionThreshold = -1,
+      const SharedNoiseModel& _noiseModel = nullptr,
+      const bool _invDepthLinearTriangulation = false) :
       rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
       landmarkDistanceThreshold(_landmarkDistanceThreshold), //
-      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
+      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
+      noiseModel(_noiseModel),
+      invDepthLinearTriangulation(_invDepthLinearTriangulation){
   }
 
   // stream to output
@@ -366,6 +382,9 @@ struct GTSAM_EXPORT TriangulationParameters {
         << std::endl;
     os << "dynamicOutlierRejectionThreshold = "
         << p.dynamicOutlierRejectionThreshold << std::endl;
+    os << "noise model" << std::endl;
+    os << "invDepthLinearTriangulation = " << p.invDepthLinearTriangulation
+       << std::endl;
     return os;
   }
 
@@ -379,6 +398,7 @@ private:
     ar & BOOST_SERIALIZATION_NVP(enableEPI);
     ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
     ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
+    ar & BOOST_SERIALIZATION_NVP(invDepthLinearTriangulation);
   }
 };
 
@@ -468,8 +488,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
   else
     // We triangulate the 3D position of the landmark
     try {
-      Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
+      Point3 point =
-          params.rankTolerance, params.enableEPI);
+          triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
+                                    params.enableEPI, params.noiseModel);
 
       // Check landmark distance and re-projection errors to avoid outliers
       size_t i = 0;