LIO-SAM代码解析：mapOptmization.cpp（一）

文章目录

  • 主流程
  • 1. `loopInfoHandler`
    • 1.1 `updateInitialGuess`
    • 1.2 `extractSurroundingKeyFrames`
    • 1.3 `downsampleCurrentScan`
    • 1.4 `scan2MapOptimization`
    • 1.5 `saveKeyFramesAndFactor`
    • 1.6 `correctPoses`
    • 1.7 `publishOdometry `
    • 1.8 `publishFrames`

主流程

在这里插入图片描述

1. loopInfoHandler

1.1 updateInitialGuess

updateInitialGuess() 函数用于更新位姿变换的初始猜测。初始化阶段，若不使用IMU方向初始化，则将位姿的偏航角设为0，否则将位姿设为IMU初始值，并保存IMU位姿。之后，若里程计可用，根据里程计初始猜测获取当前变换，若无上一帧IMU预积分位姿则直接保存，否则计算两帧间的增量位姿并与当前位姿变换相乘得到最终位姿，再提取平移和欧拉角并保存当前帧IMU预积分位姿及更新IMU位姿；若仅IMU可用，计算当前IMU位姿与上一帧IMU位姿的增量变换，应用该增量更新位姿并提取平移和欧拉角，最后保存当前帧IMU位姿。

void updateInitialGuess()
{
    // 保存当前位姿变换（增量位姿的初始状态）
    incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);

    static Eigen::Affine3f lastImuTransformation; // 保存上一帧IMU的位姿
    // 初始化阶段
    if (cloudKeyPoses3D->points.empty())
    {
        // 设置位姿为IMU初始值
        transformTobeMapped[0] = cloudInfo.imuRollInit;
        transformTobeMapped[1] = cloudInfo.imuPitchInit;
        transformTobeMapped[2] = cloudInfo.imuYawInit;

        // 如果不使用IMU方向初始化，则设置偏航角为0
        if (!useImuHeadingInitialization)
            transformTobeMapped[2] = 0;

        // 保存IMU位姿
        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
        return;
    }

    // 使用IMU预积分的估计值更新位姿
    static bool lastImuPreTransAvailable = false; // 标记是否有上一帧IMU预积分位姿
    static Eigen::Affine3f lastImuPreTransformation; // 保存上一帧的IMU预积分位姿
    if (cloudInfo.odomAvailable == true)
    {
        // 根据里程计初始猜测获取当前变换
        Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ, 
                                                           cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);

        if (lastImuPreTransAvailable == false)
        {
            // 如果没有上一帧预积分位姿，直接保存当前位姿
            lastImuPreTransformation = transBack;
            lastImuPreTransAvailable = true;
        } 
        else 
        {
            // 计算两帧之间的增量位姿
            Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
            // 将当前位姿变换和增量相乘得到最终位姿
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;

            // 提取最终位姿的平移和欧拉角
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            // 保存当前帧IMU预积分位姿
            lastImuPreTransformation = transBack;

            // 更新IMU位姿
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
            return;
        }
    }

    // 使用IMU增量估计位姿（仅旋转部分）
    if (cloudInfo.imuAvailable == true)
    {
        // 当前IMU位姿
        Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
        // 计算增量变换
        Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

        // 应用增量变换更新位姿
        Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
        Eigen::Affine3f transFinal = transTobe * transIncre;

        // 提取最终位姿的平移和欧拉角
        pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

        // 保存当前帧IMU位姿
        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
        return;
    }
}

(1) 里程计增量变换
当里程计数据可用时，通过前后两帧的增量位姿来修正全局位姿。里程计提供的信息包括当前帧和上一帧的平移和旋转，这些信息可以用变换矩阵来表示。

公式
Δ T = T previous − 1 ⋅ T current \Delta \mathbf{T} = \mathbf{T}_{\text{previous}}^{-1} \cdot \mathbf{T}_{\text{current}} ΔT=Tprevious−1​⋅Tcurrent​
T updated = T global ⋅ Δ T \mathbf{T}_{\text{updated}} = \mathbf{T}_{\text{global}} \cdot \Delta \mathbf{T} Tupdated​=Tglobal​⋅ΔT

原理

  • T previous \mathbf{T}_{\text{previous}} Tprevious​ 表示上一帧的全局位姿；
  • T current \mathbf{T}_{\text{current}} Tcurrent​ 表示当前帧的位姿；
  • Δ T \Delta \mathbf{T} ΔT 表示当前帧相对于上一帧的相对位姿，称为增量位姿。

通过增量位姿 Δ T \Delta \mathbf{T} ΔT 和上一帧的全局位姿 T global \mathbf{T}_{\text{global}} Tglobal​，可以计算出更新后的全局位姿 T updated \mathbf{T}_{\text{updated}} Tupdated​。这一步的物理意义是将两帧之间的相对运动累加到全局位姿中，得到当前帧在全局坐标系下的位置和姿态。

