习题一:基础题-代码编写

1、完成单目Bundle Adjustment求解器problem.cc中的部分代码。

  • 完成Problem::MakeHessian()中信息矩阵H的计算。
  • 完成Problem::SolveLinearSystem()SLAM问题的求解。

2、完成滑动窗口算法测试函数。

  • 完成 Problem::TestMarginalize() 中的代码,并通过测试。

答案

1、H矩阵的补充,按照顺序填写相应的矩阵块即可。另外一个问题,求解SLAM问题,需要注意,b代码中和PPT里面不太一样,多了一个符号,其余的就是注意有的编号已经包含维度信息了。具体的代码如下:

//void Problem::MakeHessian() 
H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian;
if (j != i) 
{
    // 对称的下三角
    H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();
}
...

b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
//void Problem::SolveLinearSystem()
MatXX Hmm = Hessian_.block(reserve_size,reserve_size, marg_size, marg_size);
MatXX Hpm = Hessian_.block(0,reserve_size, reserve_size, marg_size);
MatXX Hmp = Hessian_.block(reserve_size,0, marg_size, reserve_size);
VecX bpp = b_.segment(0,reserve_size);
VecX bmm = b_.segment(reserve_size,marg_size);

...

MatXX tempH = Hpm * Hmm_inv;
H_pp_schur_ = Hessian_.block(0,0,reserve_size,reserve_size) - tempH * Hpm.transpose();
b_pp_schur_ = bpp - tempH * bmm;
...

VecX delta_x_ll(marg_size);
delta_x_ll = Hmm_inv*(bmm-Hpm.transpose()*delta_x_pp);
delta_x_.tail(marg_size) = delta_x_ll;

2、滑动窗口算法测试函数的修改如下:

//void Problem::TestMarginalize() 
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;

// 将 col i 移动矩阵最右边
Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
Eigen::MatrixXd temp_rightCols = H_marg.block(0, idx + dim, reserve_size, reserve_size - idx - dim);
H_marg.block(0, idx, reserve_size, reserve_size - idx - dim) = temp_rightCols;
H_marg.block(0, reserve_size - dim, reserve_size, dim) = temp_cols;

...

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;

上述两个问题的结果,如下图所示:

习题二:提升题-论文阅读

请总结论文On the comparison of gauge freedom handling in optimization-based visual-inertial state estimation中,优化过程中处理H自由度的不同操作方式。总结内容包括:具体处理方式,实验效果,结论。

答案

论文中关于优化过程中处理H自由度的不同操作方式共有三种方法gauge fixation approachgauge prior approachfree gauge approach,具体的分析如下。

1、具体的处理方式:

  • gauge fixation approach通过讲雅克比矩阵设为零来固定第一帧的优化参数。
  • free gauge approach对于奇异H矩阵使用伪逆或者LM的方法进行求解。
  • gauge prior approach通过在误差函数中加入一个prior error,即第一帧位置初值与优化值的误差,并分配权重wp,当wp=0时,相当于free gauge approach,当wp趋于无穷大时,相当于gauge fixation approach

2、处理效果: 三种方法有相似的性能,但free gauge approach的计算效率更快,迭代次数少,易收敛。

3、实验效果及结论: 三种方式的优化性能效果类似

  • gauge fixation approach由于实际需要优化参数维度比其他两种方式少,因此每次迭代所需的计算时间最少;
  • free gauge approach的迭代次数少,且容易收敛,所需的计算时间最少。
  • Gauge Prior approach中,权重大于一定阈值后,RMSE稳定,迭代次数和收敛时间稳定。不同的先验权重对于算法的精度影响较小,对计算成本有影响,选择合适的先验权重可以加快计算速度。