1. 代码功能

头文件里主要是放了一些通用的参数定义，比方说：

nh.param("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");

这个意思就是我launch文件里面有这个"lio_sam/pointCloudTopic"（前面这个）的参数值的赋值，那么就赋值给pointCloudTopic（后面这个），后面的"/points_raw"就会忽略。那假如launch文件里面没有这个"lio_sam/pointCloudTopic"的定义，则就用"points_raw"。我们打开run.launch文件，可以看到是有加载参数的：

那么相关的参数就放在params.yaml文件中。

关于这个params.yaml，里面的东西乍一看很多，其实就几个比较重要：

第一：IMU Settings 来记录IMU的bias和噪声。当然IMU其实是加速度计和陀螺仪一共三个轴，这里却不分轴，用了一个平均数。如果没有转台之类的设备，就跑艾伦方差也可以标出各个轴的bias。作者的处理是：在imu预积分部分，三个轴都用了同样的bias方差（也就是写在配置文件里的这个），当然你要是有能力标的更准，那不妨这里把各个轴的bias都写进去，然后在imupreintegration.cpp文件里面改一改。

第二，IMU和雷达之间的外参。我不知道为什么作者TiXiaoshan很骚包的在这里写了一个Extrinsics (lidar -> IMU)，其实不能这样写，应该写成IMU->Lidar。因为它其实是T_{LB}，也就是说在IMU坐标系下的一个点，左乘T_{LB}，就可以变换到lidar坐标系下。 而且作者用的IMU也不是正常IMU，我推测他用的应该是左手系IMU。对于正常人使用的话，就普通的IMU就行，假如你就是那个机器人，x射向正前方，y射向左边，z射向头顶。雷达和IMU都是这样摆放，所以extrinsicRot和extrinsicRPY全部弄成单位矩阵就行。差的不太远的话，extrinsicTrans弄成[0，0，0]就行。

不过有精力的话还是标定了一下，就用浙大在2020开源的标定工具，lidar_IMU_calib ，（这个博客讲怎么配置的，我用的是这个），我个人拿尺子量出来大致比一下，我觉得他们估的还挺准。顺便提一嘴，ETH还有一个标定雷达和IMU的，那个准确来说标的是雷达和里程计的，原理也比较简单，反正雷达运动算一个轨迹，IMU也来一个轨迹，两边用外参联系起来，构成一个环，来求解外参。但是这种是有问题的，因为IMU估计的轨迹本身也就不准，这样估出来的外参也就不对，所以人家是标雷达和里程计的，不能单估雷达和IMU。浙大的这个lidar_IMU_calib的文章我大致看了看，大致是用样条插值，相机去对齐IMU，通过这种方式来初始化，之后构建surfelsMap，迭代优化来精修。（细节以我的性格正常我会展开说的，但是我不喜欢他们这篇论文所以不讲，主要是因为他们在论文里表示坐标系变换，R_{LI}非要写成^L_I R，像这种反人类的写法我们应该坚决抵制 ^ _ ^）

第三，z_tollerance，可以给z一个比较大的限制，如果用的是无人车，那就不可能在z轴变化过大，这里就可以限制它，防止它飘走。

第四，voxel filter paprams，这个是一些体素滤波的下采样参数，注意室内和室外是有区别的。

现在回到这个头文件里，可以注意两个内容：

第一，imuConverter函数，这个函数之后会被频繁调用。它主要的作用，是把IMU的信息，从IMU坐标系，转换到雷达坐标系。（注意，这个函数只旋转，没有平移，和真正的雷达坐标系之间还是差了一个平移的。至于为什么没有平移，先提前剧透一下，在imuPreintegration.cpp文件中，还有两个imu2Lidar，lidar2imu变量，这俩变量只有平移，没有旋转。

事实上，作者后续是把imu数据先用imuConverter旋转到雷达系下（但其实还差了个平移）。然后他把雷达数据又根据lidar2Imu反向平移了一下，和转换以后差了个平移的imu数据在“中间系”对齐，之后算完又从中间系通过imu2Lidar挪回了雷达系进行publish。

