gtsam  4.1.0
gtsam
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/optional.hpp>
29 #include <boost/serialization/shared_ptr.hpp>
30 #include <boost/serialization/singleton.hpp>
31 
32 namespace gtsam {
33 namespace noiseModel {
34 // clang-format off
53 // clang-format on
54 namespace mEstimator {
55 
56 //---------------------------------------------------------------------------------------
57 
58 class GTSAM_EXPORT Base {
59  public:
60  enum ReweightScheme { Scalar, Block };
61  typedef boost::shared_ptr<Base> shared_ptr;
62 
63  protected:
66  ReweightScheme reweight_;
67 
68  public:
69  Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
70  virtual ~Base() {}
71 
72  /*
73  * This method is responsible for returning the total penalty for a given
74  * amount of error. For example, this method is responsible for implementing
75  * the quadratic function for an L2 penalty, the absolute value function for
76  * an L1 penalty, etc.
77  *
78  * TODO(mikebosse): When the loss function has as input the norm of the
79  * error vector, then it prevents implementations of asymmeric loss
80  * functions. It would be better for this function to accept the vector and
81  * internally call the norm if necessary.
82  */
83  virtual double loss(double distance) const { return 0; };
84 
85  /*
86  * This method is responsible for returning the weight function for a given
87  * amount of error. The weight function is related to the analytic derivative
88  * of the loss function. See
89  * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
90  * for details. This method is required when optimizing cost functions with
91  * robust penalties using iteratively re-weighted least squares.
92  */
93  virtual double weight(double distance) const = 0;
94 
95  virtual void print(const std::string &s) const = 0;
96  virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
97 
98  double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
99 
102  Vector weight(const Vector &error) const;
103 
105  Vector sqrtWeight(const Vector &error) const {
106  return weight(error).cwiseSqrt();
107  }
108 
111  void reweight(Vector &error) const;
112  void reweight(std::vector<Matrix> &A, Vector &error) const;
113  void reweight(Matrix &A, Vector &error) const;
114  void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
115  void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
116 
117  private:
119  friend class boost::serialization::access;
120  template <class ARCHIVE>
121  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
122  ar &BOOST_SERIALIZATION_NVP(reweight_);
123  }
124 };
125 
127 class GTSAM_EXPORT Null : public Base {
128  public:
129  typedef boost::shared_ptr<Null> shared_ptr;
130 
131  Null(const ReweightScheme reweight = Block) : Base(reweight) {}
132  ~Null() {}
133  double weight(double /*error*/) const override { return 1.0; }
134  double loss(double distance) const override { return 0.5 * distance * distance; }
135  void print(const std::string &s) const override;
136  bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
137  static shared_ptr Create();
138 
139  private:
141  friend class boost::serialization::access;
142  template <class ARCHIVE>
143  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
144  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
145  }
146 };
147 
149 class GTSAM_EXPORT Fair : public Base {
150  protected:
151  double c_;
152 
153  public:
154  typedef boost::shared_ptr<Fair> shared_ptr;
155 
156  Fair(double c = 1.3998, const ReweightScheme reweight = Block);
157  double weight(double distance) const override;
158  double loss(double distance) const override;
159  void print(const std::string &s) const override;
160  bool equals(const Base &expected, double tol = 1e-8) const override;
161  static shared_ptr Create(double c, const ReweightScheme reweight = Block);
162 
163  private:
165  friend class boost::serialization::access;
166  template <class ARCHIVE>
167  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
168  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
169  ar &BOOST_SERIALIZATION_NVP(c_);
170  }
171 };
172 
174 class GTSAM_EXPORT Huber : public Base {
175  protected:
176  double k_;
177 
178  public:
179  typedef boost::shared_ptr<Huber> shared_ptr;
180 
181  Huber(double k = 1.345, const ReweightScheme reweight = Block);
182  double weight(double distance) const override;
183  double loss(double distance) const override;
184  void print(const std::string &s) const override;
185  bool equals(const Base &expected, double tol = 1e-8) const override;
186  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
187 
188  private:
190  friend class boost::serialization::access;
191  template <class ARCHIVE>
192  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
193  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
194  ar &BOOST_SERIALIZATION_NVP(k_);
195  }
196 };
197 
204 class GTSAM_EXPORT Cauchy : public Base {
205  protected:
206  double k_, ksquared_;
207 
208  public:
209  typedef boost::shared_ptr<Cauchy> shared_ptr;
210 
211  Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
212  double weight(double distance) const override;
213  double loss(double distance) const override;
214  void print(const std::string &s) const override;
215  bool equals(const Base &expected, double tol = 1e-8) const override;
216  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
217 
218  private:
220  friend class boost::serialization::access;
221  template <class ARCHIVE>
222  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
223  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
224  ar &BOOST_SERIALIZATION_NVP(k_);
225  }
226 };
227 
229 class GTSAM_EXPORT Tukey : public Base {
230  protected:
231  double c_, csquared_;
232 
233  public:
234  typedef boost::shared_ptr<Tukey> shared_ptr;
235 
236  Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
237  double weight(double distance) const override;
238  double loss(double distance) const override;
239  void print(const std::string &s) const override;
240  bool equals(const Base &expected, double tol = 1e-8) const override;
241  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
242 
243  private:
245  friend class boost::serialization::access;
246  template <class ARCHIVE>
247  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
248  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
249  ar &BOOST_SERIALIZATION_NVP(c_);
250  }
251 };
252 
254 class GTSAM_EXPORT Welsch : public Base {
255  protected:
256  double c_, csquared_;
257 
258  public:
259  typedef boost::shared_ptr<Welsch> shared_ptr;
260 
261  Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
262  double weight(double distance) const override;
263  double loss(double distance) const override;
264  void print(const std::string &s) const override;
265  bool equals(const Base &expected, double tol = 1e-8) const override;
266  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
267 
268  private:
270  friend class boost::serialization::access;
271  template <class ARCHIVE>
272  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
273  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
274  ar &BOOST_SERIALIZATION_NVP(c_);
275  }
276 };
277 
284 class GTSAM_EXPORT GemanMcClure : public Base {
285  public:
286  typedef boost::shared_ptr<GemanMcClure> shared_ptr;
287 
288  GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
289  ~GemanMcClure() {}
290  double weight(double distance) const override;
291  double loss(double distance) const override;
292  void print(const std::string &s) const override;
293  bool equals(const Base &expected, double tol = 1e-8) const override;
294  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
295 
296  protected:
297  double c_;
298 
299  private:
301  friend class boost::serialization::access;
302  template <class ARCHIVE>
303  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
304  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
305  ar &BOOST_SERIALIZATION_NVP(c_);
306  }
307 };
308 
314 class GTSAM_EXPORT DCS : public Base {
315  public:
316  typedef boost::shared_ptr<DCS> shared_ptr;
317 
318  DCS(double c = 1.0, const ReweightScheme reweight = Block);
319  ~DCS() {}
320  double weight(double distance) const override;
321  double loss(double distance) const override;
322  void print(const std::string &s) const override;
323  bool equals(const Base &expected, double tol = 1e-8) const override;
324  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
325 
326  protected:
327  double c_;
328 
329  private:
331  friend class boost::serialization::access;
332  template <class ARCHIVE>
333  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
334  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
335  ar &BOOST_SERIALIZATION_NVP(c_);
336  }
337 };
338 
345 class GTSAM_EXPORT L2WithDeadZone : public Base {
346  protected:
347  double k_;
348 
349  public:
350  typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
351 
352  L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
353  double weight(double distance) const override;
354  double loss(double distance) const override;
355  void print(const std::string &s) const override;
356  bool equals(const Base &expected, double tol = 1e-8) const override;
357  static shared_ptr Create(double k, const ReweightScheme reweight = Block);
358 
359  private:
361  friend class boost::serialization::access;
362  template <class ARCHIVE>
363  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
364  ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
365  ar &BOOST_SERIALIZATION_NVP(k_);
366  }
367 };
368 
369 } // namespace mEstimator
370 } // namespace noiseModel
371 } // namespace gtsam
gtsam::noiseModel::mEstimator::Huber
Huber implements the "Huber" robust error model (Zhang97ivc)
Definition: LossFunctions.h:174
Testable.h
Concept check for values that can be used in unit tests.
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::noiseModel::mEstimator::Null
Null class should behave as Gaussian.
Definition: LossFunctions.h:127
gtsam::noiseModel::mEstimator::Tukey
Tukey implements the "Tukey" robust error model (Zhang97ivc)
Definition: LossFunctions.h:229
gtsam::noiseModel::mEstimator::Base
Definition: LossFunctions.h:58
gtsam::noiseModel::mEstimator::GemanMcClure
GemanMcClure implements the "Geman-McClure" robust error model (Zhang97ivc).
Definition: LossFunctions.h:284
gtsam::noiseModel::mEstimator::Base::sqrtWeight
Vector sqrtWeight(const Vector &error) const
square root version of the weight function
Definition: LossFunctions.h:105
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::noiseModel::mEstimator::L2WithDeadZone
L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k,...
Definition: LossFunctions.h:345
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::noiseModel::mEstimator::Base::reweight_
ReweightScheme reweight_
the rows can be weighted independently according to the error or uniformly with the norm of the right...
Definition: LossFunctions.h:66
gtsam::noiseModel::mEstimator::Fair
Fair implements the "Fair" robust error model (Zhang97ivc)
Definition: LossFunctions.h:149
gtsam::noiseModel::mEstimator::DCS
DCS implements the Dynamic Covariance Scaling robust error model from the paper Robust Map Optimizati...
Definition: LossFunctions.h:314
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::noiseModel::mEstimator::Cauchy
Cauchy implements the "Cauchy" robust error model (Lee2013IROS).
Definition: LossFunctions.h:204
gtsam::noiseModel::mEstimator::Welsch
Welsch implements the "Welsch" robust error model (Zhang97ivc)
Definition: LossFunctions.h:254