LIO-SAM源码解析-图像投影imageProjection.cpp

节点数据传递

从上图中可以看出，节点(红色)/lio_sam_imageProjection 订阅(蓝色)/imu, /rslidar_points, /odometry/imu_incremental三个话题，并发布(绿色)/lio_sam/deskew/cloud_info, /lio_sam/deskew/cloud_deskewed两个话题．其中*/lio_sam/deskew/cloud_deskewed*为偏斜矫正后的点云，是显示用的，只被rviz节点订阅，因此该话题不加讨论．

订阅 /imu, /odometry/imu_incremental 干了什么？

void imuHandler(const sensor_msgs::Imu::ConstPtr &imuMsg)
{
    // 将imu信息在雷达坐标系下表达
    sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

    std::lock_guard<std::mutex> lock1(imuLock);
    imuQueue.push_back(thisImu); // 缓存imu数据
}

void odometryHandler(const nav_msgs::Odometry::ConstPtr &odometryMsg)
{
    // 缓存imu里程计增量信息
    std::lock_guard<std::mutex> lock2(odoLock);
    odomQueue.push_back(*odometryMsg);
}

从上面可以看出，这两个话题数据暂时是被缓存在队列中的，留作后面用，其中一个原始IMU数据，一个是IMU预积分节点发布的预积分结果．

订阅 /rslidar_points 干了什么？

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    // 转换雷达点云信息为可处理点云数据并缓存
    if (!cachePointCloud(laserCloudMsg))
        return;

    // 点云偏斜矫正所需的imu数据的预处理(计算雷达帧扫描开始和结束时间戳之间的imu相对位姿变换)
    if (!deskewInfo())
        return;

    // 对点云进行偏斜矫正，并投影到深度图中
    projectPointCloud();

    // 确定每根线的起始和结束点索引，并提取出偏斜矫正后点云及对应的点云信息
    cloudExtraction();

    // 发布有效点云和激光点云信息(包括每根线的起始和结束点索引、点深度、列索引)
    publishClouds();

    // 重置中间变量
    resetParameters();
}

从代码中可以看出，这部分实现了点云的偏斜矫正、深度图的生成、点云的提取和点云信息的发布等多个内容，下面我将对中间的重要内容进行分析．

点云偏斜矫正所需的imu数据的预处理deskewInfo()

(1) 当imu数据可得到时，对当前雷达帧起始扫描时间戳到扫描结束时间戳之间的IMU数据进行姿态积分，获得这段时间内IMU姿态的变化，用于后面偏斜矫正时使用．

void imuDeskewInfo()
{
    cloudInfo.imuAvailable = false;

    while (!imuQueue.empty())
    {
        // 丢弃早于当前雷达帧开始扫描时间戳的缓存的imu帧
        if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
            imuQueue.pop_front();
        else
            break;
    }

    // 如果没有imu数据，直接返回
    if (imuQueue.empty())
        return;

    imuPointerCur = 0;

    for (int i = 0; i < (int)imuQueue.size(); ++i)
    {
        // 获取imu帧时间戳
        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)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

        // 如果imu帧时间戳大于雷达帧扫描结束的时间戳，则退出
        if (currentImuTime > timeScanEnd + 0.01)
            break;

        // 初始化第一帧imu姿态信息为0
        if (imuPointerCur == 0)
        {
            imuRotX[0] = 0;
            imuRotY[0] = 0;
            imuRotZ[0] = 0;
            imuTime[0] = currentImuTime;
            ++imuPointerCur;
            continue;
        }

        // get angular velocity
        // 获取imu角速度信息
        double angular_x, angular_y, angular_z;
        imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

        // integrate rotation
        // 积分imu的姿态信息
        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;

    if (imuPointerCur <= 0)
        return;

    cloudInfo.imuAvailable = true;
}

(2) 当IMU预积分获得的里程计数据可得到时，查找当前雷达帧起始扫描时间戳到扫描结束时间戳之间的IMU里程计数据，并计算这段时间内的IMU姿态的变化，用于后面点云的偏斜矫正使用

