LIO-SAM笔记（三）适配Livox 激光雷达

1.固态激光雷达和机械激光雷达的区别

LOAM、Lego-LOAM、LIO-SAM使用的激光雷达Velodyne都是机械激光雷达。通过机械方式改变扫描方向。通俗点就是一个激光笔放在一个电动机上旋转，激光笔跟着电动机进行360度高速旋转。

工作原理参考：[科普]激光雷达LIDAR工作原理|英飞凌开发者技术社区

固定激光雷达（以livox为例）

通过阵列干涉或者是mems改变扫描的方向。说白了就是一个激光笔放在那不动，他可以自己改变激光射出的方向。如下就是livox激光线的示意图，“激光笔”是不动的，但是内部有阵列干涉，通过干涉、反射等操作，让“激光笔”发射的点打在不同的位置。如下图，就打出了一个类似雪花的的形状。看下图中的第1张图，蓝色的是起点，红色的终点。

原文链接：https://blog.csdn.net/weixin_40331125/article/details/106136136

传统激光雷达普遍采用机械扫描方式，扫描路径随时间重复。而Livox 激光雷达采用了独特的扫描⽅式，扫描路径不会重复。在非重复扫描方式中，视场中被激光照射到的区域面积会随时间增大，这意味着视场覆盖率随时间推移而显著提高，可减小视场内物体被漏检的概率，有助于探测视场中的更多细节。

2.LIO-SAM 适配MID360

LIO-SAM支持的激光雷达类型是机械激光雷达，其主要特点是点云数据以线束顺序进行排列，知道每个点在哪个线ring以及该点距离该帧点云起始时刻的时间间隔time，对于角特征和面特征的提取也是利用了这一特点。直接将Livox-Mid-360的点云数据放入LIO-SAM中是无法运行的，它的点云数据格式有三种

// 0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format
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
float64 timestamp       # Timestamp of point// 1 -- Livox customized pointcloud format
std_msgs/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// 2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS)

采用自定义的点云格式，在livox的ROS驱动包中，将Mid-360的点云数据等效于以线束顺序进行排列，这样就可以直接使用LIO-SAM。

2.1 MID-360坐标系

lidar坐标系IMU到坐标系的外参

// lidar和imu xyz方向相同，旋转矩阵是单位阵； 
[0.9999161, 0.0026676,  0.0126707, -0.011,-0.0025826, 0.9999741, -0.0067201, -0.0234,-0.0126883, 0.0066868,  0.9998971, 0.044,0.0,       0.0,        0.0,       1.0]

 

2.2 代码修改

参考：nkymzsy/LIO-SAM-MID360

 代码运行

// MID360是六轴IMU
roslaunch lio_sam run6axis.launch

1.配置文件修改

  # Sensor Settingssensor: livox                              # lidar sensor type, 'velodyne' or 'ouster' or 'livox'N_SCAN: 4                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)Horizon_SCAN: 6000                         # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)downsampleRate: 1                          # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1lidarMinRange: 1.0                         # default: 1.0, minimum lidar range to be usedlidarMaxRange: 40.0                        # default: 1000.0, maximum lidar range to be used

mid360相当于4根线的激光雷达

2.MID360自定义数据对应的结构体

struct LiovxPointCustomMsg
{PCL_ADD_POINT4DPCL_ADD_INTENSITY;float time;uint16_t ring;uint16_t tag;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (LiovxPointCustomMsg,(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, time, time)(uint16_t, ring, ring) (uint16_t, tag, tag)
)
using PointXYZIRT = LiovxPointCustomMsg;// 自定义的点云msg转为PCL点云
for(uint i=0;i<Msg.point_num-1;i++)
{point.x=Msg.points[i].x; point.y=Msg.points[i].y; point.z=Msg.points[i].z; point.intensity=Msg.points[i].reflectivity; point.tag=Msg.points[i].tag; point.time=Msg.points[i].offset_time*1e-9; point.ring=Msg.points[i].line; cloud.push_back(point);
}

3.曲率计算不同

//mid360
void calculateSmoothness()
{int cloudSize = extractedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++){float diffRange = cloudInfo.pointRange[i-2]  + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 4+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2];            cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;cloudNeighborPicked[i] = 0;cloudLabel[i] = 0;// cloudSmoothness for sortingcloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}
}//原代码
void calculateSmoothness()
{int cloudSize = extractedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++){float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]+ cloudInfo.pointRange[i+5];            cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;cloudNeighborPicked[i] = 0;cloudLabel[i] = 0;// cloudSmoothness for sortingcloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}
}

---

疑问

1.对于MID360六轴IMU sensor_msgs::Imu 中有加速度和角速度，orientation中有数据么？？？

2.六轴IMU不是没有直接测得RPY角么？为什么代码transformUpdate()没有修改呢？

/**用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
*/
void transformUpdate()