手写VIO学习总结（四）

系列笔记:
手写VIO学习总结（一）
手写VIO学习总结（二）
手写VIO学习总结（三）

1.作业1：信息矩阵与边缘化

2.作业2：BA信息矩阵代码实战

• 推导过程:
  

• 代码填充:

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;

• 验证零空间维度
  代码部分:

    Eigen::JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << svd.singularValues() <

• 打印结果:
  

发现最后七维接近0

• eigen的svd分解
  

JacobiSVD svd(J, ComputeThinU | ComputeThinV);
    U = svd.matrixU();
    V = svd.matrixV();
    A = svd.singularValues();

其中A 为Σ 对角线元素.