从零手写vio-作业5-后端优化实践-逐行手写求解器

1

在MakeHseesian()函数中;

 // 所有的信息矩阵叠加起来
 // TODO:: home work. 完成 H index 的填写.
 // 确定不会有错误的产生，那么可以使用noaliasd()函数来避免产生临时变量,使用noalias()函数来声明这里没有混淆直接赋值
 //这里index_i、index_j是对应矩阵块的位置，i代表行。j代表列 此时对应的是对角线部分加上三角部分
 H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
 if (j != i) {
 // 对称的下三角  index_j, index_i对应的值下三角的行和列
 // TODO:: home work. 完成 H index 的填写.
     H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();//
 }

在SolveLinearSystem()函数中:

        // 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(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);

        // Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
        MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
        for (auto landmarkVertex : idx_landmark_vertices_) {
            int idx = landmarkVertex.second->OrderingId() - reserve_size;
            int size = landmarkVertex.second->LocalDimension();
            Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
        }

        // 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 * bpp;

        // step2: solve Hpp * delta_x = bpp
        VecX delta_x_pp(VecX::Zero(reserve_size));
        // PCG Solver
        for (ulong i = 0; i < ordering_poses_; ++i) {
            H_pp_schur_(i, i) += currentLambda_;
        }

        int n = H_pp_schur_.rows() * 2;                       // 迭代次数
        delta_x_pp = PCGSolver(H_pp_schur_, b_pp_schur_, n);  // 哈哈，小规模问题，搞 pcg 花里胡哨
        delta_x_.head(reserve_size) = delta_x_pp;
        //        std::cout << delta_x_pp.transpose() << std::endl;

        // 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;

PCG求解：添加链接描述

2

在TestMarginalize()函数中：

    // TODO:: home work. 将变量移动到右下角
    /// 准备工作： move the marg pose to the Hmm bottown right
    // 将 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;

    /// 开始 marg ： schur
    double eps = 1e-8;
    int m2 = dim;
    int n2 = reserve_size - dim;   // 剩余变量的维度
    Eigen::MatrixXd Amm = 0.5 * (H_marg.block(n2, n2, m2, m2) + H_marg.block(n2, n2, m2, m2).transpose());

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd(
            (saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
                              saes.eigenvectors().transpose();

    // 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;

补充：参考博客 添加链接描述
代码中求Amm的逆Amm_inv用到了特征值分解的方法
其中Eigen::SelfAdjointEigenSolver解释为：
A matrix A A A is selfadjoint if it equals its adjoint. For real matrices, this means that the matrix is symmetric: it equals its transpose.
这里感觉应该说的是共轭矩阵(Hermite矩阵)而不是自伴随矩阵（伴随矩阵是那个由代数余子式构成的与逆矩阵有关的矩阵）