GncParams.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 
27 #pragma once
28 
31 
32 namespace gtsam {
33 
34 /* ************************************************************************* */
37  GM /*Geman McClure*/,
38  TLS /*Truncated least squares*/
39 };
40 
41 template<class BaseOptimizerParameters>
42 class GncParams {
43  public:
45  typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
46 
48  enum Verbosity {
49  SILENT = 0,
51  MU,
54  };
55 
57  GncParams(const BaseOptimizerParameters& baseOptimizerParams)
58  : baseOptimizerParams(baseOptimizerParams) {
59  }
60 
64  }
65 
67  BaseOptimizerParameters baseOptimizerParams;
70  size_t maxIterations = 100;
71  double muStep = 1.4;
72  double relativeCostTol = 1e-5;
73  double weightsTol = 1e-4;
75 
82 
85  lossType = type;
86  }
87 
89  void setMaxIterations(const size_t maxIter) {
90  std::cout
91  << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
92  << std::endl;
93  maxIterations = maxIter;
94  }
95 
97  void setMuStep(const double step) {
98  muStep = step;
99  }
100 
102  void setRelativeCostTol(double value) {
103  relativeCostTol = value;
104  }
105 
107  void setWeightsTol(double value) {
108  weightsTol = value;
109  }
110 
113  verbosity = value;
114  }
115 
122  void setKnownInliers(const IndexVector& knownIn) {
123  for (size_t i = 0; i < knownIn.size(); i++){
124  knownInliers.push_back(knownIn[i]);
125  }
126  std::sort(knownInliers.begin(), knownInliers.end());
127  }
128 
133  void setKnownOutliers(const IndexVector& knownOut) {
134  for (size_t i = 0; i < knownOut.size(); i++){
135  knownOutliers.push_back(knownOut[i]);
136  }
137  std::sort(knownOutliers.begin(), knownOutliers.end());
138  }
139 
141  bool equals(const GncParams& other, double tol = 1e-9) const {
142  return baseOptimizerParams.equals(other.baseOptimizerParams)
143  && lossType == other.lossType && maxIterations == other.maxIterations
144  && std::fabs(muStep - other.muStep) <= tol
145  && verbosity == other.verbosity && knownInliers == other.knownInliers
146  && knownOutliers == other.knownOutliers;
147  }
148 
150  void print(const std::string& str) const {
151  std::cout << str << "\n";
152  switch (lossType) {
153  case GM:
154  std::cout << "lossType: Geman McClure" << "\n";
155  break;
156  case TLS:
157  std::cout << "lossType: Truncated Least-squares" << "\n";
158  break;
159  default:
160  throw std::runtime_error("GncParams::print: unknown loss type.");
161  }
162  std::cout << "maxIterations: " << maxIterations << "\n";
163  std::cout << "muStep: " << muStep << "\n";
164  std::cout << "relativeCostTol: " << relativeCostTol << "\n";
165  std::cout << "weightsTol: " << weightsTol << "\n";
166  std::cout << "verbosity: " << verbosity << "\n";
167  for (size_t i = 0; i < knownInliers.size(); i++)
168  std::cout << "knownInliers: " << knownInliers[i] << "\n";
169  for (size_t i = 0; i < knownOutliers.size(); i++)
170  std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
171  baseOptimizerParams.print("Base optimizer params: ");
172  }
173 };
174 
175 }
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
BaseOptimizerParameters::OptimizerType OptimizerType
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition: GncParams.h:45
void setWeightsTol(double value)
Set the maximum difference between the weights and their rounding in {0,1} to stop iterating...
Definition: GncParams.h:107
bool equals(const GncParams &other, double tol=1e-9) const
Equals.
Definition: GncParams.h:141
IndexVector knownOutliers
Definition: GncParams.h:81
IndexVector knownInliers
Slots in the factor graph corresponding to measurements that we know are outliers.
Definition: GncParams.h:79
Verbosity verbosity
Verbosity level.
Definition: GncParams.h:74
GncLossType lossType
any other specific GNC parameters:
Definition: GncParams.h:69
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
GncParams()
Default constructor.
Definition: GncParams.h:62
double muStep
Multiplicative factor to reduce/increase the mu in gnc.
Definition: GncParams.h:71
double relativeCostTol
If relative cost change is below this threshold, stop iterating.
Definition: GncParams.h:72
BaseOptimizerParameters baseOptimizerParams
GNC parameters.
Definition: GncParams.h:67
double weightsTol
If the weights are within weightsTol from being binary, stop iterating (only for TLS) ...
Definition: GncParams.h:73
Real fabs(const Real &a)
FastVector< uint64_t > IndexVector
Slots in the factor graph corresponding to measurements that we know are inliers. ...
Definition: GncParams.h:78
GncParams(const BaseOptimizerParameters &baseOptimizerParams)
Constructor.
Definition: GncParams.h:57
size_t maxIterations
Maximum number of iterations.
Definition: GncParams.h:70
void setKnownInliers(const IndexVector &knownIn)
Definition: GncParams.h:122
void setLossType(const GncLossType type)
Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
Definition: GncParams.h:84
void setMuStep(const double step)
Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep...
Definition: GncParams.h:97
Definition: pytypes.h:1403
void setVerbosityGNC(const Verbosity value)
Set the verbosity level.
Definition: GncParams.h:112
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
void setMaxIterations(const size_t maxIter)
Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate...
Definition: GncParams.h:89
void setKnownOutliers(const IndexVector &knownOut)
Definition: GncParams.h:133
GncLossType
Choice of robust loss function for GNC.
Definition: GncParams.h:36
traits
Definition: chartTesting.h:28
void setRelativeCostTol(double value)
Set the maximum relative difference in mu values to stop iterating.
Definition: GncParams.h:102
void print(const std::string &str) const
Print.
Definition: GncParams.h:150
const G double tol
Definition: Group.h:86
Verbosity
Verbosity levels.
Definition: GncParams.h:48
Definition: pytypes.h:1370


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