imageProjection.cpp 是 LIO-SAM 前端里非常关键的一环。它处在 原始传感器数据进入系统之后、特征提取之前。它的输入主要有三类：

1. LiDAR 原始点云
2. IMU 数据
3. Odom 增量里程计数据

它的输出主要有两类：

1. lio_sam/deskew/cloud_deskewed
   去畸变后的有效点云

2. lio_sam/deskew/cloud_info
   当前帧点云的完整信息包

这个文件的核心任务不是建图，也不是回环，也不是后端优化，而是把原始点云处理成后续模块容易用的结构。原始点云刚进来时，只是一堆无序的 3D 点。后面的 featureExtraction 不希望直接面对这种原始点云，而是希望点云已经被整理好，比如每个点属于哪条线、在水平扫描方向是第几列、距离雷达多远、有没有去畸变、IMU/Odom 是否可用等。

所以可以把它理解为：

原始点云加工厂

原始 LiDAR 点云
    ↓
按时间找 IMU / Odom
    ↓
用 IMU 做旋转去畸变
    ↓
把 3D 点云投影成二维 range image
    ↓
提取有效点云
    ↓
打包成 cloud_info
    ↓
交给 featureExtraction

最关键的几个概念是：

ring:
    表示点属于第几条激光线，决定 range image 的行号。

time / t:
    表示点在一帧点云中的相对采样时间，决定能不能点级去畸变。

rangeMat:
    二维距离图，行是 ring，列是水平角，值是距离。

fullCloud:
    和 rangeMat 一一对应的完整投影点云。

extractedCloud:
    从 fullCloud 中提取出来的有效点云。

cloudInfo:
    当前帧点云的完整信息包，会发给后面的 featureExtraction。

文件开头与点类型定义：第 1～38 行

这一段主要解决一个问题：不同雷达的点云字段不一样，代码需要先定义自己能识别的点类型。

#include "utility.h"  // 引入 LIO-SAM 公共工具头文件，里面有参数、点类型、工具函数、ROS/PCL/Eigen等。
#include "lio_sam/cloud_info.h"  // 引入 cloud_info 自定义消息，用来传递去畸变点云和各种辅助信息。

struct VelodynePointXYZIRT  // 定义 Velodyne/Livox 常用点类型：XYZ + intensity + ring + time。
{
    PCL_ADD_POINT4D  // PCL 宏，添加 x、y、z 坐标字段，并做点类型对齐。
    PCL_ADD_INTENSITY;  // 添加 intensity 字段，表示反射强度。
    uint16_t ring;  // 当前点属于第几条激光线，例如 16 线雷达 ring 范围通常是 0~15。
    float time;  // 当前点相对本帧点云起始时刻的时间偏移，用于点云去畸变。
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // Eigen 内存对齐宏，避免 PCL/Eigen 内存对齐错误。
} EIGEN_ALIGN16;  // 让结构体 16 字节对齐。

POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,  // 把 VelodynePointXYZIRT 注册给 PCL。
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)  // 注册坐标和强度字段。
    (uint16_t, ring, ring) (float, time, time)  // 注册 ring 和 time 字段。
)

struct OusterPointXYZIRT {  // 定义 Ouster 雷达点类型，Ouster 原始字段比 Velodyne 多。
    PCL_ADD_POINT4D;  // 添加 x、y、z 坐标。
    float intensity;  // 反射强度。
    uint32_t t;  // Ouster 点时间字段，通常是纳秒级。
    uint16_t reflectivity;  // Ouster 反射率字段。
    uint8_t ring;  // 当前点属于第几条激光线。
    uint16_t noise;  // Ouster 噪声字段。
    uint32_t range;  // Ouster 原始距离字段。
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // Eigen 内存对齐宏。
} EIGEN_ALIGN16;  // 16 字节对齐。

POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,  // 把 OusterPointXYZIRT 注册给 PCL。
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)  // 注册坐标和强度。
    (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)  // 注册 Ouster 时间和反射率。
    (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)  // 注册 ring、noise、range。
)

// Use the Velodyne point format as a common representation
using PointXYZIRT = VelodynePointXYZIRT;  // 后面统一用 PointXYZIRT 表示带 ring/time 的点。

const int queueLength = 2000;  // IMU 积分缓存长度，最多保存 2000 个 IMU 时间点的积分结果。

详细解释：
普通 PCL 点云一般只需要 x、y、z，但 LIO-SAM 的前端必须知道每个点的 ring 和 time。ring 决定这个点属于哪一条激光线，也就是后面 range image 的行号；time 决定这个点在一帧扫描中的采样时刻，也就是后面做点云去畸变的关键。

机械式 LiDAR 扫描一帧需要时间。比如一帧开始时车头朝 0 度，扫到一半时车可能已经转了 2 度，扫到最后可能转了 4 度。如果没有每个点的 time，代码就不知道这个点是在车转到多少度时采集的，也就没法把它准确拉回到帧起始时刻。

Ouster 和 Velodyne 的点云字段不同。Velodyne 常用 time，Ouster 常用 t，而且 Ouster 的 t 往往是纳秒，所以后面代码会把 Ouster 的 t 乘 1e-9 转成秒。这样后面的处理就能统一用 PointXYZIRT，不用每个函数都区分雷达型号。

这里容易误解的一点是：intensity 不是去畸变必须字段，真正关键的是 ring 和 time。intensity 更多是反射强度，可用于显示、辅助处理或保持点云属性。

类成员变量：第 40～92 行

这一段定义 ImageProjection 内部要用的数据结构。可以把它看成这个模块的“工作区”。

class ImageProjection : public ParamServer  // 定义 ImageProjection 类，继承 ParamServer，可以直接读取配置参数。
{
private:

    std::mutex imuLock;  // IMU 队列锁，防止多线程同时读写 imuQueue。
    std::mutex odoLock;  // Odom 队列锁，防止多线程同时读写 odomQueue。

    ros::Subscriber subLaserCloud;  // 原始 LiDAR 点云订阅器。
    ros::Publisher  pubLaserCloud;  // 点云发布器，这里基本没有实际使用。
    
