From 9c40a2cd8b54b298e8d7e0bd28fbf40bbd6851f1 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 18 Aug 2023 17:09:47 -0400 Subject: [PATCH 1/2] Add a custom loss function for Robust, fix a bug in Asymmetric Signed-off-by: Fan Jiang --- gtsam/linear/LossFunctions.cpp | 38 +++++++++++++++++++++-- gtsam/linear/LossFunctions.h | 44 +++++++++++++++++++++++++++ gtsam/linear/linear.i | 18 +++++++++++ gtsam/linear/tests/testNoiseModel.cpp | 39 ++++++++++++++++++++++-- 4 files changed, 135 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 8099ca81e9..682783e7c0 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -19,6 +19,7 @@ #include #include +#include #include using namespace std; @@ -438,7 +439,7 @@ AsymmetricTukey::AsymmetricTukey(double c, const ReweightScheme reweight) : Base double AsymmetricTukey::weight(double distance) const { distance = -distance; if (distance >= 0.0) { - return distance; + return 1.0; } else if (distance > -c_) { const double one_minus_xc2 = 1.0 - distance * distance / csquared_; return one_minus_xc2 * one_minus_xc2; @@ -486,7 +487,7 @@ AsymmetricCauchy::AsymmetricCauchy(double k, const ReweightScheme reweight) : Ba double AsymmetricCauchy::weight(double distance) const { distance = -distance; if (distance >= 0.0) { - return distance; + return 1.0; } return ksquared_ / (ksquared_ + distance*distance); @@ -517,6 +518,39 @@ AsymmetricCauchy::shared_ptr AsymmetricCauchy::Create(double k, const ReweightSc } +/* ************************************************************************* */ +// Custom +/* ************************************************************************* */ + +Custom::Custom(std::function weight, std::function loss, const ReweightScheme reweight, + std::string name) + : Base(reweight), weight_(std::move(weight)), loss_(loss), name_(std::move(name)) {} + +double Custom::weight(double distance) const { + return weight_(distance); +} + +double Custom::loss(double distance) const { + return loss_(distance); +} + +void Custom::print(const std::string &s = "") const { + std::cout << s << ": Custom (" << name_ << ")" << std::endl; +} + +bool Custom::equals(const Base &expected, double tol) const { + const auto *p = dynamic_cast(&expected); + if (p == nullptr) + return false; + return name_ == p->name_ && weight_.target() == p->weight_.target() && + loss_.target() == p->loss_.target() && reweight_ == p->reweight_; +} + +Custom::shared_ptr Custom::Create(std::function weight, std::function loss, + const ReweightScheme reweight, const std::string &name) { + return std::make_shared(std::move(weight), std::move(loss), reweight, name); +} + } // namespace mEstimator } // namespace noiseModel } // gtsam diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 479c183ee2..a8902e11b2 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -544,6 +544,50 @@ class GTSAM_EXPORT AsymmetricCauchy : public Base { #endif }; +// Type alias for the custom loss and weight functions +using CustomLossFunction = std::function; +using CustomWeightFunction = std::function; + +/** Implementation of the "Custom" robust error model. + * + * This model just takes two functions as input, one for the loss and one for the weight. + */ +class GTSAM_EXPORT Custom : public Base { + protected: + std::function weight_, loss_; + std::string name_; + + public: + typedef std::shared_ptr shared_ptr; + + Custom(CustomWeightFunction weight, CustomLossFunction loss, + const ReweightScheme reweight = Block, std::string name = "Custom"); + 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(std::function weight, std::function loss, + const ReweightScheme reweight = Block, const std::string &name = "Custom"); + inline std::string& name() { return name_; } + + inline std::function& weightFunction() { return weight_; } + inline std::function& lossFunction() { return loss_; } + + // Default constructor for serialization + inline Custom() = default; + + 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(name_); + } +#endif +}; + } // namespace mEstimator } // namespace noiseModel } // namespace gtsam diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 5404964a1a..d4a045f097 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -203,6 +203,24 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { double loss(double error) const; }; +virtual class Custom: gtsam::noiseModel::mEstimator::Base { + Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight, + gtsam::noiseModel::mEstimator::CustomLossFunction loss, + gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight, + std::string name); + static gtsam::noiseModel::mEstimator::Custom* Create( + gtsam::noiseModel::mEstimator::CustomWeightFunction weight, + gtsam::noiseModel::mEstimator::CustomLossFunction loss, + gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight, + std::string name); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + }///\namespace mEstimator diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 7d2c283a60..725680501d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -512,7 +512,7 @@ TEST(NoiseModel, robustFunctionAsymmetricCauchy) 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(error3), 1e-8); DOUBLES_EQUAL(1.0, cauchy->weight(error4), 1e-8); DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); @@ -575,7 +575,7 @@ TEST(NoiseModel, robustFunctionAsymmetricTukey) 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(error3), 1e-8); DOUBLES_EQUAL(1.0, tukey->weight(error4), 1e-8); DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); @@ -703,6 +703,35 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) } } +/* ************************************************************************* */ +TEST(NoiseModel, robustNoiseCustomHuber) { + const double k = 10.0, error1 = 1.0, error2 = 100.0; + Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = + Robust::Create(mEstimator::Custom::Create( + [k](double e) { + const auto abs_e = std::abs(e); + return abs_e <= k ? 1.0 : k / abs_e; + }, + [k](double e) { + const auto abs_e = std::abs(e); + return abs_e <= k ? abs_e * abs_e / 2.0 : k * abs_e - k * k / 2.0; + }, + noiseModel::mEstimator::Custom::Scalar, "Huber"), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + DOUBLES_EQUAL(error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt(k * error2), b(1), 1e-8); + + DOUBLES_EQUAL(1.0, A(0, 0), 1e-8); + DOUBLES_EQUAL(10.0, A(0, 1), 1e-8); + DOUBLES_EQUAL(sqrt(k * 100.0), A(1, 0), 1e-8); + DOUBLES_EQUAL(sqrt(k / 100.0) * 1000.0, A(1, 1), 1e-8); +} + TEST(NoiseModel, lossFunctionAtZero) { const double k = 5.0; @@ -730,6 +759,12 @@ TEST(NoiseModel, lossFunctionAtZero) auto lsdz = mEstimator::L2WithDeadZone::Create(k); DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); + auto assy_cauchy = mEstimator::AsymmetricCauchy::Create(k); + DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); + DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); + auto assy_tukey = mEstimator::AsymmetricTukey::Create(k); + DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); + DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); } From 7d04f4bce6f20dbe3cc23bb90f2398c8d2320965 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 21 Aug 2023 10:59:14 -0400 Subject: [PATCH 2/2] Add custom unit test for Python robust function --- python/gtsam/tests/test_Robust.py | 43 +++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 python/gtsam/tests/test_Robust.py diff --git a/python/gtsam/tests/test_Robust.py b/python/gtsam/tests/test_Robust.py new file mode 100644 index 0000000000..c2ab9d4a47 --- /dev/null +++ b/python/gtsam/tests/test_Robust.py @@ -0,0 +1,43 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Simple unit test for custom robust noise model. +Author: Fan Jiang +""" +import unittest + +import gtsam +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + + +class TestRobust(GtsamTestCase): + + def test_RobustLossAndWeight(self): + k = 10.0 + + def custom_weight(e): + abs_e = abs(e) + return 1.0 if abs_e <= k else k / abs_e + + def custom_loss(e): + abs_e = abs(e) + return abs_e * abs_e / 2.0 if abs_e <= k else k * abs_e - k * k / 2.0 + + custom_robust = gtsam.noiseModel.Robust.Create( + gtsam.noiseModel.mEstimator.Custom(custom_weight, custom_loss, + gtsam.noiseModel.mEstimator.Base.ReweightScheme.Scalar, + "huber"), + gtsam.noiseModel.Isotropic.Sigma(1, 2.0)) + f = gtsam.PriorFactorDouble(0, 1.0, custom_robust) + v = gtsam.Values() + v.insert(0, 0.0) + + self.assertAlmostEquals(f.error(v), 0.125) + +if __name__ == "__main__": + unittest.main()