Add the two-dimensional subspace search to DoglegStrategy
Change-Id: I5163744c100cdf07dd93343d0734ffe0e80364f3
diff --git a/internal/ceres/dogleg_strategy.cc b/internal/ceres/dogleg_strategy.cc
index 4b1f074..131de39 100644
--- a/internal/ceres/dogleg_strategy.cc
+++ b/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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
(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 @@
//
// 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 @@
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 @@
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
diff --git a/internal/ceres/dogleg_strategy.h b/internal/ceres/dogleg_strategy.h
index 8c2ff7b..ad3257c 100644
--- a/internal/ceres/dogleg_strategy.h
+++ b/internal/ceres/dogleg_strategy.h
@@ -47,6 +47,11 @@
// 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 @@
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 @@
// 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
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 530b47c..072f8bb 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -183,6 +183,7 @@
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();
diff --git a/internal/ceres/trust_region_strategy.h b/internal/ceres/trust_region_strategy.h
index 7d94ca2..391da97 100644
--- a/internal/ceres/trust_region_strategy.h
+++ b/internal/ceres/trust_region_strategy.h
@@ -59,7 +59,8 @@
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 @@
// 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.
