IMU 预积分- 2. 预备知识 (1) 关键帧

创作时间:

作者:

@小白创作中心

IMU 预积分- 2. 预备知识 (1) 关键帧

引用

CSDN

https://blog.csdn.net/qq_40301351/article/details/142097881

IMU预积分是SLAM（同时定位与地图构建）系统中的关键技术之一，用于处理IMU（惯性测量单元）数据。在进行IMU预积分之前，理解关键帧的概念至关重要。本文将详细介绍关键帧的定义、选择关键帧的原因，并通过代码示例展示LIO-SAM和VINS-Mono中关键帧生成的具体实现。

IMU 预积分推导说明系列

IMU 预积分 - 1. 介绍
IMU 预积分 - 2. 预备知识 (1) 关键帧
IMU 预积分 - 2. 预备知识 (2) 3D 旋转与不确定性
IMU 预积分 - 3. IMU 模型推导与运动积分
IMU 预积分 - 4. IMU 预积分测量值推导
IMU 预积分 - 5. LIO-SAM 中的 IMU 预积分

文章来源主要是翻译Hyungtae Lim的博客。想看原文的直接看原文。

在理解预积分之前，我们首先需要了解什么是关键帧。关键帧，顾名思义就是“重要的（key）帧”，在这里，“重要”意味着，在估计两帧之间的相对位姿时，能够利用周围环境的几何特性，通过丰富的、可靠且可重复的特征，进行足够准确的位姿估计。

之所以需要选择关键帧，主要原因是如果使用传感器的所有数据进行 SLAM，将需要大量的内存。例如，3D LiDAR 传感器通常以 10 Hz 的频率获取 3D 点云，而相机传感器的频率大约为 30 Hz。如果假设使用 LiDAR 传感器获取了一小时的大城市环境数据，那么将获得大约 36,000 帧。每帧由大约 10 万个点组成（以 64 通道为例，Velodyne HDL 64E 每帧约获得 130,000 个点）。如果将这些原始数据解析为 (x, y, z) 格式，每个点需要 3 个浮点数存储。大概估算一下，存储这些原始数据需要约 36,000 * 130,000 * 3 * 4，大约 56 GB 的内存（这个计算只是为了帮助理解，并不准确，实际上在 SLAM 中会使用体素化等技术只保存必要的点云数据）。

因此，为了解决这个问题，大多数 SLAM 算法会采用以下策略：
a) 当相对于前一个关键帧，机器人已经移动了足够的距离（但这个距离不能太大，过大的关键帧间距会导致位姿估计的准确性下降）。
b) 当相对于前一个关键帧，经过了足够的时间时，会生成下一个关键帧。

这种关键帧生成的方法与传感器的配置无关，无论是 LiDAR 惯性里程计 (LIO) 还是视觉惯性里程计 (VIO)，都采用类似的方式。以下是 LIO-SAM 和 VINS-Mono 中关键帧生成部分的代码示例，用注释的方式标注了情况 a 和情况 b。

在 LIO-SAM 中 (

saveFrame()
函数和
laserCloudInfoHandler
回调函数位于
mapOptmization.cpp
)

saveFrame()
函数
saveFrame()
主要用于确定是否需要保存当前激光帧作为关键帧。它通常通过比较当前帧与上一关键帧之间的距离或时间，决定是否生成新的关键帧。具体过程包括以下步骤：

位置检查：
检查当前帧与最后一个关键帧的平移距离是否超过阈值。如果超过，则认为机器人已经移动了足够远，可以将当前帧保存为新的关键帧。

时间检查：
如果没有足够的移动，检查从上一关键帧以来的时间是否超过了预设阈值。如果时间足够长，则保存当前帧为新的关键帧。

关键帧保存：
一旦满足上述任一条件，当前帧将被保存为新的关键帧，并用于后续的优化和位姿估计。

