LIO-SAM代码逐行解读（2）-点云预处理

本篇博客中，我们主要讨论接收点云后投影到一个平面上，为后续的特征提取做准备的相关内容，称之为“点云预处理”部分。此部分主要使用imu数据，进行激光雷达数据的运动畸变去除与点云投影。

准备工作

引用头文件

// 引用自定义的函数
#include "utility.h"
// 自定义的消息类型
#include "lio_sam/cloud_info.h"

PCL中自定义点云数据结构类型

// 定义Velodyne数据类型  包含 xyz,i强度，r线号，t时间戳
struct VelodynePointXYZIRT
{
    // PCL中的宏定义，包含x、y、z 还有一个对齐变量
    PCL_ADD_POINT4D; // 添加xyz
    PCL_ADD_INTENSITY; // 添加强度
    uint16_t ring; // 添加线号
    float time; // 添加每个点的时间
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
 // 注册点类型宏 XYZI + "ring" + "time"
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)
)

// 定义Ouster数据类型  包含 xyz,intensity强度，t时间戳，reflectivity反射率，ring线号，noise噪声，range距离
struct OusterPointXYZIRT {
    PCL_ADD_POINT4D;
    float intensity;
    uint32_t t;
    uint16_t reflectivity;
    uint8_t ring;
    uint16_t noise;
    uint32_t range;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
    (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
)

ImageProjection 类，定义各种变量

// Use the Velodyne point format as a common representation
// 默认使用Velodyne点类型
using PointXYZIRT = VelodynePointXYZIRT;

const int queueLength = 2000;

class ImageProjection : public ParamServer
{
private:

    std::mutex imuLock;
    std::mutex odoLock;

    ros::Subscriber subLaserCloud;
    ros::Publisher  pubLaserCloud;
    
    ros::Publisher pubExtractedCloud;
    ros::Publisher pubLaserCloudInfo;

    ros::Subscriber subImu;
    std::deque<sensor_msgs::Imu> imuQueue;

    ros::Subscriber subOdom;
    std::deque<nav_msgs::Odometry> odomQueue;

    std::deque<sensor_msgs::PointCloud2> cloudQueue;
    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];

    // 用于去除运动畸变的IMU数量
    int imuPointerCur;
    // 是否为第一个激光点
    bool firstPointFlag;
    // 第一个激光点对应的IMU姿态变换
    Eigen::Affine3f transStartInverse;

    // 接收Velodyne格式的点云数据
    pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
    // Ouster格式的点云数据
    pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
    pcl::PointCloud<PointType>::Ptr   fullCloud;
    pcl::PointCloud<PointType>::Ptr   extractedCloud;

    // 是否进行了去畸变处理
    int deskewFlag;
    cv::Mat rangeMat;

    // IMU里程计数据去畸变标志位
    bool odomDeskewFlag;
    // 激光点云起始与结束时刻之间的相对平移量
    float odomIncreX;
    float odomIncreY;
    float odomIncreZ;

    // 点云信息
    lio_sam::cloud_info cloudInfo;
    double timeScanCur;
    double timeScanEnd;
    std_msgs::Header cloudHeader;

ImageProjection 类，构造函数

public:
    // 构造函数，设置deskewFlag的值为0
    ImageProjection():deskewFlag(0)
    {
        // 订阅imu数据，后端里程记数据，原始点云数据
        // "/imu/data"   "odometry/imu_incremental"  "点云原始数据topic"
        subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
        subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
        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::console::setVerbosityLevel(pcl::console::L_ERROR);
    }

分配内存空间，重设参数

/**
     * @brief 各变量分配空间
     * 对各种指针实例化
     * 赋予变量大小
     */
    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>());
        
        // N_SCAN; 扫描线数，例如16、64
        // Horizon_SCAN; 扫描一周计数，例如每隔0.2°扫描一次，一周360°可以扫描1800次
        // 点云的数量重新设置为  线数*水平点数
        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();
    }

    /**
     * @brief 重设参数
     * 清除变量，定义一个距离图像
     * 重设标志位，以及IMU插值的变量
     */
    void resetParameters()
    {
        laserCloudIn->clear();
        extractedCloud->clear();
        // reset range matrix for range image projection 
        // 设置每一帧点云对应的距离图像大小为N_SCAN与Horizon_SCAN
        rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
        
        // 当前可以用于去除畸变处理的IMU数据个数
        imuPointerCur = 0;
        // 是否为第一个点
        firstPointFlag = true;
        // 是否可以用odom来进行点云畸变补偿
        odomDeskewFlag = false;
        // 重设为0
        for (int i = 0; i < queueLength; ++i)
        {
            // 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时间戳;
            // 用于插值计算当前激光帧起止时间范围内，每一时刻的旋转姿态
            imuTime[i] = 0;
            imuRotX[i] = 0;
            imuRotY[i] = 0;
            imuRotZ[i] = 0;
        }
    }

    ~ImageProjection(){}

