MSCKF中左零空间的计算

问题

如下公式(1)对应MSCKF论文中的公式(23)，其表示特征$j$与所有观测到该特征的状态构成的观测方程，但这个方程中的 ${}^G{\tilde{\mathbf{p}}}_j$ 是由状态 $\tilde{\mathbf{X}}$ 推导出来的，于是这两类状态之间存在耦合，这与EKF所要求的观测方程不一致，因此需要调整该观测方程。
$${\mathbf{r}}^{(j)}\simeq {\mathbf{H}}_{\mathbf{x}}^{(j)}\tilde{\mathbf{X}}+{\mathbf{H}}_f^{(j)}{}^G{\tilde{\mathbf{p}}}_j+{\mathbf{n}}^{(j)}\text{\hspace{1em}(1)}$$
调整的策略是只保留状态 $\tilde{\mathbf{X}}$ 而将 ${}^G{\tilde{\mathbf{p}}}_j$ 移除，那自然会想到在公式(1)左右两侧同时乘以"某个矩阵"使得右侧第二项直接变成 $0$，而"某个矩阵"如果是 ${\mathbf{H}}_f^{(j)}$ 的左零空间构成的矩阵既可以达到这个效果。于是调整后观测方程为:
$$\begin{array}{rl}{\mathbf{A}}^T{\mathbf{r}}^{(j)}& \simeq {\mathbf{A}}^T{\mathbf{H}}_{\mathbf{x}}^{(j)}\tilde{\mathbf{X}}+{\mathbf{A}}^T{\mathbf{H}}_f^{(j)}{}^G{\tilde{\mathbf{p}}}_j+{\mathbf{A}}^T{\mathbf{n}}^{(j)}\\ {\mathbf{r}}_o^{(j)}& \simeq {\mathbf{H}}_o^{(j)}\tilde{\mathbf{X}}+{\mathbf{n}}_o^{(j)}\end{array}\text{\hspace{1em}(2)}$$
式中 ${\mathbf{A}}^T$即为 ${\mathbf{H}}_f^{(j)}$ 的左零空间构成的矩阵。

注: 零空间是满足$\mathbf{A}\mathbf{x}=0$的所有$\mathbf{x}$组成的空间，而左零空间是满足${\mathbf{x}}^T\mathbf{A}=0$$({\mathbf{A}}^T\mathbf{x}=0)$的所有$\mathbf{x}$组成的空间

原理

假设特征点 ${\mathbf{p}}_j$ 被n个相机位姿看到，则可知 ${\mathbf{H}}_f^{(j)}$ 的维度为 $2n\times 3（n>1）$，于是可对 ${\mathbf{H}}_f^{(j)}$ 进行QR分解得:

其中$\mathbf{Q}$为正交矩阵，即该矩阵的所有列向量彼此正交，于是可知 ${\mathbf{Q}}_2$ 任意一列 ${\mathbf{q}}_{2i}$ 满足${\mathbf{q}}_{2i}^T{\mathbf{Q}}_1=0$，根据左零空间的定义可知矩阵${\mathbf{Q}}_2$的所有列向量构成的空间即为 ${\mathbf{Q}}_1$ 的左零空间，所以可得 ${\mathbf{Q}}_2^T{\mathbf{Q}}_1=0$，于是 ${\mathbf{Q}}_2^T$ 即为公式(2)中的 ${\mathbf{A}}^T$.

实现

利用eigen中的qr分解对象，按如下计算过程获得矩阵 ${\mathbf{H}}_f$ 的左零空间:

Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(H_f.rows(), H_f.cols());
qr.compute(H_f);
Eigen::MatrixXd Q = qr.householderQ();
Eigen::MatrixXd Q1 = Q.block(0,0,Q.rows(),3);
Eigen::MatrixXd Q2 = Q.block(0,3,Q.rows(),Q.cols()-3);

参考

MSCKF论文
MSCKF Nullspace Projection
零空间1
零空间2
QR分解1
QR分解2