IMU预积分- 5. LIO-SAM中的IMU预积分

• 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) 这行代码注释掉，你会发现位姿会发散并"飞向太空"。）
参考上图中的 ${\eta }^{a_d}(t)$ ${\eta }^{g_d}(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$ 个 keyframe 和第 $j$ 个 keyframe 之间的测量值作为一个因子。因此，进行优化时至少需要两个位姿。由于在收到第一个位姿时仅有一个位姿，因此无法进行优化。因此，执行 if (systemInitialized == false) 语句内部的代码，设置初始值，然后回调函数返回。

这部分主要分为两个步骤。首先，从队列中移除时间戳早于当前 currentCorrectionTime（即比当前位姿早到的）数据：

（预积分将第 $i$ 个 keyframe 和第 $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$ 个和第 $j$ 个 keyframe 之间的时间差，deltaXij_ 是我们最终需要计算的预积分测量值，即下图中的绿色框的值。

NavState 可以看到它包含 Rot3、Point3 和 Velocity3 成员变量。deltaXij_ 变量下的 5 个项是用来计算 bias 优化时的 Jacobian 项。总结来说，通过这些，我们可以确认 i 和 j 之间的时间差、预积分测量值以及 Jacobian 项都被初始化了。

iii) 然后，当接收到第一个位姿之后的其他位姿时，将进行优化以修正 bias。首先，当接收到第 $j$ 个位姿时，将使用第 $j-1$ 个和第 $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$ 个位姿之间的 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$ 个位姿之前，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”。