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;
};

typedef FastMap<char, Vector> ISAM2ThresholdMap;
typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
struct GTSAM_EXPORT ISAM2Params {
  typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
      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;

  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) {}

  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.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(OptimizationParams optimizationParams) {
    this->optimizationParams = optimizationParams;
  }
  void setRelinearizeThreshold(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