（秋招复习）自动驾驶与机器人中的SLAM技术（三）

第八章 紧耦合LIO 系统

紧耦合的概念

其实回顾松耦合我们可以发现，松耦合中IMU和Lidar之间还是一种解耦的状态，虽然互相之间有影响，但是两者之间还是没有进一步地将IMU的运动方程和激光雷达的配准部分放在一起，所以紧组合理论上就是实现这一功能的一种形式，在紧耦合系统里，一个模块的工作状态能够直接反映到另一个模块中，帮助它们更好地约束自身的工作维度。

基于IEKF的LIO系统

因为Lidar的配准涉及到迭代的操作，所以这里引入了迭代EKF，也就是IEKF。IEKF的运动方程和之前没有任何区别，但是在迭代的过程中会引入一些问题：

所以这里的关键问题就是，如何处理这个迭代过程中的切空间变换问题，以及如何处理点云残差过大维度的影响，接下来先来看一下切空间投影变换的影响

这里有提到，其实相当于只有最后一次迭代，我们才记成了有效的，中间的迭代过程只是改变了更新的起点

而针对高维数据的处理，这里主要引入了SMW恒等式，关于这个恒等式的证明很多文献和资料都有记载

这样我们就能把上边对权阵的处理转换到低维的形式了，分析下面这个等式我们可以发现，求逆的操作变成了18×18的了，这显然安全和合理很多

NDT和Kalman filter之间的关系

这里高博是更为仔细地推导了NDT与卡尔曼滤波之间的关系

这样协方差的更新也就可以写成

所以，我们在进行IEKF LIO系统设计的时候，重点要关注的矩阵也就是HTV-1H和HTV-1r，然后在Kalman滤波中可以直接用这两个矩阵

在最后实现的代码的内部，IEKF为NDT提供当前状态的估计值，然后NDT计算前文所述的两个矩阵返回给IEKF，然后IEKF再去更新自己内部的状态估计，这样就得到了当前扫描对应的位姿了。

基于预积分的LIO

这里的预积分实现，相比之下没有那么多原理了，因为用到的东西前面其实都有实现，所以这里更多的是整个系统如何设计的问题。书中的设计是让两个关键帧之间的图优化问题有8个顶点（位姿、速度和两个零偏），1个预积分边，2个随机游走边以及1个NDT观测位姿约束边和1个先验约束边

• 所以这里我有个疑问，这不是相当于Lidar的配准误差又没有被直接加到系统里么？那这样还算是紧耦合么。不过这里倒是把IMU的零偏什么的加进来一起约束了，所以感觉算个半紧耦合？

当然这里还涉及到了一个边缘化的概念，我们通过看代码发现，其实还是一个Schur补问题，和之前VINS中涉及到的边缘化似乎也没有很大区别，而且这里因为没有地图点一起，所以整体的维度小了很多

