diff options
Diffstat (limited to 'internal/ceres/corrector.cc')
| -rw-r--r-- | internal/ceres/corrector.cc | 44 |
1 files changed, 32 insertions, 12 deletions
diff --git a/internal/ceres/corrector.cc b/internal/ceres/corrector.cc index eff4dff..60269a6 100644 --- a/internal/ceres/corrector.cc +++ b/internal/ceres/corrector.cc @@ -32,7 +32,6 @@ #include <cstddef> #include <cmath> -#include "ceres/internal/eigen.h" #include "glog/logging.h" namespace ceres { @@ -90,7 +89,7 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. // // Start by calculating the discriminant D. - const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1]; // Since both rho[1] and rho[2] are guaranteed to be positive at // this point, we know that D > 1.0. @@ -102,23 +101,44 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { alpha_sq_norm_ = alpha / sq_norm; } -void Corrector::CorrectResiduals(int nrow, double* residuals) { +void Corrector::CorrectResiduals(int num_rows, double* residuals) { DCHECK(residuals != NULL); - VectorRef r_ref(residuals, nrow); // Equation 11 in BANS. - r_ref *= residual_scaling_; + for (int r = 0; r < num_rows; ++r) { + residuals[r] *= residual_scaling_; + } } -void Corrector::CorrectJacobian(int nrow, int ncol, - double* residuals, double* jacobian) { +void Corrector::CorrectJacobian(int num_rows, + int num_cols, + double* residuals, + double* jacobian) { DCHECK(residuals != NULL); DCHECK(jacobian != NULL); - ConstVectorRef r_ref(residuals, nrow); - MatrixRef j_ref(jacobian, nrow, ncol); - // Equation 11 in BANS. - j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * - r_ref * (r_ref.transpose() * j_ref)); + // + // J = sqrt(rho) * (J - alpha^2 r * r' J) + // + // In days gone by this loop used to be a single Eigen expression of + // the form + // + // J = sqrt_rho1_ * (J - alpha_sq_norm_ * r* (r.transpose() * J)); + // + // Which turns out to about 17x slower on bal problems. The reason + // is that Eigen is unable to figure out that this expression can be + // evaluated columnwise and ends up creating a temporary. + for (int c = 0; c < num_cols; ++c) { + double r_transpose_j = 0.0; + for (int r = 0; r < num_rows; ++r) { + r_transpose_j += jacobian[r * num_cols + c] * residuals[r]; + } + + for (int r = 0; r < num_rows; ++r) { + jacobian[r * num_cols + c] = sqrt_rho1_ * + (jacobian[r * num_cols + c] - + alpha_sq_norm_ * residuals[r] * r_transpose_j); + } + } } } // namespace internal |