    ros::Publisher pubExtractedCloud;  // 发布去畸变后的有效点云。
    ros::Publisher pubLaserCloudInfo;  // 发布 cloud_info 消息。

    ros::Subscriber subImu;  // IMU 订阅器。
    std::deque<sensor_msgs::Imu> imuQueue;  // IMU 缓存队列。

    ros::Subscriber subOdom;  // Odom 订阅器。
    std::deque<nav_msgs::Odometry> odomQueue;  // Odom 缓存队列。

    std::deque<sensor_msgs::PointCloud2> cloudQueue;  // 点云缓存队列。
    sensor_msgs::PointCloud2 currentCloudMsg;  // 当前正在处理的点云消息。

    double *imuTime = new double[queueLength];  // 每个 IMU 积分点的时间。
    double *imuRotX = new double[queueLength];  // x 轴累计旋转量。
    double *imuRotY = new double[queueLength];  // y 轴累计旋转量。
    double *imuRotZ = new double[queueLength];  // z 轴累计旋转量。

    int imuPointerCur;  // 当前 IMU 积分数组末尾索引。
    bool firstPointFlag;  // 当前帧第一个点是否已经处理。
    Eigen::Affine3f transStartInverse;  // 帧起始时刻位姿的逆变换。

    pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;  // 输入点云，统一格式。
    pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;  // Ouster 临时点云。
    pcl::PointCloud<PointType>::Ptr   fullCloud;  // 投影后的完整点云，对应 range image。
    pcl::PointCloud<PointType>::Ptr   extractedCloud;  // 从 range image 中提取出来的有效点云。

    int deskewFlag;  // 去畸变标志：0未检查，1可去畸变，-1不能去畸变。
    cv::Mat rangeMat;  // range image，保存每个格子的距离。

    bool odomDeskewFlag;  // Odom 平移去畸变是否可用。
    float odomIncreX;  // 当前帧扫描期间 Odom 的 x 平移增量。
    float odomIncreY;  // 当前帧扫描期间 Odom 的 y 平移增量。
    float odomIncreZ;  // 当前帧扫描期间 Odom 的 z 平移增量。

    lio_sam::cloud_info cloudInfo;  // 当前帧要发布的 cloud_info。
    double timeScanCur;  // 当前 LiDAR 帧起始时间。
    double timeScanEnd;  // 当前 LiDAR 帧结束时间。
    std_msgs::Header cloudHeader;  // 当前点云 header。

    vector<int> columnIdnCountVec;  // Livox 用的列计数器。

详细解释：
这些成员变量按照功能可以分成几组。第一组是订阅器和发布器，负责和 ROS 通信。subImu 接 IMU，subOdom 接 Odom，subLaserCloud 接 LiDAR 点云；pubExtractedCloud 发布去畸变点云，pubLaserCloudInfo 发布完整的 cloud_info。

第二组是队列：imuQueue、odomQueue、cloudQueue。因为 IMU、Odom、LiDAR 的频率不同，三者不会同时到达，所以必须先缓存起来。比如 LiDAR 是 10Hz，IMU 可能是 200Hz，一帧 LiDAR 扫描期间会有很多帧 IMU 数据。处理 LiDAR 时，代码会从 IMU 队列中找覆盖这一帧扫描时间段的数据。

第三组是 IMU 积分数组：imuTime、imuRotX、imuRotY、imuRotZ。IMU 直接给的是角速度，不是“当前已经转了多少角度”。代码需要把角速度按时间积分，得到扫描期间的累计旋转。这个累计旋转后面用于每个点的去畸变。

第四组是点云容器：laserCloudIn 是输入点云，fullCloud 是按照 range image 排布后的完整点云，extractedCloud 是从 fullCloud 里拿出来的有效点云。fullCloud 像一张大表，有些位置可能没点；extractedCloud 是紧凑数组，只保存有效点，后续特征提取主要用它。

第五组是时间和状态：timeScanCur 是当前 LiDAR 帧开始时间，timeScanEnd 是结束时间。deskewFlag 表示能不能做去畸变，firstPointFlag 表示当前帧第一个点是否处理过，transStartInverse 记录帧起始位姿的逆变换。

这里要注意，rangeMat 不是普通图片，而是一个二维距离矩阵。行是激光线号 ring，列是水平角度索引，值是点到雷达的距离。这个结构让后续特征提取能很方便地找同一条扫描线上的前后邻居。

ImageProjection 构造函数：第 94～108 行

作用：模块启动时初始化 ROS 订阅、发布和内存。

ImageProjection():
deskewFlag(0)  // 初始化 deskewFlag 为 0，表示还没有检查点云是否带 time/t 字段。
{
    subImu = nh.subscribe<sensor_msgs::Imu>(
        imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay()
    );  // 订阅 IMU 话题，收到后进入 imuHandler。

    subOdom = nh.subscribe<nav_msgs::Odometry>(
        odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay()
    );  // 订阅增量 Odom，通常来自 IMU 预积分模块。

    subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(
        pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay()
    );  // 订阅原始 LiDAR 点云，收到后进入 cloudHandler。

    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
    );  // 发布 cloud_info 消息。

    allocateMemory();  // 分配点云容器和 cloudInfo 数组空间。
    resetParameters();  // 重置当前帧临时变量。

    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);  // 设置 PCL 只输出错误日志，减少刷屏。
}

详细解释：
构造函数就是这个模块的“开机初始化”。程序一创建 ImageProjection IP，这个函数就会自动执行。它首先订阅三个输入话题：IMU、增量 Odom、原始 LiDAR 点云。IMU 的队列长度设置成 2000，是因为 IMU 频率高，短时间内会来很多帧；点云队列只设置成 5，是因为 LiDAR 频率低，点云本身也大，不适合堆太多。

这里的 Odom 订阅的是 odomTopic + "_incremental"，这个很关键。它不是普通意义上的地图位姿，而通常是 IMU 预积分模块输出的短时间连续里程计。这个 Odom 的主要作用是给当前 LiDAR 帧提供一个初始位姿，让后面的 scan-to-map 优化从一个比较接近真实值的位置开始。

