LIO-SAM前端点云去畸变源码-imageProjection.cpp解析

LIO-SAM前端点云去畸是在imageProjection.cpp中的，从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");

    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
    
    return 0;
}

创建点云投影对象，所有的操作都在ImageProjection这个类中实现。看构造函数：

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);
        pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);

        allocateMemory();   //内存初始化
        resetParameters();  //参数初始化

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

输入源为经过预积分处理过的里程计，原始的IMU，初始的激光点云数据。

接着看传感器数据回调：

IMU回调：只是存储IMU数据到队列中；里程计回调也是一样

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)  //仅接收放队列
    {
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

        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)
    {
    //检测一下点数量是否够2个，线数通道和时间通道是否存在        
    if (!cachePointCloud(laserCloudMsg)) 
            return;

        //去畸变
        //IMU不是空，imu序列的第一个时间戳大于当前帧雷达时间戳，IMU最后一个时间戳小于下一帧雷达时间戳
        if (!deskewInfo())  
            return;

        projectPointCloud();

        cloudExtraction();

        publishClouds();

        resetParameters();
    }

    //清除临时点云，并检查点云帧里面是否有ring和time通道
    bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        // cache point cloud 存入队列中
        cloudQueue.push_back(*laserCloudMsg);
        if (cloudQueue.size() <= 2)
        {std::cout << "cloudQueue.size() <= 2" << std::endl;
            return false;}
        //啥意思呢，就是点云需要累积最少两帧，然后才能对前面那一帧进行点云去畸变，所以当前帧进来时，对上一帧进行去畸变

        //提取队列第一个做timeScanCur，之后将处理过的pop出，选第二个做timeScanNext。后面与imu对比时间戳
        // convert cloud
        currentCloudMsg = cloudQueue.front(); //要对队列中最前的那帧点云去畸变
        cloudQueue.pop_front();
        pcl::PointCloud<PointXYZIRT>::Ptr laserCloudInCopy(new pcl::PointCloud<PointXYZIRT>());
        pcl::PointCloud<PointXYZIRT>::Ptr tmp_range(new pcl::PointCloud<PointXYZIRT>());
        //pcl::fromROSMsg(currentCloudMsg, *laserCloudIn);
        pcl::fromROSMsg(currentCloudMsg, *laserCloudInCopy);

        // get timestamp
        cloudHeader = currentCloudMsg.header;
        timeScanCur = cloudHeader.stamp.toSec();  //前一帧头部时间
        //timeScanEnd = timeScanCur + laserCloudIn->points.back().time; // Velodyne
         //前一帧尾部时间
        timeScanEnd = timeScanCur + laserCloudInCopy->points.back().time; 
        // timeScanEnd = timeScanCur + (float)laserCloudIn->points.back().t / 1000000000.0; // Ouster
       
		// Remove Nan points
		// std::vector<int> indices;
		// pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

        // check dense flag 确保点云没有NAN值
        // if (laserCloudIn->is_dense == false)
        // {
        //     ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
        //     ros::shutdown();
        // }

        // check ring channel
        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;
                }
            }
            // if (ringFlag == -1) //不带有ringFlag参数
            // {
            //     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 (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
            {
                if (currentCloudMsg.fields[i].name == timeField)
                {
                    deskewFlag = 1;
                    break;
                }
            }
            // if (deskewFlag == -1)
            //     ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
        }

        return true;
    }

然后就是点云去畸变了

bool deskewInfo()
    {
       //因为IMU去畸变和里程计去畸变会操作IMU和里程计队列，即剔除一些不符合要求的数据，因此它需要两个锁
        std::lock_guard<std::mutex> lock1(imuLock);
        std::lock_guard<std::mutex> lock2(odoLock);
         //检测队列数量 //IMU不是空，imu序列的第一个时间戳大于当前帧雷达时间戳，IMU最后一个时间戳小于下一帧雷达时间戳
        // make sure IMU data available for the scan
        if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
        {
            ROS_DEBUG("Waiting for IMU data ...");
            return false;
        }//一般用于错误判断,基本不会发生

        imuDeskewInfo();//一帧点云进来一次

        odomDeskewInfo();

        return true;
    }

