DSO优化中零空间的处理

对于纯视觉SLAM，其零空间有7维：3 rotation+3 translation+1 scale；对于VIO系统而言，充分的加速度计激励可以为系统提供尺度信息，加速度计同时可以提供重力方向，使得pitch和roll可观，因此其不客观的维度为4维：3 translation+1 yaw。

SLAM中的零空间其实是整个优化问题的零空间，而不是说是优化中某个节点的零空间。就是说整个优化问题存在不可观的维度，这个不客观的维度会通过优化问题进而影响到某个节点的优化，导致那个节点出现问题，常见的比如说纯视觉SLAM在转弯的时候，表现在地图中可能是地图的尺度突然缩小，相机移动量也对应缩小，同时不会对能量函数造成影响

在数学上： 对于正规方程： $\mathbf{H} \Delta x=\mathbf{b}$ (1)

其中hession矩阵的零空间满足：$\mathbf{H} \Delta x_{n s}=0$ (2)

结合两个式子，一定有：$\mathbf{H}\left(\Delta x+\Delta x_{n s}\right)=\mathbf{b}$

也就是说hession矩阵不满秩

因此在零空间上的漂移不会违反系统的约束,但是会对结果产生影响

DSO初始化一般进行归一化，相当于是人为设了一个尺度，并且固定第一帧，为什么还会有漂移问题呢？在优化过程中，线性方程的解不唯一。尽管每次迭代的步长都很小，但是时间长了，也容易让整个系统在这个零空间中游荡，令尺度发生漂移。

在dso中，使用了零空间的正交化去避免零空间对最终的增量产生的影响

其中红色的箭头表示增量方程中求解出来的增量，黑色的虚线表示零空间在这个节点上可能产生的漂移，而蓝色的箭头表示最终我们正交化之后的增量结果，当正交化之后，相机最终的位置会到蓝色三角显示的位置。

位姿的零空间的基

对于VO来说,是7个自由度不可观的,即零空间是以这7个状态量为基构成的。需
要注意的是零空间的变换是global的,求基方法即增加global小量扰动。

$\mathbf{T}_{w c} \operatorname{Exp}\left(\delta \xi_{c}\right)=\operatorname{Exp}\left(\delta \xi_{w}\right) \mathbf{T}_{w c}$

$\operatorname{Exp}\left(\delta \xi_{c}\right)=\mathbf{T}_{c w} \operatorname{Exp}\left(\delta \xi_{w}\right) \mathbf{T}_{w c}$

$\Delta \mathrm{T}_{\mathrm{p}}=\operatorname{TExp}\left(\delta \xi_{1}\right) \mathrm{T}^{-1}$
$\Delta \mathrm{T}_{\mathrm{n}}=\operatorname{TExp}\left(\delta \xi_{2}\right) \mathrm{T}^{-1}$

其中，$\delta \xi_{1}=0.001, \delta \xi_{2}=-0.001$

$T_{x_{\text {nullspace}}}=\frac{\log \left(\Delta \mathrm{T}_{\mathrm{p}}\right)-\log \left(\Delta \mathrm{T}_{\mathrm{n}}\right)}{\delta \xi_{1}-\delta \xi_{2}}$ ：

尺度的零空间的基

$\Delta \mathrm{T}_{\mathrm{p}}=\operatorname{Exp}\left(\delta t_{w1}\right) \mathbf{T}_{w c}$

$\Delta \mathrm{T}_{\mathrm{n}}=\operatorname{Exp}\left(\delta t_{w2}\right) \mathbf{T}_{w c}$

其中，$\delta t_{w1}=t_{cw}*1.00001, \delta t_{w2}=t_{cw}/1.00001$

$t_{x_{\text {nullspace}}}=\frac{\log \left(\Delta \mathrm{T}_{\mathrm{p}}\right)-\log \left(\Delta \mathrm{T}_{\mathrm{n}}\right)}{\delta \xi_{1}-\delta \xi_{2}}$

如何处理零空间

• Gauge fixation:把不可观的状态固定为某一些值,具体的为固定第一帧的位姿,等价于设对应的残差雅克比为0。相当于权重趋近于无穷大。

• Gauge prior:给这些状态设置先验,增加相应的惩罚项。

  信息矩阵不满秩的根本原因在于整个系统求解的初值不确定，因此如果将状态量的初值固定住，就可以有效约束系统的不确定性。常用的方法包括：1）对信息矩阵中的添加单位矩阵I；2）强行将信息矩阵设定为极大值（$10^{14}$)。（本质都是使得初值对应的Δx=0，从而固定初值。）

• Free gauge:让它自由演变,不管,使用伪逆或者增加阻尼(LM)使得问题可解。相
  当于先验权重为0。

DSO中除了给第一帧设置先验以外，使用LM算法求解。由于LM求解过程中会对信息矩阵添加阻尼项，添加之后的信息矩阵必定是满秩的该方法虽然能够保证正则方程有解，但解可能会向零空间偏移，所以dso还使用伪逆来求解。

设空间$\mathcal{M}$是零空间

$\mathbf{Nx=v}$,其中，N是零空间的一组基，v是增量方程的解，通常情况下方程是无解的，但我们可以找一个唯一的解$x_0$使得$\|\mathbf{N} \mathbf{x}-\mathbf{v}\|$最小，在图中就是蓝色箭头和虚线的垂足。

这时$x_{0}=\left(\mathbf{N}^{\mathrm{T}} \mathbf{N}\right)^{-1} \mathbf{N}^{\mathbf{T}} \mathbf{v}$, 其中$\left(\mathbf{N}^{T} \mathbf{N}\right)^{-1} \mathbf{N}^{T}$其实就是N的伪逆$\mathbf{N}^{\dagger}$，这时v在零空间的分量为$\mathbf{NN}^{\dagger}\mathbf V$

去除零空间影响的解为$\mathbf{v} \leftarrow \mathbf{v}-\mathbf{N} \mathbf{N}^{\dagger} \mathbf{v}$

参考文献：

1. On the comparison of gauge freedom handling in optimization-based visual-inertial state estimation[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 2710-2717.
2. https://blog.csdn.net/wubaobao1993/article/details/105106301/
3. https://blog.csdn.net/xxxlinttp/article/details/100080080