从零开始手写VIO第四章作业（含关键点的参考资料）

前言·与同主题博文的不同

延续该系列博文的风格，本博文站在前人优秀博文的基础上，尽量补充同主题博文所没有涉及到的关键点。对于第四章，所补充的关键点包含以下几个方面：第2题具体需要证明什么及对应的参考资料链接；第3题中源代码和需要补充的代码依赖的理论依据具体对应于高翔《视觉SLAM十四讲》的哪一页内容。有任何问题都欢迎大家在评论区批评交流。

1.信息矩阵

1.1绘制信息矩阵Λ

课件中的算例以及网上已有的博文都是画出各个子信息矩阵，最后通过“相加”得到最终的信息矩阵。这里用高翔《视觉SLAM十四讲》P252中提到的“邻接矩阵”来说明如何跳过前面的过程直接画出最终的信息矩阵（两种方法的本质是一样的，后者更快捷方便）。书中对邻接矩阵的描述见下图红框中的内容：

通过邻接矩阵的概念，可以快速画出非对角线元素，而对角线元素上的数量就是每行（或者每列）非对角线元素数量之和。所以直接画出最终的信息矩阵如下图：

1.2绘制相机ξ₁被marg以后的信息矩阵Λ′

参考课件中的算例，被marg以后的信息矩阵绘制如下：

2.证明信息矩阵和协方差的逆之间的关系

2.1明确问题

第2题要求阅读《Relationship between the Hessian and Covariance Matrix for Gaussian Random Variables》，证明信息矩阵和协方差的逆之间的关系。
通过阅读该文献可知，该文献给出了结论：多元高斯概率密度函数对应的负的对数似然（Log Likelihood）函数的Hessian矩阵与协方差矩阵的逆相等。文中的证明过程如下图：

所以下面只需要证明信息矩阵与Hessian矩阵相等即可。下面的2.2中给出给证明。

2.2证明过程

对于不同资料中所提到的“负的对数似然（Log Likelihood）函数的Hessian矩阵”与“对数似然（Log Likelihood）函数的负的Hessian矩阵”是相等的，本文中不再区别这两个概念。
外文文章《Fisher Information and the Hessian of Log Likelihood》给出了信息矩阵与对数似然（Log Likelihood）函数的负的Hessian矩阵相等的证明。文中的证明过程如下图：

由于小编目前水平有限，并不能完全理解外文文章中这部分证明内容，这里先给出调研到的这部分资料，之后慢慢理解消化。也欢迎感兴趣的博友讨论交流。在这里给出引出这篇外文的博客园文章【原创】极大似然估计中，信息矩阵、Hessian矩阵和协方差矩阵的关系。
结合2.1和2.2，即可完成整个证明过程。

3.作业代码中单目 Bundle Adjustment 信息矩阵的计算

3.1雅克比矩阵的计算

源代码中计算了两个雅克比矩阵，第一个是jacobian_Pj，表示重投影误差（观测方程）对特征点（表示在世界坐标系下）的雅克比矩阵，第二个是jacobian_Ti，表示重投影误差（观测方程）对相机位姿的雅克比矩阵。作业中需要用这两种雅克比矩阵来求解信息矩阵H。
源代码中的相关部分如下：

    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;  

计算jacobian_Pj的理论依据是高翔《视觉SLAM十四讲》P167的公式（7.47），如下图：

计算jacobian_Ti的理论依据是高翔《视觉SLAM十四讲》P167的公式（7.45），如下图：

正如书中所说，源代码中表示位姿的变量camera_pose是旋转在前，平移在后，所以需要把公式（7.45）中的前三列与后三列对调。

3.2补充计算信息矩阵H的代码

这部分需要用到的理论知识可以参考高翔《视觉SLAM十四讲》P250的图（10.3）和公式（10.52）前后的内容，如下图：

下面先给出代码，再将理论和代码结合起来说明。需要补充的代码如下：

    H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;                                                                                                
  	H.block(i*6,poseNums*6+j*3,6,3) += jacobian_Ti.transpose() * jacobian_Pj;        
  	H.block(poseNums*6+j*3,i*6,3,6) += jacobian_Pj.transpose() * jacobian_Ti;        
    H.block(poseNums*6+j*3,poseNums*6 +j*3,3,3) += jacobian_Pj.transpose() * jacobian_Pj;

代码中的H矩阵在循环过程中不断自增求和，对应公式（10.52），每次循环所需要计算的四个矩阵块对应于图（10.3）中的四个矩阵块。
block函数前两个参数为子块在大block中的起始行数和列数，后两个参数为子块自身的维度。

3.3仿真结果

编译运行得到的仿真结果如下：

从上图可以看出，奇异值最后7维接近于0，即零空间维度为7。