diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 3981e9a84..75444d62a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -238,6 +238,12 @@ public: // } bool retriangulate = decideIfTriangulate(cameras); + +// if(!retriangulate) +// std::cout << "retriangulate = false" << std::endl; +// +// bool retriangulate = true; + if (retriangulate) { std::vector reprojections; @@ -285,7 +291,8 @@ public: result_ = TriangulationResult::BehindCamera(); } - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); + if(params_.triangulation.enableEPI) + pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); result_ = TriangulationResult(pw_avg);