From a1c828c8bb2ba1063945c0b85c618543eaaee4b5 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Wed, 8 Mar 2017 10:31:53 -0500 Subject: [PATCH] Revert "split M-Estimators out from NoiseModel" This reverts commit afb6c376301c9eb7ddb14428abd3b621509e44a2. --- gtsam/linear/MEstimators.cpp | 304 ----------------------------- gtsam/linear/MEstimators.h | 368 ----------------------------------- gtsam/linear/NoiseModel.cpp | 295 ++++++++++++++++++++++++++++ gtsam/linear/NoiseModel.h | 356 ++++++++++++++++++++++++++++++++- 4 files changed, 649 insertions(+), 674 deletions(-) delete mode 100644 gtsam/linear/MEstimators.cpp delete mode 100644 gtsam/linear/MEstimators.h diff --git a/gtsam/linear/MEstimators.cpp b/gtsam/linear/MEstimators.cpp deleted file mode 100644 index 36fa939a6..000000000 --- a/gtsam/linear/MEstimators.cpp +++ /dev/null @@ -1,304 +0,0 @@ -#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(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { - if (c_ <= 0) { - throw runtime_error("mEstimator Fair takes only positive double in constructor."); - } -} - -/* ************************************************************************* */ -// Fair -/* ************************************************************************* */ - -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."); - } -} - -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."); - } -} - -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) {} - -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)); -} - -/* ************************************************************************* */ -// Welsh -/* ************************************************************************* */ -Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} - -void Welsh::print(const std::string &s="") const { - std::cout << s << ": Welsh (" << c_ << ")" << std::endl; -} - -bool Welsh::equals(const Base &expected, double tol) const { - const Welsh* p = dynamic_cast(&expected); - if (p == NULL) return false; - return std::abs(c_ - p->c_) < tol; -} - -Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { - return shared_ptr(new Welsh(c, reweight)); -} - -/* ************************************************************************* */ -// GemanMcClure -/* ************************************************************************* */ -GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double GemanMcClure::sqrtWeight(double error) const { - const double c2 = c_*c_; - const double c2error = c2 + error*error; - return c2/c2error; -} - -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 fabs(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::sqrtWeight(double error) const { - const double e2 = error*error; - if (e2 > c_) - { - const double w = 2.0*c_/(c_ + e2); - return w; - } - - return 1.0; -} - -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 fabs(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."); - } -} - -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 fabs(k_ - p->k_) < tol; -} - -L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme reweight) { - return shared_ptr(new L2WithDeadZone(k, reweight)); -} - -} // namespace mEstimator - -} // noiseModel -} // gtsam diff --git a/gtsam/linear/MEstimators.h b/gtsam/linear/MEstimators.h deleted file mode 100644 index 77c8cee8e..000000000 --- a/gtsam/linear/MEstimators.h +++ /dev/null @@ -1,368 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include -#include -#include - -namespace gtsam { - namespace noiseModel { - /** - * The mEstimator name space contains all robust error functions. - * It mirrors the exposition at - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html - * 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() {} - ReweightScheme reweightScheme() const { return reweight_; } - - /* - * 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 - * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html - * for details. This method is required when optimizing cost functions with robust penalties - * using iteratively re-weighted least squares. - */ - double weight(double error) const { - return sqrtWeight(error)*sqrtWeight(error); - } - - virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - - virtual double sqrtWeight(double error) const = 0; - - /** 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) {} - virtual ~Null() {} - virtual double sqrtWeight(double /*error*/) const { return 1.0; } - virtual void print(const std::string &s) const; - virtual 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 sqrtWeight(double error) const { - return 1.0 / sqrt(1.0 + fabs(error) / c_); - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - 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 sqrtWeight(double error) const { - return (error < k_) ? (1.0) : sqrt(k_ / fabs(error)); - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - 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 sqrtWeight(double error) const { - return k_ / sqrt(ksquared_ + error*error); - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - 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 sqrtWeight(double error) const { - if (std::fabs(error) <= c_) { - double xc2 = error*error/csquared_; - return (1.0-xc2); - } - return 0.0; - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - 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_); - } - }; - - /// Welsh implements the "Welsh" robust error model (Zhang97ivc) - class GTSAM_EXPORT Welsh : public Base { - protected: - double c_, csquared_; - - public: - typedef boost::shared_ptr shared_ptr; - - Welsh(double c = 2.9846, const ReweightScheme reweight = Block); - double sqrtWeight(double error) const { - double xc2 = (error*error)/csquared_; - return std::exp(-xc2/2.0); - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - 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_); - } - }; - - /// 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); - virtual ~GemanMcClure() {} - virtual double sqrtWeight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; - 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); - virtual ~DCS() {} - virtual double sqrtWeight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; - 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, const ReweightScheme reweight = Block); - double residual(double error) const { - const double abs_error = fabs(error); - return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); - } - double sqrtWeight(double error) const { - // note that this code is slightly uglier than above, because there are three distinct - // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two - // cases (deadzone, non-deadzone) above. - if (fabs(error) <= k_) return 0.0; - else if (error > k_) return sqrt((-k_+error)/error); - else return sqrt((k_+error)/error); - } - void print(const std::string &s) const; - bool equals(const Base& expected, double tol=1e-8) const; - 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 - } -} \ No newline at end of file diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 4e599d45f..1569f61da 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -616,6 +616,301 @@ 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(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { + if (c_ <= 0) { + throw runtime_error("mEstimator Fair takes only positive double in constructor."); + } +} + +/* ************************************************************************* */ +// Fair +/* ************************************************************************* */ + +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."); + } +} + +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."); + } +} + +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) {} + +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)); +} + +/* ************************************************************************* */ +// Welsh +/* ************************************************************************* */ +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} + +void Welsh::print(const std::string &s="") const { + std::cout << s << ": Welsh (" << c_ << ")" << std::endl; +} + +bool Welsh::equals(const Base &expected, double tol) const { + const Welsh* p = dynamic_cast(&expected); + if (p == NULL) return false; + return std::abs(c_ - p->c_) < tol; +} + +Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new Welsh(c, reweight)); +} + +/* ************************************************************************* */ +// GemanMcClure +/* ************************************************************************* */ +GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double GemanMcClure::sqrtWeight(double error) const { + const double c2 = c_*c_; + const double c2error = c2 + error*error; + return c2/c2error; +} + +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 fabs(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::sqrtWeight(double error) const { + const double e2 = error*error; + if (e2 > c_) + { + const double w = 2.0*c_/(c_ + e2); + return w; + } + + return 1.0; +} + +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 fabs(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."); + } +} + +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 fabs(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 d8295e1cd..b1e7d562a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,8 +18,6 @@ #pragma once -#include - #include #include #include @@ -628,6 +626,360 @@ namespace gtsam { } }; + /** + * The mEstimator name space contains all robust error functions. + * It mirrors the exposition at + * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * 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() {} + ReweightScheme reweightScheme() const { return reweight_; } + + /* + * 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 + * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * for details. This method is required when optimizing cost functions with robust penalties + * using iteratively re-weighted least squares. + */ + double weight(double error) const { + return sqrtWeight(error)*sqrtWeight(error); + } + + virtual void print(const std::string &s) const = 0; + virtual bool equals(const Base& expected, double tol=1e-8) const = 0; + + virtual double sqrtWeight(double error) const = 0; + + /** 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) {} + virtual ~Null() {} + virtual double sqrtWeight(double /*error*/) const { return 1.0; } + virtual void print(const std::string &s) const; + virtual 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 sqrtWeight(double error) const { + return 1.0 / sqrt(1.0 + fabs(error) / c_); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + 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 sqrtWeight(double error) const { + return (error < k_) ? (1.0) : sqrt(k_ / fabs(error)); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + 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 sqrtWeight(double error) const { + return k_ / sqrt(ksquared_ + error*error); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + 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 sqrtWeight(double error) const { + if (std::fabs(error) <= c_) { + double xc2 = error*error/csquared_; + return (1.0-xc2); + } + return 0.0; + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + 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_); + } + }; + + /// Welsh implements the "Welsh" robust error model (Zhang97ivc) + class GTSAM_EXPORT Welsh : public Base { + protected: + double c_, csquared_; + + public: + typedef boost::shared_ptr shared_ptr; + + Welsh(double c = 2.9846, const ReweightScheme reweight = Block); + double sqrtWeight(double error) const { + double xc2 = (error*error)/csquared_; + return std::exp(-xc2/2.0); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + 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_); + } + }; + + /// 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); + virtual ~GemanMcClure() {} + virtual double sqrtWeight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + 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); + virtual ~DCS() {} + virtual double sqrtWeight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + 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, const ReweightScheme reweight = Block); + double residual(double error) const { + const double abs_error = fabs(error); + return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); + } + double sqrtWeight(double error) const { + // note that this code is slightly uglier than above, because there are three distinct + // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two + // cases (deadzone, non-deadzone) above. + if (fabs(error) <= k_) return 0.0; + else if (error > k_) return sqrt((-k_+error)/error); + else return sqrt((k_+error)/error); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; + 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