From 3ef6974235a62356e80b4be0267933e6c8d56d37 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 8 Jan 2022 20:25:00 -0500 Subject: [PATCH 1/9] added robustness to triangulation --- gtsam/geometry/tests/testTriangulation.cpp | 88 ++++++++++++++++++++++ gtsam/geometry/triangulation.h | 59 ++++++++++----- 2 files changed, 128 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 5fdb911d02..3a09f49bc7 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -182,6 +182,94 @@ TEST(triangulation, fourPoses) { #endif } +//****************************************************************************** +TEST(triangulation, threePoses_robustNoiseModel) { + + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *sharedCal); + Point2 z3 = camera3.project(landmark); + + vector poses; + Point2Vector measurements; + poses += pose1, pose2, pose3; + measurements += z1, z2, z3; + + // noise free, so should give exactly the landmark + boost::optional actual = + triangulatePoint3(poses, sharedCal, measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); + + // Add outlier + measurements.at(0) += Point2(100, 120); // very large pixel noise! + + // now estimate does not match landmark + boost::optional actual2 = // + triangulatePoint3(poses, sharedCal, measurements); + // DLT is surprisingly robust, but still off (actual error is around 0.26m): + EXPECT( (landmark - *actual2).norm() >= 0.2); + EXPECT( (landmark - *actual2).norm() <= 0.5); + + // Again with nonlinear optimization + boost::optional actual3 = + triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + // result from nonlinear (but non-robust optimization) is close to DLT and still off + EXPECT(assert_equal(*actual2, *actual3, 0.1)); + + // Again with nonlinear optimization, this time with robust loss + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); + boost::optional actual4 = triangulatePoint3( + poses, sharedCal, measurements, 1e-9, true, model); + // using the Huber loss we now have a quite small error!! nice! + EXPECT(assert_equal(landmark, *actual4, 0.05)); +} + +//****************************************************************************** +TEST(triangulation, fourPoses_robustNoiseModel) { + + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + PinholeCamera camera3(pose3, *sharedCal); + Point2 z3 = camera3.project(landmark); + + vector poses; + Point2Vector measurements; + poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1 + measurements += z1, z1, z2, z3; + + // noise free, so should give exactly the landmark + boost::optional actual = + triangulatePoint3(poses, sharedCal, measurements); + EXPECT(assert_equal(landmark, *actual, 1e-2)); + + // Add outlier + measurements.at(0) += Point2(100, 120); // very large pixel noise! + // add noise on other measurements: + measurements.at(1) += Point2(0.1, 0.2); // small noise + measurements.at(2) += Point2(0.2, 0.2); + measurements.at(3) += Point2(0.3, 0.1); + + // now estimate does not match landmark + boost::optional actual2 = // + triangulatePoint3(poses, sharedCal, measurements); + // DLT is surprisingly robust, but still off (actual error is around 0.17m): + EXPECT( (landmark - *actual2).norm() >= 0.1); + EXPECT( (landmark - *actual2).norm() <= 0.5); + + // Again with nonlinear optimization + boost::optional actual3 = + triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); + // result from nonlinear (but non-robust optimization) is close to DLT and still off + EXPECT(assert_equal(*actual2, *actual3, 0.1)); + + // Again with nonlinear optimization, this time with robust loss + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); + boost::optional actual4 = triangulatePoint3( + poses, sharedCal, measurements, 1e-9, true, model); + // using the Huber loss we now have a quite small error!! nice! + EXPECT(assert_equal(landmark, *actual4, 0.05)); +} + //****************************************************************************** TEST(triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index aaa8d1a269..5651ae8d86 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -14,6 +14,7 @@ * @brief Functions for triangulation * @date July 31, 2013 * @author Chris Beall + * @author Luca Carlone */ #pragma once @@ -105,18 +106,18 @@ template std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, - const Point3& initialEstimate) { + const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); graph.emplace_shared > // - (camera_i, measurements[i], unit2, landmarkKey); + (camera_i, measurements[i], model? model : unit2, landmarkKey); } return std::make_pair(graph, values); } @@ -134,7 +135,8 @@ template std::pair triangulationGraph( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, - const Point3& initialEstimate) { + const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; @@ -143,7 +145,7 @@ std::pair triangulationGraph( for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; graph.emplace_shared > // - (camera_i, measurements[i], unit, landmarkKey); + (camera_i, measurements[i], model? model : unit, landmarkKey); } return std::make_pair(graph, values); } @@ -169,13 +171,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, template Point3 triangulateNonlinear(const std::vector& poses, boost::shared_ptr sharedCal, - const Point2Vector& measurements, const Point3& initialEstimate) { + const Point2Vector& measurements, const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; boost::tie(graph, values) = triangulationGraph // - (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model); return optimize(graph, values, Symbol('p', 0)); } @@ -190,13 +193,14 @@ Point3 triangulateNonlinear(const std::vector& poses, template Point3 triangulateNonlinear( const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) { + const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { // Create a factor graph and initial values Values values; NonlinearFactorGraph graph; boost::tie(graph, values) = triangulationGraph // - (cameras, measurements, Symbol('p', 0), initialEstimate); + (cameras, measurements, Symbol('p', 0), initialEstimate, model); return optimize(graph, values, Symbol('p', 0)); } @@ -239,7 +243,8 @@ template Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false) { + bool optimize = false, + const SharedNoiseModel& model = nullptr) { assert(poses.size() == measurements.size()); if (poses.size() < 2) @@ -254,7 +259,7 @@ Point3 triangulatePoint3(const std::vector& poses, // Then refine using non-linear optimization if (optimize) point = triangulateNonlinear // - (poses, sharedCal, measurements, point); + (poses, sharedCal, measurements, point, model); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras @@ -284,7 +289,8 @@ template Point3 triangulatePoint3( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, - bool optimize = false) { + bool optimize = false, + const SharedNoiseModel& model = nullptr) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -298,7 +304,7 @@ Point3 triangulatePoint3( // The n refine using non-linear optimization if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + point = triangulateNonlinear(cameras, measurements, point, model); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras @@ -317,9 +323,10 @@ template Point3 triangulatePoint3( const CameraSet >& cameras, const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false) { + bool optimize = false, + const SharedNoiseModel& model = nullptr) { return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize); + (cameras, measurements, rank_tol, optimize, model); } struct GTSAM_EXPORT TriangulationParameters { @@ -341,20 +348,29 @@ struct GTSAM_EXPORT TriangulationParameters { */ double dynamicOutlierRejectionThreshold; + SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation + + bool invDepthLinearTriangulation; ///< if set to true, we use an inverse depth alternative to DL + /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate * @param enableEPI if true refine triangulation with embedded LM iterations * @param landmarkDistanceThreshold flag as degenerate if point further than this * @param dynamicOutlierRejectionThreshold or if average error larger than this + * @param invDepthLinearTriangulation use inverse depth approach to linear triangulation * */ TriangulationParameters(const double _rankTolerance = 1.0, const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, - double _dynamicOutlierRejectionThreshold = -1) : + double _dynamicOutlierRejectionThreshold = -1, + const SharedNoiseModel& _noiseModel = nullptr, + const bool _invDepthLinearTriangulation = false) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), // landmarkDistanceThreshold(_landmarkDistanceThreshold), // - dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), + noiseModel(_noiseModel), + invDepthLinearTriangulation(_invDepthLinearTriangulation){ } // stream to output @@ -366,6 +382,9 @@ struct GTSAM_EXPORT TriangulationParameters { << std::endl; os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl; + os << "noise model" << std::endl; + os << "invDepthLinearTriangulation = " << p.invDepthLinearTriangulation + << std::endl; return os; } @@ -379,6 +398,7 @@ struct GTSAM_EXPORT TriangulationParameters { ar & BOOST_SERIALIZATION_NVP(enableEPI); ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + ar & BOOST_SERIALIZATION_NVP(invDepthLinearTriangulation); } }; @@ -468,8 +488,9 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, else // We triangulate the 3D position of the landmark try { - Point3 point = triangulatePoint3(cameras, measured, - params.rankTolerance, params.enableEPI); + Point3 point = + triangulatePoint3(cameras, measured, params.rankTolerance, + params.enableEPI, params.noiseModel); // Check landmark distance and re-projection errors to avoid outliers size_t i = 0; From c407609b8f8d2d6f7c110bc757325826f83e75e1 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 8 Jan 2022 20:27:27 -0500 Subject: [PATCH 2/9] moved invDepth-related variables --- gtsam/geometry/triangulation.h | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 5651ae8d86..af01d3a364 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -350,27 +350,23 @@ struct GTSAM_EXPORT TriangulationParameters { SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation - bool invDepthLinearTriangulation; ///< if set to true, we use an inverse depth alternative to DL - /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate * @param enableEPI if true refine triangulation with embedded LM iterations * @param landmarkDistanceThreshold flag as degenerate if point further than this * @param dynamicOutlierRejectionThreshold or if average error larger than this - * @param invDepthLinearTriangulation use inverse depth approach to linear triangulation + * @param noiseModel noise model to use during nonlinear triangulation * */ TriangulationParameters(const double _rankTolerance = 1.0, const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, double _dynamicOutlierRejectionThreshold = -1, - const SharedNoiseModel& _noiseModel = nullptr, - const bool _invDepthLinearTriangulation = false) : + const SharedNoiseModel& _noiseModel = nullptr) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), // landmarkDistanceThreshold(_landmarkDistanceThreshold), // dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), - noiseModel(_noiseModel), - invDepthLinearTriangulation(_invDepthLinearTriangulation){ + noiseModel(_noiseModel){ } // stream to output @@ -383,8 +379,6 @@ struct GTSAM_EXPORT TriangulationParameters { os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl; os << "noise model" << std::endl; - os << "invDepthLinearTriangulation = " << p.invDepthLinearTriangulation - << std::endl; return os; } @@ -398,7 +392,6 @@ struct GTSAM_EXPORT TriangulationParameters { ar & BOOST_SERIALIZATION_NVP(enableEPI); ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); - ar & BOOST_SERIALIZATION_NVP(invDepthLinearTriangulation); } }; From b60ca0c10724ea3c85ca04575295d38c1947418c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 12 Jan 2022 09:57:59 -0700 Subject: [PATCH 3/9] 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 4/9] 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 5/9] 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 6/9] 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 7/9] 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 8/9] 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 9/9] 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