From 242728df02ddde9604f4a54598a4facfa6c0f87f Mon Sep 17 00:00:00 2001
From: Daniel Lu <daniel@lawrence.lu>
Date: Tue, 18 Apr 2023 20:38:12 -0700
Subject: [PATCH] add useLOST to triangulateSafe

---
 gtsam/geometry/geometry.i      |  2 ++
 gtsam/geometry/triangulation.h | 10 +++++++++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index e9929227a..a7fca34d1 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1146,11 +1146,13 @@ class TriangulationParameters {
   bool enableEPI;
   double landmarkDistanceThreshold;
   double dynamicOutlierRejectionThreshold;
+  bool useLOST;
   gtsam::SharedNoiseModel noiseModel;
   TriangulationParameters(const double rankTolerance = 1.0,
                           const bool enableEPI = false,
                           double landmarkDistanceThreshold = -1,
                           double dynamicOutlierRejectionThreshold = -1,
+                          const bool useLOST = false,
                           const gtsam::SharedNoiseModel& noiseModel = nullptr);
 };
 
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 7e58cee2d..d8578947c 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -571,6 +571,11 @@ struct GTSAM_EXPORT TriangulationParameters {
    */
   double dynamicOutlierRejectionThreshold;
 
+  /**
+   * if true, will use the LOST algorithm instead of DLT
+   */
+  bool useLOST;
+
   SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
 
   /**
@@ -585,10 +590,12 @@ struct GTSAM_EXPORT TriangulationParameters {
   TriangulationParameters(const double _rankTolerance = 1.0,
       const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
       double _dynamicOutlierRejectionThreshold = -1,
+      const bool _useLOST = false,
       const SharedNoiseModel& _noiseModel = nullptr) :
       rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
       landmarkDistanceThreshold(_landmarkDistanceThreshold), //
       dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
+      useLOST(_useLOST),
       noiseModel(_noiseModel){
   }
 
@@ -601,6 +608,7 @@ struct GTSAM_EXPORT TriangulationParameters {
         << std::endl;
     os << "dynamicOutlierRejectionThreshold = "
         << p.dynamicOutlierRejectionThreshold << std::endl;
+    os << "useLOST = " << p.useLOST << std::endl;
     os << "noise model" << std::endl;
     return os;
   }
@@ -698,7 +706,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
     try {
       Point3 point =
           triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
-                                    params.enableEPI, params.noiseModel);
+                                    params.enableEPI, params.noiseModel, params.useLOST);
 
       // Check landmark distance and re-projection errors to avoid outliers
       size_t i = 0;