处理IMU数据

接收IMU原始数据，里程计数据

/**
     * @brief 接收IMU数据"imu/data"
     * 由于ShanTixiao使用的IMU磁力计与加速度计、陀螺仪的坐标系不统一，所以先做一个坐标变换
     * 接着存放到队列imuQueue中
     * @param imuMsg 
     */
    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);   // 对imu做一个坐标转换
        // 加一个线程锁，把imu数据保存进队列
        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;
    }

    /**
     * @brief 接收预积分节点的数据（"odometry/imu_incremental"消息名称）
     * 存放到队列odomQueue中
     * @param odometryMsg 
     */
    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
    {
        std::lock_guard<std::mutex> lock2(odoLock);
        odomQueue.push_back(*odometryMsg);
    }

处理点云数据

接收激光点云数据并处理

 /**
     * @brief 接收点云数据，并处理（主要的处理在这里）
     * 1、缓存点云，检测点云消息中的相关属性是否齐全
     * 2、准备去除点云运动畸变所需要的IMU原始数据（旋转），IMU里程计数据（平移）
     * 3、投影点云到一个”距离平面“，计算行号列号，去除点云运动畸变，
     * 4、提取点，记录各种属性（如点云的起始索引与终止索引，列号），整理点云信息集合
     * 5、发布lio_sam/deskew/cloud_deskewed与lio_sam/deskew/cloud_info消息
     * @param laserCloudMsg 
     */
    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {   
        // 判断是否缓存够了足够多的点云  
        // 检测点云消息中的数据是否有序排列，是否有扫描线号，扫描点的时间等属性
        if (!cachePointCloud(laserCloudMsg))
            return;
        
        // 点云去畸变信息采集
        if (!deskewInfo())
            return;
        
        // 投影点到rangeMat变量中，并将点存放在fullcloud变量中
        projectPointCloud();

        cloudExtraction();

        publishClouds();

        resetParameters();
    }

cachePointCloud函数，缓存点云，判断点云数据中的属性是否合格

