diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index d2771edbd..8e3c93224 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 c079e76b8..8630e1da7 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -228,17 +228,17 @@ class TestTriangulationExample(GtsamTestCase): 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 @@ class TestTriangulationExample(GtsamTestCase): 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