gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
LossFunctions.h
1
2/* ----------------------------------------------------------------------------
3
4 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
5 * Atlanta, Georgia 30332-0415
6 * All Rights Reserved
7 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
8
9 * See LICENSE for the license information
10
11 * -------------------------------------------------------------------------- */
12
20#pragma once
21
22#include <gtsam/base/Matrix.h>
23#include <gtsam/base/Testable.h>
24#include <gtsam/dllexport.h>
25
26#include <boost/serialization/extended_type_info.hpp>
27#include <boost/serialization/nvp.hpp>
28#include <boost/serialization/version.hpp>
29#include <boost/serialization/optional.hpp>
30#include <boost/serialization/shared_ptr.hpp>
31#include <boost/serialization/singleton.hpp>
32
33namespace gtsam {
34namespace noiseModel {
35// clang-format off
54// clang-format on
55namespace mEstimator {
56
63class GTSAM_EXPORT Base {
64 public:
67 enum ReweightScheme { Scalar, Block };
68 typedef boost::shared_ptr<Base> shared_ptr;
69
70 protected:
73
74 public:
75 Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
76 virtual ~Base() {}
77
79 ReweightScheme reweightScheme() const { return reweight_; }
80
94 virtual double loss(double distance) const { return 0; }
95
106 virtual double weight(double distance) const = 0;
107
108 virtual void print(const std::string &s) const = 0;
109 virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
110
111 double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
112
115 Vector weight(const Vector &error) const;
116
118 Vector sqrtWeight(const Vector &error) const;
119
122 void reweight(Vector &error) const;
123 void reweight(std::vector<Matrix> &A, Vector &error) const;
124 void reweight(Matrix &A, Vector &error) const;
125 void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
126 void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
127
128 private:
130 friend class boost::serialization::access;
131 template <class ARCHIVE>
132 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
133 ar &BOOST_SERIALIZATION_NVP(reweight_);
134 }
135};
136
146class GTSAM_EXPORT Null : public Base {
147 public:
148 typedef boost::shared_ptr<Null> shared_ptr;
149
150 Null(const ReweightScheme reweight = Block) : Base(reweight) {}
151 ~Null() override {}
152 double weight(double /*error*/) const override { return 1.0; }
153 double loss(double distance) const override { return 0.5 * distance * distance; }
154 void print(const std::string &s) const override;
155 bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
156 static shared_ptr Create();
157
158 private:
160 friend class boost::serialization::access;
161 template <class ARCHIVE>
162 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
163 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
164 }
165};
166
175class GTSAM_EXPORT Fair : public Base {
176 protected:
177 double c_;
178
179 public:
180 typedef boost::shared_ptr<Fair> shared_ptr;
181
182 Fair(double c = 1.3998, const ReweightScheme reweight = Block);
183 double weight(double distance) const override;
184 double loss(double distance) const override;
185 void print(const std::string &s) const override;
186 bool equals(const Base &expected, double tol = 1e-8) const override;
187 static shared_ptr Create(double c, const ReweightScheme reweight = Block);
188 double modelParameter() const { return c_; }
189
190 private:
192 friend class boost::serialization::access;
193 template <class ARCHIVE>
194 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
195 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
196 ar &BOOST_SERIALIZATION_NVP(c_);
197 }
198};
199
208class GTSAM_EXPORT Huber : public Base {
209 protected:
210 double k_;
211
212 public:
213 typedef boost::shared_ptr<Huber> shared_ptr;
214
215 Huber(double k = 1.345, const ReweightScheme reweight = Block);
216 double weight(double distance) const override;
217 double loss(double distance) const override;
218 void print(const std::string &s) const override;
219 bool equals(const Base &expected, double tol = 1e-8) const override;
220 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
221 double modelParameter() const { return k_; }
222
223 private:
225 friend class boost::serialization::access;
226 template <class ARCHIVE>
227 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
228 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
229 ar &BOOST_SERIALIZATION_NVP(k_);
230 }
231};
232
246class GTSAM_EXPORT Cauchy : public Base {
247 protected:
248 double k_, ksquared_;
249
250 public:
251 typedef boost::shared_ptr<Cauchy> shared_ptr;
252
253 Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
254 double weight(double distance) const override;
255 double loss(double distance) const override;
256 void print(const std::string &s) const override;
257 bool equals(const Base &expected, double tol = 1e-8) const override;
258 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
259 double modelParameter() const { return k_; }
260
261 private:
263 friend class boost::serialization::access;
264 template <class ARCHIVE>
265 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
266 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
267 ar &BOOST_SERIALIZATION_NVP(k_);
268 ar &BOOST_SERIALIZATION_NVP(ksquared_);
269 }
270};
271
280class GTSAM_EXPORT Tukey : public Base {
281 protected:
282 double c_, csquared_;
283
284 public:
285 typedef boost::shared_ptr<Tukey> shared_ptr;
286
287 Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
288 double weight(double distance) const override;
289 double loss(double distance) const override;
290 void print(const std::string &s) const override;
291 bool equals(const Base &expected, double tol = 1e-8) const override;
292 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
293 double modelParameter() const { return c_; }
294
295 private:
297 friend class boost::serialization::access;
298 template <class ARCHIVE>
299 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
300 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
301 ar &BOOST_SERIALIZATION_NVP(c_);
302 }
303};
304
313class GTSAM_EXPORT Welsch : public Base {
314 protected:
315 double c_, csquared_;
316
317 public:
318 typedef boost::shared_ptr<Welsch> shared_ptr;
319
320 Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
321 double weight(double distance) const override;
322 double loss(double distance) const override;
323 void print(const std::string &s) const override;
324 bool equals(const Base &expected, double tol = 1e-8) const override;
325 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
326 double modelParameter() const { return c_; }
327
328 private:
330 friend class boost::serialization::access;
331 template <class ARCHIVE>
332 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
333 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
334 ar &BOOST_SERIALIZATION_NVP(c_);
335 ar &BOOST_SERIALIZATION_NVP(csquared_);
336 }
337};
338
349class GTSAM_EXPORT GemanMcClure : public Base {
350 public:
351 typedef boost::shared_ptr<GemanMcClure> shared_ptr;
352
353 GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
354 ~GemanMcClure() override {}
355 double weight(double distance) const override;
356 double loss(double distance) const override;
357 void print(const std::string &s) const override;
358 bool equals(const Base &expected, double tol = 1e-8) const override;
359 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
360 double modelParameter() const { return c_; }
361
362 protected:
363 double c_;
364
365 private:
367 friend class boost::serialization::access;
368 template <class ARCHIVE>
369 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
370 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
371 ar &BOOST_SERIALIZATION_NVP(c_);
372 }
373};
374
387class GTSAM_EXPORT DCS : public Base {
388 public:
389 typedef boost::shared_ptr<DCS> shared_ptr;
390
391 DCS(double c = 1.0, const ReweightScheme reweight = Block);
392 ~DCS() override {}
393 double weight(double distance) const override;
394 double loss(double distance) const override;
395 void print(const std::string &s) const override;
396 bool equals(const Base &expected, double tol = 1e-8) const override;
397 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
398 double modelParameter() const { return c_; }
399
400 protected:
401 double c_;
402
403 private:
405 friend class boost::serialization::access;
406 template <class ARCHIVE>
407 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
408 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
409 ar &BOOST_SERIALIZATION_NVP(c_);
410 }
411};
412
426class GTSAM_EXPORT L2WithDeadZone : public Base {
427 protected:
428 double k_;
429
430 public:
431 typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
432
433 L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
434 double weight(double distance) const override;
435 double loss(double distance) const override;
436 void print(const std::string &s) const override;
437 bool equals(const Base &expected, double tol = 1e-8) const override;
438 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
439 double modelParameter() const { return k_; }
440
441 private:
443 friend class boost::serialization::access;
444 template <class ARCHIVE>
445 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
446 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
447 ar &BOOST_SERIALIZATION_NVP(k_);
448 }
449};
450
451} // namespace mEstimator
452} // namespace noiseModel
453} // namespace gtsam
Concept check for values that can be used in unit tests.
typedef and functions to augment Eigen's MatrixXd
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
Template to create a binary predicate.
Definition Testable.h:111
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
virtual double loss(double distance) const
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.h:94
ReweightScheme reweight_
Strategy for reweighting.
Definition LossFunctions.h:72
ReweightScheme reweightScheme() const
Returns the reweight scheme, as explained in ReweightScheme.
Definition LossFunctions.h:79
virtual double weight(double distance) const =0
This method is responsible for returning the weight function for a given amount of error.
ReweightScheme
the rows can be weighted independently according to the error or uniformly with the norm of the right...
Definition LossFunctions.h:67
"Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain least-squares (non-ro...
Definition LossFunctions.h:146
double weight(double) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.h:152
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.h:153
Implementation of the "Fair" robust error model (Zhang97ivc)
Definition LossFunctions.h:175
The "Huber" robust error model (Zhang97ivc).
Definition LossFunctions.h:208
Implementation of the "Cauchy" robust error model (Lee2013IROS).
Definition LossFunctions.h:246
Implementation of the "Tukey" robust error model (Zhang97ivc).
Definition LossFunctions.h:280
Implementation of the "Welsch" robust error model (Zhang97ivc).
Definition LossFunctions.h:313
Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
Definition LossFunctions.h:349
DCS implements the Dynamic Covariance Scaling robust error model from the paper Robust Map Optimizati...
Definition LossFunctions.h:387
L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k,...
Definition LossFunctions.h:426