前言

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);
  • 判断该队列不为空
  • 将imu 消息取出来
  • 得到imu时间戳
     if (imuTime < currentCorrectionTime - delta_t)
            { 
                double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);

然后要求 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);

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

  • 加速度计 三轴数据
  • 陀螺仪 三轴数据
  • imu 帧间时间差
                lastImuT_opt = imuTime;
                imuQueOpt.pop_front();
  • 更新lastImuT_opt
  • 将处理的imu数据pop出去
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);

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

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

预积分约束对相邻两帧之间的位姿 速度 零偏(上一帧) 形成 约束
gtsam::PriorFactor 是先验约束,先验约束通常只会对某个状态进行约束
预积分的约束会对多个状态进行约束

graphFactors.add(imu_factor);

加入到因子图中

上面就将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)));

零偏的约束 ,两帧间零偏相差不会太大,因此使用常量约束
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);
  • 将lidar里程计的位姿转到imu坐标系下
  • 根据是否退化 选择 不同 的 置信度,作为这一帧的先验估计
  • 将lidar里程计位姿约束加到因子图中

如果退化 则 协方差的矩阵 值 为 1,比较大
在这里插入图片描述
如果没有退化 则 协方差的矩阵 值 为 0.05及0.1,比较小
在这里插入图片描述


执行因子图优化

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

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

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

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

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

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

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

优化完成后重置

  • 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));

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

imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

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

        if (failureDetection(prevVel_, prevBias_))
        {
            resetParams();
            return;
        }

约束任务完成后,进行 一个 异常检测
检测的就是速度和零偏
在这里插入图片描述
如果当前速度大于30m/s ,108km/h 则认为 异常状态。
如果想让算法应用于更大速度的平台,则需要修改此处

根据imu状态进行传播

在这里插入图片描述
下面代码要做的就是绿色的部分

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

        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;

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

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

把前面imu的数据仍掉(红色箭头前面)

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

使用 现在 最优的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;
            }

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

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

处理因子图过大的情况

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

        if (key == 100)
        {
            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)));

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

resetOptimization();

因子图复位

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

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

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

添加变量因子

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

优化一次

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

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

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

取出当前帧的时间
计算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);

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

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

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

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

将最新的预测值发布出去

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

总结

在这里插入图片描述