Merge pull request #1549 from borglab/python/backwards-compatibility

python/gtsam/__init__.py

@@ -4,9 +4,49 @@
 
 import sys
 
+from gtsam.utils import findExampleDataFile
+
 from gtsam import gtsam, utils
 from gtsam.gtsam import *
-from gtsam.utils import findExampleDataFile
+
+#### Typedefs to allow for backwards compatibility
+#TODO(Varun) deprecate in future release
+# gtsam
+KeyVector = list
+# base
+IndexPairSetMap = dict
+IndexPairVector = list
+# geometry
+Point2Vector = list
+Pose3Vector = list
+Rot3Vector = list
+Point2Pairs = list
+Point3Pairs = list
+Pose2Pairs = list
+Pose3Pairs = list
+# sfm
+BinaryMeasurementsPoint3 = list
+BinaryMeasurementsUnit3 = list
+BinaryMeasurementsRot3 = list
+KeyPairDoubleMap = dict
+SfmTrack2dVector = list
+SfmTracks = list
+SfmCameras = list
+SfmMeasurementVector = list
+MatchIndicesMap = dict
+KeypointsVector = list
+# slam
+BetweenFactorPose3s = list
+BetweenFactorPose2s = list
+
+
+class FixedLagSmootherKeyTimestampMap(dict):
+    """Class to provide backwards compatibility"""
+    def insert(self, key_value):
+        self[key_value[0]] = key_value[1]
+
+
+#### End typedefs
 
 
 def _init():

python/gtsam/tests/test_Cal3Fisheye.py

@@ -11,11 +11,12 @@ Refactored: Roderick Koehle
 """
 import unittest
 
-import gtsam
 import numpy as np
 from gtsam.symbol_shorthand import K, L, P
 from gtsam.utils.test_case import GtsamTestCase
 
+import gtsam
+
 
 def ulp(ftype=np.float64):
     """
@@ -26,7 +27,7 @@ def ulp(ftype=np.float64):
 
 
 class TestCal3Fisheye(GtsamTestCase):
