[从零写VIO|第五节]——后端优化实践：逐行手写求解器(作业)

1.1 信息矩阵H的计算

对于每一条边都要进行这样两个for循环，分别是遍历方阵H的行和列，第一个i的for循环是行数，当遍历到行列相等，也就是对角的时候，只用放入一次hessian，而其他情况下都要放入关于对角对称的两个hessian，这是由于这两个位置的hessian是转置的关系。而b则在行遍历的时候一行行填入。

MatXX hessian = JtW * jacobian_j;                
// 所有的信息矩阵叠加起来                
// 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();                     
    }          

1.2. SLAM问题的求解
将问题转换成normal equation,其中，$H_{pp}$表示相机位姿之间的关系，下图中$H_{ll}$对应代码中的$H_{mm}$表示路标点之间的关系，$H_{pl}$对应代码中的$H_{pm}$表示相机和路标之间的关系。

矩阵块的取值：

// TODO:: home work. 完成矩阵块取值，Hmm，Hpm，Hmp，bpp，bmm：landmark        
MatXX Hmm = Hessian_.block(reserve_size,reserve_size, marg_size, marg_size);        
MatXX Hmp = Hessian_.block(reserve_size,0, marg_size, reserve_size);        
MatXX Hpm = Hessian_.block(0,reserve_size,reserve_size, marg_size);        
VecX bmm = b_.segment(reserve_size,marg_size );        
VecX bpp = b_.segment(0,reserve_size);

完成舒尔补代码：

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

求解路标点$\delta x_l$：

// TODO:: home work. step3: solve landmark        
VecX delta_x_ll(marg_size);        
delta_x_ll =  Hmm_inv * (bmm - Hmp * delta_x_pp) ;        
delta_x_.tail(marg_size) = delta_x_ll; // x_ll 路标点信息

2. 滑动窗口算法测试函数
   
   其信息矩阵为：

现在要从这个例子中marg掉$x_2$(绿色部分)，求解之后的信息矩阵。需要将绿色部分移动到信息矩阵的右下角，具体操作为先二、三行互换，再二、三列互换。然后进行舒尔补运算，marg掉$x_2$。

// 将 row i 移动矩阵最下面    
Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size); //第二行    
Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size); // 第三行    
H_marg.block(idx, 0,reserve_size - idx - dim, reserve_size) = temp_botRows; // 二、三行颠倒    
H_marg.block(reserve_size - dim, 0, dim, reserve_size) = temp_rows;

舒尔补运算：

// TODO:: home work. 完成舒尔补操作    
Eigen::MatrixXd Arm = H_marg.block(0,n2,n2,m2);    
Eigen::MatrixXd Amr = H_marg.block(n2,0,m2,n2);    
Eigen::MatrixXd Arr = H_marg.block(0,0,n2,n2);
    
Eigen::MatrixXd tempB = Arm * Amm_inv;    
Eigen::MatrixXd H_prior = Arr - tempB * Amr;

编译说明：

cd BA_schur  \\ 进入文件夹
mkdir build   
cd build
cmake ..
make -j4    
cd app
./testMonoBA

运行结果如下：

【提升题】论文总结
在我的另一篇博客,指路

另，关于BA的相关代码解析在我的另一篇博客——指路