ISAM2Params.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
 
 * -------------------------------------------------------------------------- */

 
// \callgraph
 
#pragma once
 
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
#include <string>
#include <variant>
 
namespace gtsam {

struct GTSAM_EXPORT ISAM2GaussNewtonParams {
  double
      wildfireThreshold;  

  ISAM2GaussNewtonParams(
      double _wildfireThreshold =
          0.001  
      )
      : wildfireThreshold(_wildfireThreshold) {}
 
  void print(const std::string str = "") const {
    using std::cout;
    cout << str << "type:              ISAM2GaussNewtonParams\n";
    cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
    cout.flush();
  }
 
  double getWildfireThreshold() const { return wildfireThreshold; }
  void setWildfireThreshold(double wildfireThreshold) {
    this->wildfireThreshold = wildfireThreshold;
  }
};

struct GTSAM_EXPORT ISAM2DoglegParams {
  double initialDelta;  
  double
      wildfireThreshold;  
  DoglegOptimizerImpl::TrustRegionAdaptationMode
      adaptationMode;  
  bool
      verbose;  

  ISAM2DoglegParams(
      double _initialDelta = 1.0,  
      double _wildfireThreshold =
          1e-5,  
      DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode =
          DoglegOptimizerImpl::
              SEARCH_EACH_ITERATION,  
      bool _verbose = false           
      )
      : initialDelta(_initialDelta),
        wildfireThreshold(_wildfireThreshold),
        adaptationMode(_adaptationMode),
        verbose(_verbose) {}
 
  void print(const std::string str = "") const {
    using std::cout;
    cout << str << "type:              ISAM2DoglegParams\n";
    cout << str << "initialDelta:      " << initialDelta << "\n";
    cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
    cout << str
         << "adaptationMode:    " << adaptationModeTranslator(adaptationMode)
         << "\n";
    cout.flush();
  }
 
  double getInitialDelta() const { return initialDelta; }
  double getWildfireThreshold() const { return wildfireThreshold; }
  std::string getAdaptationMode() const {
    return adaptationModeTranslator(adaptationMode);
  }
  bool isVerbose() const { return verbose; }
  void setInitialDelta(double initialDelta) {
    this->initialDelta = initialDelta;
  }
  void setWildfireThreshold(double wildfireThreshold) {
    this->wildfireThreshold = wildfireThreshold;
  }
  void setAdaptationMode(const std::string& adaptationMode) {
    this->adaptationMode = adaptationModeTranslator(adaptationMode);
  }
  void setVerbose(bool verbose) { this->verbose = verbose; }
 
  std::string adaptationModeTranslator(
      const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode)
      const;
  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(
      const std::string& adaptationMode) const;
};

struct GTSAM_EXPORT ISAM2DoglegLineSearchParams {
  DoglegLineSearchImpl::Params dllsParams;  
  double wildfireThreshold;  

  ISAM2DoglegLineSearchParams(double minDelta = 0.02, double maxDelta = 0.5,
                              double stepSize = 1.5,
                              double sufficientDecreaseCoeff = 1e-3,
                              bool verbose = false,
                              double wildfireThreshold = 1e-4)
      : dllsParams{minDelta, maxDelta, stepSize, sufficientDecreaseCoeff,
                   verbose},
        wildfireThreshold(wildfireThreshold) {
    if (minDelta < 1e-12 || maxDelta < 1e-12 || stepSize < 1.0) {
      throw std::invalid_argument(
          "ISAM2DoglegLineSearchParams constructed with invalid configuration. "
          "Search Bounds [minDelta, maxDelta] ~ 0 or stepSize < 1.0");
    }
  }
 
  void print(const std::string str = "") const {
    using std::cout;
    cout << str << "type:                      ISAM2DoglegLineSearchParams\n";
    cout << str << "minDelta:                 " << dllsParams.minDelta << "\n";
    cout << str << "maxDelta:                 " << dllsParams.maxDelta << "\n";
    cout << str << "stepSize:                 " << dllsParams.stepSize << "\n";
    cout << str
         << "sufficientDecreaseCoeff: " << dllsParams.sufficientDecreaseCoeff
         << "\n";
    cout << str << "verbose:        " << dllsParams.verbose << "\n";
    cout << str << "wildfireThreshold:        " << wildfireThreshold << "\n";
    cout.flush();
  }
  double getMinDelta() const { return dllsParams.minDelta; }
  double getMaxDelta() const { return dllsParams.maxDelta; }
  double getStepSize() const { return dllsParams.stepSize; }
  double getSufficientDecreaseCoeff() const {
    return dllsParams.sufficientDecreaseCoeff;
  }
  bool isVerbose() const { return dllsParams.verbose; }
  double getWildfireThreshold() const { return wildfireThreshold; }