---

        然后细看利用IMU数据如何补偿，这是重点

 void imuDeskewInfo()
    {
        cloudInfo.imuAvailable = false;
        //计算当前激光时间戳前，每个时刻imu累计出的角速度，并判定imu数据是否可用（数量够）
        while (!imuQueue.empty())
        {
            if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
                imuQueue.pop_front();
            else
                break;
        }
        //提出IMU队列中早于当前激光帧时间戳(因为去畸变的其实是上一帧点云，其实不是当前最新的激光时间戳)的0.01s的UMU数据
        if (imuQueue.empty())
            return;

        imuPointerCur = 0;

        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的姿态角从ros格式转为欧拉角
                imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

            if (currentImuTime > timeScanEnd + 0.01)
                break;

            if (imuPointerCur == 0){
                imuRotX[0] = 0;
                imuRotY[0] = 0;
                imuRotZ[0] = 0;
                imuTime[0] = currentImuTime; //存的是最接近当前点云帧的IMU数据,刚好>timeScanCur - 0.01
                ++imuPointerCur;
                continue;
            }//第一次才会进,目的是赋值给imuTime进行初始化
            //也就是说imu队列的最开始记录的第一个符合要求的IMU时间戳,这时候将IMU的姿态认为0

            // get angular velocity
            double angular_x, angular_y, angular_z;
            //存储第二个及之后满足时间要求的IMU数据中的角速度获取出来
            imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

            // integrate rotation 利用角度和角速度进行积分,得到IMU时间的
            //以最近当前点云帧的IMU的姿态角为起点,然后逐个进行积分
            //获取的是当前点云帧开始时base坐标系下的值IMU的测量值：角速度值，这里没有用到姿态角，可以不九轴IMU去畸变
            //imuPointerCur从1开始加
            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; //for循环最后++了一次,这里就减去一次

        if (imuPointerCur <= 0) //这里是确保没有一次符合要求的,避免出现异常,基本不会出现
            return;

        cloudInfo.imuAvailable = true; //积分点云之间的姿态角 IMU去畸变
    }

        里程计的值也是类似的，但差值用的是位移

 //读取odom数据，并根据协方差判断是否相信这个里程计有效，因为IMU预积分那会发出一个协方差
