【LVI-SAM】激光雷达点云处理点云帧投影LIO-SAM 之ImageProjection实现细节

- 1. ImageProjection激光雷达点云预处理算法
- - 1.0 总结：
  - 1.1 功能概述：
  - 1.2 算法流程：
- 2. ImageProjection激光雷达点云预处理算法数学推倒
- 3. ImageProjection激光雷达点云预处理算法内部实现细节
- - 3.0 点云预处理实现流程
  - 3.1 缓存点云数据 cachePointCloud
  - 3.2 获取去畸变信息 deskewInfo
  - - 3.2.1 imuDeskewInfo
    - - 1. 实现原理：
      - 2. 数学推导：
    - 3.2.2 odomDeskewInfo
  - 3.3 点云投影 projectPointCloud: 将输入的激光点云投影到距离图像上
  - - 3.3.1. projectPointCloud算法流程
    - 3.3.2. projectPointCloud math formula derivation
    - 3.3.3. projectPointCloud代码实现
  - 3.4 提取点云 cloudExtraction: 激光雷达扫描数据集中提取有效的点云数据,更改到目标数据格式
  - 3.5 发布点云 publishClouds: 发布提取后的点云数据
  - 3.5 点的位置去畸变 deskewPoint
  - - 3.5.1 实现原理：
    - 3.5.2 数学推导：
    - 3.5.3 code achieve

1. ImageProjection激光雷达点云预处理算法

激光雷达点云预处理算法，主要用于对来自激光雷达的点云数据进行去畸变（Deskewing）、投影（Projection）、提取（Extraction）和发布。
目标是处理由不同类型的激光雷达传感器（如Velodyne、Livox、Ouster）采集的点云数据，并将其转换为后续处理和SLAM等任务所需的格式。

1.0 总结：

主要实现了激光雷达数据的去畸变、投影和提取的功能，并使用IMU和里程计数据来消除激光雷达点云在运动中的畸变误差。通过将三维点云投影到二维图像平面，便于后续的处理，并对投影后的点云进行提取和下采样，优化了点云的质量和计算效率。最终，这段代码为下游模块提供了处理后的点云数据，适用于激光雷达SLAM或地图构建的应用场景。

1.1 功能概述：

缓存和处理点云数据：缓存传入的点云消息，并根据不同的激光雷达传感器类型转换点云格式，提取时间戳和其他必要的信息。
IMU和里程计信息的去畸变：结合IMU和里程计数据，修正激光雷达点云中的动态畸变，确保点云在移动场景中的精确性。
投影和提取点云：将点云投影到二维图像平面中，并对点云进行下采样，提取有效的点云信息用于后续处理。
发布处理后的点云：最终发布处理后的点云，供其他算法模块使用。

1.2 算法流程：

缓存点云数据 cachePointCloud：
- 将接收到的点云数据存入缓存队列，并根据传感器类型（如Velodyne、Livox、Ouster）进行格式转换。
- 检查点云是否为密集格式、是否具有ring（激光雷达线束ID）和time（点云时间戳）等必要的字段。
获取去畸变信息 deskewInfo：
- 检查IMU和里程计（Odometry）数据队列，确保有足够的信息覆盖整个扫描时间段。
- 调用 imuDeskewInfo 和 odomDeskewInfo 处理IMU和里程计的数据。
IMU去畸变 imuDeskewInfo：
- 清理过早的IMU数据，并通过IMU的角速度信息，计算每个时间点的角度变化（Roll、Pitch、Yaw）。
- 计算每个时刻点的旋转矩阵，供后续的去畸变使用。
里程计去畸变 odomDeskewInfo：
- 从里程计数据队列中获取当前点云的起始和结束里程计信息，计算点云期间传感器的位移增量（平移和旋转）。
去畸变操作 deskewPoint：
- 对每一个点，通过IMU数据获取旋转信息，并根据里程计信息计算位置信息，修正点云坐标。
点云投影 projectPointCloud：
- 将去畸变后的点云投影到二维的图像坐标系中（即根据激光雷达线束ID ring 和水平角度计算图像坐标），生成范围图（range image）。
- 使用去畸变后的点云坐标更新全局点云矩阵。
点云提取 cloudExtraction：
- 遍历范围图，提取有效的点云数据，形成稀疏点云，并记录各个激光束（ring）的起始和结束点，用于后续的处理。
发布处理后的点云 publishClouds：
- 将提取后的点云数据发布出去，供其他模块（如SLAM、地图构建）使用。

