基于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_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的！