LIO-SAM前端点云预处理

这是imageProjection.cpp中的 ImageProjection IP类
前端点云预处理其实就是 提取运动补偿信息、 用提取的信息对点进行补偿到最开始的点的时刻、 保存有效点云数据信息和 确定计算曲率的点索引的起止点，实际操作在原始点云回调函数中

订阅的消息数据

1. imu原始数据 imuTopic
2. 后端里程记数据 odomTopic+“_incremental” 位姿融合后的，详见LIO-SAM中位姿融合输出
3. 原始点云数据 pointCloudTopic

订阅 imu原始数据 的回调函数

把imu的数据旋转到前左上坐标系下，并存起来

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

订阅 后端里程计 的回调函数

也只是把数据存起来而已

	void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
	{
   
   	  std::lock_guard<std::mutex> lock2(odoLock);
  	  odomQueue.push_back(*odometryMsg);
	}

订阅 原始点云 的回调函数

真正处理运动补偿的函数是这里，处理流程比较简洁明了，下面将一个个函数进行解析

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
   
        if (!cachePointCloud(laserCloudMsg))
            return;

        if (!deskewInfo())
            return;

        projectPointCloud();

        cloudExtraction();

        publishClouds();

        resetParameters();
    }

cachePointCloud(laserCloudMsg)

1. 点云数据保存进队列 并 确保队列里大于两帧点云数据

	// 点云数据保存进队列
	cloudQueue.push_back(*laserCloudMsg);
	// 确保队列里大于两帧点云数据
	if (cloudQueue.size() <= 2)
    	return false;

2. 缓存足够的点云后将其转换成pcl格式，有分别对VELODYNE或者OUSTER不同型号的多线雷达进行不同的操作

        // 缓存了足够多的点云之后
        // convert cloud
        currentCloudMsg = std::move(cloudQueue.front());
        cloudQueue.pop_front();
        if (sensor == SensorType::VELODYNE)
        {
   
            pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);    // 转成pcl的点云格式
        }
        else if (sensor == SensorType::OUSTER)
        {
   
            // Convert to Velodyne format
            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();
        }

3. 获取时间戳

        // get timestamp
        cloudHeader = currentCloudMsg.header;
        timeScanCur = cloudHeader.stamp.toSec();
        timeScanEnd = timeScanCur + laserCloudIn->points.back().time;

4. 查看点云是否有序排列

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

5. 查看驱动里是否把每个点属于哪一根扫描scan这个信息，没有的话就需要像loam那样自己算了，一般都是带有的

        // 查看驱动里是否把每个点属于哪一根扫描scan这个信息
        static int ringFlag = 0;
        if (ringFlag == 0)
        {
   
            ringFlag = -1;
            for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
            {
   
                if (currentCloudMsg.fields[i].name == "ring")
                {
   
                    ringFlag = 1;
                    break;
                }