diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index fd0e60bc8..438eac7a8 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -28,18 +28,18 @@ namespace gtsam { typedef internal::NonlinearOptimizerState State; -/// Fletcher-Reeves formula for computing β, the direction of steepest descent. -static double FletcherReeves(const VectorValues& currentGradient, - const VectorValues& prevGradient) { +/* ************************************************************************* */ +double FletcherReeves(const VectorValues& currentGradient, + const VectorValues& prevGradient) { // Fletcher-Reeves: beta = g_n'*g_n/g_n-1'*g_n-1 const double beta = std::max(0.0, currentGradient.dot(currentGradient) / prevGradient.dot(prevGradient)); return beta; } -/// Polak-Ribiere formula for computing β, the direction of steepest descent. -static double PolakRibiere(const VectorValues& currentGradient, - const VectorValues& prevGradient) { +/* ************************************************************************* */ +double PolakRibiere(const VectorValues& currentGradient, + const VectorValues& prevGradient) { // Polak-Ribiere: beta = g_n'*(g_n-g_n-1)/g_n-1'*g_n-1 const double beta = std::max(0.0, currentGradient.dot(currentGradient - prevGradient) / @@ -47,20 +47,20 @@ static double PolakRibiere(const VectorValues& currentGradient, return beta; } -/// The Hestenes-Stiefel formula for computing β, the direction of steepest descent. -static double HestenesStiefel(const VectorValues& currentGradient, - const VectorValues& prevGradient, - const VectorValues& direction) { +/* ************************************************************************* */ +double HestenesStiefel(const VectorValues& currentGradient, + const VectorValues& prevGradient, + const VectorValues& direction) { // Hestenes-Stiefel: beta = g_n'*(g_n-g_n-1)/(-s_n-1')*(g_n-g_n-1) VectorValues d = currentGradient - prevGradient; const double beta = std::max(0.0, currentGradient.dot(d) / -direction.dot(d)); return beta; } -/// The Dai-Yuan formula for computing β, the direction of steepest descent. -static double DaiYuan(const VectorValues& currentGradient, - const VectorValues& prevGradient, - const VectorValues& direction) { +/* ************************************************************************* */ +double DaiYuan(const VectorValues& currentGradient, + const VectorValues& prevGradient, + const VectorValues& direction) { // Dai-Yuan: beta = g_n'*g_n/(-s_n-1')*(g_n-g_n-1) const double beta = std::max(0.0, currentGradient.dot(currentGradient) / @@ -110,7 +110,8 @@ NonlinearConjugateGradientOptimizer::System::advance(const State& current, GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() { const auto [newValues, dummy] = nonlinearConjugateGradient( - System(graph_), state_->values, params_, true /* single iteration */); + System(graph_), state_->values, params_, true /* single iteration */, + directionMethod_); state_.reset( new State(newValues, graph_.error(newValues), state_->iterations + 1)); @@ -121,8 +122,8 @@ GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() { const Values& NonlinearConjugateGradientOptimizer::optimize() { // Optimize until convergence System system(graph_); - const auto [newValues, iterations] = - nonlinearConjugateGradient(system, state_->values, params_, false); + const auto [newValues, iterations] = nonlinearConjugateGradient( + system, state_->values, params_, false, directionMethod_); state_.reset( new State(std::move(newValues), graph_.error(newValues), iterations)); return state_->values; diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 78f11a600..09b932d42 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -23,6 +23,31 @@ namespace gtsam { +/// Fletcher-Reeves formula for computing β, the direction of steepest descent. +double FletcherReeves(const VectorValues ¤tGradient, + const VectorValues &prevGradient); + +/// Polak-Ribiere formula for computing β, the direction of steepest descent. +double PolakRibiere(const VectorValues ¤tGradient, + const VectorValues &prevGradient); + +/// The Hestenes-Stiefel formula for computing β, +/// the direction of steepest descent. +double HestenesStiefel(const VectorValues ¤tGradient, + const VectorValues &prevGradient, + const VectorValues &direction); + +/// The Dai-Yuan formula for computing β, the direction of steepest descent. +double DaiYuan(const VectorValues ¤tGradient, + const VectorValues &prevGradient, const VectorValues &direction); + +enum class DirectionMethod { + FletcherReeves, + PolakRibiere, + HestenesStiefel, + DaiYuan +}; + /** An implementation of the nonlinear CG method using the template below */ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { @@ -49,13 +74,6 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer typedef NonlinearOptimizerParams Parameters; typedef std::shared_ptr shared_ptr; - enum class DirectionMethod { - FletcherReeves, - PolakRibiere, - HestenesStiefel, - DaiYuan - }; - protected: Parameters params_; DirectionMethod directionMethod_ = DirectionMethod::PolakRibiere; @@ -149,7 +167,9 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { template std::tuple nonlinearConjugateGradient( const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, - const bool singleIteration, const bool gradientDescent = false) { + const bool singleIteration, + const DirectionMethod &directionMethod = DirectionMethod::PolakRibiere, + const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V) size_t iteration = 0; @@ -186,10 +206,23 @@ std::tuple nonlinearConjugateGradient( } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 - const double beta = - std::max(0.0, currentGradient.dot(currentGradient - prevGradient) / - prevGradient.dot(prevGradient)); + + double beta; + switch (directionMethod) { + case DirectionMethod::FletcherReeves: + beta = FletcherReeves(currentGradient, prevGradient); + break; + case DirectionMethod::PolakRibiere: + beta = PolakRibiere(currentGradient, prevGradient); + break; + case DirectionMethod::HestenesStiefel: + beta = HestenesStiefel(currentGradient, prevGradient, direction); + break; + case DirectionMethod::DaiYuan: + beta = DaiYuan(currentGradient, prevGradient, direction); + break; + } + direction = currentGradient + (beta * direction); }