LIO_SAM程序实现原理学习笔记（四）--imageProjection.cpp

        最近发现详细到每行代码的实现这种程度的学习对于现在的我来说确实是在时间上不太允许，尤其是在我看到最后那个cpp长达1800行的代码量。

 

        正好在学习lio sam后面的代码过程中，我找到了一篇博客，里面对程序的代码有非常详细的注释。对于每一个功能在代码上如何实现或者每行代码有什么样的作用作者都在代码中标注的非常清楚。在找到这篇文章后，我可以更专注于程序实现流程原理而不是这行代码究竟是啥意思了。

        在这里将这篇注释非常详细的博客链接挂在这里，接下来的内容将更加围绕程序运行顺序以及函数功能来展开，对于某个功能的实现可以直接参考下面的博客：

https://blog.csdn.net/zkk9527/article/details/117957067

        imageProjection.cpp这个文件的主要功能是对原始的点云数据进行一个去畸变的处理。

        激光雷达的工作原理其实可以简单理解为雷达中有一列激光发射器，对于我使用的16线雷达就有对应16个，激光经发射器发出遇到障碍物返回并被雷达接收，一个发射器在回转一周能够得到一圈的点，即一个平面上的环境轮廓，而16个就可以得到16个不同高度的环境轮廓，这样就可以得到环境的情况。因此更大的线数也代表着有更精确的环境轮廓。但是雷达每转一圈是需要时间的，而雷达载体是在不断运动的，这样就会导致移动中的雷达扫描的一帧点云与静止的雷达扫描的实际环境轮廓有错位的情况，而这段代码程序的功能就是将每一帧的点云产生的这种畸变进行矫正。

        首先附上程序的函数流程图：

        main函数结构比较简单，与上一个imu预积分的main函数类似，在这里就不再叙述了。

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

    ImageProjection IP;
    
    ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");

    //线程设置为3
    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
    
    return 0;
}

        从构造函数ImageProjection开始看起，构造函数也比较简洁，主要是订阅了三个数据并且发布了两个数据。然后紧接分配内存的一个函数以及重置参数的一个函数。

ImageProjection():
    deskewFlag(0)
    {
        //imuTopic话题名，2000队列长度，&ImageProjection::imuHandler回调函数
        //ros::TransportHints().tcpNoDelat()用于指定hints，确定传输层的作用话题方式采用无延时的TCP传输方式
        subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
        //订阅来自imuPreintegration中发布的odomTopic
        subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
        //订阅pointCloudTopic
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

        //发布去畸变点云，引号内话题名，队列数1
        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();

        //setVerbosityLevel设置控制台输出信息，L_ALWAYS不输出任何信息，下面命令只输出错误信息。
        pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
    }

        先看imuHandler这个回调函数，这个函数内部非常简单，一共三行代码，先是将imu坐标系转移至雷达坐标系下（只用了旋转，没使用平移）。然后一行上锁命令，最后将数据储存进队列。

//调用头文件将IMU坐标系转换到雷达坐标系(旋转)
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
        //锁定数据，添加数据时阻止队列
        std::lock_guard<std::mutex> lock1(imuLock);
        imuQueue.push_back(thisImu);

       odometryHandler回调函数也是差不多的结构，上锁，然后储存进队列。

std::lock_guard<std::mutex> lock2(odoLock);
        odomQueue.push_back(*odometryMsg);

       然后是本程序最重要的cloudHandler回调函数。

        //cachepointcloud函数将雷达信息转为pcl点云格式，并进行时间偏移信息、有序排列、ring、时间戳消息等检查工作
        if (!cachePointCloud(laserCloudMsg))
            return;
        //deskewInfo函数预处理了imu与odom补偿信息
        if (!deskewInfo())
            return;
        //将点云中的点进行去除异常点、运动补偿等操作
        projectPointCloud();
        //记录下有效激光点的信息
        //要记录每根扫描线在一维数组中的索引，在原始图像中的距离与垂直线等信息
        cloudExtraction();
        //发布去畸变后的点云数据
        publishClouds();
        //重置参数
        resetParameters();

函数首先使用了两个if判断句，两个判断条件里面引用了两个函数：cachePointCloud函数以及deskewInfo函数。

        cachePointCloud函数首先进行了缓存了点云数据并进行了一个判断，如果队列里面的数据小于等于二，直接给return一个false不再执行。接下来根据雷达型号进行一个点云格式转换，如果雷达并非velodyne或者ouster雷达，将会报错并关闭ros。如果上面都没有问题，将接着进行获取时间戳、检查dense flag标志位、检查ring、检查点云点的时间信息。全部通过后return true。

        deskewInfo函数下又调用了两个函数，这里不再展开，在这个函数里首先给imu和odom数据上锁，首先通过一个if条件判断确保雷达帧前后都有imu数据覆盖，保证数据覆盖后调用imuDeskewInfo函数来计算出点云帧时间内imu数据的角度等信息，来保证后续对应点云矫正的时间值。之后又通过odomDeskewInfo函数找出点云时间帧内的姿态变化。做完这一切后函数返回true。

        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);
            if (range < lidarMinRange || range > lidarMaxRange)
                continue;

        下面的代码对scan进行一系列计算检查等操作，包括降采样、水平角、水平分辨率等计算检查，然后数据传入deskewPoint函数，该函数将点云全部矫正至初始时刻，主要依据findRotation函数及findPosition函数，通过角度变化以及平移变化将所有点云统一至初始时刻。但特别需要注意的是findPosition函数内参数全部被设置为了0，也就是作者忽略了点云帧时间内的位移，将这帧时间内看作静止，因此作者也在预积分部分设置了速度上限，如果想修改速度上限需要同时修改这两处。否则可能会有较大误差。

        作者也在此处进行了注释，表示步行这样的慢速度下还是设为零更好。注释部分为为快速运动时编写的代码。

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

这些工作做完之后，矫正工作基本就结束了。余下的cloudExtraction函数将记录有效的激光点信息，记录每根扫描线scan在一位数组中的索引、在原始图像中的距离以及垂直线等信息。publishClouds函数顾名思义为发布去畸变后的点云数据。resetParameters函数则为重置部分参数的函数。

如此，imageProjection.cpp文件的流程基本上就差不多了。