LIO-SAM 激光SLAM 代码学习（二）

写在前面：除了代码学习，本周还下载编译了Lio-sam的源代码，并运行了示例数据集。示例数据集以及最终运行保存的结果如下
示例数据及结果https://pan.baidu.com/s/1iK2jc5-u2zfnN1e7GRrlTg?pwd=6666 

一、 概述

        针对《LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping》的所有代码进行学习。依托Deepseek/ChatGPT等AI工具学习代码，并对代码进行详细注释，以此学习SLAM。

        原文：https://github.com/TixiaoShan/LIO-SAM/blob/master/config/doc/paper.pdf

        代码：https://github.com/TixiaoShan/LIO-SAM

        LIO-SAM的系统架构

二、项目中各个文件

<1> .github：通常存放 GitHub 相关配置文件。

<2> config：一般存储项目的配置文件，比如参数配置、算法配置等。

<3> include：用于放置头文件（.h 等），供项目代码引用。

<4> launch：存放 ROS 的启动文件（.launch），用于一键启动多个节点、配置参数等。

<5> msg：定义 ROS 自定义消息类型的文件夹。

<6> src：存储项目的源文件（.cpp 等），是代码实现的主要地方。

<7> srv：定义 ROS 服务（Service）类型的文件夹。

<8> MakeLists.txt：CMake 的构建配置文件，用于定义项目编译规则。

<9> Dockerfile：Docker 镜像构建文件。

<10> LICENSE：许可证文件，规定项目使用、分发等权限。

<11> README.md：项目说明文档，介绍项目功能、使用方法等。

<12>package.xml：ROS 包的描述文件，声明依赖、包信息等。

三、深入代码逻辑

3.1 cloud_info.msg 文件

        cloud_info.msg是 LIO-SAM 系统中定义的核心自定义消息，用于封装激光雷达点云处理过程中的关键信息，实现各模块间的数据交互。该消息整合了点云特征、IMU 状态、位姿估计等多源数据，是连接激光雷达预处理、特征提取与后端优化的桥梁。

# 1. 消息头部（标准 ROS 消息头）
Header header 
# 说明：包含时间戳、坐标系等基础信息，用于消息同步和坐标关联

# 2. 点云环索引信息（用于点云投影处理）
int32[] startRingIndex
int32[] endRingIndex
# 说明：
# - 激光雷达环形扫描的起始和结束索引
# - 对于多线激光雷达（如16线、64线），用于标识每一圈扫描的点云范围
# - 配合 Horizon_SCAN 参数，可还原三维点云的空间分布

# 3. 点云特征索引与距离信息
int32[] pointColInd  # 点云在距离图像中的列索引
float32[] pointRange # 点云距离值（米）
# 说明：
# - pointColInd 记录点云在二维投影图像中的位置
# - pointRange 存储对应点的距离测量值
# - 两者结合可快速定位和检索三维点云特征

# 4. 传感器数据可用性标记
int64 imuAvailable
int64 odomAvailable
# 说明：
# - 布尔型标记（0/1），指示 IMU 数据和里程计数据是否有效
# - 用于后端优化时的数据有效性判断，避免无效数据引入误差

# 5. LOAM 初始化姿态（来自 IMU）
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit
# 说明：
# - IMU 提供的初始横滚角、俯仰角、偏航角
# - 用于 LOAM 算法的初始姿态估计，特别是在系统启动或重定位时

# 6. IMU 预积分初始猜测值
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw
# 说明：
# - 基于 IMU 预积分计算的位姿初始猜测值（x, y, z 位置和 r, p, y 姿态）
# - 为激光雷达扫描匹配提供初始值，加速收敛并减少局部最小值风险

# 7. 点云消息（核心数据载体）
sensor_msgs/PointCloud2 cloud_deskewed  # 去畸变后的原始点云
sensor_msgs/PointCloud2 cloud_corner    # 提取的角点特征
sensor_msgs/PointCloud2 cloud_surface   # 提取的平面点特征
# 说明：
# - cloud_deskewed：经过运动畸变补偿的原始点云（未提取特征）
# - cloud_corner：从点云中提取的角点特征（边缘特征）
# - cloud_surface：从点云中提取的平面点特征（表面特征）
# - 这三类点云构成 LOAM 前端里程计的输入数据

# 8. 第三方消息（用于可视化和建图）
sensor_msgs/PointCloud2 key_frame_cloud    # 关键帧点云
sensor_msgs/PointCloud2 key_frame_color    # 带颜色的关键帧点云
sensor_msgs/PointCloud2 key_frame_poses    # 关键帧位姿可视化
sensor_msgs/PointCloud2 key_frame_map      # 关键帧构建的局部地图
# 说明：
# - 用于 Rviz 可视化和地图构建
# - key_frame_cloud：关键帧的点云数据
# - key_frame_color：带颜色编码的点云（如按距离或高度染色）
# - key_frame_poses：关键帧的位姿轨迹
# - key_frame_map：基于关键帧构建的局部地图片段

3.1.1 消息在 LIO-SAM 中的数据流

<1> 数据生成：

        1.imageProjection节点生成cloud_deskewed和点云索引（startRingIndex,endRingIndex）

        2.featureExtraction节点从cloud_deskewed中提取cloud_corner和cloud_surface

        3.imuPreintegration节点计算初始姿态猜测值（initialGuess*）

<2> 数据传递：

        1.该消息通过 ROS 话题发布，被mapOptimization节点订阅

        2.mapOptimization利用点云特征和 IMU 姿态进行扫描匹配和因子图优

<3> 数据使用：

        1.后端优化模块根据cloud_corner和cloud_surface构建激光雷达约束

        2.initialGuess*为优化提供初始值，提高收敛速度

        3.可视化模块读取key_frame_*消息更新 Rviz 显示

3.1.2 设计逻辑

