From f68c06a10d2aad69e95951f806b6f566eb9511b5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 18 Sep 2019 18:44:21 -0400
Subject: [PATCH 01/17] wrapped more mEstimators as well as their weight
 functions

---
 gtsam.h | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index bf3575580..23590f5f8 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1362,6 +1362,8 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
 };
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@@ -1370,6 +1372,8 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
 };
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@@ -1378,6 +1382,18 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
+};
+
+virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
+  Cauchy(double k);
+  static gtsam::noiseModel::mEstimator::Cauchy* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
 };
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@@ -1386,6 +1402,8 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
 };
 
 virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@@ -1394,8 +1412,21 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
 
   // enabling serialization functionality
   void serializable() const;
+
+  double weight(double error) const;
 };
 
+virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
+  GemanMcClure(double k);
+  static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+};
+
+//TODO DCS and L2WithDeadZone mEstimators
 
 }///\namespace mEstimator
 

From c8e18c95d0cd6efb5212515280ae61632000f2a2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 18 Sep 2019 20:08:10 -0400
Subject: [PATCH 02/17] added script to visualize values of different
 mEstimators

---
 matlab/gtsam_examples/VisualizeMEstimators.m | 160 +++++++++++++++++++
 1 file changed, 160 insertions(+)
 create mode 100644 matlab/gtsam_examples/VisualizeMEstimators.m

diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
new file mode 100644
index 000000000..b8ffc03c4
--- /dev/null
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -0,0 +1,160 @@
+import gtsam.*
+
+clear all;
+close all;
+
+x = linspace(-10, 10, 1000);
+% x = linspace(-10, 10, 21)
+
+[rho, psi, w] = fair(x);
+plot_rho(x, rho, 1, -1, 30)
+title('Fair');
+plot_psi(x, psi, 7, -3, 3);
+plot_w(x, w, 13, 3);
+
+[rho, psi, w] = huber(x);
+plot_rho(x, rho, 2, -1, 30);
+title('Huber');
+plot_psi(x, psi, 8, -15, 15);
+plot_w(x, w, 14, 5);
+
+[rho, psi, w] = cauchy(x);
+plot_rho(x, rho, 3, -0.1, 0.1);
+title('Cauchy');
+plot_psi(x, psi, 9, -0.2, 0.2);
+plot_w(x, w, 15, 1.5);
+
+[rho, psi, w] = gemancclure(x);
+plot_rho(x, rho, 4, -1, 1);
+title('Geman-McClure');
+plot_psi(x, psi, 10, -1, 1);
+plot_w(x, w, 16, 1.5);
+
+[rho, psi, w] = welsch(x);
+plot_rho(x, rho, 5, -5, 10);
+title('Welsch');
+plot_psi(x, psi, 11, -2, 2);
+plot_w(x, w, 17, 2);
+
+[rho, psi, w] = tukey(x);
+plot_rho(x, rho, 6, -5, 10);
+title('Tukey');
+plot_psi(x, psi, 12, -2, 2);
+plot_w(x, w, 18, 2);
+
+function plot_rho(x, y, idx, yll, ylu)
+    subplot(3, 6, idx);
+    plot(x, y);
+    xlim([-15, 15]);
+    ylim([yll, ylu]);
+end
+
+function plot_psi(x, y, idx, yll, ylu)
+    subplot(3, 6, idx);
+    plot(x, y);
+    xlim([-15, 15]);
+    ylim([yll, ylu]);
+end
+
+function plot_w(x, y, idx, yl)
+    subplot(3, 6, idx);
+    plot(x, y);
+    xlim([-15, 15]);
+    ylim([-yl, yl]);
+end
+
+function [rho, psi, w] = fair(x)
+    import gtsam.*
+    c = 1.3998;
+
+    rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) );
+    est = noiseModel.mEstimator.Fair(c);
+    
+    w = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = est.weight(x(i));
+    end
+
+    psi = w .* x;
+end
+
+function [rho, psi, w] = huber(x)
+    import gtsam.*
+    k = 5;
+    t = (abs(x) > k);
+
+    rho = (x .^ 2) ./ 2;
+    rho(t) = k * (abs(x(t)) - (k/2));
+    est = noiseModel.mEstimator.Huber(k);
+
+    w = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = est.weight(x(i));
+    end
+
+    psi = w .* x;
+end
+
+function [rho, psi, w] = cauchy(x)
+    import gtsam.*
+    c = 0.1;
+    
+    rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2));
+
+    est = noiseModel.mEstimator.Cauchy(c);
+    
+    w = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = est.weight(x(i));
+    end
+
+    psi = w .* x;
+end
+
+function [rho, psi, w] = gemancclure(x)
+    import gtsam.*
+    c = 1.0;
+    rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2);
+    
+    est = noiseModel.mEstimator.GemanMcClure(c);
+    
+    w = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = est.weight(x(i));
+    end
+
+    psi = w .* x;
+end
+
+function [rho, psi, w] = welsch(x)
+    import gtsam.*
+    c = 2.9846;
+    rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 ));
+    
+    est = noiseModel.mEstimator.Welsh(c);
+
+    w = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = est.weight(x(i));
+    end
+
+    psi = w .* x;
+end
+
+function [rho, psi, w] = tukey(x)
+    import gtsam.*
+    c = 4.6851;
+    t = (abs(x) > c);
+
+    rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 );
+    rho(t) = c .^ 2 / 6;
+
+    est = noiseModel.mEstimator.Tukey(c);
+
+    w = zeros(size(x));
+    for i = 1:size(x, 2)
+        w(i) = est.weight(x(i));
+    end
+
+    psi = w .* x;
+end

From ab044b693b004ca9e229736cdfd1043f09fa55dd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 18 Sep 2019 23:41:20 -0400
Subject: [PATCH 03/17] different plots for each mEstimator

---
 matlab/gtsam_examples/VisualizeMEstimators.m | 54 ++++++++++++--------
 1 file changed, 33 insertions(+), 21 deletions(-)

diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
index b8ffc03c4..8fce828aa 100644
--- a/matlab/gtsam_examples/VisualizeMEstimators.m
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -7,57 +7,69 @@ x = linspace(-10, 10, 1000);
 % x = linspace(-10, 10, 21)
 
 [rho, psi, w] = fair(x);
+figure(1);
 plot_rho(x, rho, 1, -1, 30)
 title('Fair');
-plot_psi(x, psi, 7, -3, 3);
-plot_w(x, w, 13, 3);
+plot_psi(x, psi, 2, -3, 3);
+plot_w(x, w, 3, 3);
+saveas(figure(1), 'fair.png');
 
 [rho, psi, w] = huber(x);
-plot_rho(x, rho, 2, -1, 30);
+figure(2);
+plot_rho(x, rho, 1, -1, 30);
 title('Huber');
-plot_psi(x, psi, 8, -15, 15);
-plot_w(x, w, 14, 5);
+plot_psi(x, psi, 2, -15, 15);
+plot_w(x, w, 3, 5);
+saveas(figure(2), 'huber.png');
 
 [rho, psi, w] = cauchy(x);
-plot_rho(x, rho, 3, -0.1, 0.1);
+figure(3);
+plot_rho(x, rho, 1, -0.1, 0.1);
 title('Cauchy');
-plot_psi(x, psi, 9, -0.2, 0.2);
-plot_w(x, w, 15, 1.5);
+plot_psi(x, psi, 2, -0.2, 0.2);
+plot_w(x, w, 3, 1.5);
+saveas(figure(3), 'cauchy.png');
 
 [rho, psi, w] = gemancclure(x);
-plot_rho(x, rho, 4, -1, 1);
+figure(4);
+plot_rho(x, rho, 1, -1, 1);
 title('Geman-McClure');
-plot_psi(x, psi, 10, -1, 1);
-plot_w(x, w, 16, 1.5);
+plot_psi(x, psi, 2, -1, 1);
+plot_w(x, w, 3, 1.5);
+saveas(figure(4), 'gemanmcclure.png');
 
 [rho, psi, w] = welsch(x);