-    
+
     @classmethod
     def setUpClass(cls):
         """
@@ -53,7 +54,7 @@ class TestCal3Fisheye(GtsamTestCase):
         cls.poses = [pose1, pose2]
         cls.cameras = [camera1, camera2]
         cls.measurements = [k.project(cls.origin) for k in cls.cameras]
-        
+
     def test_Cal3Fisheye(self):
         K = gtsam.Cal3Fisheye()
         self.assertEqual(K.fx(), 1.)
@@ -62,7 +63,7 @@ class TestCal3Fisheye(GtsamTestCase):
     def test_distortion(self):
         """Fisheye distortion and rectification"""
         equidistant = gtsam.Cal3Fisheye()
-        perspective_pt = self.obj_point[0:2]/self.obj_point[2]
+        perspective_pt = self.obj_point[0:2] / self.obj_point[2]
         distorted_pt = equidistant.uncalibrate(perspective_pt)
         rectified_pt = equidistant.calibrate(distorted_pt)
         self.gtsamAssertEquals(distorted_pt, self.img_point)
@@ -166,7 +167,7 @@ class TestCal3Fisheye(GtsamTestCase):
         pose = gtsam.Pose3()
         camera = gtsam.Cal3Fisheye()
         state = gtsam.Values()
-        camera_key, pose_key, landmark_key = K(0), P(0), L(0)
+        pose_key, landmark_key = P(0), L(0)
         state.insert_point3(landmark_key, obj_point)
         state.insert_pose3(pose_key, pose)
         g = gtsam.NonlinearFactorGraph()

python/gtsam/tests/test_Cal3Unified.py

@@ -10,11 +10,12 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 import unittest
 
-import gtsam
 import numpy as np
 from gtsam.symbol_shorthand import K, L, P
 from gtsam.utils.test_case import GtsamTestCase
 
+import gtsam
+
 
 class TestCal3Unified(GtsamTestCase):
 
@@ -106,7 +107,7 @@ class TestCal3Unified(GtsamTestCase):
         state = gtsam.Values()
         measured = self.img_point
         noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
-        camera_key, pose_key, landmark_key = K(0), P(0), L(0)      
+        camera_key, pose_key, landmark_key = K(0), P(0), L(0)
         k = self.stereographic
         state.insert_cal3unified(camera_key, k)
         state.insert_pose3(pose_key, gtsam.Pose3())
@@ -141,7 +142,7 @@ class TestCal3Unified(GtsamTestCase):
         Dcal = np.zeros((2, 10), order='F')
         Dp = np.zeros((2, 2), order='F')
         camera.calibrate(img_point, Dcal, Dp)
-        
+
         self.gtsamAssertEquals(Dcal, np.array(
             [[ 0.,  0.,  0., -1.,  0.,  0.,  0.,  0.,  0.,  0.],
             [ 0.,  0.,  0.,  0., -1.,  0.,  0.,  0.,  0.,  0.]]))

python/gtsam/tests/test_FixedLagSmootherExample.py

@@ -14,7 +14,6 @@ import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-import gtsam_unstable
 
 
 class TestFixedLagSmootherExample(GtsamTestCase):

python/gtsam/tests/test_GaussianFactorGraph.py

@@ -14,11 +14,12 @@ from __future__ import print_function
 
 import unittest
 
-import gtsam
 import numpy as np
 from gtsam.symbol_shorthand import X
 from gtsam.utils.test_case import GtsamTestCase
 
+import gtsam
+
 
 def create_graph():
     """Create a basic linear factor graph for testing"""
@@ -40,6 +41,7 @@ def create_graph():
 
 class TestGaussianFactorGraph(GtsamTestCase):
     """Tests for Gaussian Factor Graphs."""
+
     def test_fg(self):
         """Test solving a linear factor graph"""
         graph, X = create_graph()
@@ -98,12 +100,11 @@ class TestGaussianFactorGraph(GtsamTestCase):
         bn = gfg.eliminateSequential(ordering)
         self.assertEqual(bn.size(), 3)
 
-        keyVector = []
+        keyVector = [keys[2]]
-        keyVector.append(keys[2])
+        ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
-        #TODO(Varun) Below code causes segfault in Debug config
+            gfg, keyVector)
-        # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector)
+        bn = gfg.eliminateSequential(ordering)
-        # bn = gfg.eliminateSequential(ordering)
+        self.assertEqual(bn.size(), 3)
-        # self.assertEqual(bn.size(), 3)
 
 
 if __name__ == '__main__':

python/gtsam/tests/test_KarcherMeanFactor.py

@@ -13,15 +13,15 @@ Author: Frank Dellaert
 
 import unittest
 
-import gtsam
 import numpy as np
-from gtsam import Rot3
 from gtsam.utils.test_case import GtsamTestCase
 
+import gtsam
+from gtsam import Rot3
+
 KEY = 0
 MODEL = gtsam.noiseModel.Unit.Create(3)
 
-
 # Rot3 version
 R = Rot3.Expmap(np.array([0.1, 0, 0]))
 
@@ -29,8 +29,10 @@ R = Rot3.Expmap(np.array([0.1, 0, 0]))
 class TestKarcherMean(GtsamTestCase):
 
     def test_find(self):
-        # Check that optimizing for Karcher mean (which minimizes Between distance)
+        """
-        # gets correct result.
+        Check that optimizing for Karcher mean (which minimizes Between distance)
+        gets correct result.
+        """
         rotations = [R, R.inverse()]
         expected = Rot3()
         actual = gtsam.FindKarcherMean(rotations)
@@ -69,8 +71,7 @@ class TestKarcherMean(GtsamTestCase):
         result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
         actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
         self.gtsamAssertEquals(expected, actual)
-        self.gtsamAssertEquals(
+        self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))
-            R12, result.atRot3(1).between(result.atRot3(2)))
 
 
 if __name__ == "__main__":

python/gtsam/tests/test_Pose2.py

@@ -12,12 +12,14 @@ import math
 import unittest
 
 import numpy as np
-from gtsam import Point2, Pose2
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import Point2, Point2Pairs, Pose2
+
 
 class TestPose2(GtsamTestCase):
     """Test selected Pose2 methods."""
+
     def test_adjoint(self) -> None:
         """Test adjoint method."""
         xi = np.array([1, 2, 3])
@@ -27,7 +29,7 @@ class TestPose2(GtsamTestCase):
 
     def test_transformTo(self):
         """Test transformTo method."""
-        pose = Pose2(2, 4, -math.pi/2)
+        pose = Pose2(2, 4, -math.pi / 2)
         actual = pose.transformTo(Point2(3, 2))
         expected = Point2(2, 1)
         self.gtsamAssertEquals(actual, expected, 1e-6)
@@ -41,7 +43,7 @@ class TestPose2(GtsamTestCase):
 
     def test_transformFrom(self):
         """Test transformFrom method."""
-        pose = Pose2(2, 4, -math.pi/2)
+        pose = Pose2(2, 4, -math.pi / 2)
         actual = pose.transformFrom(Point2(2, 1))
         expected = Point2(3, 2)
         self.gtsamAssertEquals(actual, expected, 1e-6)

python/gtsam/tests/test_Pose3.py

@@ -12,11 +12,12 @@ Author: Frank Dellaert, Duy Nguyen Ta
 import math
 import unittest
 
-import gtsam
 import numpy as np
-from gtsam import Point3, Pose3, Rot3
 from gtsam.utils.test_case import GtsamTestCase
 
+import gtsam
+from gtsam import Point3, Pose3, Rot3
+
 
 def numerical_derivative_pose(pose, method, delta=1e-5):
     jacobian = np.zeros((6, 6))

python/gtsam/tests/test_ShonanAveraging.py

@@ -12,12 +12,13 @@ Author: Frank Dellaert
 
 import unittest
 
-import gtsam
 import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+import gtsam
 from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2,
                    ShonanAveraging2, ShonanAveraging3,
                    ShonanAveragingParameters2, ShonanAveragingParameters3)
-from gtsam.utils.test_case import GtsamTestCase
 
 DEFAULT_PARAMS = ShonanAveragingParameters3(
     gtsam.LevenbergMarquardtParams.CeresDefaults()
@@ -139,7 +140,6 @@ class TestShonanAveraging(GtsamTestCase):
         self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
         result, _lambdaMin = shonan.run(initial, 3, 3)
         self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
-    
 
     def test_constructorBetweenFactorPose2s(self) -> None:
         """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