(2) IMU增量旋转
当里程计不可用时，使用IMU的旋转信息更新位姿。IMU提供了帧间旋转角度的测量值，主要包括横滚角（Roll）、俯仰角（Pitch）和偏航角（Yaw）。

公式
Δ R = R previous − 1 ⋅ R current \Delta \mathbf{R} = \mathbf{R}_{\text{previous}}^{-1} \cdot \mathbf{R}_{\text{current}} ΔR=Rprevious−1​⋅Rcurrent​
R updated = R global ⋅ Δ R \mathbf{R}_{\text{updated}} = \mathbf{R}_{\text{global}} \cdot \Delta \mathbf{R} Rupdated​=Rglobal​⋅ΔR

原理

  • R previous \mathbf{R}_{\text{previous}} Rprevious​ 表示上一帧IMU的旋转矩阵；
  • R current \mathbf{R}_{\text{current}} Rcurrent​ 表示当前帧IMU的旋转矩阵；
  • Δ R \Delta \mathbf{R} ΔR 表示IMU测量的旋转增量。

通过计算两帧之间的旋转增量 Δ R \Delta \mathbf{R} ΔR，可以将其叠加到全局旋转矩阵 R global \mathbf{R}_{\text{global}} Rglobal​ 中，得到更新后的全局旋转矩阵 R updated \mathbf{R}_{\text{updated}} Rupdated​。这一步的物理意义是利用IMU测得的帧间角度变化，细化当前帧的旋转估计。

(3) 输出位姿
最终，函数输出的位姿为六维向量形式，包含平移和旋转信息。

公式
T output = [ x y z r p y ] \mathbf{T}_{\text{output}} = \begin{bmatrix} x \\ y \\ z \\ r \\ p \\ y \end{bmatrix} Toutput​= ​xyzrpy​ ​

原理

  • 平移部分 ( x , y , z ) (x, y, z) (x,y,z)：表示点云在全局坐标系中的三维位置；
  • 旋转部分 ( r , p , y ) (r, p, y) (r,p,y)：用欧拉角表示点云的旋转状态。

无论使用里程计增量还是IMU增量，最终的目标是统一输出六维位姿，用于后续点云配准的初始位姿估计。

1.2 extractSurroundingKeyFrames

extractSurroundingKeyFrames 函数的主要功能是提取当前关键帧周围的历史关键帧点云，通过KD树和半径搜索结合时间筛选，生成局部地图；随后利用缓存机制和点云变换构建全局坐标下的角点和平面点地图，并通过降采样优化点云数据，最终为后续点云配准提供高效且精确的初始环境信息。

void extractSurroundingKeyFrames()
{
    // 如果没有关键帧点云，直接返回
    if (cloudKeyPoses3D->points.empty() == true)
        return;

    // 提取附近关键帧
    extractNearby();
}

void extractNearby()
{
    // 定义存储周围关键帧的点云
    pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());

    std::vector<int> pointSearchInd; // KD树搜索的点索引
    std::vector<float> pointSearchSqDis; // KD树搜索的平方距离

    // 使用KD树提取所有周围的关键帧并进行降采样
    kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // 设置KD树的输入点云
    kdtreeSurroundingKeyPoses->radiusSearch(
        cloudKeyPoses3D->back(), 
        (double)surroundingKeyframeSearchRadius, 
        pointSearchInd, 
        pointSearchSqDis); // 半径搜索，查找周围关键帧

    // 根据搜索结果提取关键帧
    for (int i = 0; i < (int)pointSearchInd.size(); ++i)
    {
        int id = pointSearchInd[i];
        surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
    }

    // 对提取的关键帧进行降采样
    downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
    downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);

    // 更新降采样关键帧点的强度信息
    for (auto& pt : surroundingKeyPosesDS->points)
    {
        kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
        pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
    }

    // 提取时间较近的关键帧，防止机器人原地旋转时丢失位姿
    int numPoses = cloudKeyPoses3D->size();
    for (int i = numPoses - 1; i >= 0; --i)
    {
        if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
            surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
        else
            break;
    }

    // 根据提取的关键帧更新地图点云
    extractCloud(surroundingKeyPosesDS);
}

代码核心原理

（1）提取周围关键帧

  • 逻辑：通过KD树搜索和半径搜索，从所有关键帧中提取周围关键帧，并对其进行降采样。
  • 关键公式：
    • 半径搜索：
      { p i ∣ d ( p i , p current ) ≤ R } \{ \mathbf{p}_i \mid d(\mathbf{p}_i, \mathbf{p}_{\text{current}}) \leq R \} {pi​∣d(pi​,pcurrent​)≤R}
      其中， p i \mathbf{p}_i pi​ 是关键帧点， p current \mathbf{p}_{\text{current}} pcurrent​ 是当前点， R R R 是搜索半径。