2. ImageProjection激光雷达点云预处理算法数学推倒

3. ImageProjection激光雷达点云预处理算法内部实现细节

3.0 点云预处理实现流程

 void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        // 缓存点云数据
        if (!cachePointCloud(laserCloudMsg))   return;
        // 去畸变处理
        if (!deskewInfo()) return;
        // 投影点云
        projectPointCloud();
        // 提取点云
        cloudExtraction();
        // 发布点云
        publishClouds();
        // 重置参数
        resetParameters();
    }

3.1 缓存点云数据 cachePointCloud

cachePointCloud 函数负责缓存接收到的点云数据，确保数据格式正确，并为后续的处理步骤做准备。如数据不符合要求，输出错误信息并停止 ROS 节点。

这代码的函数 cachePointCloud 主要负责缓存和转换接收到的点云数据，具体功能如下：

缓存点云数据：将接收到的点云消息 laserCloudMsg 添加到 cloudQueue 队列中。
检查缓存大小：如果 cloudQueue 的大小小于或等于2，表示还没有足够的数据进行处理，函数返回 false。
转换点云数据：如果 sensor 类型是 VELODYNE 或 LIVOX，直接将 cloudQueue 队列前面的点云消息转换为 laserCloudIn 点云对象。如果是 OUSTER 类型，先转换为临时 tmpOusterCloudIn 对象，然后再转换为 laserCloudIn 点云对象。
获取时间戳：从 currentCloudMsg 中获取时间戳，并计算扫描的开始时间和结束时间。
检查点云格式：验证点云数据是否为密集格式，如果不是，输出错误并关闭 ROS 节点。
检查环（Ring）通道：确认点云数据中是否包含环（Ring）信息，如果没有，输出错误并关闭 ROS 节点。
检查时间戳：确认点云数据中是否包含时间戳信息，如果没有，输出警告并禁用去倾斜功能。
返回处理结果：如果所有检查都通过，函数返回 true，表示点云数据已准备好进行下一步处理。

3.2 获取去畸变信息 deskewInfo

    /*@brief 纠正信息
     * 通过加锁保证线程安全，对IMU和里程计数据进行纠正处理。
     * @return 如果成功纠正信息，则返回true；否则返回false。*/
    bool deskewInfo()
    {
        std::lock_guard<std::mutex> lock1(imuLock);
        std::lock_guard<std::mutex> lock2(odoLock);
        // make sure IMU data available for the scan
        if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
        {
            ROS_DEBUG("Waiting for IMU data ...");
            return false;
        }

        imuDeskewInfo();

        odomDeskewInfo();

        return true;
    }

3.2.1 imuDeskewInfo

imuDeskewInfo 的函数，其主要功能是处理IMU数据，以便于后续的点云去畸变处理。以下是该函数的输入、输出、全局变量和功能概述：

*  输入：
1. `imuQueue`：一个存储IMU数据的队列，其中每个元素包含一个`sensor_msgs::Imu`消息。
2. `timeScanCur`：当前扫描开始的时间戳。
3. `timeScanEnd`：当前扫描结束的时间戳。

*  输出：
函数本身没有直接的输出。但是，它会更新 `cloudInfo` 结构体中的一些成员变量，这些变量可以被视为输出：

1. cloudInfo.imuAvailable：一个布尔值，指示IMU数据是否可用于去畸变处理。
2. cloudInfo.imuRollInit、cloudInfo.imuPitchInit、cloudInfo.imuYawInit：分别存储初始的滚转角、俯仰角和偏航角。

*  全局变量：
1. `imuRotX`、`imuRotY`、`imuRotZ`：分别存储每个IMU数据点的滚转、俯仰和偏航角。
2. `imuTime`：存储每个IMU数据点的时间戳。
3. `imuPointerCur`：当前处理到的IMU数据点的索引。

*  函数功能：
1. **清理IMU队列**：移除队列中时间戳早于扫描开始时间0.01秒之前的IMU数据。
2. **初始化变量**：如果IMU队列不为空，初始化 `imuPointerCur` 为0，并开始处理队列中的IMU数据。
3. **处理IMU数据**：遍历IMU队列中的每个数据点，对于每个数据点：
   - 如果是第一个数据点，初始化旋转角度为0，并记录时间戳。
   - 计算角速度，并根据前一个数据点的时间戳和当前的角速度积分得到旋转角度。
   - 更新IMU数据点的时间戳和旋转角度。