@@ -189,11 +189,11 @@ class TestShonanAveraging(GtsamTestCase):
 
         wRi_list = [result_values.atRot2(i) for i in range(num_images)]
         thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
-        
+
         # map all angles to [0,360)
         thetas_deg = thetas_deg % 360
         thetas_deg -= thetas_deg[0]
-        
+
         expected_thetas_deg = np.array([0.0, 90.0, 0.0])
         np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
 

python/gtsam/tests/test_Sim2.py

@@ -12,9 +12,10 @@ Author: John Lambert
 import unittest
 
 import numpy as np
-from gtsam import Pose2, Rot2, Similarity2
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import Pose2, Rot2, Similarity2
+
 
 class TestSim2(GtsamTestCase):
     """Test selected Sim2 methods."""
@@ -55,7 +56,7 @@ class TestSim2(GtsamTestCase):
             self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
 
     def test_align_poses_along_straight_line_gauge(self):
-        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+        """Test if Pose2 Align method can account for gauge ambiguity.
 
         Scenario:
            3 object poses
@@ -90,7 +91,7 @@ class TestSim2(GtsamTestCase):
             self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
 
     def test_align_poses_scaled_squares(self):
-        """Test if Align Pose2Pairs method can account for gauge ambiguity.
+        """Test if Align method can account for gauge ambiguity.
 
         Make sure a big and small square can be aligned.
         The u's represent a big square (10x10), and v's represents a small square (4x4).

python/gtsam/tests/test_Sim3.py

@@ -12,17 +12,18 @@ Author: John Lambert
 import unittest
 from typing import List, Optional
 
-import gtsam
 import numpy as np
-from gtsam import Point3, Pose3, Rot3, Similarity3
 from gtsam.utils.test_case import GtsamTestCase
 
+import gtsam
+from gtsam import Point3, Pose3, Rot3, Similarity3
+
 
 class TestSim3(GtsamTestCase):
     """Test selected Sim3 methods."""
 
     def test_align_poses_along_straight_line(self):
-        """Test Align Pose3Pairs method.
+        """Test Pose3 Align method.
 
         Scenario:
            3 object poses
@@ -57,7 +58,7 @@ class TestSim3(GtsamTestCase):
             self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
 
     def test_align_poses_along_straight_line_gauge(self):
-        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+        """Test if Pose3 Align method can account for gauge ambiguity.
 
         Scenario:
            3 object poses
@@ -92,7 +93,7 @@ class TestSim3(GtsamTestCase):
             self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
 
     def test_align_poses_scaled_squares(self):