<1> 多维度数据整合：

        1.将原始数据（cloud_deskewed）、特征数据（cloud_corner, cloud_surface）和姿态数据（initialGuess*）封装在同一消息中，减少数据传输开销。

        2.通过 imuAvailable 和 odomAvailable 实现数据有效性管理，增强系统鲁棒性。

<2> 点云投影优化：

        1.startRingIndex 和 endRingIndex 记录环形扫描边界，配合 pointColInd 和 pointRange，可快速定位三维点云在二维投影中的位置，加速特征提取和扫描匹配。

<3> 初始化与优化支持：

        1.imuRollInit 等字段提供 IMU 初始姿态，解决激光雷达在静止状态下的初始化问题。

        2.initialGuess* 基于 IMU 预积分，为激光雷达扫描匹配提供合理初始值，避免陷入局部最优

3.1.3 与其他模块的交互

3.2 include 中 utility.h文件

3.2.1 文件整体结构与功能

该头文件是 LIO-SAM 项目的核心工具文件，主要包含：

        1.参数服务器类 ParamServer：从 ROS 参数服务器加载系统配置参数，并提供传感器数据转换功能。

        2.模板工具函数：点云数据发布、IMU 数据解析等通用功能。

        3.辅助计算函数：点云距离计算等数学工具。

3.2.2 关键头文件包含与定义

#pragma once
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
#define _UTILITY_LIDAR_ODOMETRY_H_
#define PCL_NO_PRECOMPILE     //PCL_NO_PRECOMPILE 禁用 PCL 预编译头文件，避免编译冲突

#include <ros/ros.h>

// ROS 标准消息类型
#include <std_msgs/Header.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>

// PCL 点云库
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

// TF 坐标变换
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

// C++ 标准库
#include <vector>
#include <cmath>
#include <mutex>

using namespace std;
typedef pcl::PointXYZI PointType;  // 定义点云类型（X,Y,Z,强度）

// 传感器类型枚举，枚举传感器类型，支持 Velodyne、Ouster、Livox 三种激光雷达
enum class SensorType { VELODYNE, OUSTER, LIVOX };

3.2.3 参数服务器类 ParamServer 

// 参数服务器类：从ROS参数服务器加载配置参数
class ParamServer
{
public:
    ros::NodeHandle nh;  // ROS节点句柄

    // 系统标识符
    std::string robot_id;

    // 话题名称配置
    string pointCloudTopic;  // 点云话题
    string imuTopic;         // IMU话题
    string odomTopic;        // 里程计话题
    string gpsTopic;         // GPS话题

    // 坐标系名称配置
    string lidarFrame;     // 激光雷达坐标系
    string baselinkFrame;  // 机器人基坐标系
    string odometryFrame;  // 里程计坐标系
    string mapFrame;       // 地图坐标系

    // GPS设置
    bool useImuHeadingInitialization;  // 是否使用IMU初始化航向角
    bool useGpsElevation;              // 是否使用GPS高程
    float gpsCovThreshold;             // GPS协方差阈值
    float poseCovThreshold;             // 位姿协方差阈值

    // 点云保存设置
    bool savePCD;              // 是否保存PCD文件
    string savePCDDirectory;   // PCD保存路径

    // 激光雷达传感器配置
    SensorType sensor;         // 传感器类型
    int N_SCAN;                // 垂直扫描线数
    int Horizon_SCAN;          // 水平扫描点数
    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;   // 外参旋转矩阵(Eigen格式)
    Eigen::Matrix3d extRPY;   // 外参欧拉角矩阵
    Eigen::Vector3d extTrans; // 外参平移向量
    Eigen::Quaterniond extQRPY; // 外参四元数

    // 特征提取参数
    float edgeThreshold;          // 边缘特征阈值
    float surfThreshold;          // 平面特征阈值
    int edgeFeatureMinValidNum;   // 最小有效边缘特征数
    int surfFeatureMinValidNum;   // 最小有效平面特征数

    // 体素滤波参数
    float odometrySurfLeafSize;   // 里程计平面特征体素大小
    float mappingCornerLeafSize;  // 建图边缘特征体素大小
    float mappingSurfLeafSize;    // 建图平面特征体素大小

    // 位姿更新容忍度
    float z_tollerance;         // Z轴变化阈值
    float rotation_tollerance;  // 旋转变化阈值

    // 处理参数
    int numberOfCores;             // 使用CPU核心数
    double mappingProcessInterval; // 建图处理间隔(秒)

    // 局部地图参数
    float surroundingkeyframeAddingDistThreshold;   // 关键帧添加距离阈值
    float surroundingkeyframeAddingAngleThreshold;  // 关键帧添加角度阈值
    float surroundingKeyframeDensity;               // 关键帧密度
    float surroundingKeyframeSearchRadius;           // 关键帧搜索半径
    
    // 回环检测参数
    bool  loopClosureEnableFlag;         // 是否启用回环检测
    float loopClosureFrequency;          // 回环检测频率
    int   surroundingKeyframeSize;       // 局部关键帧数量
    float historyKeyframeSearchRadius;   // 历史关键帧搜索半径
    float historyKeyframeSearchTimeDiff; // 历史关键帧时间差阈值
    int   historyKeyframeSearchNum;      // 历史关键帧搜索数量
    float historyKeyframeFitnessScore;   // 回环匹配得分阈值

    // 全局地图可视化参数
    float globalMapVisualizationSearchRadius;  // 可视化搜索半径
    float globalMapVisualizationPoseDensity;   // 位姿密度
    float globalMapVisualizationLeafSize;       // 可视化点云体素大小