bool saveFrame()
{
    if (cloudKeyPoses3D->points.empty())
        return true;
    if (sensor == SensorType::LIVOX)
    {
        if (timeLaserInfoCur - cloudKeyPoses6D->back().time > 1.0)
            return true;
    }
    Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
    Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                        transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
    Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
    // - - - - - - - - - - - - - - - - - - - - - - -
    // HERE!!!! An example of the case (a)
    // Check the magnitude of the relative movement
    // - - - - - - - - - - - - - - - - - - - - - - -
    if (abs(roll)  < surroundingkeyframeAddingAngleThreshold &&
        abs(pitch) < surroundingkeyframeAddingAngleThreshold && 
        abs(yaw)   < surroundingkeyframeAddingAngleThreshold &&
        sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
        return false;
    return true;
}

laserCloudInfoHandler
回调函数
laserCloudInfoHandler
是
mapOptmization.cpp
中的一个回调函数，它用于处理激光雷达帧的数据。该函数主要负责：

接收并处理点云数据：
从激光雷达传感器接收当前帧的点云数据并进行处理。

特征提取：
从点云中提取用于后续优化的特征点。

融合 IMU 数据：
利用惯性传感器(IMU)的数据来增强点云的处理精度。

调用
saveFrame()
函数：
在处理完当前帧的点云信息后，决定是否将该帧保存为关键帧。

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
    // extract time stamp
    timeLaserInfoStamp = msgIn->header.stamp;
    timeLaserInfoCur = msgIn->header.stamp.toSec();
    // extract info and feature cloud
    cloudInfo = *msgIn;
    pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);
    pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
    std::lock_guard<std::mutex> lock(mtx);
    // - - - - - - - - - - - - - - - - - - 
    // HERE!!!! An example of the case (b)
    // - - - - - - - - - - - - - - - - - - 
    static double timeLastProcessing = -1;
    if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
    {
        timeLastProcessing = timeLaserInfoCur;
        updateInitialGuess();
        extractSurroundingKeyFrames();
        downsampleCurrentScan();
        scan2MapOptimization();
        saveKeyFramesAndFactor();
        correctPoses();
        publishOdometry();
        publishFrames();
    }
}

在 VINS-Mono 中 (

process()
函数位于
pose_graph_node.cpp
)

process()
函数
process()
函数是 VINS-Mono 中位姿图优化的一部分，位于
pose_graph_node.cpp
文件中。它的主要功能是处理姿态图（pose graph），并进行全局位姿优化。

主要流程：

接收关键帧数据：
process()
函数会接收来自前端的关键帧信息，包括关键帧的位姿、IMU 数据和其他传感器数据。
位姿图更新：
函数会将接收到的关键帧添加到位姿图中，并根据已知的运动模型和传感器数据，更新各个关键帧之间的相对位姿。
全局优化：
使用图优化方法（如 BA，Bundle Adjustment），对整个位姿图进行全局优化，确保所有关键帧和位姿之间的关系在全局范围内是一致的。
回环检测和闭环优化：
在此函数中，还会执行回环检测（Loop Closure）以检测是否存在闭环，并在闭环时进行全局闭环优化，以减少累计误差（drift）。
关键帧保存和发布：
最后，经过优化的关键帧和位姿会被保存，并发布给系统的其他模块使用，如地图构建和定位。

Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
                        pose_msg->pose.pose.position.y,
                        pose_msg->pose.pose.position.z);
Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
                            pose_msg->pose.pose.orientation.x,
                            pose_msg->pose.pose.orientation.y,
                            pose_msg->pose.pose.orientation.z).toRotationMatrix();
// - - - - - - - - - - - - - - - - - - - - - - -
// HERE!!!! An example of the case (a)
// Check the magnitude of the relative translation
// - - - - - - - - - - - - - - - - - - - - - - -
if((T - last_t).norm() > SKIP_DIS)
{
    vector<cv::Point3f> point_3d; 
    vector<cv::Point2f> point_2d_uv; 
    vector<cv::Point2f> point_2d_normal;
    vector<double> point_id;
    for (unsigned int i = 0; i < point_msg->points.size(); i++)
    {
        cv::Point3f p_3d;
        p_3d.x = point_msg->points[i].x;
        p_3d.y = point_msg->points[i].y;
        p_3d.z = point_msg->points[i].z;
        point_3d.push_back(p_3d);
        cv::Point2f p_2d_uv, p_2d_normal;
        double p_id;
        p_2d_normal.x = point_msg->channels[i].values[0];
        p_2d_normal.y = point_msg->channels[i].values[1];
        p_2d_uv.x = point_msg->channels[i].values[2];
        p_2d_uv.y = point_msg->channels[i].values[3];
        p_id = point_msg->channels[i].values[4];
        point_2d_normal.push_back(p_2d_normal);
        point_2d_uv.push_back(p_2d_uv);
        point_id.push_back(p_id);
        //printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);
    }
    KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
                        point_3d, point_2d_uv, point_2d_normal, point_id, sequence);   
    m_process.lock();
    start_flag = 1;
    posegraph.addKeyFrame(keyframe, 1);
    m_process.unlock();
    frame_index++;
    last_t = T;
}

// - - - - - - - - - - - - - - - - - - 
// HERE!!!! An example of the case (b)
// - - - - - - - - - - - - - - - - - - 
if (skip_cnt < SKIP_CNT)
{
    skip_cnt++;
    continue;
}
else
{
    skip_cnt = 0;
}