3D 激光SLAM ->loam_velodyne论文与代码解析Lidar Odometry and Mapping

3D SLAM ->loam_velodyne论文与代码解析

一直做2D 的激光SLAM,最近终于接触到3D的了,想彻底的看透一个开源的3D 激光SLAM,选择了Loam_velodyne从论文到代码彻底看一下。论文:LOAM Lidar Odometry and Mapping in Real-time。


结构关系:订阅与发布节点关系

其中最终输出构建3D地图的点云是/laser_cloud_surround;需要更改的是最左侧输入、即订阅的/velodyne_points的topic名称。Remap成为需要订阅的主题。从图中可以看出scanRegister的工作是将源数据处理成/laser_cloud_sharp & /laser_cloud_flat等等,这些点云是具有特征信息的点云。
这里写图片描述
具体的操作为在论文中给出这样一幅图:
这里写图片描述
由此可以知道laserOdometry从特征点中估计运动(10HZ),然后整合数据发送给laserMapping(1HZ),在这个过程中,算是一种数据的降采样吧。也是为了保证运行online的时效性,以及节省内存占用的大小。

具体部分的实现思想

相对于其它直接匹配两个点云,loam是通过提取特征点匹配后计算坐标变换。

特征点提取

一次扫描的点通过曲率(c)值来分类,特征点是c的最大值点–边缘点;特征点是c的最小值点–平面点。为了使特征点均匀的分布在环境中,将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点,同时特征点的选择需要满足阈值的要求。
同时需要考虑特征点选择中的一些越是:比如如果一个点被其它特征点包围,那么就不被选择;以及一些点满足c的要求不过是不稳定的特征点,比如断点等。不稳定的点如下图:
这里写图片描述

代码部分:主要是在scanRegistration.cpp这个文件的任务是提取特征点,并且发布出去。
代码具体的阅读理解:
① 论文中存储每个点的曲率用的是数组,因此需要考虑数组的大小:

float cloudCurvature[80000];
int cloudSortInd[80000];
int cloudNeighborPicked[80000];
int cloudLabel[80000];

前言:N_SCANS是将3D的激光点云按照激光的接受器做了个划分,比如N_SCANS是16表明是16线的激光(程序中的默认值,作者用过velodyne16)。
对于一堆点云并不是像LaserScan(二维的数据结构)那样按照角度给出个距离值,保证每次的扫描都能够有相同大小的数据量。PointCloud2接受到的点云的大小在变化,因此在数据到达需要一些运算来判断点的一些特性。例如下面这段通过计算pitch角度值将点划分到不同的“线”中。代码坑,point.x=laserCloudIn.points[i].y;做个迷惑的赋值。Pitch=atan(z/(x2+y2));和代码中只是形式的不同。

  PointType point;
  std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

    float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
    int scanID;
    int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); 
    if (roundedAngle > 0){
      scanID = roundedAngle;
    }
    else {
      scanID = roundedAngle + (N_SCANS - 1);
    }
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
      count--;
      continue;
    }

算出scan ID后,又将intensity属性充分利用起来,整数部分:scan ID,小数部分:每个点扫描的时间(在startOri->endOri按照均匀划分)

  float relTime = (ori - startOri) / (endOri - startOri);
  point.intensity = scanID + scanPeriod * relTime;

然后,将点压入到每个线中,同时更新总的点云laserCloud。

   ……
   laserCloudScans[scanID].push_back(point);
  }
  cloudSize = count;

  pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
  for (int i = 0; i < N_SCANS; i++) {
    *laserCloud += laserCloudScans[i];
  }

含义说明:cloudCurvature是存储每个点的曲率,由上面的laserCloud+=……,可以知道的是这个时候的点云是按照线的数据方式存储的,线的数量自己定义。作者给出的计算公式:

    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;
    //jc : cloudCurvature calculate 
    cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;

因为是按照线的序列存储,因此接下来能够得到起始和终止的index;在这里滤除前五个和后五个。

  if (int(laserCloud->points[i].intensity) != scanCount) {
      scanCount = int(laserCloud->points[i].intensity);
      //N_SCANS is 16
      if (scanCount > 0 && scanCount < N_SCANS) {
    //std::vector<int> scanStartInd(N_SCANS, 0);
    //std::vector<int> scanEndInd(N_SCANS, 0);
        scanStartInd[scanCount] = i + 5;
        scanEndInd[scanCount - 1] = i - 5;
      }
    }

cloudSortInd是对曲率排序得到的序列:这里作者将每一线划分为等间距的6段分别处理,在每一段升序排列。 变量说明sp startPoint;ep endPoint.

for (int i = 0; i < N_SCANS; i++) {
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
    for (int j = 0; j < 6; j++) {

      int sp = (scanStartInd[i] * (6 - j)  + scanEndInd[i] * j) / 6;
      int ep = (scanStartInd[i] * (5 - j)  + scanEndInd[i] * (j + 1)) / 6 - 1;

      for (int k = sp + 1; k <= ep; k++) {
        for (int l = k; l >= sp + 1; l--) {
          if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
            int temp = cloudSortInd[l - 1];
            cloudSortInd[l - 1] = cloudSortInd[l];
            cloudSortInd[l] = temp;
          }
        }
      }

cloudNeighborPicked是考虑一个特征点的周围不能再设置成特征点约束的判断标志位。
cloudLabel按照如下计算,选择最大曲率的作为sharp和lessSharp,因为是按照升序排列的cloudSortInd,因此从ep->sp逐个选择:

    int largestPickedNum = 0;
      for (int k = ep; k >= sp; k--) {
        int ind = cloudSortInd[k];
        // jc : in cloudNeighborPicked array  1 is nerighbor and 0 is alone
        // jc : if it 's  alone and the curudCurvature is bigger then 0.1, 
        if (cloudNeighborPicked[ind] == 
  • 34
    点赞
  • 262
    收藏
    觉得还不错? 一键收藏
  • 26
    评论
评论 26
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值