    // 构造函数：从ROS参数服务器加载所有参数
    ParamServer()
    {
        // 加载机器人ID
        nh.param<std::string>("/robot_id", robot_id, "roboat");

        // 加载话题名称参数
        nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
        nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
        nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
        nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");

        // 加载坐标系参数
        nh.param<std::string>("lio_sam/lidarFrame", lidarFrame, "base_link");
        nh.param<std::string>("lio_sam/baselinkFrame", baselinkFrame, "base_link");
        nh.param<std::string>("lio_sam/odometryFrame", odometryFrame, "odom");
        nh.param<std::string>("lio_sam/mapFrame", mapFrame, "map");

        // 加载GPS/IMU参数
        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);

        // 加载PCD保存参数
        nh.param<bool>("lio_sam/savePCD", savePCD, false);
        nh.param<std::string>("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");

        // 加载传感器类型参数
        std::string sensorStr;
        nh.param<std::string>("lio_sam/sensor", sensorStr, "");
        if (sensorStr == "velodyne") {
            sensor = SensorType::VELODYNE;
        } else if (sensorStr == "ouster") {
            sensor = SensorType::OUSTER;
        } else if (sensorStr == "livox") {
            sensor = SensorType::LIVOX;
        } else {
            ROS_ERROR_STREAM("Invalid sensor type: " << 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);

        // 加载IMU参数
        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.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>());
        nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
        nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
        
        // 将向量转换为Eigen矩阵
        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).inverse();

        // 加载特征提取参数
        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坐标系转换函数（雷达系->基座系）
    sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
    {
        sensor_msgs::Imu imu_out = imu_in;
        
        // 转换加速度数据
        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();
        
        // 转换角速度数据
        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();
        
        // 转换姿态数据
        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! Use 9-axis IMU.");
            ros::shutdown();
        }

        return imu_out;
    }
};

核心功能说明：

        1.参数组织：将系统参数按功能模块分类定义，便于管理和调优。

        2.动态加载：构造函数通过 nh.param 从 ROS 参数服务器动态加载配置，支持运行时修改。

        3.坐标变换：imuConverter 方法使用外参矩阵将 IMU 数据从传感器坐标系转换到机器人基座坐标系。

        4.矩阵运算：利用 Eigen 库进行高效矩阵和四元数运算，确保坐标变换的准确性。

3.3.4 模板工具函数与辅助计算函数

// 发布点云到ROS话题的模板函数
template<typename T>
sensor_msgs::PointCloud2 publishCloud(const ros::Publisher& thisPub, 
                                      const T& thisCloud, 
                                      ros::Time thisStamp, 
                                      std::string thisFrame)
{
    sensor_msgs::PointCloud2 tempCloud;
    pcl::toROSMsg(*thisCloud, tempCloud);  // PCL点云转ROS消息
    tempCloud.header.stamp = thisStamp;
    tempCloud.header.frame_id = thisFrame;
    if (thisPub.getNumSubscribers() != 0) {
        thisPub.publish(tempCloud);
    }
    return tempCloud;
}

// 获取ROS消息时间戳（转换为秒）
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);  // ROS四元数转TF四元数
    tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); // 计算RPY角

    *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  // 结束头文件保护

3.3 src 主函数

3.3.1 featureExtraction.cpp

功能简介

        对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。

订阅

        订阅当前激光帧运动畸变校正后的点云信息，来自ImageProjection。

发布

        1.发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等，发布给MapOptimization；

        2.发布当前激光帧提取的角点点云，用于rviz展示；

        3.发布当前激光帧提取的平面点点云，用于rviz展示。

(1). 激光点曲率定义

/**
 * 激光点曲率
*/
struct smoothness_t{ 
    float value; // 曲率值
    size_t ind;  // 激光点一维索引
};

/**
 * 曲率比较函数，从小到大排序
*/
struct by_value{ 
    bool operator()(smoothness_t const &left, smoothness_t const &right) { 
        return left.value < right.value;
    }
};

(2). 成员变量

ros::Subscriber subLaserCloudInfo;

// 发布当前激光帧提取特征之后的点云信息
ros::Publisher pubLaserCloudInfo;
// 发布当前激光帧提取的角点点云
ros::Publisher pubCornerPoints;
// 发布当前激光帧提取的平面点点云
ros::Publisher pubSurfacePoints;

// 当前激光帧运动畸变校正后的有效点云
pcl::PointCloud<PointType>::Ptr extractedCloud;
// 当前激光帧角点点云集合
pcl::PointCloud<PointType>::Ptr cornerCloud;
// 当前激光帧平面点点云集合
pcl::PointCloud<PointType>::Ptr surfaceCloud;

pcl::VoxelGrid<PointType> downSizeFilter;

// 当前激光帧点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等
lio_sam::cloud_info cloudInfo;
std_msgs::Header cloudHeader;

// 当前激光帧点云的曲率
std::vector<smoothness_t> cloudSmoothness;
float *cloudCurvature;
// 特征提取标记，1表示遮挡、平行，或者已经进行特征提取的点，0表示还未进行特征提取处理
int *cloudNeighborPicked;
// 1表示角点，-1表示平面点
int *cloudLabel;

(3). 构造函数及初始化

/**
 * 构造函数
*/
FeatureExtraction()
{
    // 订阅当前激光帧运动畸变校正后的点云信息
    subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());

    // 发布当前激光帧提取特征之后的点云信息
    pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);
    // 发布当前激光帧的角点点云
    pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);
    // 发布当前激光帧的面点点云
    pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);
    
    // 初始化
    initializationValue();
}

// 初始化
void initializationValue()
{
    cloudSmoothness.resize(N_SCAN*Horizon_SCAN);

    downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);

    extractedCloud.reset(new pcl::PointCloud<PointType>());
    cornerCloud.reset(new pcl::PointCloud<PointType>());
    surfaceCloud.reset(new pcl::PointCloud<PointType>());

    cloudCurvature = new float[N_SCAN*Horizon_SCAN];
    cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
    cloudLabel = new int[N_SCAN*Horizon_SCAN];
}