-plot_rho(x, rho, 5, -5, 10);
+figure(5);
+plot_rho(x, rho, 1, -5, 10);
 title('Welsch');
-plot_psi(x, psi, 11, -2, 2);
-plot_w(x, w, 17, 2);
+plot_psi(x, psi, 2, -2, 2);
+plot_w(x, w, 3, 2);
+saveas(figure(5), 'welsch.png');
 
 [rho, psi, w] = tukey(x);
-plot_rho(x, rho, 6, -5, 10);
+figure(6);
+plot_rho(x, rho, 1, -5, 10);
 title('Tukey');
-plot_psi(x, psi, 12, -2, 2);
-plot_w(x, w, 18, 2);
+plot_psi(x, psi, 2, -2, 2);
+plot_w(x, w, 3, 2);
+saveas(figure(6), 'tukey.png');
 
 function plot_rho(x, y, idx, yll, ylu)
-    subplot(3, 6, idx);
+    subplot(3, 1, idx);
     plot(x, y);
     xlim([-15, 15]);
     ylim([yll, ylu]);
 end
 
 function plot_psi(x, y, idx, yll, ylu)
-    subplot(3, 6, idx);
+    subplot(3, 1, idx);
     plot(x, y);
     xlim([-15, 15]);
     ylim([yll, ylu]);
 end
 
 function plot_w(x, y, idx, yl)
-    subplot(3, 6, idx);
+    subplot(3, 1, idx);
     plot(x, y);
     xlim([-15, 15]);
     ylim([-yl, yl]);
@@ -131,7 +143,7 @@ function [rho, psi, w] = welsch(x)
     c = 2.9846;
     rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 ));
     
-    est = noiseModel.mEstimator.Welsh(c);
+    est = noiseModel.mEstimator.Welsch(c);
 
     w = zeros(size(x));
     for i = 1:size(x, 2)

From f71e156bced9d6a1aed094f004d647fcf71becea Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 19 Sep 2019 16:13:59 -0400
Subject: [PATCH 04/17] Fixed comment

---
 gtsam/linear/NoiseModel.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 5c540caa3..464bb38ed 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -635,9 +635,9 @@ namespace gtsam {
      * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
      *
      * Name        Symbol          Least-Squares   L1-norm    Huber
-     * Residual    \rho(x)         0.5*x^2         |x|        0.5*x^2 if x<k, 0.5*k^2 + k|x-k| otherwise
-     * Derivative  \phi(x)         x               sgn(x)     x       if x<k, k sgn(x)         otherwise
-     * Weight      w(x)=\phi(x)/x  1               1/|x|      1       if x<k, k/|x|            otherwise
+     * Residual    \rho(x)         0.5*x^2         |x|        0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise
+     * Derivative  \phi(x)         x               sgn(x)     x       if |x|<k, k sgn(x)         otherwise
+     * Weight      w(x)=\phi(x)/x  1               1/|x|      1       if |x|<k, k/|x|            otherwise
      *
      * With these definitions, D(\rho(x), p) = \phi(x) D(x,p) = w(x) x D(x,p) = w(x) D(L2(x), p),
      * and hence we can solve the equivalent weighted least squares problem \sum w(r_i) \rho(r_i)

From 65b309e5cda2e73ca8229017307b6e4d5d649519 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 19 Sep 2019 12:58:27 -0400
Subject: [PATCH 05/17] renamed global variables in smallExample.h and added
 optional noise model parameters

---
 tests/smallExample.h | 55 +++++++++++++++++++++++---------------------
 1 file changed, 29 insertions(+), 26 deletions(-)

diff --git a/tests/smallExample.h b/tests/smallExample.h
index 146289dac..d268f2787 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -159,10 +159,10 @@ namespace example {
 namespace impl {
 typedef boost::shared_ptr<NonlinearFactor> shared_nlf;
 
-static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
-static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
-static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
-static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
+static SharedDiagonal kSigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
+static SharedDiagonal kSigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
+static SharedDiagonal kSigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
+static SharedDiagonal kConstrainedModel = noiseModel::Constrained::All(2);
 
 static const Key _l1_=0, _x1_=1, _x2_=2;
 static const Key _x_=0, _y_=1, _z_=2;
@@ -170,40 +170,43 @@ static const Key _x_=0, _y_=1, _z_=2;
 
 
 /* ************************************************************************* */
-inline boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph() {
+inline boost::shared_ptr<const NonlinearFactorGraph>
+sharedNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1,
+                           const noiseModel::Base &noiseModel2 = kSigma0_2) {
   using namespace impl;
-  using symbol_shorthand::X;
   using symbol_shorthand::L;
+  using symbol_shorthand::X;
   // Create
-  boost::shared_ptr<NonlinearFactorGraph> nlfg(
-      new NonlinearFactorGraph);
+  boost::shared_ptr<NonlinearFactorGraph> nlfg(new NonlinearFactorGraph);
 
   // prior on x1
-  Point2 mu(0,0);
-  shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
+  Point2 mu(0, 0);
+  shared_nlf f1(new simulated2D::Prior(mu, noiseModel1, X(1)));
   nlfg->push_back(f1);
 
   // odometry between x1 and x2
   Point2 z2(1.5, 0);
-  shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2)));
+  shared_nlf f2(new simulated2D::Odometry(z2, noiseModel1, X(1), X(2)));
   nlfg->push_back(f2);
 
   // measurement between x1 and l1
   Point2 z3(0, -1);
-  shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1)));
+  shared_nlf f3(new simulated2D::Measurement(z3, noiseModel2, X(1), L(1)));
   nlfg->push_back(f3);
 
   // measurement between x2 and l1
   Point2 z4(-1.5, -1.);
-  shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1)));
+  shared_nlf f4(new simulated2D::Measurement(z4, noiseModel2, X(2), L(1)));
   nlfg->push_back(f4);
 
   return nlfg;
 }
 
 /* ************************************************************************* */
-inline NonlinearFactorGraph createNonlinearFactorGraph() {
-  return *sharedNonlinearFactorGraph();
+inline NonlinearFactorGraph
+createNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1,
+                           const noiseModel::Base &noiseModel2 = kSigma0_2) {
+  return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2);
 }
 
 /* ************************************************************************* */
@@ -372,19 +375,19 @@ inline std::pair<NonlinearFactorGraph, Values> createNonlinearSmoother(int T) {
 
   // prior on x1
   Point2 x1(1.0, 0.0);
-  shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1)));
+  shared_nlf prior(new simulated2D::Prior(x1, kSigma1_0, X(1)));
   nlfg.push_back(prior);
   poses.insert(X(1), x1);
 
   for (int t = 2; t <= T; t++) {
     // odometry between x_t and x_{t-1}
     Point2 odo(1.0, 0.0);
-    shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t)));
+    shared_nlf odometry(new simulated2D::Odometry(odo, kSigma1_0, X(t - 1), X(t)));
     nlfg.push_back(odometry);
 
     // measurement on x_t is like perfect GPS
     Point2 xt(t, 0);
-    shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t)));
+    shared_nlf measurement(new simulated2D::Prior(xt, kSigma1_0, X(t)));
     nlfg.push_back(measurement);
 
     // initial estimate
@@ -412,7 +415,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
   Vector b1(2);
   b1(0) = 1.0;
   b1(1) = -1.0;
-  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
+  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1));
 
   // create binary constraint factor
   // between _x_ and _y_, that is going to be the only factor on _y_
@@ -422,7 +425,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
   Matrix Ay1 = I_2x2 * -1;
   Vector b2 = Vector2(0.0, 0.0);
   JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
-      constraintModel));
+      kConstrainedModel));
 
   // construct the graph
   GaussianFactorGraph fg;
@@ -453,8 +456,8 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
   Vector b1(2);
   b1(0) = 1.0;
   b1(1) = -1.0;
