From f4da76cea6e6d47421f324967541544c72bcac91 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 18:55:23 -0400 Subject: [PATCH] renamed residual to loss, follow PR #188 --- gtsam.h | 18 +++--- gtsam/linear/LossFunctions.cpp | 16 ++--- gtsam/linear/LossFunctions.h | 33 +++++----- gtsam/linear/NoiseModel.h | 6 +- gtsam/linear/tests/testNoiseModel.cpp | 64 ++++++++++---------- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- 6 files changed, 71 insertions(+), 68 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8ee778f4cf..1094d9dd92 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2cd..8bb670a925 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -141,7 +141,7 @@ double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::residual(double error) const { +double Fair::loss(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -175,7 +175,7 @@ double Huber::weight(double error) const { return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::residual(double error) const { +double Huber::loss(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -212,7 +212,7 @@ double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::residual(double error) const { +double Cauchy::loss(double error) const { const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -249,7 +249,7 @@ double Tukey::weight(double error) const { return 0.0; } -double Tukey::residual(double error) const { +double Tukey::loss(double error) const { double absError = std::abs(error); if (absError <= c_) { const double one_minus_xc2 = 1.0 - error*error/csquared_; @@ -285,7 +285,7 @@ double Welsch::weight(double error) const { return std::exp(-xc2); } -double Welsch::residual(double error) const { +double Welsch::loss(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } -double GemanMcClure::residual(double error) const { +double GemanMcClure::loss(double error) const { const double c2 = c_*c_; const double error2 = error*error; return 0.5 * (c2 * error2) / (c2 + error2); @@ -356,7 +356,7 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::residual(double error) const { +double DCS::loss(double error) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. const double e2 = error*error; @@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } -double L2WithDeadZone::residual(double error) const { +double L2WithDeadZone::loss(double error) const { const double abs_error = std::abs(error); 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 1f7cc1377b..1b6f444e85 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * 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. + * which talks about minimizing \sum \rho(r_i), where \rho is a loss 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; @@ -131,7 +134,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double residual(double error) const { return error; } + double loss(double error) const { return 0.5 * error * error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); @@ -155,7 +158,7 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -180,7 +183,7 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -210,7 +213,7 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -235,7 +238,7 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -260,7 +263,7 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -296,7 +299,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -326,7 +329,7 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} double weight(double error) const override; - double residual(double error) const override; + double loss(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); @@ -359,7 +362,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(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); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c4128909b1..a4d15c0f81 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -705,11 +705,11 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - // Fold the use of the m-estimator residual(...) function into distance(...) + // Fold the use of the m-estimator loss(...) function into distance(...) inline virtual double distance(const Vector& v) const - { return robust_->residual(this->unweightedWhiten(v).norm()); } + { return robust_->loss(this->unweightedWhiten(v).norm()); } inline virtual double distance_non_whitened(const Vector& v) const - { return robust_->residual(v.norm()); } + { return robust_->loss(v.norm()); } // 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; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 523a38b2b1..0290fc5d84 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); } /* ************************************************************************* */ diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8a0485334a..ce505df5d2 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.residual(x(i)); + rho(i) = model.loss(x(i)); end psi = w .* x;