void odomDeskewInfo()
 {
     cloudInfo.odomAvailable = false;

     // 丢弃早于当前雷达帧开始扫描时间的imu里程计帧
     while (!odomQueue.empty())
     {
         if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
             odomQueue.pop_front();
         else
             break;
     }

     // 如果没有imu里程计帧直接返回
     if (odomQueue.empty())
         return;

     if (odomQueue.front().header.stamp.toSec() > timeScanCur) // [cn]???
         return;

     // get start odometry at the beinning of the
     // 获得imu里程计起始帧(也就是在雷达帧扫描开始和结束时间戳之间的第一个imu里程计帧)
     nav_msgs::Odometry startOdomMsg;

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

         if (ROS_TIME(&startOdomMsg) < timeScanCur)
             continue;
         else
             break;
     }

     // 将imu里程计帧的姿态信息转换为欧拉角表示
     tf::Quaternion orientation;
     tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
     double roll, pitch, yaw;
     tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

     // Initial guess used in mapOptimization
     // 将imu里程计的位姿记录，并将被发布出去用于地图优化的初始值
     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;

     if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
         return;

     // 获得imu里程计结束帧(也就是在雷达帧扫描开始和结束时间戳之间的最后一个imu里程计帧)
     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;

     // 获得imu里程计起始帧位姿
     Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);

     // 获得imu里程计结束帧位姿
     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);
     // 获得起始和结束帧之间的相对转换
     Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

     // 获得起始和结束帧之间的位姿增量，其中姿态用欧拉角形式表示
     float rollIncre, pitchIncre, yawIncre;
     pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

     odomDeskewFlag = true;
 }

对点云进行偏斜矫正，并投影到深度图中projectPointCloud()

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; // 获得当前点所在的线索引，也即在深度图中的行索引
        if (rowIdn < 0 || rowIdn >= N_SCAN)        // 如果在给定的线数范围之外，则直接丢弃
            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);                         // 计算水平角分辨率
        int columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2; // 计算当前点在在深度图中的列索引
        // 做一些冗余判断，防止越界
        if (columnIdn >= Horizon_SCAN)
            columnIdn -= Horizon_SCAN;

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

        if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
            continue;

        // 对当前点进行偏斜矫正
        thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

        // 填充深度图
        rangeMat.at<float>(rowIdn, columnIdn) = range;

        // 记录完整点云
        int index = columnIdn + rowIdn * Horizon_SCAN;
        fullCloud->points[index] = thisPoint;
    }
}

其中对点的偏斜矫正过程如下:

PointType deskewPoint(PointType *point, double relTime)
{
    // 等待imu数据就绪才能进行处理
    if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
        return *point;

    double pointTime = timeScanCur + relTime; // 当前点采集时的时间戳

    float rotXCur, rotYCur, rotZCur;
    findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur); // 插值获得当前点时间戳对应的imu姿态

    float posXCur, posYCur, posZCur;
    findPosition(relTime, &posXCur, &posYCur, &posZCur); // 插值获得当前点时间戳对应imu位置(由于移动较慢，此处直接置为0)

    // 获取第一个点时间戳对应的位姿变化
    if (firstPointFlag == true)
    {
        transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
        firstPointFlag = false;
    }

    // transform points to start
    // 获得当前点对应的imu位姿和第一个点对应的imu位姿之间的相对变换
    Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
    Eigen::Affine3f transBt = transStartInverse * transFinal;

    // 变换当前点到第一个点所在的坐标系也即雷达坐标系(至此完成偏斜矫正)
    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;
}

确定每根线的起始和结束点索引，并提取出偏斜矫正后点云及对应的点云信息cloudExtraction()

void cloudExtraction()
{
    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;
    }
}

/lio_sam/deskew/cloud_info话题是什么?

# Cloud Info
Header header 

# 点云中的点被投影到深度图中，并按行优先的方式用数组来存储这个深度图，下面这两个数据分别是每根线(一般是16线)
起始激光点对应的深度图数组索引和结束激光点对应的深度图数组索引
int32[] startRingIndex # 
int32[] endRingIndex

int32[]  pointColInd # point column index in range image # 每个激光点在深度图中的列索引
float32[] pointRange # point range  # 保存深度图的数组

int64 imuAvailable # imu是否可用于匹配的初始位姿估计(imuDeskewInfo()有没有成功执行)
int64 odomAvailable # imu里程计是否可用于匹配的初始位姿估计(odomDeskewInfo()有没有成功执行)

# 当imu是可得到时，提供的初始姿态估计
# Attitude for LOAM initialization
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit

# 当imu里程计是可得到时，提供的初始位姿估计
# Initial guess from imu pre-integration
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw

# 点云相关信息
# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed  # original cloud deskewed 经过偏斜矫正的点云
sensor_msgs/PointCloud2 cloud_corner    # extracted corner feature 提取的角点特征点云(特征提取节点使用)
sensor_msgs/PointCloud2 cloud_surface   # extracted surface feature 提取的面点特征点云(特征提取节点使用)

注释代码路径

https://github.com/chennuo0125-HIT/LIO-SAM-note