PCL 点云变换函数解析：transformPointCloud 函数详解

该程序是一个函数，用于将输入的点云按照给定的姿态变换矩阵进行变换，并返回变换后的点云。具体步骤如下：

创建一个新的空点云'cloudOut'。
获取输入点云的size，并将'cloudOut'的size设置为与之相同。
获取输入的姿态变换矩阵'transformIn'，将其转换为Eigen::Affine3f类型的变量'T_w_b_'。
获取当前点云相对于世界坐标系的变换矩阵'T_w_b'，即将'T_w_b_'转换为Eigen::Isometry3d类型变量'T_w_b'，并将其乘以相对于车身坐标系的变换矩阵'T_b_lidar'，得到当前点云相对于激光雷达坐标系的变换矩阵'T_w_lidar'。
并行地遍历输入点云中的每一个点，将其按照'T_w_lidar'变换矩阵进行变换，得到变换后的点，并将其存储到'cloudOut'中。
返回变换后的点云'cloudOut'。


pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn)
{
    pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

    int cloudSize = cloudIn->size();
    cloudOut->resize(cloudSize);

    // 注意：lio_sam 中的姿态用的euler表示，而fastlio存的姿态角是旋转矢量。而 pcl::getTransformation是将euler_angle 转换到rotation_matrix 不合适，注释
    // Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
    Eigen::Isometry3d T_b_lidar(state_point.offset_R_L_I); //  获取  body2lidar  外参
    T_b_lidar.pretranslate(state_point.offset_T_L_I);

    Eigen::Affine3f T_w_b_ = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
    Eigen::Isometry3d T_w_b; //   world2body
    T_w_b.matrix() = T_w_b_.matrix().cast<double>();

    Eigen::Isometry3d T_w_lidar = T_w_b * T_b_lidar; //  T_w_lidar  转换矩阵

    Eigen::Isometry3d transCur = T_w_lidar;

#pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < cloudSize; ++i)
    {
        const auto &pointFrom = cloudIn->points[i];
        cloudOut->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
        cloudOut->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
        cloudOut->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
        cloudOut->points[i].intensity = pointFrom.intensity;
    }
    return cloudOut;
}

代码解析：

cloudIn：输入的点云。
transformIn：姿态变换矩阵，包含平移和旋转信息，一般是一个结构体或类。
cloudOut：变换后的点云。
T_b_lidar：车身坐标系到激光雷达坐标系的变换矩阵。
T_w_b_：世界坐标系到车身坐标系的变换矩阵，使用pcl::getTransformation函数将欧拉角转换为旋转矩阵和平移向量，并构造Affine3f类型的矩阵。
T_w_b：将T_w_b_转换为Isometry3d类型矩阵，用于矩阵运算。
T_w_lidar：世界坐标系到激光雷达坐标系的变换矩阵，通过T_w_b * T_b_lidar计算得到。
transCur：保存当前点云相对于激光雷达坐标系的变换矩阵。
pointFrom：输入点云中的一个点。
cloudOut->points[i]：变换后的点云中的一个点。

关键变量说明：

state_point.offset_R_L_I：车身坐标系到激光雷达坐标系的旋转矩阵。
state_point.offset_T_L_I：车身坐标系到激光雷达坐标系的平移向量。
transformIn->x、transformIn->y、transformIn->z：姿态变换矩阵的平移向量。
transformIn->roll、transformIn->pitch、transformIn->yaw：姿态变换矩阵的欧拉角。
numberOfCores：CPU核心数，用于控制并行计算的线程数。

注意：

该程序使用了Eigen库进行矩阵运算，需要在编译时链接Eigen库。
该程序使用了OpenMP库进行并行计算，需要在编译时链接OpenMP库。
该程序的姿态变换矩阵使用了欧拉角表示，可以根据实际情况进行修改。