SLAM中最小二乘优化

BA最小二乘的本质

BA优化的是最小二乘问题，那么为什么可以使用最小二乘来估计呢？小二乘问题与概率之间的关系：

使用最大后验概率（MAP）来得到状态量，其中z是观测值，x是状态：
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\max }p(\mathbf{x}|\mathbf{z})\text{\hspace{1em}(1)}$$
使用贝叶斯展开：

$$\begin{array}{rl}\hat{\mathbf{x}}& =\arg \underset{\mathbf{x}}{\max }p(\mathbf{x}|\mathbf{z})=\arg \underset{\mathbf{x}}{\max }\frac{p(\mathbf{z}|\mathbf{x})p(\mathbf{x})}{p(\mathbf{z})}\\ & =\arg \underset{\mathbf{x}}{\max }p(\mathbf{z}|\mathbf{x})p(\mathbf{x})\end{array}$$

其中第一项化为：
$$p(\mathbf{z}|\mathbf{x})=\prod _{k=0}^{K}p\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)\text{\hspace{1em}(2)}$$
第二项化为：
$$p(\mathbf{x})=p\left({\mathbf{x}}_0|{\check{\mathbf{x}}}_0\right)\prod _{k=1}^{K}p\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1}\right)\text{\hspace{1em}(3)}$$
第二项可以假设一个运动模型，忽略。

可以转化为最大似然估计（MLE）：
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\max }\sum _{k=0}^K\log p\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)\text{\hspace{1em}(4)}$$
其中使用高斯模型来假设：
$$\log p\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)=-\frac{1}{2}{\left({\mathbf{z}}_k-z({\mathbf{x}}_k)\right)}^T{\mathbf{\Sigma }}_k^{-1}\left({\mathbf{z}}_k-z({\mathbf{x}}_k)\right)-\frac{1}{2}\log \left((2\pi {)}^M\det {\mathbf{\Sigma }}_k\right)\text{\hspace{1em}(5)}$$
最后一项与状态无关，因此MLE可以化为：
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\min }\sum _{k=0}^K\Vert r({\mathbf{z}}_k,{\mathbf{x}}_k){\Vert }_{{\Sigma }_k}^2\text{\hspace{1em}(6)}$$
其中残差项为马氏距离：
$$\Vert r{\Vert }_{\Sigma }^2=r^T{\Sigma }^{-1}r\text{\hspace{1em}(7)}$$
这样就转化为了非线性最小二乘问题。

