激光SLAM后端优化总结之图优化

优化问题，可以用图的方式表示，图的节点是需要优化的变量，边是优化变量之间的约束。节点可以是位姿，也可以是landmark；边可以是位姿之间的关系也可以是观测量；

1. 图的稀疏性与边缘化

考虑到位姿$i$处对路标$j$的一次观测$z_{ij}$:
$$e_{ij}=Z_{ij}-h(x_i,y_j)$$
我们根据Gauss-Newton 或 L-M 非线性优化方法，最终我们需要求解一个$H\Delta x=b$形式的方程。其中$H=J^{T}J$ 为一个稀疏矩阵；

考虑多个位姿和路标的情况：

所以$H$的表示为：

求解$H\Delta x=b$方程可以表示为：

利用舒尔补消元计算$\Delta x_c$:

然后把解得的$\Delta x_c$代入到原方程，然后求解$\Delta x_p$。

相比于直接解线性方程的做法，它的优势在于：

1. 在消元过程中，由于 $C$ 为对角块，所以 $C^{-1}$ 容易解得。
2. 求解了$\Delta x_c$之后，路标部分的增量方程由$\Delta x_p=C^{-1}(w-E^T\Delta x_c)$给出。这依然用到了$C^{-1}$ 易于求解的特性。

2. 位姿图

图优化是把位姿和空间点一起，进行优化。随着轨迹越走越长，特征点的增长也很快。因此位姿图优化的意义在于：再优化几次以后把特征点固定住不再优化，只当作位姿估计的约束，之后主要优化位姿；

在基于图优化激光SLAM 中，可以看作在激光里程计部分是不断累积图中的节点(位姿) 和边（位姿变换关系）；在后端优化部分通过构建图来最小化位姿之间的误差；

3. 因子图

因子图中，圆圈表示待优化的变量，如位姿；红框表示一次观测，蓝框表示一次运动；

蓝色框可以表示为：
$$P(x_3|x_2,u_3)$$
红色框可以表示为：
$$P(z_1|x_1,l_1)$$
所以因子图模型可以表示为：
{ x , l } ∗ = a r g m a x ( x 0 ) ∏ P ( x k ∣ x k − 1 , u k ) ∏ P ( z k ∣ x i , l j ) \{x,l\}^*=arg max(x_0)\prod P(x_k|x_{k-1},u_k) \prod P(z_k|x_{i},l_j) {x,l}∗=argmax(x0​)∏P(xk​∣xk−1​,uk​)∏P(zk​∣xi​,lj​)

因子图是增量的处理后端优化，每加入一个节点，普通图优化是对整个图进行优化。因子图相当于保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。

4. 滤波与图优化

滤波的特点：

1. 方法简明，当前时刻只和前一时刻有关；
2. 需要假设马尔可夫性，不适用于带有回环的情况；
3. 需要线性化和假设高斯性；
4. 需要存储所有状态量的矩阵和方差；

图优化方法：

1. 可以将历史时刻的变量考虑进来一起优化；
2. 随着长时间的累积，求解规模也会逐渐变大；