（2） 降采样与时间筛选

  • 逻辑：对周围的关键帧进行降采样，并保留时间上接近的关键帧，以应对原地旋转的情况。
  • 关键公式：
    • 时间筛选：
      { p i ∣ t current − t i < T } \{ \mathbf{p}_i \mid t_{\text{current}} - t_i < T \} {pi​∣tcurrent​−ti​<T}
      其中， t current t_{\text{current}} tcurrent​ 是当前时间， t i t_i ti​ 是关键帧的时间戳， T T T 是时间阈值。

（3）构建周围点云地图

  • 逻辑：根据提取的关键帧，从缓存或原始点云中提取角点和平面点，并转换到全局坐标系，融合生成周围点云地图。
  • 关键公式：
    • 位姿变换：
      p global = T key ⋅ p local \mathbf{p}_{\text{global}} = \mathbf{T}_{\text{key}} \cdot \mathbf{p}_{\text{local}} pglobal​=Tkey​⋅plocal​
      其中， T key \mathbf{T}_{\text{key}} Tkey​ 是关键帧的全局位姿， p local \mathbf{p}_{\text{local}} plocal​ 是点云的局部坐标。

（4）降采样优化

  • 逻辑：对角点和平面点地图分别进行降采样，以减少计算负担。
  • 目标：
    • 保留必要的点云特征，减少冗余点。

1.3 downsampleCurrentScan

downsampleCurrentScan 函数的主要功能是对当前扫描的角点和平面点点云分别进行降采样，以减少点云的冗余数据，从而提高点云处理的效率，为后续点云配准提供精简的输入数据。

void downsampleCurrentScan()
{
    // 对当前扫描的点云进行降采样

    // 清空降采样后的角点点云，并对角点点云进行降采样
    laserCloudCornerLastDS->clear(); // 清空存储降采样角点点云的容器
    downSizeFilterCorner.setInputCloud(laserCloudCornerLast); // 设置角点点云的输入
    downSizeFilterCorner.filter(*laserCloudCornerLastDS); // 对角点点云进行降采样
    laserCloudCornerLastDSNum = laserCloudCornerLastDS->size(); // 获取降采样后的角点数量

    // 清空降采样后的平面点点云，并对平面点点云进行降采样
    laserCloudSurfLastDS->clear(); // 清空存储降采样平面点点云的容器
    downSizeFilterSurf.setInputCloud(laserCloudSurfLast); // 设置平面点点云的输入
    downSizeFilterSurf.filter(*laserCloudSurfLastDS); // 对平面点点云进行降采样
    laserCloudSurfLastDSNum = laserCloudSurfLastDS->size(); // 获取降采样后的平面点数量
}

（1） 点云降采样

  • 逻辑：使用体素滤波（Voxel Grid Filter）对当前扫描的角点和平面点点云分别进行降采样，减少冗余点，提高点云处理效率。
  • 公式：
    • 降采样：
      p DS = VoxelGrid ( p ) \mathbf{p}_{\text{DS}} = \text{VoxelGrid}(\mathbf{p}) pDS​=VoxelGrid(p)
      其中， p \mathbf{p} p 为输入点云， p DS \mathbf{p}_{\text{DS}} pDS​ 为降采样后的点云。

（2）数据更新

  • 在完成降采样后，将降采样后的点云存储，并记录点云的点数。

1.4 scan2MapOptimization

scan2MapOptimization 函数的主要功能是通过角点和平面点特征优化，将当前帧的点云与地图进行配准，利用LM算法不断迭代优化位姿，并结合IMU信息对结果进行约束，为后续的建图和定位提供准确的位姿估计。