4. 设置IMU可用性：如果处理了至少一个IMU数据点，设置 cloudInfo.imuAvailable为 true，表示IMU数据可用于去畸变处理。

总结来说，`imuDeskewInfo` 函数负责处理和整合IMU数据，为点云去畸变提供必要的旋转角度和时间信息。如果IMU数据不足以进行去
畸变处理，函数将不会更新 `cloudInfo.imuAvailable` 为 `true`。

这段代码的实现原理是对IMU数据进行处理，以便将LiDAR点云数据与IMU数据对齐，从而校正点云数据中由于传感器运动而产生的畸变。这个过程通常称为“去畸变”或“时间戳校正”。下面是详细的实现原理和数学推导过程：

1. 实现原理：

初始化和清理：
- 首先，设置cloudInfo.imuAvailable为false，表示IMU数据尚未准备好。
- 遍历imuQueue队列，移除那些时间戳早于扫描开始时间0.01秒之前的IMU数据点。
处理IMU数据：
- 如果imuQueue不为空，开始处理队列中的IMU数据。
- 初始化imuPointerCur为0，用于跟踪当前处理的IMU数据点。
积分角速度：
- 遍历imuQueue中的每个IMU数据点，对于每个数据点，获取其角速度（angular velocity）。
- 使用角速度和时间差（timeDiff）来积分计算旋转角度的变化量。
更新旋转角度：
- 对于每个IMU数据点，更新其在三个轴上的旋转角度（imuRotX, imuRotY, imuRotZ）。
标记IMU数据可用性：
- 如果至少处理了一个IMU数据点，设置cloudInfo.imuAvailable为true，表示IMU数据已经准备好用于去畸变处理。

2. 数学推导：

对于角速度的积分，我们假设在时间间隔 $t_i, t_{i+1}]$ 内，IMU的角速度是恒定的。角速度 $\omega$ 在三个轴上的分量分别是 $\omega_x$ , $\omega_y$ , $\omega_z$ 。在时间间隔 $\Delta t = t_{i+1} - t_i$ 内，每个轴上的旋转角度变化量 $\Delta \theta$ 可以通过以下公式计算：

$\Delta \theta_x = \omega_x \cdot \Delta t \\ \Delta \theta_y = \omega_y \cdot \Delta t \\\Delta \theta_z = \omega_z \cdot \Delta t$

其中， $\Delta t$ 是两个连续IMU数据点之间的时间差， $\omega_x$ , $\omega_y$ , $\omega_z$ 分别是在这个时间间隔内IMU数据点的角速度分量。

累积每个轴上的旋转角度，我们可以得到到当前IMU数据点为止的总旋转角度：

$\theta_x[i] = \theta_x[i-1] + \Delta \theta_x \\ \theta_y[i] = \theta_y[i-1] + \Delta \theta_y \\ \theta_z[i] = \theta_z[i-1] + \Delta \theta_z$

这里的 $\theta_x[i]$ , $\theta_y[i]$ , $\theta_z[i]$ 分别是在第i个IMU数据点时，X轴、Y轴和Z轴上的累积旋转角度。

代码实现：代码中的实现与上述数学推导相对应：

for (int i = 0; i < (int)imuQueue.size(); ++i) {
    sensor_msgs::Imu thisImuMsg = imuQueue[i];
    double currentImuTime = thisImuMsg.header.stamp.toSec();
    // ... 省略部分代码 ...
    // 获取角速度
    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;
}
// ... 省略部分代码 ...

在这段代码中，imuAngular2rosAngular函数用于从IMU消息中提取角速度，imuRotX、imuRotY、imuRotZ数组用于存储每个数据点的旋转角度，imuTime数组用于存储每个数据点的时间戳。通过遍历IMU数据并累加旋转角度的变化量，最终得到每个数据点的旋转角度。

