LOAM源码解析——scanRegistration

最新推荐文章于 2024-04-26 19:46:50 发布

原创

最新推荐文章于 2024-04-26 19:46:50 发布 · 1.6k 阅读

23 ·

CC 4.0 BY-SA版权

本文深入解析经典LOAM算法的scanRegistration.cpp模块，涵盖点云预处理、IMU数据插值、特征点提取等关键步骤。通过预处理消除无效点，利用IMU数据矫正点云畸变，计算点云曲率并提取边缘点和平面点。主要回调函数laserCloudHandler处理点云，imuHandler处理IMU数据，用于消除非匀速运动影响。通过对源码的详细解读，有助于理解LOAM的工作原理。

入手激光雷达SLAM，从最经典的LOAM源码开始，之后会对秦通在此基础上改进的A-LOAM源码进行观看，应该就会很简单了，第三步是把LEGO-LOAM的源码进行解读。之前对LOAM的论文进行了详细解读。

LOAM的源码主要分为scanRegistration提取特征点、laserOdometry 10HZ估计位姿、laserMapping 1HZ构建三维地图、transforMaintenance 位姿优化等四个模块。本文主要参考LOAM_NOTED，昨天看到攻城狮说工作项目中是来不及你详细阅读代码的，从一个灵感出现到实现大概只给你一周时间，这中间会包括两三天时间阅读论文、源码，两三天在源码基础上进行修改，时间就是这么紧张。

本着不重复造轮子的原则，本文将在关键地方加上自己理解，帮助大家更加详尽了解。这里注释的是LOAM的velodyne三维激光版本，主要考虑到KITTI上有Velodyne16线的数据，可以用来测试。今天进行第一部分Point Clond Registration模板，对应的scanRegistration.cpp，主要完成的工作有：对点云和IMU数据预处理、对接收的点云数据划分到不同线中存储、特征点（边缘点+平面点）提取等。
在这里插入图片描述

scanRegistration.cpp

下面从main函数开始
在ros中订阅了2个话题，发布了6个话题。两个主要的回调函数：laserCloudHandler、imuHandler分别处理点云和IMU数据。其中，最主要的是laserCloudHandler回调函数。

main.cpp

int main(int argc, char** argv)
{
   
   
  ros::init(argc, argv, "scanRegistration");
  ros::NodeHandle nh;
  // 两个订阅话题：点云、IMU
  // 两个主要的回调函数：laserCloudHandler、imuHandler
  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> 
                                  ("/velodyne_points", 2, laserCloudHandler);
  ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);
  // 6个发布话题
  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/velodyne_cloud_2", 2);
  pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>
                                        ("/laser_cloud_sharp", 2);
  pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>
                                            ("/laser_cloud_less_sharp", 2);
  pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>
                                       ("/laser_cloud_flat", 2);
  pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_less_flat", 2);
  pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);
  // ros::spin()进入循环，一直调用回调函数，用户输入Ctrl+C退出
  ros::spin();
  return 0;
}

回调函数 laserCloudHandler

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)

主要包括以下模块：

1、点云预处理：过滤无效点、计算起始和终止点的方位角
2、根据每个点的仰角将点划入不同线中，共16线，记录线束号和获取的相对时间（也就是代码中的intensity，这个不是强度）
3、使用IMU数据插值计算点云中点的位置，消除由于非匀速运动造成的运动畸变
4、计算所有点的曲率，剔除两类点
5、提取边缘点和平面点
6、ROS发布消息

1、点云预处理

首先过滤无效点、计算起始和终止点的方位角。计算起始、终止点的角度是为了计算点云数据中每个点在本次扫描点云数据中的相对位置，从而得到每个点获取的时间relTime，为后面IMU插值做准备。

后面会用上的变量为：

1、每次扫描点开始和结束的索引：scanStartInd、scanEndInd
2、pcl点云数据：laserCloudIn
3、起始和终止点的方位角：startOri、endOri

  //------------1、点云预处理----------------//

  //丢弃前20个点云数据，确保有IMU数据 后再进行点云数据的处理
  if (!systemInited) {
   
   
    systemInitCount++;
    if (systemInitCount >= systemDelay) {
   
   
      systemInited = true;
    }
    return;
  }

  //记录每个scan有曲率的点的开始和结束索引，初始值为0·
  std::vector<int> scanStartInd(N_SCANS, 0);
  std::vector<int> scanEndInd(N_SCANS, 0);
  
  //点云时间戳
  double timeScanCur = laserCloudMsg->header.stamp.toSec();
  //把sensor_msg类型转换成pcl模板点云数据laserCloudIn
  pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
  
  //移除空点,无效值
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
  //点云点的数量
  int cloudSize = laserCloudIn.points.size();

  // 点云的方位角：起始角度/终止角度？？？
  // 扫描开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转
  float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
  // 扫描结束点的旋转角，加2*pi使点云旋转周期为2*pi
  float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                        laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

  //结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描
  //正常情况下在这个范围内：pi < endOri - startOri < 3*pi，异常则修正
  // 为什么不是pi< X <2*pi ？？？？？
  if (endOri - startOri > 3 * M_PI) {
   
   
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
   
   
    endOri += 2 * M_PI;
  }