void scan2MapOptimization()
{
    // 如果没有关键帧点云，直接返回
    if (cloudKeyPoses3D->points.empty())
        return;

    // 确保当前帧的角点和平面点特征数量满足最低要求
    if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
    {
        // 设置KD树的输入点云，用于角点和平面点的匹配
        kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
        kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

        // 迭代优化，最多进行30次
        for (int iterCount = 0; iterCount < 30; iterCount++)
        {
            // 清空上一轮的匹配点和优化系数
            laserCloudOri->clear();
            coeffSel->clear();

            // 分别进行角点和平面点的优化
            cornerOptimization();
            surfOptimization();

            // 合并角点和平面点的优化系数
            combineOptimizationCoeffs();

            // 执行LM优化，判断是否收敛
            if (LMOptimization(iterCount) == true)
                break;
        }

        // 更新当前帧的位姿
        transformUpdate();
    } 
    else 
    {
        // 如果特征数量不足，输出警告信息
        ROS_WARN("Not enough features! Only %d edge and %d planar features available.", 
                  laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
    }
}

（1）. 角点和平面点优化：

  • 通过KD树匹配角点和平面点的邻域点，计算协方差矩阵，用特征值分析提取特征。
  • 角点优化：利用线性特征构造误差方程。
  • 平面点优化：利用平面特征拟合平面，并构造误差方程。

（2）. LM优化：

  • 利用Levenberg-Marquardt算法迭代优化当前帧的位姿，通过增量更新获得收敛解。

（3）. 位姿更新：

  • 优化完成后，结合IMU数据对位姿进行约束更新，确保最终位姿合理性。

1.5 saveKeyFramesAndFactor

saveKeyFramesAndFactor 函数的主要功能是判断是否需要保存当前帧为关键帧，并在因子图中添加相应的约束（里程计因子、GPS因子和回环因子）；随后利用iSAM进行优化，提取优化后的关键帧位姿并保存，同时存储当前帧的角点和平面点点云作为关键帧点云，并更新路径信息以便可视化显示。

void saveKeyFramesAndFactor()
{
    // 判断是否需要保存关键帧，如果不需要则直接返回
    if (saveFrame() == false)
        return;

    // 添加里程计因子
    addOdomFactor();

    // 添加GPS因子
    addGPSFactor();

    // 添加回环因子
    addLoopFactor();

    // 更新iSAM优化图
    isam->update(gtSAMgraph, initialEstimate);
    isam->update();

    // 如果发生了回环闭合，多次更新以确保结果收敛
    if (aLoopIsClosed == true)
    {
        isam->update();
        isam->update();
        isam->update();
        isam->update();
        isam->update();
    }

    // 清空因子图和初始估计值
    gtSAMgraph.resize(0);
    initialEstimate.clear();

    // 保存当前关键帧的位姿
    PointType thisPose3D;
    PointTypePose thisPose6D;
    Pose3 latestEstimate;

    // 从iSAM中获取优化后的最新位姿
    isamCurrentEstimate = isam->calculateEstimate();
    latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);

    // 提取平移信息并存储为3D点
    thisPose3D.x = latestEstimate.translation().x();
    thisPose3D.y = latestEstimate.translation().y();
    thisPose3D.z = latestEstimate.translation().z();
    thisPose3D.intensity = cloudKeyPoses3D->size(); // 使用关键帧的索引作为强度值
    cloudKeyPoses3D->push_back(thisPose3D);

    // 提取旋转信息并存储为6D位姿
    thisPose6D.x = thisPose3D.x;
    thisPose6D.y = thisPose3D.y;
    thisPose6D.z = thisPose3D.z;
    thisPose6D.intensity = thisPose3D.intensity; // 索引值
    thisPose6D.roll  = latestEstimate.rotation().roll();
    thisPose6D.pitch = latestEstimate.rotation().pitch();
    thisPose6D.yaw   = latestEstimate.rotation().yaw();
    thisPose6D.time = timeLaserInfoCur; // 当前激光帧时间戳
    cloudKeyPoses6D->push_back(thisPose6D);

    // 计算并保存当前关键帧的协方差
    poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);

    // 更新当前位姿
    transformTobeMapped[0] = latestEstimate.rotation().roll();
    transformTobeMapped[1] = latestEstimate.rotation().pitch();
    transformTobeMapped[2] = latestEstimate.rotation().yaw();
    transformTobeMapped[3] = latestEstimate.translation().x();
    transformTobeMapped[4] = latestEstimate.translation().y();
    transformTobeMapped[5] = latestEstimate.translation().z();

    // 保存当前帧的角点和平面点点云
    pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
    pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
    pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);

    // 将当前帧的点云存储为关键帧点云
    cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
    surfCloudKeyFrames.push_back(thisSurfKeyFrame);

    // 更新路径信息以便可视化
    updatePath(thisPose6D);
}

核心原理

  1. 关键帧判断与因子添加：

    • 根据保存关键帧的条件，判断当前帧是否需要存储。
    • 添加里程计因子、GPS因子和回环因子，构建完整的因子图。

  2. iSAM优化：

    • 利用iSAM进行因子图优化，获取全局最优位姿。
    • 若检测到回环闭合，多次更新优化图以提高结果的稳定性。

  3. 关键帧保存：

    • 从iSAM中提取最新的关键帧位姿，包括平移和旋转信息。
    • 保存优化后的位姿为3D点（平移）和6D位姿（平移+旋转）。

  4. 点云保存：

    • 将当前帧的角点和平面点降采样点云存储为关键帧点云，供后续配准使用。

  5. 路径更新：

    • 将最新的关键帧位姿添加到路径信息中，用于可视化显示。

1.6 correctPoses

correctPoses 函数的主要功能是在检测到回环闭合时，利用 iSAM 优化器的结果更新所有关键帧的位姿信息（包括平移和旋转），同时清空地图缓存和全局路径，重新生成优化后的全局路径，用于后续的定位和可视化显示。