去畸变（Deskew）：
LiDAR点云数据通常是随时间连续扫描得到的，如果在扫描过程中传感器（如LiDAR）发生了旋转或移动，那么点云数据会出现畸变。
通过IMU提供的角速度数据，可以计算出在每个LiDAR点的时间戳对应的IMU姿态。这样，可以对每个点云数据进行逆向旋转变换，以补偿这种运动畸变。
时间戳校正：
LiDAR点云数据中的每个点都有一个时间戳，表示该点被扫描时的时间。但是，由于LiDAR和IMU的采样率可能不同，这些时间戳可能不会完美对齐。
通过计算 imuRotX、imuRotY 和 imuRotZ，我们可以插值找到每个LiDAR点对应的IMU姿态，从而校正点云的时间戳。
传感器融合：
在SLAM或其他导航算法中，通常需要融合多种传感器数据来提高定位和建图的准确性。
通过计算IMU在每个LiDAR点时间戳上的姿态，可以将IMU的测量结果（如速度、加速度）与LiDAR的测量结果（如距离、角度）结合起来，以获得更准确和鲁棒的状态估计。

3.2.2 odomDeskewInfo

总结来说，odomDeskewInfo 函数负责处理和整合里程计数据，为点云去畸变提供必要的位置和姿态变化信息。如果里程计数据不足以进行去畸变处理，函数将不会更新cloudInfo.odomAvailable为true。

odomDeskewInfo 函数，其主要功能是处理里程计（Odometry）数据，以便于后续的点云去畸变处理。以下是该函数的输入、输出、全局变量和功能概述：

输入：

odomQueue：一个存储里程计数据的队列，其中每个元素包含一个nav_msgs::Odometry消息。
timeScanCur：当前扫描开始的时间戳。
timeScanEnd：当前扫描结束的时间戳。

输出：
函数本身没有直接的输出。但是，它会更新 cloudInfo 结构体中的一些成员变量，这些变量可以被视为输出：

cloudInfo.odomAvailable：一个布尔值，指示里程计数据是否可用于去畸变处理。
cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ：分别存储初始猜测的位置坐标。
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw：分别存储初始猜测的滚转角、俯仰角偏航角。
odomDeskewFlag：一个布尔值，指示是否成功计算了扫描期间的里程计变化。

全局变量：

odomDeskewFlag：用于标记是否成功计算了里程计的去畸变信息。
odomIncreX, odomIncreY, odomIncreZ：分别存储扫描期间里程计位置的增量变化。
odomIncreRoll, odomIncrePitch, odomIncreYaw：分别存储扫描期间里程计姿态的增量变化（如果代码中有这些变量的话）。

函数功能：

清理里程计队列：移除队列中所有时间戳早于扫描开始时间0.01秒之前的里程计数据。
检查队列状态：如清理后odomQueue为空，或队列中的第一个数据点的时间戳晚于扫描开始时间，不进行处理。
获取开始里程计：找到与扫描开始时间最接近的里程计数据点，提取其位置和姿态信息。
设置初始猜测：将提取的位置和姿态信息存储在cloudInfo结构体中，用于后续的去畸变处理。
标记里程计数据可用性：如果成功获取了开始里程计数据，设置cloudInfo.odomAvailable为true。
获取结束里程计：如扫描结束时间在队列中的某个里程计数据点之后，找到与扫描结束时间最接近的里程计数据点。
检查协方差：如果开始和结束里程计数据点的协方差不同，可能表示有异常，因此不进行处理。
计算里程计变化：计算扫描期间里程计位置和姿态的增量变化，并存储在相应的全局变量中。
标记里程计去畸变标志：如果成功计算了里程计变化，设置odomDeskewFlag为true。

3.3 点云投影 projectPointCloud: 将输入的激光点云投影到距离图像上

这个 projectPointCloud 函数是用于将LiDAR点云数据投影到二维范围图像上的过程。以下是该函数的输入、输出和更新的> 变量：

输入：

laserCloudIn：一个包含原始LiDAR点云数据的pcl::PointCloud<PointXYZIRT>::Ptr智能指针，其中包含了点的三维坐标、强度等信息。
lidarMinRange 和 lidarMaxRange：LiDAR数据中有效范围的最小值和最大值(1m-1000m)。
N_SCAN：LiDAR扫描通道的数量(16,32)。
Horizon_SCAN：LiDAR水平分辨率(1800 and so on)。
downsampleRate：下采样率，用于减少处理的数据量(1)。
sensor：当前使用的LiDAR传感器类型。
columnIdnCountVec：一个向量，用于记录每个扫描通道的列索引计数，特别是对于Livox传感器。

输出：
没有直接的输出，它会更新几个变量，这些变量为：