-  //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
-  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1));
+  //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, kSigma0_1->Whiten(Ax), kSigma0_1->whiten(b1), kSigma0_1));
+  JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, kSigma0_1));
 
   // create binary constraint factor
   // between _x_ and _y_, that is going to be the only factor on _y_
@@ -468,7 +471,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
   Matrix Ay1 = I_2x2 * 10;
   Vector b2 = Vector2(1.0, 2.0);
   JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
-      constraintModel));
+      kConstrainedModel));
 
   // construct the graph
   GaussianFactorGraph fg;
@@ -493,7 +496,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   // unary factor 1
   Matrix A = I_2x2;
   Vector b = Vector2(-2.0, 2.0);
-  JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));
+  JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, kSigma0_1));
 
   // constraint 1
   Matrix A11(2, 2);
@@ -512,7 +515,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   b1(0) = 1.0;
   b1(1) = 2.0;
   JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
-      constraintModel));
+      kConstrainedModel));
 
   // constraint 2
   Matrix A21(2, 2);
@@ -531,7 +534,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
   b2(0) = 3.0;
   b2(1) = 4.0;
   JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
-      constraintModel));
+      kConstrainedModel));
 
   // construct the graph
   GaussianFactorGraph fg;

From f58d421b71836dbb850913fcf55356282595d18a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 24 Sep 2019 12:48:38 -0400
Subject: [PATCH 06/17] improve m-estimator visualization code

---
 matlab/gtsam_examples/VisualizeMEstimators.m | 196 ++++++-------------
 1 file changed, 63 insertions(+), 133 deletions(-)

diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
index 8fce828aa..5120934fe 100644
--- a/matlab/gtsam_examples/VisualizeMEstimators.m
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -1,172 +1,102 @@
-import gtsam.*
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Plot visualizations of residuals, residual derivatives, and weights for the various mEstimators.
+% @author Varun Agrawal
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+% import gtsam.*
 
 clear all;
 close all;
 
 x = linspace(-10, 10, 1000);
-% x = linspace(-10, 10, 21)
+% x = linspace(-5, 5, 101);
 
-[rho, psi, w] = fair(x);
-figure(1);
-plot_rho(x, rho, 1, -1, 30)
-title('Fair');
-plot_psi(x, psi, 2, -3, 3);
-plot_w(x, w, 3, 3);
-saveas(figure(1), 'fair.png');
+c = 1.3998;
+rho = fair(x, c);
+fairNoiseModel = gtsam.noiseModel.mEstimator.Fair(c);
+plot_m_estimator(x, fairNoiseModel, rho, 'Fair', 1, 'fair.png')
 
-[rho, psi, w] = huber(x);
-figure(2);
-plot_rho(x, rho, 1, -1, 30);
-title('Huber');
-plot_psi(x, psi, 2, -15, 15);
-plot_w(x, w, 3, 5);
-saveas(figure(2), 'huber.png');
+c = 1.345;
+rho = huber(x, c);
+huberNoiseModel = gtsam.noiseModel.mEstimator.Huber(c);
+plot_m_estimator(x, huberNoiseModel, rho, 'Huber', 2, 'huber.png')
 
-[rho, psi, w] = cauchy(x);
-figure(3);
-plot_rho(x, rho, 1, -0.1, 0.1);
-title('Cauchy');
-plot_psi(x, psi, 2, -0.2, 0.2);
-plot_w(x, w, 3, 1.5);
-saveas(figure(3), 'cauchy.png');
+c = 0.1;
+rho = cauchy(x, c);
+cauchyNoiseModel = gtsam.noiseModel.mEstimator.Cauchy(c);
+plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png')
 
-[rho, psi, w] = gemancclure(x);
-figure(4);
-plot_rho(x, rho, 1, -1, 1);
-title('Geman-McClure');
-plot_psi(x, psi, 2, -1, 1);
-plot_w(x, w, 3, 1.5);
-saveas(figure(4), 'gemanmcclure.png');
+c = 1.0;
+rho = gemanmcclure(x, c);
+gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c);
+plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'GemanMcClure', 4, 'gemanmcclure.png')
 
-[rho, psi, w] = welsch(x);
-figure(5);
-plot_rho(x, rho, 1, -5, 10);
-title('Welsch');
-plot_psi(x, psi, 2, -2, 2);
-plot_w(x, w, 3, 2);
-saveas(figure(5), 'welsch.png');
+c = 2.9846;
+rho = welsch(x, c);
+welschNoiseModel = gtsam.noiseModel.mEstimator.Welsch(c);
+plot_m_estimator(x, welschNoiseModel, rho, 'Welsch', 5, 'welsch.png')
 
-[rho, psi, w] = tukey(x);
-figure(6);
-plot_rho(x, rho, 1, -5, 10);
-title('Tukey');
-plot_psi(x, psi, 2, -2, 2);
-plot_w(x, w, 3, 2);
-saveas(figure(6), 'tukey.png');
+c = 4.6851;
+rho = tukey(x, c);
+tukeyNoiseModel = gtsam.noiseModel.mEstimator.Tukey(c);
+plot_m_estimator(x, tukeyNoiseModel, rho, 'Tukey', 6, 'tukey.png')
 
-function plot_rho(x, y, idx, yll, ylu)
-    subplot(3, 1, idx);
-    plot(x, y);
-    xlim([-15, 15]);
-    ylim([yll, ylu]);
-end
-
-function plot_psi(x, y, idx, yll, ylu)
-    subplot(3, 1, idx);
-    plot(x, y);
-    xlim([-15, 15]);
-    ylim([yll, ylu]);
-end
-
-function plot_w(x, y, idx, yl)
-    subplot(3, 1, idx);
-    plot(x, y);
-    xlim([-15, 15]);
-    ylim([-yl, yl]);
-end
-
-function [rho, psi, w] = fair(x)
-    import gtsam.*
-    c = 1.3998;
-
-    rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) );
-    est = noiseModel.mEstimator.Fair(c);
-    
+%% Plot rho, psi and weights of the mEstimator.
+function plot_m_estimator(x, model, rho, plot_title, fig_id, filename)
     w = zeros(size(x));
     for i = 1:size(x, 2)
-        w(i) = est.weight(x(i));
+        w(i) = model.weight(x(i));
     end
 
     psi = w .* x;
+
+    figure(fig_id);
+    subplot(3, 1, 1);
+    plot(x, rho);
+    xlim([-5, 5]);
+    title(plot_title);
+    subplot(3, 1, 2);
+    plot(x, psi);
+    xlim([-5, 5]);
+    subplot(3, 1, 3);
+    plot(x, w);
+    xlim([-5, 5]);
+    saveas(figure(fig_id), filename);
 end
 
-function [rho, psi, w] = huber(x)
-    import gtsam.*
-    k = 5;
+function rho = fair(x, c)
+    rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) );
+end
+
+function rho = huber(x, k)
     t = (abs(x) > k);
 
     rho = (x .^ 2) ./ 2;
     rho(t) = k * (abs(x(t)) - (k/2));
-    est = noiseModel.mEstimator.Huber(k);
-
-    w = zeros(size(x));
-    for i = 1:size(x, 2)
-        w(i) = est.weight(x(i));
-    end
-
-    psi = w .* x;
 end
 
-function [rho, psi, w] = cauchy(x)
-    import gtsam.*
-    c = 0.1;
-    
+function rho = cauchy(x, c)    
     rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2));
-
-    est = noiseModel.mEstimator.Cauchy(c);
-    
-    w = zeros(size(x));
-    for i = 1:size(x, 2)
-        w(i) = est.weight(x(i));
-    end
-
-    psi = w .* x;
 end
 
-function [rho, psi, w] = gemancclure(x)
-    import gtsam.*
-    c = 1.0;
+function rho = gemanmcclure(x, c)
     rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2);
-    
-    est = noiseModel.mEstimator.GemanMcClure(c);
-    
-    w = zeros(size(x));
-    for i = 1:size(x, 2)
-        w(i) = est.weight(x(i));
-    end
-
-    psi = w .* x;
 end
 
