Merged in enriquefernandezperdomo/gtsam/add_dcs_robust_kernel (pull request #180)

Add DCS robust kernel

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 {
@@ -799,6 +799,66 @@ 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 {
+  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 {
+  std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl;
+}
+
+bool GemanMcClure::equals(const Base &expected, double tol) const {
+  const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&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
+/* ************************************************************************* */
+DCS::DCS(double c, const ReweightScheme reweight)
+  : Base(reweight), c_(c) {
+}
+
+double DCS::weight(double error) const {
+  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 {
+  std::cout << s << ": DCS (" << c_ << ")" << std::endl;
+}
+
+bool DCS::equals(const Base &expected, double tol) const {
+  const DCS* p = dynamic_cast<const DCS*>(&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
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -823,6 +823,65 @@ 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, so we actually have
+      /// the generalized Geman-McClure from (Agarwal15phd).
+      class GTSAM_EXPORT GemanMcClure : public Base {
+      public:
+        typedef boost::shared_ptr<GemanMcClure> 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<class ARCHIVE>
+        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<DCS> 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<class ARCHIVE>
+        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

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.25      , weight1, 1e-8);
+  DOUBLES_EQUAL(9.80296e-5, 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.00039211, 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();
@@ -353,6 +373,57 @@ 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 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);
+
+  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 = 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());\