Livox雷达驱动程序发布点云格式CustomMsg、PointCloud2、pcl::PointXYZI、pcl::PointXYZINormal解析

Livox ROS驱动程序专门用于连接Livox LiDAR 产品。
该驱动程序安装可以参考R3LIVE(升级R2LIVE)：编译与运行中的2.2小节。

livox_ros_driver可以发布多种格式的点云数据，今天仔细看看这些点云的具体差别：

1. launch文件说明

驱动程序中所有的 launch 文件都位于 “ws_livox/src/livox_ros_driver/launch” 路径下，不同的 launch 文件拥有不同的配置参数值， 应用在不同的场景中:

• livox_lidar.launch、livox_hub.launch ：向外发布 pointcloud2 格式的点云数据
• livox_lidar_msg.launch、livox_hub_msg.launch：向外发布览沃自定义点云数据
• livox_lidar_rviz.launch、livox_hub_rviz.launch：向外发布 pointcloud2格式的点云数据并自动加载rviz

2. livox_ros驱动程序内部主要参数配置说明

览沃 ROS 驱动程序中的所有内部参数都位于 launch 文件中，这是经常用到的三个参数:

参数名	详细说明	默认值
publish_freq	设置点云发布频率 浮点数据类型，推荐值 5.0，10.0，20.0，50.0 等。	10.0
multi_topic	LiDAR 设备是否拥有独立的 topic 发布点云数据 0 – 所有 LiDAR 设备共同使用同一个 topic 发送点云数据 1 – 每个 LiDAR 设备各自拥有独立的 topic 发布点云数据	0
xfer_format	设置点云格式 0 – 览沃 pointcloud2(PointXYZRTL) 点云格式 1 – 览沃自定义点云数据格式 2 – PCL库中标准 pointcloud2(pcl::PointXYZI) 点云格式	0
最终点云发布的格式主要取决于.launch文件中的xfer_format参数，如：
livox_lidar_msg.launch中参数xfer_format = 1

	<arg name="xfer_format" default="1"/>

发布出的/livox/lidar点云话题就是livox_ros_driver/CustomMsg格式。

3. 点云数据格式详细说明

3.1 pointcloud2(PointXYZRTL) 点云格式

float32 x                                 # X axis, unit:m
float32 y                                 # Y axis, unit:m
float32 z                                 # Z axis, unit:m
float32 intensity                 # the value is reflectivity, 0.0~255.0
uint8 tag                                # livox tag
uint8 line                               # laser number in lidar

3.2 CustomMsg格式

Header header             # ROS standard message header
uint64 timebase          # The time of first point
uint32 point_num      # Total number of pointclouds
uint8 lidar_id               # Lidar device id number
uint8[3] rsvd                 # Reserved use
CustomPoint[] points    # Pointcloud data

上述自定义数据包中的自定义点云（CustomPoint）格式 :

uint32 offset_time      # offset time relative to the base time
float32 x               # X axis, unit:m
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
uint8 reflectivity      # reflectivity, 0~255
uint8 tag               # livox tag
uint8 line              # laser number in lidar

先来看看livox坐标系的定义：

（1）line

最后一行line是激光雷达扫描的线数（livox avia为6线），从原始扫描点云可以看出，后期可以按线来对原始点处理、筛选

（2）Tag

Tag: 主要指示多回波信息及噪点信息。是一个二进制表达，标记信息的格式如下：
bit7 bit6 bit5 bit4 bit3 bit2 bit1 bit0

bit7 bit6 保留位

bit5 bit4 回波序号 :
00: 第 0 个回波
01: 第 1 个回波
10: 第 2 个回波
11: 第 3 个回波
由于 Livox Avia 采用同轴光路，即使外部无被测物体，其内部的光学系统也会产生一个回波，该回波记为第 0 个回波。随后，若激光出射方向存在可被探测的物体，则最先返回系统的激光回波记为第 1 个回波，随后为第 2 个回波，以此类推。如果被探测物体距离过近（例如 1.5 m），第 1 个回波将会融合到第 0 个回波里，该回波记为第 0 个回波。