  void setMinDelta(double minDelta) { this->dllsParams.minDelta = minDelta; }
  void setMaxDelta(double maxDelta) { this->dllsParams.maxDelta = maxDelta; }
  void setStepSize(double stepSize) { this->dllsParams.stepSize = stepSize; }
  void setSufficientDecreaseCoeff(double sufficientDecreaseCoeff) {
    this->dllsParams.sufficientDecreaseCoeff = sufficientDecreaseCoeff;
  }
  void setVerbose(bool verbose) { this->dllsParams.verbose = verbose; }
  void setWildfireThreshold(double wildfireThreshold) {
    this->wildfireThreshold = wildfireThreshold;
  }
};

typedef FastMap<char, Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct GTSAM_EXPORT ISAM2Params {
  typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams,
                       ISAM2DoglegLineSearchParams>
      OptimizationParams;  
  typedef std::variant<double, FastMap<char, Vector> >
      RelinearizationThreshold;  

  OptimizationParams optimizationParams;

  RelinearizationThreshold relinearizeThreshold;
 
  int relinearizeSkip;  
 
  bool enableRelinearization;  
 
  bool evaluateNonlinearError;  
 
  enum Factorization { CHOLESKY, QR };
  Factorization factorization;

  bool cacheLinearizedFactors;
 
  KeyFormatter
      keyFormatter;  

  bool enableDetailedResults;

  bool enablePartialRelinearizationCheck;

  bool findUnusedFactorSlots;

  bool enableAdaptiveReorder;

  double adaptiveReorderThreshold;

  ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(),
              RelinearizationThreshold _relinearizeThreshold = 0.1,
              int _relinearizeSkip = 10, bool _enableRelinearization = true,
              bool _evaluateNonlinearError = false,
              Factorization _factorization = ISAM2Params::CHOLESKY,
              bool _cacheLinearizedFactors = true,
              const KeyFormatter& _keyFormatter =
                  DefaultKeyFormatter,  
              bool _enableDetailedResults = false)
      : optimizationParams(_optimizationParams),
        relinearizeThreshold(_relinearizeThreshold),
        relinearizeSkip(_relinearizeSkip),
        enableRelinearization(_enableRelinearization),
        evaluateNonlinearError(_evaluateNonlinearError),
        factorization(_factorization),
        cacheLinearizedFactors(_cacheLinearizedFactors),
        keyFormatter(_keyFormatter),
        enableDetailedResults(_enableDetailedResults),
        enablePartialRelinearizationCheck(false),
        findUnusedFactorSlots(false),
        enableAdaptiveReorder(false),
        adaptiveReorderThreshold(2.0) {}

  void print(const std::string& str = "") const {
    using std::cout;
    cout << str << "\n";
 
    static const std::string kStr("optimizationParams:                ");
    if (std::holds_alternative<ISAM2GaussNewtonParams>(optimizationParams)) {
      std::get<ISAM2GaussNewtonParams>(optimizationParams).print();
    } else if (std::holds_alternative<ISAM2DoglegParams>(optimizationParams)) {
      std::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
    } else {
      cout << kStr << "{unknown type}\n";
    }
 
    cout << "relinearizeThreshold:              ";
    if (std::holds_alternative<double>(relinearizeThreshold)) {
      cout << std::get<double>(relinearizeThreshold) << "\n";
    } else {
      cout << "{mapped}\n";
      for (const ISAM2ThresholdMapValue& value :
           std::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
        cout << "                                   '" << value.first
             << "' -> [" << value.second.transpose() << " ]\n";
      }
    }
 
    cout << "relinearizeSkip:                   " << relinearizeSkip << "\n";
    cout << "enableRelinearization:             " << enableRelinearization
         << "\n";
    cout << "evaluateNonlinearError:            " << evaluateNonlinearError
         << "\n";
    cout << "factorization:                     "
         << factorizationTranslator(factorization) << "\n";
    cout << "cacheLinearizedFactors:            " << cacheLinearizedFactors
         << "\n";
    cout << "enableDetailedResults:             " << enableDetailedResults
         << "\n";
    cout << "enablePartialRelinearizationCheck: "
         << enablePartialRelinearizationCheck << "\n";
    cout << "findUnusedFactorSlots:             " << findUnusedFactorSlots
         << "\n";
    cout << "enableAdaptiveReorder:             " << enableAdaptiveReorder
         << "\n";
    if (enableAdaptiveReorder)
      cout << "adaptiveReorderThreshold:          " << adaptiveReorderThreshold
           << "\n";
    cout.flush();
  }

 
  OptimizationParams getOptimizationParams() const {
    return this->optimizationParams;
  }
  RelinearizationThreshold getRelinearizeThreshold() const {
    return relinearizeThreshold;
  }
  std::string getFactorization() const {
    return factorizationTranslator(factorization);
  }
  KeyFormatter getKeyFormatter() const { return keyFormatter; }
 
  void setOptimizationParams(const OptimizationParams& optimizationParams) {
    this->optimizationParams = optimizationParams;
  }
  void setRelinearizeThreshold(
      const RelinearizationThreshold& relinearizeThreshold) {
    this->relinearizeThreshold = relinearizeThreshold;
  }
  void setFactorization(const std::string& factorization) {
    this->factorization = factorizationTranslator(factorization);
  }
  void setKeyFormatter(KeyFormatter keyFormatter) {
    this->keyFormatter = keyFormatter;
  }
 
  GaussianFactorGraph::Eliminate getEliminationFunction() const {
    return factorization == CHOLESKY
               ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
               : (GaussianFactorGraph::Eliminate)EliminateQR;
  }


 
  static Factorization factorizationTranslator(const std::string& str);
  static std::string factorizationTranslator(const Factorization& value);

};
 
}  // namespace gtsam