NonlinearOptimizerParams.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 
22 #pragma once
23 
26 
27 #include <string>
28 #include <optional>
29 
30 namespace gtsam {
31 
35 class GTSAM_EXPORT NonlinearOptimizerParams {
36 public:
38  enum Verbosity {
39  SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
40  };
41 
42  size_t maxIterations = 100;
43  double relativeErrorTol = 1e-5;
44  double absoluteErrorTol = 1e-5;
45  double errorTol = 0.0;
46  Verbosity verbosity = SILENT;
48 
49  size_t getMaxIterations() const { return maxIterations; }
50  double getRelativeErrorTol() const { return relativeErrorTol; }
51  double getAbsoluteErrorTol() const { return absoluteErrorTol; }
52  double getErrorTol() const { return errorTol; }
53  std::string getVerbosity() const { return verbosityTranslator(verbosity); }
54 
55  void setMaxIterations(int value) { maxIterations = value; }
56  void setRelativeErrorTol(double value) { relativeErrorTol = value; }
57  void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
58  void setErrorTol(double value) { errorTol = value; }
59  void setVerbosity(const std::string& src) {
60  verbosity = verbosityTranslator(src);
61  }
62 
63  static Verbosity verbosityTranslator(const std::string &s) ;
64  static std::string verbosityTranslator(Verbosity value) ;
65 
68  using IterationHook = std::function<
69  void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
70 
96 
103  Iterative, /* Experimental Flag */
104  CHOLMOD, /* Experimental Flag */
105  };
106 
107  LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY;
108  std::optional<Ordering> ordering;
110 
111  NonlinearOptimizerParams() = default;
113  }
114 
115  virtual void print(const std::string& str = "") const;
116 
117  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
118  return maxIterations == other.getMaxIterations()
119  && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
120  && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
121  && std::abs(errorTol - other.getErrorTol()) <= tol
122  && verbosityTranslator(verbosity) == other.getVerbosity();
123  // && orderingType.equals(other.getOrderingType()_;
124  // && linearSolverType == other.getLinearSolverType();
125  // TODO: check ordering, iterativeParams, and iterationsHook
126  }
127 
128  inline bool isMultifrontal() const {
129  return (linearSolverType == MULTIFRONTAL_CHOLESKY)
130  || (linearSolverType == MULTIFRONTAL_QR);
131  }
132 
133  inline bool isSequential() const {
134  return (linearSolverType == SEQUENTIAL_CHOLESKY)
135  || (linearSolverType == SEQUENTIAL_QR);
136  }
137 
138  inline bool isCholmod() const {
139  return (linearSolverType == CHOLMOD);
140  }
141 
142  inline bool isIterative() const {
143  return (linearSolverType == Iterative);
144  }
145 
147  switch (linearSolverType) {
148  case MULTIFRONTAL_CHOLESKY:
149  case SEQUENTIAL_CHOLESKY:
151 
152  case MULTIFRONTAL_QR:
153  case SEQUENTIAL_QR:
154  return EliminateQR;
155 
156  default:
157  throw std::runtime_error(
158  "Nonlinear optimization parameter \"factorization\" is invalid");
159  }
160  }
161 
162  std::string getLinearSolverType() const {
163  return linearSolverTranslator(linearSolverType);
164  }
165 
166  void setLinearSolverType(const std::string& solver) {
167  linearSolverType = linearSolverTranslator(solver);
168  }
169 
170  void setIterativeParams(const std::shared_ptr<IterativeOptimizationParameters> params);
171 
172  void setOrdering(const Ordering& ordering) {
173  this->ordering = ordering;
174  this->orderingType = Ordering::CUSTOM;
175  }
176 
177  std::string getOrderingType() const {
178  return orderingTypeTranslator(orderingType);
179  }
180 
181  // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type
182  void setOrderingType(const std::string& ordering){
183  orderingType = orderingTypeTranslator(ordering);
184  }
185 
186 private:
187  std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
188  LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
189  std::string orderingTypeTranslator(Ordering::OrderingType type) const;
190  Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
191 };
192 
193 // For backward compatibility:
195 
196 } /* namespace gtsam */
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
void setLinearSolverType(const std::string &solver)
NonlinearOptimizerParams SuccessiveLinearizationParams
Subgraph Solver from IROS 2010.
std::function< void(size_t, double, double)> IterationHook
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
BiCGSTAB< SparseMatrix< double > > solver
static enum @1107 ordering
static const SmartProjectionParams params
void setVerbosity(const std::string &src)
Definition: pytypes.h:1403
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
void setOrderingType(const std::string &ordering)
void setOrdering(const Ordering &ordering)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Linear Factor Graph where all factors are Gaussians.
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
OrderingType
Type of ordering to use.
GaussianFactorGraph::Eliminate getEliminationFunction() const
traits
Definition: chartTesting.h:28
std::shared_ptr< IterativeOptimizationParameters > shared_ptr
const G double tol
Definition: Group.h:86
bool equals(const NonlinearOptimizerParams &other, double tol=1e-9) const
std::optional< Ordering > ordering
The optional variable elimination ordering, or empty to use COLAMD (default: empty) ...
#define abs(x)
Definition: datatypes.h:17
Definition: pytypes.h:1370


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:57