diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 399cf019d..ec50692c3 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -9,6 +9,7 @@ Test Triangulation Author: Frank Dellaert & Fan Jiang (Python) """ import unittest +from typing import Union import numpy as np @@ -20,14 +21,16 @@ from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, from gtsam.utils.test_case import GtsamTestCase -class TestVisualISAMExample(GtsamTestCase): +UPRIGHT = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) + + +class TestTriangulationExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """ def setUp(self): """ 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,7 +42,7 @@ 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, cal_params, camera_set=None): """ Generate vector of measurements for given calibration and camera model. @@ -48,6 +51,7 @@ class TestVisualISAMExample(GtsamTestCase): 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,7 +70,7 @@ class TestVisualISAMExample(GtsamTestCase): return measurements, cameras - def test_TriangulationExample(self): + def test_TriangulationExample(self) -> None: """ Tests triangulation with shared Cal3_S2 calibration""" # Some common constants sharedCal = (1500, 1200, 0, 640, 480) @@ -95,7 +99,7 @@ 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 """ # two camera parameters K1 = (1500, 1200, 0, 640, 480) @@ -112,7 +116,7 @@ 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""" # two camera parameters K1 = (1500, 0, 0, 640, 480) @@ -128,7 +132,53 @@ class TestVisualISAMExample(GtsamTestCase): rank_tol=1e-9, 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, -.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 = [pose1, pose2, pose3] + measurements = Point2Vector([z1, z2, z3]) + + # noise free, so should give exactly the landmark + actual = gtsam.triangulatePoint3(poses, sharedCal, measurements) + self.assert_equal(landmark, actual, 1e-2) + + # Add outlier + measurements.at(0) += Point2(100, 120) # very large pixel noise! + + # now estimate does not match landmark + actual2 = gtsam.triangulatePoint3(poses, sharedCal, measurements) + # DLT is surprisingly robust, but still off (actual error is around 0.26m) + self.assertTrue( (landmark - actual2).norm() >= 0.2) + self.assertTrue( (landmark - actual2).norm() <= 0.5) + + # Again with nonlinear optimization + actual3 = gtsam.triangulatePoint3(poses, sharedCal, measurements, 1e-9, true) + # result from nonlinear (but non-robust optimization) is close to DLT and still off + self.assertEqual(actual2, actual3, 0.1) + + # Again with nonlinear optimization, this time with robust loss + model = noiseModel.Robust.Create(noiseModel.mEstimator.Huber.Create(1.345), noiseModel.Unit.Create(2)) + actual4 = gtsam.triangulatePoint3(poses, sharedCal, measurements, 1e-9, true, model) + # using the Huber loss we now have a quite small error!! nice! + self.assertEqual(landmark, actual4, 0.05) + if __name__ == "__main__": unittest.main()