-        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+        """Test if Pose3 Align method can account for gauge ambiguity.
 
         Make sure a big and small square can be aligned.
         The u's represent a big square (10x10), and v's represents a small square (4x4).

python/gtsam/tests/test_Triangulation.py

@@ -12,13 +12,14 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
 import unittest
 from typing import Iterable, List, Optional, Tuple, Union
 
-import gtsam
 import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+import gtsam
 from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
                    CameraSetCal3Bundler, PinholeCameraCal3_S2,
                    PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3,
                    TriangulationParameters, TriangulationResult)
-from gtsam.utils.test_case import GtsamTestCase
 
 UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
 

python/gtsam/tests/test_backwards_compatibility.py

@@ -0,0 +1,897 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests to ensure backwards compatibility of the Python wrapper.
+Author: Varun Agrawal
+"""
+import unittest
+from typing import Iterable, List, Optional, Tuple, Union
+
+import numpy as np
+from gtsam.gtsfm import Keypoints
+from gtsam.symbol_shorthand import X
+from gtsam.utils.test_case import GtsamTestCase
+
+import gtsam
+from gtsam import (BetweenFactorPose2, Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
+                   CameraSetCal3Bundler, IndexPair, LevenbergMarquardtParams,
+                   PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point2,
+                   Point2Pairs, Point3, Pose2, Pose2Pairs, Pose3, Rot2, Rot3,
+                   SfmTrack2d, ShonanAveraging2, ShonanAveragingParameters2,
+                   Similarity2, Similarity3, TriangulationParameters,
+                   TriangulationResult)
+
+UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
+
+
+class TestBackwardsCompatibility(GtsamTestCase):
+    """Tests for the backwards compatibility for the Python wrapper."""
+
+    def setUp(self):
+        """Setup test fixtures"""
+        p1 = [-1.0, 0.0, -1.0]
+        p2 = [1.0, 0.0, -1.0]
+        q1 = Rot3(1.0, 0.0, 0.0, 0.0)
+        q2 = Rot3(1.0, 0.0, 0.0, 0.0)
+        pose1 = Pose3(q1, p1)
+        pose2 = Pose3(q2, p2)
+        camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
+        camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
+        self.origin = np.array([0.0, 0.0, 0.0])
+        self.poses = gtsam.Pose3Vector([pose1, pose2])
+
+        self.fisheye_cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2])
+        self.fisheye_measurements = gtsam.Point2Vector(
+            [k.project(self.origin) for k in self.fisheye_cameras])
+
+        fx, fy, s, u0, v0 = 2, 2, 0, 0, 0
+        k1, k2, p1, p2 = 0, 0, 0, 0
+        xi = 1
+        self.stereographic = gtsam.Cal3Unified(fx, fy, s, u0, v0, k1, k2, p1,
+                                               p2, xi)
+        camera1 = gtsam.PinholeCameraCal3Unified(pose1, self.stereographic)
+        camera2 = gtsam.PinholeCameraCal3Unified(pose2, self.stereographic)
+        self.unified_cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
+        self.unified_measurements = gtsam.Point2Vector(
+            [k.project(self.origin) for k in self.unified_cameras])
+
+        ## Set up two camera poses
+        # Looking along X-axis, 1 meter above ground plane (x-y)
+        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)))
+        # twoPoses
+        self.triangulation_poses = gtsam.Pose3Vector()
+        self.triangulation_poses.append(pose1)
+        self.triangulation_poses.append(pose2)
+
+        # landmark ~5 meters infront of camera
+        self.landmark = Point3(5, 0.5, 1.2)
+
+    def test_Cal3Fisheye_triangulation_rectify(self):
+        """
+        Estimate spatial point from image measurements using
+        rectification from a Cal3Fisheye camera model.
+        """
+        rectified = gtsam.Point2Vector([
+            k.calibration().calibrate(pt)
+            for k, pt in zip(self.fisheye_cameras, self.fisheye_measurements)
+        ])
+        shared_cal = gtsam.Cal3_S2()
+        triangulated = gtsam.triangulatePoint3(self.poses,
+                                               shared_cal,
+                                               rectified,
+                                               rank_tol=1e-9,
+                                               optimize=False)
+        self.gtsamAssertEquals(triangulated, self.origin)
+
+    def test_Cal3Unified_triangulation_rectify(self):
+        """
+        Estimate spatial point from image measurements using
+        rectification from a Cal3Unified camera model.
+        """
+        rectified = gtsam.Point2Vector([
+            k.calibration().calibrate(pt)
+            for k, pt in zip(self.unified_cameras, self.unified_measurements)
+        ])
+        shared_cal = gtsam.Cal3_S2()
+        triangulated = gtsam.triangulatePoint3(self.poses,
+                                               shared_cal,
+                                               rectified,
+                                               rank_tol=1e-9,
+                                               optimize=False)
+        self.gtsamAssertEquals(triangulated, self.origin)
+
+    def test_track_generation(self) -> None:
+        """Ensures that DSF generates three tracks from measurements
+        in 3 images (H=200,W=400)."""
+        kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]]))
+        kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]]))
+        kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]]))
+
+        keypoints_list = gtsam.KeypointsVector()
+        keypoints_list.append(kps_i0)
+        keypoints_list.append(kps_i1)
+        keypoints_list.append(kps_i2)
+
+        # For each image pair (i1,i2), we provide a (K,2) matrix
+        # of corresponding image indices (k1,k2).
+        matches_dict = gtsam.MatchIndicesMap()
+        matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]])
+        matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]])
+
+        tracks = gtsam.gtsfm.tracksFromPairwiseMatches(
+            matches_dict,
+            keypoints_list,
+            verbose=False,
+        )
+        assert len(tracks) == 3
+
+        # Verify track 0.
+        track0 = tracks[0]
+        assert track0.numberMeasurements() == 2
+        np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20))
+        np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60))
+        assert track0.measurements[0][0] == 0
+        assert track0.measurements[1][0] == 1
+        np.testing.assert_allclose(
+            track0.measurementMatrix(),
+            [
+                [10, 20],
+                [50, 60],
+            ],
+        )
+        np.testing.assert_allclose(track0.indexVector(), [0, 1])
+
+        # Verify track 1.
+        track1 = tracks[1]
+        np.testing.assert_allclose(
+            track1.measurementMatrix(),
+            [
+                [30, 40],
+                [70, 80],
+                [130, 140],
+            ],
+        )
+        np.testing.assert_allclose(track1.indexVector(), [0, 1, 2])
+
+        # Verify track 2.
+        track2 = tracks[2]
+        np.testing.assert_allclose(
+            track2.measurementMatrix(),
+            [
+                [90, 100],
+                [110, 120],
+            ],
+        )
+        np.testing.assert_allclose(track2.indexVector(), [1, 2])
+
+    def test_sfm_track_2d_constructor(self) -> None:
+        """Test construction of 2D SfM track."""
+        measurements = gtsam.SfmMeasurementVector()
+        measurements.append((0, Point2(10, 20)))
+        track = SfmTrack2d(measurements=measurements)
+        track.measurement(0)
+        assert track.numberMeasurements() == 1
+
+    def test_FixedLagSmootherExample(self):
+        '''
+        Simple test that checks for equality between C++ example
+        file and the Python implementation. See
+        gtsam_unstable/examples/FixedLagSmootherExample.cpp
+        '''
+        # Define a batch fixed lag smoother, which uses
+        # Levenberg-Marquardt to perform the nonlinear optimization
+        lag = 2.0
+        smoother_batch = gtsam.BatchFixedLagSmoother(lag)
+
+        # Create containers to store the factors and linearization points
+        # that will be sent to the smoothers
+        new_factors = gtsam.NonlinearFactorGraph()
+        new_values = gtsam.Values()
+        new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap()
+
+        # Create  a prior on the first pose, placing it at the origin
+        prior_mean = Pose2(0, 0, 0)
+        prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
+            np.array([0.3, 0.3, 0.1]))
+        X1 = 0
+        new_factors.push_back(
+            gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
+        new_values.insert(X1, prior_mean)
+        new_timestamps.insert((X1, 0.0))
+
+        delta_time = 0.25
+        time = 0.25
+
+        i = 0
+
+        ground_truth = [
+            Pose2(0.995821, 0.0231012, 0.0300001),
+            Pose2(1.49284, 0.0457247, 0.045),
+            Pose2(1.98981, 0.0758879, 0.06),
+            Pose2(2.48627, 0.113502, 0.075),
+            Pose2(2.98211, 0.158558, 0.09),
+            Pose2(3.47722, 0.211047, 0.105),
+            Pose2(3.97149, 0.270956, 0.12),
+            Pose2(4.4648, 0.338272, 0.135),
+            Pose2(4.95705, 0.41298, 0.15),
+            Pose2(5.44812, 0.495063, 0.165),
+            Pose2(5.9379, 0.584503, 0.18),
+        ]
+
+        # Iterates from 0.25s to 3.0s, adding 0.25s each loop
+        # In each iteration, the agent moves at a constant speed
+        # and its two odometers measure the change. The smoothed
+        # result is then compared to the ground truth
+        while time <= 3.0:
+            previous_key = int(1000 * (time - delta_time))
+            current_key = int(1000 * time)
+
+            # assign current key to the current timestamp
+            new_timestamps.insert((current_key, time))
+
+            # Add a guess for this pose to the new values
+            # Assume that the robot moves at 2 m/s. Position is time[s] *
+            # 2[m/s]
+            current_pose = Pose2(time * 2, 0, 0)
+            new_values.insert(current_key, current_pose)
+
+            # Add odometry factors from two different sources with different
+            # error stats
+            odometry_measurement_1 = Pose2(0.61, -0.08, 0.02)
+            odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
+                np.array([0.1, 0.1, 0.05]))
+            new_factors.push_back(
+                gtsam.BetweenFactorPose2(previous_key, current_key,
+                                         odometry_measurement_1,
+                                         odometry_noise_1))
+
+            odometry_measurement_2 = Pose2(0.47, 0.03, 0.01)
+            odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
+                np.array([0.05, 0.05, 0.05]))
+            new_factors.push_back(
+                gtsam.BetweenFactorPose2(previous_key, current_key,
+                                         odometry_measurement_2,
+                                         odometry_noise_2))
+
+            # Update the smoothers with the new factors. In this case,
+            # one iteration must pass for Levenberg-Marquardt to accurately
+            # estimate
+            if time >= 0.50:
+                smoother_batch.update(new_factors, new_values, new_timestamps)
+
+                estimate = smoother_batch.calculateEstimatePose2(current_key)
+                self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+                i += 1
+
+                new_timestamps.clear()
+                new_values.clear()
+                new_factors.resize(0)
+
+            time += delta_time
+
+    def test_ordering(self):
+        """Test ordering"""
+        gfg = gtsam.GaussianFactorGraph()
+
+        x0 = X(0)
+        x1 = X(1)
+        x2 = X(2)
+
+        BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
+        PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
+
+        gfg.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
+        gfg.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE)
+        gfg.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
+
+        keys = (x0, x1, x2)
+        ordering = gtsam.Ordering()
+        for key in keys[::-1]:
+            ordering.push_back(key)
+
+        bn = gfg.eliminateSequential(ordering)
+        self.assertEqual(bn.size(), 3)
+
+        keyVector = gtsam.KeyVector()
+        keyVector.append(keys[2])
+        ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
+            gfg, keyVector)
+        bn = gfg.eliminateSequential(ordering)
+        self.assertEqual(bn.size(), 3)
+
+    def test_find(self):
+        """
+        Check that optimizing for Karcher mean (which minimizes Between distance)
+        gets correct result.
+        """
+        R = Rot3.Expmap(np.array([0.1, 0, 0]))
+
+        rotations = gtsam.Rot3Vector([R, R.inverse()])
+        expected = Rot3()
+        actual = gtsam.FindKarcherMean(rotations)
+        self.gtsamAssertEquals(expected, actual)
+
+    def test_find_karcher_mean_identity(self):
+        """Averaging 3 identity rotations should yield the identity."""
+        a1Rb1 = Rot3()
+        a2Rb2 = Rot3()
+        a3Rb3 = Rot3()
+
+        aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
+        aRb_expected = Rot3()
+
+        aRb = gtsam.FindKarcherMean(aRb_list)
+        self.gtsamAssertEquals(aRb, aRb_expected)
+
+    def test_factor(self):
+        """Check that the InnerConstraint factor leaves the mean unchanged."""
+        # Make a graph with two variables, one between, and one InnerConstraint
+        # The optimal result should satisfy the between, while moving the other
+        # variable to make the mean the same as before.
+        # Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
+        # R*R*R, i.e. geodesic length is 3 rather than 2.
+        R = Rot3.Expmap(np.array([0.1, 0, 0]))
+        MODEL = gtsam.noiseModel.Unit.Create(3)
+
+        graph = gtsam.NonlinearFactorGraph()
+        R12 = R.compose(R.compose(R))
+        graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
+        keys = gtsam.KeyVector()
+        keys.append(1)
+        keys.append(2)
+        graph.add(gtsam.KarcherMeanFactorRot3(keys))
+
+        initial = gtsam.Values()
+        initial.insert(1, R.inverse())
+        initial.insert(2, R)
+        expected = Rot3()
+
+        result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
+        actual = gtsam.FindKarcherMean(
+            gtsam.Rot3Vector([result.atRot3(1),
+                              result.atRot3(2)]))
+        self.gtsamAssertEquals(expected, actual)
+        self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))
+
+    def test_align(self) -> None:
+        """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.
+
+        Two point clouds represent horseshoe-shapes of the same size, just rotated and translated:
+
+                |  X---X
+                |  |
+                |  X---X
+        ------------------
+                |
+                |
+              O | O
+              | | |
+              O---O
+        """
+        pts_a = [
+            Point2(1, -3),
+            Point2(1, -5),
+            Point2(-1, -5),
+            Point2(-1, -3),
+        ]
+        pts_b = [
+            Point2(3, 1),
+            Point2(1, 1),
+            Point2(1, 3),
+            Point2(3, 3),
+        ]
+
+        ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
+        aTb = Pose2.Align(ab_pairs)
+        self.assertIsNotNone(aTb)
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            np.testing.assert_allclose(pt_a, pt_a_)
+
+        # Matrix version
+        A = np.array(pts_a).T
+        B = np.array(pts_b).T
+        aTb = Pose2.Align(A, B)
+        self.assertIsNotNone(aTb)
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            np.testing.assert_allclose(pt_a, pt_a_)
+
+    def test_align_squares(self):
+        """Test if Align method can align 2 squares."""
+        square = np.array([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0]],
+                          float).T
+        sTt = Pose3(Rot3.Rodrigues(0, 0, -np.pi), gtsam.Point3(2, 4, 0))
+        transformed = sTt.transformTo(square)
+
+        st_pairs = gtsam.Point3Pairs()
+        for j in range(4):
+            st_pairs.append((square[:, j], transformed[:, j]))
+
+        # Recover the transformation sTt
+        estimated_sTt = Pose3.Align(st_pairs)
+        self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
+
+        # Matrix version
+        estimated_sTt = Pose3.Align(square, transformed)
+        self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
+
+    def test_constructorBetweenFactorPose2s(self) -> None:
+        """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
+
+        GT pose graph:
+
+           | cam 1 = (0,4)
+         --o
+           | .
+           .   .
+           .     .
+           |       |
+           o-- ... o--
+        cam 0       cam 2 = (4,0)
+          (0,0)
+        """
+        num_images = 3
+
+        wTi_list = [
+            Pose2(Rot2.fromDegrees(0), np.array([0, 0])),
+            Pose2(Rot2.fromDegrees(90), np.array([0, 4])),
+            Pose2(Rot2.fromDegrees(0), np.array([4, 0])),
+        ]
+
+        edges = [(0, 1), (1, 2), (0, 2)]
+        i2Ri1_dict = {(i1, i2):
+                      wTi_list[i2].inverse().compose(wTi_list[i1]).rotation()
+                      for (i1, i2) in edges}
+
+        lm_params = LevenbergMarquardtParams.CeresDefaults()
+        shonan_params = ShonanAveragingParameters2(lm_params)
+        shonan_params.setUseHuber(False)
+        shonan_params.setCertifyOptimality(True)
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        between_factors = gtsam.BetweenFactorPose2s()
+        for (i1, i2), i2Ri1 in i2Ri1_dict.items():
+            i2Ti1 = Pose2(i2Ri1, np.zeros(2))
+            between_factors.append(
+                BetweenFactorPose2(i2, i1, i2Ti1, noise_model))
+
+        obj = ShonanAveraging2(between_factors, shonan_params)
+        initial = obj.initializeRandomly()
+        result_values, _ = obj.run(initial, min_p=2, max_p=100)
+
+        wRi_list = [result_values.atRot2(i) for i in range(num_images)]
+        thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
+
+        # map all angles to [0,360)
+        thetas_deg = thetas_deg % 360
+        thetas_deg -= thetas_deg[0]
+
+        expected_thetas_deg = np.array([0.0, 90.0, 0.0])
+        np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
+
+    def test_align_poses2_along_straight_line(self) -> None:
+        """Test Align of list of Pose2Pair.
+
+        Scenario:
+           3 object poses
+           same scale (no gauge ambiguity)
+           world frame has poses rotated about 180 degrees.
+           world and egovehicle frame translated by 15 meters w.r.t. each other
+        """
+        R180 = Rot2.fromDegrees(180)
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose2(Rot2(), np.array([5, 0]))
+        eTo1 = Pose2(Rot2(), np.array([10, 0]))
+        eTo2 = Pose2(Rot2(), np.array([15, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world "w" frame)
+        wTo0 = Pose2(R180, np.array([-10, 0]))
+        wTo1 = Pose2(R180, np.array([-5, 0]))
+        wTo2 = Pose2(R180, np.array([0, 0]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity2.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses2_along_straight_line_gauge(self):
+        """Test if Align Pose2Pairs method can account for gauge ambiguity.
+
+        Scenario:
+           3 object poses
+           with gauge ambiguity (2x scale)
+           world frame has poses rotated by 90 degrees.
+           world and egovehicle frame translated by 11 meters w.r.t. each other
+        """
+        R90 = Rot2.fromDegrees(90)
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose2(Rot2(), np.array([1, 0]))
+        eTo1 = Pose2(Rot2(), np.array([2, 0]))
+        eTo2 = Pose2(Rot2(), np.array([4, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world/city "w" frame)
+        wTo0 = Pose2(R90, np.array([0, 12]))
+        wTo1 = Pose2(R90, np.array([0, 14]))
+        wTo2 = Pose2(R90, np.array([0, 18]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity2.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses2_scaled_squares(self):
+        """Test if Align Pose2Pairs method can account for gauge ambiguity.
+
+        Make sure a big and small square can be aligned.
+        The u's represent a big square (10x10), and v's represents a small square (4x4).
+
+        Scenario:
+           4 object poses
+           with gauge ambiguity (2.5x scale)
+        """
+        # 0, 90, 180, and 270 degrees yaw
+        R0 = Rot2.fromDegrees(0)
+        R90 = Rot2.fromDegrees(90)
+        R180 = Rot2.fromDegrees(180)
+        R270 = Rot2.fromDegrees(270)
+
+        aTi0 = Pose2(R0, np.array([2, 3]))
+        aTi1 = Pose2(R90, np.array([12, 3]))
+        aTi2 = Pose2(R180, np.array([12, 13]))
+        aTi3 = Pose2(R270, np.array([2, 13]))
+
+        aTi_list = [aTi0, aTi1, aTi2, aTi3]
+
+        bTi0 = Pose2(R0, np.array([4, 3]))
+        bTi1 = Pose2(R90, np.array([8, 3]))
+        bTi2 = Pose2(R180, np.array([8, 7]))
+        bTi3 = Pose2(R270, np.array([4, 7]))
+
+        bTi_list = [bTi0, bTi1, bTi2, bTi3]
+
+        ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        aSb = Similarity2.Align(ab_pairs)
+
+        for aTi, bTi in zip(aTi_list, bTi_list):
+            self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
+
+    def test_align_poses3_along_straight_line(self):
+        """Test Align Pose3Pairs method.
+
+        Scenario:
+           3 object poses
+           same scale (no gauge ambiguity)
+           world frame has poses rotated about x-axis (90 degree roll)
+           world and egovehicle frame translated by 15 meters w.r.t. each other
+        """
+        Rx90 = Rot3.Rx(np.deg2rad(90))
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose3(Rot3(), np.array([5, 0, 0]))
+        eTo1 = Pose3(Rot3(), np.array([10, 0, 0]))
+        eTo2 = Pose3(Rot3(), np.array([15, 0, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world/city "w" frame)
+        wTo0 = Pose3(Rx90, np.array([-10, 0, 0]))
+        wTo1 = Pose3(Rx90, np.array([-5, 0, 0]))
+        wTo2 = Pose3(Rx90, np.array([0, 0, 0]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity3.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses3_along_straight_line_gauge(self):
+        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+
+        Scenario:
+           3 object poses
+           with gauge ambiguity (2x scale)
+           world frame has poses rotated about z-axis (90 degree yaw)
+           world and egovehicle frame translated by 11 meters w.r.t. each other
+        """
+        Rz90 = Rot3.Rz(np.deg2rad(90))
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose3(Rot3(), np.array([1, 0, 0]))
+        eTo1 = Pose3(Rot3(), np.array([2, 0, 0]))
+        eTo2 = Pose3(Rot3(), np.array([4, 0, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world/city "w" frame)
+        wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
+        wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
+        wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity3.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses3_scaled_squares(self):
+        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+
+        Make sure a big and small square can be aligned.
+        The u's represent a big square (10x10), and v's represents a small square (4x4).
+
+        Scenario:
+           4 object poses
+           with gauge ambiguity (2.5x scale)
+        """
+        # 0, 90, 180, and 270 degrees yaw
+        R0 = Rot3.Rz(np.deg2rad(0))
+        R90 = Rot3.Rz(np.deg2rad(90))
+        R180 = Rot3.Rz(np.deg2rad(180))
+        R270 = Rot3.Rz(np.deg2rad(270))
+
+        aTi0 = Pose3(R0, np.array([2, 3, 0]))
+        aTi1 = Pose3(R90, np.array([12, 3, 0]))
+        aTi2 = Pose3(R180, np.array([12, 13, 0]))
+        aTi3 = Pose3(R270, np.array([2, 13, 0]))
+
+        aTi_list = [aTi0, aTi1, aTi2, aTi3]
+
+        bTi0 = Pose3(R0, np.array([4, 3, 0]))
+        bTi1 = Pose3(R90, np.array([8, 3, 0]))
+        bTi2 = Pose3(R180, np.array([8, 7, 0]))
+        bTi3 = Pose3(R270, np.array([4, 7, 0]))
+
+        bTi_list = [bTi0, bTi1, bTi2, bTi3]
+
+        ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        aSb = Similarity3.Align(ab_pairs)
+
+        for aTi, bTi in zip(aTi_list, bTi_list):
+            self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
+
+    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[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2,
+                                   List[Cal3Bundler], List[Cal3_S2]]]:
+        """
+        Generate vector of measurements for given calibration and camera model.
+
+        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
+        """
+        if camera_set is not None:
+            cameras = camera_set()
+        else:
+            cameras = []
+        measurements = gtsam.Point2Vector()
+
+        for k, pose in zip(cal_params, self.triangulation_poses):
+            K = calibration(*k)
+            camera = camera_model(pose, K)
+            cameras.append(camera)
+            z = camera.project(self.landmark)
+            measurements.append(z)
+
+        return measurements, cameras
+
+    def test_TriangulationExample(self) -> None:
+        """Tests triangulation with shared Cal3_S2 calibration"""
+        # Some common constants
+        sharedCal = (1500, 1200, 0, 640, 480)
+
+        measurements, _ = self.generate_measurements(
+            calibration=Cal3_S2,
+            camera_model=PinholeCameraCal3_S2,
+            cal_params=(sharedCal, sharedCal))
+
+        triangulated_landmark = gtsam.triangulatePoint3(
+            self.triangulation_poses,
+            Cal3_S2(sharedCal),
+            measurements,
+            rank_tol=1e-9,
+            optimize=True)
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
+
+        # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+        measurements_noisy = gtsam.Point2Vector()
+        measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
+        measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
+
+        triangulated_landmark = gtsam.triangulatePoint3(
+            self.triangulation_poses,
+            Cal3_S2(sharedCal),
+            measurements_noisy,
+            rank_tol=1e-9,
+            optimize=True)
+
+        self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
+
+    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 = gtsam.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))
+
+    def test_outliers_and_far_landmarks(self) -> None:
+        """Check safe triangulation function."""
+        pose1, pose2 = self.poses
+
+        K1 = Cal3_S2(1500, 1200, 0, 640, 480)
+        # create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+        camera1 = PinholeCameraCal3_S2(pose1, K1)
+
+        # create second camera 1 meter to the right of first camera
+        K2 = Cal3_S2(1600, 1300, 0, 650, 440)
+        camera2 = PinholeCameraCal3_S2(pose2, K2)
+
+        # 1. Project two landmarks into two cameras and triangulate
+        z1 = camera1.project(self.landmark)
+        z2 = camera2.project(self.landmark)
+
+        cameras = CameraSetCal3_S2()
+        cameras.append(camera1)
+        cameras.append(camera2)
+
+        measurements = gtsam.Point2Vector()
+        measurements.append(z1)
+        measurements.append(z2)
+
+        landmarkDistanceThreshold = 10  # landmark is closer than that
+        # all default except landmarkDistanceThreshold:
+        params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
+        actual: TriangulationResult = gtsam.triangulateSafe(
+            cameras, measurements, params)
+        self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
+        self.assertTrue(actual.valid())
+
+        landmarkDistanceThreshold = 4  # landmark is farther than that
+        params2 = TriangulationParameters(1.0, False,
+                                          landmarkDistanceThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params2)
+        self.assertTrue(actual.farPoint())
+
+        # 3. Add a slightly rotated third camera above with a wrong measurement
+        # (OUTLIER)
+        pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
+        K3 = Cal3_S2(700, 500, 0, 640, 480)
+        camera3 = PinholeCameraCal3_S2(pose3, K3)
+        z3 = camera3.project(self.landmark)
+
+        cameras.append(camera3)
+        measurements.append(z3 + Point2(10, -10))
+
+        landmarkDistanceThreshold = 10  # landmark is closer than that
+        outlierThreshold = 100  # loose, the outlier is going to pass
+        params3 = TriangulationParameters(1.0, False,
+                                          landmarkDistanceThreshold,
+                                          outlierThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params3)
+        self.assertTrue(actual.valid())
+
+        # now set stricter threshold for outlier rejection
+        outlierThreshold = 5  # tighter, the outlier is not going to pass
+        params4 = TriangulationParameters(1.0, False,
+                                          landmarkDistanceThreshold,
+                                          outlierThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params4)
+        self.assertTrue(actual.outlier())
+
+
+if __name__ == "__main__":
+    unittest.main()