-function [rho, psi, w] = welsch(x)
-    import gtsam.*
-    c = 2.9846;
+function rho = welsch(x, c)
     rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 ));
-    
-    est = noiseModel.mEstimator.Welsch(c);
-
-    w = zeros(size(x));
-    for i = 1:size(x, 2)
-        w(i) = est.weight(x(i));
-    end
-
-    psi = w .* x;
 end
 
-function [rho, psi, w] = tukey(x)
-    import gtsam.*
-    c = 4.6851;
+function rho = tukey(x, c)
     t = (abs(x) > c);
 
     rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 );
     rho(t) = c .^ 2 / 6;
-
-    est = noiseModel.mEstimator.Tukey(c);
-
-    w = zeros(size(x));
-    for i = 1:size(x, 2)
-        w(i) = est.weight(x(i));
-    end
-
-    psi = w .* x;
 end

From 3eb8e3d9bcb3b2fdc22370828433a018579a5cd1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 25 Sep 2019 23:02:21 -0400
Subject: [PATCH 07/17] fixed function declarations which use globally declared
 noise models

---
 tests/smallExample.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/tests/smallExample.h b/tests/smallExample.h
index d268f2787..2b29a6d10 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -171,8 +171,8 @@ static const Key _x_=0, _y_=1, _z_=2;
 
 /* ************************************************************************* */
 inline boost::shared_ptr<const NonlinearFactorGraph>
-sharedNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1,
-                           const noiseModel::Base &noiseModel2 = kSigma0_2) {
+sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1,
+                           const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
   using namespace impl;
   using symbol_shorthand::L;
   using symbol_shorthand::X;
@@ -204,8 +204,8 @@ sharedNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1,
 
 /* ************************************************************************* */
 inline NonlinearFactorGraph
-createNonlinearFactorGraph(const noiseModel::Base &noiseModel1 = kSigma0_1,
-                           const noiseModel::Base &noiseModel2 = kSigma0_2) {
+createNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1,
+                           const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) {
   return *sharedNonlinearFactorGraph(noiseModel1, noiseModel2);
 }
 

From 8280a082bd25f21b2144c79f5ed583fb6d41c93a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 2 Oct 2019 04:04:34 -0400
Subject: [PATCH 08/17] implemented residual functions for common M-estimators
 and added corresponding tests

---
 gtsam.h                               |  6 ++
 gtsam/linear/NoiseModel.cpp           |  6 ++
 gtsam/linear/NoiseModel.h             | 40 +++++++++++++
 gtsam/linear/tests/testNoiseModel.cpp | 84 +++++++++++++++++++++++++--
 4 files changed, 131 insertions(+), 5 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 23590f5f8..35256e92c 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1364,6 +1364,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@@ -1384,6 +1385,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
@@ -1394,6 +1396,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@@ -1404,6 +1407,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@@ -1414,6 +1418,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
@@ -1424,6 +1429,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 //TODO DCS and L2WithDeadZone mEstimators
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index a01233fad..d4cf2a5c2 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -858,6 +858,12 @@ double GemanMcClure::weight(double error) const {
   return c4/(c2error*c2error);
 }
 
+double GemanMcClure::residual(double error) const {
+  const double c2 = c_*c_;
+  const double error2 = error*error;
+  return 0.5 * (c2 * error2) / (c2 + error2);
+}
+
 void GemanMcClure::print(const std::string &s="") const {
   std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl;
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 464bb38ed..2ef830c81 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include<iostream>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
@@ -727,6 +728,7 @@ namespace gtsam {
         Null(const ReweightScheme reweight = Block) : Base(reweight) {}
         virtual ~Null() {}
         virtual double weight(double /*error*/) const { return 1.0; }
+        virtual double residual(double error) const { return error; }
         virtual void print(const std::string &s) const;
         virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
         static shared_ptr Create() ;
@@ -752,6 +754,12 @@ namespace gtsam {
         double weight(double error) const {
           return 1.0 / (1.0 + std::abs(error) / c_);
         }
+        double residual(double error) const {
+          double absError = std::abs(error);
+          double normalizedError = absError / c_;
+          double val = normalizedError - std::log(1 + normalizedError);
+          return c_ * c_ * val;
+        }
         void print(const std::string &s) const;
         bool equals(const Base& expected, double tol=1e-8) const;
         static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
@@ -779,6 +787,14 @@ namespace gtsam {
           double absError = std::abs(error);
           return (absError < k_) ? (1.0) : (k_ / absError);
         }
+        double residual(double error) const {
+          double absError = std::abs(error);
+          if (absError <= k_) {  // |x| <= k
+            return error*error / 2;
+          } else { // |x| > k
+            return k_ * (absError - (k_/2));
+          }
+        }
         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) ;
@@ -809,6 +825,11 @@ namespace gtsam {
         double weight(double error) const {
           return ksquared_ / (ksquared_ + error*error);
         }
+        double residual(double error) const {
+          double normalizedError = error / k_;
+          double val = std::log(1 + (normalizedError*normalizedError));
+          return ksquared_ * val * 0.5;
+        }
         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) ;
@@ -839,6 +860,16 @@ namespace gtsam {
           }
           return 0.0;
         }
+        double residual(double error) const {
+          double absError = std::abs(error);
+          if (absError <= c_) {
+            double t = csquared_ - (error*error);
+            double val = t * t * t;
+            return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_);
+          } else {
+            return csquared_ / 6.0;
+          }
+        }
         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) ;
@@ -866,6 +897,10 @@ namespace gtsam {
           double xc2 = (error*error)/csquared_;
           return std::exp(-xc2);
         }
+        double residual(double error) const {
+          double xc2 = (error*error)/csquared_;
+          return csquared_ * 0.5 * (1 - 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) ;
@@ -897,6 +932,10 @@ namespace gtsam {
           double xc2 = (error*error)/csquared_;
           return std::exp(-xc2);
         }
+        double residual(double error) const {
+          double xc2 = (error*error)/csquared_;
+          return csquared_ * 0.5 * (1 - 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) ;
@@ -925,6 +964,7 @@ namespace gtsam {
         GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
         virtual ~GemanMcClure() {}
         virtual double weight(double error) const;
+        virtual double residual(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) ;
diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 85dc4735d..6419f2b9d 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -457,6 +457,22 @@ TEST(NoiseModel, WhitenInPlace)
  * penalties using iteratively re-weighted least squares.
  */
 
+TEST(NoiseModel, robustFunctionFair)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k);
+  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8);
+  DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8);
+  DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8);
+  DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8);
+}
+
 TEST(NoiseModel, robustFunctionHuber)
 {
   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
@@ -466,16 +482,74 @@ TEST(NoiseModel, robustFunctionHuber)
   // Test negative value to ensure we take absolute value of error.
   DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
   DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8);
+  DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8);
+  DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8);
+  DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8);
+}
+
+TEST(NoiseModel, robustFunctionCauchy)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k);
+  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8);
+  DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8);
+  DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8);
+  DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionGemanMcClure)
 {
-  const double k = 1.0, error1 = 1.0, error2 = 10.0;
+  const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
   const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
-  const double weight1 = gmc->weight(error1),
-               weight2 = gmc->weight(error2);
-  DOUBLES_EQUAL(0.25      , weight1, 1e-8);
-  DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8);
+  DOUBLES_EQUAL(0.25      , gmc->weight(error1), 1e-8);
+  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8);
+  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.25      , gmc->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8);
+  DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8);
+  DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8);
+  DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8);
+}
+
+TEST(NoiseModel, robustFunctionWelsch)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k);
+  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8);
+  DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8);
+  DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8);
+  DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8);
+}
+
+TEST(NoiseModel, robustFunctionTukey)
+{
+  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
+  const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k);
+  DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8);
+  // Test negative value to ensure we take absolute value of error.
+  DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
+  DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
+
+  DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8);
+  DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8);
+  DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8);
+  DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionDCS)

