Added safe triangulation tests

python/gtsam/tests/test_Triangulation.py

@@ -8,26 +8,17 @@ See LICENSE for the license information
 Test Triangulation
 Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
 """
+# pylint: disable=no-name-in-module, invalid-name, no-member
 import unittest
 from typing import Iterable, List, Optional, Tuple, Union
-
 import numpy as np
 
 import gtsam
-from gtsam import (
+from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
-    Cal3_S2,
+                   CameraSetCal3Bundler, PinholeCameraCal3_S2,
-    Cal3Bundler,
+                   PinholeCameraCal3Bundler, Point2, Point2Vector, Point3,
-    CameraSetCal3_S2,
+                   Pose3, Pose3Vector, Rot3, TriangulationParameters,
-    CameraSetCal3Bundler,
+                   TriangulationResult)
-    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)
@@ -218,6 +209,68 @@ class TestTriangulationExample(GtsamTestCase):
         # 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()
+        measurements = Point2Vector()
+
+        cameras.push_back(camera1)
+        cameras.push_back(camera2)
+        measurements.push_back(z1)
+        measurements.push_back(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, 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.push_back(camera3)
+        measurements.push_back(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()