最小二乘服从卡方分布
$$z=(\mathbf{x}-\mathbf{m}{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{m})\sim {\chi }_n^2\text{\hspace{1em}(8)}$$

这相当于一种加权的最小二乘。

---

最小二乘问题（Least-squares Minimization）

对于线性方程组，解的判别条件如下：

1. $A\mathbf{x}=0$ 总有解，至少有零解

2. $A_{m\times n}\mathbf{x}=0$

   当$r(A)=n$，只有零解
   当$r(A)<n$，有无穷多解

3. $A_{m\times n}\mathbf{x}=\mathbf{b}$

   当$r(A)\ne r(A|b)$，无解
   当$r(A)=r(A|b)=n$，有唯一解
   当$r(A)=r(A|b)=r<n$，有无穷多解

针对上面第三个方程，根据未知数和方程数量的关系可以分为以下几种情况：

如果$m<n$，未知数大于方程数。那么解不唯一，存在一个解矢量空间。

如果$m=n$，那么只要$A$可逆（非奇异，也就是满秩）就有唯一解，解为$x=A^{-1}b$。

如果$m>n$，方程数大于未知数。方程一般没有解，除非$\mathbf{b}$属于$A$的列向量组成的子空间。

这里介绍针对$Ax=b$使用直接方法进行求解，即不使用迭代的方式。

超定方程

考虑$m>n$，$r(A)=n$，这时无解，但可以找到一个最小二乘解，解最小二乘的解法有奇异值分解、正规方程和QR分解，可以参考许可师兄的博客。

正规方程的几何解释：

我们要寻找的是使得$\Vert A\mathbf{x}-\mathbf{b}\Vert$最小的矢量$\mathbf{x}$的值，把$A$写成列空间的形式$A=(a_1,a_2,...,a_n)$，当$\mathbf{x}$取不同的值时，$A\mathbf{x}$看作是列空间$A$生成的子空间，但是在存在一个最接近向量$\mathbf{b}$的子空间，我们要找到这个子空间。

针对这个子空间的理解：

假设$A$是$m\times n$维（$m>n$ ）的矩阵，那么就可以理解为在m维的空间上有以这n个（或者更少，据相关性决定）向量为基底组成的子空间中，找到一个向量与m维空间中的一个向量b最接近，由于是子空间中，若b不在这个子空间中那么是找不到解析解的，只有最接近的最小二乘解，也就是图中垂直的情况。

如图，平面为$Rank(A)$的子空间，当向量$A\mathbf{x}-\mathbf{b}$垂直如图的列空间时最小，其它情况都是斜边比它大。

因此让向量$A\mathbf{x}-\mathbf{b}$垂直与$A$的列空间，得到：
$$A^T(A\mathbf{x}-\mathbf{b})=0\text{\hspace{1em}(9)}$$
整理得到：
$$\begin{gathered} A^{T}A\mathbf{x}=A^T\mathbf{b} \\ \mathbf{x}=(A^{T}A{)}^{-1}{A}^T\mathbf{b}\text{\hspace{1em}(10)} \end{gathered}$$

为什么有解，因为$A^{T}b$在$A^{T}A$的列空间中。

加权的最小二乘问题：
$$\left({\mathrm{A}}^{\mathrm{T}}\mathrm{C}\mathrm{A}\right)\mathbf{x}={\mathrm{A}}^{\mathrm{T}}\mathrm{C}\mathrm{b}\text{\hspace{1em}(11)}$$

适定方程

考虑$A\in R^{n\times n}$，$r(A)=n$的情况，这种情况下通常可以使用公式（12）求解，但是当矩阵维数很大时，或者要求实时性时就变得不实用了。
$$\mathbf{x}={\mathbf{A}}^{-1}\mathbf{b}\text{\hspace{1em}(12)}$$
针对稀疏模式，我们可以利用一些系数矩阵结构来更快的进行求解。通常包括带状（如下图）、分块对角和稀疏矩阵。

通常方便求解的一些特殊系数矩阵包括：

对角矩阵：直接可以求出。

上三角矩阵：从最后一个未知数开始解，使用后向带入（back-substitution）的方式。

下三角矩阵：从第一个未知数开始解，使用前向带入（forward-substitution）的方式。

正交矩阵：转置求解，特殊的有Householder矩阵$A=I-2u{u}^T$形式可以加快求解。

排列矩阵：实现矩阵的行交换和列交换的矩阵，只有1或者0。直接求解。

矩阵的求解方法可以分为：

因式分解的方法，把系数矩阵A分解成上面所描述的一些方便求解的矩阵的连乘形式进行求解。

分块消元的方法，对系数矩阵具有特殊结构的形式，可以进行分块消元来求解。

因式分解

LU分解

针对非奇异矩阵 ，可以因式分解为
$$A=PLU\text{\hspace{1em}(13)}$$
式中P是排列矩阵，L是单位下三角矩阵，U是上三角矩阵。

带状结构矩阵和稀疏矩阵可以获得更快的运算。对于稀疏矩阵有如下分解：
$$A=P_{1}LU{P}_2\text{\hspace{1em}(14)}$$
LU若是稀疏的，可以加快求解，它的稀疏性依赖于排列矩阵$P_1{P}_2$的选择。系数矩阵的LU分解成本依赖于维数、非零元素的个数、稀疏模式和使用的算法等。

Cholesky分解

针对对称正定矩阵，可以因式分解为
$$A=L{L}^T\text{\hspace{1em}(15)}$$
其中L为下三角非奇异矩阵，对角元素是正数。快于LU分解一倍，对带状和稀疏矩阵，使用特殊算法复杂度远低于稠密矩阵的复杂度。

对于稀疏矩阵，有分解形式：
$$A=PL{L}^T{P}^T\text{\hspace{1em}(16)}$$
同样排列矩阵P对L的稀疏性有很大影响，它由系数矩阵的稀疏模式所决定，不取决于非零元素的具体值。因此可以分为两步骤，符号因式分解和数值因式分解。（LU分解的P依赖于具体数值）

LDLT分解

针对非奇异对称矩阵，可以分解为
$$A=$$