From 73542d980ab6cef78a2866b404ac01d05d6178a5 Mon Sep 17 00:00:00 2001 From: Francesco Papi Date: Tue, 12 Nov 2019 10:27:34 -0800 Subject: [PATCH] Move mEstimator into separate file --- gtsam/linear/LossFunctions.cpp | 423 +++++++++++++++++++++++++++++++++ gtsam/linear/LossFunctions.h | 379 +++++++++++++++++++++++++++++ gtsam/linear/NoiseModel.cpp | 397 ------------------------------- gtsam/linear/NoiseModel.h | 346 +-------------------------- 4 files changed, 804 insertions(+), 741 deletions(-) create mode 100644 gtsam/linear/LossFunctions.cpp create mode 100644 gtsam/linear/LossFunctions.h diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp new file mode 100644 index 000000000..80b3e3312 --- /dev/null +++ b/gtsam/linear/LossFunctions.cpp @@ -0,0 +1,423 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NoiseModel.cpp + * @date Jan 13, 2010 + * @author Richard Roberts + * @author Frank Dellaert + */ + +#include + +using namespace std; + +namespace gtsam { +namespace noiseModel { + +/* ************************************************************************* */ +// M-Estimator +/* ************************************************************************* */ + +namespace mEstimator { + +Vector Base::weight(const Vector& error) const { + const size_t n = error.rows(); + Vector w(n); + for (size_t i = 0; i < n; ++i) + w(i) = weight(error(i)); + return w; +} + +// The following three functions re-weight block matrices and a vector +// according to their weight implementation + +void Base::reweight(Vector& error) const { + if (reweight_ == Block) { + const double w = sqrtWeight(error.norm()); + error *= w; + } else { + error.array() *= weight(error).cwiseSqrt().array(); + } +} + +// Reweight n block matrices with one error vector +void Base::reweight(vector &A, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + for(Matrix& Aj: A) { + Aj *= w; + } + error *= w; + } + else { + const Vector W = sqrtWeight(error); + for(Matrix& Aj: A) { + vector_scale_inplace(W,Aj); + } + error = W.cwiseProduct(error); + } +} + +// Reweight one block matrix with one error vector +void Base::reweight(Matrix &A, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + A *= w; + error *= w; + } + else { + const Vector W = sqrtWeight(error); + vector_scale_inplace(W,A); + error = W.cwiseProduct(error); + } +} + +// Reweight two block matrix with one error vector +void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + A1 *= w; + A2 *= w; + error *= w; + } + else { + const Vector W = sqrtWeight(error); + vector_scale_inplace(W,A1); + vector_scale_inplace(W,A2); + error = W.cwiseProduct(error); + } +} + +// Reweight three block matrix with one error vector +void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { + if ( reweight_ == Block ) { + const double w = sqrtWeight(error.norm()); + A1 *= w; + A2 *= w; + A3 *= w; + error *= w; + } + else { + const Vector W = sqrtWeight(error); + vector_scale_inplace(W,A1); + vector_scale_inplace(W,A2); + vector_scale_inplace(W,A3); + error = W.cwiseProduct(error); + } +} + +/* ************************************************************************* */ +// Null model +/* ************************************************************************* */ + +void Null::print(const std::string &s="") const +{ cout << s << "null ()" << endl; } + +Null::shared_ptr Null::Create() +{ return shared_ptr(new Null()); } + +/* ************************************************************************* */ +// Fair +/* ************************************************************************* */ + +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { + if (c_ <= 0) { + throw runtime_error("mEstimator Fair takes only positive double in constructor."); + } +} + +double Fair::weight(double error) const { + return 1.0 / (1.0 + std::abs(error) / c_); +} + +double Fair::residual(double error) const { + const double absError = std::abs(error); + const double normalizedError = absError / c_; + const double c_2 = c_ * c_; + return c_2 * (normalizedError - std::log(1 + normalizedError)); +} + +void Fair::print(const std::string &s="") const +{ cout << s << "fair (" << c_ << ")" << endl; } + +bool Fair::equals(const Base &expected, double tol) const { + const Fair* p = dynamic_cast (&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_ ) < tol; +} + +Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) +{ return shared_ptr(new Fair(c, reweight)); } + +/* ************************************************************************* */ +// Huber +/* ************************************************************************* */ + +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator Huber takes only positive double in constructor."); + } +} + +double Huber::weight(double error) const { + const double absError = std::abs(error); + return (absError <= k_) ? (1.0) : (k_ / absError); +} + +double Huber::residual(double error) const { + const double absError = std::abs(error); + if (absError <= k_) { // |x| <= k + return error*error / 2; + } else { // |x| > k + return k_ * (absError - (k_/2)); + } +} + +void Huber::print(const std::string &s="") const { + cout << s << "huber (" << k_ << ")" << endl; +} + +bool Huber::equals(const Base &expected, double tol) const { + const Huber* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(k_ - p->k_) < tol; +} + +Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Huber(c, reweight)); +} + +/* ************************************************************************* */ +// Cauchy +/* ************************************************************************* */ + +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { + if (k <= 0) { + throw runtime_error("mEstimator Cauchy takes only positive double in constructor."); + } +} + +double Cauchy::weight(double error) const { + return ksquared_ / (ksquared_ + error*error); +} + +double Cauchy::residual(double error) const { + const double xc2 = error / k_; + const double val = std::log(1 + (xc2*xc2)); + return ksquared_ * val * 0.5; +} + +void Cauchy::print(const std::string &s="") const { + cout << s << "cauchy (" << k_ << ")" << endl; +} + +bool Cauchy::equals(const Base &expected, double tol) const { + const Cauchy* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(ksquared_ - p->ksquared_) < tol; +} + +Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Cauchy(c, reweight)); +} + +/* ************************************************************************* */ +// Tukey +/* ************************************************************************* */ + +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) { + if (c <= 0) { + throw runtime_error("mEstimator Tukey takes only positive double in constructor."); + } +} + +double Tukey::weight(double error) const { + if (std::abs(error) <= c_) { + const double xc2 = error*error/csquared_; + return (1.0-xc2)*(1.0-xc2); + } + return 0.0; +} + +double Tukey::residual(double error) const { + double absError = std::abs(error); + if (absError <= c_) { + const double xc2 = error*error/csquared_; + const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); + return csquared_ * (1 - t) / 6.0; + } else { + return csquared_ / 6.0; + } +} + +void Tukey::print(const std::string &s="") const { + std::cout << s << ": Tukey (" << c_ << ")" << std::endl; +} + +bool Tukey::equals(const Base &expected, double tol) const { + const Tukey* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Tukey(c, reweight)); +} + +/* ************************************************************************* */ +// Welsch +/* ************************************************************************* */ + +Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} + +double Welsch::weight(double error) const { + const double xc2 = (error*error)/csquared_; + return std::exp(-xc2); +} + +double Welsch::residual(double error) const { + const double xc2 = (error*error)/csquared_; + return csquared_ * 0.5 * (1 - std::exp(-xc2) ); +} + +void Welsch::print(const std::string &s="") const { + std::cout << s << ": Welsch (" << c_ << ")" << std::endl; +} + +bool Welsch::equals(const Base &expected, double tol) const { + const Welsch* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Welsch(c, reweight)); +} + +/* ************************************************************************* */ +// GemanMcClure +/* ************************************************************************* */ +GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double GemanMcClure::weight(double error) const { + const double c2 = c_*c_; + const double c4 = c2*c2; + const double c2error = c2 + error*error; + return c4/(c2error*c2error); +} + +double GemanMcClure::residual(double error) const { + const double c2 = c_*c_; + const double error2 = error*error; + return 0.5 * (c2 * error2) / (c2 + error2); +} + +void GemanMcClure::print(const std::string &s="") const { + std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; +} + +bool GemanMcClure::equals(const Base &expected, double tol) const { + const GemanMcClure* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new GemanMcClure(c, reweight)); +} + +/* ************************************************************************* */ +// DCS +/* ************************************************************************* */ +DCS::DCS(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double DCS::weight(double error) const { + const double e2 = error*error; + if (e2 > c_) + { + const double w = 2.0*c_/(c_ + e2); + return w*w; + } + + return 1.0; +} + +double DCS::residual(double error) const { + // This is the simplified version of Eq 9 from (Agarwal13icra) + // after you simplify and cancel terms. + const double e2 = error*error; + const double e4 = e2*e2; + const double c2 = c_*c_; + + return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_)); +} + +void DCS::print(const std::string &s="") const { + std::cout << s << ": DCS (" << c_ << ")" << std::endl; +} + +bool DCS::equals(const Base &expected, double tol) const { + const DCS* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new DCS(c, reweight)); +} + +/* ************************************************************************* */ +// L2WithDeadZone +/* ************************************************************************* */ + +L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) + : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); + } +} + +double L2WithDeadZone::weight(double error) const { + // note that this code is slightly uglier than residual, because there are three distinct + // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two + // cases (deadzone, non-deadzone) in residual. + if (std::abs(error) <= k_) return 0.0; + else if (error > k_) return (-k_+error)/error; + else return (k_+error)/error; +} + +double L2WithDeadZone::residual(double error) const { + const double abs_error = std::abs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); +} + +void L2WithDeadZone::print(const std::string &s="") const { + std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; +} + +bool L2WithDeadZone::equals(const Base &expected, double tol) const { + const L2WithDeadZone* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(k_ - p->k_) < tol; +} + +L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { + return shared_ptr(new L2WithDeadZone(k, reweight)); +} + +} // namespace mEstimator +} // namespace noiseModel +} // gtsam diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h new file mode 100644 index 000000000..1f7cc1377 --- /dev/null +++ b/gtsam/linear/LossFunctions.h @@ -0,0 +1,379 @@ + +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file NoiseModel.h + * @date Jan 13, 2010 + * @author Richard Roberts + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtsam { +namespace noiseModel { +// clang-format off +/** + * The mEstimator name space contains all robust error functions. + * It mirrors the exposition at + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * + * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: + * + * Name Symbol Least-Squares L1-norm Huber + * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; + + protected: + /** the rows can be weighted independently according to the error + * or uniformly with the norm of the right hand side */ + ReweightScheme reweight_; + + public: + Base(const ReweightScheme reweight = Block) : reweight_(reweight) {} + virtual ~Base() {} + + /* + * This method is responsible for returning the total penalty for a given + * amount of error. For example, this method is responsible for implementing + * the quadratic function for an L2 penalty, the absolute value function for + * an L1 penalty, etc. + * + * TODO(mikebosse): When the residual function has as input the norm of the + * residual vector, then it prevents implementations of asymmeric loss + * functions. It would be better for this function to accept the vector and + * internally call the norm if necessary. + */ + virtual double residual(double error) const { return 0; }; + + /* + * This method is responsible for returning the weight function for a given + * amount of error. The weight function is related to the analytic derivative + * of the residual function. See + * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf + * for details. This method is required when optimizing cost functions with + * robust penalties using iteratively re-weighted least squares. + */ + virtual double weight(double error) 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 error) const { return std::sqrt(weight(error)); } + + /** produce a weight vector according to an error vector and the implemented + * robust function */ + Vector weight(const Vector &error) const; + + /** square root version of the weight function */ + Vector sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); + } + + /** reweight block matrices and a vector according to their weight + * implementation */ + void reweight(Vector &error) const; + void reweight(std::vector &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: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(reweight_); + } +}; + +/// Null class is not robust so is a Gaussian ? +class GTSAM_EXPORT Null : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + Null(const ReweightScheme reweight = Block) : Base(reweight) {} + ~Null() {} + double weight(double /*error*/) const { return 1.0; } + double residual(double error) const { return error; } + void print(const std::string &s) const; + bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } + static shared_ptr Create(); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } +}; + +/// Fair implements the "Fair" robust error model (Zhang97ivc) +class GTSAM_EXPORT Fair : public Base { + protected: + double c_; + + public: + typedef boost::shared_ptr shared_ptr; + + Fair(double c = 1.3998, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(double error) 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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// Huber implements the "Huber" robust error model (Zhang97ivc) +class GTSAM_EXPORT Huber : public Base { + protected: + double k_; + + public: + typedef boost::shared_ptr shared_ptr; + + Huber(double k = 1.345, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(double error) 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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(k_); + } +}; + +/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed +/// by: +/// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for +/// Information Technology, Karlsruhe, Germany. +/// oberlaender@fzi.de +/// Thanks Jan! +class GTSAM_EXPORT Cauchy : public Base { + protected: + double k_, ksquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Cauchy(double k = 0.1, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(double error) 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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(k_); + } +}; + +/// Tukey implements the "Tukey" robust error model (Zhang97ivc) +class GTSAM_EXPORT Tukey : public Base { + protected: + double c_, csquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Tukey(double c = 4.6851, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(double error) 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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// Welsch implements the "Welsch" robust error model (Zhang97ivc) +class GTSAM_EXPORT Welsch : public Base { + protected: + double c_, csquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Welsch(double c = 2.9846, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(double error) 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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +/// @name Deprecated +/// @{ +// Welsh implements the "Welsch" robust error model (Zhang97ivc) +// This was misspelled in previous versions of gtsam and should be +// removed in the future. +using Welsh = Welsch; +#endif + +/// GemanMcClure implements the "Geman-McClure" robust error model +/// (Zhang97ivc). +/// +/// Note that Geman-McClure weight function uses the parameter c == 1.0, +/// but here it's allowed to use different values, so we actually have +/// the generalized Geman-McClure from (Agarwal15phd). +class GTSAM_EXPORT GemanMcClure : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); + ~GemanMcClure() {} + double weight(double error) const override; + double residual(double error) 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); + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// DCS implements the Dynamic Covariance Scaling robust error model +/// from the paper Robust Map Optimization (Agarwal13icra). +/// +/// Under the special condition of the parameter c == 1.0 and not +/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. +class GTSAM_EXPORT DCS : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + DCS(double c = 1.0, const ReweightScheme reweight = Block); + ~DCS() {} + double weight(double error) const override; + double residual(double error) 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); + + protected: + double c_; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(c_); + } +}; + +/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of +/// width 2*k, centered at the origin. The resulting penalty within the dead +/// zone is always zero, and grows quadratically outside the dead zone. In this +/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being +/// robust to outliers. This penalty can be used to create barrier functions in +/// a general way. +class GTSAM_EXPORT L2WithDeadZone : public Base { + protected: + double k_; + + public: + typedef boost::shared_ptr shared_ptr; + + L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); + double weight(double error) const override; + double residual(double error) 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); + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + 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 diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index e0b7c8b17..4e599d45f 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -616,403 +616,6 @@ void Unit::print(const std::string& name) const { cout << name << "unit (" << dim_ << ") " << endl; } -/* ************************************************************************* */ -// M-Estimator -/* ************************************************************************* */ - -namespace mEstimator { - -Vector Base::weight(const Vector& error) const { - const size_t n = error.rows(); - Vector w(n); - for (size_t i = 0; i < n; ++i) - w(i) = weight(error(i)); - return w; -} - -// The following three functions re-weight block matrices and a vector -// according to their weight implementation - -void Base::reweight(Vector& error) const { - if (reweight_ == Block) { - const double w = sqrtWeight(error.norm()); - error *= w; - } else { - error.array() *= weight(error).cwiseSqrt().array(); - } -} - -// Reweight n block matrices with one error vector -void Base::reweight(vector &A, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - for(Matrix& Aj: A) { - Aj *= w; - } - error *= w; - } - else { - const Vector W = sqrtWeight(error); - for(Matrix& Aj: A) { - vector_scale_inplace(W,Aj); - } - error = W.cwiseProduct(error); - } -} - -// Reweight one block matrix with one error vector -void Base::reweight(Matrix &A, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - A *= w; - error *= w; - } - else { - const Vector W = sqrtWeight(error); - vector_scale_inplace(W,A); - error = W.cwiseProduct(error); - } -} - -// Reweight two block matrix with one error vector -void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - A1 *= w; - A2 *= w; - error *= w; - } - else { - const Vector W = sqrtWeight(error); - vector_scale_inplace(W,A1); - vector_scale_inplace(W,A2); - error = W.cwiseProduct(error); - } -} - -// Reweight three block matrix with one error vector -void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { - if ( reweight_ == Block ) { - const double w = sqrtWeight(error.norm()); - A1 *= w; - A2 *= w; - A3 *= w; - error *= w; - } - else { - const Vector W = sqrtWeight(error); - vector_scale_inplace(W,A1); - vector_scale_inplace(W,A2); - vector_scale_inplace(W,A3); - error = W.cwiseProduct(error); - } -} - -/* ************************************************************************* */ -// Null model -/* ************************************************************************* */ - -void Null::print(const std::string &s="") const -{ cout << s << "null ()" << endl; } - -Null::shared_ptr Null::Create() -{ return shared_ptr(new Null()); } - -/* ************************************************************************* */ -// Fair -/* ************************************************************************* */ - -Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { - if (c_ <= 0) { - throw runtime_error("mEstimator Fair takes only positive double in constructor."); - } -} - -double Fair::weight(double error) const { - return 1.0 / (1.0 + std::abs(error) / c_); -} - -double Fair::residual(double error) const { - const double absError = std::abs(error); - const double normalizedError = absError / c_; - const double c_2 = c_ * c_; - return c_2 * (normalizedError - std::log(1 + normalizedError)); -} - -void Fair::print(const std::string &s="") const -{ cout << s << "fair (" << c_ << ")" << endl; } - -bool Fair::equals(const Base &expected, double tol) const { - const Fair* p = dynamic_cast (&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_ ) < tol; -} - -Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) -{ return shared_ptr(new Fair(c, reweight)); } - -/* ************************************************************************* */ -// Huber -/* ************************************************************************* */ - -Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { - if (k_ <= 0) { - throw runtime_error("mEstimator Huber takes only positive double in constructor."); - } -} - -double Huber::weight(double error) const { - const double absError = std::abs(error); - return (absError <= k_) ? (1.0) : (k_ / absError); -} - -double Huber::residual(double error) const { - const double absError = std::abs(error); - if (absError <= k_) { // |x| <= k - return error*error / 2; - } else { // |x| > k - return k_ * (absError - (k_/2)); - } -} - -void Huber::print(const std::string &s="") const { - cout << s << "huber (" << k_ << ")" << endl; -} - -bool Huber::equals(const Base &expected, double tol) const { - const Huber* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(k_ - p->k_) < tol; -} - -Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Huber(c, reweight)); -} - -/* ************************************************************************* */ -// Cauchy -/* ************************************************************************* */ - -Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { - if (k <= 0) { - throw runtime_error("mEstimator Cauchy takes only positive double in constructor."); - } -} - -double Cauchy::weight(double error) const { - return ksquared_ / (ksquared_ + error*error); -} - -double Cauchy::residual(double error) const { - const double xc2 = error / k_; - const double val = std::log(1 + (xc2*xc2)); - return ksquared_ * val * 0.5; -} - -void Cauchy::print(const std::string &s="") const { - cout << s << "cauchy (" << k_ << ")" << endl; -} - -bool Cauchy::equals(const Base &expected, double tol) const { - const Cauchy* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(ksquared_ - p->ksquared_) < tol; -} - -Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Cauchy(c, reweight)); -} - -/* ************************************************************************* */ -// Tukey -/* ************************************************************************* */ - -Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) { - if (c <= 0) { - throw runtime_error("mEstimator Tukey takes only positive double in constructor."); - } -} - -double Tukey::weight(double error) const { - if (std::abs(error) <= c_) { - const double xc2 = error*error/csquared_; - return (1.0-xc2)*(1.0-xc2); - } - return 0.0; -} - -double Tukey::residual(double error) const { - double absError = std::abs(error); - if (absError <= c_) { - const double xc2 = error*error/csquared_; - const double t = (1 - xc2)*(1 - xc2)*(1 - xc2); - return csquared_ * (1 - t) / 6.0; - } else { - return csquared_ / 6.0; - } -} - -void Tukey::print(const std::string &s="") const { - std::cout << s << ": Tukey (" << c_ << ")" << std::endl; -} - -bool Tukey::equals(const Base &expected, double tol) const { - const Tukey* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Tukey(c, reweight)); -} - -/* ************************************************************************* */ -// Welsch -/* ************************************************************************* */ - -Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} - -double Welsch::weight(double error) const { - const double xc2 = (error*error)/csquared_; - return std::exp(-xc2); -} - -double Welsch::residual(double error) const { - const double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); -} - -void Welsch::print(const std::string &s="") const { - std::cout << s << ": Welsch (" << c_ << ")" << std::endl; -} - -bool Welsch::equals(const Base &expected, double tol) const { - const Welsch* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Welsch(c, reweight)); -} - -/* ************************************************************************* */ -// GemanMcClure -/* ************************************************************************* */ -GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double GemanMcClure::weight(double error) const { - const double c2 = c_*c_; - const double c4 = c2*c2; - const double c2error = c2 + error*error; - return c4/(c2error*c2error); -} - -double GemanMcClure::residual(double error) const { - const double c2 = c_*c_; - const double error2 = error*error; - return 0.5 * (c2 * error2) / (c2 + error2); -} - -void GemanMcClure::print(const std::string &s="") const { - std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; -} - -bool GemanMcClure::equals(const Base &expected, double tol) const { - const GemanMcClure* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new GemanMcClure(c, reweight)); -} - -/* ************************************************************************* */ -// DCS -/* ************************************************************************* */ -DCS::DCS(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double DCS::weight(double error) const { - const double e2 = error*error; - if (e2 > c_) - { - const double w = 2.0*c_/(c_ + e2); - return w*w; - } - - return 1.0; -} - -double DCS::residual(double error) const { - // This is the simplified version of Eq 9 from (Agarwal13icra) - // after you simplify and cancel terms. - const double e2 = error*error; - const double e4 = e2*e2; - const double c2 = c_*c_; - - return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_)); -} - -void DCS::print(const std::string &s="") const { - std::cout << s << ": DCS (" << c_ << ")" << std::endl; -} - -bool DCS::equals(const Base &expected, double tol) const { - const DCS* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new DCS(c, reweight)); -} - -/* ************************************************************************* */ -// L2WithDeadZone -/* ************************************************************************* */ - -L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) - : Base(reweight), k_(k) { - if (k_ <= 0) { - throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor."); - } -} - -double L2WithDeadZone::weight(double error) const { - // note that this code is slightly uglier than residual, because there are three distinct - // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two - // cases (deadzone, non-deadzone) in residual. - if (std::abs(error) <= k_) return 0.0; - else if (error > k_) return (-k_+error)/error; - else return (k_+error)/error; -} - -double L2WithDeadZone::residual(double error) const { - const double abs_error = std::abs(error); - return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); -} - -void L2WithDeadZone::print(const std::string &s="") const { - std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl; -} - -bool L2WithDeadZone::equals(const Base &expected, double tol) const { - const L2WithDeadZone* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(k_ - p->k_) < tol; -} - -L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { - return shared_ptr(new L2WithDeadZone(k, reweight)); -} - -} // namespace mEstimator - /* ************************************************************************* */ // Robust /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index f73fde51c..6fe50ed4c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -39,6 +40,7 @@ namespace gtsam { class Constrained; class Isotropic; class Unit; + class RobustModel; //--------------------------------------------------------------------------------------- @@ -626,350 +628,6 @@ namespace gtsam { } }; - /** - * The mEstimator name space contains all robust error functions. - * It mirrors the exposition at - * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. - * - * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: - * - * Name Symbol Least-Squares L1-norm Huber - * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; - - protected: - /** the rows can be weighted independently according to the error - * or uniformly with the norm of the right hand side */ - ReweightScheme reweight_; - - public: - Base(const ReweightScheme reweight = Block):reweight_(reweight) {} - virtual ~Base() {} - - /* - * This method is responsible for returning the total penalty for a given amount of error. - * For example, this method is responsible for implementing the quadratic function for an - * L2 penalty, the absolute value function for an L1 penalty, etc. - * - * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes - * implement a residual function, and GTSAM calls the weight function to evaluate the - * total penalty, rather than calling the residual function. The weight function should be - * used during iteratively reweighted least squares optimization, but should not be used to - * evaluate the total penalty. The long-term solution is for all mEstimators to implement - * both a weight and a residual function, and for GTSAM to call the residual function when - * evaluating the total penalty. But for now, I'm leaving this residual method as pure - * virtual, so the existing mEstimators can inherit this default fallback behavior. - */ - virtual double residual(double error) const { return 0; }; - - /* - * This method is responsible for returning the weight function for a given amount of error. - * The weight function is related to the analytic derivative of the residual function. See - * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * for details. This method is required when optimizing cost functions with robust penalties - * using iteratively re-weighted least squares. - */ - virtual double weight(double error) 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 error) const { - return std::sqrt(weight(error)); - } - - /** produce a weight vector according to an error vector and the implemented - * robust function */ - Vector weight(const Vector &error) const; - - /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const { - return weight(error).cwiseSqrt(); - } - - /** reweight block matrices and a vector according to their weight implementation */ - void reweight(Vector &error) const; - void reweight(std::vector &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: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(reweight_); - } - }; - - /// Null class is not robust so is a Gaussian ? - class GTSAM_EXPORT Null : public Base { - public: - typedef boost::shared_ptr shared_ptr; - - Null(const ReweightScheme reweight = Block) : Base(reweight) {} - ~Null() {} - double weight(double /*error*/) const { return 1.0; } - double residual(double error) const { return error; } - void print(const std::string &s) const; - bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } - static shared_ptr Create() ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } - }; - - /// Fair implements the "Fair" robust error model (Zhang97ivc) - class GTSAM_EXPORT Fair : public Base { - protected: - double c_; - - public: - typedef boost::shared_ptr shared_ptr; - - Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(double error) 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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// Huber implements the "Huber" robust error model (Zhang97ivc) - class GTSAM_EXPORT Huber : public Base { - protected: - double k_; - - public: - typedef boost::shared_ptr shared_ptr; - - Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(double error) 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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(k_); - } - }; - - /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: - /// Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for - /// Information Technology, Karlsruhe, Germany. - /// oberlaender@fzi.de - /// Thanks Jan! - class GTSAM_EXPORT Cauchy : public Base { - protected: - double k_, ksquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(double error) 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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(k_); - } - }; - - /// Tukey implements the "Tukey" robust error model (Zhang97ivc) - class GTSAM_EXPORT Tukey : public Base { - protected: - double c_, csquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(double error) 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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// Welsch implements the "Welsch" robust error model (Zhang97ivc) - class GTSAM_EXPORT Welsch : public Base { - protected: - double c_, csquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(double error) 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) ; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /// @name Deprecated - /// @{ - // Welsh implements the "Welsch" robust error model (Zhang97ivc) - // This was misspelled in previous versions of gtsam and should be - // removed in the future. - using Welsh = Welsch; -#endif - - /// GemanMcClure implements the "Geman-McClure" robust error model - /// (Zhang97ivc). - /// - /// Note that Geman-McClure weight function uses the parameter c == 1.0, - /// but here it's allowed to use different values, so we actually have - /// the generalized Geman-McClure from (Agarwal15phd). - class GTSAM_EXPORT GemanMcClure : public Base { - public: - typedef boost::shared_ptr shared_ptr; - - GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); - ~GemanMcClure() {} - double weight(double error) const override; - double residual(double error) 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) ; - - protected: - double c_; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// DCS implements the Dynamic Covariance Scaling robust error model - /// from the paper Robust Map Optimization (Agarwal13icra). - /// - /// Under the special condition of the parameter c == 1.0 and not - /// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. - class GTSAM_EXPORT DCS : public Base { - public: - typedef boost::shared_ptr shared_ptr; - - DCS(double c = 1.0, const ReweightScheme reweight = Block); - ~DCS() {} - double weight(double error) const override; - double residual(double error) 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) ; - - protected: - double c_; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(c_); - } - }; - - /// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k, - /// centered at the origin. The resulting penalty within the dead zone is always zero, and - /// grows quadratically outside the dead zone. In this sense, the L2WithDeadZone penalty is - /// "robust to inliers", rather than being robust to outliers. This penalty can be used to - /// create barrier functions in a general way. - class GTSAM_EXPORT L2WithDeadZone : public Base { - protected: - double k_; - - public: - typedef boost::shared_ptr shared_ptr; - - L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); - double weight(double error) const override; - double residual(double error) 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); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(k_); - } - }; - - } ///\namespace mEstimator - /** * Base class for robust error models * The robust M-estimators above simply tell us how to re-weight the residual, and are