From b5e9dc04fd478c12bca67ba02f0b6e8216958db8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:56:55 -0400 Subject: [PATCH 1/9] Make status a public field --- gtsam/geometry/triangulation.h | 66 ++++++++++++++-------------------- 1 file changed, 26 insertions(+), 40 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 49b5aef04b..401fd2d0bb 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -510,18 +511,18 @@ struct GTSAM_EXPORT TriangulationParameters { }; /** - * TriangulationResult is an optional point, along with the reasons why it is invalid. + * TriangulationResult is an optional point, along with the reasons why it is + * invalid. */ -class TriangulationResult: public boost::optional { - enum Status { - VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT - }; - Status status_; - TriangulationResult(Status s) : - status_(s) { - } -public: +class TriangulationResult : public boost::optional { + public: + enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; + Status status; + private: + TriangulationResult(Status s) : status(s) {} + + public: /** * Default constructor, only for serialization */ @@ -530,54 +531,38 @@ class TriangulationResult: public boost::optional { /** * Constructor */ - TriangulationResult(const Point3& p) : - status_(VALID) { - reset(p); - } + TriangulationResult(const Point3& p) : status(VALID) { reset(p); } static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } - static TriangulationResult Outlier() { - return TriangulationResult(OUTLIER); - } + static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); } static TriangulationResult FarPoint() { return TriangulationResult(FAR_POINT); } static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); } - bool valid() const { - return status_ == VALID; - } - bool degenerate() const { - return status_ == DEGENERATE; - } - bool outlier() const { - return status_ == OUTLIER; - } - bool farPoint() const { - return status_ == FAR_POINT; - } - bool behindCamera() const { - return status_ == BEHIND_CAMERA; - } + bool valid() const { return status == VALID; } + bool degenerate() const { return status == DEGENERATE; } + bool outlier() const { return status == OUTLIER; } + bool farPoint() const { return status == FAR_POINT; } + bool behindCamera() const { return status == BEHIND_CAMERA; } // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationResult& result) { + friend std::ostream& operator<<(std::ostream& os, + const TriangulationResult& result) { if (result) os << "point = " << *result << std::endl; else - os << "no point, status = " << result.status_ << std::endl; + os << "no point, status = " << result.status << std::endl; return os; } -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(status_); + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& BOOST_SERIALIZATION_NVP(status); } }; @@ -644,6 +629,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; From 58cbdcddd52b8fb81f9f5fe2f4b9a3c310529d58 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:57:38 -0400 Subject: [PATCH 2/9] Wrap TriangulationResult --- gtsam/geometry/geometry.i | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0dc23c160e..8d70fb6795 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1089,6 +1089,11 @@ class StereoCamera { #include +virtual class TriangulationResult : boost::optional { + enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; + Status status; +}; + // Templates appear not yet supported for free functions - issue raised at // borglab/wrap#14 to add support gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, From e60f4ac0c6c426617ed2be875d8d64eee195e869 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:58:07 -0400 Subject: [PATCH 3/9] Be consistent in wrapping templated triangulation versions --- gtsam/geometry/geometry.i | 69 +++++++++++++++++++++++++++++++-------- 1 file changed, 56 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8d70fb6795..c297ef82eb 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1096,22 +1096,46 @@ virtual class TriangulationResult : boost::optional { // Templates appear not yet supported for free functions - issue raised at // borglab/wrap#14 to add support + +// Cal3_S2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, 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, + const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3DS2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3Bundler versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); @@ -1119,33 +1143,52 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); + +// Cal3Fisheye versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize, const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, + gtsam::Cal3Fisheye* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); + +// Cal3Unified versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Unified* sharedCal, + const gtsam::Point2Vector& measurements, + 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, + const gtsam::SharedNoiseModel& model = nullptr); gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, + gtsam::Cal3Unified* sharedCal, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, - const gtsam::Point3& initialEstimate); -gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); + + #include template class BearingRange { From 02828e2ec7aa62a117341c641de64e9bcc76f57f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 23:11:40 -0400 Subject: [PATCH 4/9] Fix unrelated warning --- gtsam/linear/SubgraphBuilder.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 18e19cd20d..de7ae7060b 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -337,7 +337,6 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, DSFVector dsf(n); size_t count = 0; - double sum = 0.0; for (const size_t index : sortedIndices) { const GaussianFactor &gf = *gfg[index]; const auto keys = gf.keys(); @@ -347,7 +346,6 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, if (dsf.find(u) != dsf.find(v)) { dsf.merge(u, v); treeIndices.push_back(index); - sum += weights[index]; if (++count == n - 1) break; } } From 8333c8f15c9a081a1a993dcce9b25d2b30afa921 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 00:04:42 -0400 Subject: [PATCH 5/9] Wrapped safe triangulation versions --- gtsam/geometry/geometry.i | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index c297ef82eb..96249b6e0b 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1094,6 +1094,19 @@ virtual class TriangulationResult : boost::optional { Status status; }; +class TriangulationParameters { + double rankTolerance; + bool enableEPI; + double landmarkDistanceThreshold; + double dynamicOutlierRejectionThreshold; + SharedNoiseModel noiseModel; + TriangulationParameters(const double rankTolerance = 1.0, + const bool enableEPI = false, + double landmarkDistanceThreshold = -1, + double dynamicOutlierRejectionThreshold = -1, + const gtsam::SharedNoiseModel& noiseModel = nullptr); +}; + // Templates appear not yet supported for free functions - issue raised at // borglab/wrap#14 to add support @@ -1114,6 +1127,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3DS2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1132,6 +1149,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3DS2& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3Bundler versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1150,6 +1171,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3Fisheye versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1168,6 +1193,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3Fisheye& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); // Cal3Unified versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, @@ -1186,6 +1215,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras, const gtsam::Point2Vector& measurements, const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3Unified& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); From 287ea289209b6cee9c9dbb6d141fb46aa2d5ee98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 00:27:50 -0400 Subject: [PATCH 6/9] Camera sets --- gtsam/geometry/geometry.i | 10 +++++++--- python/gtsam/specializations/geometry.h | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 96249b6e0b..1e9444b02d 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1040,7 +1040,11 @@ class Similarity3 { double scale() const; }; -template +template class CameraSet { CameraSet(); @@ -1088,10 +1092,10 @@ class StereoCamera { }; #include - -virtual class TriangulationResult : boost::optional { +class TriangulationResult { enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status; + const gtsam::Point3& get() const; }; class TriangulationParameters { diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 5a0ea35ef8..99f40253fb 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,6 +21,8 @@ py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( m_, "CameraSetCal3_S2"); +py::bind_vector>>( + m_, "CameraSetCal3DS2"); py::bind_vector>>( m_, "CameraSetCal3Bundler"); py::bind_vector>>( From db371c2fdb0ff463049ebee3f7574da4e98e9136 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 00:28:01 -0400 Subject: [PATCH 7/9] Added safe triangulation tests --- python/gtsam/tests/test_Triangulation.py | 83 +++++++++++++++++++----- 1 file changed, 68 insertions(+), 15 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 0a258a0afc..c079e76b80 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -8,26 +8,17 @@ 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 ( - Cal3_S2, - Cal3Bundler, - CameraSetCal3_S2, - CameraSetCal3Bundler, - PinholeCameraCal3_S2, - PinholeCameraCal3Bundler, - Point2, - Point2Vector, - Point3, - Pose3, - Pose3Vector, - Rot3, -) +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2, Point2Vector, Point3, + Pose3, Pose3Vector, Rot3, TriangulationParameters, + TriangulationResult) from gtsam.utils.test_case import GtsamTestCase UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) @@ -218,6 +209,68 @@ def test_triangulation_robust_three_poses(self) -> None: # 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() From 7bd0ae799de8d6ce52ff3edf346dfa5a9a3bd11a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 10:11:25 -0400 Subject: [PATCH 8/9] Fix pybind11 issue --- gtsam/geometry/geometry.i | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 1e9444b02d..d2771edbd6 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1040,11 +1040,7 @@ class Similarity3 { double scale() const; }; -template +template class CameraSet { CameraSet(); From b78a4e64b19d10c989de67cc8b151d35d19d8a87 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 May 2022 10:11:57 -0400 Subject: [PATCH 9/9] Include all constructors, methods --- gtsam/geometry/geometry.i | 10 ++++++++++ python/gtsam/tests/test_Triangulation.py | 14 +++++++------- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index d2771edbd6..8e3c93224e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1091,7 +1091,17 @@ class StereoCamera { class TriangulationResult { enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status; + TriangulationResult(const gtsam::Point3& p); const gtsam::Point3& get() const; + static TriangulationResult Degenerate(); + static TriangulationResult Outlier(); + static TriangulationResult FarPoint(); + static TriangulationResult BehindCamera(); + bool valid() const; + bool degenerate() const; + bool outlier() const; + bool farPoint() const; + bool behindCamera() const; }; class TriangulationParameters { diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index c079e76b80..8630e1da75 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -228,17 +228,17 @@ def test_outliers_and_far_landmarks(self) -> None: cameras = CameraSetCal3_S2() measurements = Point2Vector() - cameras.push_back(camera1) - cameras.push_back(camera2) - measurements.push_back(z1) - measurements.push_back(z2) + cameras.append(camera1) + cameras.append(camera2) + measurements.append(z1) + measurements.append(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.gtsamAssertEquals(actual.get(), self.landmark, 1e-2) self.assertTrue(actual.valid()) landmarkDistanceThreshold = 4 # landmark is farther than that @@ -254,8 +254,8 @@ def test_outliers_and_far_landmarks(self) -> None: camera3 = PinholeCameraCal3_S2(pose3, K3) z3 = camera3.project(self.landmark) - cameras.push_back(camera3) - measurements.push_back(z3 + Point2(10, -10)) + cameras.append(camera3) + measurements.append(z3 + Point2(10, -10)) landmarkDistanceThreshold = 10 # landmark is closer than that outlierThreshold = 100 # loose, the outlier is going to pass