第二，publishCloud函数，这个函数传入句柄，然后发布形参里的内容，在cpp文件涉及到话题发布的地方，都会调用它。

其他的函数都是一些数据转换函数，没什么可说的。

2. 代码

#pragma once
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
#define _UTILITY_LIDAR_ODOMETRY_H_#include #include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include #include #include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include  
#include #include 
#include 
#include 
#include #include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include using namespace std;typedef pcl::PointXYZI PointType;// 传感器型号
enum class SensorType { VELODYNE, OUSTER };class ParamServer
{
public:ros::NodeHandle nh;std::string robot_id;// 话题string pointCloudTopic; // points_raw 原始点云数据string imuTopic;        // imu_raw 对应park数据集，imu_correct对应outdoor数据集，都是原始imu数据，不同的坐标系表示string odomTopic;       // odometry/imu，imu里程计，imu积分计算得到string gpsTopic;        // odometry/gps，gps里程计// 坐标系string lidarFrame;      // 激光坐标系string baselinkFrame;   // 载体坐标系string odometryFrame;   // 里程计坐标系string mapFrame;        // 世界坐标系// GPS参数bool useImuHeadingInitialization;   //bool useGpsElevation;float gpsCovThreshold;float poseCovThreshold;// 保存PCDbool savePCD;               // 是否保存地图string savePCDDirectory;    // 保存路径// 激光传感器参数SensorType sensor;      // 传感器型号int N_SCAN;             // 扫描线数，例如16、64int Horizon_SCAN;       // 扫描一周计数，例如每隔0.2°扫描一次，一周360°可以扫描1800次int downsampleRate;     // 扫描线降采样，跳过一些扫描线float lidarMinRange;    // 最小范围float lidarMaxRange;    // 最大范围// IMU参数float imuAccNoise;          // 加速度噪声标准差float imuGyrNoise;          // 角速度噪声标准差float imuAccBiasN;          //float imuGyrBiasN;float imuGravity;           // 重力加速度float imuRPYWeight;vector extRotV;vector extRPYV;vector extTransV;Eigen::Matrix3d extRot;     // xyz坐标系旋转Eigen::Matrix3d extRPY;     // RPY欧拉角的变换关系Eigen::Vector3d extTrans;   // xyz坐标系平移Eigen::Quaterniond extQRPY;// LOAMfloat edgeThreshold;float surfThreshold;int edgeFeatureMinValidNum;int surfFeatureMinValidNum;// voxel filter papramsfloat odometrySurfLeafSize;float mappingCornerLeafSize;float mappingSurfLeafSize ;float z_tollerance; float rotation_tollerance;// CPU Paramsint numberOfCores;double mappingProcessInterval;// Surrounding mapfloat surroundingkeyframeAddingDistThreshold; float surroundingkeyframeAddingAngleThreshold; float surroundingKeyframeDensity;float surroundingKeyframeSearchRadius;// Loop closurebool  loopClosureEnableFlag;float loopClosureFrequency;int   surroundingKeyframeSize;float historyKeyframeSearchRadius;float historyKeyframeSearchTimeDiff;int   historyKeyframeSearchNum;float historyKeyframeFitnessScore;// global map visualization radiusfloat globalMapVisualizationSearchRadius;float globalMapVisualizationPoseDensity;float globalMapVisualizationLeafSize;ParamServer(){nh.param("/robot_id", robot_id, "roboat");// 从param server中读取key为"lio_sam/pointCloudTopic"对应的参数，存pointCloudTopic，第三个参数是默认值// launch文件中定义，从yaml文件加载参数nh.param("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");nh.param("lio_sam/imuTopic", imuTopic, "imu_correct");nh.param("lio_sam/odomTopic", odomTopic, "odometry/imu");nh.param("lio_sam/gpsTopic", gpsTopic, "odometry/gps");nh.param("lio_sam/lidarFrame", lidarFrame, "base_link");nh.param("lio_sam/baselinkFrame", baselinkFrame, "base_link");nh.param("lio_sam/odometryFrame", odometryFrame, "odom");nh.param("lio_sam/mapFrame", mapFrame, "map");nh.param("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);nh.param("lio_sam/useGpsElevation", useGpsElevation, false);nh.param("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);nh.param("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);nh.param("lio_sam/savePCD", savePCD, false);nh.param("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");std::string sensorStr;nh.param("lio_sam/sensor", sensorStr, "");if (sensorStr == "velodyne"){sensor = SensorType::VELODYNE;}else if (sensorStr == "ouster"){sensor = SensorType::OUSTER;}else{ROS_ERROR_STREAM("Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr);ros::shutdown();}nh.param("lio_sam/N_SCAN", N_SCAN, 16);nh.param("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);nh.param("lio_sam/downsampleRate", downsampleRate, 1);nh.param("lio_sam/lidarMinRange", lidarMinRange, 1.0);nh.param("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0);nh.param("lio_sam/imuAccNoise", imuAccNoise, 0.01);nh.param("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);nh.param("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);nh.param("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);nh.param("lio_sam/imuGravity", imuGravity, 9.80511);nh.param("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);nh.param>("lio_sam/extrinsicRot", extRotV, vector());nh.param>("lio_sam/extrinsicRPY", extRPYV, vector());nh.param>("lio_sam/extrinsicTrans", extTransV, vector());extRot = Eigen::Map>(extRotV.data(), 3, 3);extRPY = Eigen::Map>(extRPYV.data(), 3, 3);extTrans = Eigen::Map>(extTransV.data(), 3, 1);extQRPY = Eigen::Quaterniond(extRPY);nh.param("lio_sam/edgeThreshold", edgeThreshold, 0.1);nh.param("lio_sam/surfThreshold", surfThreshold, 0.1);nh.param("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);nh.param("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);nh.param("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);nh.param("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);nh.param("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);nh.param("lio_sam/z_tollerance", z_tollerance, FLT_MAX);nh.param("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);nh.param("lio_sam/numberOfCores", numberOfCores, 2);nh.param("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);nh.param("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);nh.param("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);nh.param("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);nh.param("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);nh.param("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);nh.param("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0);nh.param("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);nh.param("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);nh.param("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);nh.param("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);nh.param("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);nh.param("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);nh.param("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);nh.param("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);usleep(100);}/*** imu原始测量数据转换到lidar系，加速度、角速度、RPY*/sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in){sensor_msgs::Imu imu_out = imu_in;// 加速度，只跟xyz坐标系的旋转有关系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();// 角速度，只跟xyz坐标系的旋转有关系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();// RPYEigen::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;}
};/*** 发布thisCloud，返回thisCloud对应msg格式
*/
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
{sensor_msgs::PointCloud2 tempCloud;pcl::toROSMsg(*thisCloud, tempCloud);tempCloud.header.stamp = thisStamp;tempCloud.header.frame_id = thisFrame;if (thisPub->getNumSubscribers() != 0)thisPub->publish(tempCloud);return tempCloud;
}/*** msg时间戳
*/
template
double ROS_TIME(T msg)
{return msg->header.stamp.toSec();
}/*** 提取imu角速度
*/
template
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{*angular_x = thisImuMsg->angular_velocity.x;*angular_y = thisImuMsg->angular_velocity.y;*angular_z = thisImuMsg->angular_velocity.z;
}/*** 提取imu加速度
*/
template
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
{*acc_x = thisImuMsg->linear_acceleration.x;*acc_y = thisImuMsg->linear_acceleration.y;*acc_z = thisImuMsg->linear_acceleration.z;
}/*** 提取imu姿态角RPY
*/
template
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{double imuRoll, imuPitch, imuYaw;tf::Quaternion orientation;tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);*rosRoll = imuRoll;*rosPitch = imuPitch;*rosYaw = imuYaw;
}/*** 点到坐标系原点距离
*/
float pointDistance(PointType p)
{return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}/*** 两点之间距离
*/
float pointDistance(PointType p1, PointType p2)
{return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
}#endif

参考文献

SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）_zkk9527的博客-CSDN博客_lio-sam