(4). 订阅运动畸变校正之后的激光点云数据

/**
 * 订阅当前激光帧运动畸变校正后的点云信息
 * 1、计算当前激光帧点云中每个点的曲率
 * 2、标记属于遮挡、平行两种情况的点，不做特征提取
 * 3、点云角点、平面点特征提取
 *   1) 遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
 *   2) 认为非角点的点都是平面点，加入平面点云集合，最后降采样
 * 4、发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
*/
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
    cloudInfo = *msgIn; 
    cloudHeader = msgIn->header; 
    // 当前激光帧运动畸变校正后的有效点云
    pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); 

    // 计算当前激光帧点云中每个点的曲率
    calculateSmoothness();

    // 标记属于遮挡、平行两种情况的点，不做特征提取
    markOccludedPoints();

    // 点云角点、平面点特征提取
    // 1、遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
    // 2、认为非角点的点都是平面点，加入平面点云集合，最后降采样
    extractFeatures();
    
    // 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
    publishFeatureCloud();
}

(5). 计算激光点曲率

/**
 * 计算当前激光帧点云中每个点的曲率
*/
void calculateSmoothness()
{
    // 遍历当前激光帧运动畸变校正后的有效点云
    int cloudSize = extractedCloud->points.size();
    for (int i = 5; i < cloudSize - 5; i++)
    {
        // 用当前激光点前后5个点计算当前点的曲率，平坦位置处曲率较小，角点处曲率较大；这个方法很简单但有效
        float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
                        + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
                        + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
                        + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
                        + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
                        + cloudInfo.pointRange[i+5];            

        // 距离差值平方作为曲率
        cloudCurvature[i] = diffRange*diffRange;

        cloudNeighborPicked[i] = 0;
        cloudLabel[i] = 0;
        
        // 存储该点曲率值、激光点一维索引
        cloudSmoothness[i].value = cloudCurvature[i];
        cloudSmoothness[i].ind = i;
    }
}

(6). 标记遮挡、平行点，不参与特征提取

/**
 * 标记属于遮挡、平行两种情况的点，不做特征提取
*/
void markOccludedPoints()
{
    int cloudSize = extractedCloud->points.size();
    // mark occluded points and parallel beam points

    for (int i = 5; i < cloudSize - 6; ++i)
    {
        // 当前点和下一个点的range值
        float depth1 = cloudInfo.pointRange[i];
        float depth2 = cloudInfo.pointRange[i+1];
        // 两个激光点之间的一维索引差值，如果在一条扫描线上，那么值为1；如果两个点之间有一些无效点被剔除了，可能会比1大，但不会特别大
        // 如果恰好前一个点在扫描一周的结束时刻，下一个点是另一条扫描线的起始时刻，那么值会很大
        int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));

        // 两个点在同一扫描线上，且距离相差大于0.3，认为存在遮挡关系（也就是这两个点不在同一平面上，如果在同一平面上，距离相差不会太大）
        // 远处的点会被遮挡，标记一下该点以及相邻的5个点，后面不再进行特征提取
        if (columnDiff < 10){
            
            if (depth1 - depth2 > 0.3){
                cloudNeighborPicked[i - 5] = 1;
                cloudNeighborPicked[i - 4] = 1;
                cloudNeighborPicked[i - 3] = 1;
                cloudNeighborPicked[i - 2] = 1;
                cloudNeighborPicked[i - 1] = 1;
                cloudNeighborPicked[i] = 1;
            }else if (depth2 - depth1 > 0.3){
                cloudNeighborPicked[i + 1] = 1;
                cloudNeighborPicked[i + 2] = 1;
                cloudNeighborPicked[i + 3] = 1;
                cloudNeighborPicked[i + 4] = 1;
                cloudNeighborPicked[i + 5] = 1;
                cloudNeighborPicked[i + 6] = 1;
            }
        }
        
        // 用前后相邻点判断当前点所在平面是否与激光束方向平行
        float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
        float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));

        // 平行则标记一下
        if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
            cloudNeighborPicked[i] = 1;
    }
}

(7). 提取特征点（角点、平面点）

