SLAM中非线性优化

这里是笔记，详细请查看链接
线性最小二乘
非线性最小二乘

基础知识

<UD分解>
$P:n阶正定的对称矩阵$
对矩阵P进行上三角-对角分解:
$U:上三角阵$
$D:对角阵$
$$\begin{gathered} P=UD{U}^T \\ P=\left[\begin{array}{cccc}{P}_{11}& P_{12}& ...& P_{1n}\\ P_{21}& P_{22}& ...& P_{2n}\\ ...& ...& ...& ...\\ P_{n1}& P_{n2}& ...& P_{nn}\end{array}\right],U=\left[\begin{array}{cccc}{U}_{11}& U_{12}& ...& U_{1n}\\ U_{21}& U_{22}& ...& U_{2n}\\ ...& ...& ...& ...\\ U_{n1}& U_{n2}& ...& U_{nn}\end{array}\right](U_{ii}=1,(i=1,2,3,..,n)),D=\left[\begin{array}{cccc}{D}_{11}& D_{12}& ...& D_{1n}\\ D_{21}& D_{22}& ...& D_{2n}\\ ...& ...& ...& ...\\ D_{n1}& D_{n2}& ...& D_{nn}\end{array}\right] \end{gathered}$$
求解
$$\begin{gathered} P_{ij}=D_{jj}{U}_{ij}{U}_{jj}+D_{j+1,j+1}{U}_{i,j+1}{U}_{j,j+1}+...+D_{nn}{U}_{in}{U}_{jn} \\ =\sum _{k=i+j}^n{D}_{kk}{U}_{ik}{U}_{jk}+D_{jj}{U}_{ij}{U}_{jj}(1\le i\le n,i\le j\le n) \end{gathered}$$