From 5c883caf168809adf36995067a1cca548e0db5a5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 7 Oct 2019 23:43:01 -0400
Subject: [PATCH 09/17] label subplots for each estimator and hyphenate
 Geman-McClure

---
 matlab/gtsam_examples/VisualizeMEstimators.m | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
index 5120934fe..1c5a14a70 100644
--- a/matlab/gtsam_examples/VisualizeMEstimators.m
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -36,7 +36,7 @@ plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png')
 c = 1.0;
 rho = gemanmcclure(x, c);
 gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c);
-plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'GemanMcClure', 4, 'gemanmcclure.png')
+plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'Geman-McClure', 4, 'gemanmcclure.png')
 
 c = 2.9846;
 rho = welsch(x, c);
@@ -59,15 +59,20 @@ function plot_m_estimator(x, model, rho, plot_title, fig_id, filename)
 
     figure(fig_id);
     subplot(3, 1, 1);
-    plot(x, rho);
+    plot(x, rho, 'LineWidth',2);
+    title('rho function');
     xlim([-5, 5]);
-    title(plot_title);
     subplot(3, 1, 2);
-    plot(x, psi);
+    plot(x, psi, 'LineWidth',2);
+    title('influence function');
     xlim([-5, 5]);
     subplot(3, 1, 3);
-    plot(x, w);
+    plot(x, w, 'LineWidth',2);
+    title('weight function');
     xlim([-5, 5]);
+
+    sgtitle(plot_title, 'FontSize', 26);
+
     saveas(figure(fig_id), filename);
 end
 

From 4be8d8a75853409b44599e0df7bb160896f4838f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 8 Oct 2019 20:20:29 -0400
Subject: [PATCH 10/17] Robust Estimator Updates

- Moved implementations of residual and weight to NoiseModel.cpp.
- Added `const` to function arguments and variables in `residual` and `weight`.
- Aliased Welsh to Welsch.
- Formatting.
---
 gtsam/linear/NoiseModel.cpp |  76 ++++++++++++++++++++++--
 gtsam/linear/NoiseModel.h   | 113 +++++++-----------------------------
 2 files changed, 92 insertions(+), 97 deletions(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index d4cf2a5c2..67aa20b10 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -718,15 +718,25 @@ void Null::print(const std::string &s="") const
 Null::shared_ptr Null::Create()
 { return shared_ptr(new Null()); }
 
+/* ************************************************************************* */
+// Fair
+/* ************************************************************************* */
+
 Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
   if (c_ <= 0) {
     throw runtime_error("mEstimator Fair takes only positive double in constructor.");
   }
 }
 
-/* ************************************************************************* */
-// Fair
-/* ************************************************************************* */
+double Fair::weight(const double error) const {
+  return 1.0 / (1.0 + std::abs(error) / c_);
+}
+double Fair::residual(const double error) const {
+  const double absError = std::abs(error);
+  const double normalizedError = absError / c_;
+  const double c_2 = c_ * c_;
+  return c_2 * (normalizedError - std::log(1 + normalizedError));
+}
 
 void Fair::print(const std::string &s="") const
 { cout << s << "fair (" << c_ << ")" << endl; }
@@ -750,6 +760,20 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
   }
 }
 
+double Huber::weight(const double error) const {
+  const double absError = std::abs(error);
+  return (absError <= k_) ? (1.0) : (k_ / absError);
+}
+
+double Huber::residual(const double error) const {
+  const double absError = std::abs(error);
+  if (absError <= k_) {  // |x| <= k
+    return error*error / 2;
+  } else { // |x| > k
+    return k_ * (absError - (k_/2));
+  }
+}
+
 void Huber::print(const std::string &s="") const {
   cout << s << "huber (" << k_ << ")" << endl;
 }
@@ -774,6 +798,16 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k),
   }
 }
 
+double Cauchy::weight(const double error) const {
+  return ksquared_ / (ksquared_ + error*error);
+}
+
+double Cauchy::residual(const double error) const {
+  const double xc2 = error / k_;
+  const double val = std::log(1 + (xc2*xc2));
+  return ksquared_ * val * 0.5;
+}
+
 void Cauchy::print(const std::string &s="") const {
   cout << s << "cauchy (" << k_ << ")" << endl;
 }
@@ -791,7 +825,30 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) {
 /* ************************************************************************* */
 // Tukey
 /* ************************************************************************* */
-Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
+
+Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {
+  if (c <= 0) {
+    throw runtime_error("mEstimator Tukey takes only positive double in constructor.");
+  }
+}
+
+double Tukey::weight(const double error) const {
+  if (std::abs(error) <= c_) {
+    const double xc2 = error*error/csquared_;
+    return (1.0-xc2)*(1.0-xc2);
+  }
+  return 0.0;
+}
+double Tukey::residual(const double error) const {
+  double absError = std::abs(error);
+  if (absError <= c_) {
+    const double xc2 = error*error/csquared_;
+    const double t = (1 - xc2)*(1 - xc2)*(1 - xc2);
+    return csquared_ * (1 - t) / 6.0;
+  } else {
+    return csquared_ / 6.0;
+  }
+}
 
 void Tukey::print(const std::string &s="") const {
   std::cout << s << ": Tukey (" << c_ << ")" << std::endl;
@@ -810,8 +867,19 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
 /* ************************************************************************* */
 // Welsch
 /* ************************************************************************* */
+
 Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
 
+double Welsch::weight(const double error) const {
+  const double xc2 = (error*error)/csquared_;
+  return std::exp(-xc2);
+}
+
+double Welsch::residual(const double error) const {
+  const double xc2 = (error*error)/csquared_;
+  return csquared_ * 0.5 * (1 - std::exp(-xc2) );
+}
+
 void Welsch::print(const std::string &s="") const {
   std::cout << s << ": Welsch (" << c_ << ")" << std::endl;
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 2ef830c81..5f957ba01 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -18,7 +18,6 @@
 
 #pragma once
 
-#include<iostream>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
@@ -677,7 +676,7 @@ namespace gtsam {
          * evaluating the total penalty. But for now, I'm leaving this residual method as pure
          * virtual, so the existing mEstimators can inherit this default fallback behavior.
          */
-        virtual double residual(double error) const { return 0; };
+        virtual double residual(const double error) const { return 0; };
 
         /*
          * This method is responsible for returning the weight function for a given amount of error.
@@ -686,7 +685,7 @@ namespace gtsam {
          * for details. This method is required when optimizing cost functions with robust penalties
          * using iteratively re-weighted least squares.
          */
-        virtual double weight(double error) const = 0;
+        virtual double weight(const double error) const = 0;
 
         virtual void print(const std::string &s) const = 0;
         virtual bool equals(const Base& expected, double tol=1e-8) const = 0;
@@ -727,8 +726,8 @@ namespace gtsam {
 
         Null(const ReweightScheme reweight = Block) : Base(reweight) {}
         virtual ~Null() {}
-        virtual double weight(double /*error*/) const { return 1.0; }
-        virtual double residual(double error) const { return error; }
+        virtual double weight(const double /*error*/) const { return 1.0; }
+        virtual double residual(const double error) const { return error; }
         virtual void print(const std::string &s) const;
         virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
         static shared_ptr Create() ;
@@ -751,15 +750,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Fair> shared_ptr;
 
         Fair(double c = 1.3998, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          return 1.0 / (1.0 + std::abs(error) / c_);
-        }
-        double residual(double error) const {
-          double absError = std::abs(error);
-          double normalizedError = absError / c_;
-          double val = normalizedError - std::log(1 + normalizedError);
-          return c_ * c_ * val;
-        }
+        double weight(const double error) const;
+        double residual(const double error) const;
         void print(const std::string &s) const;
         bool equals(const Base& expected, double tol=1e-8) const;
         static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
@@ -783,18 +775,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Huber> shared_ptr;
 
         Huber(double k = 1.345, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          double absError = std::abs(error);
-          return (absError < k_) ? (1.0) : (k_ / absError);
-        }
-        double residual(double error) const {
-          double absError = std::abs(error);
-          if (absError <= k_) {  // |x| <= k
-            return error*error / 2;
-          } else { // |x| > k
-            return k_ * (absError - (k_/2));
-          }
-        }
+        double weight(const double error) const;
+        double residual(const double error) const;
         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) ;
@@ -822,14 +804,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Cauchy> shared_ptr;
 
         Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          return ksquared_ / (ksquared_ + error*error);
-        }
-        double residual(double error) const {
-          double normalizedError = error / k_;
-          double val = std::log(1 + (normalizedError*normalizedError));
-          return ksquared_ * val * 0.5;
-        }
+        double weight(const double error) const;
+        double residual(const double error) const;
         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) ;
@@ -853,23 +829,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Tukey> shared_ptr;
 
         Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          if (std::abs(error) <= c_) {
-            double xc2 = error*error/csquared_;
-            return (1.0-xc2)*(1.0-xc2);
-          }
-          return 0.0;
-        }
-        double residual(double error) const {
-          double absError = std::abs(error);
-          if (absError <= c_) {
-            double t = csquared_ - (error*error);
-            double val = t * t * t;
-            return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_);
-          } else {
-            return csquared_ / 6.0;
-          }
-        }
+        double weight(const double error) const;
+        double residual(const double error) const;
         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) ;
