diff --git a/gtsam.h b/gtsam.h index 23590f5f8..35256e92c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1364,6 +1364,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1384,6 +1385,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1394,6 +1396,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1404,6 +1407,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1414,6 +1418,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1424,6 +1429,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; + double residual(double error) const; }; //TODO DCS and L2WithDeadZone mEstimators diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a01233fad..d4cf2a5c2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -858,6 +858,12 @@ double GemanMcClure::weight(double error) const { 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; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 464bb38ed..2ef830c81 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -727,6 +728,7 @@ 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 void print(const std::string &s) const; virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; @@ -752,6 +754,12 @@ namespace gtsam { 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; + } 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) ; @@ -779,6 +787,14 @@ namespace gtsam { 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)); + } + } 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) ; @@ -809,6 +825,11 @@ namespace gtsam { 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; + } 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) ; @@ -839,6 +860,16 @@ namespace gtsam { } 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; + } + } 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) ; @@ -866,6 +897,10 @@ namespace gtsam { 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) ; @@ -897,6 +932,10 @@ namespace gtsam { 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) ; @@ -925,6 +964,7 @@ 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 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) ; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 85dc4735d..6419f2b9d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -457,6 +457,22 @@ TEST(NoiseModel, WhitenInPlace) * penalties using iteratively re-weighted least squares. */ +TEST(NoiseModel, robustFunctionFair) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k); + DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8); + DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); +} + TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; @@ -466,16 +482,74 @@ TEST(NoiseModel, robustFunctionHuber) // Test negative value to ensure we take absolute value of error. 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); +} + +TEST(NoiseModel, robustFunctionCauchy) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k); + DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8); + DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); } TEST(NoiseModel, robustFunctionGemanMcClure) { - const double k = 1.0, error1 = 1.0, error2 = 10.0; + const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); - const double weight1 = gmc->weight(error1), - weight2 = gmc->weight(error2); - DOUBLES_EQUAL(0.25 , weight1, 1e-8); - DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8); + DOUBLES_EQUAL(0.25 , gmc->weight(error1), 1e-8); + DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8); + 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); +} + +TEST(NoiseModel, robustFunctionWelsch) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k); + DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8); + DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); +} + +TEST(NoiseModel, robustFunctionTukey) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k); + DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8); + DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8); + // Test negative value to ensure we take absolute value of error. + 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); } TEST(NoiseModel, robustFunctionDCS)