/**
 * 点云角点、平面点特征提取
 * 1、遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
 * 2、认为非角点的点都是平面点，加入平面点云集合，最后降采样
*/
void extractFeatures()
{
    cornerCloud->clear();
    surfaceCloud->clear();

    pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());

    // 遍历扫描线
    for (int i = 0; i < N_SCAN; i++)
    {
        surfaceCloudScan->clear();

        // 将一条扫描线扫描一周的点云数据，划分为6段，每段分开提取有限数量的特征，保证特征均匀分布
        for (int j = 0; j < 6; j++)
        {
            // 每段点云的起始、结束索引；startRingIndex为扫描线起始第5个激光点在一维数组中的索引
            int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
            int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;

            if (sp >= ep)
                continue;

            // 按照曲率从小到大排序点云
            std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

            // 按照曲率从大到小遍历
            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)
            {
                // 激光点的索引
                int ind = cloudSmoothness[k].ind;
                // 当前激光点还未被处理，且曲率大于阈值，则认为是角点
                if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                {
                    // 每段只取20个角点，如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点
                    largestPickedNum++;
                    if (largestPickedNum <= 20){
                        // 标记为角点
                        cloudLabel[ind] = 1;
                        // 加入角点点云
                        cornerCloud->push_back(extractedCloud->points[ind]);
                    } else {
                        break;
                    }

                    // 标记已被处理
                    cloudNeighborPicked[ind] = 1;
                    // 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
                    for (int l = 1; l <= 5; l++)
                    {
                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                        if (columnDiff > 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    // 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
                    for (int l = -1; l >= -5; l--)
                    {
                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
                        if (columnDiff > 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // 按照曲率从小到大遍历
            for (int k = sp; k <= ep; k++)
            {
                // 激光点的索引
                int ind = cloudSmoothness[k].ind;
                // 当前激光点还未被处理，且曲率小于阈值，则认为是平面点
                if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
                {
                    // 标记为平面点
                    cloudLabel[ind] = -1;
                    // 标记已被处理
                    cloudNeighborPicked[ind] = 1;

                    // 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
                    for (int l = 1; l <= 5; l++) {

                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                        if (columnDiff > 10)
                            break;

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    // 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
                    for (int l = -1; l >= -5; l--) {

                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
                        if (columnDiff > 10)
                            break;

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // 平面点和未被处理的点，都认为是平面点，加入平面点云集合
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0){
                    surfaceCloudScan->push_back(extractedCloud->points[k]);
                }
            }
        }

        // 平面点云降采样
        surfaceCloudScanDS->clear();
        downSizeFilter.setInputCloud(surfaceCloudScan);
        downSizeFilter.filter(*surfaceCloudScanDS);

        // 加入平面点云集合
        *surfaceCloud += *surfaceCloudScanDS;
    }
}

(8). 清理、发布点云

/**
 * 清理
*/
void freeCloudInfoMemory()
{
    cloudInfo.startRingIndex.clear();
    cloudInfo.endRingIndex.clear();
    cloudInfo.pointColInd.clear();
    cloudInfo.pointRange.clear();
}

/**
 * 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
*/
void publishFeatureCloud()
{
    // 清理
    freeCloudInfoMemory();
    // 发布角点、面点点云，用于rviz展示
    cloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, lidarFrame);
    cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
    // 发布当前激光帧点云信息，加入了角点、面点点云数据，发布给mapOptimization
    pubLaserCloudInfo.publish(cloudInfo);
}

3.3.2 ImageProjection.cpp

功能简介

        1.利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；

        2.同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

订阅

        1.订阅原始IMU数据；

        2.订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿；

        3.订阅原始激光点云数据。

发布

        1.发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；

        2.发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。

(1). 点云数据结构定义

/**
 * Velodyne点云结构，变量名XYZIRT是每个变量的首字母
*/
struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D     // 位置
    PCL_ADD_INTENSITY;  // 激光点反射强度，也可以存点的索引
    uint16_t ring;      // 扫描线
    float time;         // 时间戳，记录相对于当前帧第一个激光点的时差，第一个点time=0
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;        // 内存16字节对齐，EIGEN SSE优化要求
// 注册为PCL点云格式
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)
)

// 本程序使用Velodyne点云结构
using PointXYZIRT = VelodynePointXYZIRT;

// imu数据队列长度
const int queueLength = 2000;

        作用：定义激光雷达点云的基本数据结构，包含空间坐标、强度、激光束编号和时间戳，是点云处理的基础单元。

(2). 成员变量初始化

// imu队列、odom队列互斥锁
std::mutex imuLock;
std::mutex odoLock;

// 订阅原始激光点云
ros::Subscriber subLaserCloud;
ros::Publisher  pubLaserCloud;

// 发布当前帧校正后点云，有效点
ros::Publisher pubExtractedCloud;
ros::Publisher pubLaserCloudInfo;

// imu数据队列（原始数据，转lidar系下）
ros::Subscriber subImu;
std::deque<sensor_msgs::Imu> imuQueue;

// imu里程计队列
ros::Subscriber subOdom;
std::deque<nav_msgs::Odometry> odomQueue;

// 激光点云数据队列
std::deque<sensor_msgs::PointCloud2> cloudQueue;
// 队列front帧，作为当前处理帧点云
sensor_msgs::PointCloud2 currentCloudMsg;

// 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时时间戳；用于插值计算当前激光帧起止时间范围内，每一时刻的旋转姿态
double *imuTime = new double[queueLength];
double *imuRotX = new double[queueLength];
double *imuRotY = new double[queueLength];
double *imuRotZ = new double[queueLength];

int imuPointerCur;
bool firstPointFlag;
Eigen::Affine3f transStartInverse;

// 当前帧原始激光点云
pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
// 当期帧运动畸变校正之后的激光点云
pcl::PointCloud<PointType>::Ptr   fullCloud;
// 从fullCloud中提取有效点
pcl::PointCloud<PointType>::Ptr   extractedCloud;

int deskewFlag;
cv::Mat rangeMat;

bool odomDeskewFlag;
// 当前激光帧起止时刻对应imu里程计位姿变换，该变换对应的平移增量；用于插值计算当前激光帧起止时间范围内，每一时刻的位置
float odomIncreX;
float odomIncreY;
float odomIncreZ;

// 当前帧激光点云运动畸变校正之后的数据，包括点云数据、初始位姿、姿态角等，发布给featureExtraction进行特征提取
lio_sam::cloud_info cloudInfo;
// 当前帧起始时刻
double timeScanCur;
// 当前帧结束时刻
double timeScanEnd;
// 当前帧header，包含时间戳信息
std_msgs::Header cloudHeader;

        作用：分配内存空间存储关键数据，包括 IMU 姿态、点云数据和距离图像，是整个处理流程的数据载体。

(3). 构造函数

/**
 * 构造函数
*/
ImageProjection():
deskewFlag(0)
{
    // 订阅原始imu数据
    subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
    // 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
    subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
    // 订阅原始lidar数据
    subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

    // 发布当前激光帧运动畸变校正后的点云，有效点
    pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
    // 发布当前激光帧运动畸变校正后的点云信息
    pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);

    // 初始化
    allocateMemory();
    // 重置参数
    resetParameters();

    // pcl日志级别，只打ERROR日志
    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}

(4). 初始化与重置

/**
 * 初始化
*/
void allocateMemory()
{
    laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
    tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
    fullCloud.reset(new pcl::PointCloud<PointType>());
    extractedCloud.reset(new pcl::PointCloud<PointType>());

    fullCloud->points.resize(N_SCAN*Horizon_SCAN);

    cloudInfo.startRingIndex.assign(N_SCAN, 0);
    cloudInfo.endRingIndex.assign(N_SCAN, 0);

    cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
    cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);

    resetParameters();
}

/**
 * 重置参数，接收每帧lidar数据都要重置这些参数
*/
void resetParameters()
{
    laserCloudIn->clear();
    extractedCloud->clear();

    rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));

    imuPointerCur = 0;
    firstPointFlag = true;
    odomDeskewFlag = false;

    for (int i = 0; i < queueLength; ++i)
    {
        imuTime[i] = 0;
        imuRotX[i] = 0;
        imuRotY[i] = 0;
        imuRotZ[i] = 0;
    }
}

