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+kpik,其中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 v ii+kv iik的合向量的模的平方作为点 p i p_i pi的离散曲率,其中 v ⃗ i i + k = p i + k − p i \vec{v}^{i+k}_i=p_{i+k}-p_i v ii+k=pi+kpi。若这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部分将在下一篇博客中介绍。

你可能感兴趣的:(3D激光SLAM源码解析系列,自动驾驶)