LOAM: LiDAR Odometry and Mapping In Real Time源码解析(一)
LOAM：LiDAR Odometry and Mapping In Real Time源码解析（一）
- 闲扯
- LOAM简介
- Scan Registration
-
- 预处理
- 特征提取
闲扯
相信了解SLAM算法的同学，无论是做激光SLAM还是视觉SLAM，应该都对LOAM并不陌生，毕竟是在KITTI Odometry里一直排名前三的学神级算法。最最重要的是，Zhang Ji大神把这项工作开源了，于是LOAM也就成为了很多激光SLAM初学者的领路人，至今仍是很多激光SLAM论文中对比的基准(鞭尸)。记得几年前第一次看LOAM论文的时候，看完的第一感觉竟然是：就这？就这？就这？都没有我看不懂的公式？这么“简单”的方法居然能既跑的快、又跑的准，还能发RSS？当然读完论文再参考开源代码复现后，那就是另一番景象了，俨然大型真香现场。
作为3D激光SLAM初学者必读的代码，知乎、CSDN上自然有很多关于LOAM论文及代码的详细解析，那么继续再写一篇很可能写不出什么新意的LOAM源码解析博客的动机在哪呢？毕竟当下LOAM已经不能算作什么时髦的东西了。我想主要有两点原因吧：首先我最近在写论文分析算法性能时，发现LOAM的有一些算法细节我已经记不太清了，于是又去花了点时间读了一下代码。通过博客的形式记录下来，这样下次再忘了翻自己的博客就好了。其次，希望能通过这篇博客分享一下自己关于LOAM的理解，倘若能给看到这篇博客的人一点收获当然是最好了。
LOAM简介
本博客解析的开源代码是由港科大沈老师组的Shaozu Cao根据LOAM源码改写的A-LOAM版本。相比于原始的LOAM开源代码，A-LOAM中坐标系定义较为清晰，利用Ceres开源优化库，简化了后端优化求解过程，较容易理解。与LOAM类似，A-LOAM可分为三部分：scanRegistration, odometry, mapping。其中，scanRegistration部分主要负责实时从原始激光雷达点云中提取线和面特征点，odometry部分负责利用scanRegistration部分提取出的特征点，关联特征并计算估计帧间的相对运，mapping部分负责以1Hz的频率更新维护特征地图，同时利用帧到地图的配准，提高里程计轨迹精度。
Scan Registration
预处理
从scanRegistration.cpp中的主函数中可以看出，这部分代码需要输入两个参数N_SCANS和MINIMUM_RANGE：
nh.param<int>("scan_line", N_SCANS, 16);
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);其中N_SCANS表示激光雷达的线数，如16、32、64，MINIMUM_RANGE表示激光雷达的最小测距距离；
接下来就是熟悉的订阅激光雷达点云消息了：
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);回调函数laserCloudHandler就是这部分的重头戏了：
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);由上面的代码可以看出，LOAM首先对原始激光雷达点云做了预处理，主要做了两件事：首先移除激光雷达点云的NaN(Not a number)点云，然后移除点云中测距结果过近的点，应该是激光雷达点云相关算法的常规操作。接下来的处理方法非常有新意，算法首先计算了该帧激光雷达点云中第一个点的水平方位角startOri和最后一个点的水平方位角endOri。
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
if (endOri - startOri > 3 * M_PI){
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI){
endOri += 2 * M_PI;
}然后LOAM利用了旋转式3D激光雷达分线扫描的特性，将原始激光雷达点云分解成了若干扫描线对应点云的集合。在这一过程中，LOAM利用了Velodyne VLP-16，HDL-32E，HDL-64三款激光雷达相邻线束之间的垂直夹角为定值的特性(代码中第9，15，22，24行)。对于扫描线垂直分布不均匀的激光雷达如Velodyne HDL-32C，则不能采用这种方法，需要自己根据扫描线分布，计算扫描点对应的扫描线。此外，如在第54行所示，LOAM还利用了旋转式3D激光雷达顺序扫描的特性，计算了每一个激光雷达点相对于该帧起始时间的相对测量时间，为后续去点云畸变做准备，如在第54行代码所示。
for (int i = 0; i < cloudSize; i++){
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
// 确定每一个点对应的scanID，即确定该点是由哪个扫描线扫描得到的
if (N_SCANS == 16){
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0){
count--; // 有效点个数：减1
continue;
}
}else if (N_SCANS == 32){
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0){
count--; // 有效点个数：减1
continue;
}
}else if (N_SCANS == 64){
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use [0 50] > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0){
count--; // 有效点个数：减1
continue;
}
}
else{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
// 确定每一个点的水平方位角
float ori = -atan2(point.y, point.x);
if (!halfPassed){
if (ori < startOri - M_PI / 2){
ori += 2 * M_PI;
}else if (ori > startOri + M_PI * 3 / 2){
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI){
halfPassed = true;
}
}else{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2){
ori += 2 * M_PI;
}else if (ori > endOri + M_PI / 2){
ori -= 2 * M_PI;
}
}
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;
laserCloudScans[scanID].push_back(point);
}特征提取
如下代码所示，在第二个for循环中，计算了每一个激光雷达扫描点对应的离散曲率。这里简单解释一下LOAM中离散曲率的定义：首先LOAM定义了待求曲率的激光雷达点 p i p_i pi 的邻域 p i + k ， p i − k p_{i+k}，p_{i-k} pi+k，pi−k，其中k=1, 2, 3, 4, 5，即 p i p_i pi前后各5个点，然后求10个向量 v ⃗ i i + k ， v ⃗ i i − k \vec{v}^{i+k}_i，\vec{v}^{i-k}_i vii+k，vii−k的合向量的模的平方作为点 p i p_i pi的离散曲率，其中 v ⃗ i i + k = p i + k − p i \vec{v}^{i+k}_i=p_{i+k}-p_i vii+k=pi+k−pi。若这11个扫描点共线，则对应的合向量的模较小或接近于0，离散曲率较小；反之，则合向量的模较大，离散曲率较大。
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++){
// 确定每一个scanline对应点云的起始和结束索引
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
printf("prepare time %f \n", t_prepare.toc());
for (int i = 5; i < cloudSize - 5; i++){
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
// 确定每一个点对应的离散曲率
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}计算完单帧激光雷达点云的离散曲率后，即可在每一个scanline内根据该值提取离散曲率较大的corner特征和离散曲率较小的surface特征。有如下几点细节需要额外关心：
- LOAM采用了很多工程细节，以保证corner和surface特征在空间中均匀分布，如对于每一个扫描线的点云，分为6个特征区域，并在每一个特征区域内分别提取特征。同时，若一个特征点邻域的点云密度较大，则不再继续在该邻域内选取特征点。
- LOAM将corner特征分为两类，分别是sharp corner特征和less sharp corner特征，前者对应曲率大于0.1的激光雷达点中曲率最大的2个点(每个特征区域内)，后者对应曲率大于0.1的激光雷达点中曲率最大的前20个点(每个特征区域内)。将surface特征同样分为两类，分别为flat surface和less flat surface特征，其中前者对应曲率小于0.1的激光雷达点云中曲率最小的4个点(每个特征区域内)，后者对应flat surface特征点云以及该帧点云中未被标记为corner特征的所有点云的集合，并对其进行了体素滤波下采样。sharp corner和flat surface特征作为激光里程计中source点云，less sharp corner和less flat surface特征作为激光里程计中的target点云。首先可以注意到，LOAM对于两种特征点的提取的曲率阈值设置的相同，都为0.1，而不是将corner对应的曲率阈值设置的极大，将surface对应的曲率阈值设置的极小，这样可以提高算法对环境的鲁棒性，即无论环境怎样变，都可以提取出曲率相对较大的corner特征以及曲率相对较小的surface特征。其次，LOAM在sharp corner和flat surface特征的基础上添加了对应的less sharp和less flat特征，刚开始看LOAM代码的时候不是很能理解这样做的意义。现在觉得应该是基于曲率的方法并不能准确、稳定地检出corner和surface特征，尤其是corner特征，将数量更多的less sharp corner和less flat surface特征作为激光里程计中的target点云，有利于提高有效关联的数量，以提高激光里程计的精度。
for (int i = 0; i < N_SCANS; i++){
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
// 将每一个scanLine分解为6个特征区域以使特征尽量均匀分布
for (int j = 0; j < 6; j++){
// 确定每一个特征区域内的起始和结束点索引
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
// 根据离散曲率的大小，对该特征区域内的点排序
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
// 离散曲率从大到小以提取角点特征
for (int k = ep; k >= sp; k--){
int ind = cloudSortInd[k];
// 附近无特征点，且该点的离散曲率大于0.1
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1){
// 曲率大的点被选取为corner特征点的个数，每个特征区域最多2个sharp corner，20个less sharp，每个scanline共6个特征区域，因此每个scanline最多12个sharp corner，120个less sharp
largestPickedNum++;
// 选取曲率最大的2个点为sharp corner特征
if (largestPickedNum <= 2){
cloudLabel[ind] = 2; // label 2 for sharp corner
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
// 选取曲率较大的前20个点为less sharp corner特征
else if (largestPickedNum <= 20){
cloudLabel[ind] = 1; // label 1 for less sharp corner
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else{
break;
}
cloudNeighborPicked[ind] = 1;
// 若附近点比较密集(相邻点的欧式距离的平方小于0.05)，则通过设置cloudNeighborPicked使周围不再出现特征点，以防止特征点过于密集
for (int l = 1; l <= 5; l++){
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--){
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
int smallestPickedNum = 0;
// 离散曲率从小到大以提取surface特征
for (int k = sp; k <= ep; k++){
int ind = cloudSortInd[k];
// 离散曲率小于0.1，且附近无特征点
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1){
cloudLabel[ind] = -1; // label -1 for flat surface feature
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
// 每个特征区域最多4个flat surface特征，共6个特征区域，每个scanline共6个特征区域，因此每个scanline最多共24个flat surface特征
if (smallestPickedNum >= 4){
break;
}
cloudNeighborPicked[ind] = 1;
// 若附近点比较密集(相邻点的欧式距离的平方小于0.05)，则通过设置cloudNeighborPicked使周围不再出现特征点，以防止特征点过于密集
for (int l = 1; l <= 5; l++){
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--){
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 将flat surface特征点以及未被分类为corner特征的点作为less flat特征
for (int k = sp; k <= ep; k++){
if (cloudLabel[k] <= 0){
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS)
surfPointsLessFlat += surfPointsLessFlatScanDS;
}LOAM中特征提取部分就介绍到这里，为避免博客篇幅过长，LiDAR Odometry部分将在下一篇博客中介绍。