inline Eigen::MatrixXd Marginalize(const Eigen::MatrixXd& H, const int& start, const int& end) {// Goal// a  | ab | ac       a*  | 0 | ac*// ba | b  | bc  -->  0   | 0 | 0// ca | cb | c        ca* | 0 | c*// Size of block before block to marginalizeconst int a = start;// Size of block to marginalizeconst int b = end - start + 1;// Size of block after block to marginalizeconst int c = H.cols() - (end + 1);// Reorder as follows:// a  | ab | ac       a  | ac | ab// ba | b  | bc  -->  ca | c  | cb// ca | cb | c        ba | bc | bEigen::MatrixXd Hn = Eigen::MatrixXd::Zero(H.rows(), H.cols());if (a > 0) {Hn.block(0, 0, a, a) = H.block(0, 0, a, a);Hn.block(0, a + c, a, b) = H.block(0, a, a, b);Hn.block(a + c, 0, b, a) = H.block(a, 0, b, a);}if (a > 0 && c > 0) {Hn.block(0, a, a, c) = H.block(0, a + b, a, c);Hn.block(a, 0, c, a) = H.block(a + b, 0, c, a);}if (c > 0) {Hn.block(a, a, c, c) = H.block(a + b, a + b, c, c);Hn.block(a, a + c, c, b) = H.block(a + b, a, c, b);Hn.block(a + c, a, b, c) = H.block(a, a + b, b, c);}Hn.block(a + c, a + c, b, b) = H.block(a, a, b, b);// Perform marginalization (Schur complement)Eigen::JacobiSVD<Eigen::MatrixXd> svd(Hn.block(a + c, a + c, b, b), Eigen::ComputeThinU | Eigen::ComputeThinV);Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType singularValues_inv = svd.singularValues();for (int i = 0; i < b; ++i) {if (singularValues_inv(i) > 1e-6) singularValues_inv(i) = 1.0 / singularValues_inv(i);elsesingularValues_inv(i) = 0;}Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose();Hn.block(0, 0, a + c, a + c) =Hn.block(0, 0, a + c, a + c) - Hn.block(0, a + c, a + c, b) * invHb * Hn.block(a + c, 0, b, a + c);Hn.block(a + c, a + c, b, b) = Eigen::MatrixXd::Zero(b, b);Hn.block(0, a + c, a + c, b) = Eigen::MatrixXd::Zero(a + c, b);Hn.block(a + c, 0, b, a + c) = Eigen::MatrixXd::Zero(b, a + c);// Inverse reorder// a*  | ac* | 0       a*  | 0 | ac*// ca* | c*  | 0  -->  0   | 0 | 0// 0   | 0   | 0       ca* | 0 | c*Eigen::MatrixXd res = Eigen::MatrixXd::Zero(H.rows(), H.cols());if (a > 0) {res.block(0, 0, a, a) = Hn.block(0, 0, a, a);res.block(0, a, a, b) = Hn.block(0, a + c, a, b);res.block(a, 0, b, a) = Hn.block(a + c, 0, b, a);}if (a > 0 && c > 0) {res.block(0, a + b, a, c) = Hn.block(0, a, a, c);res.block(a + b, 0, c, a) = Hn.block(a, 0, c, a);}if (c > 0) {res.block(a + b, a + b, c, c) = Hn.block(a, a, c, c);res.block(a + b, a, c, b) = Hn.block(a, a + c, c, b);res.block(a, a + b, b, c) = Hn.block(a + c, a, b, c);}res.block(a, a, b, b) = Hn.block(a + c, a + c, b, b);return res;
}

第九章 地图构建

这里涉及到的地图构建主要指的是离线构建这么一个流程，主要的流程如上图所示，具体分为：


我们可以看出来，这里的核心其实在于进行了两轮的优化。至于前端的话其实很简单，首先我们先取到RTK观测数据，然后把第一个有效的RTK数据作为地图原点，这样我们的LIO系统得到的位姿和地图相当于都有了一个世界坐标系下的参考，然后我们再对LIO系统进行抽取关键帧的处理，保存其中的关键帧结果（位姿和点云），也就是我们最后好建图的依据。

位姿优化

第一轮优化–RTK

RTK可以作为一种位姿的强约束（没有姿态的时候就只约束位置），然后LIO系统可以提供给系统运行过程中的局部连续性约束，这个其实也就是之前前端得到的结果；这里也是比起原理，代码的实现更重要的一个部分，第一轮优化里其实就是以RTK位姿为主，我们可以看到加入的边分别是GNSS位移边以及LIO的相对运动约束边

第二轮优化–回环检测与修正

这里其实和2D SLAM 的有些类似，因为这里是离线的形式，所以这里是把回环一次性都检测完了

与2D的类似，之前2D是为似然场添加了一个多分辨率的过程，这里也是为NDT添加了一个多分辨率的过程，不过这里是每一次都用到了上一次的配准结果