构造函数后半部分创建两个发布器。cloud_deskewed 是给人看和给后续模块用的去畸变点云；cloud_info 是更完整的信息包，里面不仅有点云，还有每条线的索引、每个点的距离、IMU/Odom 是否可用、初始位姿等。后续 featureExtraction 主要依赖 cloud_info。

最后调用 allocateMemory() 和 resetParameters()。前者分配长期容器，后者清空当前帧临时变量。PCL 日志级别设成 L_ERROR，是为了避免 PCL 输出过多普通信息，影响终端观察。

allocateMemory()：第 110～126 行

作用：提前分配点云容器和 cloudInfo 相关数组。

void allocateMemory()
{
    laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());  // 创建输入点云容器。
    tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());  // 创建 Ouster 临时点云容器。
    fullCloud.reset(new pcl::PointCloud<PointType>());  // 创建完整投影点云容器。
    extractedCloud.reset(new pcl::PointCloud<PointType>());  // 创建有效点云容器。

    fullCloud->points.resize(N_SCAN*Horizon_SCAN);  // fullCloud 大小固定为线数 × 水平列数。

    cloudInfo.startRingIndex.assign(N_SCAN, 0);  // 为每条 ring 的起始索引分配空间。
    cloudInfo.endRingIndex.assign(N_SCAN, 0);  // 为每条 ring 的结束索引分配空间。

    cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);  // 为每个有效点的列号数组分配最大空间。
    cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);  // 为每个有效点的距离数组分配最大空间。

    resetParameters();  // 分配完成后重置临时变量。
}

详细解释：
这个函数负责创建几个核心容器。laserCloudIn 存当前输入点云，所有雷达最终都会尽量统一成 PointXYZIRT 格式。tmpOusterCloudIn 只在 Ouster 雷达时使用，因为 Ouster 的原始字段和内部统一格式不同。fullCloud 是一个固定大小的点云数组，它的大小等于 N_SCAN * Horizon_SCAN，本质上就是把二维 range image 摊平成一维数组。extractedCloud 是最终提取出来的有效点云。

为什么 fullCloud 要固定大小？因为它要和 rangeMat 一一对应。假设 N_SCAN=16，Horizon_SCAN=1800，那 rangeMat 是 16 行 1800 列，fullCloud 也要能放下 16×1800 个位置。二维坐标 (row, col) 转成一维就是：

index = col + row * Horizon_SCAN

cloudInfo.startRingIndex 和 cloudInfo.endRingIndex 是后续特征提取按线处理的基础。比如第 5 条扫描线在 extractedCloud 中从第 1200 个点开始，到第 1500 个点结束，后面提曲率时就只在这一段里找邻居，不会把不同 ring 的点混在一起。

pointColInd 和 pointRange 的空间按照最大可能点数分配。实际有效点一般少于 N_SCAN * Horizon_SCAN，但提前分配最大空间可以避免每帧动态扩容，提高稳定性。

resetParameters()：第 128～148 行

作用：一帧处理完后清空临时变量，避免污染下一帧。

void resetParameters()
{
    laserCloudIn->clear();  // 清空输入点云。
    extractedCloud->clear();  // 清空有效点云。

    rangeMat = cv::Mat(
        N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)
    );  // 创建 range image，初始值 FLT_MAX 表示该格子没有点。

    imuPointerCur = 0;  // IMU 积分数组指针归零。
    firstPointFlag = true;  // 新一帧开始，第一个点还没处理。
    odomDeskewFlag = false;  // 新一帧开始，先认为 Odom 平移去畸变不可用。

    for (int i = 0; i < queueLength; ++i)
    {
        imuTime[i] = 0;  // 清空 IMU 时间。
        imuRotX[i] = 0;  // 清空 x 方向旋转积分。
        imuRotY[i] = 0;  // 清空 y 方向旋转积分。
        imuRotZ[i] = 0;  // 清空 z 方向旋转积分。
    }

    columnIdnCountVec.assign(N_SCAN, 0);  // Livox 每条扫描线的列计数器清零。
}

详细解释：
这个函数是每帧处理结束后的清理动作。laserCloudIn 和 extractedCloud 都是当前帧临时点云，下一帧不能继续用上一帧的数据，所以要清空。rangeMat 也必须重新初始化，因为它记录的是当前帧每个 row/col 格子的距离。如果不重新置为 FLT_MAX，上一帧占用过的格子会被误认为这一帧也有点。

FLT_MAX 是一个很大的 float 值，这里用它表示“空格子”。后面投影点云时，如果某个位置仍然是 FLT_MAX，说明还没有点；如果不是 FLT_MAX，说明已经被某个点填过了。

firstPointFlag = true 非常关键。每帧点云去畸变时，第一个点会被当作当前帧的时间参考，代码会记录这个起始位姿的逆变换。下一帧开始时必须重新允许第一个点设置新的起始位姿，否则下一帧会错误地使用上一帧的起始变换。

imuTime/imuRotX/Y/Z 也要清零，因为这些积分结果只对应当前 LiDAR 扫描时间段。下一帧 LiDAR 有新的起止时间，需要重新根据 IMU 队列积分。

imuHandler()：第 152～175 行

作用：接收 IMU，转换坐标系，放入队列。

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
    sensor_msgs::Imu thisImu = imuConverter(*imuMsg);  // 把原始 IMU 转换到 LIO-SAM 统一坐标系。

    std::lock_guard<std::mutex> lock1(imuLock);  // 加锁，防止多线程同时访问 imuQueue。
    imuQueue.push_back(thisImu);  // 把转换后的 IMU 放入队列。

    // debug IMU data
    // 下面注释部分只是调试打印，用来查看 IMU 加速度、角速度、姿态角是否正常。
}

详细解释：
这个函数本身不做积分，也不直接参与点云去畸变，它只负责把 IMU 数据存起来。真正使用 IMU 的地方是 imuDeskewInfo() 和 findRotation()。

