Merge pull request #1020 from borglab/feature/robustTriangulation

gtsam/geometry/geometry.i

@@ -923,27 +923,34 @@ class StereoCamera {
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3_S2* sharedCal,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3DS2* sharedCal,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3Bundler* sharedCal,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
                                 const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize);
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
                                    gtsam::Cal3_S2* sharedCal,
                                    const gtsam::Point2Vector& measurements,

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,25 @@ struct GTSAM_EXPORT TriangulationParameters {
    */
   double dynamicOutlierRejectionThreshold;
 
+  SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
+
   /**
    * 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 noiseModel noise model to use during nonlinear triangulation
    *
    */
   TriangulationParameters(const double _rankTolerance = 1.0,
       const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
-      double _dynamicOutlierRejectionThreshold = -1) :
+      double _dynamicOutlierRejectionThreshold = -1,
+      const SharedNoiseModel& _noiseModel = nullptr) :
       rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
       landmarkDistanceThreshold(_landmarkDistanceThreshold), //
-      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
+      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
+      noiseModel(_noiseModel){
   }
 
   // stream to output
@@ -366,6 +378,7 @@ struct GTSAM_EXPORT TriangulationParameters {
         << std::endl;
     os << "dynamicOutlierRejectionThreshold = "
         << p.dynamicOutlierRejectionThreshold << std::endl;
+    os << "noise model" << std::endl;
     return os;
   }
 
@@ -468,8 +481,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;

python/gtsam/tests/test_Triangulation.py

@@ -6,28 +6,40 @@ All Rights Reserved
 See LICENSE for the license information
 
 Test Triangulation
-Author: Frank Dellaert & Fan Jiang (Python)
+Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
 """
 import unittest
+from typing import Iterable, List, Optional, Tuple, Union
 
 import numpy as np
 
 import gtsam
-from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
+from gtsam import (
-                   CameraSetCal3Bundler, PinholeCameraCal3_S2,
+    Cal3_S2,
-                   PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3,
+    Cal3Bundler,
-                   Pose3Vector, Rot3)
+    CameraSetCal3_S2,
+    CameraSetCal3Bundler,
+    PinholeCameraCal3_S2,
+    PinholeCameraCal3Bundler,
+    Point2,
+    Point2Vector,
+    Point3,
+    Pose3,
+    Pose3Vector,
+    Rot3,
+)
 from gtsam.utils.test_case import GtsamTestCase
 
+UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
 
-class TestVisualISAMExample(GtsamTestCase):
+
-    """ Tests for triangulation with shared and individual calibrations """
+class TestTriangulationExample(GtsamTestCase):
+    """Tests for triangulation with shared and individual calibrations"""
 
     def setUp(self):
-        """ Set up two camera poses """
+        """Set up two camera poses"""
         # Looking along X-axis, 1 meter above ground plane (x-y)
-        upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
+        pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
-        pose1 = Pose3(upright, Point3(0, 0, 1))
 
         # create second camera 1 meter to the right of first camera
         pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
@@ -39,15 +51,24 @@ class TestVisualISAMExample(GtsamTestCase):
         # landmark ~5 meters infront of camera
         self.landmark = Point3(5, 0.5, 1.2)
 
-    def generate_measurements(self, calibration, camera_model, cal_params, camera_set=None):
+    def generate_measurements(
+        self,
+        calibration: Union[Cal3Bundler, Cal3_S2],
+        camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2],
+        cal_params: Iterable[Iterable[Union[int, float]]],
+        camera_set: Optional[Union[CameraSetCal3Bundler,
+                                   CameraSetCal3_S2]] = None,
+    ) -> Tuple[Point2Vector, Union[CameraSetCal3Bundler, CameraSetCal3_S2,
+                                   List[Cal3Bundler], List[Cal3_S2]]]:
         """
         Generate vector of measurements for given calibration and camera model.
 
-        Args: 
+        Args:
             calibration: Camera calibration e.g. Cal3_S2
             camera_model: Camera model e.g. PinholeCameraCal3_S2
             cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
             camera_set: Cameraset object (for individual calibrations)
