From a3fb17c8de2e926e2a9dd103a04ca252d2e5155e Mon Sep 17 00:00:00 2001 From: Markus Moll Date: Wed, 15 Aug 2012 15:37:27 +0200 Subject: [PATCH] Use elliptical trust region in DoglegStrategy. In the Levenberg-Marquardt algorithm, the diagonal of J^T J is used to regularize the problem. This corresponds to an elliptical trust region || D step || <= r, where D = sqrt(diag(J^T J)). This commit adds the same elliptical trust region to the dogleg strategy. The trust region problem becomes min. x^T H x + g^T x s.t. || D x || <= r By substituting y = D x, it becomes min. y^T D^-1 H D^-1 y + g^T D^-1 y s.t. || y || <= r which is the traditional spherical trust region problem. This commit changes the DoglegStrategy so that the Gauss-Newton point, the gradient, and the Cauchy point are scaled correctly (without modifying the Jacobian directly). Then the dogleg step is computed the same way as before, and finally the step is rescaled to obtain x = D^-1 y. Change-Id: Iea25a9113ecba911b746e269bc6e6fe51cb59003 --- internal/ceres/dogleg_strategy.cc | 57 +++++++++++++++++++++++++------ internal/ceres/dogleg_strategy.h | 5 +-- 2 files changed, 50 insertions(+), 12 deletions(-) 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_;