(5). 订阅原始IMU数据、IMU里程计

/**
 * 订阅原始imu数据
 * 1、imu原始测量数据转换到lidar系，加速度、角速度、RPY
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
    // imu原始测量数据转换到lidar系，加速度、角速度、RPY
    sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

    // 上锁，添加数据的时候队列不可用
    std::lock_guard<std::mutex> lock1(imuLock);
    imuQueue.push_back(thisImu);

    // debug IMU data
    // cout << std::setprecision(6);
    // cout << "IMU acc: " << endl;
    // cout << "x: " << thisImu.linear_acceleration.x << 
    //       ", y: " << thisImu.linear_acceleration.y << 
    //       ", z: " << thisImu.linear_acceleration.z << endl;
    // cout << "IMU gyro: " << endl;
    // cout << "x: " << thisImu.angular_velocity.x << 
    //       ", y: " << thisImu.angular_velocity.y << 
    //       ", z: " << thisImu.angular_velocity.z << endl;
    // double imuRoll, imuPitch, imuYaw;
    // tf::Quaternion orientation;
    // tf::quaternionMsgToTF(thisImu.orientation, orientation);
    // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
    // cout << "IMU roll pitch yaw: " << endl;
    // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
}

/**
 * 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
*/
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
    std::lock_guard<std::mutex> lock2(odoLock);
    odomQueue.push_back(*odometryMsg);
}

        作用：接收 IMU 数据并转换到统一坐标系，存入队列供后续去畸变使用。互斥锁确保多线程环境下数据一致性。

(6). 点云缓存与预处理

/**
 * 订阅原始lidar数据
 * 1、添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
 * 2、当前帧起止时刻对应的imu数据、imu里程计数据处理
 *   imu数据：
 *   1) 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
 *   2) 用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
 *   imu里程计数据：
 *   1) 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
 *   2) 用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
 * 3、当前帧激光点云运动畸变校正
 *   1) 检查激光点距离、扫描线是否合规
 *   2) 激光运动畸变校正，保存激光点
 * 4、提取有效激光点，存extractedCloud
 * 5、发布当前帧校正后点云，有效点
 * 6、重置参数，接收每帧lidar数据都要重置这些参数
*/
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    // 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
    if (!cachePointCloud(laserCloudMsg))
        return;
    
    // 当前帧起止时刻对应的imu数据、imu里程计数据处理
    if (!deskewInfo())
        return;

    // 当前帧激光点云运动畸变校正
    // 1、检查激光点距离、扫描线是否合规
    // 2、激光运动畸变校正，保存激光点
    projectPointCloud();

    // 提取有效激光点，存extractedCloud
    cloudExtraction();

    // 发布当前帧校正后点云，有效点
    publishClouds();

    // 重置参数，接收每帧lidar数据都要重置这些参数
    resetParameters();
}

(7). 从激光点云队列中取出最早一帧作为当前帧

/**
 * 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
*/
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    cloudQueue.push_back(*laserCloudMsg);
    if (cloudQueue.size() <= 2)
        return false;

    // 取出激光点云队列中最早的一帧
    currentCloudMsg = std::move(cloudQueue.front());
    cloudQueue.pop_front();
    if (sensor == SensorType::VELODYNE)
    {
        // 转换成pcl点云格式
        pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
    }
    else if (sensor == SensorType::OUSTER)
    {
        // 转换成Velodyne格式
        pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
        laserCloudIn->points.resize(tmpOusterCloudIn->size());
        laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
        for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
        {
            auto &src = tmpOusterCloudIn->points[i];
            auto &dst = laserCloudIn->points[i];
            dst.x = src.x;
            dst.y = src.y;
            dst.z = src.z;
            dst.intensity = src.intensity;
            dst.ring = src.ring;
            dst.time = src.t * 1e-9f;
        }
    }
    else
    {
        ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
        ros::shutdown();
    }

    // 当前帧头部
    cloudHeader = currentCloudMsg.header;
    // 当前帧起始时刻
    timeScanCur = cloudHeader.stamp.toSec();
    // 当前帧结束时刻，注：点云中激光点的time记录相对于当前帧第一个激光点的时差，第一个点time=0
    timeScanEnd = timeScanCur + laserCloudIn->points.back().time;

    // 存在无效点，Nan或者Inf
    if (laserCloudIn->is_dense == false)
    {
        ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
        ros::shutdown();
    }

    // 检查是否存在ring通道，注意static只检查一次
    static int ringFlag = 0;
    if (ringFlag == 0)
    {
        ringFlag = -1;
        for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
        {
            if (currentCloudMsg.fields[i].name == "ring")
            {
                ringFlag = 1;
                break;
            }
        }
        if (ringFlag == -1)
        {
            ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
            ros::shutdown();
        }
    }

    // 检查是否存在time通道
    if (deskewFlag == 0)
    {
        deskewFlag = -1;
        for (auto &field : currentCloudMsg.fields)
        {
            if (field.name == "time" || field.name == "t")
            {
                deskewFlag = 1;
                break;
            }
        }
        if (deskewFlag == -1)
            ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
    }

    return true;
}

