SLAM中的零空间问题

0. 引言
1.矩阵零空间
2.一个解题的例子
3.SLAM系统中的可观性定义
4.SLAM中的零空间问题
5.解决
6.update

0. 引言

首先回顾一下张宇讲的。对Ax=0Ax=0Ax=0、矩阵AAA.

线性相关，有非0解，不是满秩，行列式为0，有多余向量。
线性无关，只有0解，满秩，行列式不为0，没有多余向量。

1.矩阵零空间

对于矩阵AAA，所有满足Ax=0Ax=0Ax=0,的向量xxx组成的集合NNN，可以证明NNN包含零向量，且对线性运算封闭，因此NNN是一个向量子空间，这个子空间叫做矩阵A的零空间。

求矩阵的零空间，就是求方程组 Ax=0Ax = 0Ax=0 的解空间。

矩阵可以看做一组列向量 C1,C2,...,CnC_1,C_2,...,C_nC1,C2,...,Cn，那么如果这组向量是线性无关的，那么AX=0AX=0AX=0的解空间只包含一个向量：零向量。反之，如果零空间包含非零向量，说明矩阵的列向量线性相关。

2.一个解题的例子

解线性方程组：
{x1+2x2+3x3+x4=52x1+4x2−x4=−3−x1−2x2+3x3+2x4=8x1+2x2−9x3−5x4=−21\left\{\begin{array}{c}{x_{1}+2 x_{2}+3 x_{3}+x_{4}=5} \\ {2 x_{1}+4 x_{2} \quad-x_{4}=-3} \\ {-x_{1}-2 x_{2}+3 x_{3}+2 x_{4}=8} \\ {x_{1}+2 x_{2}-9 x_{3}-5 x_{4}=-21}\end{array}\right. ⎩⎪⎪⎨⎪⎪⎧x1+2x2+3x3+x4=52x1+4x2−x4=−3−x1−2x2+3x3+2x4=8x1+2x2−9x3−5x4=−21
解：对方程组的增广矩阵A‾\overline{\mathbf{A}}A作初等行变换有：

A‾=(12315240−1−3−1−232812−9−5−21)\overline{A}=\left(\begin{array}{ccccc}{1} & {2} & {3} & {1} & {5} \\ {2} & {4} & {0} & {-1} & {-3} \\ {-1} & {-2} & {3} & {2} & {8} \\ {1} & {2} & {-9} & {-5} & {-21}\end{array}\right)A=⎝⎜⎜⎛12−1124−22303−91−12−55−38−21⎠⎟⎟⎞→(120−12−32001121360000000000)\rightarrow\left(\begin{array}{ccccc}{1} & {2} & {0} & {-\frac{1}{2}} & {-\frac{3}{2}} \\ {0} & {0} & {1} & {\frac{1}{2}} & {\frac{13}{6}} \\ {0} & {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} & {0}\end{array}\right)→⎝⎜⎜⎛100020000100−212100−2361300⎠⎟⎟⎞

通过画阶梯线可以知道，该方程组的自由变量为x2x_2x2、x4x_4x4两个自由变量不可控（SLAM中的自由度，不可观状态量），故其解也不唯一，只能求解通解。后面的就不求解了，讲到这儿就和SLAM系统联系上了！！更进一步，线性代数中的自由变量个数=n−Rank(A)n-Rank(A)n−Rank(A),其中nnn为xxx变量的个数。

3.SLAM系统中的可观性定义

对于测量系统z=h(θ)+ε\mathbf{z}=h(\boldsymbol{\theta})+\varepsilonz=h(θ)+ε，其中z∈Rn\mathbf{z} \in \mathbb{R}^{n}z∈Rn为测量值，θ∈Rd\boldsymbol{\theta} \in \mathbb{R}^{d}θ∈Rd为系统状态量, ε\varepsilonε为测量噪声向量。h(⋅)h(\cdot)h(⋅) 是个非线性函数，将状态量映射成测量。对于理想数据，如果以下条件成立，则系统状态量θ\boldsymbol{\theta}θ可观：
∀θ,∀θ′∈Rd,{θ≠θ′}⇒{h(θ)≠h(θ′)}\forall \theta, \forall \theta^{\prime} \in \mathbb{R}^{d},\left\{\theta \neq \theta^{\prime}\right\} \Rightarrow\left\{h(\theta) \neq h\left(\theta^{\prime}\right)\right\}∀θ,∀θ′∈Rd,{θ=θ′}⇒{h(θ)=h(θ′)}（简单的讲就是状态量变，观测值就变）“ ≠\neq=“这个是不等于符号，显示出来有问题！

对于SLAM系统而言（如单目VO），当我们改变状态量时，测量不变意味着损失函数不会改变，更意味着求解最小二乘时对应的信息矩阵Λ\LambdaΛ存在着零空间。

4.SLAM中的零空间问题

只做逻辑分析，不做理论推导。证明麻烦的是，旋转参数化比较难。因为虽然说旋转只有3个自由度，但是参数化形式并没有三自由度的表达形式(四元数不是矩阵形式了，另外rpy的形式不知道能不能用)，这样就很难直接从矩阵的形式直接说明自由度的问题。故只动嘴皮子，理论推导希望大佬指点。

单目SLAM系统有7自由度不可观：6自由度姿态+尺度。
单目+IMU系统有4自由度不可观：yaw角+3自由度位置不可观。roll和pitch由于重力的存在而可观，尺度因子由于加速度计的存在而可观。

在求解增量方程中，δx\delta xδx就是我们需要求解的状态量，从结果逆推，若是VO系统状态量中我们已经知道结果为7个不可观自由度（即是二小节中的自由变量），那么逆推就可以知道Hessian矩阵存在一个7维的零空间，Hessian矩阵一定不满秩。而在滑动窗口的Margin过程中会导致Hessian矩阵的零空间变化，会造成求解出的δx\delta xδx产生漂移，导致系统漂移！

比如: 求解单目 SLAM 进行 Bundle Adjustment 优化时，问题对应的信息矩阵Λ\LambdaΛ 不满秩，对应的零空间为 N\mathbf{N}N, 用高斯牛顿求解时有：
Λδx=b\mathbf{\Lambda }\delta \mathbf{x}=\mathbf{b}Λδx=bΛδx+Nδx=b\mathbf{\Lambda }\delta \mathbf{x}+\mathbf{N}\delta \mathbf{x}\text{ }=\mathbf{b} Λδx+Nδx =b
增量δx\delta xδx在零空间维度下变化，并不会改变我们的残差。这意味着系统可以有多个满足最小化损失函数的解 x。而认为的加以干预很容易导致漂移！

5.解决

First Estimate Jacobian(FEJ)算法！

6.update

@larry_dongy 评论指正：
　　 “5. 解决。FEJ算法”，我个人觉得有误导性。（1）FEJ确实解决的是零空间的某些问题，但具体而言是在线性化时通过固定线性化点避免零空间维度的减少，从而避免引入错误信息而造成误差；（2）而零空间内的漂移，不是由于零空间塌缩导致的，而是由于优化求解过程中导致的（这部分可以参考这篇文章，里面的推导，感觉不错）。避免零空间漂移的方法，正交投影保留垂直部分增量。（3）按照我的理解，JEF和零空间漂移貌似是两个问题，博主可以再考虑一下？也避免其他看到这里的朋友产生误会。