LIO-SAM源码解析（七）：utility.h

1. 代码功能

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

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

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

    <rosparam file="$(find lio_sam)/config/params.yaml" command="load" />

那么相关的参数就放在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;
 
    // 保存PCD
    bool savePCD;               // 是否保存地图
    string savePCDDirectory;    // 保存路径
 
    // 激光传感器参数
    SensorType sensor;      // 传感器型号
    int N_SCAN;             // 扫描线数，例如16、64
    int 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<double> extRotV;
    vector<double> extRPYV;
    vector<double> extTransV;
    Eigen::Matrix3d extRot;     // xyz坐标系旋转
    Eigen::Matrix3d extRPY;     // RPY欧拉角的变换关系
    Eigen::Vector3d extTrans;   // xyz坐标系平移
    Eigen::Quaterniond extQRPY;
 
    // LOAM
    float edgeThreshold;
    float surfThreshold;
    int edgeFeatureMinValidNum;
    int surfFeatureMinValidNum;
 
    // voxel filter paprams
    float odometrySurfLeafSize;
    float mappingCornerLeafSize;
    float mappingSurfLeafSize ;
 
    float z_tollerance; 
    float rotation_tollerance;
 
    // CPU Params
    int numberOfCores;
    double mappingProcessInterval;
 
    // Surrounding map
    float surroundingkeyframeAddingDistThreshold; 
    float surroundingkeyframeAddingAngleThreshold; 
    float surroundingKeyframeDensity;
    float surroundingKeyframeSearchRadius;
    
    // Loop closure
    bool  loopClosureEnableFlag;
    float loopClosureFrequency;
    int   surroundingKeyframeSize;
    float historyKeyframeSearchRadius;
    float historyKeyframeSearchTimeDiff;
    int   historyKeyframeSearchNum;
    float historyKeyframeFitnessScore;
 
    // global map visualization radius
    float 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<bool>("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
        nh.param<bool>("lio_sam/useGpsElevation", useGpsElevation, false);
        nh.param<float>("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
        nh.param<float>("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);
 
        nh.param<bool>("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<int>("lio_sam/N_SCAN", N_SCAN, 16);
        nh.param<int>("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);
        nh.param<int>("lio_sam/downsampleRate", downsampleRate, 1);
        nh.param<float>("lio_sam/lidarMinRange", lidarMinRange, 1.0);
        nh.param<float>("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0);
 
        nh.param<float>("lio_sam/imuAccNoise", imuAccNoise, 0.01);
        nh.param<float>("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
        nh.param<float>("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
        nh.param<float>("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
        nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511);
        nh.param<float>("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);
        nh.paramdouble>>("lio_sam/extrinsicRot", extRotV, vector<double>());
        nh.paramdouble>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
        nh.paramdouble>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
        extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
        extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
        extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
        extQRPY = Eigen::Quaterniond(extRPY);
 
        nh.param<float>("lio_sam/edgeThreshold", edgeThreshold, 0.1);
        nh.param<float>("lio_sam/surfThreshold", surfThreshold, 0.1);
        nh.param<int>("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
        nh.param<int>("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
 
        nh.param<float>("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
        nh.param<float>("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
        nh.param<float>("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);
 
        nh.param<float>("lio_sam/z_tollerance", z_tollerance, FLT_MAX);
        nh.param<float>("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);
 
        nh.param<int>("lio_sam/numberOfCores", numberOfCores, 2);
        nh.param<double>("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);
 
        nh.param<float>("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
        nh.param<float>("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
        nh.param<float>("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
        nh.param<float>("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
 
        nh.param<bool>("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);
        nh.param<float>("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0);
        nh.param<int>("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);
        nh.param<float>("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
        nh.param<float>("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
        nh.param<int>("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
        nh.param<float>("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
 
        nh.param<float>("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
        nh.param<float>("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
        nh.param<float>("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();
        // RPY
        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;
    }
};
 
 
/**
 * 发布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<typename T>
double ROS_TIME(T msg)
{
    return msg->header.stamp.toSec();
}
 
/**
 * 提取imu角速度
*/
template<typename T>
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<typename T>
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<typename T>
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

相关阅读:
大新闻｜高通将为Meta定制VR芯片，Quest端Beat Saber售出650万份
 内网渗透之内网信息收集（五）
2.3物理层设备
 Vue watch computed 生命周期执行顺序
 使用Docker部署Gitlab的记录
 GDPU 数据结构天码行空8
防静电离子风扇的应用及优点
 python计算点到面的距离
 Vue 生命周期钩子解读
 [附源码]Python计算机毕业设计Django三星小区车辆登记系统
原文地址：https://blog.csdn.net/xhtchina/article/details/128160537