diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index f8d0c747d..7fbdb37fa 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -97,8 +97,6 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout << " lambdaFactor: " << lambdaFactor << "\n"; std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; - std::cout << " disableInnerIterations: " << disableInnerIterations - << "\n"; std::cout << " minModelFidelity: " << minModelFidelity << "\n"; std::cout << " diagonalDamping: " << diagonalDamping << "\n"; std::cout << " verbosityLM: " @@ -141,10 +139,6 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( const GaussianFactorGraph& linear) { - //Set two parameters as Ceres, will move out later - static const double min_diagonal_ = 1e-6; - static const double max_diagonal_ = 1e32; - gttic(damp); if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) cout << "building damped system with lambda " << state_.lambda << endl; @@ -154,7 +148,7 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), min_diagonal_), max_diagonal_); + v(aa) = std::min(std::max(v(aa), params_.min_diagonal_), params_.max_diagonal_); v(aa) = sqrt(v(aa)); } } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index bae82b7c3..47952f9e4 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -51,17 +51,19 @@ public: double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity - bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] bool diagonalDamping; ///< if true, use diagonal of Hessian - bool reuse_diagonal_; //an additional option in Ceres for diagonalDamping (related to efficiency) - bool useFixedLambdaFactor_; // if true applies constant increase (or decrease) to lambda according to lambdaFactor + bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency) + bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( - 0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity( - 1e-3), diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true) { + 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), + diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), + min_diagonal_(1e-6), max_diagonal_(1e32) { } virtual ~LevenbergMarquardtParams() { }