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

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

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

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

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

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

    // 所有 Posevector<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_;             // 6nMatXX H(MatXX::Zero(size, size));           // 6n*6nVecX b(VecX::Zero(size));                   // 6n*1for (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 里不需要添加它的信息，也就是它的雅克比为 0auto jacobian_i = jacobians[i];ulong index_i = v_i->OrderingId();	// 顶点在H实际索引ulong dim_i = v_i->LocalDimension();// 优化参数维度6/3MatXX 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 solverMatXX 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*3Hmm_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*3Hmm_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 ： schurdouble 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 / focalfor (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 = PcEigen::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));}}
}