Added Cauchy noise model contributed by Jan Oberländer

gtsam/linear/NoiseModel.cpp

@@ -614,6 +614,36 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) {
   return shared_ptr(new Huber(c, reweight));
 }
 
+/* ************************************************************************* */
+// Cauchy
+/* ************************************************************************* */
+
+Cauchy::Cauchy(const double k, const ReweightScheme reweight)
+  : Base(reweight), k_(k) {
+  if ( k_ <= 0 ) {
+    cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl;
+    k_ = 1.0;
+  }
+}
+
+double Cauchy::weight(const double &error) const {
+  return k_*k_ / (k_*k_ + error*error);
+}
+
+void Cauchy::print(const std::string &s="") const {
+  cout << s << "cauchy (" << k_ << ")" << endl;
+}
+
+bool Cauchy::equals(const Base &expected, const double tol) const {
+  const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
+  if (p == NULL) return false;
+  return fabs(k_ - p->k_) < tol;
+}
+
+Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) {
+  return shared_ptr(new Cauchy(c, reweight));
+}
+
 /* ************************************************************************* */
 // Tukey
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -689,6 +689,35 @@ namespace gtsam {
         }
       };
 
+      /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS).  Contributed by:
+      ///   Dipl.-Inform. Jan Oberländer (M.Sc.), FZI Research Center for
+      ///   Information Technology, Karlsruhe, Germany.
+      ///   oberlaender@fzi.de
+      /// Thanks Jan!
+      class GTSAM_EXPORT Cauchy : public Base {
+      public:
+        typedef boost::shared_ptr<Cauchy> shared_ptr;
+
+        virtual ~Cauchy() {}
+        Cauchy(const double k = 0.1, const ReweightScheme reweight = Block);
+        virtual double weight(const double &error) const ;
+        virtual void print(const std::string &s) const ;
+        virtual bool equals(const Base& expected, const double tol=1e-8) const ;
+        static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ;
+
+      protected:
+        double k_;
+
+      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(k_);
+        }
+      };
+
       /// Tukey implements the "Tukey" robust error model (Zhang97ivc)
       class GTSAM_EXPORT Tukey : public Base {
       public: