LOAM: LiDAR Odometry and Mapping In Real Time源码解析(一)

闲扯

相信了解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+k}，p_{i-k}$，其中k=1, 2, 3, 4, 5，即$p_i$前后各5个点，然后求10个向量$v_i^{i+k}，v_i^{i-k}$的合向量的模的平方作为点$p_i$的离散曲率，其中$v_i^{i+k}=p_{i+k}-p_i$。若这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特征。有如下几点细节需要额外关心：

1. LOAM采用了很多工程细节，以保证corner和surface特征在空间中均匀分布，如对于每一个扫描线的点云，分为6个特征区域，并在每一个特征区域内分别提取特征。同时，若一个特征点邻域的点云密度较大，则不再继续在该邻域内选取特征点。
2. 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部分将在下一篇博客中介绍。