(8). 处理当前激光帧起止时刻对应的IMU数据、IMU里程计数据

/**
 * 当前帧起止时刻对应的imu数据、imu里程计数据处理
*/
bool deskewInfo()
{
    std::lock_guard<std::mutex> lock1(imuLock);
    std::lock_guard<std::mutex> lock2(odoLock);

    // 要求imu数据包含激光数据，否则不往下处理了
    if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
    {
        ROS_DEBUG("Waiting for IMU data ...");
        return false;
    }

    // 当前帧对应imu数据处理
    // 1、遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
    // 2、用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
    // 注：imu数据都已经转换到lidar系下了
    imuDeskewInfo();

    // 当前帧对应imu里程计处理
    // 1、遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
    // 2、用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
    // 注：imu数据都已经转换到lidar系下了
    odomDeskewInfo();

    return true;
}

/**
 * 当前帧对应imu数据处理
 * 1、遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
 * 2、用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
 * 注：imu数据都已经转换到lidar系下了
*/
void imuDeskewInfo()
{
    cloudInfo.imuAvailable = false;

    // 从imu队列中删除当前激光帧0.01s前面时刻的imu数据
    while (!imuQueue.empty())
    {
        if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
            imuQueue.pop_front();
        else
            break;
    }

    if (imuQueue.empty())
        return;

    imuPointerCur = 0;

    // 遍历当前激光帧起止时刻（前后扩展0.01s）之间的imu数据
    for (int i = 0; i < (int)imuQueue.size(); ++i)
    {
        sensor_msgs::Imu thisImuMsg = imuQueue[i];
        double currentImuTime = thisImuMsg.header.stamp.toSec();

        // 提取imu姿态角RPY，作为当前lidar帧初始姿态角
        if (currentImuTime <= timeScanCur)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

        // 超过当前激光帧结束时刻0.01s，结束
        if (currentImuTime > timeScanEnd + 0.01)
            break;

        // 第一帧imu旋转角初始化
        if (imuPointerCur == 0){
            imuRotX[0] = 0;
            imuRotY[0] = 0;
            imuRotZ[0] = 0;
            imuTime[0] = currentImuTime;
            ++imuPointerCur;
            continue;
        }

        // 提取imu角速度
        double angular_x, angular_y, angular_z;
        imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

        // imu帧间时差
        double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
        // 当前时刻旋转角 = 前一时刻旋转角 + 角速度 * 时差
        imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
        imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
        imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
        imuTime[imuPointerCur] = currentImuTime;
        ++imuPointerCur;
    }

    --imuPointerCur;

    // 没有合规的imu数据
    if (imuPointerCur <= 0)
        return;

    cloudInfo.imuAvailable = true;
}

/**
 * 当前帧对应imu里程计处理
 * 1、遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
 * 2、用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
 * 注：imu数据都已经转换到lidar系下了
*/
void odomDeskewInfo()
{
    cloudInfo.odomAvailable = false;

    // 从imu里程计队列中删除当前激光帧0.01s前面时刻的imu数据
    while (!odomQueue.empty())
    {
        if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
            odomQueue.pop_front();
        else
            break;
    }

    if (odomQueue.empty())
        return;

    // 要求必须有当前激光帧时刻之前的imu里程计数据
    if (odomQueue.front().header.stamp.toSec() > timeScanCur)
        return;

    // 提取当前激光帧起始时刻的imu里程计
    nav_msgs::Odometry startOdomMsg;

    for (int i = 0; i < (int)odomQueue.size(); ++i)
    {
        startOdomMsg = odomQueue[i];

        if (ROS_TIME(&startOdomMsg) < timeScanCur)
            continue;
        else
            break;
    }

    // 提取imu里程计姿态角
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);

    double roll, pitch, yaw;
    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

    // 用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmization
    cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
    cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
    cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
    cloudInfo.initialGuessRoll  = roll;
    cloudInfo.initialGuessPitch = pitch;
    cloudInfo.initialGuessYaw   = yaw;

    cloudInfo.odomAvailable = true;

    odomDeskewFlag = false;

    // 如果当前激光帧结束时刻之后没有imu里程计数据，返回
    if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
        return;

    // 提取当前激光帧结束时刻的imu里程计
    nav_msgs::Odometry endOdomMsg;

    for (int i = 0; i < (int)odomQueue.size(); ++i)
    {
        endOdomMsg = odomQueue[i];

        if (ROS_TIME(&endOdomMsg) < timeScanEnd)
            continue;
        else
            break;
    }

    // 如果起止时刻对应imu里程计的方差不等，返回
    if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
        return;

    Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);

    tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
    Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);

    // 起止时刻imu里程计的相对变换
    Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

    // 相对变换，提取增量平移、旋转（欧拉角）
    float rollIncre, pitchIncre, yawIncre;
    pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

    odomDeskewFlag = true;
}

(9). 在当前激光帧起止时间范围内，计算某一时刻的旋转、平移增量

/**
 * 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）
*/
void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
    *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

    // 查找当前时刻在imuTime下的索引
    int imuPointerFront = 0;
    while (imuPointerFront < imuPointerCur)
    {
        if (pointTime < imuTime[imuPointerFront])
            break;
        ++imuPointerFront;
    }

    // 设为离当前时刻最近的旋转增量
    if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
    {
        *rotXCur = imuRotX[imuPointerFront];
        *rotYCur = imuRotY[imuPointerFront];
        *rotZCur = imuRotZ[imuPointerFront];
    } else {
        // 前后时刻插值计算当前时刻的旋转增量
        int imuPointerBack = imuPointerFront - 1;
        double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
        double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
        *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
        *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
        *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
    }
}

