激光SLAM——点云初识

作为笔者的处女座，必须得留给我心心念的激光SLAM！激光雷达点云在ROS接口下，包含二维点云格式sensor_msgs::laserscan、三维点云格式sensor_msgs::PointCloud2（实际上还有sensor_msgs::PointCloud，不过目前没用到）以及PCL的一系列格式。

1、sensor_msgs::laserscan解析

sensor_msgs::laserscan官方释义
先上图
具体二维点云格式如下：

//头文件，关键是time_stamped与frame id
header: 
  seq: 448
  stamp: 
    secs: 93
    nsecs: 955000000
  frame_id: "base_scan"
//最大角度与最小角度，这里是0~2pi,角度为rad
angle_min: 0.0
angle_max: 6.28318977356
//角度增量，实际上就是角度步长，单位为rad；
//(angle_max-angle_min)/angle_increment即为激光束选择一圈所获得的点云数据数量
angle_increment: 0.0175019223243
//关于激光点云运动时间的问题，目前未应用，直接借鉴官网解释吧
time_increment: 0.0
scan_time: 0.0
//测距最大范围与最小范围
range_min: 0.119999997318
range_max: 3.5
//点云所携带的环境信息，包括测距信息与强度信息，该数组的长度即为(angle_max-angle_min)/angle_increment
ranges: [0.6837663650512695, 0.6689223051071167, 0.6657941937446594,.....]
intensities: [5.823023349397141e-37, 5.823023349397141e-37, 5.823023349397141e-37......]
---

关键：ranges数组与intensities数组的数据分别包含了距离信息与强度信息，同时每个数据在数组中的位置索引包含了对应点云在激光雷达坐标系下的夹角。
那么，假如可爱的你获取到一帧二维点云数据，现在就很容易获取该帧点云中每个点云在激光雷达坐标系下的位置与角度以及所携带的环境强度信息。

2、sensor_msgs::PointCloud2解析

sensor_msgs::PointCloud2官方解释
sensor_msgs::PointCloud官方解释，用过的小伙伴教教我
笔者使用的是Velodyne 16线激光雷达，现在逐行解释吧！
先上图
三维点云具体格式如下：

//头文件
header: 
  seq: 18664
  stamp: 
    secs: 1551087603
    nsecs: 700827000
  frame_id: "velodyne"
//height可视为点云数据的行数；
//如果为1，则表示无序；
//否则为有组织点云数据集的高度/行数
height: 1
//width为一行的总点数
//对于无序数据集而言，width即为点云总数
width: 22998
//看到fields，可以先跳过看完point_step和row_step之后再回来
//field格式包括name,offset,datatype,count，具体定义见sensor_msgs/PointField.msg
//该点云所包括的信息有点云的xyz信息（即点云在激光雷达坐标系的位姿）以及强度信息
//offset此处的含义不是很明白，我的理解是对于name“x”而言，它的信息包含在第0字节-第3字节中；name“y”而言，包含在第4字节-第7字节；依次类推。欢迎有大佬帮我在此处解疑下
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "intensity"
    offset: 16
    datatype: 7
    count: 1
  - 
    name: "ring"
    offset: 20
    datatype: 4
    count: 1
//数据存储方式，包括大端与小端，具体解释见后文参考链接
is_bigendian: False
//存储一个点云所需的字节长度，单位为byte
point_step: 32
//存储一行点云所需的字节长度，单位为byte
//row_step = point_step * width
row_step: 735936
//点云数据信息，该数组解析方式由field进行定义
//data数组的大小为row_step * height
data: [51, 81, 165, 64, 127, 62, 141, 192, 255, ......]

根据field所定义的方式，可以对data数组进行解码，获取点云信息。不过我是直接用PCL库获取相关信息的。

3、PCL与sensor_msgs::PointCloud2的转换

笔者是基于ROS下作的相关点云转换，相关API接口如下所示，有兴趣可以自行探究：
ROS下相关PCL的API
主要是sensor_msgs::PointCloud2 、 pcl::PointCloud2 、pcl::PointCloud2 < pcl::PointXYZ > 三者的相互转换。实际上感觉就是几种结构体的相互转换，可以参考API。

3.1 sensor_msgs::PointCloud2 and pcl::PointCloud2

主要是利用pcl_conversions，根据自己转换需求，对应参数调用即可。

Demo：

const sensor_msgs::PointCloud2ConstPtr& cloud_msg
pcl::PCLPointCloud2* cloud_pcl = new pcl::PCLPointCloud2;
pcl_conversions::toPCL(*cloud_msg, *cloud_pcl);

sensor_msgs::PointCloud2 cloud_msg;
pcl::PCLPointCloud2 cloud_pcl;
pcl_conversions::moveFromPCL(cloud_pcl, cloud_msg);

3.2 sensor_msgs::PointCloud2 and pcl::PointCloud2< pcl::PointXYZ >

主要是利用pcl
Demo:

sensor_msgs::PointCloud2 cloud_msg;
pcl::PointCloud<pcl::PointXYZ> cloud_pcl_xyz;
pcl::fromROSMsg(cloud_msg, cloud_pcl_xyz);

pcl::PointCloud<pcl::PointXYZ> cloud_pcl_xyz;
sensor_msgs::PointCloud2 cloud_msg;
pcl::toROSMsg(cloud_pcl_xyz,cloud_msg);

相关链接：
1、大端与小端
2、点云处理方法