未理解的问题：
为什么要保证起始和终止点云方位角差距都在(PI,3PI)范围，>PI如果是为了保证扫描至少180度，雷达旋转扫描范围足够，那么<3PI？？？不是一次scan 应该扫描一个圆周吗，2PI吗？那结果为什么不是(PI,2PI)，这样就可以保证扫描一个圆周了。(2PI,3PI)这180度的点可是已经扫描过了哦。

if (endOri - startOri > 3 * M_PI) {
   
   
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
   
   
    endOri += 2 * M_PI;
  }

2、根据角度将点划入不同线中

遍历所有点，首先从激光雷达坐标系转移到车体坐标系，计算点的仰角，根据仰角过滤16线以外的点，并为每个点分配激光线号scanID，根据点云中点旋转角度和整个周期旋转角度的比率获得相对时间relTime，并获得点强度（限号+点相对时间）。中间，根据扫描线是否过半，选择与起始位置还是终止位置进行差值计算，从而进行补偿。

重要的变量：

laserCloudScans：根据线号分布的点云数据
point.intensity：点的强度（线号+点相对时间）

  // ---------------------2、根据角度将点划入不同线中------------//

  bool halfPassed = false;  //lidar扫描线是否旋转过半
  int count = cloudSize;
  PointType point;
  std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);// 保留不有不同特征的点云
  // 遍历点云，计算每个点的角度
  for (int i = 0; i < cloudSize; i++) {
   
   
    // 1.坐标轴交换，velodyne lidar的坐标系也转换到z轴向前，x轴向左的右手坐标系
    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

    // 2. 计算点的仰角，过滤16线以外的点
    //计算点的仰角(根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度
    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)); //仰角四舍五入(加减0.5截断效果等于四舍五入)
    if (roundedAngle > 0){
   
   
      scanID = roundedAngle;
    }
    else {
   
   
      scanID = roundedAngle + (N_SCANS - 1);
    }
    //过滤点，只挑选[-15度，+15度]范围内的点,scanID属于[0,15]，剔除16线以外的杂点。
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
   
   
      count--;
      continue;
    }

    // 3.根据扫描线是否旋转过半，选择与起始位置还是终止位置进行差值计算，从而进行补偿
    float ori = -atan2(point.x, point.z);//该点的旋转角
    // 如果没有旋转过半
    if (!halfPassed) {
   
   
      //确保-pi/2 < ori - startOri < 3*pi/2
      if (ori < startOri - M_PI / 2) {
   
   
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
   
   
        ori -= 2 * M_PI;
      }
      // 旋转过半，旋转角度超过180度
      if (ori - startOri > M_PI) {
   
   
        halfPassed = true;
      }
    } 
    // 旋转过半
    else {
   
   
      ori += 2 * M_PI;// 先加2*pi

      //确保-3*pi/2 < ori - endOri < pi/2
      if (ori < endOri - M_PI * 3 / 2) {
   
   
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
   
   
        ori -= 2 * M_PI;
      } 
    }

    //点云中点的相对时间:点旋转的角度与整个周期旋转角度的比率, -0.5 < relTime < 1.5
    float relTime = (ori - startOri) / (endOri - startOri);
    //点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号scanID，小数部分是该点的相对时间）
    //匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
    point.intensity = scanID + scanPeriod * relTime;// scanPeriod 扫描一圈0.1s
   ........
   ...第三步、IMU插值计算点云中点位置.........
   ........
   //将每个补偿矫正的点放入对应线号的容器，将点按照每一层线，分类压入16个数组中
    laserCloudScans[scanID].push_back(point);
    }
    // 更新总的点云 laserCloud
  cloudSize = count;//获得有效范围内的点的数量
  pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
  for (int i = 0; i < N_SCANS; i++) {
   
   //将所有的点按照线号从小到大放入一个容器
    *laserCloud += laserCloudScans[i];
  }

坐标轴转化：从蓝色的传感器坐标系到红色的车体坐标系。velodyne lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系，车体坐标系为z轴向前,x轴向左,y轴向上的右手坐标系

    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

在这里插入图片描述
在LOAM源码中对于仰角的定义为：
$\theta =arctan(z/\sqrt{x^{2}+ y^{2}})$

最低0.47元/天解锁文章