diff options
Diffstat (limited to 'internal/ceres/corrector.cc')
-rw-r--r-- | internal/ceres/corrector.cc | 29 |
1 files changed, 21 insertions, 8 deletions
diff --git a/internal/ceres/corrector.cc b/internal/ceres/corrector.cc index 60269a6..581fc6d 100644 --- a/internal/ceres/corrector.cc +++ b/internal/ceres/corrector.cc @@ -32,14 +32,14 @@ #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]); // If sq_norm = 0.0, the correction becomes trivial, the residual @@ -84,6 +84,14 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { return; } + // We now require that the first derivative of the loss function be + // positive only if the second derivative is positive. This is + // because when the second derivative is non-positive, we do not use + // the second order correction suggested by BANS and instead use a + // simpler first order strategy which does not use a division by the + // gradient of the loss function. + CHECK_GT(rho[1], 0.0); + // Calculate the smaller of the two solutions to the equation // // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. @@ -101,20 +109,25 @@ Corrector::Corrector(double sq_norm, const double rho[3]) { 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) |