imuConverter() 是关键。实际硬件安装时，IMU 坐标系可能和 LiDAR 坐标系不一致。比如 IMU 的 x 轴可能指向车体右侧，而 LiDAR 的 x 轴可能指向前方。如果不转换坐标系，后面用 IMU 角速度去修正点云时，方向就会错。比如实际是 yaw 方向转动，代码可能误认为是 roll 方向转动，点云会越去畸变越歪。

这里使用 std::lock_guard<std::mutex> 是因为程序用了多线程 spinner。IMU 回调可能正在往 imuQueue 写数据，而 LiDAR 点云回调可能正在从 imuQueue 读数据。没有锁的话，可能出现队列读写冲突。

小白可以这样理解：
imuHandler() 就像一个收件员。IMU 数据来了以后，它不拆开分析，只是先转换坐标系，然后按时间顺序放入仓库。等 LiDAR 点云来了，点云处理函数再去仓库里取需要的 IMU 数据。

odometryHandler()：第 177～181 行

作用：接收增量 Odom，放入队列。

void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
    std::lock_guard<std::mutex> lock2(odoLock);  // 加锁，保护 odomQueue。
    odomQueue.push_back(*odometryMsg);  // 把 Odom 放入队列。
}

详细解释：
这个函数和 imuHandler() 类似，也只是缓存数据。Odom 的主要用途有两个：第一，给后端 scan-to-map 匹配提供初始位姿；第二，理论上可以辅助做平移去畸变。

不过当前这份代码默认没有真正启用平移去畸变，因为 findPosition() 中相关代码被注释掉了。所以在这个模块里，Odom 更重要的作用是提供 initialGuessX/Y/Z/Roll/Pitch/Yaw。这些初值会进入 cloudInfo，后面 mapOptimization 会用它作为当前帧匹配初值。

这里也需要加锁，因为 Odom 回调和点云回调可能同时访问 odomQueue。

cloudHandler()：第 183～198 行

作用：一帧 LiDAR 点云的主处理入口。

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    if (!cachePointCloud(laserCloudMsg))  // 缓存并转换点云；如果点云不足或格式不合格，返回 false。
        return;

    if (!deskewInfo())  // 准备 IMU/Odom 去畸变信息；如果 IMU 覆盖不了当前扫描，返回 false。
        return;

    projectPointCloud();  // 点云去畸变，并投影到 range image。

    cloudExtraction();  // 从 range image 提取有效点云，记录 ring 起止索引、列号、距离。

    publishClouds();  // 发布 cloud_deskewed 和 cloud_info。

    resetParameters();  // 清空当前帧临时变量，准备下一帧。
}

详细解释：
这是整个文件的主线。每来一帧 LiDAR 点云，都会进入这个函数。它本身不展开具体细节，而是按顺序调用多个子函数完成一帧点云加工。

第一步 cachePointCloud() 是点云入门检查。它会缓存点云、转换格式、检查有没有 ring、有没有 time/t、有没有 NaN，并计算当前帧的起止时间。如果点云格式不满足要求，后续流程就不能做。

第二步 deskewInfo() 是去畸变前的数据准备。它要确认 IMU 是否覆盖当前 LiDAR 扫描时间段。如果 IMU 不够，说明这一帧点云中某些点无法找到对应的 IMU 旋转量，就直接返回等待数据。

第三步 projectPointCloud() 是最核心的点级处理。它会遍历当前帧每个点，计算这个点的距离、ring、列号，然后调用 deskewPoint() 对点做去畸变，最后放进 rangeMat 和 fullCloud。

第四步 cloudExtraction() 把 fullCloud 中有效的点取出来，形成紧凑的 extractedCloud，并记录每个点的列号和距离。

第五步 publishClouds() 发布结果。

最后 resetParameters() 清空当前帧临时数据。这个调用必须在发布之后，否则数据会被提前清掉。

cachePointCloud()：第 200～287 行

作用：缓存点云、统一雷达格式、检查字段、计算扫描起止时间。

bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    cloudQueue.push_back(*laserCloudMsg);  // 把最新点云加入队列。
    if (cloudQueue.size() <= 2)  // 如果缓存点云数量小于等于 2。
        return false;  // 先不处理，等待更多数据，方便 IMU/Odom 时间覆盖。

    currentCloudMsg = std::move(cloudQueue.front());  // 取队首点云作为当前处理帧。
    cloudQueue.pop_front();  // 弹出已经取出的点云。

    if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX)
    {
        pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);  // ROS PointCloud2 转 PCL 点云。
    }
    else if (sensor == SensorType::OUSTER)
    {
        pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);  // 先转成 Ouster 点云。
        laserCloudIn->points.resize(tmpOusterCloudIn->size());  // 统一点云分配同样点数。
        laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;  // 复制 dense 标志。

        for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
        {
            auto &src = tmpOusterCloudIn->points[i];  // Ouster 原始点。
            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;  // Ouster 纳秒时间转秒。
        }
    }
    else
    {
        ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
        ros::shutdown();
    }

    cloudHeader = currentCloudMsg.header;  // 保存点云 header。
    timeScanCur = cloudHeader.stamp.toSec();  // 当前帧起始时间。
    timeScanEnd = timeScanCur + laserCloudIn->points.back().time;  // 当前帧结束时间。

    if (laserCloudIn->is_dense == false)
    {
        ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
        ros::shutdown();
    }

    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();
        }
    }

    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;
}

详细解释：
这个函数是点云进入系统后的第一道门。它首先把点云放进 cloudQueue，但不会马上处理，而是要求队列里至少超过 2 帧。这样做的目的不是为了算法必须要三帧点云，而是为了启动阶段的数据同步更稳。因为刚启动时 LiDAR 点云可能先到了，但 IMU/Odom 还没有足够覆盖当前扫描时间段，直接处理容易失败。

点云格式转换是这个函数的第二个重点。Velodyne 和 Livox 可以直接转成 PointXYZIRT，Ouster 需要先转成 OusterPointXYZIRT，再逐点拷贝到统一格式。Ouster 的时间字段是 t，一般是纳秒，所以要乘 1e-9 转成秒。这样后面所有代码都可以统一用 laserCloudIn->points[i].time。

