gtsam 4.1.1
gtsam
GncParams.h
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
32namespace gtsam {
33
34/* ************************************************************************* */
37 GM /*Geman McClure*/,
38 TLS /*Truncated least squares*/
39};
40
41template<class BaseOptimizerParameters>
42class GTSAM_EXPORT GncParams {
43 public:
45 typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
46
48 enum Verbosity {
49 SILENT = 0,
50 SUMMARY,
51 VALUES
52 };
53
55 GncParams(const BaseOptimizerParameters& baseOptimizerParams)
56 : baseOptimizerParams(baseOptimizerParams) {
57 }
58
61 : baseOptimizerParams() {
62 }
63
65 BaseOptimizerParameters baseOptimizerParams;
67 GncLossType lossType = TLS;
68 size_t maxIterations = 100;
69 double muStep = 1.4;
70 double relativeCostTol = 1e-5;
71 double weightsTol = 1e-4;
72 Verbosity verbosity = SILENT;
73 std::vector<size_t> knownInliers = std::vector<size_t>();
74 std::vector<size_t> knownOutliers = std::vector<size_t>();
75
77 void setLossType(const GncLossType type) {
78 lossType = type;
79 }
80
82 void setMaxIterations(const size_t maxIter) {
83 std::cout
84 << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
85 << std::endl;
86 maxIterations = maxIter;
87 }
88
90 void setMuStep(const double step) {
91 muStep = step;
92 }
93
95 void setRelativeCostTol(double value) {
96 relativeCostTol = value;
97 }
98
100 void setWeightsTol(double value) {
101 weightsTol = value;
102 }
103
105 void setVerbosityGNC(const Verbosity value) {
106 verbosity = value;
107 }
108
115 void setKnownInliers(const std::vector<size_t>& knownIn) {
116 for (size_t i = 0; i < knownIn.size(); i++){
117 knownInliers.push_back(knownIn[i]);
118 }
119 std::sort(knownInliers.begin(), knownInliers.end());
120 }
121
126 void setKnownOutliers(const std::vector<size_t>& knownOut) {
127 for (size_t i = 0; i < knownOut.size(); i++){
128 knownOutliers.push_back(knownOut[i]);
129 }
130 std::sort(knownOutliers.begin(), knownOutliers.end());
131 }
132
134 bool equals(const GncParams& other, double tol = 1e-9) const {
135 return baseOptimizerParams.equals(other.baseOptimizerParams)
136 && lossType == other.lossType && maxIterations == other.maxIterations
137 && std::fabs(muStep - other.muStep) <= tol
138 && verbosity == other.verbosity && knownInliers == other.knownInliers
139 && knownOutliers == other.knownOutliers;
140 }
141
143 void print(const std::string& str) const {
144 std::cout << str << "\n";
145 switch (lossType) {
146 case GM:
147 std::cout << "lossType: Geman McClure" << "\n";
148 break;
149 case TLS:
150 std::cout << "lossType: Truncated Least-squares" << "\n";
151 break;
152 default:
153 throw std::runtime_error("GncParams::print: unknown loss type.");
154 }
155 std::cout << "maxIterations: " << maxIterations << "\n";
156 std::cout << "muStep: " << muStep << "\n";
157 std::cout << "relativeCostTol: " << relativeCostTol << "\n";
158 std::cout << "weightsTol: " << weightsTol << "\n";
159 std::cout << "verbosity: " << verbosity << "\n";
160 for (size_t i = 0; i < knownInliers.size(); i++)
161 std::cout << "knownInliers: " << knownInliers[i] << "\n";
162 for (size_t i = 0; i < knownOutliers.size(); i++)
163 std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
164 baseOptimizerParams.print(str);
165 }
166};
167
168}
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
GncLossType
Choice of robust loss function for GNC.
Definition: GncParams.h:36
Definition: GncParams.h:42
BaseOptimizerParameters baseOptimizerParams
GNC parameters.
Definition: GncParams.h:65
std::vector< size_t > knownInliers
Slots in the factor graph corresponding to measurements that we know are inliers.
Definition: GncParams.h:73
GncParams()
Default constructor.
Definition: GncParams.h:60
BaseOptimizerParameters::OptimizerType OptimizerType
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition: GncParams.h:45
void print(const std::string &str) const
Print.
Definition: GncParams.h:143
GncParams(const BaseOptimizerParameters &baseOptimizerParams)
Constructor.
Definition: GncParams.h:55
double muStep
Multiplicative factor to reduce/increase the mu in gnc.
Definition: GncParams.h:69
Verbosity verbosity
Verbosity level.
Definition: GncParams.h:72
bool equals(const GncParams &other, double tol=1e-9) const
Equals.
Definition: GncParams.h:134
void setRelativeCostTol(double value)
Set the maximum relative difference in mu values to stop iterating.
Definition: GncParams.h:95
GncLossType lossType
any other specific GNC parameters:
Definition: GncParams.h:67
void setKnownOutliers(const std::vector< size_t > &knownOut)
(Optional) Provide a vector of measurements that must be considered outliers.
Definition: GncParams.h:126
size_t maxIterations
Maximum number of iterations.
Definition: GncParams.h:68
void setLossType(const GncLossType type)
Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
Definition: GncParams.h:77
void setKnownInliers(const std::vector< size_t > &knownIn)
(Optional) Provide a vector of measurements that must be considered inliers.
Definition: GncParams.h:115
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:90
void setVerbosityGNC(const Verbosity value)
Set the verbosity level.
Definition: GncParams.h:105
std::vector< size_t > knownOutliers
Slots in the factor graph corresponding to measurements that we know are outliers.
Definition: GncParams.h:74
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:82
Verbosity
Verbosity levels.
Definition: GncParams.h:48
void setWeightsTol(double value)
Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
Definition: GncParams.h:100