bit3 bit2 基于强度的点属性：
00：正常点
01：回波能量噪点置信度高
10：回波能量噪点置信度中
11：保留位
基于回波能量强度判断采样点是否为噪点。通常情况下，激光光束受到类似灰尘、雨雾、雪等干扰产生的噪点的回波能量很小。目前按照回波能量强度大小将噪点置信度分为二档：01 表示回波能量很弱，这类采样点有较高概率为噪点，例如灰尘点；10 表示回波能量中等，该类采样点有中等概率为噪点，例如雨雾噪点。噪点置信度越低，说明该点是噪点的可能性越低。

bit1 bit0 基于空间位置的点属性 :
00：正常点
01：空间噪点置信度高
10：空间噪点置信度中
11：空间噪点置信度低
基于采样点的空间位置判断是否为噪点。例如，激光探测测距仪在测量前后两个距离十分相近的物体时，两个物体之间可能会产生拉丝状的噪点。目前按照不同的噪点置信度分为三档， 噪点置信度越低，说明该点是噪点的可能性越低。

（3）时间戳

点云数据及 IMU 数据中都包含时间戳信息。

（4）reflectivity 目标反射率

目标反射率：以 0 至 255 表示。其中 0 至 150 对应反射率介于 0 至 100% 的漫散射物体；而 151 至255 对应全反射物体。

（5）x y z 坐标信息

坐标信息：Livox Avia 的坐标信息可表示为直角坐标（x, y, z）或球坐标（r, θ, φ），其直角坐标和球坐标的对应关系如下图所示。如果前方无被探测物体或者被探测物体超出量程范围（例如 600 m），在直角坐标系下，点云输出为（0，0，0）；在球坐标系下，点云输出为（0，θ, φ）。

通过以下程序可以把数据打印出来详细研究：

#include 
#include 
#include 
#include 
using namespace std;
void livox_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
  cout << "point_num:" << msg->point_num << " id:" << msg->lidar_id << " time:" << msg->timebase << endl;
  for (uint i = 1; i < msg->point_num; i++)
  {

    cout << i << ": " << msg->points[i].x << " " << msg->points[i].y << " " << msg->points[i].z;
    cout << " reflectivity:" << msg->points[i].reflectivity << " tag:" << msg->points[i].tag  << " line:" << msg->points[i].line << " offset_time:" << msg->points[i].offset_time << endl;
    
  }

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "front_end");
  ros::NodeHandle n;

  ros::Subscriber sub_points;
  sub_points = n.subscribe("/livox/lidar", 1000, livox_handler, ros::TransportHints().tcpNoDelay());
  
  ros::spin();
  return 0;
}

3.3 pointcloud2(pcl::PointXYZI) 点云格式

请参考 PCL 库 point_types.hpp 文件中 the pcl::PointXYZI 数据结构。
pcl::PointXYZI ：

  struct PointXYZI : public _PointXYZI
  {
    inline PointXYZI (const _PointXYZI &p)
    {
      x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
      intensity = p.intensity;
    }

    inline PointXYZI ()
    {
      x = y = z = 0.0f;
      data[3] = 1.0f;
      intensity = 0.0f;
    }
    inline PointXYZI (float _intensity)
    {
      x = y = z = 0.0f;
      data[3] = 1.0f;
      intensity = _intensity;
    }
    friend std::ostream& operator << (std::ostream& os, const PointXYZI& p);
  };

pcl::PointXYZ 这是最简单的点的类型，存储着点的x,y,z信息。pcl::PointXYZI除此之外还包含了点的密集程度信息。

另外还经常用到PointXYZINormal：

struct PointXYZINormal : public _PointXYZINormal
  {
    inline PointXYZINormal (const _PointXYZINormal &p)
    {
      x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
      normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f;
      curvature = p.curvature;
      intensity = p.intensity;
    }

    inline PointXYZINormal ()
    {
      x = y = z = 0.0f;
      data[3] = 1.0f;
      normal_x = normal_y = normal_z = data_n[3] = 0.0f;
      intensity = 0.0f;
      curvature = 0;
    }
  
    friend std::ostream& operator << (std::ostream& os, const PointXYZINormal& p);
  };

两者相比较，PointXYZINormal包含除了点的x,y,z信息和密集程度信息之外，还有坐标(x,y,z)和曲率。