diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index f1830f37f..7d2c283a6 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;