timeScanCur 和 timeScanEnd 是整个去畸变流程的时间边界。timeScanCur 来自点云 header，一般表示这一帧扫描开始时间。timeScanEnd 用最后一个点的相对时间估计：

timeScanEnd = timeScanCur + 最后一个点的相对时间

比如一帧点云 header 时间是 100.000s，最后一个点 time=0.100s，那么这一帧扫描时间就是 100.000s ~ 100.100s。

接下来检查 is_dense。如果点云不是 dense，说明里面可能有 NaN 点。NaN 参与距离计算、角度计算、矩阵变换时会造成异常，所以这里直接报错退出。

然后检查 ring。这个字段必须存在，否则代码不知道点属于哪条扫描线。没有 ring，range image 的行号就无法确定，所以直接关闭节点。

最后检查 time 或 t。没有点时间字段时，不会直接退出，因为系统还可以勉强运行，但是会关闭点级去畸变。这样在机器人运动明显时，点云会产生畸变，后面匹配和建图都可能变差。

deskewInfo()：第 289～306 行

作用：确认 IMU 数据是否足够，并准备 IMU/Odom 去畸变信息。

bool deskewInfo()
{
    std::lock_guard<std::mutex> lock1(imuLock);  // 锁住 IMU 队列。
    std::lock_guard<std::mutex> lock2(odoLock);  // 锁住 Odom 队列。

    if (imuQueue.empty() || 
        imuQueue.front().header.stamp.toSec() > timeScanCur || 
        imuQueue.back().header.stamp.toSec() < timeScanEnd)
    {
        ROS_DEBUG("Waiting for IMU data ...");
        return false;
    }

    imuDeskewInfo();  // 计算 IMU 旋转积分。
    odomDeskewInfo();  // 获取 Odom 初始位姿和扫描期间增量。

    return true;
}

详细解释：
这个函数是去畸变前的总检查。点云去畸变需要 IMU 覆盖整帧扫描时间。比如当前 LiDAR 一帧从 10.000s 扫到 10.100s，那么 IMU 队列至少要包含 10.000s 前后的 IMU，也要包含 10.100s 后的 IMU。否则某些点的采样时刻找不到对应 IMU 旋转，插值就不可靠。

这里有三个失败条件：

imuQueue.empty():
    完全没有 IMU 数据。

imuQueue.front() > timeScanCur:
    IMU 最早数据比点云开始还晚，缺少扫描开头的 IMU。

imuQueue.back() < timeScanEnd:
    IMU 最新数据比点云结束还早，缺少扫描结尾的 IMU。

只有 IMU 覆盖了整帧扫描，才继续调用 imuDeskewInfo() 和 odomDeskewInfo()。这两个函数分别准备旋转去畸变信息和 Odom 初始位姿。

这个函数同时锁住 IMU 和 Odom 队列，是为了避免在处理过程中回调线程继续修改队列。否则可能刚检查完队列，另一个线程就弹入或改动数据，造成时间判断不一致。

imuDeskewInfo()：第 308～365 行

作用：把 IMU 角速度积分成扫描期间的累计旋转，用于点级去畸变。

void imuDeskewInfo()
{
    cloudInfo.imuAvailable = false;  // 先标记 IMU 不可用。

    while (!imuQueue.empty())
    {
        if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
            imuQueue.pop_front();  // 清理太旧的 IMU。
        else
            break;
    }

    if (imuQueue.empty())
        return;

    imuPointerCur = 0;

    for (int i = 0; i < (int)imuQueue.size(); ++i)
    {
        sensor_msgs::Imu thisImuMsg = imuQueue[i];
        double currentImuTime = thisImuMsg.header.stamp.toSec();

        if (currentImuTime <= timeScanCur)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

        if (currentImuTime > timeScanEnd + 0.01)
            break;

        if (imuPointerCur == 0){
            imuRotX[0] = 0;
            imuRotY[0] = 0;
            imuRotZ[0] = 0;
            imuTime[0] = currentImuTime;
            ++imuPointerCur;
            continue;
        }

        double angular_x, angular_y, angular_z;
        imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

        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;

    if (imuPointerCur <= 0)
        return;

    cloudInfo.imuAvailable = true;
}

详细解释：
这个函数是 IMU 去畸变准备的核心。IMU 的陀螺仪给的是角速度，例如绕 z 轴每秒转多少弧度。点云去畸变需要的是某个点采样时刻相对帧起始时刻已经转了多少角度，所以必须对角速度做积分。

积分逻辑可以理解成：

上一时刻累计旋转 + 当前角速度 × 时间间隔 = 当前累计旋转

代码分别对 x、y、z 三个方向做这个操作，结果存在：

imuRotX
imuRotY
imuRotZ

imuTime 存每个积分结果对应的时间。后面 findRotation() 会根据某个点的采样时间，在这些数组里查找或插值得到该点时刻的旋转量。

开头的 while 循环用于清理太旧的 IMU。太早的 IMU 已经不属于当前 LiDAR 扫描范围，继续留在队列里没有意义，还会增加遍历开销。这里用 timeScanCur - 0.01 而不是严格 timeScanCur，是为了保留一点点前置时间余量，方便插值。

imuRPY2rosRPY() 的作用是把当前扫描起始附近的 IMU 姿态保存到 cloudInfo.imuRollInit/imuPitchInit/imuYawInit。它不是最终优化后的位姿，只是 IMU 给出的姿态参考。

imuPointerCur == 0 时，第一条 IMU 数据作为积分起点，累计旋转设为 0。后面每来一个 IMU，就用和上一个 IMU 的时间差乘角速度，得到这一小段时间内的旋转增量。

最后 --imuPointerCur 是因为循环中每写入一个积分点都会先 ++imuPointerCur，循环结束时指针多指到了下一个空位置，所以要回退到最后一个有效位置。

这个函数成功后，cloudInfo.imuAvailable = true。如果这个标志不是 true，后面 deskewPoint() 会直接返回原始点，不做去畸变。

odomDeskewInfo()：第 367～447 行作用：从 Odom 中取当前帧初始位姿，填入 cloudInfo；同时尝试计算一帧内的 Odom 增量。

void odomDeskewInfo()
{
    cloudInfo.odomAvailable = false;

    while (!odomQueue.empty())
    {
        if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
            odomQueue.pop_front();
        else
            break;
    }

    if (odomQueue.empty())
        return;

    if (odomQueue.front().header.stamp.toSec() > timeScanCur)
        return;

    nav_msgs::Odometry startOdomMsg;

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

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

    tf::Quaternion orientation;
    tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);

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

    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;

    if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
        return;

    nav_msgs::Odometry endOdomMsg;

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

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

    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
    );

    Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

    float rollIncre, pitchIncre, yawIncre;
    pcl::getTranslationAndEulerAngles(
        transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre
    );

    odomDeskewFlag = true;
}