//应该是为了groundtruth对比用的，pose信息保存在cloudInfo里
    void odomDeskewInfo()
    {
        cloudInfo.odomAvailable = false; //先初始化变量，以免后面直接return而没有数据

        while (!odomQueue.empty()) //确保里程计不为空,保证队列的时间戳和当前激光的时间差不会早于0.01
        {
            if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
                odomQueue.pop_front();
            else
                break;
        }

        if (odomQueue.empty())
            return;

        if (odomQueue.front().header.stamp.toSec() > timeScanCur)
            return;
           //此时队列里队首的时间戳在当前处理激光帧(上一帧激光帧)早10ms内，和IMU时间对齐一样
        // get start odometry at the beinning of the scan
        nav_msgs::Odometry startOdomMsg;

        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            startOdomMsg = odomQueue[i];

            if (ROS_TIME(&startOdomMsg) < timeScanCur)
                continue;
            else
                break;
        }
        //为最接近当前处理激光点云时间戳(上一激光帧)的里程计
        tf::Quaternion orientation;
        //将msg信息转为TF信息
        tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);

        double roll, pitch, yaw; //获取欧拉角
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

        // Initial guess used in mapOptimization
        //这些initial的信息是通过最接近点云时间的里程计获取的
        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;

        // get end odometry at the end of the scan
        odomDeskewFlag = false; //去畸变

        //最新的里程计时间在当前帧末尾还有数据时
        //这里就是说在里程计队列中 odomQueue.front() < timeScanEnd <= odomQueue.back()
        //保证要差值的点云时间持续在里程计队列的覆盖中，否则就不用里程计进行去畸变
        if (odomQueue.back().header.stamp.toSec() < timeScanEnd) 
            return;

        nav_msgs::Odometry endOdomMsg;

        for (int i = 0; i < (int)odomQueue.size(); ++i)
        {
            endOdomMsg = odomQueue[i];

            if (ROS_TIME(&endOdomMsg) < timeScanEnd)
                continue;
            else
                break;
        }//当前激光帧末尾
        //保证前后协方差没有变化
        if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
            return;

        Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);

        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);
        
        //一帧之间的变化量，帧尾相对于帧首的变化量，用SE(3)表示

        Eigen::Affine3f transBt = transBegin.inverse() * transEnd; 
        float rollIncre, pitchIncre, yawIncre;
        //获取一个点云之间的位移变化量 odomIncreX，这里要注意，位移量是类成员变量，但里程计之间的角度变化量是局部量，算了就丢弃不用的量，因此里程计的角度变化是不会参与到畸变校正中
        pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

        odomDeskewFlag = true;
    }

        以上两个畸变计算补偿量是独立的，独立计算IMU数据在当前处理帧(上一激光帧)的角度变化量，并存下来作为数值以备使用；同理里程计也是类似，不同的是，里程会记录下当前处理帧(上一激光帧)点云开始时刻的里程计的测量值，包括位置和姿态，其坐标量为odom topic的坐标系。
        cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x 这个数据用于后端的初始化。

        存下来在哪里进行用呢？就在下一个函数中：projectPointCloud();投影每一个点，将它的距离图投影出来，这个和LeGO-LOAM是一致的。

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;

            //距离雷达原点的距离,剔除一定范围外的点云
            float range = pointDistance(thisPoint);
            if (range < lidarMinRange || range > lidarMaxRange)
                continue;

            int rowIdn = laserCloudIn->points[i].ring;
            //std::cout << "rowIdn = " << rowIdn << std::endl;
            if (rowIdn < 0 || rowIdn >= N_SCAN) //确保线数在1-16之间的数据
                continue;

            if (rowIdn % downsampleRate != 0) //默认为一,没有降采样
                continue;

            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            static float ang_res_x = 360.0/float(Horizon_SCAN); //水平角分辨率 360/1800
            int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN; //1800个线

            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX) //初始化时设置为FLT_MAX,所以不会进
                continue;

            //遍历点云一帧的16X1800个每一个点
            thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne
            // thisPoint = deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0); // Ouster

            rangeMat.at<float>(rowIdn, columnIdn) = range; //距离图上显示的每个点到原点的距离

            int index = columnIdn + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint; //然后按照一维数组的形式存放
        }
    }

        遍历当前处理帧(最新回调进的一个帧的前一帧）点云的每一个点，每一个点都不错过，然后调用去畸变的函数deskewPoint(&thisPoint, laserCloudIn->points[i].time)去这一个点的畸变。

 //针对一帧点云一个点,输入点坐标和点云时间, relTime为从驱动获取的，所以需要自带reltime的点云数据，或者通过Lego-loam那样插值标记点的时间
    PointType deskewPoint(PointType *point, double relTime)
    {
        if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
            return *point;
    
        double pointTime = timeScanCur + relTime; //当前帧首的时间

        float rotXCur, rotYCur, rotZCur; //去畸变补偿的角度值
        findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
        
        float posXCur, posYCur, posZCur;//去畸变补偿的位移值
        //这里没有计算平移补偿 如果运动不快的话
        findPosition(relTime, &posXCur, &posYCur, &posZCur);

        if (firstPointFlag == true) //第一次才会进
        {
            //第一次进行畸变矫正,获取第一帧点云首到点云尾的坐标变换
            transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
            firstPointFlag = false;
        }
        //transStartInverse为全局变量
        // transform points to start //第一次的时候transBt为单位阵
        //这些为base0 -base-i 即从
        Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
        //第一次的时候transBt为单位阵,姿态变化为0,位移变化为0
        Eigen::Affine3f transBt = transStartInverse * transFinal; 


        //R*p+t ，把点补偿到当前处理帧的第一个点对应的时刻的位姿，这样所有的点都去畸变了
        PointType newPoint;
        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; //去畸变后的
    }

这里的findRotation()

  void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
    {
        *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;//只是保证几个输出值为0

        int imuPointerFront = 0;
        while (imuPointerFront < imuPointerCur) //imuPointerCur补偿的队列长度
        {
            if (pointTime < imuTime[imuPointerFront]) //找到当前的点的时间大于等于补偿的最新的IMU的时间
                break;
            ++imuPointerFront;
        }
        //imuPointerFront是只有imuPointerCur为0的时候才会为0,或者第一帧进去就break,即pointTime < imuTime[0]
        //pointTime > imuTime[imuPointerFront]是在点云时间pointTime
        //imuRotX[0] = 0
        //而pointTime > imuTime[imuPointerFront] 只有imuPointerCur= 0时候才可能会出现
        //这时候pointTime > imuTime[0], imuTime[0]与当前帧早0.01s
        if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
        {
            *rotXCur = imuRotX[imuPointerFront];
            *rotYCur = imuRotY[imuPointerFront];
            *rotZCur = imuRotZ[imuPointerFront];
        } else { //其他情况,通常满足这个条件  pointTime < imuTime[imuPointerFront]
            int imuPointerBack = imuPointerFront - 1;
            //连续两帧imu时间戳差值  imuTime[imuPointerBack] < pointTime < imuTime[imuPointerFront]
            double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            //按照上面的推论 ratioFront + ratioBack =1
            //根据激光点的时间进行插值而不是近似取imuPointerFront或者imuPointerBack
            *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
            *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
            *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
        }//这里用IMU的角度插值雷达之间的角度
    }

    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; //点云去畸变,补偿里程计之间的变化
    }

可以看到，前面算了半天里程计的位移增量，结果小哥没有用上里程计的插值去畸变，可能是速度太低用起来反而会有问题？ 我猜的，反正是没有用上。

而再后面的就是点云提取和发布给下一个节点用了

 void cloudExtraction() //将点云range图存到cloudinfo中随后发出去
    {
        int count = 0;
        // extract segmented cloud for lidar odometry
        for (int i = 0; i < N_SCAN; ++i)
        {
            cloudInfo.startRingIndex[i] = count - 1 + 5;

            for (int j = 0; j < Horizon_SCAN; ++j)
            {
                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
                    extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of extracted cloud
                    ++count;
                }
            }
            cloudInfo.endRingIndex[i] = count -1 - 5;
        }
    }
    
    void publishClouds()
    {
        cloudInfo.header = cloudHeader;
        cloudInfo.cloud_deskewed  = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
        pubLaserCloudInfo.publish(cloudInfo);
    }