基于SC-LIO-SAM的SLAM实践

LIO-SAM是一个基于多传感器紧耦合的程序包：
https://github.com/TixiaoShan/LIO-SAM

SC-LIO-SAM在LIO-SAM的基础上把回环检测修改为Scan Context：

https://github.com/gisbi-kim/SC-LIO-SAM

感谢并膜拜大佬的开源！

详细安装配置步骤请直接查看github作者提供的步骤，

本文主要描述了如何在自己的设备上运行SC-LIO-SAM得到点云地图。

一.激光雷达数据准备

首先由于激光雷达是速腾聚创的，然而SC-LIO-SAM中默认的参数配置中只有velodyne和ouster的。其实很多SLAM的开源程序包都是基于velodyne参数的，每次都去修改各种参数很是麻烦，所以这里就一劳永逸即直接把rslidar的数据转化为velodyne的数据发布出去！

参考: https://blog.csdn.net/weixin_42141088/article/details/117222678?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163048329916780357259963%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=163048329916780357259963&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~baidu_landing_v2~default-1-117222678.first_rank_v2_pc_rank_v29&utm_term=%E9%80%9F%E8%85%BE%E8%81%9A%E5%88%9B%E8%BD%ACvelodyne&spm=1018.2226.3001.4187

rslidar_to_velodyne的代码如下:

#include <ros/ros.h>#include <sensor_msgs/PointCloud2.h>#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>// rslidar和velodyne的格式有微小的区别
// rslidar的点云格式
struct RsPointXYZIRT {PCL_ADD_POINT4D;uint8_t intensity;uint16_t ring = 0;double timestamp = 0;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT,(float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp))// velodyne的点云格式
struct VelodynePointXYZIRT {PCL_ADD_POINT4DPCL_ADD_INTENSITY;uint16_t ring;float time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;POINT_CLOUD_REGISTER_POINT_STRUCT(VelodynePointXYZIRT,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring)(float, time, time))ros::Subscriber subRobosensePC;
ros::Publisher pubRobosensePC;bool has_nan(RsPointXYZIRT point)
{if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) {return true;} else {return false;}
}void rsHandler_XYZIRT(const sensor_msgs::PointCloud2& pc_msg)
{pcl::PointCloud<RsPointXYZIRT> pc_in;pcl::fromROSMsg(pc_msg, pc_in);pcl::PointCloud<VelodynePointXYZIRT> pc_out;if (pc_in.points.empty())return;double timestamp = pc_in.points.front().timestamp;for (auto& point : pc_in.points) {if (has_nan(point))continue;VelodynePointXYZIRT new_point;new_point.x = point.x;new_point.y = point.y;new_point.z = point.z;new_point.intensity = point.intensity;new_point.ring = point.ring;new_point.time = point.timestamp - timestamp;pc_out.points.emplace_back(new_point);}// publishsensor_msgs::PointCloud2 pc_new_msg;pcl::toROSMsg(pc_out, pc_new_msg);pc_new_msg.header = pc_msg.header;pc_new_msg.header.frame_id = "velodyne";pubRobosensePC.publish(pc_new_msg);
}int main(int argc, char** argv)
{ros::init(argc, argv, "rs_to_velodyne_node");ros::NodeHandle nh;pubRobosensePC = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 1);subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZIRT);ROS_INFO("Listening to /rslidar_points ......");ros::spin();return 0;
}

二.IMU参数配置

设置imuTopic

//IMU和激光雷达的位姿关系
extrinsicRot: [1, 0, 0,0, 1, 0,0, 0, 1]extrinsicRPY: [1, 0, 0,0, 1, 0,0, 0, 1]

三.一些需要调整的地方

程序在运行的时候需要保存一些中间文件以及最后建立的点云地图，所以你需要设置好保存的位置。例如在yaml文件里进行修改路径：

savePCDDirectory: "/home/tf/ROS/auto_drive/src/slam/SC-LIO-SAM/map/"

当然我推荐是把yaml的设置路径注释掉，然后之间在launch文件里设置路径,这样更有泛用性：

<param name="lio_sam/savePCDDirectory" value="$(find lio_sam)/map/"/>

另外原程序中是ctrl+c后自动保存点云地图，但是我在建立一张比较大型的点云地图时出现了还没有保存完毕，程序就退出了的尴尬情况。于是需要稍微修改一下程序，大概就思想就是，添加的一个话题接口，然后再回调函数里保存点云地图,然后记得把原来保存地图的程序段删掉:

void saveCB(const std_msgs::String::ConstPtr& msg)
{// save pose graph (runs when programe is closing)cout << "****************************************************" << endl;cout << "Saving the posegraph ..." << endl; // giseopfor (auto& _line : vertices_str)pgSaveStream << _line << std::endl;for (auto& _line : edges_str)pgSaveStream << _line << std::endl;pgSaveStream.close();// pgVertexSaveStream.close();// pgEdgeSaveStream.close();const std::string kitti_format_pg_filename { savePCDDirectory + "optimized_poses.txt" };saveOptimizedVerticesKITTIformat(isamCurrentEstimate, kitti_format_pg_filename);// save mapcout << "****************************************************" << endl;cout << "Saving map to pcd files ..." << endl;// save key frame transformationspcl::io::savePCDFileASCII(savePCDDirectory + "trajectory.pcd", *cloudKeyPoses3D);pcl::io::savePCDFileASCII(savePCDDirectory + "transformations.pcd", *cloudKeyPoses6D);// extract global point cloud mappcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";}// down-sample and save corner clouddownSizeFilterCorner.setInputCloud(globalCornerCloud);downSizeFilterCorner.filter(*globalCornerCloudDS);pcl::io::savePCDFileASCII(savePCDDirectory + "cloudCorner.pcd", *globalCornerCloudDS);// down-sample and save surf clouddownSizeFilterSurf.setInputCloud(globalSurfCloud);downSizeFilterSurf.filter(*globalSurfCloudDS);pcl::io::savePCDFileASCII(savePCDDirectory + "cloudSurf.pcd", *globalSurfCloudDS);// down-sample and save global point cloud map*globalMapCloud += *globalCornerCloud;*globalMapCloud += *globalSurfCloud;pcl::io::savePCDFileASCII(savePCDDirectory + "cloudGlobal.pcd", *globalMapCloud);cout << "****************************************************" << endl;cout << "Saving map to pcd files completed" << endl;
}

四.建图啦

程序开始会提示无法删除xxx，这个没啥事，只要你确定设置的路径是对的，就无视就好。。。

roslaunch lio_sam run.launch

建图效果还是非常nice的！