3d激光SLAM：LIO-SAM框架---IMU预积分流程_雷达里程计因子图

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

在这篇博客中，对于IMU预积分功能数据初始化部分进行了代码解读。总结如下图所示：

本篇博客主要介绍在 IMU预积分系统初始化完成后，进行 IMU预积分主要的优化过程

IMU预积分主要的优化过程

上面初始化完成 则有了 一个初始的lidar转到imu的位姿

将imu约束加到因子图中

下面将两帧间的IMU做积分

        while (!imuQueOpt.empty())
        {
        	sensor_msgs::Imu *thisImu = &imuQueOpt.front();
        	double imuTime = ROS_TIME(thisImu);
1
2
3
4

• 判断该队列不为空
• 将imu 消息取出来
• 得到imu时间戳

     if (imuTime < currentCorrectionTime - delta_t)
            { 
          	  double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
1
2
3

然后要求 imu的时间 小于 当前lidar里程计的时间
计算两个imu量之间的时间差
lastImuT_opt 的初始化为-1 ，当第一次运行的时候 将该值设置为 接近于0

                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);
1
2
3

调用GT-SAM预积分接口将imu数据送进去处理.送入的数据包括：

• 加速度计 三轴数据
• 陀螺仪 三轴数据
• imu 帧间时间差

                lastImuT_opt = imuTime;
                imuQueOpt.pop_front();
1
2

• 更新lastImuT_opt
• 将处理的imu数据pop出去

const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
1

两帧间imu预积分完成之后，就将其转换成预积分约束

gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
1

预积分约束对相邻两帧之间的位姿 速度 零偏（上一帧） 形成 约束
gtsam::PriorFactorgtsam::imuBias::ConstantBias 是先验约束，先验约束通常只会对某个状态进行约束
预积分的约束会对多个状态进行约束

graphFactors.add(imu_factor);
1

加入到因子图中

上面就将imu的约束加到了因子图中

将零偏及lidar里程计约束加到因子图中

        graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                         gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
1
2

零偏的约束 ，两帧间零偏相差不会太大，因此使用常量约束
gtsam::imuBias::ConstantBias() 就是 gtsam提供的一种imu零偏约束
imu的零偏属于随机游走，和时间有关，gtsam提供了deltaTij，计算相邻帧的时间。

        gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
        gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
        graphFactors.add(pose_factor);
1
2
3

• 将lidar里程计的位姿转到imu坐标系下
• 根据是否退化 选择 不同 的 置信度，作为这一帧的先验估计
• 将lidar里程计位姿约束加到因子图中

如果退化 则 协方差的矩阵 值 为 1，比较大

如果没有退化 则 协方差的矩阵 值 为 0.05及0.1，比较小

---

执行因子图优化

 gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
1

根据上一时刻状态，结合预积分结果，对当前状态进行预测
可以预测出 位姿及速度
就是相对对imu数据做积分

        graphValues.insert(X(key), propState_.pose());
        graphValues.insert(V(key), propState_.v());
        graphValues.insert(B(key), prevBias_);
1
2
3

各当前状态赋 根据预积分预测出来的初值
零偏还是用上一帧的零偏

optimizer.update(graphFactors, graphValues);
optimizer.update();
1
2

进行优化
执行了两次优化，每次优化的调整幅度不会太大，根据经验，两次的结果比较理想

        graphFactors.resize(0);
        graphValues.clear();
1
2

优化完成后重置

• graphFactors
• graphValues

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));
1
2
3
4
5

获取优化后的 当前状态作为当前帧的最佳估计
为下一次优化准备

imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
1

当前约束任务已经完成，预积分约束复位，同时需要设置一下零偏作为下一次预积分的先决条件

        if (failureDetection(prevVel_, prevBias_))
        {
            resetParams();
            return;
        }
1
2
3
4
5

约束任务完成后，进行 一个 异常检测
检测的就是速度和零偏

如果当前速度大于30m/s ,108km/h 则认为 异常状态。
如果想让算法应用于更大速度的平台，则需要修改此处

根据imu状态进行传播

下面代码要做的就是绿色的部分

通过上面的代码完成了imu预积分的优化过程，但是雷达里程计的更新是比较慢的，imu的更新频率要快很多，如果需要以imu的频率发布一个里程计信息，那么则需要做绿色重传播的过程

        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
1
2

获得雷达里程计处的优化后的 位姿和零偏

        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }
1
2
3
4
5

把前面imu的数据仍掉（红色箭头前面）

        if (!imuQueImu.empty())
        {
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
1
2
3

使用 现在 最优的bias 估计 重置 预积分

            for (int i = 0; i < (int)imuQueImu.size(); ++i)
            {     //步骤跟之前一致
                // 利用imuQueImu中的数据进行预积分 主要区别旧在于上一行的更新了bias 
                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;
            }
1
2
3
4
5
6
7
8
9
10
11

和之前的操作基本一致
将imu的数据和dt ，送到预积分中，为后续的 gtsam做预测提供数据

做推算不在整个回调函数中，因为这个回调函数是雷达里程计的回调函数。然后在 imu的回调函数中做 gtsam的预测。

处理因子图过大的情况

添加因子很频繁的，每来一个 雷达里程计，就会往里面添加因子，为避免因子图比较大，则每一百次做一个简单的清空

        if (key == 100)
        {
1
2

            gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
            gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
            gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
1
2
3

取出上一刻，三个状态量的协方差

resetOptimization();
1

因子图复位

            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
            graphFactors.add(priorPose);

            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
            graphFactors.add(priorVel);

            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
            graphFactors.add(priorBias);
1
2
3
4
5
6
7
8

将最新的位姿、速度、零偏 以及对应的协方差矩阵加入到因子图中

            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);
1
2
3

添加变量因子

            optimizer.update(graphFactors, graphValues);
            graphFactors.resize(0);
            graphValues.clear();
            key = 1;
1
2
3
4

优化一次

以IMU频率向外发布位姿估计

下面则回到imu的回调函数中，执行 以IMU频率向外发布位姿估计

        double imuTime = ROS_TIME(&thisImu);
        double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
        lastImuT_imu = imuTime;
1
2
3

取出当前帧的时间
计算dt

        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);
1
2

每来一个imu数据就加入到预积分状态中

gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
1

根据这个值预测最新的状态

        nav_msgs::Odometry odometry;
        odometry.header.stamp = thisImu.header.stamp;
        odometry.header.frame_id = odometryFrame;
        odometry.child_frame_id = "odom_imu";

        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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

将最新的预测值发布出去

至此完成了以imu频率发布位姿估计

总结