VIO第5讲后端优化实践：逐行手写求解器

1 非线性最小二乘求解流程

1.1 H矩阵不满秩的解决办法

解决办法1：使用LM算法，加阻尼因子使得系统满秩！可求解，但是求得的结
果可能会往零空间变化。结果之间的相对关系是不会发生变化的，但是整体可能与实际结果都差一个相对变换T，所以需要我们变换到原始结果上。

解决办法2：添加先验约束，增加系统的可观性。g2o、ceres中对系统的第一个pose的信息矩阵加上单位阵${\mathbf{H}}_{[11]}+=\mathbf{I}.$

解决办法3：固定一个相机pose和一个特征点，或者固定两个相机pose，限定优化值不乱飘。

    // 所有 Pose
    vector<shared_ptr<VertexPose> > vertexCams_vec;
    for (size_t i = 0; i < cameras.size(); ++i) {
        shared_ptr<VertexPose> vertexCam(new VertexPose());
        Eigen::VectorXd pose(7);
        pose << cameras[i].twc, cameras[i].qwc.x(), cameras[i].qwc.y(), cameras[i].qwc.z(), cameras[i].qwc.w(); //平移和四元数
        vertexCam->SetParameters(pose);

            //    if(i < 2)		// 这里就是解决办法3
            //        vertexCam->SetFixed();

        problem.AddVertex(vertexCam);
        vertexCams_vec.push_back(vertexCam);
    }

1.2 H矩阵的构建

海塞矩阵的构建主要有一下流程

1.2.1 确定维度

1 确定维度-----优化变量的维度n = 6p+3l，残差维度m，则单个残差雅可比J（1*n），推断出单个H（n*n）。然后把m个残差对应的H矩阵叠加拼接！

  ordering_generic_ += vertex.second->LocalDimension(); 是所有优化变量的总维度，也是H矩阵的维度！

  MatXX H(MatXX::Zero(ordering_generic_, ordering_generic_))

  其中LocalDimension()是优化变量要优化的维度，比如位姿，如果用四元数表示，输入参数维度是7，但实际优化维度是6！

// 统计优化变量的维数，为构建 H 矩阵做准备
void Problem::SetOrdering() {

    // 每次重新计数
    ordering_poses_ = 0;
    ordering_generic_ = 0;
    ordering_landmarks_ = 0;
    int debug = 0;

    // Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
    for (auto vertex: verticies_) {
        ordering_generic_ += vertex.second->LocalDimension();  // 所有的优化变量总维数

        if (IsPoseVertex(vertex.second)) {
            debug += vertex.second->LocalDimension();
        }

        if (problemType_ == ProblemType::SLAM_PROBLEM)    // 如果是 slam 问题，还要分别统计 pose 和 landmark 的维数，后面会对他们进行排序
        {
            AddOrderingSLAM(vertex.second);
        }

        if (IsPoseVertex(vertex.second)) {
            std::cout << vertex.second->Id() << " order: " << vertex.second->OrderingId() << std::endl;
        }
    }
    
        if (problemType_ == ProblemType::SLAM_PROBLEM) {
        // 这里要把 landmark 的 ordering 加上 pose 的数量，就保持了 landmark 在后,而 pose 在前
        ulong all_pose_dimension = ordering_poses_;
        for (auto landmarkVertex : idx_landmark_vertices_) {
            landmarkVertex.second->SetOrderingId(
                landmarkVertex.second->OrderingId() + all_pose_dimension
            );
        }
    }
}

  ordering_poses_是海塞矩阵中每个优化姿态的实际索引，因为每一个姿态占6维，所以要每次更新其在H中的左上角索引！AddOrderingSLAM会对其每一个位姿顶点进行更新，并按照这个顺序对位姿顶点进行排序idx_pose_vertices_！

void Problem::AddOrderingSLAM(std::shared_ptr<myslam::backend::Vertex> v) {
    if (IsPoseVertex(v)) {  // Pose顶点
        v->SetOrderingId(ordering_poses_);
        idx_pose_vertices_.insert(pair<ulong, std::shared_ptr<Vertex>>(v->Id(), v));
        ordering_poses_ += v->LocalDimension();     // 顶点实际优化的维度，比如姿态就是6+6+6+6+...
    } else if (IsLandmarkVertex(v)) {
        v->SetOrderingId(ordering_landmarks_);
        ordering_landmarks_ += v->LocalDimension();
        idx_landmark_vertices_.insert(pair<ulong, std::shared_ptr<Vertex>>(v->Id(), v));
    }
}

