From b9c63ef5df83767b055b2fb4165100dffc139dd8 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Thu, 20 Aug 2015 15:46:03 -0400 Subject: [PATCH] Fix and optimize DCS --- gtsam/linear/NoiseModel.cpp | 10 ++++++++-- gtsam/linear/tests/testNoiseModel.cpp | 4 ++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index e539d9579..cb77902d0 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -835,8 +835,14 @@ DCS::DCS(double c, const ReweightScheme reweight) } double DCS::weight(double error) const { - double scale = 2.0*c_/(c_ + error*error); - return std::min(scale, 1.0); + const double e2 = error*error; + if (e2 > c_) + { + const double w = 2.0*c_/(c_ + e2); + return w*w; + } + + return 1.0; } void DCS::print(const std::string &s="") const { diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 01f653d73..d6857c6ff 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -349,7 +349,7 @@ TEST(NoiseModel, robustFunctionDCS) const double weight1 = dcs->weight(error1), weight2 = dcs->weight(error2); DOUBLES_EQUAL(1.0 , weight1, 1e-8); - DOUBLES_EQUAL(0.01980198, weight2, 1e-8); + DOUBLES_EQUAL(0.00039211, weight2, 1e-8); } /* ************************************************************************* */ @@ -413,7 +413,7 @@ TEST(NoiseModel, robustNoiseDCS) robust->WhitenSystem(A, b); - const double sqrt_weight = sqrt(2.0*k/(k + error2*error2)); + const double sqrt_weight = 2.0*k/(k + error2*error2); DOUBLES_EQUAL(error1, b(0), 1e-8); DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8);