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

闲扯

相信了解SLAM算法的同学，无论是做激光SLAM还是视觉SLAM，应该都对LOAM并不陌生，毕竟是在KITTI Odometry里一直排名前三的学神级算法。最最重要的是，Zhang Ji大神把这项工作开源了，于是LOAM也就成为了很多激光SLAM初学者的领路人，至今仍是很多激光SLAM论文中对比的基准(鞭尸)。记得几年前第一次看LOAM论文的时候，看完的第一感觉竟然是：就这？就这？就这？都没有我看不懂的公式？这么“简单”的方法居然能既跑的快、又跑的准，还能发RSS？当然读完论文再参考开源代码复现后，那就是另一番景象了，俨然大型真香现场。
作为3D激光SLAM初学者必读的代码，知乎、优快云上自然有很多关于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