change namespace "MEstimator" to "mEstimator" to wrap robust noise models to matlab. Add Tukey.

gtsam.h

@@ -1014,6 +1014,43 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
   static gtsam::noiseModel::Unit* Create(size_t dim);
   void print(string s) const;
 };
+
+namespace mEstimator {
+virtual class Base {
+};
+
+virtual class Null: gtsam::noiseModel::mEstimator::Base {
+  Null();
+  void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Null* Create();
+};
+
+virtual class Fair: gtsam::noiseModel::mEstimator::Base {
+  Fair(double c);
+  void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
+};
+
+virtual class Huber: gtsam::noiseModel::mEstimator::Base {
+  Huber(double k);
+  void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
+};
+
+virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
+  Tukey(double k);
+  void print(string s) const;
+  static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
+};
+
+}///\namespace mEstimator
+
+virtual class Robust : gtsam::noiseModel::Base {
+  Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
+  static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
+  void print(string s) const;
+};
+
 }///\namespace noiseModel
 
 #include <gtsam/linear/Sampler.h>

gtsam/linear/NoiseModel.cpp

@@ -438,7 +438,7 @@ void Unit::print(const std::string& name) const {
 // M-Estimator
 /* ************************************************************************* */
 
-namespace MEstimator {
+namespace mEstimator {
 
 /** produce a weight vector according to an error vector and the implemented
  * robust function */
@@ -606,6 +606,36 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) {
   return shared_ptr(new Huber(c, reweight));
 }
 
+/* ************************************************************************* */
+// Tukey
+/* ************************************************************************* */
+Tukey::Tukey(const double c, const ReweightScheme reweight)
+  : Base(reweight), c_(c) {
+}
+
+double Tukey::weight(const double &error) const {
+  if (fabs(error) <= c_) {
+    double xc2 = (error/c_)*(error/c_);
+    double one_xc22 = (1.0-xc2)*(1.0-xc2);
+    return one_xc22;
+  }
+  return 0.0;
+}
+
+void Tukey::print(const std::string &s="") const {
+  std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
+}
+
+bool Tukey::equals(const Base &expected, const double tol) const {
+  const Tukey* p = dynamic_cast<const Tukey*>(&expected);
+  if (p == NULL) return false;
+  return fabs(c_ - p->c_) < tol;
+}
+
+Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) {
+  return shared_ptr(new Tukey(c, reweight));
+}
+
 } // namespace MEstimator
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -572,7 +572,7 @@ namespace gtsam {
 
     // TODO: should not really exist
     /// The MEstimator namespace contains all robust error functions (not models)
-    namespace MEstimator {
+    namespace mEstimator {
 
       //---------------------------------------------------------------------------------------
 
@@ -661,6 +661,23 @@ namespace gtsam {
         Huber(){}
       };
 
+      /// Tukey implements the "Tukey" robust error model (Zhang97ivc)
+      class Tukey : public Base {
+      public:
+        typedef boost::shared_ptr<Tukey> shared_ptr;
+      protected:
+        double c_;
+      public:
+        Tukey(const double c, const ReweightScheme reweight = Block);
+        virtual ~Tukey() {}
+        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) ;
+      private:
+        Tukey(){}
+      };
+
     } ///\namespace MEstimator
 
     /// Base class for robust error models
@@ -669,7 +686,7 @@ namespace gtsam {
       typedef boost::shared_ptr<Robust> shared_ptr;
 
     protected:
-      typedef MEstimator::Base RobustModel;
+      typedef mEstimator::Base RobustModel;
       typedef noiseModel::Base NoiseModel;
 
       const RobustModel::shared_ptr robust_; ///< robust error function used