基于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
#include
#include
#include
#include
// 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_POINT4D
PCL_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 pc_in;
pcl::fromROSMsg(pc_msg, pc_in);
pcl::PointCloud 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);
}
// publish
sensor_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("/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文件里设置路径,这样更有泛用性：
另外原程序中是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; // giseop
for (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 map
cout << "****************************************************" << endl;
cout << "Saving map to pcd files ..." << endl;
// save key frame transformations
pcl::io::savePCDFileASCII(savePCDDirectory + "trajectory.pcd", *cloudKeyPoses3D);
pcl::io::savePCDFileASCII(savePCDDirectory + "transformations.pcd", *cloudKeyPoses6D);
// extract global point cloud map
pcl::PointCloud::Ptr globalCornerCloud(new pcl::PointCloud());
pcl::PointCloud::Ptr globalCornerCloudDS(new pcl::PointCloud());
pcl::PointCloud::Ptr globalSurfCloud(new pcl::PointCloud());
pcl::PointCloud::Ptr globalSurfCloudDS(new pcl::PointCloud());
pcl::PointCloud::Ptr globalMapCloud(new pcl::PointCloud());
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 cloud
downSizeFilterCorner.setInputCloud(globalCornerCloud);
downSizeFilterCorner.filter(*globalCornerCloudDS);
pcl::io::savePCDFileASCII(savePCDDirectory + "cloudCorner.pcd", *globalCornerCloudDS);
// down-sample and save surf cloud
downSizeFilterSurf.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的！