| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2015 Google Inc. All rights reserved. |
| // http://ceres-solver.org/ |
| // |
| // Redistribution and use in source and binary forms, with or without |
| // modification, are permitted provided that the following conditions are met: |
| // |
| // * Redistributions of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // * Redistributions in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // * Neither the name of Google Inc. nor the names of its contributors may be |
| // used to endorse or promote products derived from this software without |
| // specific prior written permission. |
| // |
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // POSSIBILITY OF SUCH DAMAGE. |
| // |
| // Author: sameeragarwal@google.com (Sameer Agarwal) |
| |
| #include "ceres/dogleg_strategy.h" |
| |
| #include <cmath> |
| #include "Eigen/Dense" |
| #include "ceres/array_utils.h" |
| #include "ceres/internal/eigen.h" |
| #include "ceres/linear_least_squares_problems.h" |
| #include "ceres/linear_solver.h" |
| #include "ceres/polynomial.h" |
| #include "ceres/sparse_matrix.h" |
| #include "ceres/trust_region_strategy.h" |
| #include "ceres/types.h" |
| #include "glog/logging.h" |
| |
| namespace ceres { |
| namespace internal { |
| namespace { |
| const double kMaxMu = 1.0; |
| const double kMinMu = 1e-8; |
| } |
| |
| DoglegStrategy::DoglegStrategy(const TrustRegionStrategy::Options& options) |
| : linear_solver_(options.linear_solver), |
| radius_(options.initial_radius), |
| max_radius_(options.max_radius), |
| min_diagonal_(options.min_lm_diagonal), |
| max_diagonal_(options.max_lm_diagonal), |
| mu_(kMinMu), |
| min_mu_(kMinMu), |
| max_mu_(kMaxMu), |
| mu_increase_factor_(10.0), |
| increase_threshold_(0.75), |
| decrease_threshold_(0.25), |
| dogleg_step_norm_(0.0), |
| reuse_(false), |
| dogleg_type_(options.dogleg_type) { |
| CHECK_NOTNULL(linear_solver_); |
| CHECK_GT(min_diagonal_, 0.0); |
| CHECK_LE(min_diagonal_, max_diagonal_); |
| CHECK_GT(max_radius_, 0.0); |
| } |
| |
| // If the reuse_ flag is not set, then the Cauchy point (scaled |
| // gradient) and the new Gauss-Newton step are computed from |
| // scratch. The Dogleg step is then computed as interpolation of these |
| // two vectors. |
| TrustRegionStrategy::Summary DoglegStrategy::ComputeStep( |
| const TrustRegionStrategy::PerSolveOptions& per_solve_options, |
| SparseMatrix* jacobian, |
| const double* residuals, |
| double* step) { |
| CHECK_NOTNULL(jacobian); |
| CHECK_NOTNULL(residuals); |
| CHECK_NOTNULL(step); |
| |
| const int n = jacobian->num_cols(); |
| if (reuse_) { |
| // Gauss-Newton and gradient vectors are always available, only a |
| // new interpolant need to be computed. For the subspace case, |
| // the subspace and the two-dimensional model are also still valid. |
| switch (dogleg_type_) { |
| case TRADITIONAL_DOGLEG: |
| ComputeTraditionalDoglegStep(step); |
| break; |
| |
| case SUBSPACE_DOGLEG: |
| ComputeSubspaceDoglegStep(step); |
| break; |
| } |
| TrustRegionStrategy::Summary summary; |
| summary.num_iterations = 0; |
| summary.termination_type = LINEAR_SOLVER_SUCCESS; |
| return summary; |
| } |
| |
| reuse_ = true; |
| // Check that we have the storage needed to hold the various |
| // temporary vectors. |
| if (diagonal_.rows() != n) { |
| diagonal_.resize(n, 1); |
| gradient_.resize(n, 1); |
| gauss_newton_step_.resize(n, 1); |
| } |
| |
| // Vector used to form the diagonal matrix that is used to |
| // 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] = std::min(std::max(diagonal_[i], min_diagonal_), |
| max_diagonal_); |
| } |
| diagonal_ = diagonal_.array().sqrt(); |
| |
| ComputeGradient(jacobian, residuals); |
| ComputeCauchyPoint(jacobian); |
| |
| LinearSolver::Summary linear_solver_summary = |
| ComputeGaussNewtonStep(per_solve_options, jacobian, residuals); |
| |
| TrustRegionStrategy::Summary summary; |
| summary.residual_norm = linear_solver_summary.residual_norm; |
| summary.num_iterations = linear_solver_summary.num_iterations; |
| summary.termination_type = linear_solver_summary.termination_type; |
| |
| if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) { |
| return summary; |
| } |
| |
| if (linear_solver_summary.termination_type != LINEAR_SOLVER_FAILURE) { |
| switch (dogleg_type_) { |
| // Interpolate the Cauchy point and the Gauss-Newton step. |
| case TRADITIONAL_DOGLEG: |
| ComputeTraditionalDoglegStep(step); |
| break; |
| |
| // Find the minimum in the subspace defined by the |
| // Cauchy point and the (Gauss-)Newton step. |
| case SUBSPACE_DOGLEG: |
| if (!ComputeSubspaceModel(jacobian)) { |
| summary.termination_type = LINEAR_SOLVER_FAILURE; |
| break; |
| } |
| ComputeSubspaceDoglegStep(step); |
| break; |
| } |
| } |
| |
| return 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(); |
| } |
| |
| // The Cauchy point is the global minimizer of the quadratic model |
| // along the one-dimensional subspace spanned by the gradient. |
| 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(); |
| } |
| |
| // The dogleg step is defined as the intersection of the trust region |
| // boundary with the piecewise linear path from the origin to the Cauchy |
| // point and then from there to the Gauss-Newton point (global minimizer |
| // of the model function). The Gauss-Newton point is taken if it lies |
| // within the trust region. |
| void DoglegStrategy::ComputeTraditionalDoglegStep(double* dogleg) { |
| VectorRef dogleg_step(dogleg, gradient_.rows()); |
| |
| // Case 1. The Gauss-Newton step lies inside the trust region, and |
| // is therefore the optimal solution to the trust-region problem. |
| const double gradient_norm = gradient_.norm(); |
| const double gauss_newton_norm = gauss_newton_step_.norm(); |
| 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; |
| } |
| |
| // Case 2. The Cauchy point and the Gauss-Newton steps lie outside |
| // the trust region. Rescale the Cauchy point to the trust region |
| // and return. |
| 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; |
| } |
| |
| // Case 3. The Cauchy point is inside the trust region and the |
| // Gauss-Newton step is outside. Compute the line joining the two |
| // points and the point on it which intersects the trust region |
| // boundary. |
| |
| // a = alpha * -gradient |
| // b = gauss_newton_step |
| const double b_dot_a = -alpha_ * gradient_.dot(gauss_newton_step_); |
| const double a_squared_norm = pow(alpha_ * gradient_norm, 2.0); |
| const double b_minus_a_squared_norm = |
| a_squared_norm - 2 * b_dot_a + pow(gauss_newton_norm, 2); |
| |
| // c = a' (b - a) |
| // = alpha * -gradient' gauss_newton_step - alpha^2 |gradient|^2 |
| const double c = b_dot_a - a_squared_norm; |
| const double d = sqrt(c * c + b_minus_a_squared_norm * |
| (pow(radius_, 2.0) - a_squared_norm)); |
| |
| double beta = |
| (c <= 0) |
| ? (d - c) / b_minus_a_squared_norm |
| : (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_; |
| } |
| |
| // The subspace method finds the minimum of the two-dimensional problem |
| // |
| // min. 1/2 x' B' H B x + g' B x |
| // s.t. || B x ||^2 <= r^2 |
| // |
| // where r is the trust region radius and B is the matrix with unit columns |
| // spanning the subspace defined by the steepest descent and Newton direction. |
| // This subspace by definition includes the Gauss-Newton point, which is |
| // therefore taken if it lies within the trust region. |
| void DoglegStrategy::ComputeSubspaceDoglegStep(double* dogleg) { |
| VectorRef dogleg_step(dogleg, gradient_.rows()); |
| |
| // The Gauss-Newton point is inside the trust region if |GN| <= radius_. |
| // This test is valid even though radius_ is a length in the two-dimensional |
| // subspace while gauss_newton_step_ is expressed in the (scaled) |
| // higher dimensional original space. This is because |
| // |
| // 1. gauss_newton_step_ by definition lies in the subspace, and |
| // 2. the subspace basis is orthonormal. |
| // |
| // As a consequence, the norm of the gauss_newton_step_ in the subspace is |
| // the same as its norm in the original space. |
| const double gauss_newton_norm = gauss_newton_step_.norm(); |
| 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; |
| } |
| |
| // The optimum lies on the boundary of the trust region. The above problem |
| // therefore becomes |
| // |
| // min. 1/2 x^T B^T H B x + g^T B x |
| // s.t. || B x ||^2 = r^2 |
| // |
| // Notice the equality in the constraint. |
| // |
| // This can be solved by forming the Lagrangian, solving for x(y), where |
| // y is the Lagrange multiplier, using the gradient of the objective, and |
| // putting x(y) back into the constraint. This results in a fourth order |
| // polynomial in y, which can be solved using e.g. the companion matrix. |
| // See the description of MakePolynomialForBoundaryConstrainedProblem for |
| // details. The result is up to four real roots y*, not all of which |
| // correspond to feasible points. The feasible points x(y*) have to be |
| // tested for optimality. |
| |
| if (subspace_is_one_dimensional_) { |
| // The subspace is one-dimensional, so both the gradient and |
| // the Gauss-Newton step point towards the same direction. |
| // In this case, we move along the gradient until we reach the trust |
| // region boundary. |
| dogleg_step = -(radius_ / gradient_.norm()) * gradient_; |
| dogleg_step_norm_ = radius_; |
| dogleg_step.array() /= diagonal_.array(); |
| VLOG(3) << "Dogleg subspace step size (1D): " << dogleg_step_norm_ |
| << " radius: " << radius_; |
| return; |
| } |
| |
| Vector2d minimum(0.0, 0.0); |
| if (!FindMinimumOnTrustRegionBoundary(&minimum)) { |
| // For the positive semi-definite case, a traditional dogleg step |
| // is taken in this case. |
| LOG(WARNING) << "Failed to compute polynomial roots. " |
| << "Taking traditional dogleg step instead."; |
| ComputeTraditionalDoglegStep(dogleg); |
| return; |
| } |
| |
| // Test first order optimality at the minimum. |
| // The first order KKT conditions state that the minimum x* |
| // has to satisfy either || x* ||^2 < r^2 (i.e. has to lie within |
| // the trust region), or |
| // |
| // (B x* + g) + y x* = 0 |
| // |
| // for some positive scalar y. |
| // Here, as it is already known that the minimum lies on the boundary, the |
| // latter condition is tested. To allow for small imprecisions, we test if |
| // the angle between (B x* + g) and -x* is smaller than acos(0.99). |
| // The exact value of the cosine is arbitrary but should be close to 1. |
| // |
| // This condition should not be violated. If it is, the minimum was not |
| // correctly determined. |
| const double kCosineThreshold = 0.99; |
| const Vector2d grad_minimum = subspace_B_ * minimum + subspace_g_; |
| const double cosine_angle = -minimum.dot(grad_minimum) / |
| (minimum.norm() * grad_minimum.norm()); |
| if (cosine_angle < kCosineThreshold) { |
| LOG(WARNING) << "First order optimality seems to be violated " |
| << "in the subspace method!\n" |
| << "Cosine of angle between x and B x + g is " |
| << cosine_angle << ".\n" |
| << "Taking a regular dogleg step instead.\n" |
| << "Please consider filing a bug report if this " |
| << "happens frequently or consistently.\n"; |
| ComputeTraditionalDoglegStep(dogleg); |
| return; |
| } |
| |
| // Create the full step from the optimal 2d solution. |
| dogleg_step = subspace_basis_ * minimum; |
| dogleg_step_norm_ = radius_; |
| dogleg_step.array() /= diagonal_.array(); |
| VLOG(3) << "Dogleg subspace step size: " << dogleg_step_norm_ |
| << " radius: " << radius_; |
| } |
| |
| // Build the polynomial that defines the optimal Lagrange multipliers. |
| // Let the Lagrangian be |
| // |
| // L(x, y) = 0.5 x^T B x + x^T g + y (0.5 x^T x - 0.5 r^2). (1) |
| // |
| // Stationary points of the Lagrangian are given by |
| // |
| // 0 = d L(x, y) / dx = Bx + g + y x (2) |
| // 0 = d L(x, y) / dy = 0.5 x^T x - 0.5 r^2 (3) |
| // |
| // For any given y, we can solve (2) for x as |
| // |
| // x(y) = -(B + y I)^-1 g . (4) |
| // |
| // As B + y I is 2x2, we form the inverse explicitly: |
| // |
| // (B + y I)^-1 = (1 / det(B + y I)) adj(B + y I) (5) |
| // |
| // where adj() denotes adjugation. This should be safe, as B is positive |
| // semi-definite and y is necessarily positive, so (B + y I) is indeed |
| // invertible. |
| // Plugging (5) into (4) and the result into (3), then dividing by 0.5 we |
| // obtain |
| // |
| // 0 = (1 / det(B + y I))^2 g^T adj(B + y I)^T adj(B + y I) g - r^2 |
| // (6) |
| // |
| // or |
| // |
| // det(B + y I)^2 r^2 = g^T adj(B + y I)^T adj(B + y I) g (7a) |
| // = g^T adj(B)^T adj(B) g |
| // + 2 y g^T adj(B)^T g + y^2 g^T g (7b) |
| // |
| // as |
| // |
| // adj(B + y I) = adj(B) + y I = adj(B)^T + y I . (8) |
| // |
| // The left hand side can be expressed explicitly using |
| // |
| // det(B + y I) = det(B) + y tr(B) + y^2 . (9) |
| // |
| // So (7) is a polynomial in y of degree four. |
| // Bringing everything back to the left hand side, the coefficients can |
| // be read off as |
| // |
| // y^4 r^2 |
| // + y^3 2 r^2 tr(B) |
| // + y^2 (r^2 tr(B)^2 + 2 r^2 det(B) - g^T g) |
| // + y^1 (2 r^2 det(B) tr(B) - 2 g^T adj(B)^T g) |
| // + y^0 (r^2 det(B)^2 - g^T adj(B)^T adj(B) g) |
| // |
| Vector DoglegStrategy::MakePolynomialForBoundaryConstrainedProblem() const { |
| const double detB = subspace_B_.determinant(); |
| const double trB = subspace_B_.trace(); |
| const double r2 = radius_ * radius_; |
| Matrix2d B_adj; |
| B_adj << subspace_B_(1, 1) , -subspace_B_(0, 1), |
| -subspace_B_(1, 0) , subspace_B_(0, 0); |
| |
| Vector polynomial(5); |
| polynomial(0) = r2; |
| polynomial(1) = 2.0 * r2 * trB; |
| polynomial(2) = r2 * (trB * trB + 2.0 * detB) - subspace_g_.squaredNorm(); |
| polynomial(3) = -2.0 * (subspace_g_.transpose() * B_adj * subspace_g_ |
| - r2 * detB * trB); |
| polynomial(4) = r2 * detB * detB - (B_adj * subspace_g_).squaredNorm(); |
| |
| return polynomial; |
| } |
| |
| // Given a Lagrange multiplier y that corresponds to a stationary point |
| // of the Lagrangian L(x, y), compute the corresponding x from the |
| // equation |
| // |
| // 0 = d L(x, y) / dx |
| // = B * x + g + y * x |
| // = (B + y * I) * x + g |
| // |
| DoglegStrategy::Vector2d DoglegStrategy::ComputeSubspaceStepFromRoot( |
| double y) const { |
| const Matrix2d B_i = subspace_B_ + y * Matrix2d::Identity(); |
| return -B_i.partialPivLu().solve(subspace_g_); |
| } |
| |
| // This function evaluates the quadratic model at a point x in the |
| // subspace spanned by subspace_basis_. |
| double DoglegStrategy::EvaluateSubspaceModel(const Vector2d& x) const { |
| return 0.5 * x.dot(subspace_B_ * x) + subspace_g_.dot(x); |
| } |
| |
| // This function attempts to solve the boundary-constrained subspace problem |
| // |
| // min. 1/2 x^T B^T H B x + g^T B x |
| // s.t. || B x ||^2 = r^2 |
| // |
| // where B is an orthonormal subspace basis and r is the trust-region radius. |
| // |
| // This is done by finding the roots of a fourth degree polynomial. If the |
| // root finding fails, the function returns false and minimum will be set |
| // to (0, 0). If it succeeds, true is returned. |
| // |
| // In the failure case, another step should be taken, such as the traditional |
| // dogleg step. |
| bool DoglegStrategy::FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const { |
| CHECK_NOTNULL(minimum); |
| |
| // Return (0, 0) in all error cases. |
| minimum->setZero(); |
| |
| // Create the fourth-degree polynomial that is a necessary condition for |
| // optimality. |
| const Vector polynomial = MakePolynomialForBoundaryConstrainedProblem(); |
| |
| // Find the real parts y_i of its roots (not only the real roots). |
| Vector roots_real; |
| if (!FindPolynomialRoots(polynomial, &roots_real, NULL)) { |
| // Failed to find the roots of the polynomial, i.e. the candidate |
| // solutions of the constrained problem. Report this back to the caller. |
| return false; |
| } |
| |
| // For each root y, compute B x(y) and check for feasibility. |
| // Notice that there should always be four roots, as the leading term of |
| // the polynomial is r^2 and therefore non-zero. However, as some roots |
| // may be complex, the real parts are not necessarily unique. |
| double minimum_value = std::numeric_limits<double>::max(); |
| bool valid_root_found = false; |
| for (int i = 0; i < roots_real.size(); ++i) { |
| const Vector2d x_i = ComputeSubspaceStepFromRoot(roots_real(i)); |
| |
| // Not all roots correspond to points on the trust region boundary. |
| // There are at most four candidate solutions. As we are interested |
| // in the minimum, it is safe to consider all of them after projecting |
| // them onto the trust region boundary. |
| if (x_i.norm() > 0) { |
| const double f_i = EvaluateSubspaceModel((radius_ / x_i.norm()) * x_i); |
| valid_root_found = true; |
| if (f_i < minimum_value) { |
| minimum_value = f_i; |
| *minimum = x_i; |
| } |
| } |
| } |
| |
| return valid_root_found; |
| } |
| |
| LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep( |
| const PerSolveOptions& per_solve_options, |
| SparseMatrix* jacobian, |
| const double* residuals) { |
| const int n = jacobian->num_cols(); |
| LinearSolver::Summary linear_solver_summary; |
| linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE; |
| |
| // The Jacobian matrix is often quite poorly conditioned. Thus it is |
| // necessary to add a diagonal matrix at the bottom to prevent the |
| // linear solver from failing. |
| // |
| // We do this by computing the same diagonal matrix as the one used |
| // by Levenberg-Marquardt (other choices are possible), and scaling |
| // it by a small constant (independent of the trust region radius). |
| // |
| // If the solve fails, the multiplier to the diagonal is increased |
| // up to max_mu_ by a factor of mu_increase_factor_ every time. If |
| // the linear solver is still not successful, the strategy returns |
| // with LINEAR_SOLVER_FAILURE. |
| // |
| // Next time when a new Gauss-Newton step is requested, the |
| // multiplier starts out from the last successful solve. |
| // |
| // When a step is declared successful, the multiplier is decreased |
| // by half of mu_increase_factor_. |
| |
| while (mu_ < max_mu_) { |
| // Dogleg, as far as I (sameeragarwal) understand it, requires a |
| // reasonably good estimate of the Gauss-Newton step. This means |
| // that we need to solve the normal equations more or less |
| // exactly. This is reflected in the values of the tolerances set |
| // below. |
| // |
| // For now, this strategy should only be used with exact |
| // factorization based solvers, for which these tolerances are |
| // automatically satisfied. |
| // |
| // The right way to combine inexact solves with trust region |
| // methods is to use Stiehaug's method. |
| LinearSolver::PerSolveOptions solve_options; |
| solve_options.q_tolerance = 0.0; |
| solve_options.r_tolerance = 0.0; |
| |
| lm_diagonal_ = diagonal_ * std::sqrt(mu_); |
| solve_options.D = lm_diagonal_.data(); |
| |
| // As in the LevenbergMarquardtStrategy, solve Jy = r instead |
| // of Jx = -r and later set x = -y to avoid having to modify |
| // either jacobian or residuals. |
| InvalidateArray(n, gauss_newton_step_.data()); |
| linear_solver_summary = linear_solver_->Solve(jacobian, |
| residuals, |
| solve_options, |
| gauss_newton_step_.data()); |
| |
| if (per_solve_options.dump_format_type == CONSOLE || |
| (per_solve_options.dump_format_type != CONSOLE && |
| !per_solve_options.dump_filename_base.empty())) { |
| if (!DumpLinearLeastSquaresProblem(per_solve_options.dump_filename_base, |
| per_solve_options.dump_format_type, |
| jacobian, |
| solve_options.D, |
| residuals, |
| gauss_newton_step_.data(), |
| 0)) { |
| LOG(ERROR) << "Unable to dump trust region problem." |
| << " Filename base: " |
| << per_solve_options.dump_filename_base; |
| } |
| } |
| |
| if (linear_solver_summary.termination_type == LINEAR_SOLVER_FATAL_ERROR) { |
| return linear_solver_summary; |
| } |
| |
| if (linear_solver_summary.termination_type == LINEAR_SOLVER_FAILURE || |
| !IsArrayValid(n, gauss_newton_step_.data())) { |
| mu_ *= mu_increase_factor_; |
| VLOG(2) << "Increasing mu " << mu_; |
| linear_solver_summary.termination_type = LINEAR_SOLVER_FAILURE; |
| continue; |
| } |
| break; |
| } |
| |
| if (linear_solver_summary.termination_type != LINEAR_SOLVER_FAILURE) { |
| // 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; |
| } |
| |
| void DoglegStrategy::StepAccepted(double step_quality) { |
| CHECK_GT(step_quality, 0.0); |
| |
| if (step_quality < decrease_threshold_) { |
| radius_ *= 0.5; |
| } |
| |
| if (step_quality > increase_threshold_) { |
| radius_ = std::max(radius_, 3.0 * dogleg_step_norm_); |
| } |
| |
| // Reduce the regularization multiplier, in the hope that whatever |
| // was causing the rank deficiency has gone away and we can return |
| // to doing a pure Gauss-Newton solve. |
| mu_ = std::max(min_mu_, 2.0 * mu_ / mu_increase_factor_); |
| reuse_ = false; |
| } |
| |
| void DoglegStrategy::StepRejected(double step_quality) { |
| radius_ *= 0.5; |
| reuse_ = true; |
| } |
| |
| void DoglegStrategy::StepIsInvalid() { |
| mu_ *= mu_increase_factor_; |
| reuse_ = false; |
| } |
| |
| double DoglegStrategy::Radius() const { |
| return radius_; |
| } |
| |
| bool DoglegStrategy::ComputeSubspaceModel(SparseMatrix* jacobian) { |
| // Compute an orthogonal basis for the subspace using QR decomposition. |
| Matrix basis_vectors(jacobian->num_cols(), 2); |
| basis_vectors.col(0) = gradient_; |
| basis_vectors.col(1) = gauss_newton_step_; |
| Eigen::ColPivHouseholderQR<Matrix> basis_qr(basis_vectors); |
| |
| switch (basis_qr.rank()) { |
| case 0: |
| // This should never happen, as it implies that both the gradient |
| // and the Gauss-Newton step are zero. In this case, the minimizer should |
| // have stopped due to the gradient being too small. |
| LOG(ERROR) << "Rank of subspace basis is 0. " |
| << "This means that the gradient at the current iterate is " |
| << "zero but the optimization has not been terminated. " |
| << "You may have found a bug in Ceres."; |
| return false; |
| |
| case 1: |
| // Gradient and Gauss-Newton step coincide, so we lie on one of the |
| // major axes of the quadratic problem. In this case, we simply move |
| // along the gradient until we reach the trust region boundary. |
| subspace_is_one_dimensional_ = true; |
| return true; |
| |
| case 2: |
| subspace_is_one_dimensional_ = false; |
| break; |
| |
| default: |
| LOG(ERROR) << "Rank of the subspace basis matrix is reported to be " |
| << "greater than 2. As the matrix contains only two " |
| << "columns this cannot be true and is indicative of " |
| << "a bug."; |
| return false; |
| } |
| |
| // The subspace is two-dimensional, so compute the subspace model. |
| // Given the basis U, this is |
| // |
| // subspace_g_ = g_scaled^T U |
| // |
| // and |
| // |
| // subspace_B_ = U^T (J_scaled^T J_scaled) U |
| // |
| // As J_scaled = J * D^-1, the latter becomes |
| // |
| // subspace_B_ = ((U^T D^-1) J^T) (J (D^-1 U)) |
| // = (J (D^-1 U))^T (J (D^-1 U)) |
| |
| subspace_basis_ = |
| basis_qr.householderQ() * Matrix::Identity(jacobian->num_cols(), 2); |
| |
| subspace_g_ = subspace_basis_.transpose() * gradient_; |
| |
| Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::RowMajor> |
| Jb(2, jacobian->num_rows()); |
| Jb.setZero(); |
| |
| Vector tmp; |
| tmp = (subspace_basis_.col(0).array() / diagonal_.array()).matrix(); |
| jacobian->RightMultiply(tmp.data(), Jb.row(0).data()); |
| tmp = (subspace_basis_.col(1).array() / diagonal_.array()).matrix(); |
| jacobian->RightMultiply(tmp.data(), Jb.row(1).data()); |
| |
| subspace_B_ = Jb * Jb.transpose(); |
| |
| return true; |
| } |
| |
| } // namespace internal |
| } // namespace ceres |