Nonlinear optimization and BA summary in SLAM

1. Nonlinear least squares problem

  Consider the simple problem first: $$\underset{x}{\min }\frac{1}{2}||f(x)|{|}_2^2$$1. When $When\; f$ is very simple: let$\frac{df}{dx}=0$ , the extreme point or saddle point will be obtained, just compare these solutions.
2. When$When\; f$ is complex ($f$ is an n-ary function):$\frac{df}{dx}$Hard to find, or $\frac{df}{dx}=0$ is difficult to solve, and iterative method is used to solve it.

  The way to iterate is:

1. Given some initial value $x_0$。
2. For the kth iteration, find a delta $\Delta x{\_}_k$,from $||f(x_k+\Delta x{\_}_k)|{|}_2^2$reaches a minimum value.
3. 若$\Delta x{\_}_k$If small enough, stop.
4. Otherwise, let $x_{k+1}=x_k+\Delta x{\_}_k$, returns 2.

  Here we need to determine the incremental method (ie gradient descent strategy): first-order or second-order. First, it needs to be Taylor expanded to get: $$||f(x_k+\Delta x{\_}_k)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x{\_}^{T}H\Delta x$$  ifOnly keep the first order gradient：$\underset{\Delta x\_}{\min }||f\left(x\right)|{|}_2^2+J\Delta x$ , the direction of increment is:$\Delta x{\_}^{\ast }=-J^T\left(x\right)$. (usually also need to calculate the step size), the method is calledsteepest descent.
  likePreserve the second order gradient：$\Delta x{\_}^{\ast }=arg\min ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x{\_}^{T}H\Delta x$, then get (make the above formula about$\Delta x$ derivative is zero):$$H\Delta x=-J^{The\; method}$$ is calledNewton's method。

  Although the steepest descent method and Newton's method are intuitive, there are some disadvantages in their use:

1. The steepest descent method may lead to an increase in the number of iterations due to being too greedy
2. The number of iterations of Newton's method is small, but it needs to calculate the complex Hessian matrix

  Therefore, the calculation of the Hessian can be circumvented by Gauss-Newton and Levenberg-Marquadt.

2. Understand Gauss-Newton, Levenburg-Marquadt and other descending strategies

Gauss-Newton

First order approximation $f(x)$：$f(x+\Delta x)\approx f(x)+J(x)\Delta x$
平方误差变为：$$\frac{1}{2}||f(x)+J(x)\Delta x|{|}^2=\frac{1}{2}(f(x)+J(x)\Delta x{)}^T(f(x)+J(x)\Delta x)=\frac{1}{2}(||f(x)|{|}_2^2+2f(x{)}^{T}J(x)\Delta x+\Delta x{\_}^{T}J(x{)}^{T}J\left(x\right)\Delta x)$$let about$\Delta x$ derivative is zero:$2J\left(x\right)\_{}^{T}f(x)+2J\left(x\right)\_{}^{T}J(x)\Delta x=0$$$J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f\left(x\right)$$
is written as:$$H\Delta x=g$$
  GN approximates H with the expression of J.
Proceed as follows:

1. Given initial value $x_0$。
2. For the kth iteration, find the current Jacobian matrix $J(x_k)$ and error$f(x_k)$。
3. Solve the delta equation: $H\Delta x_k=g$。
4. 若$\Delta x{\_}_k$If small enough, stop. Otherwise, let $x_{k+1}=x_k+\Delta x{\_}_k$。

Levenberg-Marquadt

  Gauss-Newton is simple and practical, but $\Delta x{\_}_k=H^{-\; H\; cannot\; be\; guaranteed\; to\; be\; invertible\; in1}g$(the quadratic approximation is not reliable).
  And the Levenberg-Marquadt method improves it to some extent.
  GN belongs to the line search method: find the direction first, and then determine the length; LM belongs to the trust region method, thinking that the approximation is only reliable in the region.
  Consider the description of the degree of approximation in LM $r=\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}$i.e. actual drop/approximate drop. If it is too small, reduce the approximation range; if it is too large, increase the approximation range.

  The process of LM is as follows:

1. Given initial value $x_0$, and the initial optimal radius $m$ .
2. For the kth iteration, solve for: $$\underset{\Delta x{\_}_k}{\min }\frac{1}{2}||f(x_k)+J(x_k)\Delta x{\_}_k|{|}^2,s.t.||D\Delta x_k|{|}^2\le \mu$$ where$\mu$ is the radius of the trust region.
3. Calculate $r$ .
4. If $r>\frac{3}{4}$，则$m=2m$ ;
5. 若$r<\frac{1}{4}$，则$m=0.5m$ ;
6. If $If\; \rho$ is greater than a certain threshold, it is considered approximately feasible. Let$x_{k+1}=x_k+\Delta x{\_}_k$。
7. Determine whether the algorithm has converged. Return 2 if not converged, otherwise end.

  For optimization in the trust region, use Lagrange multipliers to transform into unconstrained: $$\underset{\Delta x{\_}_k}{\min }\frac{1}{2}||f(x_k)+J(x_k)\Delta x{\_}_k|{|}^2+\frac{}{2}||D\Delta x|{|}^2$$ is still expanded with reference to the Gauss-Newton method, and the incremental equation is:$$(H+\lambda D^{T}D)\Delta x=gIn$$ the Levenberg method, take D=I, then:$$(H+\lambda I)\Delta x=$$  Compared with GN,$$g\; LM\; can\; guarantee\; the\; positive\; definiteness\; of\; the\; incremental\; equation,\; that\; is,\; it\; believes\; that\; the\; approximation\; is\; only\; valid\; within\; a\; certain\; range,\; and\; if\; the\; approximation\; is\; not\; good,\; the\; range\; will\; be\; reduced;\; from\; the\; perspective\; of\; the\; incremental\; equation,$$Can be seen as a mixture of first-order and second-order, the parameter $\lambda$ controls the weights on both sides, if $\lambda$ is 0, then$H\Delta x=g$ , that is, the second-order method Newton method; if$If\; \lambda$ is very large, the steepest descent method of the first-order method is used.

Three, BA

  First, what is the error? How to express. The variables to be optimized in BA are pose and landmark points. How to find the derivative of the error function with respect to pose and landmark points? What is the Lie algebraic perturbation model? What is the Jacobian matrix? What is the specific form in BA?
  Rotation matrix group and transformation matrix group: $$SO(3)=R\in R^{3\times 3}|R{R}^T=I,det(R)=1$$$$SE\left(3\right)=\left\{T=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\in R^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\right\}$$hascontinuous smooth propertyThe group of is called a Lie group, and there is a problem: it is not closed for addition, and cannot be derived.

  There are two ways of seeking：

1. Add a small amount to the Lie algebra corresponding to R, and find the rate of change relative to the small amount ( derivative model );
2. Multiply R by a small amount on the left or right, and find the rate of change of the Lie algebra relative to the small amount ( disturbance model ):
   In the disturbance model (left multiplication), multiply the small amount on the left, and make its Lie algebra zero, get:
   
   The derivative model of pose transformation is:
   

4. Graph optimization and g2o

Reprojection error:

Constructing a least squares problem on the error function:

Matrix form: $$s_i{u}_i=Kexp({\xi }^{\wedge })P_i$$误差函数：$$X^{\ast }=arg\underset{X}{\min }\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}Kexp({\xi }^{\wedge })P_i|{|}_2^2$$Write this error function as $e\left(x\right)$：$$and(x+\Delta x)=and\left(x\right)+J(x)\Delta x$$相机模型：$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y$$Use the disturbance model to derive:




we multiply the two derivatives obtained:

this matrix is ​​the Jacobian matrix we obtained, which guides the direction of iteration during optimization, so that the pose of the camera is optimized.

Spatial point locations of optimized features:

This matrix is ​​the Jacobian matrix when optimizing the spatial position of feature points, which guides the direction of iteration.