Make canned loss functions more robust.

The loss functions that ship with ceres can sometimes
generate a zero first derivative if the residual is too
large.

In such cases Corrector fails with an ugly undebuggable
crash. This CL is the first in a series of fixes to
take care of this.

We clamp the values of rho' from below by
numeric_limits<double>::min().

Also included here is some minor cleanup where the constants
are treated as doubles rather than integers.

Thanks to Pierre Moulon for reporting this problem.

Change-Id: I3aaf375303ecc2659bbf6fb56a812e7dc3a41106

internal/ceres/loss_function.cc

diff --git a/internal/ceres/loss_function.cc b/internal/ceres/loss_function.cc
index b948f28..4ad01e3 100644
--- a/internal/ceres/loss_function.cc
+++ b/internal/ceres/loss_function.cc
@@ -39,8 +39,8 @@
 
 void TrivialLoss::Evaluate(double s, double rho[3]) const {
   rho[0] = s;
-  rho[1] = 1;
-  rho[2] = 0;
+  rho[1] = 1.0;
+  rho[2] = 0.0;
 }
 
 void HuberLoss::Evaluate(double s, double rho[3]) const {
@@ -48,32 +48,32 @@
     // Outlier region.
     // 'r' is always positive.
     const double r = sqrt(s);
-    rho[0] = 2 * a_ * r - b_;
-    rho[1] = a_ / r;
-    rho[2] = - rho[1] / (2 * s);
+    rho[0] = 2.0 * a_ * r - b_;
+    rho[1] = std::max(std::numeric_limits<double>::min(), a_ / r);
+    rho[2] = - rho[1] / (2.0 * s);
   } else {
     // Inlier region.
     rho[0] = s;
-    rho[1] = 1;
-    rho[2] = 0;
+    rho[1] = 1.0;
+    rho[2] = 0.0;
   }
 }
 
 void SoftLOneLoss::Evaluate(double s, double rho[3]) const {
-  const double sum = 1 + s * c_;
+  const double sum = 1.0 + s * c_;
   const double tmp = sqrt(sum);
   // 'sum' and 'tmp' are always positive, assuming that 's' is.
-  rho[0] = 2 * b_ * (tmp - 1);
-  rho[1] = 1 / tmp;
-  rho[2] = - (c_ * rho[1]) / (2 * sum);
+  rho[0] = 2.0 * b_ * (tmp - 1.0);
+  rho[1] = std::max(std::numeric_limits<double>::min(), 1.0 / tmp);
+  rho[2] = - (c_ * rho[1]) / (2.0 * sum);
 }
 
 void CauchyLoss::Evaluate(double s, double rho[3]) const {
-  const double sum = 1 + s * c_;
-  const double inv = 1 / sum;
+  const double sum = 1.0 + s * c_;
+  const double inv = 1.0 / sum;
   // 'sum' and 'inv' are always positive, assuming that 's' is.
   rho[0] = b_ * log(sum);
-  rho[1] = inv;
+  rho[1] = std::max(std::numeric_limits<double>::min(), inv);
   rho[2] = - c_ * (inv * inv);
 }
 
@@ -82,8 +82,8 @@
   const double inv = 1 / sum;
   // 'sum' and 'inv' are always positive.
   rho[0] = a_ * atan2(s, a_);
-  rho[1] = inv;
-  rho[2] = -2 * s * b_ * (inv * inv);
+  rho[1] = std::max(std::numeric_limits<double>::min(), inv);
+  rho[2] = -2.0 * s * b_ * (inv * inv);
 }
 
 TolerantLoss::TolerantLoss(double a, double b)
@@ -108,7 +108,7 @@
   } else {
     const double e_x = exp(x);
     rho[0] = b_ * log(1.0 + e_x) - c_;
-    rho[1] = e_x / (1.0 + e_x);
+    rho[1] = std::max(std::numeric_limits<double>::min(), e_x / (1.0 + e_x));
     rho[2] = 0.5 / (b_ * (1.0 + cosh(x)));
   }
 }