镭神16线激光雷达跑SC-LeGo-LOAM算法

一、运行环境

环境：ubutu18.04+ros:melodic+pcl:1.8+gtsam+metis

环境配置：

• Eigen 3.3.4
• PCL 1.8.1 （1.11不能用）
• ceres 2.0.0
• gtsam 4.0.0

雷达参数：

二、下载编译运行

已修改配置的源码文件，下载catkin_make即可跑通。

https://gitee.com/zhankun3280/lslidar_c16_lego_loam

首先，要注意的是SC-LeGo-LOAM和16线雷神激光雷达源码的版本不一样，源码也可能稍有区别。这里找的是修改之后能跑得通的两个版本。
还要注意雷达的IP，我配置的是192.168.0.200

git clone https://gitee.com/zhankun3280/lslidar_c16_lego_loam

catkin_make
roslaunch lslidar_c16_decoder lslidar_c16.launch --screen

roslaunch lego_loam run.launch

三、lslidar的相关修改

• 首先LeGO-LOAM默认的接受的topic name是velodyne_points，点云的frame_id是velodyne。
• 镭神驱动发布的topic name为lslidar_point_cloud，frame_id为laser_link。
• 这里需要修改一下lslidar_c16.launch文件，remap一下topic name，再修改frame_id为velodyne。

<launch>
  <arg name="device_ip" default="192.168.0.200" />
  <arg name="msop_port" default="2368" />
  <arg name="difop_port" default="2369" />
  <arg name="return_mode" default="1" />
  <arg name="time_synchronization" default="false" />


  <node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen">
    <param name="device_ip" value="$(arg device_ip)" />
    <param name="msop_port" value="$(arg msop_port)" />
    <param name="difop_port" value="$(arg difop_port)"/>
    <param name="frame_id" value="velodyne"/>      <!--laser_link-->
    <param name="add_multicast" value="false"/> 
    <param name="group_ip" value="224.1.1.2"/>
    <param name="rpm" value="600"/>
    <param name="return_mode" value="$(arg return_mode)"/>
    <param name="time_synchronization" value="$(arg time_synchronization)"/>
  </node>

  <node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen">
    <param name="calibration_file" value="$(find lslidar_c16_decoder)/params/lslidar_c16_db.yaml" />
    <param name="scan_frame_id" value="laser_link"/>
    <param name="min_range" value="0.15"/>
    <param name="max_range" value="150.0"/>
    <param name="cbMethod" value="true"/>
    <param name="config_vert" value="true"/>
    <param name="print_vert" value="false"/>
    <param name="return_mode" value="$(arg return_mode)"/>
    <param name="degree_mode" value="2"/>
    <param name="config_vert_file" value="false"/>
    <param name="distance_unit" value="0.25"/>
    <param name="time_synchronization" value="$(arg time_synchronization)"/>
    <param name="scan_start_angle" value="0.0"/>
    <param name="scan_end_angle" value="36000.0"/>
    <param name="scan_num" value="8"/>
    <param name="publish_scan" value="false"/>
    <remap from="lslidar_point_cloud" to="/velodyne_points" />   <!--add-->
  </node>
  
</launch>

由于镭神C16的驱动中对点云强度的定义是uint8_t类型,LOAM能接收的是float32类型，并且LOAM在运行时是根据采集的点云不停的计算来估计自身位置的，所以如果点云强度不匹配，会出现坐标轴baselink漂移的现象。（注意：由于没有惯导，在LOAM运行时，如果雷达前方出现不规则快速运动的物体，比如拿书快速扫，雷达坐标系都会出现漂移现象，所以建议利用传感器来获取位置信息，这是最稳妥的方法）。

将lslidar_c16/lslidar_c16_decoder/src/rawdata.cc 中的 数据类型修改为 PCL常用的点云处理数据类型 pcl::PointXYZI point 即可 ，修改后注释雷达输出的 ring 和 timestamp的两个话题.

                    pcl::PointXYZI point;
                    //VPoint point;

                    if ((azimuth_corrected_f < scan_start_angle_) || (azimuth_corrected_f > scan_end_angle_)) continue;
                    if (distance2 > max_distance_ || distance2 < min_distance_) {
                        point.x = NAN;
                        point.y = NAN;
                        point.z = NAN;
                        point.intensity = 0;
                        //point.lines = dsr;
                        pointcloud->at(2 * this->block_num + firing, dsr) = point;
                    } else {
                        if (cbMethod_) {
                            point.x = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth +
                                      R1_ * cos(arg_horiz_orginal);
                            point.y = -distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth +
                                      R1_ * sin(arg_horiz_orginal);
                            point.z = distance2 * sin_scan_altitude_caliration[dsr] + 0.426 / 100.f;
                        } else {
                            point.x = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth;
                            point.y = -distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth;
                            point.z = distance2 * sin_scan_altitude_caliration[dsr];
                        }

                        point.intensity = intensity;
                        //point.lines = dsr;

                        pointcloud->at(2 * this->block_num + firing, dsr) = point;

四、SC-LeGO-LOAM的相关修改

（1）修改utility.h

extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used

...

// leishen16
// lslidar_c16
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 2000;
extern const float ang_res_x = 0.18;
extern const float ang_res_y = 2.0;
extern const float ang_bottom = 15.0;
extern const int groundScanInd = 10;

（2）修改imageproject.cpp

void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

	cloudHeader = laserCloudMsg->header;
	// cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
	pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
	// Remove Nan points
	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
	// have "ring" channel in the cloud
	if (useCloudRing == true){
		pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
		if (laserCloudInRing->is_dense == false) {
			ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
			ros::shutdown();
		}  
	}
}

注释掉cloudHeader.stamp = ros::Time::now()

五、离线建图

# 运行 lego-loam
roslaunch lego_loam run.launch

# 播放 指定的rosbag
rosbag play *.bag --clock --topic /velodyne_points /imu/data  
rosbag play *.bag --clock /lslidar_point_cloud:=/velodyne_points

如果没有用到imu，就只订阅雷达话题。虽然 /imu/data 是可选的，但如果提供的话，它可以大大提高估计的准确性。

六、实时在线建图

<param name="/use_sim_time" value="false" />

参考链接：
[1] 镭神16线雷达适配Lego-Loam开源框架
[2] 32线镭神雷达跑LeGO-LOAM：3D 激光SLAM
[3] LeGO-LOAM初探：原理，安装和测试
[4] 搭建实验室3d slam 移动小车 4.1jackal小车+镭神32线激光雷达lego-loam建图