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

SC-LIO-SAM在LIO-SAM的基礎上把回環(huán)檢測修改為Scan Context：

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

感謝并膜拜大佬的開源！

詳細安裝配置步驟請直接查看github作者提供的步驟，

本文主要描述了如何在自己的設備上運行SC-LIO-SAM得到點云地圖。

一.激光雷達數(shù)據準備

????????首先由于激光雷達是速騰聚創(chuàng)的，然而SC-LIO-SAM中默認的參數(shù)配置中只有velodyne和ouster的。其實很多SLAM的開源程序包都是基于velodyne參數(shù)的，每次都去修改各種參數(shù)很是麻煩，所以這里就一勞永逸即直接把rslidar的數(shù)據轉化為velodyne的數(shù)據發(fā)布出去！

????????參考:?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的格式有微小的區(qū)別 // 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參數(shù)配置

? ? ? ? 設置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后自動保存點云地圖，但是我在建立一張比較大型的點云地圖時出現(xiàn)了還沒有保存完畢，程序就退出了的尷尬情況。于是需要稍微修改一下程序，大概就思想就是，添加的一個話題接口，然后再回調函數(shù)里保存點云地圖,然后記得把原來保存地圖的程序段刪掉:

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的！

總結

以上是生活随笔為你收集整理的基于SC-LIO-SAM的SLAM实践的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。