LOAM SLAM原理之防止非线性优化解退化

1. 概述

LOAM中用于防止非线性优化解退化的方法，指出原始参考代码中出现错误的地方，希望能帮助大家，同时供自己以后参考。

2. 理解

对于SLAM而言，视觉传感器需要纹理信息决定特征点，距离传感器需要空间几何结构信息决定特征点，在特征点缺乏时，状态估计方法会进行退化，为了解决这种退化问题，本文提出了一种在线方法。
处理退化的常用方法有:

• 1）当出现退化时换一种方法;这要求在设计时需要一个备用方法可用
• 2）在状态估计过程中加入人工约束，如恒速模型的约束。即使问题本身是可解决的情况下也会带来不必要的误差。

我们的方法是在原始问题的部分子空间添加约束。原始问题的解可以分为非退化方向和退化方向，在处理问题时，首先确定退化方向，然后将求出原始问题非退化方向的解，对于退化方向的解使用猜测值。在最后的实践算法中，只使用非线性优化解在非退化方向的分量，不考虑退化方向的分量。

正常情况下，约束应该是分布在空间中的多个方向，从各个角度约束解，如下面所示，绿色点表示问题的解，被非退化约束限制在了一个小区域。这样的解就比较准确，约束发生小变化时，解的变化也很小，被限制在一个局部区域，这样的解就是一个比较理想的解。

如果解的约束大多近似平行，那么他们就是退化的方向（蓝色箭头表示的方向），这个时候解在退化方向收到的约束就很差。考虑绝对平行时，解的约束也是一个平行的方向，那么这种情况下的解是需要避免的，如果其中一个约束发生了小的偏移，那么解所在的局部区域会发生较大的变化，这样的解是比较糟糕的解。

我们提出的方法只是线性问题、非线性问题求解的一个小步骤，计算复杂度可以忽略。实验结果表明该方法能够改善环境退化下的位姿解估计结果。

这种方法有点类似可观测子空间、可控子空间的概念。
每次求解时

• 优化解=原始解在非退化方向投影+估计值在退化方向投影（实际算法中可以将退化方向解丢弃，只考虑非退化方向）

考虑方程
$$\underset{x}{\min }{f}^2(x)\text{\hspace{1em}(1)}$$
那么$(1)$为线性方程时可以直接使用奇异值分解SVD、QR分解求解，但是$(1)$为非线性方程时需要进行局部微分转换为求解线性方程，因此线性、非线性问题都可以转换为下面的形式
$$arg\underset{x}{\min }||Ax-b|{|}^2\text{\hspace{1em}(2)}$$
我们的方法将(2)中的每一行看作$x$空间中的一个(超)平面，这些平面约束着$x$（通过方向$A$和位置$b$共同约束得到$x$的解），并研究平面的几何分布来确定退化方向。
做出两个假设：

• 假设在考虑传感器噪声的情况下对矩阵A进行了适当的加权。也就是$(1)$问题的线性加权形式
• 原始问题不受约束，并且在问题非退化时，解由真正的传感器测量值决定。

第一个问题：给定线性化系统$(2)$，确定状态空间中时候存在退化并确定相应的退化方向。在退化的情况下，防止在退化方向上出现错误解。
假设$(3)$的原始解为$x_0$，一开始加入经过$x_0$的任意方向扰动$c$，解并不发生变化

如果扰动$c$相对于$x_0$偏离$\delta d$，那么原始解$x_0$也会偏离$\delta x_c$，因为这个时候的约束区域已经发生了改变。由于扰动$c$的方向任意，因此存在一个原始解的最大偏移，$\delta {x_c}^{\ast }=\underset{c}{\max }{x}_c$
定义1：退化因子$D$
$$D=$$