Use squared scale in DCS as in g2o

fuse_loss/fuse_plugins.xml

@@ -33,8 +33,8 @@
     <description>
     DCS (Dynamic Covariance Scaling) loss function with scaling parameter 'a', defined as follows for the residual 's':
 
-    rho(s) = s * 2 * a / (a + s)  if s > a   // outlier region
-           = s                    otherwise  // inlier region
+    rho(s) = s * (2 * a / (a + s))^2  if s > a   // outlier region
+           = s                        otherwise  // inlier region
     </description>
   </class>
   <class type="fuse_loss::FairLoss" base_class_type="fuse_core::Loss">

fuse_loss/include/fuse_loss/loss_function.h

@@ -82,21 +82,27 @@
 namespace ceres
 {
 
-// Dynamic Covariance Scaling (DCS), equivalent to the Geman-McClure with the tuning constant 'a', but scaled by 2.0.
+// Dynamic Covariance Scaling (DCS), equivalent to the Geman-McClure with the tuning constant 'a', but scaled by 2.0 and
+// with the scale term squared.
 //
 // The term is computed as:
 //
-//   rho(s) = s * 2 * a / (a + s)    for s <  a
-//   rho(s) = s                      for s >= a
+//   rho(s) = s * (2 * a / (a + s))^2    for s >  a
+//   rho(s) = s                          for s <= a
 //
-// See http://www2.informatik.uni-freiburg.de/~spinello/agarwalICRA13.pdf (p. 3), where 's' is multiplied by the factor:
+// See http://www2.informatik.uni-freiburg.de/~spinello/agarwalICRA13.pdf (p. 3), where the residual 'r' is multiplied
+// by the factor:
 //
 //   min(1, 2 * a / (a + s))
 //
-// The g2o implementation seems to use the squared of that factor:
-// https://github.com/RainerKuemmerle/g2o/blob/master/g2o/core/robust_kernel_impl.cpp#L167
+// where s = r^2.
 //
-// At s = 0: rho = [0, 1, 0].
+// Here, as in the g2o implementation, we use the squared of that factor because we are multiplying the squared residual
+// 's':
+//
+//   https://github.com/RainerKuemmerle/g2o/blob/master/g2o/core/robust_kernel_impl.cpp#L167
+//
+// At s = 0: rho = [0, 4, 16/a].
 class DCSLoss : public ceres::LossFunction
 {
 public:

fuse_loss/src/loss_function.cpp

@@ -45,14 +45,15 @@ void DCSLoss::Evaluate(double s, double rho[3]) const
   if (s > a_)
   {
     // Outlier region
+    const double diff = a_ - s;
     const double sum = a_ + s;
     const double inv = 1.0 / sum;
-    const double scale = a_ * inv;
-    const double scale2 = 2.0 * scale;
+    const double scale = 2.0 * a_ * inv;
+    const double scale_sqr = scale * scale;
 
-    rho[0] = s * scale2;
-    rho[1] = scale * scale2;
-    rho[2] = -2.0 * inv * rho[1];
+    rho[0] = s * scale_sqr;
+    rho[1] = scale_sqr * diff * inv;
+    rho[2] = -inv * (scale_sqr + 3.0 * rho[1]);
   }
   else
   {