/**
     * @brief 缓存点云
     * 1、缓存laserCloudMsg消息数据到cloudQueue队列中
     * 2、保证队列中至少有两帧点云数据，读取其中时间最早的点云
     * 3、确保点云中没有无效点，点云信息中包含线号、每个点的时间戳。
     * @param laserCloudMsg 
     * @return true 
     * @return false 
     */
    bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        // cache point cloud
        // 点云数据保存进队列
        cloudQueue.push_back(*laserCloudMsg);
        // 确保队列里大于两帧点云数据
        if (cloudQueue.size() <= 2)
            return false;

        // 缓存了足够多的点云之后，取出队列中时间最早的一帧数据
        // convert cloud
        currentCloudMsg = std::move(cloudQueue.front());
        cloudQueue.pop_front();
        if (sensor == SensorType::VELODYNE)
        {
            // fromROSMsg： 标准版本执行对数据的深复制。
            // moveFromROSMsg： 而move 版本执行浅复制并注销源数据容器。这称为“移动语义”
            pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);    // 转成pcl的点云格式
        }
        else if (sensor == SensorType::OUSTER)
        {
            // Convert to Velodyne format
            // ouster转换为velodyne点云格式
            // fromROSMsg： 标准版本执行对数据的深复制。
            // moveFromROSMsg： 而move 版本执行浅复制并注销源数据容器。这称为“移动语义”
            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();
        }

        // get timestamp  起始时间是文件头的时间戳，终止时间是最后一个点的时间
        // timeScanCur：当前扫描的起始时间  timeScanEnd：当前扫描的终止时间
        cloudHeader = currentCloudMsg.header;
        timeScanCur = cloudHeader.stamp.toSec();
        timeScanEnd = timeScanCur + laserCloudIn->points.back().time;

        // check dense flag
        // is_dense是点云是否有序排列的标志
        if (laserCloudIn->is_dense == false)
        {
            ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
            ros::shutdown();
        }

        // check ring channel
        // 查看驱动里是否把每个点属于哪一根扫描scan这个信息
        static int ringFlag = 0;
        if (ringFlag == 0)
        {
            ringFlag = -1;
            // 查看点云消息中是否有ring这个属性
            for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
            {
                if (currentCloudMsg.fields[i].name == "ring")
                {
                    ringFlag = 1;
                    break;
                }
            }
            // 如果没有这个信息就需要像loam或者lego loam那样手动计算scan id，现在velodyne的驱动里都会携带这些信息的
            if (ringFlag == -1)
            {
                ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
                ros::shutdown();
            }
        }

        // check point time
        // 同样，检查是否有时间戳信息
        if (deskewFlag == 0)
        {
            deskewFlag = -1;
            for (auto &field : currentCloudMsg.fields)
            {      
                // 检查点云消息中是否有time或者t这个属性
                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;
    }

收集去除点云畸变所需信息
包括IMU数据中的陀螺仪测量值（三个轴的旋转数据），以及IMU里程计数据（用于平移，实际并没有使用）

/**
     * @brief 获取点云运动补偿所需的信息
     * 1、确保IMU队列中的数据能够覆盖当前帧点云数据时间
     * 2、准备IMU原始数据（"/imu/data"）去畸变的信息（角度）
     * 3、准备IMU里程计数据（"odometry/imu_incremental"）去畸变信息（平移量，实际中未使用）
     * @return true 
     * @return 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
        // 确保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补偿的信息("/imu/data"数据)
        imuDeskewInfo();
        // 准备IMU里程计畸变补偿信息("odometry/imu_incremental"数据)
        odomDeskewInfo();

        return true;
    }

    /**
     * @brief imu去除畸变信息整理
     * 1、去除IMU队列中过早的数据
     * 2、遍历剩余的IMU数据，取出最接近激光点云起始时刻的磁力计测量角度作为初始姿态
     * 3、累计 IMU角速度×时间，作为每一时刻的角度，去除点云运动畸变。
     */
    void imuDeskewInfo()
    {
        cloudInfo.imuAvailable = false;
        // 当队列中非空，弹出过早的IMU数据
        while (!imuQueue.empty())
        {
            if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01) // 把过早的imu扔掉
                imuQueue.pop_front();
            else
                break;
        }

        if (imuQueue.empty())
            return;

        imuPointerCur = 0;

        // 遍历所有的imu数据
        for (int i = 0; i < (int)imuQueue.size(); ++i)
        {
            sensor_msgs::Imu thisImuMsg = imuQueue[i];
            double currentImuTime = thisImuMsg.header.stamp.toSec();

            // get roll, pitch, and yaw estimation for this scan
            // 从最靠近当前激光点云起始时间的IMU数据中获取起始的朝向
            if (currentImuTime <= timeScanCur)
                // 把imu磁力计获取的姿态数据转成欧拉角
                imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

            // IMU时间超出最后一个点时间0.01秒
            if (currentImuTime > timeScanEnd + 0.01)    // 这一帧遍历完了就break
                break;
            
            // 用于去除运动畸变的IMU数量为0
            if (imuPointerCur == 0){    // 起始帧
                imuRotX[0] = 0;
                imuRotY[0] = 0;
                imuRotZ[0] = 0;
                imuTime[0] = currentImuTime;
                ++imuPointerCur;
                continue;
            }

            // get angular velocity
            double angular_x, angular_y, angular_z;
            // 取出当前帧的角速度
            imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

            // integrate rotation
            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;
        
        // 可以使用imu数据进行运动补偿，标志为true
        cloudInfo.imuAvailable = true;
    }

    /**
     * @brief IMU里程计去除畸变信息
     * 1、去除过早的IMU里程计数据，并确保IMU里程计数据能够覆盖一帧激光点云
     * 2、找到激光点云最早时刻的对应IMU里程计位姿，作为cloudInfo中的initialGuess数据。之后转换成Affine3格式。
     * 3、找到激光点云最晚时刻的对应IMU里程计位姿，转换为Affine3格式。
     * 4、计算起始与终止时刻之间的差值，找到两个时刻之间的相对平移量odomIncreX/Y/Z。
     */
    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;
        
        // 点云时间   ×××××××
        // odom时间     ×××××
        // 显然不能覆盖整个点云的时间
        if (odomQueue.front().header.stamp.toSec() > timeScanCur)
            return;

        // get start odometry at the beinning of the scan
        nav_msgs::Odometry startOdomMsg;
        // 遍历里程计队列，找到对应的最早的点云时间的odom数据，即时间差异最小
        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            startOdomMsg = odomQueue[i];

            if (ROS_TIME(&startOdomMsg) < timeScanCur)
                continue;
            else
                break;
        }
        
        // 将ros消息格式中的姿态转成tf的格式
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
        // 然后将四元数转成欧拉角
        double roll, pitch, yaw;
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        
        // 记录点云起始时刻的对应的odom姿态，作为位姿的初值
        // Initial guess used in mapOptimization
        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; // odom提供了这一帧点云的初始位姿

        // get end odometry at the end of the scan
        odomDeskewFlag = false;
        // 这里发现没有覆盖到最后的点云，那就不能用odom数据来做运动补偿
        if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
            return;

        nav_msgs::Odometry endOdomMsg;
        // 找到点云最晚时间对应的odom数据（距离当前帧点云结束时间最近的里程计数据）
        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            endOdomMsg = odomQueue[i];

            if (ROS_TIME(&endOdomMsg) < timeScanEnd)
                continue;
            else
                break;
        }
        
        // 这个代表odom退化了，就置信度不高了
        if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
            return;

        // 起始位姿和结束位姿都转成Affine3f这个数据结构
        // 激光点云起始时刻的位姿
        Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
        
        // nav_msgs::Odometry转换为tf
        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);
        
        // 计算起始位姿和结束位姿之间的delta pose
        Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
        // 将这个增量转成xyz和欧拉角的形式
        float rollIncre, pitchIncre, yawIncre;
        pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

        odomDeskewFlag = true;  // 表示可以用odom来做运动补偿
    }