void correctPoses()
{
    // 如果关键帧点云为空，直接返回
    if (cloudKeyPoses3D->points.empty())
        return;

    // 如果发生了回环闭合
    if (aLoopIsClosed == true)
    {
        // 清空地图缓存
        laserCloudMapContainer.clear();
        // 清空全局路径
        globalPath.poses.clear();
        // 更新关键帧的位姿
        int numPoses = isamCurrentEstimate.size();
        for (int i = 0; i < numPoses; ++i)
        {
            // 更新3D关键帧的平移信息
            cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
            cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
            cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();

            // 更新6D关键帧的平移和旋转信息
            cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
            cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
            cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
            cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
            cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
            cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();

            // 更新全局路径，用于可视化
            updatePath(cloudKeyPoses6D->points[i]);
        }

        // 标志回环处理完成
        aLoopIsClosed = false;
    }
}

  1. 回环闭合校正：

    • 当检测到回环闭合时，通过优化结果更新所有关键帧的位姿，包括平移和旋转信息。

  2. 关键帧位姿更新：

    • 从 iSAM 优化器中获取优化后的位姿，分别更新3D关键帧点云的平移信息和6D关键帧点云的平移与旋转信息。

  3. 全局路径更新：

    • 根据优化后的6D关键帧位姿，重新生成全局路径，确保路径数据与优化结果一致。

1.7 publishOdometry

publishOdometry 函数的主要功能是将当前帧的全局和增量位姿发布为 ROS 里程计消息，并发送对应的 TF 变换。同时结合 IMU 数据对位姿进行优化，为后续模块提供实时的全局和增量位姿信息，支持导航、建图和定位任务。