详细解释：
这个函数最重要的输出是 cloudInfo.initialGuessX/Y/Z/Roll/Pitch/Yaw。这几个值会被后面的 mapOptimization 当成当前帧 LiDAR 匹配的初始猜测。

为什么后端需要初始猜测？因为 LiDAR scan-to-map 匹配通常是一个非线性优化问题，它不是凭空一次性算出正确位姿，而是从一个初始位姿开始不断迭代调整。如果初值接近真实位姿，优化容易收敛；如果初值偏得太远，可能匹配到错误位置。

这个函数先清理太旧的 Odom，然后检查队列里是否有覆盖当前点云起始时刻的 Odom。如果没有，就直接返回。之后它找到第一个时间大于等于 timeScanCur 的 Odom，把它作为当前扫描起始 Odom。

接着，代码把 Odom 里的四元数转换成 roll、pitch、yaw。四元数是姿态的数学表示，roll/pitch/yaw 是更直观的欧拉角。然后把位置和姿态都写进 cloudInfo.initialGuess。

后半部分尝试找扫描结束时刻的 Odom，也就是 endOdomMsg。如果 Odom 队列没有覆盖扫描结束时间，就不能计算完整一帧内的 Odom 增量，直接返回。

这里有一个比较特殊的判断：

if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
    return;

这个字段在这里不像传统 covariance 那样只表示协方差大小，而是被借用来判断起止 Odom 是否属于同一段连续有效的里程计。如果起始和结束编号不一致，说明中间可能发生了重置、跳变或状态变化，那么这段 Odom 增量不可靠，不能用于去畸变。

transBegin.inverse() * transEnd 的意思是计算从扫描开始到扫描结束的相对运动。比如扫描开始时车在 A 位姿，结束时车在 B 位姿，那么 transBt 就是 A 到 B 的变化量。然后通过 getTranslationAndEulerAngles() 提取出平移和旋转增量。

不过要注意，当前代码虽然计算了 odomIncreX/Y/Z，但 findPosition() 里真正使用它们的代码被注释了，所以默认情况下 Odom 平移去畸变没有启用。它主要还是给后端提供初始位姿。

findRotation()：第 449～474 行

作用：根据某个点的实际采样时间，从 IMU 积分数组中取出对应旋转量。

void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
    *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

    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;
    }
}

详细解释：
imuDeskewInfo() 得到的是一组离散的 IMU 积分结果，比如：

10.000s -> 旋转 0.00 rad
10.005s -> 旋转 0.01 rad
10.010s -> 旋转 0.02 rad

但 LiDAR 点的采样时间不一定刚好等于某个 IMU 时间。某个点可能是在 10.007s 采集的，它夹在 10.005s 和 10.010s 之间。所以 findRotation() 要做两件事：先找到点时间前后的两个 IMU 积分点，然后用线性插值估计该点时刻的旋转。

线性插值可以理解成：如果点时间更靠近后一个 IMU，就更多使用后一个 IMU 的旋转；如果更靠近前一个 IMU，就更多使用前一个 IMU 的旋转。

例如：

IMU A: 10.000s, yaw = 0.00
IMU B: 10.010s, yaw = 0.10
点时间: 10.005s

点在 A 和 B 正中间，所以 yaw 大约是 0.05

这个函数输出的是 rotXCur/rotYCur/rotZCur，它们会被 deskewPoint() 用来构造当前点采样时刻的姿态变换。

容易误解的一点是：这里的旋转量不是全局姿态，也不是最终 SLAM 位姿，而是当前 LiDAR 扫描时间段内相对起始时刻的旋转积分结果，主要用于修正一帧点云内部的运动畸变。

findPosition()：第 476～490 行

作用：理论上用于平移去畸变，但当前默认关闭。

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;
}

详细解释：
这个函数看起来像是要计算每个点采样时刻的平移量，但实际一进来就把 posXCur/Y/Z 全部设成 0。也就是说，默认情况下，这份代码不做平移去畸变，只做旋转去畸变。

为什么原作者会这样处理？因为对于很多手持、背包或低速车场景，一帧 LiDAR 的扫描时间很短，例如 0.1 秒。在这 0.1 秒内，如果平移距离很小，那么平移畸变相对旋转畸变影响更弱。尤其是旋转会让整面墙明显扭曲，而低速平移造成的误差可能没有那么明显。因此代码默认注释了平移补偿。

注释掉的逻辑其实很容易理解。如果启用，它会先判断 Odom 是否可用，然后计算当前点在整帧扫描中的时间比例：

ratio = 当前点相对时间 / 一帧扫描总时间

如果当前点在一帧中间采集，ratio 大约是 0.5；如果接近帧尾，ratio 接近 1。然后用这个比例乘以整帧 Odom 平移增量，得到当前点的平移估计。

但当前默认不用它，所以 deskewPoint() 里平移一直是 0。这意味着点云去畸变主要依赖 IMU 的旋转补偿。如果车速度很快、急加速、急刹车，或者 LiDAR 帧率较低，那么关闭平移去畸变可能会带来误差。

deskewPoint()：第 492～522 行