找到当前点对应的旋转与平移量，并去除点云畸变

/**
     * @brief 找到当前激光点对应的旋转
     * 1、找到当前激光点对应的IMU数据（前后各一个）
     * 2、插值，得到当前激光点时刻对应的旋转变换
     * @param pointTime 
     * @param rotXCur 
     * @param rotYCur 
     * @param rotZCur 
     */
    void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
    {
        *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

        int imuPointerFront = 0;
        // imuPointerCur是imu计算的旋转buffer的总共大小，这里用的就是一种朴素的确保不越界的方法
        while (imuPointerFront < imuPointerCur)
        {   
            // 寻找imuPointerFront，使得其对应的imu时间大于当前点
            if (pointTime < imuTime[imuPointerFront])
                break;
            ++imuPointerFront;
        }

        // imuPointerBack     imuPointerFront
        //    ×                      ×
        //               ×
        //           pointTime
        
        // 如果当前激光点的时间戳不在两个imu的旋转之间（没有找到对应的IMU），就直接赋值了
        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;
        }
    }
    
    /**
     * @brief 找到当前激光点对应的平移量
     * 使用IMU里程计数据中的平移量，按比例插值
     * 注意：此处并没有使用平移量（作者说运动相对较慢的情况去除平移畸变对效果提升不大）
     * 三个量均为0
     * @param relTime 
     * @param posXCur 
     * @param posYCur 
     * @param posZCur 
     */
    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;
    }

	/**
     * @brief 点云去畸变 （针对每一个点进行处理）
     * 1、判断激光点是否有独立的时间戳，是否有用于去除运动畸变的IMU信息
     * 2、找到当前激光点对应的旋转量与平移量，计算起始点到当前点的相对旋转与相对平移
     * 3、坐标转换，去除运动畸变
     * @param point 
     * @param relTime 
     * @return PointType 
     */
    PointType deskewPoint(PointType *point, double relTime)
    {
        // 每个点是否有单独的时间戳，或者是否有用于去畸变的IMU信息
        if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
            return *point;

        // relTime是相对时间，加上起始时间就是绝对时间
        // timeScanCur是帧的起始时间，relTime是该点的相对起始的时间
        double pointTime = timeScanCur + relTime;

        float rotXCur, rotYCur, rotZCur;
        // 计算当前点相对起始点的相对旋转
        findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
        // 这里没有计算平移补偿，直接被赋值为0
        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;
        // 就是R × p + t，把点补偿到第一个点对应时刻的位姿   （把点转换到当前帧第一个点对应的时刻）
        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;
    }

点云投影

