Add Geman-McClure robust kernel

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<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
 /* ************************************************************************* */

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<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;