diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 64ded7cc34..8099ca81e9 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -424,6 +424,99 @@ L2WithDeadZone::shared_ptr L2WithDeadZone::Create(double k, const ReweightScheme return shared_ptr(new L2WithDeadZone(k, reweight)); } + +/* ************************************************************************* */ +// AsymmetricTukey +/* ************************************************************************* */ + +AsymmetricTukey::AsymmetricTukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) { + if (c <= 0) { + throw runtime_error("mEstimator AsymmetricTukey takes only positive double in constructor."); + } +} + +double AsymmetricTukey::weight(double distance) const { + distance = -distance; + if (distance >= 0.0) { + return distance; + } else if (distance > -c_) { + const double one_minus_xc2 = 1.0 - distance * distance / csquared_; + return one_minus_xc2 * one_minus_xc2; + } + return 0.0; +} + +double AsymmetricTukey::loss(double distance) const { + distance = -distance; + if (distance >= 0.0) { + return distance * distance / 2.0; + } else if (distance >= -c_) { + 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; + } + return csquared_ / 6.0; +} + +void AsymmetricTukey::print(const std::string &s="") const { + std::cout << s << ": AsymmetricTukey (" << c_ << ")" << std::endl; +} + +bool AsymmetricTukey::equals(const Base &expected, double tol) const { + const AsymmetricTukey* p = dynamic_cast(&expected); + if (p == nullptr) return false; + return std::abs(c_ - p->c_) < tol; +} + +AsymmetricTukey::shared_ptr AsymmetricTukey::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new AsymmetricTukey(c, reweight)); +} + + +/* ************************************************************************* */ +// AsymmetricCauchy +/* ************************************************************************* */ + +AsymmetricCauchy::AsymmetricCauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { + if (k <= 0) { + throw runtime_error("mEstimator AsymmetricCauchy takes only positive double in constructor."); + } +} + +double AsymmetricCauchy::weight(double distance) const { + distance = -distance; + if (distance >= 0.0) { + return distance; + } + + return ksquared_ / (ksquared_ + distance*distance); + +} + +double AsymmetricCauchy::loss(double distance) const { + distance = -distance; + if (distance >= 0.0) { + return distance * distance / 2.0; + } + const double val = std::log1p(distance * distance / ksquared_); + return ksquared_ * val * 0.5; +} + +void AsymmetricCauchy::print(const std::string &s="") const { + std::cout << s << ": AsymmetricCauchy (" << k_ << ")" << std::endl; +} + +bool AsymmetricCauchy::equals(const Base &expected, double tol) const { + const AsymmetricCauchy* p = dynamic_cast(&expected); + if (p == nullptr) return false; + return std::abs(k_ - p->k_) < tol; +} + +AsymmetricCauchy::shared_ptr AsymmetricCauchy::Create(double k, const ReweightScheme reweight) { + return shared_ptr(new AsymmetricCauchy(k, reweight)); +} + + } // namespace mEstimator } // namespace noiseModel } // gtsam diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 70166d97e1..1e28755bbf 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -15,6 +15,7 @@ * @date Jan 13, 2010 * @author Richard Roberts * @author Frank Dellaert + * @author Fan Jiang */ #pragma once @@ -470,6 +471,79 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { #endif }; +/** Implementation of the "AsymmetricTukey" robust error model. + * + * This model has a scalar parameter "c". + * + * - Following are all for one side, the other is standard L2 + * - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x| shared_ptr; + + AsymmetricTukey(double c = 4.6851, const ReweightScheme reweight = Block); + 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); + double modelParameter() const { return c_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** 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_); + } +#endif +}; + +/** Implementation of the "AsymmetricCauchy" robust error model. + * + * This model has a scalar parameter "k". + * + * - Following are all for one side, the other is standard L2 + * - Loss \rho(x) = 0.5 k² log(1+x²/k²) + * - Derivative \phi(x) = (k²x)/(x²+k²) + * - Weight w(x) = \phi(x)/x = k²/(x²+k²) + */ +class GTSAM_EXPORT AsymmetricCauchy : public Base { + protected: + double k_, ksquared_; + + public: + typedef std::shared_ptr shared_ptr; + + AsymmetricCauchy(double k = 0.1, const ReweightScheme reweight = Block); + 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); + double modelParameter() const { return k_; } + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** 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_); + ar &BOOST_SERIALIZATION_NVP(ksquared_); + } +#endif +}; + } // namespace mEstimator } // namespace noiseModel } // namespace gtsam diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index c0230f1c21..63359ae2be 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -89,6 +89,7 @@ virtual class Unit : gtsam::noiseModel::Isotropic { namespace mEstimator { virtual class Base { + enum ReweightScheme { Scalar, Block }; void print(string s = "") const; }; @@ -191,6 +192,18 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { double loss(double error) const; }; +virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { + AsymmetricTukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); + static gtsam::noiseModel::mEstimator::AsymmetricTukey* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + + }///\namespace mEstimator virtual class Robust : gtsam::noiseModel::Base { diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index f1830f37f0..7d2c283a60 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -14,6 +14,7 @@ * @date Jan 13, 2010 * @author Richard Roberts * @author Frank Dellaert + * @author Fan Jiang */ @@ -504,6 +505,22 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); } +TEST(NoiseModel, robustFunctionAsymmetricCauchy) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::AsymmetricCauchy::shared_ptr cauchy = mEstimator::AsymmetricCauchy::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(10.0, cauchy->weight(error3), 1e-8); + DOUBLES_EQUAL(1.0, cauchy->weight(error4), 1e-8); + + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); + DOUBLES_EQUAL(50.0, cauchy->loss(error3), 1e-8); + DOUBLES_EQUAL(0.5, cauchy->loss(error4), 1e-8); +} + TEST(NoiseModel, robustFunctionGemanMcClure) { const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; @@ -551,6 +568,22 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); } +TEST(NoiseModel, robustFunctionAsymmetricTukey) +{ + const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; + const mEstimator::AsymmetricTukey::shared_ptr tukey = mEstimator::AsymmetricTukey::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(10.0, tukey->weight(error3), 1e-8); + DOUBLES_EQUAL(1.0, tukey->weight(error4), 1e-8); + + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); + DOUBLES_EQUAL(50.0, tukey->loss(error3), 1e-8); + DOUBLES_EQUAL(0.5, tukey->loss(error4), 1e-8); +} + TEST(NoiseModel, robustFunctionDCS) { const double k = 1.0, error1 = 1.0, error2 = 10.0;