From 7d256ff2fb6fb9f091a9edc29fab6beaf4eefdda Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Wed, 12 Aug 2015 17:59:24 -0400 Subject: [PATCH 1/6] Add DCS robust kernel DCS (Dynamic Covariance Scaling) from Robust Map Optimization (Agarwal13icra) --- gtsam/linear/NoiseModel.cpp | 26 ++++++++++++++++++++++++++ gtsam/linear/NoiseModel.h | 26 ++++++++++++++++++++++++++ 2 files changed, 52 insertions(+) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 609c03acf..29dc5aa0b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -799,6 +799,32 @@ Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } +/* ************************************************************************* */ +// DCS +/* ************************************************************************* */ +DCS::DCS(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double DCS::weight(double error) const { + double scale = 2.0*c_/(c_ + error*error); + return std::min(scale, 1.0); +} + +void DCS::print(const std::string &s="") const { + std::cout << s << ": DCS (" << c_ << ")" << std::endl; +} + +bool DCS::equals(const Base &expected, double tol) const { + const DCS* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new DCS(c, reweight)); +} + } // namespace mEstimator /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2a4d7c746..4c2f68719 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -823,6 +823,32 @@ namespace gtsam { } }; + /// DCS implements the Dynamic Covariance Scaling robust error model + /// from the paper Robust Map Optimization (Agarwal13icra). + class GTSAM_EXPORT DCS : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + DCS(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~DCS() {} + virtual double weight(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) ; + + protected: + double c_; + + private: + /** 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_); + } + }; + } ///\namespace mEstimator /// Base class for robust error models From 2969d6151963602fd0d4b3bedfe6bd8905c66ed8 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Tue, 18 Aug 2015 17:14:02 -0400 Subject: [PATCH 2/6] Add Geman-McClure robust kernel --- gtsam/linear/NoiseModel.cpp | 25 +++++++++++++++++++++++++ gtsam/linear/NoiseModel.h | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 29dc5aa0b..1cf9ff109 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -799,6 +799,31 @@ Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } +/* ************************************************************************* */ +// GemanMcClure +/* ************************************************************************* */ +GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) + : Base(reweight), c_(c) { +} + +double GemanMcClure::weight(double error) const { + return c_/(c_ + error*error); +} + +void GemanMcClure::print(const std::string &s="") const { + std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; +} + +bool GemanMcClure::equals(const Base &expected, double tol) const { + const GemanMcClure* p = dynamic_cast(&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_) < tol; +} + +GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) { + return shared_ptr(new GemanMcClure(c, reweight)); +} + /* ************************************************************************* */ // DCS /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4c2f68719..25b34ce18 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -823,8 +823,40 @@ namespace gtsam { } }; + /// GemanMcClure implements the "Geman-McClure" robust error model + /// (Zhang97ivc). + /// + /// Note that Geman-McClure weight function uses the parameter c == 1.0, + /// but here it's allowed to use different values. + class GTSAM_EXPORT GemanMcClure : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); + virtual ~GemanMcClure() {} + virtual double weight(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) ; + + protected: + double c_; + + private: + /** 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_); + } + }; + /// DCS implements the Dynamic Covariance Scaling robust error model /// from the paper Robust Map Optimization (Agarwal13icra). + /// + /// Under the special condition of the parameter c == 1.0 and not + /// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure. class GTSAM_EXPORT DCS : public Base { public: typedef boost::shared_ptr shared_ptr; From 47d787b478bf8225f96be98fca483b50a88f0578 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Tue, 18 Aug 2015 22:46:24 -0400 Subject: [PATCH 3/6] Add DCS & Geman-McClure unit tests --- gtsam/linear/tests/testNoiseModel.cpp | 73 +++++++++++++++++++++++++-- 1 file changed, 70 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 37b55fc03..c1e9d95f6 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -322,7 +322,7 @@ TEST(NoiseModel, WhitenInPlace) } /* ************************************************************************* */ -TEST(NoiseModel, robustFunction) +TEST(NoiseModel, robustFunctionHuber) { const double k = 5.0, error1 = 1.0, error2 = 10.0; const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k); @@ -332,8 +332,28 @@ TEST(NoiseModel, robustFunction) DOUBLES_EQUAL(0.5, weight2, 1e-8); } +TEST(NoiseModel, robustFunctionGemanMcClure) +{ + const double k = 1.0, error1 = 1.0, error2 = 10.0; + const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); + const double weight1 = gmc->weight(error1), + weight2 = gmc->weight(error2); + DOUBLES_EQUAL(0.5 , weight1, 1e-8); + DOUBLES_EQUAL(0.00990099, weight2, 1e-8); +} + +TEST(NoiseModel, robustFunctionDCS) +{ + const double k = 1.0, error1 = 1.0, error2 = 10.0; + const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k); + const double weight1 = dcs->weight(error1), + weight2 = dcs->weight(error2); + DOUBLES_EQUAL(1.0 , weight1, 1e-8); + DOUBLES_EQUAL(0.01980198, weight2, 1e-8); +} + /* ************************************************************************* */ -TEST(NoiseModel, robustNoise) +TEST(NoiseModel, robustNoiseHuber) { 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(); @@ -342,7 +362,7 @@ TEST(NoiseModel, robustNoise) mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); - robust->WhitenSystem(A,b); + robust->WhitenSystem(A, b); DOUBLES_EQUAL(error1, b(0), 1e-8); DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8); @@ -353,6 +373,53 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +TEST(NoiseModel, robustNoiseGemanMcClure) +{ + const double k = 1.0, error1 = 1.0, error2 = 100.0; + const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; + Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = Robust::Create( + mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + const double sqrt_weight_error1 = sqrt(0.5); + const double sqrt_weight_error2 = sqrt(k/(k + error2*error2)); + + DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8); + + DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8); +} + +TEST(NoiseModel, robustNoiseDCS) +{ + const double k = 1.0, error1 = 1.0, error2 = 100.0; + const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0; + Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished(); + Vector b = Vector2(error1, error2); + const Robust::shared_ptr robust = Robust::Create( + mEstimator::DCS::Create(k, mEstimator::DCS::Scalar), + Unit::Create(2)); + + robust->WhitenSystem(A, b); + + const double sqrt_weight = sqrt(2.0*k/(k + error2*error2)); + + DOUBLES_EQUAL(error1, b(0), 1e-8); + DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8); + + DOUBLES_EQUAL(a00, A(0,0), 1e-8); + DOUBLES_EQUAL(a01, A(0,1), 1e-8); + DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8); + DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8); +} + /* ************************************************************************* */ #define TEST_GAUSSIAN(gaussian)\ EQUALITY(info, gaussian->information());\ From 6cdc1de2683b8380971f2646cf8c1de5737ed03a Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Thu, 20 Aug 2015 15:45:12 -0400 Subject: [PATCH 4/6] Use fabs in Huber condition --- gtsam/linear/NoiseModel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 1cf9ff109..80210c3b7 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -696,7 +696,7 @@ Huber::Huber(double k, const ReweightScheme reweight) } double Huber::weight(double error) const { - return (error < k_) ? (1.0) : (k_ / fabs(error)); + return (fabs(error) > k_) ? k_ / fabs(error) : 1.0; } void Huber::print(const std::string &s="") const { From 44ae7bfe016642f58e9cf221d27c85bd6d08b6e1 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Thu, 20 Aug 2015 15:45:34 -0400 Subject: [PATCH 5/6] Fix generalized Geman-McClure --- gtsam/linear/NoiseModel.cpp | 5 ++++- gtsam/linear/NoiseModel.h | 3 ++- gtsam/linear/tests/testNoiseModel.cpp | 12 ++++++++---- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 80210c3b7..e539d9579 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -807,7 +807,10 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight) } double GemanMcClure::weight(double error) const { - return c_/(c_ + error*error); + const double c2 = c_*c_; + const double c4 = c2*c2; + const double c2error = c2 + error*error; + return c4/(c2error*c2error); } void GemanMcClure::print(const std::string &s="") const { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 25b34ce18..db01152f6 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -827,7 +827,8 @@ namespace gtsam { /// (Zhang97ivc). /// /// Note that Geman-McClure weight function uses the parameter c == 1.0, - /// but here it's allowed to use different values. + /// but here it's allowed to use different values, so we actually have + /// the generalized Geman-McClure from (Agarwal15phd). class GTSAM_EXPORT GemanMcClure : public Base { public: typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index c1e9d95f6..01f653d73 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -338,8 +338,8 @@ TEST(NoiseModel, robustFunctionGemanMcClure) const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); const double weight1 = gmc->weight(error1), weight2 = gmc->weight(error2); - DOUBLES_EQUAL(0.5 , weight1, 1e-8); - DOUBLES_EQUAL(0.00990099, weight2, 1e-8); + DOUBLES_EQUAL(0.25 , weight1, 1e-8); + DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -385,8 +385,12 @@ TEST(NoiseModel, robustNoiseGemanMcClure) robust->WhitenSystem(A, b); - const double sqrt_weight_error1 = sqrt(0.5); - const double sqrt_weight_error2 = sqrt(k/(k + error2*error2)); + const double k2 = k*k; + const double k4 = k2*k2; + const double k2error = k2 + error2*error2; + + const double sqrt_weight_error1 = sqrt(0.25); + const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error)); DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8); DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8); From b9c63ef5df83767b055b2fb4165100dffc139dd8 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Thu, 20 Aug 2015 15:46:03 -0400 Subject: [PATCH 6/6] 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);