void publishOdometry()
{
    // 发布全局里程计信息（global odometry for ROS）
    nav_msgs::Odometry laserOdometryROS;
    laserOdometryROS.header.stamp = timeLaserInfoStamp; // 时间戳
    laserOdometryROS.header.frame_id = odometryFrame; // 参考坐标系
    laserOdometryROS.child_frame_id = "odom_mapping"; // 子坐标系
    laserOdometryROS.pose.pose.position.x = transformTobeMapped[3]; // 平移x
    laserOdometryROS.pose.pose.position.y = transformTobeMapped[4]; // 平移y
    laserOdometryROS.pose.pose.position.z = transformTobeMapped[5]; // 平移z
    laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(
        transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); // 旋转（四元数）
    pubLaserOdometryGlobal.publish(laserOdometryROS); // 发布全局里程计消息

    // 发布TF变换
    static tf::TransformBroadcaster br;
    tf::Transform t_odom_to_lidar = tf::Transform(
        tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
        tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
    tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(
        t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
    br.sendTransform(trans_odom_to_lidar); // 发送TF变换

    // 发布增量里程计信息（incremental odometry for ROS）
    static bool lastIncreOdomPubFlag = false; // 标记是否为第一次发布
    static nav_msgs::Odometry laserOdomIncremental; // 增量里程计消息
    static Eigen::Affine3f increOdomAffine; // 增量里程计仿射变换
    if (lastIncreOdomPubFlag == false)
    {
        // 初始化增量里程计
        lastIncreOdomPubFlag = true;
        laserOdomIncremental = laserOdometryROS;
        increOdomAffine = trans2Affine3f(transformTobeMapped);
    } 
    else 
    {
        // 计算增量里程计
        Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
        increOdomAffine = increOdomAffine * affineIncre;

        // 提取平移和欧拉角
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(increOdomAffine, x, y, z, roll, pitch, yaw);

        // 与IMU数据融合
        if (cloudInfo.imuAvailable == true)
        {
            if (std::abs(cloudInfo.imuPitchInit) < 1.4)
            {
                double imuWeight = 0.1; // IMU权重
                tf::Quaternion imuQuaternion;
                tf::Quaternion transformQuaternion;
                double rollMid, pitchMid, yawMid;

                // 进行roll方向的球面线性插值（slerp）
                transformQuaternion.setRPY(roll, 0, 0);
                imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                roll = rollMid;

                // 进行pitch方向的球面线性插值（slerp）
                transformQuaternion.setRPY(0, pitch, 0);
                imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                pitch = pitchMid;
            }
        }

        // 填充增量里程计消息
        laserOdomIncremental.header.stamp = timeLaserInfoStamp; // 时间戳
        laserOdomIncremental.header.frame_id = odometryFrame; // 参考坐标系
        laserOdomIncremental.child_frame_id = "odom_mapping"; // 子坐标系
        laserOdomIncremental.pose.pose.position.x = x; // 平移x
        laserOdomIncremental.pose.pose.position.y = y; // 平移y
        laserOdomIncremental.pose.pose.position.z = z; // 平移z
        laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); // 旋转
        laserOdomIncremental.pose.covariance[0] = isDegenerate ? 1 : 0; // 是否退化
    }

    // 发布增量里程计消息
    pubLaserOdometryIncremental.publish(laserOdomIncremental);
}

  1. 全局里程计发布：

    • 将优化后的位姿（包括平移和旋转信息）封装为 nav_msgs::Odometry 消息，并通过 ROS 话题发布。
    • 还发布对应的 TF 变换，以支持其他模块或工具的实时定位需求。

  2. 增量里程计计算与发布：

    • 通过计算两次优化之间的增量变换，逐帧累积得到增量里程计。
    • 对增量位姿进行 IMU 数据融合，进一步优化位姿估计。
    • 将增量位姿封装为消息并发布。

  3. IMU数据融合：

    • 使用球面线性插值（Slerp）融合激光雷达和 IMU 的 Roll 和 Pitch 信息，增强位姿估计的精确度。

1.8 publishFrames

publishFrames 函数的主要功能是通过 ROS 话题发布关键帧点云、周围关键帧点云、当前帧配准点云和高分辨率原始点云，同时更新并发布全局路径信息，并向第三方模块提供包含关键帧点云、关键帧位姿和局部地图点云的 SLAM 信息，以支持实时定位和可视化需求。

void publishFrames()
{
    // 如果关键帧点云为空，直接返回
    if (cloudKeyPoses3D->points.empty())
        return;

    // 发布关键帧位姿
    publishCloud(pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);

    // 发布周围关键帧点云
    publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);

    // 发布配准后的当前关键帧点云
    if (pubRecentKeyFrame.getNumSubscribers() != 0)
    {
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
        PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
        *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D); // 转换角点
        *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);   // 转换平面点
        publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
    }

    // 发布高分辨率未滤波的原始点云
    if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
    {
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
        pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut); // 从ROS消息转换为点云
        PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
        *cloudOut = *transformPointCloud(cloudOut, &thisPose6D); // 转换为全局坐标
        publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
    }

    // 发布全局路径
    if (pubPath.getNumSubscribers() != 0)
    {
        globalPath.header.stamp = timeLaserInfoStamp;
        globalPath.header.frame_id = odometryFrame;
        pubPath.publish(globalPath);
    }

    // 发布SLAM相关信息，供第三方使用
    static int lastSLAMInfoPubSize = -1; // 上次发布的SLAM信息大小
    if (pubSLAMInfo.getNumSubscribers() != 0)
    {
        if (lastSLAMInfoPubSize != cloudKeyPoses6D->size())
        {
            lio_sam::cloud_info slamInfo;
            slamInfo.header.stamp = timeLaserInfoStamp;

            // 发布当前关键帧点云
            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
            *cloudOut += *laserCloudCornerLastDS; // 添加角点
            *cloudOut += *laserCloudSurfLastDS;   // 添加平面点
            slamInfo.key_frame_cloud = publishCloud(ros::Publisher(), cloudOut, timeLaserInfoStamp, lidarFrame);

            // 发布关键帧位姿
            slamInfo.key_frame_poses = publishCloud(ros::Publisher(), cloudKeyPoses6D, timeLaserInfoStamp, odometryFrame);

            // 发布局部地图点云
            pcl::PointCloud<PointType>::Ptr localMapOut(new pcl::PointCloud<PointType>());
            *localMapOut += *laserCloudCornerFromMapDS; // 添加局部角点
            *localMapOut += *laserCloudSurfFromMapDS;   // 添加局部平面点
            slamInfo.key_frame_map = publishCloud(ros::Publisher(), localMapOut, timeLaserInfoStamp, odometryFrame);

            // 发布SLAM信息
            pubSLAMInfo.publish(slamInfo);
            lastSLAMInfoPubSize = cloudKeyPoses6D->size(); // 更新发布大小
        }
    }
}

  1. 关键帧发布：

    • 发布关键帧的位姿点云 (pubKeyPoses)。
    • 发布周围关键帧的降采样点云 (pubRecentKeyFrames)。
    • 将当前帧的角点和平面点转换到全局坐标后，发布配准结果 (pubRecentKeyFrame)。

  2. 高分辨率点云发布：

    • 将去畸变的原始点云转换为全局坐标后，发布高分辨率的未滤波点云 (pubCloudRegisteredRaw)。

  3. 路径和SLAM信息发布：

    • 发布全局路径信息 (pubPath)。
    • 发布包含当前帧点云、关键帧位姿和局部地图点云的 SLAM 信息，供第三方模块使用 (pubSLAMInfo)。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/952423.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Django学习笔记之数据库（一）

Django学习笔记之数据库（一）

文章目录 安装一、数据库配置二、基本操作步骤1.增加2.查看3.排序4.更新5.删除数据 三、一对多&#xff0c;多对多&#xff0c;一对一1.一对多1.一对一1.多对多 四、查询操作五、聚合操作六、F和Q操作 安装 首先就是安装Mysql和Navicat。 一、数据库配置 其实整个就是连接前端…
阅读更多...
《分布式光纤传感：架设于桥梁监测领域的 “智慧光网” 》

