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/base/timing.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;
}

struct GTSAM_EXPORT DoglegLineSearchImpl {
  struct Params {
    double minDelta;  
    double maxDelta;  
    double stepSize;  
    double sufficientDecreaseCoeff;  
    bool verbose;                    
  };

  template <class M, class F, class VALUES>
  static DoglegOptimizerImpl::IterationResult Iterate(const Params& params,
                                                      const VectorValues& dx_u,
                                                      const VectorValues& dx_n,
                                                      const M& Rd, const F& f,
                                                      const VALUES& x0);
};
 
/* ************************************************************************* */
template <class M, class F, class VALUES>
typename DoglegOptimizerImpl::IterationResult DoglegLineSearchImpl::Iterate(
    const Params& params, const VectorValues& dx_u, const VectorValues& dx_n,
    const M& Rd, const F& f, const VALUES& x0) {
  if (params.verbose)
    std::cout << "DoglegLineSearch | minDelta: " << params.minDelta
              << " maxDelta: " << params.maxDelta
              << " stepSize: " << params.stepSize << std::endl;
  const double fInitError = f.error(x0);
  const double gnStep = dx_n.norm();
  const size_t numVars = dx_n.size();
 
  double maxStep, step;
  maxStep = std::min(params.maxDelta * numVars, gnStep);
  step = std::min(params.minDelta * numVars, gnStep);
  step = std::min(step, maxStep);  // Edge case min_step > maxStep
 
  if (params.verbose)
    std::cout << "Search Region: [ " << step << " -> " << maxStep << "]"
              << std::endl;
 
  DoglegOptimizerImpl::IterationResult result;
  result.dx_d =
      DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, params.verbose);
  result.f_error = f.error(x0.retract(result.dx_d));
  result.delta = step;
  if (params.verbose)
    std::cout << "Initial Step Error: " << result.f_error << std::endl;
 
  // Validate Step size will terminate search
  if (step < 1e-12 || params.stepSize < 1.0)
    throw std::runtime_error(
        "Invalid DoglegLineSearch configuration. Would cause infinite search.");
 
  // Search Increase delta
  double eps = std::numeric_limits<double>::epsilon();
  while (step < maxStep - eps) {
    // Compute the trust region for the evaluation point according to the
    // Geometric Series
    step = std::min(maxStep, step * params.stepSize);
 
    // Compute the Evaluation Point and evaluate the error
    VectorValues dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(
        step, dx_u, dx_n, params.verbose);
    VALUES x_d(x0.retract(dx_d));
    double fNewError = f.error(x_d);
 
    if (params.verbose)
      std::cout << "Step: " << step << " | Error: " << fNewError << std::endl;
 
    // Check step acceptance conditions
    bool updateDecreasedError = fNewError < result.f_error;
    bool updateHasSufficientDecrease =
        fNewError < (fInitError - params.sufficientDecreaseCoeff * step);
 
    if (params.verbose)
      std::cout << "Decrease Error: " << updateDecreasedError
                << " | Suff. Dec.: " << updateHasSufficientDecrease
                << std::endl;
 
    if (updateDecreasedError && updateHasSufficientDecrease) {
      result.f_error = fNewError;
      result.dx_d = dx_d;
      result.delta = step;
 
      if (params.verbose) std::cout << "Step Accepted" << std::endl;
    }
  }
 
  return result;
}
}