Merge pull request #1517 from dllu/develop

borglab · Sep 4, 2023 · 52ecb09 · 52ecb09
2 parents 0948fa8 + 242728d
commit 52ecb09
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
@@ -1180,11 +1180,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
@@ -574,6 +574,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
 
   /**
@@ -588,10 +593,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){
   }
 
@@ -604,6 +611,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;
   }
@@ -701,7 +709,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;