在SLAM中如何拼接使用多个激光雷达传感器数据

为了保证激光雷达的360°环境覆盖，准备在车体上安装三个雷神16线的激光雷达，以前二后一的布局分布，大致位置情况如下所示：

其中蓝色区域为雷达1所保留的点云区域，黄色区域为雷达2所保留的点云区域，绿色区域为雷达3所保留的点云区域，从而实现整车的周围环境的全覆盖。但是这种情况会降低了多个激光雷达的鲁棒性，万一某个激光雷达出现故障或者污渍遮挡，就会出现数据点云盲区，因此后面会尝试融合多个激光雷达的点云数据。

关于激光雷达的IP配置问题，雷神16线的激光雷达出厂默认配置ip为192.168.1.200。UDP设备包端口号2368，UDP数据包端口号2369。

使用多个激光雷达只需要使用一个交换机组成一个局域网，当然路由器也可以，配置在同一网段也就当作交换机去使用，配置在不同网段的话就需要配置电脑的网关了。这里将其配置在同一个网段中。

在同一个网段中，可以将不同的主机设置不同的IP地址用于区分，也可以使用相同的IP地址与不同的端口号去区分，这里使用第二种，即设置不同的端口号。但是要保证激光雷达的IP地址和本机IP地址不能冲突。

<arg name="device_ip1" default="192.168.1.200" />
<arg name="device_ip2" default="192.168.1.200" />

<arg name="msop_port1" default="2368" />
<arg name="difop_port1" default="2369" />
<arg name="msop_port2" default="2370" />
<arg name="difop_port2" default="2371" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="true" />

可在雷神激光雷达提供的上位机软件中修改激光雷达的IP地址以及端口号，具体不在详细说明，操作流程比较简单。当修改好端口号之后，就可以直接使用launch文件去同时启动三个激光雷达，在默认的launch文件中提供了同时启动两个激光雷达的启动方法，三个同理，分别启动三次node节点即可，当然每次的node节点都要给不同的命名，否则就会冲突崩溃掉。

该三个激光雷达的激光点云数据分别在其自身的坐标系下，即laser_link_left，laser_link_right，laser_link_rear。关于雷神激光雷达的坐标系据售后所说方向不确定，当前所使用的激光雷达为x轴指向雷达后方的右手系。在提供的launch文件中，可以通过修改参数：

    <param name="scan_start_angle" value="0.0"/>
    <param name="scan_end_angle" value="36000.0"/>

确定激光雷达的起始角度和终止角度，不过0°的方向为水平向左（以雷神标志为前方向），且没有负值，所以如果根据角度去切割的话比较麻烦。

if ((azimuth_corrected_f < scan_start_angle_) || (azimuth_corrected_f > scan_end_angle_)) continue;

因此直接在将激光点（range，angle）转换成点云数据（x,y,z）的时候，根据坐标将其过滤掉。如下所示，关于阈值就随便举个列子，根据实际情况进行调整，即可把重叠区域的点云数据过滤掉。

if(dir = "left" && point.x < x_threshold_left && point.y < y_threshold_left) continue;
if(dir = "right" && point.x < x_threshold_right && point.y < y_threshold_right) continue;
if(dir = "rear" && point.x < x_threshold_rear && point.y < y_threshold_rear) continue;

// Pack the data into point msg
new_point.x = point.x;
new_point.y = point.y;
new_point.z = point.z;
new_point.azimuth = angles::from_degrees(azimuth_corrected * ROTATION_RESOLUTION);
new_point.distance = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2));
new_point.intensity = point.intensity;

接下来要对三个激光雷达进行外参的标定Autoware Multi LiDAR Calibrator，使用Autoware提供的开源算法包，通过NDT算法去计算两组点云的外参。

提供一个初始的外参数，输入两组点云数据便可以得到标定的外参。做好外参标定后，设置其中一个激光雷达作为主传感器，将其余的传感器的点云数据平移旋转过去，最后点云拼接。这里以laser_link_right为主传感器。

laser_link_right laser_link_left 0 -0.7 0 0 0 0
laser_link_right laser_link_rear 1.14792 -0.362289 -0.0637275 3.1415927 0 0

当得到标定后的tf关系后，使用四元数的变换去旋转坐标值，如果使用tf的监听的话太耗时，并且这里是固定的静态tf关系，直接变换会更快。为了提高实时性，直接在生成激光点云的地方进行tf变换，即在rawdata.cc中，发布在同一个frame_id下的三个topic名称。

tf::Quaternion transform;
transform.setRPY(row, pitch, yaw);
transform = transform.inverse();
tf::Quaternion transform_point(point.x,point.y,point.z,0);
transform_point= transform*transform_point*transform.inverse();
new_point.x = transform_point.x()+transform_x;
new_point.y = transform_point.y()+transform_y;
new_point.z = transform_point.z()+transform_z;

以上为一个通过四元数进行旋转平移的模板。在使用激光点云数据时，目前是打算通过同步回调三个激光topic去使用，不知道如果对其进行拼接再发布会不会影响实时性，使其与其他传感器的时间戳不同步，有待测试！

自己的想法，后面会持续优化更新！其他大神有新的建议欢迎私信！

在使用雷神16线激光雷达的过程中发现几个问题：
1.雷神 is_dense = false，而在velodyne中是为true，不过可以在源码中直接修改。
2.在雷神中，设定的范围阈值，超过该阈值的点会被设置为NAN，就直接修改成跳过continue了。
3.雷神中的线数不叫ring，而叫lines。
4.雷神的自定义点云中并没有time。
以上不同导致不能直接适配于某些SLAM算法，需要对自定义的点云结构体进行修改。