• The Problem Definition
  • Terminalogy
• Newton’s Method
• Line Search - Gauss-Newton Method
• Trust Region - The Levenberg–Marquardt Method
• The Golden Bundle Adjustment (BA)
  • Schur Complement trick
  • Conditioning of BA
  • Parameters (e.g. of ceres solver)
• Generalized Least Squares
  • Covariance Estimation [3][4]
• Reference

The Problem Definition

The problem is to find solutions to a system of equations that have the form:

where \(\mathbf{x} = [x_1, x_2, ..., x_n]^T\) is a \(n\)-dimensional vector. For convenience, we denote \((f_1, f_2, ..., f_m)^T\) by a vector-valued function \(\mathbf{f}\) and \(\mathbf{f}\) can be nonlinear functions. Then the system of equations can be re-written as

i.e., we wish to find a vector that makes the vector function equal to the zero vector. Since it is the general case that \(m\) is larger that \(n\), i.e., the system of equations is overdetermined, the problem would infeasible. Therefore, the alternative is to find the optimal solution which minimizes the sum of the least squares which is also the objective of the least squares problem:

Terminalogy

The first and second derivatives of the objective function are given below:

where \(\mathbf{J}(\mathbf{x})\) is the \(m \times n\) jacobian matrix of the transformation \(\mathbf{f} : \mathbb{R}^m \times \mathbb{R}^n\).

where \(\mathbf{H}_i(\mathbf{x}) = f_i''(\mathbf{x})\) is the \(n \times n\) Hessian matrix of the scalar-valued function \(f_i(\mathbf{x})\).

Newton’s Method

Newton’s method is an algorithm originally designated to locate roots of equations, e.g., \(F(\mathbf{x})=0\). It is derived from the linear approximation of the function \(F(\mathbf{x})\) by first-order Tayor series expansion. From Calculus, the following is the linear approximation of \(F(\mathbf{x})\) at \(\mathbf{x}+\mathbf{h}\):

if \(\|\mathbf{h}\|\) is sufficiently small. By rearranging the equation, we obtain

The update step is explicitly written as

