LIO-SAM 详读代码笔记 -- 1

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 成员变量

变量名	类型	注释	备注
nh	ros::NodeHandle	ros 节点句柄，订阅，发布，读取参数等工作	-
robot_id	std::string	机器人id	-
//*消息名称统一定义*
pointCloudTopic	std::string	初始lidar点云数据的ros topic 名称，从ros参数服务器中读取，初始化后不再改变	-
imuTopic	std::string	初始imu数据的ros topic 名称，从ros参数服务器中读取，初始化后不再改变	-
odomTopic	std::string	从ros参数服务器中读取，初始化值"odometry/imu"，该消息从 imuPreintegration.cpp 中发出	-
gpsTopic	std::string	初始gps数据的ros topic 名称，从ros参数服务器中读取，初始化后不再改变	-
//*坐标系*
lidarFrame	std::string	激光雷达lidar坐标系名称，初始值为"base_link",在该系统中定义成与baselink 相同，在配置文件可以修改，但是需要提供lidar到baselink 的外参	-
baselinkFrame	std::string	机器人坐标系名称，初始值为"base_link"	-
odometryFrame	std::string	里程计坐标系名称，初始值为"odom"	-
mapFrame	std::string	地图坐标系名称，初始值为"map"	-
// GPS Settings
useImuHeadingInitialization	bool	是否采用imu 数据进行初始化
useGpsElevation	bool	是否使用gps的高度数据
gpsCovThreshold	float	添加gps 因子的协方差阈值
poseCovThreshold	float	添加gps 因子的协方差阈值
// Save pcd 点云地图保存设置
savePCD	bool	是否保存地图	-
savePCDDirectory	string	地图保存路径	-
// Lidar Sensor Configuration 激光雷达传感器设置
sensor	SensorType	激光雷达的品牌型号，枚举类型{ VELODYNE, OUSTER, LIVOX }	-
N_SCAN	int	激光雷达 线数 如：16线，32线，64线，128线等	-
Horizon_SCAN	int	激光雷达 单线 点数 如：1800	-
downsampleRate	int	降采样比列 default =1 这个字段是用来滤波的压缩数据，downsampleRate =2 时 128线 ->64线 在imageProjection模块中使用	-
lidarMinRange	float	激光雷达识别最小距离，小于该距离为盲区 默认值为1.0	-
lidarMaxRange	float	激光雷达识别最大距离，大于该距离为盲区 默认值为1000.0	-
// IMU 参数设置
imuAccNoise	float	imu加速度计的协方差对角值	-
imuGyrNoise	float	imu陀螺仪的协方差对角值	-
imuAccBiasN	float	imu加速度计bias的协方差对角值	-
imuGyrBiasN	float	imu陀螺仪bias的协方差对角值	-
imuGravity	float	重力	-
imuRPYWeight	float	imu 旋转角加权值，用于里程计修正	-
extRotV	vector	imu 与激光雷达位姿的外参旋转角	-
extRPYV	vector	imu 与激光雷达位姿的外参旋转角	-
extTransV	vector	imu 与激光雷达位姿的外参	-
extRot	Eigen::Matrix3d	imu 与激光雷达位姿的外参旋转矩阵	-
extRPY	Eigen::Matrix3d	imu 与激光雷达位姿的外参旋转矩阵	-
extTrans	Eigen::Vector3d	imu 与激光雷达位姿的外参位移向量	-
extQRPY	Eigen::Quaterniond	imu 与激光雷达位姿的外参旋转四元素	-
// LOAM 参数设置
edgeThreshold	float	边角点曲率阈值	-
surfThreshold	float	平面点点曲率阈值	-
edgeFeatureMinValidNum	int	最小处理边角点的阈值 // 进行scan-map 需要点云角点最小特征数，在MapOptimization中用	-
surfFeatureMinValidNum	int	最小处理平面点的阈值， 进行scan-map 需要点云平面点最小特征数	-
// voxel filter paprams 降采样过滤器参数		注意室内和室外是有区别的。
odometrySurfLeafSize	float	里程计点的降采样参数	-
mappingCornerLeafSize	float	角点地图的降采样参数	-
mappingSurfLeafSize	float	平面点地图的降采样参数	-
z_tollerance	float	可以给z一个比较大的限制，如果用的是无人车，那就不可能在z轴变化过大，这里就可以限制它，防止它飘走	-
rotation_tollerance	float	待定	-
// CPU Params
numberOfCores	int	cpu 核数	-
mappingProcessInterval	double	mapOptimization 的执行时间间隔	-
// Surrounding map 局部地图参数设置
surroundingkeyframeAddingDistThreshold	float	距离当前帧多远的距离 才添加近局部地图的阈值（添加关键帧的条件）	-
surroundingkeyframeAddingAngleThreshold	float	距离当前帧多远的角度 才添加近局部地图的阈值（添加关键帧的条件）	-
surroundingKeyframeDensity	float	待定	-
surroundingKeyframeSearchRadius	float	添加关键帧的搜索半径， 局部地图的距离最新帧中心阈值	-
//Loop closure
loopClosureEnableFlag	bool	是否启动回环检测	-
loopClosureFrequency	float	添加回环检测的因子的频率	-
surroundingKeyframeSize	int	局部地图的关键帧数	-
historyKeyframeSearchRadius	float	在mapoptimization 中用，取局部地图时的参数	-
historyKeyframeSearchTimeDiff	float	在mapoptimization 中用，取局部地图时的参数	-
historyKeyframeSearchNum	int	在mapoptimization 中用，取局部地图时的参数	-
historyKeyframeFitnessScore	float	在mapoptimization 中用，取局部地图时的参数	-
//global map visualization radius
globalMapVisualizationSearchRadius	float	在mapOptimization 发布“全局地图”时设定的可视范围	-
globalMapVisualizationPoseDensity	float	在mapOptimization 发布“全局地图”时设定的关键帧密度	-
globalMapVisualizationLeafSize	float	在mapOptimization 发布“全局地图”时设定的下采样参数	-

关于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。