DoglegOptimizerImpl.h

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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <iomanip>
#include <cassert>
 
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Ordering.h>
 
namespace gtsam {
 
struct GTSAM_EXPORT DoglegOptimizerImpl {
 
  struct GTSAM_EXPORT IterationResult {
    double delta;
    VectorValues dx_d;
    double f_error;
  };
 
  enum TrustRegionAdaptationMode {
    SEARCH_EACH_ITERATION,
    SEARCH_REDUCE_ONLY,
    ONE_STEP_PER_ITERATION
  };
 
  template<class M, class F, class VALUES>
  static IterationResult Iterate(
      double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
      const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false);
 
  static VectorValues ComputeDoglegPoint(double delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false);
 
  static VectorValues ComputeBlend(double delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false);
};
 
 
/* ************************************************************************* */
template<class M, class F, class VALUES>
typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
    double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
    const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose)
{
  gttic(M_error);
  const double M_error = Rd.error(VectorValues::Zero(dx_u));
  gttoc(M_error);
 
  // Result to return
  IterationResult result;
 
  bool stay = true;
  enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration
  while(stay) {
    gttic(Dog_leg_point);
    // Compute dog leg point
    result.dx_d = ComputeDoglegPoint(delta, dx_u, dx_n, verbose);
    gttoc(Dog_leg_point);
 
    if(verbose) std::cout << "delta = " << delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
 
    gttic(retract);
    // Compute expmapped solution
    const VALUES x_d(x0.retract(result.dx_d));
    gttoc(retract);
 
    gttic(decrease_in_f);
    // Compute decrease in f
    result.f_error = f.error(x_d);
    gttoc(decrease_in_f);
 
    gttic(new_M_error);
    // Compute decrease in M
    const double new_M_error = Rd.error(result.dx_d);
    gttoc(new_M_error);
 
    if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl;
    if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl;
 
    gttic(adjust_delta);
    // Compute gain ratio.  Here we take advantage of the invariant that the
    // Bayes' net error at zero is equal to the nonlinear error
    const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ?
        0.5 :
        (f_error - result.f_error) / (M_error - new_M_error);
 
    if(verbose) std::cout << std::setprecision(15) << "rho = " << rho << std::endl;
 
    if(rho >= 0.75) {
      // M agrees very well with f, so try to increase lambda
      const double dx_d_norm = result.dx_d.norm();
      const double newDelta = std::max(delta, 3.0 * dx_d_norm); // Compute new delta
 
      if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
        stay = false;   // If not searching, just return with the new delta
      else if(mode == SEARCH_EACH_ITERATION) {
        if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA)
          stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region
        else {
          stay = true;  // Searching and increased delta, so try again to increase delta
          lastAction = INCREASED_DELTA;
        }
      } else {
        assert(false); }
 
      delta = newDelta; // Update delta from new delta
 
    } else if(0.75 > rho && rho >= 0.25) {
      // M agrees so-so with f, keep the same delta
      stay = false;
 
    } else if(0.25 > rho && rho >= 0.0) {
      // M does not agree well with f, decrease delta until it does
      double newDelta;
      bool hitMinimumDelta;
      if(delta > 1e-5) {
        newDelta = 0.5 * delta;
        hitMinimumDelta = false;
      } else {
        newDelta = delta;
        hitMinimumDelta = true;
      }
      if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ lastAction == INCREASED_DELTA || hitMinimumDelta)
        stay = false;   // If not searching, just return with the new smaller delta
      else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) {
        stay = true;
        lastAction = DECREASED_DELTA;
      } else {
        assert(false); }
 
      delta = newDelta; // Update delta from new delta
 
    } else {
      // f actually increased, so keep decreasing delta until f does not decrease.
      // NOTE:  NaN and Inf solutions also will fall into this case, so that we
      // decrease delta if the solution becomes undetermined.
      assert(0.0 > rho);
      if(delta > 1e-5) {
        delta *= 0.5;
        stay = true;
        lastAction = DECREASED_DELTA;
      } else {
        if(verbose) std::cout << "Warning:  Dog leg stopping because cannot decrease error with minimum delta" << std::endl;
        result.dx_d.setZero(); // Set delta to zero - don't allow error to increase
        result.f_error = f_error;
        stay = false;
      }
    }
    gttoc(adjust_delta);
  }
 
  // dx_d and f_error have already been filled in during the loop
  result.delta = delta;
  return result;
}
 
}