24#include <gtsam/dllexport.h>
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>
68 typedef boost::shared_ptr<Base> shared_ptr;
94 virtual double loss(
double distance)
const {
return 0; }
106 virtual double weight(
double distance)
const = 0;
108 virtual void print(
const std::string &s)
const = 0;
109 virtual bool equals(
const Base &expected,
double tol = 1e-8)
const = 0;
111 double sqrtWeight(
double distance)
const {
return std::sqrt(weight(distance)); }
115 Vector weight(
const Vector &error)
const;
118 Vector sqrtWeight(
const Vector &error)
const;
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;
130 friend class boost::serialization::access;
131 template <
class ARCHIVE>
132 void serialize(ARCHIVE &ar,
const unsigned int ) {
133 ar &BOOST_SERIALIZATION_NVP(reweight_);
148 typedef boost::shared_ptr<Null> shared_ptr;
152 double weight(
double )
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 & ,
double )
const override {
return true; }
156 static shared_ptr Create();
160 friend class boost::serialization::access;
161 template <
class ARCHIVE>
162 void serialize(ARCHIVE &ar,
const unsigned int ) {
163 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
180 typedef boost::shared_ptr<Fair> shared_ptr;
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_; }
192 friend class boost::serialization::access;
193 template <
class ARCHIVE>
194 void serialize(ARCHIVE &ar,
const unsigned int ) {
195 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
196 ar &BOOST_SERIALIZATION_NVP(c_);
213 typedef boost::shared_ptr<Huber> shared_ptr;
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_; }
225 friend class boost::serialization::access;
226 template <
class ARCHIVE>
227 void serialize(ARCHIVE &ar,
const unsigned int ) {
228 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
229 ar &BOOST_SERIALIZATION_NVP(k_);
248 double k_, ksquared_;
251 typedef boost::shared_ptr<Cauchy> shared_ptr;
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_; }
263 friend class boost::serialization::access;
264 template <
class ARCHIVE>
265 void serialize(ARCHIVE &ar,
const unsigned int ) {
266 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
267 ar &BOOST_SERIALIZATION_NVP(k_);
268 ar &BOOST_SERIALIZATION_NVP(ksquared_);
282 double c_, csquared_;
285 typedef boost::shared_ptr<Tukey> shared_ptr;
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_; }
297 friend class boost::serialization::access;
298 template <
class ARCHIVE>
299 void serialize(ARCHIVE &ar,
const unsigned int ) {
300 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
301 ar &BOOST_SERIALIZATION_NVP(c_);
315 double c_, csquared_;
318 typedef boost::shared_ptr<Welsch> shared_ptr;
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_; }
330 friend class boost::serialization::access;
331 template <
class ARCHIVE>
332 void serialize(ARCHIVE &ar,
const unsigned int ) {
333 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
334 ar &BOOST_SERIALIZATION_NVP(c_);
335 ar &BOOST_SERIALIZATION_NVP(csquared_);
351 typedef boost::shared_ptr<GemanMcClure> shared_ptr;
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_; }
367 friend class boost::serialization::access;
368 template <
class ARCHIVE>
369 void serialize(ARCHIVE &ar,
const unsigned int ) {
370 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
371 ar &BOOST_SERIALIZATION_NVP(c_);
389 typedef boost::shared_ptr<DCS> shared_ptr;
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_; }
405 friend class boost::serialization::access;
406 template <
class ARCHIVE>
407 void serialize(ARCHIVE &ar,
const unsigned int ) {
408 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
409 ar &BOOST_SERIALIZATION_NVP(c_);
431 typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
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_; }
443 friend class boost::serialization::access;
444 template <
class ARCHIVE>
445 void serialize(ARCHIVE &ar,
const unsigned int ) {
446 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
447 ar &BOOST_SERIALIZATION_NVP(k_);
typedef and functions to augment Eigen's MatrixXd
Concept check for values that can be used in unit tests.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
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