rangeMat：一个二维矩阵，用于存储范围图像，其中每个元素对应一个特定的角度和扫描行的位置。如果该位置的点云数据有效，则存储该点的距离，否则为FLT_MAX。
fullCloud：一个pcl::PointCloud<PointType>::Ptr智能指针，用于存储投影后的点云数据。

更新的变量：

rangeMat：在函数内部，根据点云数据更新范围图像矩阵，有效点的距离会被记录。
fullCloud：根据点云数据更新全云数据，将投影后的点添加到相应的位置。
columnIdnCountVec：对于Livox传感器，这个向量会在每次投影一个点后更新，以确保每个扫描通道的列索引是正确的。

函数逻辑概述：

遍历 laserCloudIn 中的每个点。
计算每个点的距离，并检查它是否在LiDAR的有效范围内。
根据点的环（ring）值确定其在范围图像中的行索引。
根据LiDAR传感器类型计算列索引。
如果该点在范围图像中的位置上还没有其他点（即rangeMat中该位置为FLT_MAX），则将该点投影到范围图像上，并更新fullCloud。
对于Velodyne或Ouster类型的LiDAR，计算水平角度并将其转换为列索引。
-> 对于Livox类型的LiDAR，直接使用columnIdnCountVec中的值作为列索引，并在每次使用后递增计数。

这个函数是LiDAR数据处理流程中的关键步骤，它将原始的三维点云数据转换为二维的范围图像，这对于后续的SLAM和点云处理算法非常重要。

这段代码是在处理LiDAR点云数据时，将点云中的每个点分配到对应的水平角度桶（column index）中。在**进行点云投影到二维范围图像（range image）**Mat size is (16*1800)<==>(N_SCAN, Horizon_SCAN)的过程中的一个步骤。以下是这段代码的原理：

N_SCAN: 16   # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
Horizon_SCAN: 1800# lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)

3.3.1. projectPointCloud算法流程

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

计算水平角度：
- 使用 atan2 函数计算点 (thisPoint.x, thisPoint.y) 在XY平面上的角度 horizonAngle。atan2 函数返回的角度是弧度制，因此需要将其转换为角度。
角度分辨率：
- ang_res_x 是水平角度分辨率，即360度除以 Horizon_SCAN（LiDAR的水平分辨率）。这个值表示每个水平角度桶所代表的角度范围。
计算列索引：
- 将计算出的角度 horizonAngle 减去90度，是因为LiDAR数据中0度通常是指向前方的，而这里将-90度（即向左的最大角度）作为起始点。
- 使用 round 函数将调整后的角度按照角度分辨率进行四舍五入，得到该点在水平方向上的索引 columnIdn。
- 由于索引是从0开始的，而LiDAR的0度通常对应于数组的中间位置（即 Horizon_SCAN / 2），因此需要将计算出的索引加上 Horizon_SCAN / 2 进行偏移。
处理索引范围：
- 如果计算出的索引 columnIdn 大于或等于 Horizon_SCAN，说明它超出了数组的范围。由于索引是循环的，所以需要从 Horizon_SCAN 中减去 columnIdn，使其回到正确的范围内。

这段代码的目的是为了将每个点云点根据其在水平面上的角度位置分配到一个特定的列索引，这是将点云数据转换为二维范围图像的必要步骤。在范围图像中，每一行代表一个固定的距离，每一列代表一个角度，这样可以将三维空间中的点云数据转换为二维图像数据，便于后续的图像处理和SLAM算法。

3.3.2. projectPointCloud math formula derivation

在这里插入图片描述
通过上述步骤，可以将每个点云点映射到一个二维范围图像的列索引上，其中每一列代表一个特定的角度范围。

3.3.3. projectPointCloud代码实现

void projectPointCloud()
    {
        int cloudSize = laserCloudIn->points.size();
        // range image projection
        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);
            int rowIdn = laserCloudIn->points[i].ring;
           	/// ``````
            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;
            }
  			/// ``````
            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

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

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

3.4 提取点云 cloudExtraction: 激光雷达扫描数据集中提取有效的点云数据,更改到目标数据格式

