From 4be8d8a75853409b44599e0df7bb160896f4838f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 8 Oct 2019 20:20:29 -0400 Subject: [PATCH] Robust Estimator Updates - Moved implementations of residual and weight to NoiseModel.cpp. - Added `const` to function arguments and variables in `residual` and `weight`. - Aliased Welsh to Welsch. - Formatting. --- gtsam/linear/NoiseModel.cpp | 76 ++++++++++++++++++++++-- gtsam/linear/NoiseModel.h | 113 +++++++----------------------------- 2 files changed, 92 insertions(+), 97 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index d4cf2a5c2..67aa20b10 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -718,15 +718,25 @@ void Null::print(const std::string &s="") const 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."); } } -/* ************************************************************************* */ -// Fair -/* ************************************************************************* */ +double Fair::weight(const double error) const { + return 1.0 / (1.0 + std::abs(error) / c_); +} +double Fair::residual(const 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; } @@ -750,6 +760,20 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { } } +double Huber::weight(const double error) const { + const double absError = std::abs(error); + return (absError <= k_) ? (1.0) : (k_ / absError); +} + +double Huber::residual(const 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; } @@ -774,6 +798,16 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), } } +double Cauchy::weight(const double error) const { + return ksquared_ / (ksquared_ + error*error); +} + +double Cauchy::residual(const 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; } @@ -791,7 +825,30 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} + +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(const 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(const 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; @@ -810,8 +867,19 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Welsch /* ************************************************************************* */ + Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} +double Welsch::weight(const double error) const { + const double xc2 = (error*error)/csquared_; + return std::exp(-xc2); +} + +double Welsch::residual(const 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; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2ef830c81..5f957ba01 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include #include @@ -677,7 +676,7 @@ namespace gtsam { * 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; }; + virtual double residual(const double error) const { return 0; }; /* * This method is responsible for returning the weight function for a given amount of error. @@ -686,7 +685,7 @@ namespace gtsam { * 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 double weight(const 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; @@ -727,8 +726,8 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(double /*error*/) const { return 1.0; } - virtual double residual(double error) const { return error; } + virtual double weight(const double /*error*/) const { return 1.0; } + virtual double residual(const double error) const { return error; } virtual void print(const std::string &s) const; virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; @@ -751,15 +750,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const { - return 1.0 / (1.0 + std::abs(error) / c_); - } - double residual(double error) const { - double absError = std::abs(error); - double normalizedError = absError / c_; - double val = normalizedError - std::log(1 + normalizedError); - return c_ * c_ * val; - } + double weight(const double error) const; + double residual(const double error) const; 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) ; @@ -783,18 +775,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const { - double absError = std::abs(error); - return (absError < k_) ? (1.0) : (k_ / absError); - } - double residual(double error) const { - double absError = std::abs(error); - if (absError <= k_) { // |x| <= k - return error*error / 2; - } else { // |x| > k - return k_ * (absError - (k_/2)); - } - } + double weight(const double error) const; + double residual(const double error) const; 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) ; @@ -822,14 +804,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const { - return ksquared_ / (ksquared_ + error*error); - } - double residual(double error) const { - double normalizedError = error / k_; - double val = std::log(1 + (normalizedError*normalizedError)); - return ksquared_ * val * 0.5; - } + double weight(const double error) const; + double residual(const double error) const; 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) ; @@ -853,23 +829,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const { - if (std::abs(error) <= c_) { - double xc2 = error*error/csquared_; - return (1.0-xc2)*(1.0-xc2); - } - return 0.0; - } - double residual(double error) const { - double absError = std::abs(error); - if (absError <= c_) { - double t = csquared_ - (error*error); - double val = t * t * t; - return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_); - } else { - return csquared_ / 6.0; - } - } + double weight(const double error) const; + double residual(const double error) const; 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) ; @@ -893,14 +854,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const { - double xc2 = (error*error)/csquared_; - return std::exp(-xc2); - } - double residual(double error) const { - double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); - } + double weight(const double error) const; + double residual(const double error) const; 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) ; @@ -920,35 +875,7 @@ namespace gtsam { // Welsh implements the "Welsch" robust error model (Zhang97ivc) // This was misspelled in previous versions of gtsam and should be // removed in the future. - 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 weight(double error) const { - double xc2 = (error*error)/csquared_; - return std::exp(-xc2); - } - double residual(double error) const { - double xc2 = (error*error)/csquared_; - return csquared_ * 0.5 * (1 - std::exp(-xc2) ); - } - 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_); - } - }; + using Welsh = Welsch; #endif /// GemanMcClure implements the "Geman-McClure" robust error model @@ -963,8 +890,8 @@ namespace gtsam { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); virtual ~GemanMcClure() {} - virtual double weight(double error) const; - virtual double residual(double error) const; + virtual double weight(const double error) const; + virtual double residual(const 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) ; @@ -993,7 +920,7 @@ namespace gtsam { DCS(double c = 1.0, const ReweightScheme reweight = Block); virtual ~DCS() {} - virtual double weight(double error) const; + virtual double weight(const 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) ; @@ -1024,11 +951,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; L2WithDeadZone(double k, const ReweightScheme reweight = Block); - double residual(double error) const { + double residual(const double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } - double weight(double error) const { + double weight(const 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.