Speed up the application of robust loss functions.

Since we added special handling for the case for rho[2] < 0,
the bulk of CorrectJacobian is pointless in the common case.
So add a simple one dimensional loop which rescales the Jacobian.
This speeds up this method immensely.

The robustification of a Jacobian gets speeded up by > 50%.

Change-Id: I97c4e897ccbb5521c053e1fb931c5d0d32f542c7

include/ceres/jet.h

diff --git a/include/ceres/jet.h b/include/ceres/jet.h
index c78364d..55caa05 100644
--- a/include/ceres/jet.h
+++ b/include/ceres/jet.h
@@ -106,8 +106,8 @@
 //   Jet<double, 2> y(1);  // Pick the 1st dual number for y.
 //   Jet<double, 2> z = f(x, y);
 //
-//   LG << "df/dx = " << z.a[0]
-//      << "df/dy = " << z.a[1];
+//   LOG(INFO) << "df/dx = " << z.a[0]
+//             << "df/dy = " << z.a[1];
 //
 // Most users should not use Jet objects directly; a wrapper around Jet objects,
 // which makes computing the derivative, gradient, or jacobian of templated

internal/ceres/corrector.cc

diff --git a/internal/ceres/corrector.cc b/internal/ceres/corrector.cc
index 60269a6..955feb5 100644
--- a/internal/ceres/corrector.cc
+++ b/internal/ceres/corrector.cc
@@ -32,12 +32,13 @@
 
 #include <cstddef>
 #include <cmath>
+#include "ceres/internal/eigen.h"
 #include "glog/logging.h"
 
 namespace ceres {
 namespace internal {
 
-Corrector::Corrector(double sq_norm, const double rho[3]) {
+Corrector::Corrector(const double sq_norm, const double rho[3]) {
   CHECK_GE(sq_norm, 0.0);
   CHECK_GT(rho[1], 0.0);
   sqrt_rho1_ = sqrt(rho[1]);
@@ -101,20 +102,25 @@
   alpha_sq_norm_ = alpha / sq_norm;
 }
 
-void Corrector::CorrectResiduals(int num_rows, double* residuals) {
+void Corrector::CorrectResiduals(const int num_rows, double* residuals) {
   DCHECK(residuals != NULL);
   // Equation 11 in BANS.
-  for (int r = 0; r < num_rows; ++r) {
-    residuals[r] *= residual_scaling_;
-  }
+  VectorRef(residuals, num_rows) *= residual_scaling_;
 }
 
-void Corrector::CorrectJacobian(int num_rows,
-                                int num_cols,
+void Corrector::CorrectJacobian(const int num_rows,
+                                const int num_cols,
                                 double* residuals,
                                 double* jacobian) {
   DCHECK(residuals != NULL);
   DCHECK(jacobian != NULL);
+
+  // The common case (rho[2] <= 0).
+  if (alpha_sq_norm_ == 0.0) {
+    VectorRef(jacobian, num_rows * num_cols) *= sqrt_rho1_;
+   return;
+  }
+
   // Equation 11 in BANS.
   //
   //  J = sqrt(rho) * (J - alpha^2 r * r' J)