gtsam  4.0.0
gtsam
ISAM2Params.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 // \callgraph
19 
20 #pragma once
21 
24 #include <boost/variant.hpp>
25 #include <string>
26 
27 namespace gtsam {
28 
35 struct GTSAM_EXPORT ISAM2GaussNewtonParams {
36  double
38 
42  double _wildfireThreshold =
43  0.001
44  )
46  : wildfireThreshold(_wildfireThreshold) {}
47 
48  void print(const std::string str = "") const {
49  using std::cout;
50  cout << str << "type: ISAM2GaussNewtonParams\n";
51  cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
52  cout.flush();
53  }
54 
55  double getWildfireThreshold() const { return wildfireThreshold; }
56  void setWildfireThreshold(double wildfireThreshold) {
57  this->wildfireThreshold = wildfireThreshold;
58  }
59 };
60 
67 struct GTSAM_EXPORT ISAM2DoglegParams {
68  double initialDelta;
69  double
74  bool
77 
80  double _initialDelta = 1.0,
81  double _wildfireThreshold =
82  1e-5,
85  SEARCH_EACH_ITERATION,
86  bool _verbose = false
87  )
88  : initialDelta(_initialDelta),
89  wildfireThreshold(_wildfireThreshold),
90  adaptationMode(_adaptationMode),
91  verbose(_verbose) {}
92 
93  void print(const std::string str = "") const {
94  using std::cout;
95  cout << str << "type: ISAM2DoglegParams\n";
96  cout << str << "initialDelta: " << initialDelta << "\n";
97  cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
98  cout << str
99  << "adaptationMode: " << adaptationModeTranslator(adaptationMode)
100  << "\n";
101  cout.flush();
102  }
103 
104  double getInitialDelta() const { return initialDelta; }
105  double getWildfireThreshold() const { return wildfireThreshold; }
106  std::string getAdaptationMode() const {
107  return adaptationModeTranslator(adaptationMode);
108  }
109  bool isVerbose() const { return verbose; }
110  void setInitialDelta(double initialDelta) {
111  this->initialDelta = initialDelta;
112  }
113  void setWildfireThreshold(double wildfireThreshold) {
114  this->wildfireThreshold = wildfireThreshold;
115  }
116  void setAdaptationMode(const std::string& adaptationMode) {
117  this->adaptationMode = adaptationModeTranslator(adaptationMode);
118  }
119  void setVerbose(bool verbose) { this->verbose = verbose; }
120 
121  std::string adaptationModeTranslator(
123  const;
124  DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(
125  const std::string& adaptationMode) const;
126 };
127 
133 typedef FastMap<char, Vector> ISAM2ThresholdMap;
134 typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
135 struct GTSAM_EXPORT ISAM2Params {
136  typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
138  typedef boost::variant<double, FastMap<char, Vector> >
141 
151 
169 
171 
175 
178 
181  enum Factorization { CHOLESKY, QR };
193  Factorization factorization;
194 
201 
204 
207 
219 
225 
231  RelinearizationThreshold _relinearizeThreshold = 0.1,
232  int _relinearizeSkip = 10, bool _enableRelinearization = true,
233  bool _evaluateNonlinearError = false,
234  Factorization _factorization = ISAM2Params::CHOLESKY,
235  bool _cacheLinearizedFactors = true,
236  const KeyFormatter& _keyFormatter =
237  DefaultKeyFormatter
238  )
239  : optimizationParams(_optimizationParams),
240  relinearizeThreshold(_relinearizeThreshold),
241  relinearizeSkip(_relinearizeSkip),
242  enableRelinearization(_enableRelinearization),
243  evaluateNonlinearError(_evaluateNonlinearError),
244  factorization(_factorization),
245  cacheLinearizedFactors(_cacheLinearizedFactors),
246  keyFormatter(_keyFormatter),
247  enableDetailedResults(false),
248  enablePartialRelinearizationCheck(false),
249  findUnusedFactorSlots(false) {}
250 
252  void print(const std::string& str = "") const {
253  using std::cout;
254  cout << str << "\n";
255 
256  static const std::string kStr("optimizationParams: ");
257  if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
258  boost::get<ISAM2GaussNewtonParams>(optimizationParams).print();
259  else if (optimizationParams.type() == typeid(ISAM2DoglegParams))
260  boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
261  else
262  cout << kStr << "{unknown type}\n";
263 
264  cout << "relinearizeThreshold: ";
265  if (relinearizeThreshold.type() == typeid(double)) {
266  cout << boost::get<double>(relinearizeThreshold) << "\n";
267  } else {
268  cout << "{mapped}\n";
269  for (const ISAM2ThresholdMapValue& value :
270  boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
271  cout << " '" << value.first
272  << "' -> [" << value.second.transpose() << " ]\n";
273  }
274  }
275 
276  cout << "relinearizeSkip: " << relinearizeSkip << "\n";
277  cout << "enableRelinearization: " << enableRelinearization
278  << "\n";
279  cout << "evaluateNonlinearError: " << evaluateNonlinearError
280  << "\n";
281  cout << "factorization: "
282  << factorizationTranslator(factorization) << "\n";
283  cout << "cacheLinearizedFactors: " << cacheLinearizedFactors
284  << "\n";
285  cout << "enableDetailedResults: " << enableDetailedResults
286  << "\n";
287  cout << "enablePartialRelinearizationCheck: "
288  << enablePartialRelinearizationCheck << "\n";
289  cout << "findUnusedFactorSlots: " << findUnusedFactorSlots
290  << "\n";
291  cout.flush();
292  }
293 
296 
297  OptimizationParams getOptimizationParams() const {
298  return this->optimizationParams;
299  }
300  RelinearizationThreshold getRelinearizeThreshold() const {
301  return relinearizeThreshold;
302  }
303  int getRelinearizeSkip() const { return relinearizeSkip; }
304  bool isEnableRelinearization() const { return enableRelinearization; }
305  bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
306  std::string getFactorization() const {
307  return factorizationTranslator(factorization);
308  }
309  bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
310  KeyFormatter getKeyFormatter() const { return keyFormatter; }
311  bool isEnableDetailedResults() const { return enableDetailedResults; }
312  bool isEnablePartialRelinearizationCheck() const {
313  return enablePartialRelinearizationCheck;
314  }
315 
316  void setOptimizationParams(OptimizationParams optimizationParams) {
317  this->optimizationParams = optimizationParams;
318  }
319  void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
320  this->relinearizeThreshold = relinearizeThreshold;
321  }
322  void setRelinearizeSkip(int relinearizeSkip) {
323  this->relinearizeSkip = relinearizeSkip;
324  }
325  void setEnableRelinearization(bool enableRelinearization) {
326  this->enableRelinearization = enableRelinearization;
327  }
328  void setEvaluateNonlinearError(bool evaluateNonlinearError) {
329  this->evaluateNonlinearError = evaluateNonlinearError;
330  }
331  void setFactorization(const std::string& factorization) {
332  this->factorization = factorizationTranslator(factorization);
333  }
334  void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
335  this->cacheLinearizedFactors = cacheLinearizedFactors;
336  }
337  void setKeyFormatter(KeyFormatter keyFormatter) {
338  this->keyFormatter = keyFormatter;
339  }
340  void setEnableDetailedResults(bool enableDetailedResults) {
341  this->enableDetailedResults = enableDetailedResults;
342  }
343  void setEnablePartialRelinearizationCheck(
344  bool enablePartialRelinearizationCheck) {
345  this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
346  }
347 
348  GaussianFactorGraph::Eliminate getEliminationFunction() const {
349  return factorization == CHOLESKY
350  ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
352  }
353 
355 
358 
359  static Factorization factorizationTranslator(const std::string& str);
360  static std::string factorizationTranslator(const Factorization& value);
361 
363 };
364 
365 } // namespace gtsam
Definition: ISAM2Params.h:35
This class contains the implementation of the Dogleg algorithm.
Definition: DoglegOptimizerImpl.h:32
KeyFormatter keyFormatter
A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
Definition: ISAM2Params.h:203
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)
Specify parameters as constructor arguments See the documentation of member variables above.
Definition: ISAM2Params.h:230
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
double wildfireThreshold
Continue updating the linear delta only when changes are above this threshold (default: 0....
Definition: ISAM2Params.h:37
void print(const std::string &str="") const
print iSAM2 parameters
Definition: ISAM2Params.h:252
Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
OptimizationParams optimizationParams
Optimization parameters, this both selects the nonlinear optimization method and specifies its parame...
Definition: ISAM2Params.h:150
bool findUnusedFactorSlots
When you will be removing many factors, e.g.
Definition: ISAM2Params.h:224
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition: JacobianFactor.cpp:721
int relinearizeSkip
Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
Definition: ISAM2Params.h:170
bool enablePartialRelinearizationCheck
Check variables for relinearization in tree-order, stopping the check once a variable does not need t...
Definition: ISAM2Params.h:218
Definition: ISAM2Params.h:135
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:33
TrustRegionAdaptationMode
Specifies how the trust region is adapted at each Dogleg iteration.
Definition: DoglegOptimizerImpl.h:53
boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams
Either ISAM2GaussNewtonParams or ISAM2DoglegParams.
Definition: ISAM2Params.h:137
bool cacheLinearizedFactors
Whether to cache linear factors (default: true).
Definition: ISAM2Params.h:200
bool enableRelinearization
Controls whether ISAM2 will ever relinearize any variables (default: true)
Definition: ISAM2Params.h:174
Definition: ISAM2Params.h:67
ISAM2DoglegParams(double _initialDelta=1.0, double _wildfireThreshold=1e-5, DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode=DoglegOptimizerImpl::SEARCH_EACH_ITERATION, bool _verbose=false)
Specify parameters as constructor arguments.
Definition: ISAM2Params.h:79
bool verbose
Whether Dogleg prints iteration and convergence information.
Definition: ISAM2Params.h:76
double initialDelta
The initial trust region radius for Dogleg.
Definition: ISAM2Params.h:68
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
RelinearizationThreshold relinearizeThreshold
Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0....
Definition: ISAM2Params.h:168
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition: ISAM2Params.h:140
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode
See description in DoglegOptimizerImpl::TrustRegionAdaptationMode.
Definition: ISAM2Params.h:73
bool enableDetailedResults
Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: ...
Definition: ISAM2Params.h:206
Linear Factor Graph where all factors are Gaussians.
Factorization factorization
Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
Definition: ISAM2Params.h:193
boost::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:89
double wildfireThreshold
Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
Definition: ISAM2Params.h:70
bool evaluateNonlinearError
Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from up...
Definition: ISAM2Params.h:177
ISAM2GaussNewtonParams(double _wildfireThreshold=0.001)
Specify parameters as constructor arguments.
Definition: ISAM2Params.h:41