LIO-SAM代码解析：imageProjection.cpp

大山同学

于 2025-01-12 19:36:53 发布

阅读量755

点赞数 18

分类专栏：代码解析文章标签： SLAM 人工智能算法移动机器人

本文链接：https://blog.youkuaiyun.com/weixin_41331879/article/details/145097679

版权

代码解析专栏收录该内容

11 篇文章

订阅专栏

文章目录

1. cloudHandler
2. deskewInfo
3. projectPointCloud
4. cloudExtraction

1. cloudHandler

cachePointCloud 函数用于缓存点云数据并对其进行格式转换和检查，以确保点云数据符合后续处理的要求。主要功能包括：

缓存接收到的点云消息，保证点云处理的连续性。
根据不同传感器类型转换点云格式。
提取点云时间戳信息并检查点云的密集性。
验证点云是否包含环号（ring channel）和时间戳字段，为去畸变（deskew）和其他算法提供基础支持。

// 函数：cachePointCloud
// 功能：缓存点云数据，转换格式并检查有效性
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    // 将接收到的点云数据存入队列
    cloudQueue.push_back(*laserCloudMsg);
    if (cloudQueue.size() <= 2)
        return false;

    // 从队列中取出当前点云消息
    currentCloudMsg = std::move(cloudQueue.front());
    cloudQueue.pop_front();

    // 根据传感器类型转换点云格式
    if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX)
    {
        pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
    }
    else if (sensor == SensorType::OUSTER)
    {
        // 转换为Velodyne格式
        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();
    }

    // 提取时间戳信息
    cloudHeader = currentCloudMsg.header;
    timeScanCur = cloudHeader.stamp.toSec();
    timeScanEnd = timeScanCur + laserCloudIn->points.back().time;

    // 检查点云是否为密集格式
    if (laserCloudIn->is_dense == false)
    {
        ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
        ros::shutdown();
    }

    // 检查点云是否包含环号（ring channel）
    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;
            }
        }
        if (ringFlag == -1)
        {
            ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
            ros::shutdown();
        }
    }

    // 检查点云是否包含时间戳字段
    if (deskewFlag == 0)
    {
        deskewFlag = -1;
        for (auto &field : currentCloudMsg.fields)
        {
            if (field.name == "time" || field.name == "t")
            {
                deskewFlag = 1;
                break;
            }
        }
        if (deskewFlag == -1)
            ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
    }

    return true;
}

2. deskewInfo

cloudHandler代码实现了点云去畸变（deskew）的信息准备过程，包括 IMU 数据和里程计（odometry）数据的处理。其核心功能包括：

IMU 数据处理 (imuDeskewInfo)：
- 校准并缓存扫描时间内的 IMU 数据。
- 通过角速度积分，计算每个时间点的姿态变化（滚转、俯仰、偏航）。
- 初始化扫描开始时的姿态信息。
里程计数据处理 (odomDeskewInfo)：
- 校准并缓存扫描时间内的里程计数据。
- 提取扫描开始和结束的位姿信息。
- 计算两时刻之间的位姿增量（位移和欧拉角增量）。

bool deskewInfo()
{
    // 使用锁来确保对 imuQueue 和 odoQueue 的访问是线程安全的
    std::lock_guard<std::mutex> lock1(imuLock);
    std::lock_guard<std::mutex> lock2(odoLock);

    // 检查 IMU 数据是否可用
    // 确保 IMU 数据队列不为空，并且数据的时间戳在扫描时间范围内
    if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
    {
        ROS_DEBUG("Waiting for IMU data ...");
        return false; // 如果 IMU 数据不可用，返回 false
    }

    // 处理 IMU 数据
    imuDeskewInfo();
    // 处理里程计数据
    odomDeskewInfo();

    return true; // 如果数据处理成功，返回 true
}

void imuDeskewInfo()
{
    // 初始化标志，表示 IMU 数据是否可用
    cloudInfo.imuAvailable = false;

    // 删除过早的 IMU 数据
    // 保留时间戳在扫描时间范围内的 IMU 数据
    while (!imuQueue.empty())
    {
        if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
            imuQueue.pop_front();
        else
            break;
    }

    // 如果 IMU 数据队列为空，直接返回
    if (imuQueue.empty()) return;

    // 初始化指针，用于遍历 IMU 数据
    imuPointerCur = 0;

    // 遍历 IMU 数据
    for (int i = 0; i < (int)imuQueue.size(); ++i)
    {
        sensor_msgs::Imu thisImuMsg = imuQueue[i];
        double currentImuTime = thisImuMsg.header.stamp.toSec();

        // 初始化扫描起始姿态
        if (currentImuTime <= timeScanCur)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);

        // 如果当前 IMU 数据的时间戳超过扫描结束时间，停止处理
        if (currentImuTime > timeScanEnd + 0.01)
            break;

        // 处理第一个 IMU 数据
        if (imuPointerCur == 0)
        {
            imuRotX[0] = imuRotY[0] = imuRotZ[0] = 0;
            imuTime[0] = currentImuTime;
            ++imuPointerCur;
            continue;
        }

        // 获取角速度并积分计算姿态变化
        double angular_x, angular_y, angular_z;
        imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
        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;

    // 如果处理了有效的 IMU 数据，设置标志为 true
    if (imuPointerCur > 0)
        cloudInfo.imuAvailable = true;
}

IMU 数据积分计算
根据角速度积分计算姿态变化：
$\theta_x(t) = \theta_x(t_0) + \int_{t_0}^{t} \omega_x(\tau) \, d\tau$
$\theta_y(t) = \theta_y(t_0) + \int_{t_0}^{t} \omega_y(\tau) \, d\tau$
$\theta_z(t) = \theta_z(t_0) + \int_{t_0}^{t} \omega_z(\tau) \, d\tau$

        // 获取角速度并积分计算姿态变化
        double angular_x, angular_y, angular_z;
        imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
        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;

