diff --git a/internal/ceres/dogleg_strategy.cc b/internal/ceres/dogleg_strategy.cc index a62a7bde2..1687bcdd7 100644 --- a/internal/ceres/dogleg_strategy.cc +++ b/internal/ceres/dogleg_strategy.cc @@ -101,20 +101,19 @@ LinearSolver::Summary DoglegStrategy::ComputeStep( } // Vector used to form the diagonal matrix that is used to - // regularize the Gauss-Newton solve. + // regularize the Gauss-Newton solve and that defines the + // elliptical trust region + // + // || D * step || <= radius_ . + // jacobian->SquaredColumnNorm(diagonal_.data()); for (int i = 0; i < n; ++i) { diagonal_[i] = min(max(diagonal_[i], min_diagonal_), max_diagonal_); + diagonal_[i] = std::sqrt(diagonal_[i]); } - gradient_.setZero(); - jacobian->LeftMultiply(residuals, gradient_.data()); - - // alpha * gradient is the Cauchy point. - Vector Jg(jacobian->num_rows()); - Jg.setZero(); - jacobian->RightMultiply(gradient_.data(), Jg.data()); - alpha_ = gradient_.squaredNorm() / Jg.squaredNorm(); + ComputeGradient(jacobian, residuals); + ComputeCauchyPoint(jacobian); LinearSolver::Summary linear_solver_summary = ComputeGaussNewtonStep(jacobian, residuals); @@ -127,6 +126,33 @@ LinearSolver::Summary DoglegStrategy::ComputeStep( return linear_solver_summary; } +// The trust region is assumed to be elliptical with the +// diagonal scaling matrix D defined by sqrt(diagonal_). +// It is implemented by substituting step' = D * step. +// The trust region for step' is spherical. +// The gradient, the Gauss-Newton step, the Cauchy point, +// and all calculations involving the Jacobian have to +// be adjusted accordingly. +void DoglegStrategy::ComputeGradient( + SparseMatrix* jacobian, + const double* residuals) { + gradient_.setZero(); + jacobian->LeftMultiply(residuals, gradient_.data()); + gradient_.array() /= diagonal_.array(); +} + +void DoglegStrategy::ComputeCauchyPoint(SparseMatrix* jacobian) { + // alpha * gradient is the Cauchy point. + Vector Jg(jacobian->num_rows()); + Jg.setZero(); + // The Jacobian is scaled implicitly by computing J * (D^-1 * (D^-1 * g)) + // instead of (J * D^-1) * (D^-1 * g). + Vector scaled_gradient = + (gradient_.array() / diagonal_.array()).matrix(); + jacobian->RightMultiply(scaled_gradient.data(), Jg.data()); + alpha_ = gradient_.squaredNorm() / Jg.squaredNorm(); +} + void DoglegStrategy::ComputeDoglegStep(double* dogleg) { VectorRef dogleg_step(dogleg, gradient_.rows()); @@ -137,6 +163,7 @@ void DoglegStrategy::ComputeDoglegStep(double* dogleg) { if (gauss_newton_norm <= radius_) { dogleg_step = gauss_newton_step_; dogleg_step_norm_ = gauss_newton_norm; + dogleg_step.array() /= diagonal_.array(); VLOG(3) << "GaussNewton step size: " << dogleg_step_norm_ << " radius: " << radius_; return; @@ -148,6 +175,7 @@ void DoglegStrategy::ComputeDoglegStep(double* dogleg) { if (gradient_norm * alpha_ >= radius_) { dogleg_step = (radius_ / gradient_norm) * gradient_; dogleg_step_norm_ = radius_; + dogleg_step.array() /= diagonal_.array(); VLOG(3) << "Cauchy step size: " << dogleg_step_norm_ << " radius: " << radius_; return; @@ -177,6 +205,7 @@ void DoglegStrategy::ComputeDoglegStep(double* dogleg) { : (radius_ * radius_ - a_squared_norm) / (d + c); dogleg_step = (alpha_ * (1.0 - beta)) * gradient_ + beta * gauss_newton_step_; dogleg_step_norm_ = dogleg_step.norm(); + dogleg_step.array() /= diagonal_.array(); VLOG(3) << "Dogleg step size: " << dogleg_step_norm_ << " radius: " << radius_; } @@ -223,7 +252,7 @@ LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( solve_options.q_tolerance = 0.0; solve_options.r_tolerance = 0.0; - lm_diagonal_ = (diagonal_ * mu_).array().sqrt(); + lm_diagonal_ = diagonal_ * std::sqrt(mu_); solve_options.D = lm_diagonal_.data(); InvalidateArray(n, gauss_newton_step_.data()); @@ -242,6 +271,14 @@ LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( break; } + // The scaled Gauss-Newton step is D * GN: + // + // - (D^-1 J^T J D^-1)^-1 (D^-1 g) + // = - D (J^T J)^-1 D D^-1 g + // = D -(J^T J)^-1 g + // + gauss_newton_step_.array() *= diagonal_.array(); + return linear_solver_summary; } diff --git a/internal/ceres/dogleg_strategy.h b/internal/ceres/dogleg_strategy.h index 17eb0759f..150565509 100644 --- a/internal/ceres/dogleg_strategy.h +++ b/internal/ceres/dogleg_strategy.h @@ -65,9 +65,10 @@ public: virtual double Radius() const; private: - void ComputeCauchyStep(); LinearSolver::Summary ComputeGaussNewtonStep(SparseMatrix* jacobian, const double* residuals); + void ComputeCauchyPoint(SparseMatrix* jacobian); + void ComputeGradient(SparseMatrix* jacobian, const double* residuals); void ComputeDoglegStep(double* step); LinearSolver* linear_solver_; @@ -92,7 +93,7 @@ public: const double increase_threshold_; const double decrease_threshold_; - Vector diagonal_; + Vector diagonal_; // sqrt(diag(J^T J)) Vector lm_diagonal_; Vector gradient_;