LIO-SAM 详读代码笔记

先上论文原著 ：T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, D. Rus. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2020.

论文地址：https://arxiv.org/pdf/2007.00258.pdf

源码链接：GitHub - TixiaoShan/LIO-SAM: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

简介：作者Tixiao Shan在2018年发表过非常经典的基于地面优化的slam方法：LeGO-LOAM，这一篇论文是在MIT工作时发表的。这两篇文章都是从LOAM 算法衍生而来，这篇文章LIO-SAM实际上是LeGO-LOAM的扩展版本，也是基于因子图优化的多传感器融合方法的经典之作，特此，详细研读其代码，学习经典，备忘。

先看官网给出的代码架构图(或者说是数据流图)：

图1.代码架构图(数据流图)

翻阅源代码或者从图1 中可以看到整slam系统，主要由imuPreintegration.cpp, imagePaProjection.cpp, featureExtraction.cpp, mapOptimization.cpp 4个文件构成。外部数据主要有IMU, Pointcloud, GPS,论文里说，还有回环检测事件输入。

图2.ros topic 发布与订阅关系图关系图

从图1 系统架构图中明确看到，imuPreintegration.cpp模块除了接收 Liar/odometry和 imu 的原数据之外，不再以来其他模块的数据，相对比较独立。本博文县先从imuPreintegration.cpp下手分析，没有太多环环相扣的依赖关系。再分析其他三个模块。其他三个模块围绕着Lidar 和 imu 的数据进行展开，耦合性比较强。

0. include/utility.h

在分析4大主要模块代码之前，先要看看include/utility.h 文件，该文件中定义了一个基类ParamServer，其他4个源文件都继承了这个基类，公用了该类中定义的系统参数变量。在该类中，读取ROS 参数服务其中的参数，ros参数服务器的参数是从config/params.yaml中间加载的；还定义了一些工具类的方法。

ParamServer 成员变量

关于IMU和雷达之间的外参。个人感觉params.yaml这里写了一个Extrinsics (lidar -> IMU)，有点问题，应该写成IMU->Lidar。因为它其实是​，也就是说在IMU坐标系下的一个点，左乘，就可以变换到lidar坐标系下。 针对不同的数据集，这个外参设置不一样，正常情况下，我们用的机器人，x射向正前方，y射向左边，z射向头顶。雷达和IMU都是这样摆放，所以extrinsicRot和extrinsicRPY全部弄成单位矩阵就行。差的不太远的话，extrinsicTrans弄成[0，0，0]就行。作者给出的外参可能有些看不懂，可以看下图，他的IMU 跟常规机器人安装方式不太一样。要根据实际情况来。 不过追求精确的话还是标定了一下，就用浙大在2020开源的标定工具，lidar_IMU_calib标定。

/*
1. imuConverter函数，这个函数之后会被频繁调用。它主要的作用，是把IMU的信息，
从IMU坐标系，转换到雷达坐标系。
（注意，这个函数只旋转，没有平移，和真正的雷达坐标系之间还是差了一个平移的。
至于为什么没有平移，在imuPreintegration.cpp文件中，
还有两个imu2Lidar，lidar2imu变量，这俩变量只有平移，没有旋转。
2. 这个slam 系统只接收9轴IMU 数据，6轴的imu 数据 将不能启动，
9轴比6轴imu 多了 roll pitch  yaw 的旋转角度数据*/
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
    {
        sensor_msgs::Imu imu_out = imu_in;
        // rotate acceleration
        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
        acc = extRot * acc;
        imu_out.linear_acceleration.x = acc.x();
        imu_out.linear_acceleration.y = acc.y();
        imu_out.linear_acceleration.z = acc.z();
        // rotate gyroscope
        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
        gyr = extRot * gyr;
        imu_out.angular_velocity.x = gyr.x();
        imu_out.angular_velocity.y = gyr.y();
        imu_out.angular_velocity.z = gyr.z();
        // rotate roll pitch yaw
        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
        Eigen::Quaterniond q_final = q_from * extQRPY;
        imu_out.orientation.x = q_final.x();
        imu_out.orientation.y = q_final.y();
        imu_out.orientation.z = q_final.z();
        imu_out.orientation.w = q_final.w();

        if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
        {
            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
            ros::shutdown();
        }

        return imu_out;
    }

事实上，作者后续是把imu数据做一次修正，先用imuConverter旋转到雷达系同一个方向的坐标系下，这样IMU跟Lidar的外参就是一个平移变换了。然后他把雷达数据又根据lidar2Imu反向平移了一下，和转换以后差了个平移的imu数据在“中间系”对齐，之后算完又从中间系通过imu2Lidar挪回了雷达系进行publish。