SLAM学习日志(二) 2D(单线)激光雷达PointCloud与LaserScan格式转换

1. 背景

  在SLAM的学习过程中，PCL点云库是非常重要的工具，有时候，我们需要将单线激光雷达的数据转变为PCL的PointCloud格式进行运算处理，有时候也需要将PointCloud格式打包成ROS的LaserScan格式。以下简单介绍个人在使用时转换的方法。

2. PointCloud与sensor_msgs::LaserScan的转换

2.1 sensor_msgs::LaserScan转PointCloud

void LaserScanToPointCloud(sensor_msgs::LaserScan::ConstPtr _laser_scan, pcl::PointCloud<pcl::PointXYZI>& _pointcloud)
{
  _pointcloud.clear();
  pcl::PointXYZI newPoint;
  newPoint.z = 0.0;
  double newPointAngle;

  int beamNum = _laser_scan->ranges.size();
  for (int i = 0; i < beamNum; i++)
  {
      newPointAngle = _laser_scan->angle_min + _laser_scan->angle_increment * i;
      newPoint.x = _laser_scan->ranges[i] * cos(newPointAngle);
      newPoint.y = _laser_scan->ranges[i] * sin(newPointAngle);
      newPoint.intensity = _laser_scan->intensities[i];
      _pointcloud.push_back(newPoint);
  }
}

2.2 PointCloud转sensor_msgs::LaserScan

sensor_msgs::LaserScan PointCloudToLaserscan(pcl::PointCloud<pcl::PointXYZI>& _pointcloud)
{
  float angle_min, angle_max, range_min, range_max, angle_increment;
  
  //需要自行调整的参数
  angle_min = -3.14159;
  angle_max =  3.14159;
  range_min = 0.5;
  range_max = 20;
  //角度分辨率，分辨率越小，转换后的误差越小
  angle_increment = 0.005

  //计算扫描点个数
  unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment);

  sensor_msgs::LaserScan output;
  output.header.stamp = ros::Time::now();
  output.header.frame_id = "laser";
  output.angle_min = angle_min;
  output.angle_max = angle_max;
  output.range_min = range_min;
  output.range_max = range_max;
  output.angle_increment = angle_increment;
  output.time_increment = 0.0;
  output.scan_time = 0.0;
  
  //先将所有数据用nan填充
  output.ranges.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
  output.intensities.assign(beam_size, std::numeric_limits<float>::quiet_NaN());

  for (auto point : _pointcloud.points)
  {
    float range = hypot(point.x, point.y);
    float angle = atan2(point.y, point.x);
    int index = (int)((angle - output.angle_min) / output.angle_increment);
    if (index >= 0 && index < beam_size)
    {
      //如果当前内容为nan，则直接赋值
      if (isnan(output.ranges[index]))
      {
        output.ranges[index] = range;
      }
      //否则，只有距离小于当前值时，才可以重新赋值
      else
      {
        if (range < output.ranges[index])
        {
          output.ranges[index] = range;
        }
      }
      output.intensities[index] = point.intensity;
    }
  }
  return output;
}

3. PointCloud与sensor_msgs::PointCloud2的转换

这里需要包含#include

3.1 PointCloud转sensor_msgs::PointCloud2

pcl::toROSMsg(pointcloud_pcl, pointcloud_msg);

3.2 sensor_msgs::PointCloud2转PointCloud

pcl::fromROSMsg(pointcloud_msg, pointcloud_pcl);