LIO-SAM 源码阅读--imageProjection.cpp(1)

#include "utility.h"
#include "lio_sam/cloud_info.h"

struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
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)
)

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

// Use the Velodyne point format as a common representation
using PointXYZIRT = VelodynePointXYZIRT;

const int queueLength = 2000;

class ImageProjection : public ParamServer
{
private:

    // 互斥锁
    std::mutex imuLock;
    std::mutex odoLock;

    // !@Subscriber
    ros::Subscriber subLaserCloud;
    ros::Subscriber subImu;
    ros::Subscriber subOdom;

    // !@ Publisher
    ros::Publisher pubLaserCloud;   // no use
    ros::Publisher pubExtractedCloud;
    ros::Publisher pubLaserCloudInfo;

    // !@ queue  回调函数中用于保存 接收的信息
    std::deque<sensor_msgs::Imu> imuQueue;
    std::deque<nav_msgs::Odometry> odomQueue;
    std::deque<sensor_msgs::PointCloud2> cloudQueue;

    sensor_msgs::PointCloud2 currentCloudMsg;   //保存 队列中取出的当前雷达点云信息

    double *imuTime = new double[queueLength];
    double *imuRotX = new double[queueLength];
    double *imuRotY = new double[queueLength];
    double *imuRotZ = new double[queueLength];

    // imuRotX、Y、Z，imuTime的数组下标，最终数组长度
    int imuPointerCur;
    // 第一个点标记
    bool firstPointFlag;
    Eigen::Affine3f transStartInverse;

    pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;  // 原始雷达点云信息
    pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;   // SensorType::OUSTER
    pcl::PointCloud<PointType>::Ptr   fullCloud;    // 保存去畸变之后的点【将点投影到起始位置】
    pcl::PointCloud<PointType>::Ptr   extractedCloud;

    int deskewFlag;   // 等于1 表示有 “time” 或者 “t”
    cv::Mat rangeMat;  // 二维图像矩阵？

    bool odomDeskewFlag;
    float odomIncreX;
    float odomIncreY;
    float odomIncreZ;

    lio_sam::cloud_info cloudInfo;
    double timeScanCur;  // 雷达一帧点云 第一个点时间
    double timeScanEnd;  // 雷达一帧点云 最后一个点时间  也就是下一帧开始时间
    std_msgs::Header cloudHeader;

    vector<int> columnIdnCountVec;


public:
    ImageProjection():
    deskewFlag(0)
    {
        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);
        //发布激光点云信息   msg文件夹下面的cloud_info.msg 定义的信息
        pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);

        allocateMemory();  // 分配内存
        resetParameters();  // 重置参数

        pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
    }

    void allocateMemory()
    {
        laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
        tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());   // SensorType::OUSTER
        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();
    }

    void resetParameters()
    {
        laserCloudIn->clear();
        extractedCloud->clear();
        // reset range matrix for range image projection
        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;
        }

        columnIdnCountVec.assign(N_SCAN, 0);
    }

    ~ImageProjection(){}

    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
        // imuConverter() 将imu坐标系的数据转到雷达坐标系下
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
        //锁存就是保证一次处理完
        std::lock_guard<std::mutex> lock1(imuLock);
        // 转换之后的IMU数据 放入队列中  这之后会有pop的操作 也均为基于imuQueue的
        imuQueue.push_back(thisImu);

        // debug IMU data
        // cout << std::setprecision(