diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index af01d3a36..a4c6b1a13 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,7 +107,7 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = unit2) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; @@ -117,7 +117,7 @@ std::pair triangulationGraph( typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); graph.emplace_shared > // - (camera_i, measurements[i], model? model : unit2, landmarkKey); + (camera_i, measurements[i], model, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 96b03e803..e5b4718d6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -299,9 +299,11 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - if (mu_init >= 0 && mu_init < 1e-6) + if (mu_init >= 0 && mu_init < 1e-6){ mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, - // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + } + return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares)