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);