《分布式光纤传感：架设于桥梁监测领域的 “智慧光网” 》

桥梁作为交通基础设施的重要组成部分&#xff0c;其结构健康状况直接关系到交通运输的安全和畅通。随着桥梁建设规模的不断扩大和服役年限的增长&#xff0c;桥梁结构的安全隐患日益凸显&#xff0c;传统的监测方法已难以满足对桥梁结构健康实时、全面、准确监测的需求。分布式…
阅读更多...
什么是顶级思维？

什么是顶级思维？

在现代社会&#xff0c;我们常常听到“顶级思维”这个概念&#xff0c;但究竟什么才是顶级思维&#xff1f;它又是如何影响一个人的成功和幸福呢&#xff1f;今天&#xff0c;我们就来探讨一下顶级思维的几个关键要素&#xff0c;并分享一些实用的生活哲学。 1. 身体不适&…
阅读更多...
更新Office后，LabVIEW 可执行程序生成失败

更新Office后，LabVIEW 可执行程序生成失败

问题描述&#xff1a; 在计算机中&#xff0c;LabVIEW 开发的源程序运行正常&#xff0c;但在生成可执行程序时提示以下错误&#xff1a; ​ A VI broke during the build process from being saved without a block diagram. Either open the build specification to include…
阅读更多...
Domain Adaptation（李宏毅）机器学习 2023 Spring HW11 （Boss Baseline）

Domain Adaptation（李宏毅）机器学习 2023 Spring HW11 （Boss Baseline）

1. 领域适配简介 领域适配是一种迁移学习方法，适用于源领域和目标领域数据分布不同但学习任务相同的情况。具体而言，我们在源领域（通常有大量标注数据）训练一个模型，并希望将其应用于目标领域（通常只有少量或没有标注数据）。然而，由于这两个领域的数据分布不同，模型在…
阅读更多...
25年无人机行业资讯 | 1.1 - 1.5

25年无人机行业资讯 | 1.1 - 1.5

25年无人机行业资讯 | 1.1 - 1.5 中央党报《经济日报》刊文&#xff1a;低空经济蓄势待发&#xff0c;高质量发展需的平衡三大关系 据新华网消息&#xff0c;2025年1月3日&#xff0c;中央党报《经济日报》发表文章指出&#xff0c;随着国家发展改革委低空经济发展司的成立&a…
阅读更多...
AI刷题-数位长度筛选问题、数值生成与运算问题

AI刷题-数位长度筛选问题、数值生成与运算问题

目录 一、数位长度筛选问题 问题描述 测试样例 解题思路&#xff1a; 问题理解 数据结构选择 算法步骤 关键点 最终代码&#xff1a; 运行结果&#xff1a; 二、数值生成与运算问题 问题描述 测试样例 解题思路&#xff1a; 问题理解 数据结构选择 算法步骤…
阅读更多...
Qiskit快速编程探索（进阶篇）

Qiskit快速编程探索（进阶篇）

五、量子电路模拟：探索量子世界的虚拟实验室 5.1 Aer模拟器：强大的模拟引擎 在量子计算的探索旅程中，Aer模拟器作为Qiskit的核心组件之一，宛如一座功能强大的虚拟实验室，为开发者提供了在经典计算机上模拟量子电路运行的卓越能力。它打破了硬件条件的限制，使得研究者无…
阅读更多...
转运机器人在物流仓储行业的优势特点

转运机器人在物流仓储行业的优势特点

在智能制造与智慧物流的浪潮中&#xff0c;一款革命性的产品正悄然改变着行业的面貌——富唯智能转运机器人&#xff0c;它以卓越的智能科技与创新的设计理念&#xff0c;引领着物流领域步入一个全新的高效、智能、无人的时代。 一、解放双手&#xff0c;重塑物流生态 富唯智能…
阅读更多...
开源项目stable-diffusion-webui部署及生成照片

开源项目stable-diffusion-webui部署及生成照片

参考链接 https://www.freedidi.com/13133.html 基础环境部署 python 官网链接 Python Release Python 3.10.6 | Python.org 下载 Python 3.10.6 版本安装包 下载好后双击 点击安装&#xff0c;这里需要选择一下&#xff0c;把环境变量加上。&#xff08;这里是默认安装到C盘…
阅读更多...
Linux：进程概念（二.查看进程、父进程与子进程、进程状态详解）

Linux：进程概念（二.查看进程、父进程与子进程、进程状态详解）

