NonlinearConjugateGradientOptimizer.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>
 
#include <cmath>
 
namespace gtsam {
 
typedef internal::NonlinearOptimizerState State;
 
static VectorValues gradientInPlace(const NonlinearFactorGraph& nfg,
                                    const Values& values) {
  // Linearize graph
  GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
  return linear->gradientAtZero();
}
 
NonlinearConjugateGradientOptimizer::NonlinearConjugateGradientOptimizer(
    const NonlinearFactorGraph& graph, const Values& initialValues,
    const Parameters& params, const DirectionMethod& directionMethod)
    : Base(graph, std::unique_ptr<State>(
                      new State(initialValues, graph.error(initialValues)))),
      params_(params) {}
 
double NonlinearConjugateGradientOptimizer::System::error(
    const State& state) const {
  return graph_.error(state);
}
 
NonlinearConjugateGradientOptimizer::System::Gradient
NonlinearConjugateGradientOptimizer::System::gradient(
    const State& state) const {
  return gradientInPlace(graph_, state);
}
 
NonlinearConjugateGradientOptimizer::System::State
NonlinearConjugateGradientOptimizer::System::advance(const State& current,
                                                     const double alpha,
                                                     const Gradient& g) const {
  Gradient step = g;
  step *= alpha;
  return current.retract(step);
}
 
GaussianFactorGraph::shared_ptr NonlinearConjugateGradientOptimizer::iterate() {
  const auto [newValues, dummy] = nonlinearConjugateGradient<System, Values>(
      System(graph_), state_->values, params_, true /* single iteration */,
      directionMethod_);
  state_.reset(
      new State(newValues, graph_.error(newValues), state_->iterations + 1));
 
  // NOTE(frank): We don't linearize this system, so we must return null here.
  return nullptr;
}
 
const Values& NonlinearConjugateGradientOptimizer::optimize() {
  // Optimize until convergence
  System system(graph_);
  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;
}
 
} /* namespace gtsam */