1.2.2 构建海塞矩阵

  这里详细的分析下海塞矩阵的构建，以下面这个图为例

  一个误差边edge，图中显示连接两个顶点，即二元边。

  以误差$r_{13}$为例，std::vector<MatXX> jacobians = edge.second->Jacobians()取出雅可比矩阵，是一个vcector向量，比如这里size = 2 = 顶点数，一个是对顶点1，一个是对顶点3。assert也可验证。

  为了方便写代码，这里并不是直接用${\mathbf{J}}_2^{\top }{\mathbf{\Sigma }}_2^{-1}\mathbf{J}$这种直接相乘的，而是进行了拆分，如下图

  实际代码中两个for循环计算的hessian是6*6或3*3或6*3或3*6小雅可比矩阵，通过找到其在大雅可比矩阵中的索引，进行填充。

H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian;

  index_i,index_j就是两个顶点对应的索引6p+3l，dim_i, dim_j就是有多种组合，看是位姿与位姿约束，或位姿与路标点约束等。

  H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();这里有个小技巧，因为H是对称的，我们计算(i,j)时，可以直接填写(j,i)。

  最后要注意的一点就是残差b的计算，就像纸上面写的，只在i这一循环计算

void Problem::MakeHessian() {
    TicToc t_h;
    // 直接构造大的 H 矩阵
    ulong size = ordering_generic_;             // 6n
    MatXX H(MatXX::Zero(size, size));           // 6n*6n
    VecX b(VecX::Zero(size));                   // 6n*1

    for (auto &edge: edges_) {

        edge.second->ComputeResidual();
        edge.second->ComputeJacobians();

        auto jacobians = edge.second->Jacobians();      // std::vector<MatXX>
        auto verticies = edge.second->Verticies();

        // 因为对一个优化变量的雅可比就是一个矩阵了，所以对所有优化变量的雅可比用vector来统计
        assert(jacobians.size() == verticies.size());   // std::vector<MatXX>大小应该是与顶点数相同的

        for (size_t i = 0; i < verticies.size(); ++i) {
            auto v_i = verticies[i];
            if (v_i->IsFixed()) continue;    // Hessian 里不需要添加它的信息，也就是它的雅克比为 0

            auto jacobian_i = jacobians[i];
            ulong index_i = v_i->OrderingId();	// 顶点在H实际索引
            ulong dim_i = v_i->LocalDimension();// 优化参数维度6/3

            MatXX JtW = jacobian_i.transpose() * edge.second->Information();
            for (size_t j = i; j < verticies.size(); ++j) {
                auto v_j = verticies[j];

                if (v_j->IsFixed()) continue;

                auto jacobian_j = jacobians[j];
                ulong index_j = v_j->OrderingId();
                ulong dim_j = v_j->LocalDimension();

                assert(v_j->OrderingId() != -1);
                MatXX hessian = JtW * jacobian_j;
                // 所有的信息矩阵叠加起来
                // TODO:: home work. 完成 H index 的填写.
                // H.block(?,?, ?, ?).noalias() += hessian;
                H.block(index_i,index_j, dim_i, dim_j).noalias() += hessian;

                // 对称矩阵，我们从j=i开始遍历，实际上只能遍历矩阵的一半，同时利用对称矩阵的性质，构建对称部分！
                if (j != i) {   // 对称的下三角
                    
		            // TODO:: home work. 完成 H index 的填写.
                    // H.block(?,?, ?, ?).noalias() += hessian.transpose();
                    H.block(index_j,index_i, dim_j, dim_i).noalias() += hessian.transpose();
                }
            }
            b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
        }

    }
    Hessian_ = H;
    b_ = b;
    t_hessian_cost_ += t_h.toc();

    if (err_prior_.rows() > 0) {
        b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_);   // update the error_prior
    }
    Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_;
    b_.head(ordering_poses_) += b_prior_;

    delta_x_ = VecX::Zero(size);  // initial delta_x = 0_n;

}

1.3 初始化μ—LM算法

$${\mu }_0=\tau \cdot \max \left\{{\left({\mathbf{J}}^{\top }\mathbf{J}\right)}_{ii}\right\}$$

void Problem::ComputeLambdaInitLM() {
    
	...
    
    for (ulong i = 0; i < size; ++i) {
        maxDiagonal = std::max(fabs(Hessian_(i, i)), maxDiagonal);
    }
    double tau = 1e-5;
    currentLambda_ = tau * maxDiagonal;
}

