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;
}
IMU 预积分在两个关键帧之间的目标
最终，关键帧如下图所示，从每次传感器获取的帧中按一定间隔筛选出来。在原论文中，相邻的两个关键帧分别标记为 i 和 j。因此，预积分的问题定义可以总结为：“如何将两个关键帧之间的几十到数百个 IMU 数据（图中的 x 标记）表示为一个因子（图中的蓝色 ■）”。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