这段代码的主要是从一个激光雷达（Lidar）的扫描数据中提取出有效的点云数据，并将这些点云数据保存到一个新的容器中。同时，它还记录了一些关于这些点云数据的附加信息，如每个扫描环（ring）中有效点云的起始和结束索引，以及每个点在其原始扫描中的列索引（可能是为了后续处理，如遮挡检测）。更新extractedCloud：
在这里插入图片描述

    /*** @brief 提取云数据* 从给定的激光雷达数据中提取分割的云数据，用于激光雷达里程计*/
    void cloudExtraction()
    {
        int count = 0;
        // extract segmented cloud for lidar odometry
        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)
                {
                    // mark the points' column index for marking occlusion later
                    cloudInfo.pointColInd[count] = j;
                    // save range info
                    cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
                    // save extracted cloud
                    extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of extracted cloud
                    ++count;
                }
            }
            cloudInfo.endRingIndex[i] = count -1 - 5;
        }
    }

3.5 发布点云 publishClouds: 发布提取后的点云数据

@brief 发布点云数据
- 将提取的点云数据发布到ROS话题中。
- @note 此函数会设置点云信息的头信息，并发布经过处理后的点云数据。

    void publishClouds()
    {
        // 设置cloudInfo的头信息为cloudHeader
        cloudInfo.header = cloudHeader;
        // 发布去偏斜的点云数据，并将结果赋值给cloudInfo的cloud_deskewed成员变量
        cloudInfo.cloud_deskewed  = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
        // 发布cloudInfo到pubLaserCloudInfo话题上
        pubLaserCloudInfo.publish(cloudInfo);
    }

3.5 点的位置去畸变 deskewPoint

代码定义了一个名为 deskewPoint 的函数，其目的是对单个LiDAR点云数据点进行去畸变处理，以补偿由于传感器移动导致的点云位置偏差。这个过程涉及到计算点在空间中的新位置，使其反映在传感器静止时的位置。以下是详细的实现原理和数学推导：

3.5.1 实现原理：

检查去畸变标志：
- 如果去畸变功能未启用（deskewFlag 为 -1）或者IMU数据不可用（cloudInfo.imuAvailable 为 false），则返回原始点。
计算点的实际时间：
- 点的实际时间 pointTime 是当前扫描开始时间 timeScanCur 加上相对时间 relTime。
获取旋转角度和平移距离：
- 使用 findRotation 函数获取点的时间对应的旋转角度（rotXCur, rotYCur, rotZCur）。
- 使用 findPosition 函数获取点的时间对应的平移距离（posXCur, posYCur, posZCur）。
计算逆变换矩阵：
- 如果是处理的第一个点，计算从当前位置到起始位置的逆变换矩阵 transStartInverse。
应用变换：
- 计算最终的变换矩阵 transFinal，它包含了当前点的时间对应的旋转和平移。
- 计算变换矩阵 transBt，它是逆变换矩阵 transStartInverse 和最终变换矩阵 transFinal 的乘积。
- 应用变换矩阵 transBt 到原始点坐标上，计算出新点的坐标。
返回新点：
- 创建一个新的点类型 PointType，设置其坐标为变换后的坐标，并保留原始点的强度值。

3.5.2 数学推导：

假设传感器在时间 t 的位置为 (p_x, p_y, p_z)，姿态（旋转）为 (R_x, R_y, R_z)。对于一个在时间 t 被扫描到的点 P(x, y, z)，其在传感器静止参考系中的位置 P'(x', y', z') 可以通过以下变换得到：

旋转：
- 首先，将点 P 绕传感器坐标系原点旋转 -R 度（逆旋转），得到点 P'。
平移：
- 然后，将点 P' 沿着传感器移动的相反方向平移 -p（逆平移），得到点 P''。
变换矩阵：
- 旋转和平移可以表示为一个仿射变换矩阵 T，其中包含了旋转矩阵 R 和平移向量 p。
逆变换矩阵：
- 逆变换矩阵 T_inv 是 T 的逆矩阵，它包含了逆旋转矩阵 R_inv 和逆平移向量 -p。
点的变换：
- 点 P 到点 P'' 的变换可以表示为：
  [ P’’ = T_{inv} \cdot P ]
- 其中，T_inv 是由 transStartInverse 和 transFinal 计算得到的。

在代码中，这个过程通过以下方式实现：

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 就代表了去畸变后的点云数据点，它的位置更准确地反映了在传感器静止时的空间位置。

3.5.3 code achieve

    /**
     * @brief 校正点的位置
     * 根据给定的相对时间和当前点的信息，对点进行位置校正。
     * @param point 点指针
     * @param relTime 相对时间
     * @return 校正后的点
     */
    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;
        }

        // 将点变换到起始位置
        // transform points to start
        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;
    }