1.4 求解线性方程

  到次为止，H有了，残差有了，LM参数有了，那么我们就能够直接求解这个方程

SolveLinearSystem函数分析

1.4.1 非SLAM问题—求逆

H矩阵是稠密矩阵，直接求逆

    if (problemType_ == ProblemType::GENERIC_PROBLEM) {

        // 非 SLAM 问题直接求解
        // PCG solver
        MatXX H = Hessian_;
        for (ulong i = 0; i < Hessian_.cols(); ++i) {
            H(i, i) += currentLambda_;
        }
//        delta_x_ = PCGSolver(H, b_, H.rows() * 2);
        delta_x_ = Hessian_.inverse() * b_;

    }

1.4.2 SLAM问题—舒尔补

H矩阵是稀疏矩阵，利用其稀疏特性，用schur消元进行边缘化marginalize操作，加速求解

$$\left[\begin{array}{cc}{H}_{pp}& H_{pm}\\ H_{mp}& H_{mm}\end{array}\right]\left[\begin{array}{c}\Delta x_p^{\ast }\\ \Delta x_m^{\ast }\end{array}\right]=\left[\begin{array}{c}{b}_{pp}\\ b_{mm}\end{array}\right]$$

① 根据pose和landmark分块

  矩阵维度$H_{(p+m)\ast (p+m)}\ast \Delta x_{(p+m)\ast 1}=b_{(p+m)\ast 1}$，填充相应位置

  ordering_poses_是所有的位姿顶点维度 = 6*位姿顶点个数，即这里的p

  ordering_landmarks_是所有的路标点顶点维度 = 3 * 路标点顶点个数，即m

// SLAM 问题采用舒尔补的计算方式
// step1: schur marginalization --> Hpp, bpp
int reserve_size = ordering_poses_;
int marg_size = ordering_landmarks_;

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

// 注意这不是行列索引，对于列向量，segment这里指的是起始值
VecX bpp = b_.segment(0,reserve_size);
VecX bmm = b_.segment(reserve_size,marg_size);

② 舒尔补

  矩阵$H_{mm}$和$H_{pp}$都是对角矩阵，即可逆。我们这里选择计算矩阵的舒尔补，即$(H_{pp}-H_{pm}{H}_{mm}^{-1}{H}_{mp})$

1 计算右下角关于路标点的矩阵Hmm之逆矩阵$H_{mm}^{-1}$

// Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
for (auto landmarkVertex : idx_landmark_vertices_) {
    // OrderingId()返回的之在H中的索引，要减去前面关于位姿的维度
    int idx = landmarkVertex.second->OrderingId() - reserve_size;
    
    int size = landmarkVertex.second->LocalDimension();     // 3*3
    
    Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
}

2 计算$(H_{pp}-H_{pm}{H}_{mm}^{-1}{H}_{mp})$

  左乘矩阵，将H矩阵变为下三角矩阵
$$\left[\begin{array}{cc}I& -H_{pm}{H}_{mm}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}{H}_{pp}& H_{pm}\\ H_{mp}& H_{mm}\end{array}\right]\left[\begin{array}{c}\Delta x_{\mathrm{p}}^{\ast }\\ \Delta x_c^{\ast }\end{array}\right]=\left[\begin{array}{cc}I& -H_{pm}{H}_{mm}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}{b}_{pp}\\ b_{mm}\end{array}\right].$$

$$\left[\begin{array}{cc}\left(H_{pp}-H_{pm}{H}_{mm}^{-1}{H}_{mp}\right)& 0\\ H_{mp}& H_{mm}\end{array}\right]\left[\begin{array}{c}\Delta x_{\mathrm{p}}^{\ast }\\ \Delta x_c^{\ast }\end{array}\right]=\left[\begin{array}{c}{b}_{pp}-H_{pm}{H}_{mm}^{-1}{b}_{mm}\\ b_{mm}\end{array}\right]$$

// 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();     // 3*3
    Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
}

$$\left(H_{pp}-H_{pm}{H}_{mm}^{-1}{H}_{mp}\right)\Delta x_p^{\ast }=b_{pp}-H_{pm}{H}_{mm}^{-1}{b}_{mm}$$

// 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_p^{\ast }$

 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;

​ 已知$\Delta x_p^{\ast }$，求$\Delta x_m^{\ast }$

