Add DCS robust kernel

DCS (Dynamic Covariance Scaling) from Robust Map Optimization (Agarwal13icra)

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