Merge pull request #107 from fishyai/welsch_model

Fixed spelling for Welsch and added to gtsam.h
borglab · Sep 7, 2019 · 5cbcfda · 5cbcfda
2 parents 6b298fa + f72322c
commit 5cbcfda
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 5 deletions.
diff --git a/gtsam.h b/gtsam.h
@@ -252,9 +252,9 @@ class FactorIndices {
 bool isDebugVersion();
 
 #include <gtsam/base/DSFMap.h>
-class IndexPair { 
-  IndexPair(); 
-  IndexPair(size_t i, size_t j); 
+class IndexPair {
+  IndexPair();
+  IndexPair(size_t i, size_t j);
   size_t i() const;
   size_t j() const;
 };
@@ -1388,6 +1388,15 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 };
 
+virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
+  Welsch(double k);
+  static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+
 }///\namespace mEstimator
 
 virtual class Robust : gtsam::noiseModel::Base {

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
@@ -808,8 +808,25 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
 }
 
 /* ************************************************************************* */
-// Welsh
+// Welsch
 /* ************************************************************************* */
+Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
+
+void Welsch::print(const std::string &s="") const {
+  std::cout << s << ": Welsch (" << c_ << ")" << std::endl;
+}
+
+bool Welsch::equals(const Base &expected, double tol) const {
+  const Welsch* p = dynamic_cast<const Welsch*>(&expected);
+  if (p == NULL) return false;
+  return std::abs(c_ - p->c_) < tol;
+}
+
+Welsch::shared_ptr Welsch::Create(double c, const ReweightScheme reweight) {
+  return shared_ptr(new Welsch(c, reweight));
+}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
 
 void Welsh::print(const std::string &s="") const {
@@ -825,6 +842,7 @@ bool Welsh::equals(const Base &expected, double tol) const {
 Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {
   return shared_ptr(new Welsh(c, reweight));
 }
+#endif
 
 /* ************************************************************************* */
 // GemanMcClure

diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
@@ -852,7 +852,38 @@ namespace gtsam {
         }
       };
 
-      /// Welsh implements the "Welsh" robust error model (Zhang97ivc)
+      /// Welsch implements the "Welsch" robust error model (Zhang97ivc)
+      class GTSAM_EXPORT Welsch : public Base {
+      protected:
+        double c_, csquared_;
+
+      public:
+        typedef boost::shared_ptr<Welsch> shared_ptr;
+
+        Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
+        double weight(double error) const {
+          double xc2 = (error*error)/csquared_;
+          return std::exp(-xc2);
+        }
+        void print(const std::string &s) const;
+        bool equals(const Base& expected, double tol=1e-8) const;
+        static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
+
+      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_);
+        }
+      };
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+      /// @name Deprecated
+      /// @{
+      // Welsh implements the "Welsch" robust error model (Zhang97ivc)
+      // This was misspelled in previous versions of gtsam and should be
+      // removed in the future.
       class GTSAM_EXPORT Welsh : public Base {
       protected:
         double c_, csquared_;
@@ -878,6 +909,7 @@ namespace gtsam {
           ar & BOOST_SERIALIZATION_NVP(c_);
         }
       };
+#endif
 
       /// GemanMcClure implements the "Geman-McClure" robust error model
       /// (Zhang97ivc).