$$H_{mm}\Delta x_m^{\ast }=b_m-H_{mp}\Delta x_p^{\ast }$$

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

2 marginalize测试

  在上面解决位姿优化问题中并没有真正使用marg，而是在最后给了一个具体的信息矩阵H，把对齐操作的结果展示了出来。

2.1 移动H矩阵中边缘化的量

int idx = 1;            // marg 中间那个变量
int dim = 1;            // marg 变量的维度
int reserve_size = 3;   // 总共变量的维度
double delta1 = 0.1 * 0.1;
double delta2 = 0.2 * 0.2;
double delta3 = 0.3 * 0.3;

int cols = 3;
MatXX H_marg(MatXX::Zero(cols, cols));
H_marg << 1./delta1, -1./delta1, 0,
-1./delta1, 1./delta1 + 1./delta2 + 1./delta3, -1./delta3,
0.,  -1./delta3, 1/delta3;

// TODO:: home work. 将变量移动到右下角
/// 准备工作： move the marg pose to the Hmm bottown right

1 将 row i 移动矩阵最下面

  把第idx行移动到矩阵的最后一行，首先取出第idx行，行维dim，列就是矩阵维度reserve_size。

  取出矩阵idx行下面的全部，对于开始索引即idx + dim, 0。要取得矩阵块大小应该是reserve_size- idx - dim，比如矩阵维度是10，idx是1，dim是1，那么取得大小就是（10 - 1 - 1）*10 。

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;

2 将 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;

2.2 舒尔补-边缘化

  本质上就是上面推导舒尔补过程，就是把这个边缘化后的先验矩阵H_prior给出来了，实质上也没有什么特别的。

    /// 开始 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(?,?,?,?);
//Eigen::MatrixXd Amr = H_marg.block(?,?,?,?);
//Eigen::MatrixXd Arr = H_marg.block(?,?,?,?);
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;

3 添加先验prior约束

解决H矩阵不满秩、不正定

解决办法2：添加先验约束，增加系统的可观性。g2o、ceres中对系统的第一个pose的信息矩阵加上单位阵${\mathbf{H}}_{[11]}+=\mathbf{I}$。

在代码中给第一帧和第二帧添加pior约束，并比较为prior设定不同权重时，BA求解收敛精度和速度。

待补充

4 其余部分程序

Frame

/*
 * Frame : 保存每帧的姿态和观测
 */
struct Frame {
    Frame(Eigen::Matrix3d R, Eigen::Vector3d t) : Rwc(R), qwc(R), twc(t) {};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;

    unordered_map<int, Eigen::Vector3d> featurePerId; // 该帧观测到的特征以及特征id
};

生成虚拟相机位姿、特征点---------代码和第四讲0海塞矩阵零空间维度验证一致

/*
 * 产生世界坐标系下的虚拟数据: 相机姿态, 特征点, 以及每帧观测
 */
void GetSimDataInWordFrame(vector<Frame> &cameraPoses, vector<Eigen::Vector3d> &points) {
    int featureNums = 20;  // 特征数目，假设每帧都能观测到所有的特征
    int poseNums = 3;     // 相机数目

    double radius = 8;
    for (int n = 0; n < poseNums; ++n) {
        double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        cameraPoses.push_back(Frame(R, t));
    }

    // 随机数生成三维特征点
    std::default_random_engine generator;
    std::normal_distribution<double> noise_pdf(0., 1. / 1000.);  // 2pixel / focal
    for (int j = 0; j < featureNums; ++j) {
        std::uniform_real_distribution<double> xy_rand(-4, 4.0);
        std::uniform_real_distribution<double> z_rand(4., 8.);

        Eigen::Vector3d Pw(xy_rand(generator), xy_rand(generator), z_rand(generator));
        points.push_back(Pw);

        // 在每一帧上的观测量
        for (int i = 0; i < poseNums; ++i) {
            // Pc = Rcw*(Pw - twc) = Rcw*Pw - Rcw*twc = Rcw*Pw + tcw = Pc
            Eigen::Vector3d Pc = cameraPoses[i].Rwc.transpose() * (Pw - cameraPoses[i].twc);
            Pc = Pc / Pc.z();  // 归一化图像平面
            Pc[0] += noise_pdf(generator);
            Pc[1] += noise_pdf(generator);
            cameraPoses[i].featurePerId.insert(make_pair(j, Pc));
        }
    }
}