<QR分解>
$A_{m\times n}:为列满秩矩阵（m\ge n）,rank(A_{m\times n})=n$
那末必有：
$$A_{m\times n}=Q_{m\times n}{R}_{n\times n},Q_{m\times n}^T{Q}_{m\times n}=I_{n\times n},R_{n\times n}为上三角阵或下三角阵$$
<Cholesky分解>
$P:n阶正定对称矩阵$
总可以得到：
$$P=\left[\begin{array}{cccc}{P}_{11}& P_{12}& ...& P_{1n}\\ P_{21}& P_{22}& ...& P_{2n}\\ ...& ...& ...& ...\\ P_{n1}& P_{n2}& ...& P_{nn}\end{array}\right],\Delta =\left[\begin{array}{cccc}{\delta }_{11}& {\delta }_{12}& ...& {\delta }_{1n}\\ 0& {\delta }_{21}& ...& {\delta }_{2n}\\ ...& ...& ...& ...\\ 0& 0& ...& {\delta }_{nn}\end{array}\right]$$
$$P=\Delta {\Delta }^T=\left[\begin{array}{cccc}{\delta }_{11}& {\delta }_{12}& ...& {\delta }_{1n}\\ 0& {\delta }_{21}& ...& {\delta }_{2n}\\ ...& ...& ...& ...\\ 0& 0& ...& {\delta }_{nn}\end{array}\right]{\left[\begin{array}{cccc}{\delta }_{11}& {\delta }_{12}& ...& {\delta }_{1n}\\ 0& {\delta }_{21}& ...& {\delta }_{2n}\\ ...& ...& ...& ...\\ 0& 0& ...& {\delta }_{nn}\end{array}\right]}^T$$
可以得到：
$$P_{ij}={\delta }_{ij}{\delta }_{jj}+{\delta }_{i,j+1}{\delta }_{i,j+1}+{\delta }_{i,j+2}{\delta }_{i,j+2}+...+{\delta }_{i,j+n}{\delta }_{i,j+n}=\sum _{k=j+1}^n{\delta }_{ik}{\delta }_{jk}+{\delta }_{ij}{\delta }_{jj}(1\le i\le n,i\le j\le n)$$
从而得到:
$${\delta }_{ij}=\{\begin{array}{ll}(P_{ij}-{\sum }_{i+j}^n{\delta }_{ik}{\delta }_{jk})/{\delta }_{jj}& i<j\\ \sqrt{P_{jj}-{\sum }_{k=j+1}^n{\delta }_{jk}^2}& i=j\\ 0& i>j\end{array}$$
<奇异值分解>
$A:当为n阶实对称矩阵,可分解为：$
$Q{Q}^T=I,Q为标准正交阵，Q的列坐标为特征向量$
$\Gamma :为特征值构成的对角矩阵$
$$\begin{gathered} A=Q\Gamma Q^T(特征值分解EVD) \\ \Gamma =\left[\begin{array}{cccc}{\lambda }_1& ...& ...& ....\\ ...& {\lambda }_2& ...& ....\\ ...& ...& ....& ....\\ ...& ...& ...& {\lambda }_n\end{array}\right] \end{gathered}$$
$A:当为n阶实矩阵，可分解为：$
$U{U}^T=I,U为单位正交阵，左奇异矩阵,U\in R_{m\times m}$
$V{V}^T=I,V为单位正交阵，右奇异矩阵,V\in R_{n\times n}$
$\Sigma :奇异值，除了对角元素，其他元素为0,\Sigma \in R_{m\times n}$
$$\begin{gathered} A=U\Sigma V^T \\ \Sigma ={\left[\begin{array}{ccccc}{\sigma }_1& 0& ...& 0& 0\\ 0& {\sigma }_2& ...& 0& 0\\ 0& 0& ...& 0& 0\\ 0& 0& ...& {\sigma }_n& 0\end{array}\right]}_{m\times n} \end{gathered}$$
求解
$$A{A}^T=U\Sigma V^{T}V{\Sigma }^T{U}^T=U\Sigma {\Sigma }^T{U}^T$$

线性最小二乘

方程：
$$min||期望-样本|{|}^2$$
当系统方程为一次线性方程时：
$$min||Ax-b|{|}^2=(Ax-b{)}^T(Ax-b)=x^T{A}^{T}Ax-2x^T{A}^{T}b+b^{T}b$$
取最小值，也就是求导数为0的点：
$$\begin{gathered} \frac{\partial ||Ax-b|{|}^2}{\partial x}=2A^{T}Ax-2A^{T}b=0 \\ {A}^{T}Ax=A^{T}b \\ x=(A^{T}A{)}^{-1}{A}^{T}b \end{gathered}$$
<Cholesky分解求解：>
令$A^{T}A=L^{T}L,\alpha =Lx,\beta =A^{T}b$
$$\begin{gathered} A^{T}Ax=A^{T}b \\ {L}^{T}Lx=A^{T}b \\ {L}^T\alpha =\beta \\ Lx=\alpha \end{gathered}$$
<QR分解>
$A=QR$
$$\begin{gathered} A^{T}Ax=A^{T}b \\ (Q^{T}R{)}^T(Q^{T}R)x=(Q^{T}R{)}^{T}b \\ {R}^{T}Rx=R^{T}Qb \\ x=R^{-1}{Q}^{T}b \end{gathered}$$
实现

非线性最小二乘

$F(X):目标函数$
$$mi{n}_{x}F(X)=\frac{1}{2}||f(x)|{|}_2^2$$
只需找到：$\frac{dF(x)}{dx}=0$的最小值的点，但有时候无法得到导数，我们使用迭代方法代替求导，步骤如下：

1. 给定某个初值$x_0$
2. 进行k次迭代，寻找一个$\Delta x_k$,使$||f(x_k+\Delta x_k)|{|}_2^2$达到极小值
3. 若$\Delta x_k$足够小，停止迭代
4. $\Delta x_k$不够小，令$x_{k+1}=x_k+\Delta x_k$,转到第2步进行迭代

如何取定增量$\Delta x_k$

一阶梯度法
使用泰勒函数将目标函数$F(x)$在$x_k$一阶展开
$J(x_k{)}^T=F^{\prime }(x):在x_k处一阶导数也称雅可比矩阵、梯度$
$$F(x_k+\Delta x_k)\approx F(x_k)+F^{\prime }(x)(x+\Delta x_k-x)=F(x_k)+J(x_k{)}^T\Delta x_k$$
取$\Delta x_k$
$$\Delta x_k=-\lambda J(x_k{)}^T$$
如何确定步长$\lambda$？
基本思想就是每次试一个步长，如果用该步长走的话，看函数值会不会比当前点下降一定的程度，如果没有，就按比例减小步长，再试，直到满足条件（根据泰勒展开式我们知道步长足够小时总会满足下降条件）。所以line search实际上是计算量比较大的，不过在以前数据量不大的情况下这都不是问题。
问题：计算量大
二阶梯度法
使用泰勒函数将目标函数$F(x)$在$x_k$二阶展开
$H(x_k):二阶导数，海塞(Hessian)矩阵$
$$F(x_k+\Delta x_k)\approx F(x_k)+F^{\prime }(x_k)(x_k+\Delta x_k-x_k)+\frac{F^{\prime \prime }(x)(x_k+\Delta x_k-x_k{)}^2}{2}=F(x_k)+J(x_k{)}^T\Delta x_k+\frac{1}{2}\Delta x_k^{T}H(x_k)\Delta x_k$$
求$F^{\prime }(x_k+\Delta x_k)$关于$\Delta x$的一阶导数
$$F^{\prime }(x_k+\Delta x_k)=J(x_0{)}^T+\frac{1}{2}(H\Delta x+H^T\Delta x)=J(x_0{)}^T+H\Delta x=0$$
$$H\Delta x=-J$$
问题：计算量大
最速下降法
什么时候不用欧氏距离
${\Delta }_{nsd}:表示下降梯度$
$||v||:范数,当||v||是欧氏范数时，与最一阶梯度下降法等价$
${\Delta }_{nsd}=argmin(J(x_k{)}^{T}v(||v||<1))$
牛顿高斯法
将原函数在x处，进行一阶泰勒展开
$$f(x+\Delta x)\approx f(x_k)+f^{\prime }(x_k)(x_k+\Delta x_k-x_k)=f(x)+J(x{)}^T\Delta x$$
重新构造目标函数:
$$F(x)=\Delta x^{\ast }=argmi{n}_{\Delta }\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2$$
求极值点$F^{\prime }(x)=0$
$$J(x)f(x)+J(x)J^T(x)\Delta x=0$$
得到高斯牛顿方程
$H(x)=J(x)J^T(x)$
$g(x)=-J(x)f(x)$
$$H(x)\Delta x=g(x)$$
高斯牛顿寻找最优值

1. 给定某个初值$x_0$
2. 进行k次迭代，寻找一个$J(x_k)$,使$f(x_k)$达到极小值
3. 求解$H(x)\Delta x_k=g(x)$
4. $\Delta x_k$足够小停止，否则令$x_{k+1}=x_k+\Delta x_k$,转到第2步进行迭代

问题：H需要可逆,H不可逆时，$J{J}^T$为奇异矩阵，增量的稳定性较差，导致算法不会收敛

阻尼牛顿法/列文伯格-马夸尔法/信赖区域法
在高斯牛顿法的基础上加上信赖区域即
$$\begin{gathered} F(x)=\Delta x^{\ast }=argmi{n}_{\Delta }\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2 \\ st:||D\Delta x_k|{|}^2\le u \end{gathered}$$
构造拉格朗日函数
$\lambda :拉格朗日算子$
$$\Gamma (\Delta x_k,\lambda )=\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2+\frac{\lambda }{2}(||D\Delta x_k|{|}^2-u)$$
令${\Gamma }^{\prime }(\Delta x_k,\lambda )=0$得到：
$D^{T}D=I$
$$\begin{gathered} (H+\lambda D^{T}D)\Delta x_k=g \\ (H+\lambda )\Delta x_k=g \end{gathered}$$
阻尼牛顿法/列文伯格-马夸尔法/信赖区域法寻找最优值

1. 给定某个初值$x_0$,以及初始优化范围$u$
2. 进行k次迭代，求解 $(H+\lambda )\Delta x_k=g$
3. 计算$p$衡量近似程度的好坏$$p=\frac{f(x+\Delta x)-f(x)}{J^T(x)\Delta x}$$
4. $p>3/4,u=2u$
5. $p<1/4,u=0.5u$
6. $p>设置的阈值$，令$x_{k+1}=x_k+\Delta x_k$
7. 判断算法收敛，不收敛返回第2步