LIO-SAM源码阅读笔记(二)---imageProjection.cpp

代码：
https://github.com/TixiaoShan/LIO-SAM
imageProjection.cpp主要分为两大块，一个是类似于Lego-loam里的将点云投射到距离图像，另一个是去除运动畸变。

1. 初始化

deskewFlag(0)
    {
      
         //订阅话题进入回调函数 imu数据 地图优化程序中发布的里程计话题（增量式）  激光点云
        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);
        //发布激光点云信息 这里建议看一下自定义的lio_sam::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()
    {
       //根据params.yaml中给出的N_SCAN Horizon_SCAN参数值分配内存
        laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
        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;
        }
    }

2. 回调函数

2.1 imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
       //IMU回调里没有执行太多复杂的操作 
        //首先imuConverter在utility.h中定义  就是将imu坐标系的数据转到雷达坐标系下
       //上一篇文章中有针对每个人的数据的适配提示
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
         //锁存就是保证一次处理完
        std::lock_guard<std::mutex> lock1(imuLock);
       //放入队列中  这之后会有pop的操作 也均为基于imuQueue的
        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;
    }

2.2 odometryHandler

void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
    {
      //锁存后存入队列 
        //在前期版本中作者在地图优化程序中只发布了一个里程计消息 现在增加了增量式里程计话题 更方便处理
        std::lock_guard<std::mutex> lock2(odoLock);
        odomQueue.push_back(*odometryMsg);
    }

2.3 cloudHandler

主要是进行了点云去畸变以及投影到距离图像等工作 如果自己的数据集跑算法没有反应建议调试下这里运行是否正常.

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
         //缓存点云信息
        if (!cachePointCloud(laserCloudMsg))
            return;
          //计算去畸变所需的参数
        if (!deskewInfo())
            return;
       //投影到距离图像中 （类似Lego-loam）
        projectPointCloud();
      //点云提取 
        cloudExtraction();
       //发布点云
        publishClouds();
        //重置参数