/**
 * 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量）
*/
void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
{
    // 如果传感器移动速度较慢，例如人行走的速度，那么可以认为激光在一帧时间范围内，平移量小到可以忽略不计
    *posXCur = 0; *posYCur = 0; *posZCur = 0;

    // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.

    // if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
    //     return;

    // float ratio = relTime / (timeScanEnd - timeScanCur);

    // *posXCur = ratio * odomIncreX;
    // *posYCur = ratio * odomIncreY;
    // *posZCur = ratio * odomIncreZ;
}

(10). 激光点云运动畸变校正

/**
 * 激光运动畸变校正
 * 利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
*/
PointType deskewPoint(PointType *point, double relTime)
{
    if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
        return *point;

    // relTime是当前激光点相对于激光帧起始时刻的时间，pointTime则是当前激光点的时间戳
    double pointTime = timeScanCur + relTime;

    // 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）
    float rotXCur, rotYCur, rotZCur;
    findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);

    // 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量）
    float posXCur, posYCur, posZCur;
    findPosition(relTime, &posXCur, &posYCur, &posZCur);

    // 第一个点的位姿增量（0），求逆
    if (firstPointFlag == true)
    {
        transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
        firstPointFlag = false;
    }

    // 当前时刻激光点与第一个激光点的位姿变换
    Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
    Eigen::Affine3f transBt = transStartInverse * transFinal;

    // 当前激光点在第一个激光点坐标系下的坐标
    PointType newPoint;
    newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
    newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
    newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
    newPoint.intensity = point->intensity;

    return newPoint;
}

(11). 畸变校正后的点云保存，提取有效点，发布

/**
 * 当前帧激光点云运动畸变校正
 * 1、检查激光点距离、扫描线是否合规
 * 2、激光运动畸变校正，保存激光点
*/
void projectPointCloud()
{
    int cloudSize = laserCloudIn->points.size();
    
    // 遍历当前帧激光点云
    for (int i = 0; i < cloudSize; ++i)
    {
        // pcl格式
        PointType thisPoint;
        thisPoint.x = laserCloudIn->points[i].x;
        thisPoint.y = laserCloudIn->points[i].y;
        thisPoint.z = laserCloudIn->points[i].z;
        thisPoint.intensity = laserCloudIn->points[i].intensity;

        // 距离检查
        float range = pointDistance(thisPoint);
        if (range < lidarMinRange || range > lidarMaxRange)
            continue;

        // 扫描线检查
        int rowIdn = laserCloudIn->points[i].ring;
        if (rowIdn < 0 || rowIdn >= N_SCAN)
            continue;

        // 扫描线如果有降采样，跳过采样的扫描线这里要跳过
        if (rowIdn % downsampleRate != 0)
            continue;

        float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

        // 水平扫描角度步长，例如一周扫描1800次，则两次扫描间隔角度0.2°
        static float ang_res_x = 360.0/float(Horizon_SCAN);
        int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
        if (columnIdn >= Horizon_SCAN)
            columnIdn -= Horizon_SCAN;

        if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
            continue;

        // 已经存过该点，不再处理
        if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
            continue;

        // 激光运动畸变校正
        // 利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
        thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

        // 矩阵存激光点的距离
        rangeMat.at<float>(rowIdn, columnIdn) = range;

        // 转换成一维索引，存校正之后的激光点
        int index = columnIdn + rowIdn * Horizon_SCAN;
        fullCloud->points[index] = thisPoint;
    }
}

/**
 * 提取有效激光点，存extractedCloud
*/
void cloudExtraction()
{
    // 有效激光点数量
    int count = 0;
    // 遍历所有激光点
    for (int i = 0; i < N_SCAN; ++i)
    {
        // 记录每根扫描线起始第5个激光点在一维数组中的索引
        cloudInfo.startRingIndex[i] = count - 1 + 5;

        for (int j = 0; j < Horizon_SCAN; ++j)
        {
            // 有效激光点
            if (rangeMat.at<float>(i,j) != FLT_MAX)
            {
                // 记录激光点对应的Horizon_SCAN方向上的索引
                cloudInfo.pointColInd[count] = j;
                // 激光点距离
                cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
                // 加入有效激光点
                extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                ++count;
            }
        }
        // 记录每根扫描线倒数第5个激光点在一维数组中的索引
        cloudInfo.endRingIndex[i] = count -1 - 5;
    }
}

/**
 * 发布当前帧校正后点云，有效点
*/
void publishClouds()
{
    cloudInfo.header = cloudHeader;
    cloudInfo.cloud_deskewed  = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
    pubLaserCloudInfo.publish(cloudInfo);
}

(12). 主函数

int main(int argc, char** argv)
{
    // 初始化ROS节点，设置节点名称为"lio_sam"
    // argc, argv：命令行参数，用于传递启动参数
    // 作用：注册节点到ROS网络，初始化ROS通信环境
    ros::init(argc, argv, "lio_sam");

    // 创建ImageProjection类的实例IP
    // 作用：初始化图像投影模块，自动调用构造函数完成资源分配和订阅发布设置
    ImageProjection IP;
    
    // 输出日志信息，使用ANSI转义序列设置绿色加粗字体
    // 作用：提示用户模块已成功启动
    ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");

    // 创建多线程 spinner，设置线程数为3
    // 作用：处理ROS回调函数的多线程执行，提高系统响应性
    ros::MultiThreadedSpinner spinner(3);
    
    // 启动 spinner，进入事件循环
    // 作用：持续处理ROS消息回调，直到节点被关闭
    spinner.spin();
    
    return 0;
}

整个 ImageProjection 模块的核心逻辑可概括为：

        1. 数据接收与缓存：接收激光雷达和 IMU 数据，统一格式并缓存

        2. 时间同步与姿态插值：通过 IMU 数据计算扫描周期内的姿态变化

        3. 点云去畸变：将每个点从采集时刻坐标系转换到统一坐标系

        4. 距离图像投影：将三维点云转换为二维结构化表示

        5. 特征准备：提取有效点并记录关键信息，为后续特征提取提供基础