Connection between Newton’s method and optimization. Newton’s method can also be used as a descent method for iterative non-linear optimization: from a starting point \(\mathbf{x}_0\) it produces a series of vectors \(..., \mathbf{x}_t, \mathbf{x}_{t+1},...\) which (hopefully) converges to \(\mathbf{x}^*\), a local stationary minimizer for the given function \(F(\mathbf{x})\). Since \(\mathbf{x}^*\) is a stationary point, it satisfies \(F'(\mathbf{x}^*) = 0\). Similarly, we have

Here the Newton’s method is used to locate the root of \(F'(\mathbf{x}^*) = 0\). The update step is

where \(\mathbf{h}_N\) is the solution to

If \(F''(\mathbf{x})\) is positive definite, \(\mathbf{h}_N^T F''(\mathbf{x}) \mathbf{h}_N = - \mathbf{h}_N^T F'(\mathbf{x}) > 0\), which means \(\mathbf{h}_N\) is a descent direction when minimizing \(F(\mathbf{x})\). Therefore, Newton’s method is only useful in the final stage of the iteration where \(\mathbf{x}\) is close to \(\mathbf{x}^*\) so that \(F''(\mathbf{x})\) is positive definite. The good news about Newton’s method is that if \(F''(\mathbf{x})\) is positive definite, we get quadratic convergence while gradient descent (the steepest descent method) can only produce linear convergence.

Line Search - Gauss-Newton Method

The Gauss-Newton method is based on the linear approximation to \(\mathbf{f}\) by the first-order Tayor series expansion in the neighbourhood of \(\mathbf{x}\): For small \(\|\mathbf{h}\|\), we have

where \(\mathbf{J}(\mathbf{x})\) is the Jacobian matrix of \(\mathbf{f}(\mathbf{x})\). Then

where \(\mathbf{f} = \mathbf{f}(\mathbf{x})\) and \(\mathbf{J} = \mathbf{J}(\mathbf{x})\).

There is a little interesting fact to address here. If we directly apply the second-order taylor expansion to \(F(\mathbf{x})\), we get

where \(\mathbf{H}\) is the Hessian matrix at current \(\mathbf{x}\). Since the Hessian matrix is generally hard to compute, the square of jacobian \(\mathbf{J}^T\mathbf{J}\) is often used to approximate the Hessian matrix \(\mathbf{H}\). As shown by the second equation in Terminalogy, the approximation just eliminates the second term \(\sum_{i=1}^m f_i(\mathbf{x}) \mathbf{H}_i(\mathbf{x})\).

Getting back to the point, the Gauss-Newton step is to minimize \(L(\mathbf{h})\), i.e., the objective at next point,

It is easily seen that the gradient and the Hessian of \(L\) with respect to \(\mathbf{h}\) are

We can see that \(L''(\mathbf{h})\) is independent of \(\mathbf{h}\). If \(\mathbf{J}\) is full rank, i.e., if the columns are linearly independent, then \(L''(\mathbf{h})\) is positive definite, which means \(L(\mathbf{h})\) is convex. Inspired by Newton’s method, the minimization problem can be solved by finding \(\mathbf{h}\) that makes \(L'(\mathbf{h}) = \mathbf{0}\), i.e.

This is a descent direction for \(F\) since

The update step of the Gauss-Newton method is typically:

where \(\alpha\) is found by line search. The classical Gauss-Newton method uses \(\alpha = 1\) in all steps. In general, the Gauss-Newton method ensure linear convergence \((\|\mathbf{x}_{t+1} - \mathbf{x}^* \| \leq a\|\mathbf{x}_t - \mathbf{x}^* \|, 0 < a < 1)\) and even quadratic convergence \((\|\mathbf{x}_{t+1} - \mathbf{x}^* \| = O(\|\mathbf{x}_t - \mathbf{x}^* \|^2))\) when \(\mathbf{x}\) is close to \(\mathbf{x}^*\).

The method of locating the direction by solving \(L'(\mathbf{h}) = 0\) is called exact line search. An alternative is inexact line search which loosely asks for a sufficient decrease in \(L(\mathbf{h})\), e.g., using backtracking line search or Wolfe conditions to loosely find step length along given search direction.

Trust Region - The Levenberg–Marquardt Method

The LM method suggests to use a damped Gauss-Newton mothod. The step \(\mathbf{h}_{LM}\) is defined by the following modification:

The damping parameter \(\mu\) has several effects:

• For all \(\mu > 0\) the coefficient matrix \(\mathbf{J}^T \mathbf{J} + \mu \mathbf{I}\) is positive definite, and this ensures that \(\mathbf{h}_{LM}\) is a descent direction.
• For large values of \(\mu\) we get \(\mathbf{h}_{LM} = -\frac{1}{\mu} \mathbf{J}^T\mathbf{f} = -\frac{1}{\mu} F'(\mathbf{x}),\) i.e. a short step in the steepest descent direction. This is desired if the current iterate is far from the solution. But, the larger damping factor \(\mu\) is, the slower convergence gets.
• For small values of \(\mu\), \(\mathbf{h}_{LM} = \mathbf{h}_{GN}\), which is a good step in the final stage of the iteration. When \(\mathbf{x}\) is close to \(\mathbf{x}^*\), the LM can have almost quatratic final convergence as the Gauss-Newton method.

Thus, the damping parameter influences both the direction and the size of the update step. This leads us to make a method without a specific line search (i.e., to explicitlt determine the step size).

The Golden Bundle Adjustment (BA)

Bundle Adjustment (BA) is a classic optimization problem developed in photogrammetry in the 50’s. Each error function corresponds to a reprojection error concerning a camera and a 3D point.

Suppose a BA problem considers a set of \(m\) cameras \(\mathcal{C} = \{c_1, c_2, \ldots, c_m \}\), a set of \(n\) points \(\mathcal{P}= \{p_1, p_2, \ldots, p_n \}\) and a set of projections \(\mathcal{E} \in \mathcal{C} \times \mathcal{P}\), the objective is to estimate the \((m+n)\)-dimensional vector \(\mathbf{x} = [c_1, c_2, \ldots, c_m, p_1, p_2, \ldots, p_n]^T\). The total \(k\) reprojection functions form a \(k\) dimensional vector denoted by \(\mathbf{e} = [e_1, e_2, \ldots, e_k]^T\). The Jacobian matrix is computed by taking the first-order derivative of the error vector \(\mathbf{e}\) with respect to the input vector \(\mathbf{x}\):

Based on the LM algorithm elaborated above, the update step is to solve the equation:

The symmetric matrix \(\mathbf{J}^T \mathbf{J}\) is quite sparse because each reprojection error function only involves one camera and one track. Here it can be re-written as

For simplicity, the damping term \(\mu \mathbf{I}\) is omitted here, so that the update equation is turned into

Schur Complement trick

Now, the problem is to solve the linear equation system of this form:

Its solution is obtained as the solution of

by using the Schur Complement. And the first equation can then be solved by applying Cholesky factorization ( \(\mathbf{A} \mathbf{x} = \mathbf{b}\) \(\Rightarrow\) \(\mathbf{L} \mathbf{L}^T \mathbf{x} = \mathbf{b}\) \(\Rightarrow\) Solve \(\mathbf{y}\) in \(\mathbf{L} \mathbf{y} = \mathbf{b}\) \(\Rightarrow\) Solve \(\mathbf{x}\) in \(\mathbf{L}^T \mathbf{x} = \mathbf{y}\) ). Depending on the sparsity of the matrix, Cholesky factorization can be implemented in dense, sparse and iterative ways.

Conditioning of BA

The BA problem is generally known to be ill-conditioned, because of the varying sensativity of the objective function to different parameters. For example, small changes in the translation of a camera affect the objective much less than small changes in the radial distortion parameters. (See Visibility Based Preconditioning for Bundle Adjustment)

Parameters (e.g. of ceres solver)

Generalized Least Squares

Here, we consider a regression problem: Given a set of observation data \(\{(\mathbf{x}_i, \mathbf{y}_i)\}_{i=1,..,n}\), we want to regress a linear/nonlinear function \(\mathbf{f}(.)\) which minimizes

The assumption is generally held that the condifition mean of \(\mathbf{Y}\) given \(\mathbf{X}\) is equal to \mathbf{f}(\mathbf{X}) and the conditional variance-covariance of \(\mathbf{Y}\) given \(\mathbf{X}\) is a known nonsingular matrix \(\mathbf{\Omega}\). Therefore, the conditional mean and covariance of the error term \(\mathbf{\epsilon} = \mathbf{Y} - \mathbf{f}(\mathbf{X})\) are zero and \(\mathbf{\Omega}\), i.e.,

Therefore, the generalized least square problem is formulated as

which minimizes the squared Mahalanobis length of this residual vector \(\mathbf{\epsilon}\).

Since the covariance \(\mathbf{\Omega}\) is generally unvailable, the Feasible Generalized Least Squares (FGLS) estimator is adopted. It proceeds in two stages:

1. the model is estimated by Ordinary Least Squares (OLS) and the residuals are used to build a consistent estimator of the errors covariance matrix \(\mathbf{\Omega}\);

2. using the estimated \(\mathbf{\Omega}\), solve the generalized least square problem.

Covariance Estimation [3][4]

“One way to assess the quality of the solution returned by a non-linear least squares solver is to analyze the covariance of the solution.” Let’s consider

Taking the first-order taylor expansion of \(\mathbf{f}(\mathbf{x})\), we have

Therefore, the covariance of \(\mathbf{\epsilon}\), \(C(\mathbf{\epsilon}) = \mathbf{J} C(\mathbf{x}) \mathbf{J}^T\). Then we get \(C(\mathbf{x}) = \mathbf{J}^{-1} C(\mathbf{\epsilon}) \mathbf{J}^{-T} = \mathbf{J}^{-1} \mathbf{S} \mathbf{J}^{-T}\). If we assume the error vector takes identity covariance (iid), i.e., \(\mathbf{S} = \sigma^2 \mathbf{I}\), the covariance of the variable \(\mathbf{x}\) becomes \(C(\mathbf{x}) = \sigma^2 \mathbf{J}^{-1} \mathbf{J}^{-T} = \sigma^2 (\mathbf{J}^T \mathbf{J})^{-1}\).

Reference

[1] Nonlinear Systems - Newton’s Method

[2] METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS

[3] Covariance estimation reference 1

[4] Covariance estimation reference 2

[5] HESSIAN MATRIX VS. GAUSS–NEWTON HESSIAN MATRIX

[6] Pushing the Envelope of Modern Methods for Bundle Adjustment

[7] Bundle Adjustment in the Large

[8] Generalized Subgraph Preconditioners for Large-Scale Bundle Adjustment

[9] Tutorial on computing Jacobian for Bundle Adjustment Problem - Part1

[9] Tutorial on computing Jacobian for Bundle Adjustment Problem - Part2

[10] Indicative parameter setting for LM based Bundle Adjustment

[11] METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS

[12] A review of trust region algorithms for optimization

[13] Pushing the Envelope of Modern Methods for Bundle Adjustment

[14] Bundle Adjustment in the Large

[15] Visibility Based Preconditioning for Bundle Adjustment

[16] Distributed Very Large Scale Bundle Adjustment by Global Camera Consensus

[17] A Consensus-Based Framework for Distributed Bundle Adjustment

[18] pOSE: Pseudo Object Space Error for Initialization-Free Bundle Adjustment

[19] ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM

[20] Is Levenberg-Marquardt the Most Efficient Optimization Algorithm for Implementing Bundle Adjustment?

[21] Sparse bundle adjustment

[22] Sparse sparse bundle adjustment

[23] Multicore Bundle Adjustment