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的数据发布出去!
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_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<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);
}
// 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<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; // 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<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 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的!