目录 1. 查看进程 1.1 准备工作 1.2 指令&#xff1a;ps—显示当前系统中运行的进程信息 1.3 查看进程属性 1.4 通过 /proc 系统文件夹看进程 2. 父进程与子进程 2.1 介绍 2.2 getpid() \getppid() 2.3 fork()函数—通过系统调用创建进程 fork()函数疑问 3. 进程状态…
阅读更多...
STM32 : PWM 基本结构

STM32 : PWM 基本结构

这张图展示了PWM&#xff08;脉冲宽度调制&#xff09;的基本结构和工作流程。PWM是一种用于控制功率转换器输出电压的技术&#xff0c;通过调整信号的占空比来实现对负载的精确控制。以下是详细讲解&#xff1a; PWM 基本结构 1. 时基单元 ARR (Auto-reload register): 自动…
阅读更多...
ElasticSearch 认识和安装ES

ElasticSearch 认识和安装ES

文章目录 一、为什么学ElasticSearch?1.ElasticSearch 简介2.ElasticSearch 与传统数据库的对比3.ElasticSearch 应用场景4.ElasticSearch 技术特点5.ElasticSearch 市场表现6.ElasticSearch 的发展 二、认识和安装ES1.认识 Elasticsearch&#xff08;简称 ES&#xff09;2.El…
阅读更多...
如何用 ESP32-CAM 做一个实时视频流服务器

如何用 ESP32-CAM 做一个实时视频流服务器

文章目录 ESP32-CAM 概述ESP32-S 处理器内存Camera 模块MicroSD 卡槽天线板载 LED 和闪光灯其他数据手册和原理图ESP32-CAM 功耗 ESP32-CAM 引脚参考引脚排列GPIO 引脚哪些 GPIO 可以安全使用&#xff1f;GPIO 0 引脚MicroSD 卡引脚 ESP32-CAM 的烧录方式使用 ESP32-CAM-MB 编程…
阅读更多...
UE5中制作地形材质

UE5中制作地形材质

在创作大场景内容时&#xff0c;地形的设计和优化是至关重要的一步。利用UE中的地形系统&#xff0c;开发者能够高效地创建复杂的地形形态&#xff0c;同时保持游戏的性能和视觉效果。 1.在创建地形之前&#xff0c;先新建一个地形使用的混合材质球&#xff0c;添加节点Landsc…
阅读更多...
通过 route 或 ip route 管理Linux主机路由

通过 route 或 ip route 管理Linux主机路由

目录 一&#xff1a;route 使用说明1、查看路由信息2、删除指定路由3、增加指定路由 二&#xff1a;ip route 使用说明1、查看主机路由2、新增主机路由3、删除主机路由 通过route 或者ip route修改Linux主机路由后属于临时生效&#xff0c;系统重启后就恢复默认值了&#xff0c…
阅读更多...
数据结构C语言描述11(图文结合)--二叉搜索树(BST树)的实现(数据采用KV存储形式进行封装)

数据结构C语言描述11(图文结合)--二叉搜索树(BST树)的实现(数据采用KV存储形式进行封装)

前言 这个专栏将会用纯C实现常用的数据结构和简单的算法&#xff1b;有C基础即可跟着学习&#xff0c;代码均可运行&#xff1b;准备考研的也可跟着写&#xff0c;个人感觉&#xff0c;如果时间充裕&#xff0c;手写一遍比看书、刷题管用很多&#xff0c;这也是本人采用纯C语言…
阅读更多...
rabbitmq的三个交换机及简单使用

rabbitmq的三个交换机及简单使用

提前说一下&#xff0c;创建队列&#xff0c;交换机&#xff0c;绑定交换机和队列都是在生产者。消费者只负责监听就行了&#xff0c;不用配其他的。 完成这个场景需要两个服务哦。 1直连交换机-生产者的代码。 在配置类中创建队列&#xff0c;交换机&#xff0c;绑定交换机…
阅读更多...
【数据库系统概论】第6章 （二）范式（重点讲在函数依赖范畴内）

【数据库系统概论】第6章 （二）范式（重点讲在函数依赖范畴内）

目录 第一范式&#xff08;1NF&#xff09; 第二范式&#xff08;2NF&#xff09; 第三范式&#xff08;3NF&#xff09; BC范式&#xff08;BCNF&#xff09; 多值依赖 第四范式&#xff08;4NF&#xff09; 范式&#xff08;Normalization&#xff09; 是一种结构化的设…
阅读更多...
Supermaven 加入 Cursor：AI 编码新篇章

Supermaven 加入 Cursor：AI 编码新篇章

引言 2024 年 11 月 11 日&#xff0c;我们迎来了一个激动人心的时刻——Supermaven 正式加入 Cursor&#xff01; 这一合作标志着 AI 编程工具进入了一个新的发展阶段&#xff0c;为开发者提供更智能、更高效的编码体验。本文将带您了解此次合并的背景、意义以及未来的发展方…
阅读更多...
最新文章

Copyright @ 2022~2023