Add the two-dimensional subspace search to DoglegStrategy

Change-Id: I5163744c100cdf07dd93343d0734ffe0e80364f3

examples/bundle_adjuster.cc

@@ -86,6 +86,7 @@ DEFINE_string(sparse_linear_algebra_library, "suitesparse",
               "Options are: suitesparse and cxsparse");
 
 DEFINE_string(ordering_type, "schur", "Options are: schur, user, natural");
+DEFINE_string(dogleg_type, "traditional", "Options are: traditional, subspace");
 DEFINE_bool(use_block_amd, true, "Use a block oriented fill reducing "
             "ordering.");
 
@@ -255,6 +256,14 @@ void SetMinimizerOptions(Solver::Options* options) {
     LOG(FATAL) << "Unknown trust region strategy: "
                << FLAGS_trust_region_strategy;
   }
+  if (FLAGS_dogleg_type == "traditional") {
+    options->dogleg_type = TRADITIONAL_DOGLEG;
+  } else if (FLAGS_dogleg_type == "subspace") {
+    options->dogleg_type = SUBSPACE_DOGLEG;
+  } else {
+    LOG(FATAL) << "Unknown dogleg type: "
+               << FLAGS_dogleg_type;
+  }
 }
 
 void SetSolverOptionsFromFlags(BALProblem* bal_problem,

include/ceres/solver.h

@@ -58,6 +58,7 @@ class Solver {
     // Default constructor that sets up a generic sparse problem.
     Options() {
       trust_region_strategy_type = LEVENBERG_MARQUARDT;
+      dogleg_type = TRADITIONAL_DOGLEG;
       use_nonmonotonic_steps = false;
       max_consecutive_nonmonotonic_steps = 5;
       max_num_iterations = 50;
@@ -121,6 +122,9 @@ class Solver {
 
     TrustRegionStrategyType trust_region_strategy_type;
 
+    // Type of dogleg strategy to use.
+    DoglegType dogleg_type;
+
     // The classical trust region methods are descent methods, in that
     // they only accept a point if it strictly reduces the value of
     // the objective function.

include/ceres/types.h

@@ -187,6 +187,24 @@ enum TrustRegionStrategyType {
   DOGLEG
 };
 
+// Ceres supports two different dogleg strategies.
+// The "traditional" dogleg method by Powell and the
+// "subspace" method described in
+// R. H. Byrd, R. B. Schnabel, and G. A. Shultz,
+// "Approximate solution of the trust region problem by minimization
+//  over two-dimensional subspaces", Mathematical Programming,
+// 40 (1988), pp. 247--263
+enum DoglegType {
+  // The traditional approach constructs a dogleg path
+  // consisting of two line segments and finds the furthest
+  // point on that path that is still inside the trust region.
+  TRADITIONAL_DOGLEG,
+
+  // The subspace approach finds the exact minimum of the model
+  // constrained to the subspace spanned by the dogleg path.
+  SUBSPACE_DOGLEG
+};
+
 enum SolverTerminationType {
   // The minimizer did not run at all; usually due to errors in the user's
   // Problem or the solver options.

internal/ceres/dogleg_strategy.cc

@@ -31,10 +31,11 @@
 #include "ceres/dogleg_strategy.h"
 
 #include <cmath>
-#include "Eigen/Core"
+#include "Eigen/Dense"
 #include "ceres/array_utils.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/linear_solver.h"
+#include "ceres/polynomial_solver.h"
 #include "ceres/sparse_matrix.h"
 #include "ceres/trust_region_strategy.h"
 #include "ceres/types.h"
@@ -60,7 +61,8 @@ DoglegStrategy::DoglegStrategy(const TrustRegionStrategy::Options& options)
       increase_threshold_(0.75),
       decrease_threshold_(0.25),
       dogleg_step_norm_(0.0),
-      reuse_(false) {
+      reuse_(false),
+      dogleg_type_(options.dogleg_type) {
   CHECK_NOTNULL(linear_solver_);
   CHECK_GT(min_diagonal_, 0.0);
   CHECK_LT(min_diagonal_, max_diagonal_);
@@ -83,8 +85,17 @@ TrustRegionStrategy::Summary DoglegStrategy::ComputeStep(
   const int n = jacobian->num_cols();
   if (reuse_) {
     // Gauss-Newton and gradient vectors are always available, only a
-    // new interpolant need to be computed.
-    ComputeDoglegStep(step);
+    // 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 = TOLERANCE;
@@ -109,8 +120,8 @@ TrustRegionStrategy::Summary DoglegStrategy::ComputeStep(
   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]);
   }
+  diagonal_ = diagonal_.array().sqrt();
 
   ComputeGradient(jacobian, residuals);
   ComputeCauchyPoint(jacobian);
@@ -118,15 +129,30 @@ TrustRegionStrategy::Summary DoglegStrategy::ComputeStep(
   LinearSolver::Summary linear_solver_summary =
       ComputeGaussNewtonStep(jacobian, residuals);
 
-  // Interpolate the Cauchy point and the Gauss-Newton step.
-  if (linear_solver_summary.termination_type != FAILURE) {
-    ComputeDoglegStep(step);
-  }
-
   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 != 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 = FAILURE;
+          break;
+        }
+        ComputeSubspaceDoglegStep(step);
+        break;
+    }
+  }
+
   return summary;
 }
 
@@ -145,6 +171,8 @@ void DoglegStrategy::ComputeGradient(
   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());
@@ -157,7 +185,12 @@ void DoglegStrategy::ComputeCauchyPoint(SparseMatrix* jacobian) {
   alpha_ = gradient_.squaredNorm() / Jg.squaredNorm();
 }
 
-void DoglegStrategy::ComputeDoglegStep(double* dogleg) {
+// 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
@@ -207,13 +240,272 @@ void DoglegStrategy::ComputeDoglegStep(double* dogleg) {
       (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 = (-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(
     SparseMatrix* jacobian,
     const double* residuals) {
@@ -239,6 +531,7 @@ LinearSolver::Summary DoglegStrategy::ComputeGaussNewtonStep(
   //
   // 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
@@ -278,19 +571,22 @@ 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();
+  if (linear_solver_summary.termination_type != 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;
   }
@@ -320,5 +616,76 @@ 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

internal/ceres/dogleg_strategy.h

@@ -47,6 +47,11 @@ namespace internal {
 // Gauss-Newton step, we compute a regularized version of it. This is
 // because the Jacobian is often rank-deficient and in such cases
 // using a direct solver leads to numerical failure.
+//
+// If SUBSPACE is passed as the type argument to the constructor, the
+// DoglegStrategy follows the approach by Shultz, Schnabel, Byrd.
+// This finds the exact optimum over the two-dimensional subspace
+// spanned by the two Dogleg vectors.
 class DoglegStrategy : public TrustRegionStrategy {
 public:
   DoglegStrategy(const TrustRegionStrategy::Options& options);
@@ -64,11 +69,21 @@ public:
   virtual double Radius() const;
 
  private:
+  typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Vector2d;
+  typedef Eigen::Matrix<double, 2, 2, Eigen::DontAlign> Matrix2d;
+
   LinearSolver::Summary ComputeGaussNewtonStep(SparseMatrix* jacobian,
                                                const double* residuals);
   void ComputeCauchyPoint(SparseMatrix* jacobian);
   void ComputeGradient(SparseMatrix* jacobian, const double* residuals);
-  void ComputeDoglegStep(double* step);
+  void ComputeTraditionalDoglegStep(double* step);
+  bool ComputeSubspaceModel(SparseMatrix* jacobian);
+  void ComputeSubspaceDoglegStep(double* step);
+
+  bool FindMinimumOnTrustRegionBoundary(Vector2d* minimum) const;
+  Vector MakePolynomialForBoundaryConstrainedProblem() const;
+  Vector2d ComputeSubspaceStepFromRoot(double lambda) const;
+  double EvaluateSubspaceModel(const Vector2d& x) const;
 
   LinearSolver* linear_solver_;
   double radius_;
@@ -122,6 +137,17 @@ public:
   // increased and a new solve should be done when ComputeStep is
   // called again, thus reuse is set to false.
   bool reuse_;
+
+  // The dogleg type determines how the minimum of the local
+  // quadratic model is found.
+  DoglegType dogleg_type_;
+
+  // If the type is SUBSPACE_DOGLEG, the two-dimensional
+  // model 1/2 x^T B x + g^T x has to be computed and stored.
+  bool subspace_is_one_dimensional_;
+  Matrix subspace_basis_;
+  Vector2d subspace_g_;
+  Matrix2d subspace_B_;
 };
 
 }  // namespace internal

internal/ceres/solver_impl.cc

@@ -183,6 +183,7 @@ void SolverImpl::Minimize(const Solver::Options& options,
   trust_region_strategy_options.lm_max_diagonal = options.lm_max_diagonal;
   trust_region_strategy_options.trust_region_strategy_type =
       options.trust_region_strategy_type;
+  trust_region_strategy_options.dogleg_type = options.dogleg_type;
   scoped_ptr<TrustRegionStrategy> strategy(
       TrustRegionStrategy::Create(trust_region_strategy_options));
   minimizer_options.trust_region_strategy = strategy.get();

internal/ceres/trust_region_strategy.h

@@ -59,7 +59,8 @@ public:
           initial_radius(1e4),
           max_radius(1e32),
           lm_min_diagonal(1e-6),
-          lm_max_diagonal(1e32) {
+          lm_max_diagonal(1e32),
+          dogleg_type(TRADITIONAL_DOGLEG) {
     }
 
     TrustRegionStrategyType trust_region_strategy_type;
@@ -74,6 +75,9 @@ public:
     // that the Gauss-Newton step computation is of full rank.
     double lm_min_diagonal;
     double lm_max_diagonal;
+
+    // Further specify which dogleg method to use
+    DoglegType dogleg_type;
   };
 
   // Per solve options.