/**
     * @brief 投影点云到一个平面上（大小为线数×每圈的点云数）
     * 1、遍历每一个点，去除点云中距离过小（1m）以及过大（1000m）的点
     * 2、计算该点在平面上的行号（线号）与列号（从x负方向起始），并把该点的距离添加到平面中的相应位置
     * 3、点云去畸变，计算索引，添加到fullCloud变量中
     */
    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;
            // 计算这个点距离lidar中心的距离
            float range = pointDistance(thisPoint);
            // 距离太小或者太远都认为是异常点
            if (range < lidarMinRange || range > lidarMaxRange)
                continue;
            // 取出对应的在第几根scan上
            int rowIdn = laserCloudIn->points[i].ring;
            // scan id必须合理
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;
            // 如果需要降采样，就根据scan id适当跳过
            if (rowIdn % downsampleRate != 0)
                continue;
            // 计算水平角  弧度转换为角度
            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            // 角度分辨率(默认设置的一圈是1800点，分辨率0.18°)
            static float ang_res_x = 360.0/float(Horizon_SCAN);
            // 计算水平线束id，转换到x负方向e为起始，顺时针为正方向，范围[0,H]
            //                x|
            //                 |
            //                 |
            //      -----------|----------y             
            //                 |
            //                 | 从此位置开始，列索引的值为零
            int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;
            // 对水平id进行检查
            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;
            // 如果这个位置已经有填充了就跳过
            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
                continue;
            // 对点做运动补偿
            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
            // 将这个点的距离数据保存进这个range矩阵中
            rangeMat.at<float>(rowIdn, columnIdn) = range;
            // 算出这个点的索引
            int index = columnIdn + rowIdn * Horizon_SCAN;
            // 保存这个点的坐标
            fullCloud->points[index] = thisPoint;
        }
    }

提取有效点，并整理为cloud_info消息类型，发布

/**
     * @brief 提取出有效的点的信息放在cloudInfo中，并提取点到extractedCloud中
     * 1、记录点云数据中每一行的起始索引与终止索引
     * 2、记录距离点云的列号与距离
     * 3、提取有效点到extractedCloud变量中
     */
    void cloudExtraction()
    {
        int count = 0;
        // extract segmented cloud for lidar odometry
        // 遍历每一根scan
        for (int i = 0; i < N_SCAN; ++i)
        {
            // 这个scan可以计算曲率的起始点（计算曲率需要左右各五个点）
            cloudInfo.startRingIndex[i] = count - 1 + 5;

            for (int j = 0; j < Horizon_SCAN; ++j)
            {   
                // 如果ij处的距离图像中有点
                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
                    // 他的3d坐标信息
                    extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of extracted cloud
                    // count只在有效点才会累加
                    ++count;
                }
            }
            // 这个scan可以计算曲率的终点
            cloudInfo.endRingIndex[i] = count -1 - 5;
        }
    }
    
    /**
     * @brief 发布消息
     * 1、发布提取出的有效点云"lio_sam/deskew/cloud_deskewed"
     * 2、发布自定义的点云信息集合"lio_sam/deskew/cloud_info"
     */
    void publishClouds()
    {
        cloudInfo.header = cloudHeader;
        // 发布提取出来的有效的点  并把sensor_msgs/PointCloud2这个类型的消息给cloudInfo.cloud_deskewed
        // "lio_sam/deskew/cloud_deskewed"
        cloudInfo.cloud_deskewed  = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
        // 发布cloudInfo  "lio_sam/deskew/cloud_info"
        pubLaserCloudInfo.publish(cloudInfo);
    }

Main函数

Main函数，调用之前定义的ImageProjection 类，并执行。

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_sam");

    /**
     * @brief 主要作用
     * 接收IMU原始数据imuTopic，预积分节点odomTopic+"_incremental"数据，原始点云数据pointCloudTopic
     * 利用IMU原始数据，对点云进行运动畸变去除处理，odomTopic+"_incremental"数据在这里没有使用
     * 投影点云到一个距离图像上，并计算cloudInfo的相关信息
     * 
     * 提取其中的有效点，发布lio_sam/deskew/cloud_deskewed。
     * 集合一个cloudInfo，发布lio_sam/deskew/cloud_info。
     */
    ImageProjection IP;
    
    ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
    
    // 单线程顺序执行，不能保证时效性
    // ROS多线程，三个subscriber，设置三个线程，防止丢失数据
    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
    
    return 0;
}