std::vector<double> res{10.0, 5.0, 4.0, 3.0};
for (auto& r : res) {ndt_pcl.setResolution(r);auto rough_map1 = VoxelCloud(submap_kf1, r * 0.1);auto rough_map2 = VoxelCloud(submap_kf2, r * 0.1);if(use_pcl_ndt_) {// LOG(INFO) << "pcl ndt: ";ndt_pcl.setResolution(r);   // 设置内部NDT网格结构的体素分辨率ndt_pcl.setInputTarget(rough_map1);ndt_pcl.setInputSource(rough_map2);ndt_pcl.align(*output, Tw2);Tw2 = ndt_pcl.getFinalTransformation();} else {// LOG(INFO) << "7th chapter ndt: ";ndt.SetResolution(r);       // 【新增】设置自己实现的3D NDT的分辨率ndt.SetTarget(rough_map1);  // 【新增】设置自己实现的3D NDT的目标点云ndt.SetSource(rough_map2);  // 【新增】设置自己实现的3D NDT的源点云ndt.AlignNdt(Tw2_se3);} 
}

注：这里只是完成了回环检测，而回环检测的修正则是基于回环检测边实现的，而在具体的边里，其实因为我们已经得到了回环边，也就是冉魏这些回环边之间是有位姿关系的，所以这里直接添加进去的最后还是相对运动关系这种残差边类型

for (const auto& lc : loop_candidates_) {auto edge = new EdgeRelativeMotion(vertices_.at(lc.idx1_), vertices_.at(lc.idx2_), lc.Tij_);edge->setInformation(info_all);auto rk = new g2o::RobustKernelCauchy();rk->setDelta(loop_rk_th);edge->setRobustKernel(rk);optimizer_.addEdge(edge);loop_edge_.emplace_back(edge);
}

地图导出

这里反而没什么好说的，其实就是把点云根据坐标进行分块处理，也就是：点云的切分实际上是根据每个点的坐标计算其所在的网格，然后把它投到对应的网格中去。具体对应到代码里

// add to grid
for (const auto& pt : kf_cloud_voxeled->points) {int gx = floor((pt.x - 50.0) / 100);int gy = floor((pt.y - 50.0) / 100);Vec2i key(gx, gy);auto iter = map_data.find(key);if (iter == map_data.end()) {// create point cloudCloudPtr cloud(new PointCloudType);cloud->points.emplace_back(pt);cloud->is_dense = false;cloud->height = 1;map_data.emplace(key, cloud);} else {iter->second->points.emplace_back(pt);}
}

第十章 自动驾驶车辆的实时定位系统

这里我们可以看到，最后实现的其实就是在有高精度地图下的一个融合配准定位，这里第一个问题还是关于RTK怎么用，RTK首先提供了一个初始的位置，如果没有有效的姿态信息的话，就在它周边进行网格搜索。我们设计的搜索流程主要用于搜索车辆的初始航向角，该算法内部调用类似上一章回环检测使用的多分辨率匹配方法。 这里的具体实现也是要结合代码看的，不过确实是和之前的多分辨率NDT一样的

std::vector<double> res{10.0, 5.0, 4.0, 3.0};
Mat4f T = gr.pose_.matrix().cast<float>();
for (auto& r : res) {auto rough_map = VoxelCloud(map, r * 0.1);ndt.setInputTarget(rough_map);ndt.setResolution(r);ndt.align(*output, T);T = ndt.getFinalTransformation();
}

这里剩下的部分就是和LIO差不多的东西了。保留了点云去畸变、IMU 激光消息同步部分的代码，但是把原先的LIO 配准改为地图配准。

总结

最后的这部分，其实没有那么多原理的部分，感觉如果有条件的同学甚至可以把这个看作利用之前所讲的原理来进行工程实现的一个部分。不过由于本人没有这种条件，所以对这部分的理解还是停留在了能推明白公式和能跑通数据集代码这个步骤，希望日后能有机会自己去实地地跑几组Lidar的数据，这样相比也会有更深刻的理解。