Loading...
Searching...
No Matches
LossFunctions.h
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/singleton.hpp>
namespace gtsam {
namespace noiseModel {
// clang-format off
// clang-format on
namespace mEstimator {
class GTSAM_EXPORT Base {
public:
enum ReweightScheme { Scalar, Block };
typedef boost::shared_ptr<Base> shared_ptr;
protected:
ReweightScheme reweight_;
public:
Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
virtual ~Base() {}
ReweightScheme reweightScheme() const { return reweight_; }
virtual double loss(double distance) const { return 0; }
virtual double weight(double distance) const = 0;
virtual void print(const std::string &s) const = 0;
virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
Vector weight(const Vector &error) const;
Vector sqrtWeight(const Vector &error) const;
void reweight(Vector &error) const;
void reweight(std::vector<Matrix> &A, Vector &error) const;
void reweight(Matrix &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_NVP(reweight_);
}
};
class GTSAM_EXPORT Null : public Base {
public:
typedef boost::shared_ptr<Null> shared_ptr;
Null(const ReweightScheme reweight = Block) : Base(reweight) {}
~Null() override {}
double weight(double /*error*/) const override { return 1.0; }
double loss(double distance) const override { return 0.5 * distance * distance; }
void print(const std::string &s) const override;
bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
static shared_ptr Create();
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
class GTSAM_EXPORT Fair : public Base {
protected:
double c_;
public:
typedef boost::shared_ptr<Fair> shared_ptr;
Fair(double c = 1.3998, const ReweightScheme reweight = Block);
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double c, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(c_);
}
};
class GTSAM_EXPORT Huber : public Base {
protected:
double k_;
public:
typedef boost::shared_ptr<Huber> shared_ptr;
Huber(double k = 1.345, const ReweightScheme reweight = Block);
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(k_);
}
};
class GTSAM_EXPORT Cauchy : public Base {
protected:
double k_, ksquared_;
public:
typedef boost::shared_ptr<Cauchy> shared_ptr;
Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(k_);
ar &BOOST_SERIALIZATION_NVP(ksquared_);
}
};
class GTSAM_EXPORT Tukey : public Base {
protected:
double c_, csquared_;
public:
typedef boost::shared_ptr<Tukey> shared_ptr;
Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(c_);
}
};
class GTSAM_EXPORT Welsch : public Base {
protected:
double c_, csquared_;
public:
typedef boost::shared_ptr<Welsch> shared_ptr;
Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(c_);
ar &BOOST_SERIALIZATION_NVP(csquared_);
}
};
class GTSAM_EXPORT GemanMcClure : public Base {
public:
typedef boost::shared_ptr<GemanMcClure> shared_ptr;
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
~GemanMcClure() override {}
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
protected:
double c_;
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(c_);
}
};
class GTSAM_EXPORT DCS : public Base {
public:
typedef boost::shared_ptr<DCS> shared_ptr;
DCS(double c = 1.0, const ReweightScheme reweight = Block);
~DCS() override {}
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return c_; }
protected:
double c_;
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(c_);
}
};
class GTSAM_EXPORT L2WithDeadZone : public Base {
protected:
double k_;
public:
typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
double modelParameter() const { return k_; }
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar &BOOST_SERIALIZATION_NVP(k_);
}
};
} // namespace mEstimator
} // namespace noiseModel
} // namespace gtsam
Matrix.h
typedef and functions to augment Eigen's MatrixXd
Testable.h
Concept check for values that can be used in unit tests.
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:156
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
gtsam::noiseModel::mEstimator
The mEstimator name space contains all robust error functions.
Definition LossFunctions.cpp:33
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::noiseModel::mEstimator::Base
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
gtsam::noiseModel::mEstimator::Base::loss
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
gtsam::noiseModel::mEstimator::Base::reweight_
ReweightScheme reweight_
Strategy for reweighting.
Definition LossFunctions.h:72
gtsam::noiseModel::mEstimator::Base::reweightScheme
ReweightScheme reweightScheme() const
Returns the reweight scheme, as explained in ReweightScheme.
Definition LossFunctions.h:79
gtsam::noiseModel::mEstimator::Base::reweight
void reweight(Vector &error) const
reweight block matrices and a vector according to their weight implementation
Definition LossFunctions.cpp:50
gtsam::noiseModel::mEstimator::Base::weight
virtual double weight(double distance) const =0
This method is responsible for returning the weight function for a given amount of error.
gtsam::noiseModel::mEstimator::Base::ReweightScheme
ReweightScheme
the rows can be weighted independently according to the error or uniformly with the norm of the right...
Definition LossFunctions.h:67
gtsam::noiseModel::mEstimator::Null::weight
double weight(double) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.h:152
gtsam::noiseModel::mEstimator::Null::loss
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
gtsam::noiseModel::mEstimator::Fair::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:149
gtsam::noiseModel::mEstimator::Fair::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:145
gtsam::noiseModel::mEstimator::Huber::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:178
gtsam::noiseModel::mEstimator::Huber::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:183
gtsam::noiseModel::mEstimator::Cauchy::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:216
gtsam::noiseModel::mEstimator::Cauchy::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:220
gtsam::noiseModel::mEstimator::Tukey::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:257
gtsam::noiseModel::mEstimator::Tukey::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:249
gtsam::noiseModel::mEstimator::Welsch::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:293
gtsam::noiseModel::mEstimator::Welsch::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:288
gtsam::noiseModel::mEstimator::GemanMcClure::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:326
gtsam::noiseModel::mEstimator::GemanMcClure::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:319
gtsam::noiseModel::mEstimator::DCS::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:353
gtsam::noiseModel::mEstimator::DCS::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:364
gtsam::noiseModel::mEstimator::L2WithDeadZone::weight
double weight(double distance) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.cpp:399
gtsam::noiseModel::mEstimator::L2WithDeadZone::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.cpp:408