+
         Returns:
             list of measurements and list/CameraSet object for cameras
         """
@@ -66,14 +87,15 @@ class TestVisualISAMExample(GtsamTestCase):
 
         return measurements, cameras
 
-    def test_TriangulationExample(self):
+    def test_TriangulationExample(self) -> None:
-        """ Tests triangulation with shared Cal3_S2 calibration"""
+        """Tests triangulation with shared Cal3_S2 calibration"""
         # Some common constants
         sharedCal = (1500, 1200, 0, 640, 480)
 
-        measurements, _ = self.generate_measurements(Cal3_S2,
+        measurements, _ = self.generate_measurements(
-                                                     PinholeCameraCal3_S2,
+            calibration=Cal3_S2,
-                                                     (sharedCal, sharedCal))
+            camera_model=PinholeCameraCal3_S2,
+            cal_params=(sharedCal, sharedCal))
 
         triangulated_landmark = gtsam.triangulatePoint3(self.poses,
                                                         Cal3_S2(sharedCal),
@@ -95,16 +117,17 @@ class TestVisualISAMExample(GtsamTestCase):
 
         self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
 
-    def test_distinct_Ks(self):
+    def test_distinct_Ks(self) -> None:
-        """ Tests triangulation with individual Cal3_S2 calibrations """
+        """Tests triangulation with individual Cal3_S2 calibrations"""
         # two camera parameters
         K1 = (1500, 1200, 0, 640, 480)
         K2 = (1600, 1300, 0, 650, 440)
 
-        measurements, cameras = self.generate_measurements(Cal3_S2,
+        measurements, cameras = self.generate_measurements(
-                                                           PinholeCameraCal3_S2,
+            calibration=Cal3_S2,
-                                                           (K1, K2),
+            camera_model=PinholeCameraCal3_S2,
-                                                           camera_set=CameraSetCal3_S2)
+            cal_params=(K1, K2),
+            camera_set=CameraSetCal3_S2)
 
         triangulated_landmark = gtsam.triangulatePoint3(cameras,
                                                         measurements,
@@ -112,16 +135,17 @@ class TestVisualISAMExample(GtsamTestCase):
                                                         optimize=True)
         self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
 
-    def test_distinct_Ks_Bundler(self):
+    def test_distinct_Ks_Bundler(self) -> None:
-        """ Tests triangulation with individual Cal3Bundler calibrations"""
+        """Tests triangulation with individual Cal3Bundler calibrations"""
         # two camera parameters
         K1 = (1500, 0, 0, 640, 480)
         K2 = (1600, 0, 0, 650, 440)
 
-        measurements, cameras = self.generate_measurements(Cal3Bundler,
+        measurements, cameras = self.generate_measurements(
-                                                           PinholeCameraCal3Bundler,
+            calibration=Cal3Bundler,
-                                                           (K1, K2),
+            camera_model=PinholeCameraCal3Bundler,
-                                                           camera_set=CameraSetCal3Bundler)
+            cal_params=(K1, K2),
+            camera_set=CameraSetCal3Bundler)
 
         triangulated_landmark = gtsam.triangulatePoint3(cameras,
                                                         measurements,
@@ -129,6 +153,71 @@ class TestVisualISAMExample(GtsamTestCase):
                                                         optimize=True)
         self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
 
+    def test_triangulation_robust_three_poses(self) -> None:
+        """Ensure triangulation with a robust model works."""
+        sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
+
+        # landmark ~5 meters infront of camera
+        landmark = Point3(5, 0.5, 1.2)
+
+        pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
+        pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
+        pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
+
+        camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
+        camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
+        camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
+
+        z1: Point2 = camera1.project(landmark)
+        z2: Point2 = camera2.project(landmark)
+        z3: Point2 = camera3.project(landmark)
+
+        poses = gtsam.Pose3Vector([pose1, pose2, pose3])
+        measurements = Point2Vector([z1, z2, z3])
+
+        # noise free, so should give exactly the landmark
+        actual = gtsam.triangulatePoint3(poses,
+                                         sharedCal,
+                                         measurements,
+                                         rank_tol=1e-9,
+                                         optimize=False)
+        self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
+
+        # Add outlier
+        measurements[0] += Point2(100, 120)  # very large pixel noise!
+
+        # now estimate does not match landmark
+        actual2 = gtsam.triangulatePoint3(poses,
+                                          sharedCal,
+                                          measurements,
+                                          rank_tol=1e-9,
+                                          optimize=False)
+        # DLT is surprisingly robust, but still off (actual error is around 0.26m)
+        self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
+        self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
+
+        # Again with nonlinear optimization
+        actual3 = gtsam.triangulatePoint3(poses,
+                                          sharedCal,
+                                          measurements,
+                                          rank_tol=1e-9,
+                                          optimize=True)
+        # result from nonlinear (but non-robust optimization) is close to DLT and still off
+        self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
+
+        # Again with nonlinear optimization, this time with robust loss
+        model = gtsam.noiseModel.Robust.Create(
+            gtsam.noiseModel.mEstimator.Huber.Create(1.345),
+            gtsam.noiseModel.Unit.Create(2))
+        actual4 = gtsam.triangulatePoint3(poses,
+                                          sharedCal,
+                                          measurements,
+                                          rank_tol=1e-9,
+                                          optimize=True,
+                                          model=model)
+        # using the Huber loss we now have a quite small error!! nice!
+        self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
+
 
 if __name__ == "__main__":
     unittest.main()