- IMU 预积分 - 1. 介绍
- IMU 预积分 - 2. 预备知识 (1) 关键帧
- IMU 预积分 - 2. 预备知识 (2) 3D 旋转与不确定性
- IMU 预积分 - 3. IMU 模型推导与运动积分
- IMU 预积分 - 4. IMU 预积分测量值推导
- IMU 预积分 - 5. LIO-SAM 中的 IMU 预积分
LIO-SAM中IMU预积分的逐行解释
过程
IMU预积分代码主要由三部分组成:a) 构造函数,b) imuHandler
回调函数,c) odometryHandler
回调函数。
a) 构造函数 (Constructor)
首先,执行代码时会设置给定的订阅器、发布器、参数以及gtsam::PreintegratedImuMeasurements
。代码原文如下:
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// integrate this single imu message
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
// predict odometry
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
// publish odometry
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame;
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
odometry.pose.pose.position.x = lidarPose.translation().x();
odometry.pose.pose.position.y = lidarPose.translation().y();
odometry.pose.pose.position.z = lidarPose.translation().z();
odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
odometry.twist.twist.linear.x = currentState.velocity().x();
odometry.twist.twist.linear.y = currentState.velocity().y();
odometry.twist.twist.linear.z = currentState.velocity().z();
odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
pubImuOdometry.publish(odometry);
}
在过程2中,为什么在此处返回并结束呢?这是因为bias项还没有通过优化被估计出来。回到公式再看看,当代码第一次运行时,下面红框中的值还没有被更新:
因此,在biases不准确的情况下,通过integrateMeasurement()
函数来预测位置,其结果会非常不准确。因此,在odometryHandler
回调中,第一次进行优化之前,只会存储IMU数据,而不会进行预测。(顺便说一句,如果将 if (doneFirstOpt == false)
这行代码注释掉,你会发现位姿会发散并"飞向太空"。)
参考上图中的
η
a
d
(
t
)
\eta^{a_d}(t)
ηad(t)
η
g
d
(
t
)
\eta^{g_d}(t)
ηgd(t)分别对应于构造函数中分配的 p->accelerometerCovariance
和 p->gyroscopeCovariance
。
c) odometryHandler 回调
odomHandler
回调函数是在 LIO-SAM 运行时,通过 ImageProjection→FeatureExtraction→MapOptimization 等一系列过程后,接收到基于 world 坐标系的 keyframe 位姿。回调接收到的第一个 keyframe 的位姿,其位置是 (0, 0, 0),旋转部分如果 params.yaml
文件中的 /lio_sam/useImuHeadingInitialization
设置为 true
,则采用绝对坐标系;如果为 false
,则用四元数 (0, 0, 0, 1) 表示。此回调按照以下顺序工作:
i) 首先,接收到消息时,将 keyframe 位姿的时间戳设为 currentCorrectionTime
,并将位姿值转换为 gtsam 库的位姿格式:
double currentCorrectionTime = ROS_TIME(odomMsg);
float p_x = odomMsg->pose.pose.position.x;
float p_y = odomMsg->pose.pose.position.y;
float p_z = odomMsg->pose.pose.position.z;
float r_x = odomMsg->pose.pose.orientation.x;
float r_y = odomMsg->pose.pose.orientation.y;
float r_z = odomMsg->pose.pose.orientation.z;
float r_w = odomMsg->pose.pose.orientation.w;
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
ii) 然后,为了进行因子图优化初始化进行设置。在这里,注意到预积分将第
i
i
i 个 keyframe 和第
j
j
j 个 keyframe 之间的测量值作为一个因子。因此,进行优化时至少需要两个位姿。由于在收到第一个位姿时仅有一个位姿,因此无法进行优化。因此,执行 if (systemInitialized == false)
语句内部的代码,设置初始值,然后回调函数返回。
这部分主要分为两个步骤。首先,从队列中移除时间戳早于当前 currentCorrectionTime
(即比当前位姿早到的)数据:
(预积分将第
i
i
i 个 keyframe 和第
j
j
j 个 keyframe 之间的测量值作为一个因子,再次记住这一点!!! 因此,之前的值在创建预积分测量时是不需要的。)
其次,初始化用于优化的因子图(gtsam::ISAM2
),并初始化预积分测量(gtsam::PreintegratedImuMeasurements
)中的 bias 和 Jacobian 项。为此,将优化器的初始值设置好,并重置用于预积分的 imuIntegratorImu_
和 imuIntegratorOpt_
。注意,以下两行的 resetIntegrationAndSetBias()
函数是:
`imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);`
`imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);`
这里定义为:
void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
biasHat_ = biasHat;
resetIntegration();
}
定义如下:
resetIntegration()
函数定义如下:
void ManifoldPreintegration::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_.setZero();
delPdelBiasAcc_.setZero();
delPdelBiasOmega_.setZero();
delVdelBiasAcc_.setZero();
delVdelBiasOmega_.setZero();
}
在这里,deltaTij_
表示第
i
i
i 个和第
j
j
j 个 keyframe 之间的时间差,deltaXij_
是我们最终需要计算的预积分测量值,即下图中的绿色框的值。
NavState
可以看到它包含 Rot3
、Point3
和 Velocity3
成员变量。deltaXij_
变量下的 5 个项是用来计算 bias 优化时的 Jacobian 项。总结来说,通过这些,我们可以确认 i
和 j
之间的时间差、预积分测量值以及 Jacobian 项都被初始化了。
iii) 然后,当接收到第一个位姿之后的其他位姿时,将进行优化以修正 bias。首先,当接收到第 j j j 个位姿时,将使用第 j − 1 j-1 j−1 个和第 j j j 个位姿之间的 IMU 测量来计算预积分测量值。
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
if (imuTime < currentCorrectionTime - delta_t)
{
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
imuIntegratorOpt_->integrateMeasurement(
gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
lastImuT_opt = imuTime;
imuQueOpt.pop_front();
}
else
break;
}
接下来,使用存储在 imuIntegratorOpt_
中的预积分测量值来设置 IMU 因子。
// add imu factor to graph
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);
然后,我们还将要优化的第
j
−
1
j-1
j−1 和第
j
j
j 个位姿之间的 bias 也添加到优化中。在这里,我们可以看到,通过 gtsam::imuBias::ConstantBias()
展示了之前在证明预积分测量值时使用的 bias 在两个 keyframe 之间是常量这一假设。
// add imu bias between factor
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
之后,代码中的以下部分是将因子图及其对应的初始值赋给优化器的过程,优化器的结果将分配给以下成员变量:
gtsam::Values result = optimizer.calculateEstimate();
prevPose_ = result.at<gtsam::Pose3>(X(key));
prevVel_ = result.at<gtsam::Vector3>(V(key));
prevState_ = gtsam::NavState(prevPose_, prevVel_);
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
最终,我们要计算的 biases 由 prevBias_
提供,该值是通过优化估计得到的 IMU biases。因此,通过使用更新后的 biases,可以更准确地估计 IMU 的增量运动。因此,在下面的代码中,直到接收到第
j
j
j 个位姿之前,IMU 数据通过运动积分进行处理,以通过运动积分得到基于 IMU 数据的位姿。
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
for (int i = 0; i < (int)imuQueImu.size(); ++i)
{
sensor_msgs::Imu *thisImu = &imuQueImu[i];
double imuTime = ROS_TIME(thisImu);
double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
lastImuQT = imuTime;
}
}
请注意,IMU 数据被复制到 imuQueOpt
和 imuQueImu
两个变量中。这两个变量的关系可能有些混淆,简而言之,它们各自的角色如下:
imuQueOpt
-imuIntegratorOpt_
:用于通过预积分生成 IMU 因子。因此,通过对两个 keyframe 之间的 IMU 数据进行预积分,计算出预积分测量值,并将这些计算结果传递给优化器。imuQueImu
-imuIntegratorImu_
:接收优化结果作为输入,进行运动积分。因此,将原本以 LiDAR 数据的频率输出的位姿提升到 IMU 数据的频率。
了解以上内容后,再次查看 imuHandler()
函数((b) 的第三部分)。一旦优化执行过一次,odometryHandler()
函数内部的 doneFirstOpt
会被设置为 true
。这样,当 IMU 数据通过 imuHandler()
输入时,函数的下面部分也会被执行。在下面的代码部分,我们可以看到 imuIntegratorImu_
在进行积分时计算出增量相对位姿。
// integrate this single imu message
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
// predict odometry
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
P.S. 在 odometryHandler()
内部的 if (key == 100)
语句是为了防止因不断添加因子而导致的内存需求持续增加,它用于重置因子。这被称为边缘化(marginalization),在 VINS-Mono 中进行滑动窗口时也会执行边缘化。在这个过程中,当累积的 key pose 超过 100 个时,会将所有这些位姿和 IMU 因子压缩为一个先验,并用来进行优化。有关详细信息,请搜索“Schur complement”。