From b60ca0c10724ea3c85ca04575295d38c1947418c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 09:57:59 -0700 Subject: [PATCH 1/7] Update test_Triangulation.py --- python/gtsam/tests/test_Triangulation.py | 64 +++++++++++++++++++++--- 1 file changed, 57 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 399cf019d0..ec50692c3f 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -9,6 +9,7 @@ Author: Frank Dellaert & Fan Jiang (Python) """ import unittest +from typing import Union import numpy as np @@ -20,14 +21,16 @@ 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 @@ def setUp(self): # 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 @@ def generate_measurements(self, calibration, camera_model, cal_params, camera_se 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 @@ def generate_measurements(self, calibration, camera_model, cal_params, camera_se 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 @@ def test_TriangulationExample(self): 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 @@ def test_distinct_Ks(self): 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 @@ def test_distinct_Ks_Bundler(self): 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() From d66b1d7a849faff591e0f39b34af80aec85db715 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 13:01:23 -0500 Subject: [PATCH 2/7] fix syntax errors --- python/gtsam/tests/test_Triangulation.py | 138 ++++++++++++----------- 1 file changed, 71 insertions(+), 67 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index ec50692c3f..1a304bdc88 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -6,29 +6,39 @@ 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 @@ def setUp(self): # 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 @@ def generate_measurements(self, calibration: Union[Cal3Bundler, Cal3_S2], camera 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 @@ def test_TriangulationExample(self) -> None: 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 @@ def test_triangulation_robust_three_poses(self) -> None: 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() From f009a14151be468772378011d938fc76bb8efa84 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 13:24:08 -0500 Subject: [PATCH 3/7] add missing type hint --- python/gtsam/tests/test_Triangulation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 1a304bdc88..9eed71ae6e 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -55,7 +55,7 @@ def setUp(self): def generate_measurements( self, calibration: Union[Cal3Bundler, Cal3_S2], - camera_model, + camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2], cal_params, camera_set: Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] = None, ): From e2993eafe61c08bd2d1ed758ed1e3004b1882b5c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 13:41:54 -0500 Subject: [PATCH 4/7] yapf pep8 reformat --- python/gtsam/tests/test_Triangulation.py | 87 +++++++++++++++++------- 1 file changed, 61 insertions(+), 26 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 9eed71ae6e..c3b9870d89 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -9,7 +9,7 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert """ import unittest -from typing import Optional, Union +from typing import Iterable, Optional, Union import numpy as np @@ -30,7 +30,6 @@ ) from gtsam.utils.test_case import GtsamTestCase - UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) @@ -56,9 +55,11 @@ def generate_measurements( self, calibration: Union[Cal3Bundler, Cal3_S2], camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2], - cal_params, - camera_set: Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] = None, - ): + 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. @@ -91,11 +92,16 @@ def test_TriangulationExample(self) -> None: # Some common constants sharedCal = (1500, 1200, 0, 640, 480) - measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, (sharedCal, sharedCal)) + measurements, _ = self.generate_measurements( + calibration=Cal3_S2, + camera_model=PinholeCameraCal3_S2, + cal_params=(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) @@ -103,9 +109,11 @@ def test_TriangulationExample(self) -> None: 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) @@ -116,10 +124,15 @@ def test_distinct_Ks(self) -> None: K2 = (1600, 1300, 0, 650, 440) 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) + calibration=Cal3_S2, + camera_model=PinholeCameraCal3_S2, + cal_params=(K1, K2), + camera_set=CameraSetCal3_S2) + + 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: @@ -129,10 +142,15 @@ def test_distinct_Ks_Bundler(self) -> None: K2 = (1600, 0, 0, 650, 440) measurements, cameras = self.generate_measurements( - Cal3Bundler, PinholeCameraCal3Bundler, (K1, K2), camera_set=CameraSetCal3Bundler - ) - - triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + calibration=Cal3Bundler, + camera_model=PinholeCameraCal3Bundler, + cal_params=(K1, K2), + camera_set=CameraSetCal3Bundler) + + 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: @@ -158,28 +176,45 @@ def test_triangulation_robust_three_poses(self) -> None: 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) + 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) + 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) + 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) + 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)) From 1614ce094f3d2db252785e3983289f4ee13e835f Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 14:55:29 -0500 Subject: [PATCH 5/7] wrap new noise model arg for gtsam.triangulatePoint3 --- gtsam/geometry/geometry.i | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0def842651..1e42966f84 100644 --- a/gtsam/geometry/geometry.i +++ b/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, From 0f1ff48db5a551b5dda15b6f1138739f977b0d05 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 16:49:12 -0500 Subject: [PATCH 6/7] add missing type hint import --- python/gtsam/tests/test_Triangulation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index c3b9870d89..46977b57e7 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -9,7 +9,7 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert """ import unittest -from typing import Iterable, Optional, Union +from typing import Iterable, Optional, Tuple, Union import numpy as np From 0ff9110f3c967ebccd1a067db5d2a0954dd88437 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 15:39:09 -0700 Subject: [PATCH 7/7] add missing type hint annotation import --- python/gtsam/tests/test_Triangulation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 46977b57e7..0a258a0afc 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -9,7 +9,7 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert """ import unittest -from typing import Iterable, Optional, Tuple, Union +from typing import Iterable, List, Optional, Tuple, Union import numpy as np