作用：单个点去畸变，把该点变换到当前帧起始时刻。

PointType deskewPoint(PointType *point, double relTime)
{
    if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
        return *point;

    double pointTime = timeScanCur + relTime;

    float rotXCur, rotYCur, rotZCur;
    findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);

    float posXCur, posYCur, posZCur;
    findPosition(relTime, &posXCur, &posYCur, &posZCur);

    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;
}

详细解释：
这个函数是点云去畸变的核心。LiDAR 一帧点云里的点不是同时采集的，而是在一段时间内陆续扫出来的。如果机器人在这一段时间内发生转动，那么这一帧点云会被扭曲。deskewPoint() 的目标就是把每个点都统一转换到当前帧起始时刻。

第一步检查能不能去畸变。如果点云没有 time/t 字段，或者 IMU 不可用，就没有办法知道这个点采样时刻对应的运动状态，只能返回原始点。

第二步计算点的绝对时间：

pointTime = 当前帧起始时间 + 当前点相对时间

然后通过 findRotation() 找到这个点采样时刻的 IMU 旋转量，通过 findPosition() 找到这个点采样时刻的平移量。当前平移默认是 0。

第三步处理当前帧第一个点。第一个点被认为是当前帧的时间参考。代码用它的位姿构造一个变换矩阵，并取逆，保存到 transStartInverse。后续所有点都用这个起始逆变换，把自己变换回帧起始坐标。

第四步构造当前点时刻的变换 transFinal，然后计算：

transBt = transStartInverse * transFinal

这个 transBt 表示从帧起始时刻到当前点采样时刻的相对运动。把原始点乘上这个变换，就得到统一到起始时刻后的点。

这里的矩阵乘法三行：

newPoint.x = ...
newPoint.y = ...
newPoint.z = ...

本质就是 3D 坐标变换：

newPoint = transBt * oldPoint

为什么要这么做？举个例子：当前帧开始时车朝正前方，扫到某个点时车已经右转了 5 度。这个点是在“右转 5 度后的雷达坐标系”里测到的。为了让整帧点云统一，代码要把它转换回“帧开始时的雷达坐标系”。这就是去畸变。

容易误解的一点是：这个函数不是把点变换到地图坐标系，也不是输出全局位姿。它只是把同一帧内部不同时间采集的点统一到这一帧的起始时刻，属于局部帧内修正。

projectPointCloud()：第 524～575 行

作用：遍历点云，过滤无效点，计算 row/column，去畸变，然后投影到 range image。

void projectPointCloud()
{
    int cloudSize = laserCloudIn->points.size();

    for (int i = 0; i < cloudSize; ++i)
    {
        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;

        int columnIdn = -1;
        if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
        {
            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            static float ang_res_x = 360.0/float(Horizon_SCAN);
            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;
        }
        else if (sensor == SensorType::LIVOX)
        {
            columnIdn = columnIdnCountVec[rowIdn];
            columnIdnCountVec[rowIdn] += 1;
        }
        
        if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
            continue;

        if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
            continue;

        thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

        rangeMat.at<float>(rowIdn, columnIdn) = range;

        int index = columnIdn + rowIdn * Horizon_SCAN;
        fullCloud->points[index] = thisPoint;
    }
}

详细解释：
这个函数完成从 3D 点云到 2D range image 的投影。原始点云是一堆 3D 点，而 LIO-SAM 后续特征提取更喜欢用类似图像的结构来处理，因为这样可以快速找到一个点在同一条扫描线上的前后邻居。

每个点先被复制成 PointType，然后计算距离 range。距离太近或太远都会被丢掉。太近的点可能是车体本身、雷达外壳、噪声；太远的点测量误差大，对匹配帮助有限。

然后取 ring 作为行号 rowIdn。如果 ring 不在 [0, N_SCAN) 范围内，说明点的线号不符合当前雷达配置，直接跳过。

downsampleRate 是按线束降采样。比如 downsampleRate=2，只有偶数 ring 会被保留，这能减少计算量，但也会损失一部分垂直方向信息。

列号 columnIdn 的计算分两种。对于 Velodyne/Ouster 这种旋转式雷达，可以通过水平角计算列号。代码用：

atan2(thisPoint.x, thisPoint.y)

算水平角，然后根据水平分辨率 ang_res_x = 360 / Horizon_SCAN 转成列索引。对于 Livox，因为扫描模式不是标准机械旋转，所以用计数器 columnIdnCountVec 来分配列号。

如果某个 (rowIdn, columnIdn) 格子已经有点了，代码会跳过后来的点。这表示一个 range image 格子只保留一个点。这样做可以避免同一个角度方向出现多个点导致结构混乱。

真正写入之前，会调用 deskewPoint() 对点去畸变。注意顺序是：先计算原始距离和投影格子，再对点坐标去畸变。距离 rangeMat 保存的是点到雷达的距离，用于后续遮挡、曲率判断；fullCloud 保存的是去畸变后的点坐标。

最后用：

index = columnIdn + rowIdn * Horizon_SCAN

把二维索引变成一维数组下标，把点存进 fullCloud。

这个函数输出两个核心结果：

rangeMat:
    保存每个格子的距离。

fullCloud:
    保存每个格子的去畸变点坐标。

cloudExtraction()：第 577～601 行

作用：从 range image 中取出有效点，生成紧凑点云，并记录辅助索引。

void cloudExtraction()
{
    int count = 0;

    for (int i = 0; i < N_SCAN; ++i)
    {
        cloudInfo.startRingIndex[i] = count - 1 + 5;

        for (int j = 0; j < Horizon_SCAN; ++j)
        {
            if (rangeMat.at<float>(i,j) != FLT_MAX)
            {
                cloudInfo.pointColInd[count] = j;
                cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
                extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                ++count;
            }
        }

        cloudInfo.endRingIndex[i] = count -1 - 5;
    }
}

详细解释：
projectPointCloud() 得到的 fullCloud 是固定大小的，它对应完整 range image。但很多格子是空的，因为不是每个角度方向都有有效点。如果后续特征提取直接遍历 fullCloud，会浪费大量时间，还要处理很多空位置。所以这里把有效点取出来，放入 extractedCloud。

