从零开始手写VIO第五章作业（含理论说明及编程思维过程）

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

延续该系列博文的风格，本博文站在前人优秀博文的基础上，尽量补充同主题博文所没有涉及到的关键点。之前同主题优秀博文已经讲解非常清楚的部分，本博文中将直接给出链接，对于之前博文没有涉及到的点，本博文会重点阐述。除此之外，还包含小编在学习过程中遇到的问题，欢迎有兴趣的朋友讨论交流。
对于第五章，所补充的关键点包含以下几个方面：代码和课件中的公式符号不统一的地方，第一题和第二题舒尔补公式形式的区别，论文总结题原论文中不易理解公式的说明，提升题第二题的原始代码中prior对应的残差和雅克比矩阵计算的说明及作业代码的说明。

1.完成单目 Bundle Adjustment (BA) 求解器 problem.cc 中的部分代码

完成本章所有作业的前提是至少看懂整个代码框架及部分代码细节，这里小编推荐一篇对代码分析相当详细的博文：[从零手写VIO|第五节]——后端优化实践——单目BA求解代码解析。

1.1完成 Problem::MakeHessian() 中信息矩阵 H 的计算

先直接给出代码：

// TODO:: home work. 完成 H index 的填写.
H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
if (j != i) {
   
   
	// 对称的下三角
	// TODO:: home work. 完成 H index 的填写.
    H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();
}

代码中关于函数noalias()的相关说明请参考博文：Eigen中的noalias()： 解决矩阵运算的混淆问题。
代码中涉及到的理论可以参考高翔《视觉SLAM十四讲》P250的图（10.3）和公式（10.52）前后的内容，如下图：

针对本问题如何具体应用上述理论具体可以参考博文从零手写VIO(五)或者其它同主题博文中的相关内容。

1.2完成 Problem::SolveLinearSystem() 中 SLAM 问题的求解

1.2.1理论及代码

这里的理论参考如下：

有几点需要强调。第一、上面图中的角标l在代码中替换为了m；第二、代码中所有与b相关的变量其实已经包含了负号，而上图中的公式没有包含，所以再应用上图公式的时候，只需要将与b相关的公式的符号都取反即可（包括bp和bl）。
第一部分代码如下，其中要说明的是，正如下面代码最开始部分所定义，reserve_size = ordering_poses_，marg_size = ordering_landmarks_，代码中这两组变量选择哪一组都可以。小编选择了ordering_poses_和 ordering_landmarks_。

// SLAM 问题采用舒尔补的计算方式
// step1: schur marginalization --> Hpp, bpp
int reserve_size = ordering_poses_;
int marg_size = ordering_landmarks_;
// TODO:: home work. 完成矩阵块取值，Hmm，Hpm，Hmp，bpp，bmm
MatXX Hmm = Hessian_.block(ordering_poses_, ordering_poses_, ordering_landmarks_, ordering_landmarks_);
MatXX Hpm = Hessian_.block(0, ordering_poses_, ordering_poses_, ordering_landmarks_);
MatXX Hmp = Hessian_.block(ordering_poses_, 0, ordering_landmarks_, ordering_poses_);
VecX bpp = b_.segment(0, ordering_poses_);
VecX bmm = b_.segment(ordering_poses_, ordering_landmarks_);

第二部分代码如下：

// TODO:: home work. 完成舒尔补 Hpp, bpp 代码
MatXX tempH = Hpm * Hmm_inv;
H_pp_schur_ = Hessian_.block(0, 0, ordering_poses_, ordering_poses_) - tempH * Hmp;
b_pp_schur_ = bpp - tempH * bmm;

第三部分代码如下：

// TODO:: home work. step3: solve landmark
VecX delta_x_ll(marg_size);
delta_x_ll = Hmm_inv * (bmm - Hmp * delta_x_pp)