@@ -893,14 +854,8 @@ namespace gtsam {
         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);
-        }
-        double residual(double error) const {
-          double xc2 = (error*error)/csquared_;
-          return csquared_ * 0.5 * (1 - std::exp(-xc2) );
-        }
+        double weight(const double error) const;
+        double residual(const double error) const;
         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) ;
@@ -920,35 +875,7 @@ namespace gtsam {
       // 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_;
-
-      public:
-        typedef boost::shared_ptr<Welsh> shared_ptr;
-
-        Welsh(double c = 2.9846, const ReweightScheme reweight = Block);
-        double weight(double error) const {
-          double xc2 = (error*error)/csquared_;
-          return std::exp(-xc2);
-        }
-        double residual(double error) const {
-          double xc2 = (error*error)/csquared_;
-          return csquared_ * 0.5 * (1 - 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_);
-        }
-      };
+      using Welsh = Welsch;
 #endif
 
       /// GemanMcClure implements the "Geman-McClure" robust error model
@@ -963,8 +890,8 @@ namespace gtsam {
 
         GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
         virtual ~GemanMcClure() {}
-        virtual double weight(double error) const;
-        virtual double residual(double error) const;
+        virtual double weight(const double error) const;
+        virtual double residual(const 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) ;
@@ -993,7 +920,7 @@ namespace gtsam {
 
         DCS(double c = 1.0, const ReweightScheme reweight = Block);
         virtual ~DCS() {}
-        virtual double weight(double error) const;
+        virtual double weight(const 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) ;
@@ -1024,11 +951,11 @@ namespace gtsam {
           typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
 
           L2WithDeadZone(double k, const ReweightScheme reweight = Block);
-          double residual(double error) const {
+          double residual(const double error) const {
             const double abs_error = std::abs(error);
             return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
           }
-          double weight(double error) const {
+          double weight(const double error) const {
             // note that this code is slightly uglier than above, because there are three distinct
             // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
             // cases (deadzone, non-deadzone) above.

From 431c37090a1e133ee400e5316cfd206cf792d82f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 8 Oct 2019 21:25:41 -0400
Subject: [PATCH 11/17] removed const declarator from  and  functions,
 implemented DCS residual

---
 gtsam/linear/NoiseModel.cpp | 43 ++++++++++++++++++++------
 gtsam/linear/NoiseModel.h   | 61 ++++++++++++++++---------------------
 2 files changed, 59 insertions(+), 45 deletions(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 67aa20b10..68e579718 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -728,10 +728,10 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
   }
 }
 
-double Fair::weight(const double error) const {
+double Fair::weight(double error) const {
   return 1.0 / (1.0 + std::abs(error) / c_);
 }
-double Fair::residual(const double error) const {
+double Fair::residual(double error) const {
   const double absError = std::abs(error);
   const double normalizedError = absError / c_;
   const double c_2 = c_ * c_;
@@ -760,12 +760,12 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
   }
 }
 
-double Huber::weight(const double error) const {
+double Huber::weight(double error) const {
   const double absError = std::abs(error);
   return (absError <= k_) ? (1.0) : (k_ / absError);
 }
 
-double Huber::residual(const double error) const {
+double Huber::residual(double error) const {
   const double absError = std::abs(error);
   if (absError <= k_) {  // |x| <= k
     return error*error / 2;
@@ -798,11 +798,11 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k),
   }
 }
 
-double Cauchy::weight(const double error) const {
+double Cauchy::weight(double error) const {
   return ksquared_ / (ksquared_ + error*error);
 }
 
-double Cauchy::residual(const double error) const {
+double Cauchy::residual(double error) const {
   const double xc2 = error / k_;
   const double val = std::log(1 + (xc2*xc2));
   return ksquared_ * val * 0.5;
@@ -832,14 +832,14 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c
   }
 }
 
-double Tukey::weight(const double error) const {
+double Tukey::weight(double error) const {
   if (std::abs(error) <= c_) {
     const double xc2 = error*error/csquared_;
     return (1.0-xc2)*(1.0-xc2);
   }
   return 0.0;
 }
-double Tukey::residual(const double error) const {
+double Tukey::residual(double error) const {
   double absError = std::abs(error);
   if (absError <= c_) {
     const double xc2 = error*error/csquared_;
@@ -870,12 +870,12 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
 
 Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
 
-double Welsch::weight(const double error) const {
+double Welsch::weight(double error) const {
   const double xc2 = (error*error)/csquared_;
   return std::exp(-xc2);
 }
 
-double Welsch::residual(const double error) const {
+double Welsch::residual(double error) const {
   const double xc2 = (error*error)/csquared_;
   return csquared_ * 0.5 * (1 - std::exp(-xc2) );
 }
@@ -964,6 +964,16 @@ double DCS::weight(double error) const {
   return 1.0;
 }
 
+double DCS::residual(double error) const {
+  // This is the simplified version of Eq 9 from (Agarwal13icra)
+  // after you simplify and cancel terms.
+  const double e2 = error*error;
+  const double e4 = e2*e2;
+  const double c2 = c_*c_;
+
+  return (c2*e2 + c_*e4) / ((e2 + c_)*(e2 + c_));
+}
+
 void DCS::print(const std::string &s="") const {
   std::cout << s << ": DCS (" << c_ << ")" << std::endl;
 }
@@ -988,6 +998,19 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(r
   }
 }
 
+double L2WithDeadZone::residual(double error) const {
+  const double abs_error = std::abs(error);
+  return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
+}
+double L2WithDeadZone::weight(double error) const {
+  // note that this code is slightly uglier than residual, because there are three distinct
+  // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
+  // cases (deadzone, non-deadzone) in residual.
+  if (std::abs(error) <= k_) return 0.0;
+  else if (error > k_) return (-k_+error)/error;
+  else return (k_+error)/error;
+}
+
 void L2WithDeadZone::print(const std::string &s="") const {
   std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl;
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 5f957ba01..4c289427b 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -676,7 +676,7 @@ namespace gtsam {
          * evaluating the total penalty. But for now, I'm leaving this residual method as pure
          * virtual, so the existing mEstimators can inherit this default fallback behavior.
          */
-        virtual double residual(const double error) const { return 0; };
+        virtual double residual(double error) const { return 0; };
 
         /*
          * This method is responsible for returning the weight function for a given amount of error.
@@ -685,7 +685,7 @@ namespace gtsam {
          * for details. This method is required when optimizing cost functions with robust penalties
          * using iteratively re-weighted least squares.
          */
-        virtual double weight(const double error) const = 0;
+        virtual double weight(double error) const = 0;
 
         virtual void print(const std::string &s) const = 0;
         virtual bool equals(const Base& expected, double tol=1e-8) const = 0;
@@ -726,8 +726,8 @@ namespace gtsam {
 
         Null(const ReweightScheme reweight = Block) : Base(reweight) {}
         virtual ~Null() {}
-        virtual double weight(const double /*error*/) const { return 1.0; }
-        virtual double residual(const double error) const { return error; }
+        virtual double weight(double /*error*/) const { return 1.0; }
+        virtual double residual(double error) const { return error; }
         virtual void print(const std::string &s) const;
         virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
         static shared_ptr Create() ;
@@ -750,8 +750,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Fair> shared_ptr;
 
         Fair(double c = 1.3998, const ReweightScheme reweight = Block);
-        double weight(const double error) const;
-        double residual(const double error) const;
+        double weight(double error) const;
+        double residual(double error) const;
         void print(const std::string &s) const;
         bool equals(const Base& expected, double tol=1e-8) const;
         static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
@@ -775,8 +775,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Huber> shared_ptr;
 
         Huber(double k = 1.345, const ReweightScheme reweight = Block);
-        double weight(const double error) const;
-        double residual(const double error) const;
+        double weight(double error) const;
+        double residual(double error) const;
         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) ;
@@ -804,8 +804,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Cauchy> shared_ptr;
 
         Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
-        double weight(const double error) const;
-        double residual(const double error) const;
+        double weight(double error) const;
+        double residual(double error) const;
         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) ;
@@ -829,8 +829,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Tukey> shared_ptr;
 
         Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
-        double weight(const double error) const;
-        double residual(const double error) const;
+        double weight(double error) const;
+        double residual(double error) const;
         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) ;
@@ -854,8 +854,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Welsch> shared_ptr;
 
         Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
-        double weight(const double error) const;
-        double residual(const double error) const;
+        double weight(double error) const;
+        double residual(double error) const;
         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) ;
@@ -889,11 +889,11 @@ namespace gtsam {
         typedef boost::shared_ptr<GemanMcClure> shared_ptr;
 
         GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
-        virtual ~GemanMcClure() {}
-        virtual double weight(const double error) const;
-        virtual double residual(const double error) const;
-        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        ~GemanMcClure() {}
+        double weight(double error) const;
+        double residual(double error) const;
+        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) ;
 
       protected:
@@ -919,10 +919,11 @@ namespace gtsam {
         typedef boost::shared_ptr<DCS> shared_ptr;
 
         DCS(double c = 1.0, const ReweightScheme reweight = Block);
-        virtual ~DCS() {}
-        virtual double weight(const double error) const;
-        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& expected, double tol=1e-8) const;
+        ~DCS() {}
+        double weight(double error) const;
+        double residual(double error) const;
+        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) ;
 
       protected:
@@ -951,18 +952,8 @@ namespace gtsam {
           typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
 
           L2WithDeadZone(double k, const ReweightScheme reweight = Block);
-          double residual(const double error) const {
-            const double abs_error = std::abs(error);
-            return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
-          }
-          double weight(const double error) const {
-            // note that this code is slightly uglier than above, because there are three distinct
-            // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
-            // cases (deadzone, non-deadzone) above.
-            if (std::abs(error) <= k_) return 0.0;
-            else if (error > k_) return (-k_+error)/error;
-            else return (k_+error)/error;
-          }
+          double residual(double error) const;
+          double weight(double error) const;
           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);

From 586a825c7c166a4b1e41a53d7ed229cd4d2a40bc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 8 Oct 2019 21:25:56 -0400
Subject: [PATCH 12/17] added DCS residual tests

---
 gtsam/linear/tests/testNoiseModel.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 6419f2b9d..10578627f 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -556,10 +556,12 @@ TEST(NoiseModel, robustFunctionDCS)
 {
   const double k = 1.0, error1 = 1.0, error2 = 10.0;
   const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k);
-  const double weight1 = dcs->weight(error1),
-               weight2 = dcs->weight(error2);
-  DOUBLES_EQUAL(1.0       , weight1, 1e-8);
-  DOUBLES_EQUAL(0.00039211, weight2, 1e-8);
+
+  DOUBLES_EQUAL(1.0       , dcs->weight(error1), 1e-8);
+  DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);
+
+  DOUBLES_EQUAL(0.5         , dcs->residual(error1), 1e-8);
+  DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8);
 }
 
 TEST(NoiseModel, robustFunctionL2WithDeadZone)

From 494005d64a6d1619adc08dc2e8a106bc8455d39a Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 8 Oct 2019 21:26:11 -0400
Subject: [PATCH 13/17] wrap DCS and L2WithDeadZone estimators

---
 gtsam.h | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/gtsam.h b/gtsam.h
index 35256e92c..04575f84d 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1432,7 +1432,27 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
   double residual(double error) const;
 };
 
-//TODO DCS and L2WithDeadZone mEstimators
+virtual class DCS: gtsam::noiseModel::mEstimator::Base {
+  DCS(double c);
+  static gtsam::noiseModel::mEstimator::DCS* Create(double c);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double residual(double error) const;
+};
+
+virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
+  L2WithDeadZone(double k);
+  static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+
+  double weight(double error) const;
+  double residual(double error) const;
+};
 
 }///\namespace mEstimator
 

From 7501d8a4b08d00270e7fe88fe3bd2157dbae77b2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 8 Oct 2019 21:31:09 -0400
Subject: [PATCH 14/17] added override keyword to derived versions of  and  for
 mEstimators

---
 gtsam/linear/NoiseModel.h | 32 ++++++++++++++++----------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 4c289427b..d93a7f825 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -750,8 +750,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Fair> shared_ptr;
 
         Fair(double c = 1.3998, const ReweightScheme reweight = Block);
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         void print(const std::string &s) const;
         bool equals(const Base& expected, double tol=1e-8) const;
         static shared_ptr Create(double c, const ReweightScheme reweight = Block) ;
@@ -775,8 +775,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Huber> shared_ptr;
 
         Huber(double k = 1.345, const ReweightScheme reweight = Block);
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         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) ;
@@ -804,8 +804,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Cauchy> shared_ptr;
 
         Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         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) ;
@@ -829,8 +829,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Tukey> shared_ptr;
 
         Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         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) ;
@@ -854,8 +854,8 @@ namespace gtsam {
         typedef boost::shared_ptr<Welsch> shared_ptr;
 
         Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         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) ;
@@ -890,8 +890,8 @@ namespace gtsam {
 
         GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
         ~GemanMcClure() {}
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         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) ;
@@ -920,8 +920,8 @@ namespace gtsam {
 
         DCS(double c = 1.0, const ReweightScheme reweight = Block);
         ~DCS() {}
-        double weight(double error) const;
-        double residual(double error) const;
+        double weight(double error) const override;
+        double residual(double error) const override;
         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) ;
@@ -952,8 +952,8 @@ namespace gtsam {
           typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
 
           L2WithDeadZone(double k, const ReweightScheme reweight = Block);
-          double residual(double error) const;
-          double weight(double error) const;
+          double residual(double error) const override;
+          double weight(double error) const override;
           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);

From 1240339dc2198f6f111ec32e744afae3c06b40d1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 9 Oct 2019 15:33:40 -0400
Subject: [PATCH 15/17] formatting; removed virtual from derived classes; added
 default values for L2WithDeadZone constructor to allow serialization

---
 gtsam.h                     |  4 ++--
 gtsam/linear/NoiseModel.cpp | 32 +++++++++-----------------------
 gtsam/linear/NoiseModel.h   | 14 +++++++-------
 3 files changed, 18 insertions(+), 32 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 04575f84d..09b5b59e6 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1422,8 +1422,8 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
 };
 
 virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
-  GemanMcClure(double k);
-  static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double k);
+  GemanMcClure(double c);
+  static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c);
 
   // enabling serialization functionality
   void serializable() const;
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 68e579718..e0b7c8b17 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -731,6 +731,7 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
 double Fair::weight(double error) const {
   return 1.0 / (1.0 + std::abs(error) / c_);
 }
