diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 57f2acffe..35c470eca 100644 --- 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 index 68795a646..d82da3459 100644 --- 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& cameras, try { Point3 point = triangulatePoint3(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;