GaussNewtonOptimizer.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/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/internal/NonlinearOptimizerState.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

namespace gtsam {

typedef internal::NonlinearOptimizerState State;

/* ************************************************************************* */
GaussNewtonOptimizer::GaussNewtonOptimizer(const NonlinearFactorGraph& graph,
                                           const Values& initialValues,
                                           const GaussNewtonParams& params)
    : NonlinearOptimizer(
          graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues)))),
      params_(ensureHasOrdering(params, graph)) {}

GaussNewtonOptimizer::GaussNewtonOptimizer(const NonlinearFactorGraph& graph,
                                           const Values& initialValues, const Ordering& ordering)
    : NonlinearOptimizer(
          graph, std::unique_ptr<State>(new State(initialValues, graph.error(initialValues)))) {
  params_.ordering = ordering;
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussNewtonOptimizer::iterate() {
  gttic(GaussNewtonOptimizer_Iterate);

  // Linearize graph
  gttic(GaussNewtonOptimizer_Linearize);
  GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_->values);
  gttoc(GaussNewtonOptimizer_Linearize);

  // Solve Factor Graph
  gttic(GaussNewtonOptimizer_Solve);
  const VectorValues delta = solve(*linear, params_);
  gttoc(GaussNewtonOptimizer_Solve);

  // Maybe show output
  if (params_.verbosity >= NonlinearOptimizerParams::DELTA)
    delta.print("delta");

  // Create new state with new values and new error
  Values newValues = state_->values.retract(delta);
  state_.reset(new State(std::move(newValues), graph_.error(newValues), state_->iterations + 1));

  return linear;
}

/* ************************************************************************* */
GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering(
    GaussNewtonParams params, const NonlinearFactorGraph& graph) const {
  if (!params.ordering)
    params.ordering = Ordering::Create(params.orderingType, graph);
  return params;
}

} /* namespace gtsam */