里程计位姿增量计算

void odomDeskewInfo()
{
    // 初始化标志，表示里程计数据是否可用
    cloudInfo.odomAvailable = false;

    // 删除过早的里程计数据
    // 保留时间戳在扫描时间范围内的里程计数据
    while (!odomQueue.empty())
    {
        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;

    // 获取扫描开始时的位姿
    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;
    tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
    double roll, pitch, yaw;
    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

    // 记录扫描开始时的位姿
    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;

    // 获取扫描结束时的位姿
    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);

    Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
    pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

    // 设置位姿增量计算完成标志
    odomDeskewFlag = true;
}

位姿增量计算
起始和结束位姿变换矩阵分别为 $T_{\text{begin}}$ 和 $T_{\text{end}}$ ，位姿增量为：
$T_{\text{bt}} = T_{\text{begin}}^{-1} \cdot T_{\text{end}}$
$translation_and_euler ( T bt ) \Delta x, \Delta y, \Delta z, \Delta \theta_x, \Delta \theta_y, \Delta \theta_z = \text{translation\_and\_euler}(T_{\text{bt}})$

    // 计算位姿增量
    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);

    Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

四元数到欧拉角转换
利用四元数 $q = (x, y, z, w)$ ，将其转换为欧拉角 $(\phi, \theta, \psi)$ ：
$\phi = \arctan\left(\frac{2(q_w q_x + q_y q_z)}{1 - 2(q_x^2 + q_y^2)}\right)$
$\theta = \arcsin(2(q_w q_y - q_z q_x))$
$\psi = \arctan\left(\frac{2(q_w q_z + q_x q_y)}{1 - 2(q_y^2 + q_z^2)}\right)$

  pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

3. projectPointCloud

projectPointCloud 函数将输入的点云数据投影到二维范围图（range image）中，用于后续点云处理。其主要功能包括：

点云过滤：剔除范围超出传感器有效范围的点以及非目标扫描线的点。
角度计算：根据传感器类型计算水平角度或列索引。
范围图构建：将点云按照行列索引投影到范围图矩阵，同时处理点去畸变（deskew）。

void projectPointCloud()
{
    // 获取输入点云的大小
    int cloudSize = laserCloudIn->points.size();

    // 遍历每个点
    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;

        // 初始化列号
        int columnIdn = -1;

        // 根据激光雷达类型计算列号
        if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
        {
            // 计算水平角度
            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            // 计算列号
            static float ang_res_x = 360.0 / float(Horizon_SCAN);
            columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;
            // 修正列号，确保其在有效范围内
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;
        }
        else if (sensor == SensorType::LIVOX)
        {
            // 对于 Livox 激光雷达，使用预先计算的列号
            columnIdn = columnIdnCountVec[rowIdn];
            columnIdnCountVec[rowIdn] += 1;
        }

        // 如果列号不在有效范围内，跳过该点
        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;
    }
}

点的距离计算
点 $P = (x, y, z)$ 的距离（range）为：
$\sqrt{x^2 + y^2 + z^2}$

        // 创建一个点类型变量，用于存储当前点的信息
        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);

水平角度计算（Velodyne/Ouster）
对于点 $P = (x, y, z)$ ，水平角度为：
$\theta = \arctan\left(\frac{x}{y}\right) \cdot \frac{180}{\pi}$
列索引为：
$ang_res_x ⌉ + Horizon_SCAN 2 \text{col} = -\left\lceil\frac{\theta - 90}{\text{ang\_res\_x}}\right\rceil + \frac{\text{Horizon\_SCAN}}{2}$

            // 计算水平角度
            float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
            // 计算列号
            static float ang_res_x = 360.0 / float(Horizon_SCAN);
            columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;

投影矩阵更新
对于点 $\theta)$ ，如果矩阵位置 $(\text{row}, \text{col})$ 为空，则更新为：
$\text{rangeMat}[\text{row}, \text{col}] = r$

        // 更新范围图像中的距离值
        rangeMat.at<float>(rowIdn, columnIdn) = range;

        // 计算点在全点云中的索引
        int index = columnIdn + rowIdn * Horizon_SCAN;
        // 更新全点云中的点
        fullCloud->points[index] = thisPoint;

4. cloudExtraction

cloudExtraction 函数从范围图中提取有效点云数据，并存储在 extractedCloud 中。提取的点云数据用于激光里程计计算。主要功能包括：

记录起止索引：为每条激光扫描线记录点云在提取后的点云中的起始和结束索引。
点云提取：遍历范围图，将有效点（非 FLT_MAX）提取到新的点云中。
信息存储：保存点的列索引和距离信息，用于后续处理。

void cloudExtraction()
{
    int count = 0; // 初始化计数器，用于记录提取的点云大小

    // 提取分段点云用于激光雷达里程计
    for (int i = 0; i < N_SCAN; ++i)
    {
        // 记录当前扫描行的起始索引
        cloudInfo.startRingIndex[i] = count - 1 + 5;

        for (int j = 0; j < Horizon_SCAN; ++j)
        {
            // 检查当前点是否有效（距离值不为 FLT_MAX）
            if (rangeMat.at<float>(i, j) != FLT_MAX)
            {
                cloudInfo.pointColInd[count] = j;
                // 保存点的范围信息
                cloudInfo.pointRange[count] = rangeMat.at<float>(i, j);
                // 保存提取的点云
                extractedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);
                // 增加计数器，记录提取的点云大小
                ++count;
            }
        }
        // 记录当前扫描行的结束索引
        cloudInfo.endRingIndex[i] = count - 1 - 5;
    }
}