From 12930acdf627b4e82c80ab06114b9bc61bb8cfb4 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Fri, 3 Apr 2020 14:04:02 -0400 Subject: [PATCH] noise in robust should be gaussian, change variable names --- gtsam/linear/LossFunctions.cpp | 74 +++++++++++++++++----------------- gtsam/linear/LossFunctions.h | 40 +++++++++--------- gtsam/linear/NoiseModel.cpp | 7 ++++ gtsam/linear/NoiseModel.h | 13 +++--- 4 files changed, 71 insertions(+), 63 deletions(-) diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 8bb670a92..45ad14f0f 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -137,12 +137,12 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } } -double Fair::weight(double error) const { - return 1.0 / (1.0 + std::abs(error) / c_); +double Fair::weight(double distance) const { + return 1.0 / (1.0 + std::abs(distance) / c_); } -double Fair::loss(double error) const { - const double absError = std::abs(error); +double Fair::loss(double distance) const { + const double absError = std::abs(distance); const double normalizedError = absError / c_; const double c_2 = c_ * c_; return c_2 * (normalizedError - std::log1p(normalizedError)); @@ -170,15 +170,15 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { } } -double Huber::weight(double error) const { - const double absError = std::abs(error); +double Huber::weight(double distance) const { + const double absError = std::abs(distance); return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::loss(double error) const { - const double absError = std::abs(error); +double Huber::loss(double distance) const { + const double absError = std::abs(distance); if (absError <= k_) { // |x| <= k - return error*error / 2; + return distance*distance / 2; } else { // |x| > k return k_ * (absError - (k_/2)); } @@ -208,12 +208,12 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), } } -double Cauchy::weight(double error) const { - return ksquared_ / (ksquared_ + error*error); +double Cauchy::weight(double distance) const { + return ksquared_ / (ksquared_ + distance*distance); } -double Cauchy::loss(double error) const { - const double val = std::log1p(error * error / ksquared_); +double Cauchy::loss(double distance) const { + const double val = std::log1p(distance * distance / ksquared_); return ksquared_ * val * 0.5; } @@ -241,18 +241,18 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c } } -double Tukey::weight(double error) const { - if (std::abs(error) <= c_) { - const double one_minus_xc2 = 1.0 - error*error/csquared_; +double Tukey::weight(double distance) const { + if (std::abs(distance) <= c_) { + const double one_minus_xc2 = 1.0 - distance*distance/csquared_; return one_minus_xc2 * one_minus_xc2; } return 0.0; } -double Tukey::loss(double error) const { - double absError = std::abs(error); +double Tukey::loss(double distance) const { + double absError = std::abs(distance); if (absError <= c_) { - const double one_minus_xc2 = 1.0 - error*error/csquared_; + const double one_minus_xc2 = 1.0 - distance*distance/csquared_; const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2; return csquared_ * (1 - t) / 6.0; } else { @@ -280,13 +280,13 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { 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_; +double Welsch::weight(double distance) const { + const double xc2 = (distance*distance)/csquared_; return std::exp(-xc2); } -double Welsch::loss(double error) const { - const double xc2 = (error*error)/csquared_; +double Welsch::loss(double distance) const { + const double xc2 = (distance*distance)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -311,16 +311,16 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double GemanMcClure::weight(double error) const { +double GemanMcClure::weight(double distance) const { const double c2 = c_*c_; const double c4 = c2*c2; - const double c2error = c2 + error*error; + const double c2error = c2 + distance*distance; return c4/(c2error*c2error); } -double GemanMcClure::loss(double error) const { +double GemanMcClure::loss(double distance) const { const double c2 = c_*c_; - const double error2 = error*error; + const double error2 = distance*distance; return 0.5 * (c2 * error2) / (c2 + error2); } @@ -345,8 +345,8 @@ DCS::DCS(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double DCS::weight(double error) const { - const double e2 = error*error; +double DCS::weight(double distance) const { + const double e2 = distance*distance; if (e2 > c_) { const double w = 2.0*c_/(c_ + e2); @@ -356,10 +356,10 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::loss(double error) const { +double DCS::loss(double distance) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. - const double e2 = error*error; + const double e2 = distance*distance; const double e4 = e2*e2; const double c2 = c_*c_; @@ -391,17 +391,17 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) } } -double L2WithDeadZone::weight(double error) const { +double L2WithDeadZone::weight(double distance) 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; + if (std::abs(distance) <= k_) return 0.0; + else if (distance > k_) return (-k_+distance)/distance; + else return (k_+distance)/distance; } -double L2WithDeadZone::loss(double error) const { - const double abs_error = std::abs(error); +double L2WithDeadZone::loss(double distance) const { + const double abs_error = std::abs(distance); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1b6f444e8..8d569a5df 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -80,10 +80,10 @@ class GTSAM_EXPORT Base { * functions. It would be better for this function to accept the vector and * internally call the norm if necessary. */ - virtual double loss(double error) const { return 0; }; + virtual double loss(double distance) const { return 0; }; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double residual(double error) const { return loss(error); }; + virtual double residual(double distance) const { return loss(distance); }; #endif /* * This method is responsible for returning the weight function for a given @@ -93,12 +93,12 @@ class GTSAM_EXPORT Base { * 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(double distance) const = 0; virtual void print(const std::string &s) const = 0; virtual bool equals(const Base &expected, double tol = 1e-8) const = 0; - double sqrtWeight(double error) const { return std::sqrt(weight(error)); } + double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); } /** produce a weight vector according to an error vector and the implemented * robust function */ @@ -157,8 +157,8 @@ class GTSAM_EXPORT Fair : public Base { typedef boost::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -182,8 +182,8 @@ class GTSAM_EXPORT Huber : public Base { typedef boost::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -212,8 +212,8 @@ class GTSAM_EXPORT Cauchy : public Base { typedef boost::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -237,8 +237,8 @@ class GTSAM_EXPORT Tukey : public Base { typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -262,8 +262,8 @@ class GTSAM_EXPORT Welsch : public Base { typedef boost::shared_ptr shared_ptr; Welsch(double c = 2.9846, const ReweightScheme reweight = Block); - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -298,8 +298,8 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -328,8 +328,8 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -361,8 +361,8 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { typedef boost::shared_ptr shared_ptr; L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); - double weight(double error) const override; - double loss(double error) const override; + double weight(double distance) const override; + double loss(double distance) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index e0ca3726b..5855ecad4 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -654,6 +654,13 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{ robust_->reweight(A1,A2,A3,b); } +Robust::shared_ptr Robust::Create(const RobustModel::shared_ptr& robust, + const noiseModel::Base::shared_ptr noise) { + SharedGaussian gaussian; + gaussian = boost::dynamic_pointer_cast(noise); + return shared_ptr(new Robust(robust, gaussian)); +} + Robust::shared_ptr Robust::Create( const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ return shared_ptr(new Robust(robust,noise)); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 627c0de2b..dff94c874 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -90,7 +90,7 @@ namespace gtsam { /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; - /// calculate the error value given error vector + /// calculate the error value given measurement error vector virtual double error(const Vector& v) const = 0; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -677,7 +677,7 @@ namespace gtsam { protected: typedef mEstimator::Base RobustModel; - typedef noiseModel::Base NoiseModel; + typedef noiseModel::Gaussian NoiseModel; const RobustModel::shared_ptr robust_; ///< robust error function used const NoiseModel::shared_ptr noise_; ///< noise model used @@ -712,9 +712,7 @@ namespace gtsam { { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } // Fold the use of the m-estimator loss(...) function into error(...) inline virtual double error(const Vector& v) const - { return robust_->loss(this->unweightedWhiten(v).norm()); } - // inline virtual double distance_non_whitened(const Vector& v) const - // { return robust_->loss(v.norm()); } + { return robust_->loss(noise_->mahalanobisDistance(v)); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; @@ -723,13 +721,16 @@ namespace gtsam { virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; virtual Vector unweightedWhiten(const Vector& v) const { - return noise_->unweightedWhiten(v); + return noise_->whiten(v); } virtual double weight(const Vector& v) const { // Todo(mikebosse): make the robust weight function input a vector. return robust_->weight(v.norm()); } + static shared_ptr Create( + const RobustModel::shared_ptr &robust, const noiseModel::Base::shared_ptr noise); + static shared_ptr Create( const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);