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