深蓝学院《从零开始手写VIO》作业四

深蓝学院《从零开始手写VIO》作业四

1. 信息矩阵分析

某时刻，SLAM系统中相机和路标点的观测关系如下图所示:

(1) 请绘制上述系统的信息矩阵$\Lambda$
(2) 请绘制相机${\xi }_1$marg以后的信息矩阵${\Lambda }^{\prime }$

(1) 信息矩阵$\Lambda$如下：

(2) marg以后的信息矩阵${\Lambda }^{\prime }$如下：

2. Bundle Adjustment信息矩阵的计算

请补充作业代码中单目Bundle Adjustment信息矩阵的计算，并输出正确的结果

代码补充如下：

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;

结果输出如下(这里只给出最后十行结果)：

...
 0.00230246
 0.00172459
0.000422374
3.21708e-17
2.06732e-17
1.43188e-17
7.66992e-18
6.08423e-18
6.05715e-18
3.94363e-18