gtsam 4.2.0
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 GncParams {
43 public:
45 typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
46
48 enum Verbosity {
49 SILENT = 0,
50 SUMMARY,
51 MU,
52 WEIGHTS,
53 VALUES
54 };
55
57 GncParams(const BaseOptimizerParameters& 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
76 //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
82 IndexVector knownOutliers = IndexVector();
83
85 void setLossType(const GncLossType type) {
86 lossType = type;
87 }
88
90 void setMaxIterations(const size_t maxIter) {
91 std::cout
92 << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
93 << std::endl;
94 maxIterations = maxIter;
95 }
96
98 void setMuStep(const double step) {
99 muStep = step;
100 }
101
103 void setRelativeCostTol(double value) {
104 relativeCostTol = value;
105 }
106
108 void setWeightsTol(double value) {
109 weightsTol = value;
110 }
111
113 void setVerbosityGNC(const Verbosity value) {
114 verbosity = value;
115 }
116
123 void setKnownInliers(const IndexVector& knownIn) {
124 for (size_t i = 0; i < knownIn.size(); i++){
125 knownInliers.push_back(knownIn[i]);
126 }
127 std::sort(knownInliers.begin(), knownInliers.end());
128 }
129
134 void setKnownOutliers(const IndexVector& knownOut) {
135 for (size_t i = 0; i < knownOut.size(); i++){
136 knownOutliers.push_back(knownOut[i]);
137 }
138 std::sort(knownOutliers.begin(), knownOutliers.end());
139 }
140
142 bool equals(const GncParams& other, double tol = 1e-9) const {
143 return baseOptimizerParams.equals(other.baseOptimizerParams)
144 && lossType == other.lossType && maxIterations == other.maxIterations
145 && std::fabs(muStep - other.muStep) <= tol
146 && verbosity == other.verbosity && knownInliers == other.knownInliers
147 && knownOutliers == other.knownOutliers;
148 }
149
151 void print(const std::string& str) const {
152 std::cout << str << "\n";
153 switch (lossType) {
154 case GM:
155 std::cout << "lossType: Geman McClure" << "\n";
156 break;
157 case TLS:
158 std::cout << "lossType: Truncated Least-squares" << "\n";
159 break;
160 default:
161 throw std::runtime_error("GncParams::print: unknown loss type.");
162 }
163 std::cout << "maxIterations: " << maxIterations << "\n";
164 std::cout << "muStep: " << muStep << "\n";
165 std::cout << "relativeCostTol: " << relativeCostTol << "\n";
166 std::cout << "weightsTol: " << weightsTol << "\n";
167 std::cout << "verbosity: " << verbosity << "\n";
168 for (size_t i = 0; i < knownInliers.size(); i++)
169 std::cout << "knownInliers: " << knownInliers[i] << "\n";
170 for (size_t i = 0; i < knownOutliers.size(); i++)
171 std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
172 baseOptimizerParams.print("Base optimizer params: ");
173 }
174};
175
176}
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition: FastVector.h:34
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:67
IndexVector knownInliers
Slots in the factor graph corresponding to measurements that we know are outliers.
Definition: GncParams.h:80
GncParams()
Default constructor.
Definition: GncParams.h:62
BaseOptimizerParameters::OptimizerType OptimizerType
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition: GncParams.h:45
void setKnownInliers(const IndexVector &knownIn)
(Optional) Provide a vector of measurements that must be considered inliers.
Definition: GncParams.h:123
void print(const std::string &str) const
Print.
Definition: GncParams.h:151
FastVector< uint64_t > IndexVector
Slots in the factor graph corresponding to measurements that we know are inliers.
Definition: GncParams.h:79
GncParams(const BaseOptimizerParameters &baseOptimizerParams)
Constructor.
Definition: GncParams.h:57
double muStep
Multiplicative factor to reduce/increase the mu in gnc.
Definition: GncParams.h:71
double weightsTol
If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Definition: GncParams.h:73
void setKnownOutliers(const IndexVector &knownOut)
(Optional) Provide a vector of measurements that must be considered outliers.
Definition: GncParams.h:134
Verbosity verbosity
Verbosity level.
Definition: GncParams.h:74
bool equals(const GncParams &other, double tol=1e-9) const
Equals.
Definition: GncParams.h:142
void setRelativeCostTol(double value)
Set the maximum relative difference in mu values to stop iterating.
Definition: GncParams.h:103
GncLossType lossType
any other specific GNC parameters:
Definition: GncParams.h:69
size_t maxIterations
Maximum number of iterations.
Definition: GncParams.h:70
void setLossType(const GncLossType type)
Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
Definition: GncParams.h:85
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:98
double relativeCostTol
If relative cost change is below this threshold, stop iterating.
Definition: GncParams.h:72
void setVerbosityGNC(const Verbosity value)
Set the verbosity level.
Definition: GncParams.h:113
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:90
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:108