遍历顺序是按扫描线 i，再按水平列 j。这样提取出来的 extractedCloud 仍然保持较好的扫描顺序。对于后续计算曲率很重要，因为曲率通常需要找当前点前后几个点，如果点云顺序乱了，邻居关系就会错。

pointColInd[count] = j 记录当前有效点在 range image 里的列号。后续遮挡判断会用列号判断两个点在扫描方向上是否相邻。

pointRange[count] 记录当前点距离。后续判断遮挡、深度突变、曲率时都会用距离。例如两个相邻列的点，如果距离突然差很多，可能说明有遮挡边界，这类点不一定适合当稳定特征。

startRingIndex 和 endRingIndex 记录每条 ring 在 extractedCloud 里的起止范围。这里不是简单地用 count，而是加减 5：

startRingIndex = count - 1 + 5
endRingIndex = count - 1 - 5

这是为了避开每条扫描线的边界点。因为后续特征提取计算曲率时，会使用当前点前后若干个邻居。如果点在扫描线开头或结尾，邻居不完整，容易越界或者算出不可靠曲率，所以直接跳过边界附近的几个点。

这个函数输出的核心结果是：

extractedCloud:
    紧凑的有效点云。

cloudInfo.startRingIndex / endRingIndex:
    每条扫描线在有效点云中的范围。

cloudInfo.pointColInd:
    每个有效点的水平列号。

cloudInfo.pointRange:
    每个有效点的距离。

publishClouds()：第 603～608 行

作用：发布当前帧预处理结果。

void publishClouds()
{
    cloudInfo.header = cloudHeader;  // 设置 cloudInfo 的时间戳和坐标系。
    cloudInfo.cloud_deskewed = publishCloud(
        pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame
    );  // 发布去畸变有效点云，并填入 cloudInfo.cloud_deskewed。
    pubLaserCloudInfo.publish(cloudInfo);  // 发布 cloud_info，给 featureExtraction 使用。
}

详细解释：
这个函数把前面所有处理结果打包发布出去。cloudInfo.header = cloudHeader 保证输出消息和输入点云使用同一个时间戳、同一个坐标系。

publishCloud() 会把 extractedCloud 转成 ROS 的 sensor_msgs::PointCloud2 并发布到 lio_sam/deskew/cloud_deskewed。同时，它返回的点云消息被存入 cloudInfo.cloud_deskewed。这样后续模块收到 cloudInfo 时，里面已经包含去畸变后的点云。

最后 pubLaserCloudInfo.publish(cloudInfo) 发布完整信息包。后面的 featureExtraction 不只是需要点云，还需要 startRingIndex/endRingIndex/pointColInd/pointRange/imuAvailable/odomAvailable/initialGuess 等信息。所以真正关键的输出不是单独的点云，而是 cloud_info。

可以理解为：

cloud_deskewed:
    单独发布的去畸变点云。

cloud_info:
    去畸变点云 + 所有辅助索引 + IMU/Odom 状态 + 初始位姿。

main()：第 611～623 行

作用：启动 ROS 节点，创建 ImageProjection 对象，并开启多线程回调。

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_sam");  // 初始化 ROS 节点，节点名为 lio_sam。

    ImageProjection IP;  // 创建 ImageProjection 对象，自动执行构造函数。
    
    ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");  // 打印启动成功信息。

    ros::MultiThreadedSpinner spinner(3);  // 创建 3 线程 spinner。
    spinner.spin();  // 进入 ROS 回调循环，持续接收 IMU/Odom/LiDAR。
    
    return 0;
}

详细解释：
main() 是程序入口。ros::init() 初始化 ROS 节点。ImageProjection IP; 创建对象后，构造函数会自动执行，于是 IMU、Odom、LiDAR 点云订阅器和发布器都会被建立起来。

ros::MultiThreadedSpinner spinner(3) 表示使用 3 个线程处理回调。这样 IMU、Odom、LiDAR 不一定要排队等一个线程。例如点云回调处理时间比较长时，IMU 回调仍然有机会被另一个线程及时处理。

但多线程也带来一个问题：多个线程可能同时访问同一个队列。所以代码前面才定义了 imuLock 和 odoLock。IMU/Odom 回调写队列时加锁，点云处理读取队列时也加锁，避免数据竞争。

启动后整体运行状态是：

IMU 来了
    ↓
imuHandler()
    ↓
进入 imuQueue

Odom 来了
    ↓
odometryHandler()
    ↓
进入 odomQueue

LiDAR 点云来了
    ↓
cloudHandler()
    ↓
触发整帧点云预处理

最终更细流程总结

程序启动 main()
    ↓
创建 ImageProjection
    ↓
构造函数订阅 IMU / Odom / LiDAR，创建发布器
    ↓
IMU 高频进入 imuQueue
    ↓
Odom 高频进入 odomQueue
    ↓
LiDAR 点云进入 cloudQueue
    ↓
cachePointCloud()
    检查点云格式，统一雷达点类型，确认 ring/time 是否存在
    ↓
deskewInfo()
    检查 IMU 是否覆盖当前 LiDAR 扫描时间段
    ↓
imuDeskewInfo()
    把 IMU 角速度积分成扫描期间的累计旋转
    ↓
odomDeskewInfo()
    取扫描起始 Odom 作为后端初始位姿
    ↓
projectPointCloud()
    遍历每个点，过滤距离，计算 row/column，调用 deskewPoint 去畸变
    ↓
deskewPoint()
    根据点时间查 IMU 旋转，把点统一变换到帧起始时刻
    ↓
rangeMat / fullCloud
    保存二维 range image 和对应点云
    ↓
cloudExtraction()
    提取有效点云，记录每条 ring 的范围、每个点列号和距离
    ↓
publishClouds()
    发布 cloud_deskewed 和 cloud_info
    ↓
featureExtraction
    使用 cloud_info 提取 corner/surface 特征

版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！

欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。