Livox Lidar 特征提取方式总结

传统的 旋转式雷达，激光固定在雷达的旋转部件上， 依靠转子的转动而获得360的旋转视野，由于旋转部件的存在，为了获得更加精准的数据，需要跟多的人工校准和更复杂的设计，也因此推高了其价格。 不过因为很直观的数据方式，所以 edge 特征和 plane 特征也比较直观。

Livox 雷达推出的特有的非重复扫描方式，使得特征提取变得不那么直观，而且LIvox 推出的自由的数据结构方式，加上ROS原有的 Pointcloud2 使得很多开源的SLAM 算法只支持一种数据格式， 导致很多辛辛苦苦采集的数据不能直接使用。

接下来就自己总结一下 Livox 激光雷达的一些特征

开源SLAM算法

Horizon 发布时间较早， 相关算法支持也最好。

• LIO-Livox : Lidar-Inertial Odometry, 使用了内置的 6 轴IMU， 目前只支持 horizon 雷达， 雷达数据结构只支持 livox_ros_driver/CustomMsg. LINK
• Livox-mapping: 仅使用激光雷达的建图包， 同时支持了 Mid 系列和 horizon 系统的扫描方式， 但Horizon 数据结构还是必须为 livox_ros_driver/CustomMsg， Mid 系列的数据类型为 sensor_msgs::PointCloud。LINK
• hku-mars/loam_livox: Lidar only 的建图包， 只支持 Mid 系列（sensor_msgs::PointCloud2） 格式的数据。 LINK
• BALM ，使用了bundle adjustment 的仅使用激光的建图包， 同时支持三种， horizon 支持 livox_ros_driver/CustomMsg 格式，Mid 系列的数据类型为 sensor_msgs::PointCloud， 也提供了velodyne 激光的 sensor_msgs::PointCloud 格式。link
• KIT-ISAS/lili-om: LINK LiDAR-Inertial Odometry, 但此处没有使用内置的IMU， 而是使用的的 9 轴 Xsens MTi-670 (200 Hz) IMU, Livox雷达内置的是一个 6 轴激光雷达， 支持Horizon 和 Velodyne 雷达 LINK

Horzion 特征提取

目前能找到的开源算法中， 都只支持 livox_ros_driver/CustomMsg 数据格式， 其内容为：
link

## livox_ros_driver/CustomMsg
# Livox publish pointcloud msg format.
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

## livox_ros_driver/CustomPoint
# Livox costom pointcloud format.
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

其特点是， 我们能够根据 激光数据的 timebase 和每个激光数据点的 offset_time 推出每个点的时间先后顺序，同时此数据结构也提供了 每个点所在的激光序号， Horizon中有五个激光束。

以官方的Livox_mapping 为例：
首先其使用 livox_repub.cpp 文件，将 livox livox_ros_driver::CustomMsg 数据转换成 sensor_msgs::PointCloud2 数据结构：

ros::Subscriber sub_livox_msg1 = nh.subscribe<livox_ros_driver::CustomMsg>(
      "/livox/lidar", 100, LivoxMsgCbk1);
pub_pcl_out1 = nh.advertise<sensor_msgs::PointCloud2>("/livox_pcl0", 100);

ros::Publisher pub_pcl_out0, pub_pcl_out1;
uint64_t TO_MERGE_CNT = 1; 
constexpr bool b_dbg_line = false;
std::vector<livox_ros_driver::CustomMsgConstPtr> livox_data;

void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
   
   
  livox_data.push_back(livox_msg_in);
  if (livox_data.size() < TO_MERGE_CNT) return;

  pcl::PointCloud<PointType> pcl_in;

  for (size_t j = 0; j < livox_data.size(); j++) {
   
   
    auto& livox_msg = livox_data[j];
    auto time_end = livox_msg->points.back().offset_time;
    for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
   
   
      PointType pt;
      pt.x = livox_msg->points[i].x;
      pt.y = livox_msg->points[i].y;
      pt.z = livox_msg->points[i].z;
      float s = livox_msg->points[i].offset_time / (float)time_end;

      pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp
      pt.curvature = s*0.1;
      pcl_in.push_back(pt);
    }
  }

  unsigned long timebase_ns = livox_data[0]->timebase;
  ros::Time timestamp;
  timestamp.fromNSec(timebase_ns);

  sensor_msgs::PointCloud2 pcl_ros_msg;
  pcl::toROSMsg(pcl_in, pcl_ros_msg);
  pcl_ros_msg.header.stamp.fromNSec(timebase_ns);
  pcl_ros_msg.header.frame_id = "/livox";
  pub_pcl_out1.publish(pcl_ros_msg);
  livox_data.clear();
}

其中主要部分是

auto time_end = livox_msg->points.back().offset_time;

PointType pt;
pt.x = livox_msg->points[i].x;
pt.y = livox_msg->points[i].y;
pt.z = livox_msg->points[i].z;
float s = livox_msg->points[i].offset_time / (float)time_end;

pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp
pt.curvature = s*0.1; 
pcl_in.push_back(pt);

其中在这里记录了currature , 是该点在总时间段的比例， 并将其放在 curvature 字段。在 intensity 字段放的 line number 和 反射率。

在 scanRegistration_horizon.cpp 中， 其定义了 horizon的特征提取的方式。

平面点(plane)的提取

    float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
                       laserCloud->points[i].y * laserCloud->points[i].y +
                       laserCloud->points[i].z * laserCloud->points[i].z);

    // if(depth < 2) depth_threshold = 0.05;
    // if(depth > 30) depth_threshold = 0.1;
    //left curvature
    float ldiffX = 
                laserCloud->points[i - 4].x + laserCloud->points[i - 3].x
                - 4 * laserCloud->points[i - 2].x
                + laserCloud->points[i - 1].x + laserCloud->points[i].x;

    float ldiffY = 
                laserCloud->points[i - 4].y + laserCloud->points[i - 3].y
                - 4 * laserCloud->points[i - 2].y
                + laserCloud->points[i - 1].y + laserCloud->points[i].y;

    float ldiffZ = 
                laserCloud->points[i - 4].z + laserCloud->points[i - 3].z
                - 4 * laserCloud->points[i - 2].z
                + laserCloud->points[i - 1].z + laserCloud->points[i].z;

    float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ;
    
 if(left_curvature < 0.01){
   
   

      std::vector<PointType> left_list;

      for(in