From 8333c8f15c9a081a1a993dcce9b25d2b30afa921 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 May 2022 00:04:42 -0400
Subject: [PATCH] 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 c297ef82e..96249b6e0 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1094,6 +1094,19 @@ virtual class TriangulationResult : boost::optional<gtsam::Point3> {
   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);