diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index ec50692c3..1a304bdc8 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -6,29 +6,39 @@ 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 Union +from typing import Optional, Union import numpy as np import gtsam -from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, - CameraSetCal3Bundler, PinholeCameraCal3_S2, - PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3, - Pose3Vector, Rot3) +from gtsam import ( + Cal3_S2, + Cal3Bundler, + 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., -np.pi / 2) +UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) class TestTriangulationExample(GtsamTestCase): - """ Tests for triangulation with shared and individual calibrations """ + """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) pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) @@ -42,16 +52,22 @@ class TestTriangulationExample(GtsamTestCase): # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) - def generate_measurements(self, calibration: Union[Cal3Bundler, Cal3_S2], camera_model, cal_params, camera_set=None): + def generate_measurements( + self, + calibration: Union[Cal3Bundler, Cal3_S2], + camera_model, + cal_params, + camera_set: Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] = None, + ): """ 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 """ @@ -71,19 +87,15 @@ class TestTriangulationExample(GtsamTestCase): return measurements, cameras 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, - PinholeCameraCal3_S2, - (sharedCal, sharedCal)) + measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, (sharedCal, sharedCal)) - triangulated_landmark = gtsam.triangulatePoint3(self.poses, - Cal3_S2(sharedCal), - measurements, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3( + self.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) @@ -91,59 +103,49 @@ class TestTriangulationExample(GtsamTestCase): 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.poses, - Cal3_S2(sharedCal), - measurements_noisy, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3( + self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True + ) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) 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, - PinholeCameraCal3_S2, - (K1, K2), - camera_set=CameraSetCal3_S2) + measurements, cameras = self.generate_measurements( + Cal3_S2, PinholeCameraCal3_S2, (K1, K2), camera_set=CameraSetCal3_S2 + ) - triangulated_landmark = gtsam.triangulatePoint3(cameras, - measurements, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) 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, - PinholeCameraCal3Bundler, - (K1, K2), - camera_set=CameraSetCal3Bundler) + measurements, cameras = self.generate_measurements( + Cal3Bundler, PinholeCameraCal3Bundler, (K1, K2), camera_set=CameraSetCal3Bundler + ) - triangulated_landmark = gtsam.triangulatePoint3(cameras, - measurements, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, 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)) - + 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) @@ -151,34 +153,36 @@ class TestTriangulationExample(GtsamTestCase): z1: Point2 = camera1.project(landmark) z2: Point2 = camera2.project(landmark) z3: Point2 = camera3.project(landmark) - - poses = [pose1, pose2, pose3] + + 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) - self.assert_equal(landmark, actual, 1e-2) - + actual = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=False) + self.assertTrue(np.allclose(landmark, actual, atol=1e-2)) + # Add outlier - measurements.at(0) += Point2(100, 120) # very large pixel noise! - + measurements[0] += Point2(100, 120) # very large pixel noise! + # now estimate does not match landmark - actual2 = gtsam.triangulatePoint3(poses, sharedCal, measurements) + 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( (landmark - actual2).norm() >= 0.2) - self.assertTrue( (landmark - actual2).norm() <= 0.5) - + 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, 1e-9, true) + 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.assertEqual(actual2, actual3, 0.1) - + self.assertTrue(np.allclose(actual2, actual3, atol=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) + 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.assertEqual(landmark, actual4, 0.05) - + self.assertTrue(np.allclose(landmark, actual4, atol=0.05)) + if __name__ == "__main__": unittest.main()