+
 double Fair::residual(double error) const {
   const double absError = std::abs(error);
   const double normalizedError = absError / c_;
@@ -839,6 +840,7 @@ double Tukey::weight(double error) const {
   }
   return 0.0;
 }
+
 double Tukey::residual(double error) const {
   double absError = std::abs(error);
   if (absError <= c_) {
@@ -894,24 +896,6 @@ 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 {
-  std::cout << s << ": Welsh (" << c_ << ")" << std::endl;
-}
-
-bool Welsh::equals(const Base &expected, double tol) const {
-  const Welsh* p = dynamic_cast<const Welsh*>(&expected);
-  if (p == NULL) return false;
-  return std::abs(c_ - p->c_) < tol;
-}
-
-Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {
-  return shared_ptr(new Welsh(c, reweight));
-}
-#endif
-
 /* ************************************************************************* */
 // GemanMcClure
 /* ************************************************************************* */
@@ -992,16 +976,13 @@ DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) {
 // L2WithDeadZone
 /* ************************************************************************* */
 
-L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
+L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight)
+ : Base(reweight), k_(k) {
   if (k_ <= 0) {
     throw runtime_error("mEstimator L2WithDeadZone takes only positive double in constructor.");
   }
 }
 
-double L2WithDeadZone::residual(double error) const {
-  const double abs_error = std::abs(error);
-  return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
-}
 double L2WithDeadZone::weight(double error) const {
   // note that this code is slightly uglier than residual, because there are three distinct
   // cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
@@ -1011,6 +992,11 @@ double L2WithDeadZone::weight(double error) const {
   else return (k_+error)/error;
 }
 
+double L2WithDeadZone::residual(double error) const {
+  const double abs_error = std::abs(error);
+  return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
+}
+
 void L2WithDeadZone::print(const std::string &s="") const {
   std::cout << s << ": L2WithDeadZone (" << k_ << ")" << std::endl;
 }
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index d93a7f825..d6b0a7331 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -725,11 +725,11 @@ namespace gtsam {
         typedef boost::shared_ptr<Null> shared_ptr;
 
         Null(const ReweightScheme reweight = Block) : Base(reweight) {}
-        virtual ~Null() {}
-        virtual double weight(double /*error*/) const { return 1.0; }
-        virtual double residual(double error) const { return error; }
-        virtual void print(const std::string &s) const;
-        virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
+        ~Null() {}
+        double weight(double /*error*/) const { return 1.0; }
+        double residual(double error) const { return error; }
+        void print(const std::string &s) const;
+        bool equals(const Base& /*expected*/, double /*tol*/) const { return true; }
         static shared_ptr Create() ;
 
       private:
@@ -951,9 +951,9 @@ namespace gtsam {
       public:
           typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
 
-          L2WithDeadZone(double k, const ReweightScheme reweight = Block);
-          double residual(double error) const override;
+          L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
           double weight(double error) const override;
+          double residual(double error) const override;
           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);

From c2b6607d188105a320635e5111655671dba96cb4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 9 Oct 2019 15:46:24 -0400
Subject: [PATCH 16/17] add residual function for Fair mEstimator wrapper

---
 gtsam.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam.h b/gtsam.h
index 09b5b59e6..c9364d9b0 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1375,6 +1375,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
+  double residual(double error) const;
 };
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {

From 4c9f9ecabf5b048db5775f778186d34334e690d5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 9 Oct 2019 15:48:12 -0400
Subject: [PATCH 17/17] cleaned up Matlab script for visualizing mEstimators

---
 matlab/gtsam_examples/VisualizeMEstimators.m | 56 ++++----------------
 1 file changed, 11 insertions(+), 45 deletions(-)

diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m
index 1c5a14a70..8a0485334 100644
--- a/matlab/gtsam_examples/VisualizeMEstimators.m
+++ b/matlab/gtsam_examples/VisualizeMEstimators.m
@@ -12,47 +12,43 @@
 
 % import gtsam.*
 
-clear all;
 close all;
 
 x = linspace(-10, 10, 1000);
-% x = linspace(-5, 5, 101);
+
+%% Define all the mEstimator models and plot them
 
 c = 1.3998;
-rho = fair(x, c);
 fairNoiseModel = gtsam.noiseModel.mEstimator.Fair(c);
-plot_m_estimator(x, fairNoiseModel, rho, 'Fair', 1, 'fair.png')
+plot_m_estimator(x, fairNoiseModel, 'Fair', 1, 'fair.png')
 
 c = 1.345;
-rho = huber(x, c);
 huberNoiseModel = gtsam.noiseModel.mEstimator.Huber(c);
-plot_m_estimator(x, huberNoiseModel, rho, 'Huber', 2, 'huber.png')
+plot_m_estimator(x, huberNoiseModel, 'Huber', 2, 'huber.png')
 
 c = 0.1;
-rho = cauchy(x, c);
 cauchyNoiseModel = gtsam.noiseModel.mEstimator.Cauchy(c);
-plot_m_estimator(x, cauchyNoiseModel, rho, 'Cauchy', 3, 'cauchy.png')
+plot_m_estimator(x, cauchyNoiseModel, 'Cauchy', 3, 'cauchy.png')
 
 c = 1.0;
-rho = gemanmcclure(x, c);
 gemanmcclureNoiseModel = gtsam.noiseModel.mEstimator.GemanMcClure(c);
-plot_m_estimator(x, gemanmcclureNoiseModel, rho, 'Geman-McClure', 4, 'gemanmcclure.png')
+plot_m_estimator(x, gemanmcclureNoiseModel, 'Geman-McClure', 4, 'gemanmcclure.png')
 
 c = 2.9846;
-rho = welsch(x, c);
 welschNoiseModel = gtsam.noiseModel.mEstimator.Welsch(c);
-plot_m_estimator(x, welschNoiseModel, rho, 'Welsch', 5, 'welsch.png')
+plot_m_estimator(x, welschNoiseModel, 'Welsch', 5, 'welsch.png')
 
 c = 4.6851;
-rho = tukey(x, c);
 tukeyNoiseModel = gtsam.noiseModel.mEstimator.Tukey(c);
-plot_m_estimator(x, tukeyNoiseModel, rho, 'Tukey', 6, 'tukey.png')
+plot_m_estimator(x, tukeyNoiseModel, 'Tukey', 6, 'tukey.png')
 
 %% Plot rho, psi and weights of the mEstimator.
-function plot_m_estimator(x, model, rho, plot_title, fig_id, filename)
+function plot_m_estimator(x, model, plot_title, fig_id, filename)
     w = zeros(size(x));
+    rho = zeros(size(x));
     for i = 1:size(x, 2)
         w(i) = model.weight(x(i));
+        rho(i) = model.residual(x(i));
     end
 
     psi = w .* x;
@@ -75,33 +71,3 @@ function plot_m_estimator(x, model, rho, plot_title, fig_id, filename)
 
     saveas(figure(fig_id), filename);
 end
-
-function rho = fair(x, c)
-    rho = c^2 * ( (abs(x) ./ c) - log(1 + (abs(x)./c)) );
-end
-
-function rho = huber(x, k)
-    t = (abs(x) > k);
-
-    rho = (x .^ 2) ./ 2;
-    rho(t) = k * (abs(x(t)) - (k/2));
-end
-
-function rho = cauchy(x, c)    
-    rho = (c^2 / 2) .* log(1 + ((x./c) .^ 2));
-end
-
-function rho = gemanmcclure(x, c)
-    rho = ((x .^ 2) ./ 2) ./ (1 + x .^ 2);
-end
-
-function rho = welsch(x, c)
-    rho = (c^2 / 2) * ( 1 - exp(-(x ./ c) .^2 ));
-end
-
-function rho = tukey(x, c)
-    t = (abs(x) > c);
-
-    rho = (c^2 / 6) * (1 - (1 - (x ./ c) .^ 2 ) .^ 3 );
-    rho(t) = c .^ 2 / 6;
-end