从零手写VIO(四)——信息矩阵

最新推荐文章于 2022-03-09 20:20:30 发布

薛定猫

最新推荐文章于 2022-03-09 20:20:30 发布

阅读量1.5k

点赞数 1

分类专栏： SLam 文章标签：人工智能算法机器学习自动驾驶线性代数

本文链接：https://blog.csdn.net/weixin_44456692/article/details/106891699

版权

SLam 专栏收录该内容

30 篇文章 33 订阅

订阅专栏

对从零手写VIO第四次作业进行总结

文章目录

1 信息矩阵与边缘化
- 1.1 信息矩阵
- 1.2 marg掉ξ1
2 BA信息矩阵计算

作业:

1 信息矩阵与边缘化

1.1 信息矩阵

由图中能够看到,我们的优化变量有6个,分别为ξ1,ξ2,ξ3,L1,L2,L3,误差项有9个,所以雅克比矩阵形式为:
$\bold { J=\frac{\partial r}{\partial \xi}=\begin{bmatrix} \frac{\partial r(\xi1,\xi 2)}{\partial \xi} \\ \\ \frac{\partial r(\xi1,L_1)}{\partial \xi} \\ \\ \frac{\partial r(\xi1,L_2)}{\partial \xi} \\ \\ \frac{\partial r(\xi2,\xi 3)}{\partial \xi} \\ \\ \frac{\partial r(\xi2,L_1)}{\partial \xi} \\ \\ \frac{\partial r(\xi2,L_2)}{\partial \xi} \\ \\ \frac{\partial r(\xi2,L_3)}{\partial \xi} \\ \\ \frac{\partial r(\xi3,L_2)}{\partial \xi} \\ \\ \frac{\partial r(\xi3,L_3)}{\partial \xi} \\ \\ \end{bmatrix} }$
雅克比矩阵具有稀疏性,拿其中的 $J_2$ 举例
$\bold{ J_2=\frac{\partial r(\xi1, L1)}{\partial \xi}=\begin{bmatrix} \frac{\partial r(\xi1,L_1)}{\partial \xi 1}& \frac{\partial r(\xi1,L_1)}{\partial \xi 2}& \frac{\partial r(\xi1,L_1)}{\partial \xi 3}& \frac{\partial r(\xi1,L_1)}{\partial L_1}& \frac{\partial r(\xi1,L_1)}{\partial L_2}& \frac{\partial r(\xi1,L_1)}{\partial L_3} \end{bmatrix} = \begin{bmatrix} \frac{\partial r(\xi1,L_1)}{\partial \xi 1}& 0& 0& \frac{\partial r(\xi1,L_1)}{\partial L_1}& 0& 0 \end{bmatrix} }$

$\underbrace{ \Lambda = J^T\varSigma^{-1} J}_{\text{H}}$
很容易就能得到信息矩阵的零项和非零项,
在这里插入图片描述

图片参考:https://blog.csdn.net/orange_littlegirl/article/details/103241537

试了几款画图工具,画出来的效果都不是很理想,最后使用MATLAB画出了效果还可以的矩阵块.
对MATLAB有兴趣的可以去看一下matlab绘制矩阵色块图

在这里插入图片描述

后记:
后来发现了一种更简单的方法,使用Ubuntu自带的libreOffice Impress也能画出这种图,而且更简单
在这里插入图片描述

1.2 marg掉ξ1

边缘化 $\xi_1$
边际概率: $P_a=\int P(a|b)db$ ,在这里, $b$ 就是 $\xi_1$

信息矩阵:
$\Lambda = \Lambda_{\alpha \alpha}-\Lambda_{\alpha \beta}\Lambda_{\beta \beta}^{-1}\Lambda_{\beta \alpha}$
其中, $\Lambda =J^T \varSigma^{-1}J$

在这里插入图片描述
结果

2 BA信息矩阵计算

2.1 雅克比计算

关于重投影误差雅克比矩阵14讲上有详细的推导过程
重投影误差对相机位姿李代数的导数:
$\frac{\partial e}{\partial \delta \xi}=-\left[\begin{array}{cccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} & -\frac{f_{x} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_{x}+\frac{f_{x} X^{2}}{Z^{\prime 2}} & -\frac{f_{x} Y^{\prime}}{Z^{\prime}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} & -f_{y}-\frac{f_{y} Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime}}{Z^{\prime}} \end{array}\right]$
重投影误差对空间特征点导数:

$\frac{\partial e}{\partial \boldsymbol{P}}=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] R$
代码实现:

double x = Pc.x();
double y = Pc.y();
double z = Pc.z();
double z_2 = z * z;
Eigen::Matrix<double,2,3> jacobian_uv_Pc;
jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                    0, fy/z, -y * fy/z_2;
Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;   // 误差关于特征点的导数
Eigen::Matrix<double,2,6> jacobian_Ti;
jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
-(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;   // 误差关于相机位姿李代数的导数

2.2 H的结构

H的结构和下面的类似
在这里插入图片描述

图片来源:视觉SLAM十四讲

填充代码:

H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;	//位姿对角线
/// 请补充完整作业信息矩阵块的计算
H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) += jacobian_Pj.transpose() *jacobian_Pj ;	// 空间点对角线
H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose() * jacobian_Pj;	// 非对角线
H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;

对于H矩阵的构建的一点理解:
$H=J^TJ$ ,所以后面H里面的block都是 jacobian_Ti.transpose()*jacobian_Ti 专业的形式。也就是子块的转置在前,而且所有的子块构建的时候都是带转置的部分在前。
关于H的block填充, i , j 坐标的理解:
首先,camera的维度是6,数量为N ;feature的维度是3,数量是M,所以整个H的维度是6×N+3×M;
H对角线上的block是方阵,所以対应每个block的真实位置为:

camera pose:(i*6,i*6) 
feature pose:(6*poseNums+j*3,6*poseNums+j*3)

关于block函数的四个参数

 
 * \param startRow the first row in the block
  * \param startCol the first column in the block
  * \param blockRows the number of rows in the block
  * \param blockCols the number of columns in the block

可以看到,前两个参数为子块在大block中的起始行数和列数,后两个参数为子块自身的维度

2.3 验证零空间维度

2.3.1 零空间问题

原因:
在这里插入图片描述
滑动窗口算法优化的时候,信息矩阵如公式(49)变成了两部分,且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化,从而在求解时引入错误信息。

对于 SLAM 系统而言(如单目 VO), 当我们改变状态量时,测量不变意味着损失函数不会改变,更意味着求解最小二乘时对应的信息矩阵 Λ 存在着零空间。
在这里插入图片描述

2.3.2 奇异值分解:

Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
std::cout << svd.singularValues() <<std::endl;

输出结果:
在这里插入图片描述
结论:最后7维确实接近于0.表明零空间的维度为7

关于奇异值分解
$J=U\varSigma V^T$
在Eigen里面:

JacobiSVD<MatrixXd> svd(J, ComputeThinU | ComputeThinV);
    U = svd.matrixU();
    V = svd.matrixV();
    A = svd.singularValues();

2.3.3 对策

First Estimated Jacobian, 即FEJ 算法:不同残差对同一个状态求雅克比时,线性化点必须一致。
这样就能避免零空间退化而使得不可观变量变得可观.

论文推荐: Tue-Cuong Dong-Si and Anastasios I Mourikis. “Consistency analysis for sliding-window visual odometry”. In: 2012IEEE International Conference on Robotics and Automation. IEEE. 2012, pp. 5202–5209

参考手写VIO学习总结（四）