LIO-SAM前端中基于IMU预积分的前端代码详解

这里将通过代码解析预积分这个节点的运行顺行

第一次进入imuHandler函数

1. 首先，该节点自己的main函数

int main(int argc, char** argv)
{
    ros::init(argc, argv, "roboat_loam");
    
    IMUPreintegration ImuP;

    TransformFusion TF;

    ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
    //开4个线程
    ros::MultiThreadedSpinner spinner(4);
    spinner.spin();
    
    return 0;
}

运行IMUPreintegration ImuP里面的构造函数中的回调函数

subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

回调函数只有这两个，第一个订阅的话题是IMU的原始数据，第二个订阅的话题是lidar里程计的位姿

2. 先讲订阅IMU原始数据的回调函数&IMUPreintegration::imuHandler

• 先把IMU的消息做一个旋转变换，把IMU数据旋转到前左上坐标系下

sensor_msgs::Imu thisImu = imuConverter(*imu_raw);

具体操作代码在这里

    sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
    {
        sensor_msgs::Imu imu_out = imu_in;
        // 这里把imu的数据旋转到前左上坐标系下，可以参考https://github.com/TixiaoShan/LIO-SAM/issues/6
        // rotate acceleration
        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
        acc = extRot * acc;
        imu_out.linear_acceleration.x = acc.x();
        imu_out.linear_acceleration.y = acc.y();
        imu_out.linear_acceleration.z = acc.z();
        // rotate gyroscope
        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
        gyr = extRot * gyr;
        imu_out.angular_velocity.x = gyr.x();
        imu_out.angular_velocity.y = gyr.y();
        imu_out.angular_velocity.z = gyr.z();
        // rotate roll pitch yaw
        // 这是一个九轴imu，因此还会有姿态信息
        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
        Eigen::Quaterniond q_final = q_from * extQRPY;
        imu_out.orientation.x = q_final.x();
        imu_out.orientation.y = q_final.y();
        imu_out.orientation.z = q_final.z();
        imu_out.orientation.w = q_final.w();
        // 简单校验一下结果
        if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
        {
            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
            ros::shutdown();
        }

        return imu_out;
    }

其中的 extRot 是params中的参数，具体长这样

  extrinsicRot: [1, 0, 0,
                  0, 1, 0,
                  0, 0, 1]

• 然后将数据存进两个队列中，一个用于执行预积分和位姿优化，一个用于更新IMU状态，第二个会用到第一个的优化结果，后面会讲

imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);

• 第一次进这个函数的时候是没有发生过优化的，此时会跳出该函数，具体什么时候会接着运行后面会顺下来的

if (doneFirstOpt == false)
     return;

跳出 imuHandler 进入 odometryHandler

3. 这个时候就跑去运行第二个回调函数了 &IMUPreintegration::odometryHandler
   这个回调函数是订阅地图优化节点的增量里程计信息

• 保存订阅的位姿的时间戳，用于后面的IMU数据时间对齐

double currentCorrectionTime = ROS_TIME(odomMsg);

• 获取里程计信息并转成gtsam的格式

// 确保imu队列中有数据
if (imuQueOpt.empty())
    return;
// 获取里程记位姿
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的格式
gtsam::Pose3 lidarPose = gtsam::Pose

初始化系统

• 第一次进入时需要初始化系统

1. 优化问题复位

    void resetOptimization()
    {
        // gtsam初始化
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;//差值大于0.1需要重新线性化
        optParameters.relinearizeSkip = 1; //每当有1个值需要重新线性化时，对贝叶斯树进行更新
        optimizer = gtsam::ISAM2(optParameters);

        gtsam::NonlinearFactorGraph newGraphFactors;
        graphFactors = newGraphFactors;

        gtsam::Values NewGraphValues;
        graphValues = NewGraphValues;
    }

2. 将当前里程计消息前的imu信息全部扔掉

            while (!imuQueOpt.empty())
            {
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }
                else
                    break;
            }

3. 初始化位姿、速度和零偏，并加入因子图中

            // 将lidar的位姿转移到imu坐标系下
            prevPose_ = lidarPose.compose(lidar2Imu);
            // 设置其初始位姿和置信度
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
            // 约束加入到因子中
            graphFactors.add(priorPose);
            
            // initial velocity
            // 初始化速度，这里就直接赋0了
            prevVel_ = gtsam::Vector3(0, 0, 0);
            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
            // 将对速度的约束也加入到因子图中
            graphFactors.add(priorVel);
            
            // initial bias
            // 初始化零偏
            prevBias_ = gtsam::imuBias::ConstantBias();
            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
            // 零偏加入到因子图中
            graphFactors.add(priorBias);

4. 将各个状态量赋成初始值

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

5. 约束和状态量更新进isam优化器

optimizer.update(graphFactors, graphValues);

6. 使用初始化零偏对预计分接口进行初始化

imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

这两个变量是gtsam库自带的类型，具体为，虽然预积分原理复杂，但是这里封装好了，预计分的操作都在库里面进行，我们只关注结果即可

gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;

上面的位姿、速度、零偏的置信度在这里设置

// 初始位姿置信度设置比较高
priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
// 初始速度置信度就设置差一些
priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
// 零偏的置信度也设置高一些
priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good

开始处理优化

• 至此完成了系统的初始化，其实主要还是对优化库的初始变量的设置
• 对两帧间的imu做积分，将原始imu数据放入库的预积分接口进行操作即可

        while (!imuQueOpt.empty())
        {
            // pop and integrate imu data that is between two optimizations
            // 将imu消息取出来
            sensor_msgs::Imu *thisImu = &imuQueOpt.front();
            double imuTime = ROS_TIME(thisImu);
            // 时间上小于当前lidar位姿的都取出来
            if (imuTime < currentCorrectionTime - delta_t)
            {
                // 计算两个imu量之间的时间差
                double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
                // 调用预积分接口将imu数据送进去处理
                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);
                // 记录当前imu时间
                lastImuT_opt = imuTime;
                imuQueOpt.pop_front();
            }
            else
                break;
        }

• 将预计分约束、零偏约束、位姿约束加入因子图中

// add imu factor to graph
// 两帧间imu预积分完成之后，就将其转换成预积分约束
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);
// add imu bias between factor
// 零偏的约束，两帧间零偏相差不会太大，因此使用常量约束
// 前一帧bias，当前帧bias，观测值，噪声协方差；deltaTij()是积分段的时间
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                    gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
// add pose factor
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
// lidar位姿补偿到imu坐标系下，同时根据是否退化选择不同的置信度，作为这一帧的先验估计
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
// 加入因子图中去
graphFactors.add(pose_factor);

• 根据上异常时刻的状态，结合预积分的结果，对当前状态进行预测，并将预测值作为初始值插入因子图中

// 根据上一时刻的状态，结合预积分结果，对当前状态进行预测
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
// 预测量作为初始值插入因子图中
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();

• 获取优化有的当前状态作为当前帧的最佳估计

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_);
// check optimization
// 一个简单的失败检测
if (failureDetection(prevVel_, prevBias_))
{
    // 状态异常就直接复位了
    resetParams();
    return;
}

优化后开始位姿传播

思路描述：假设当前时间是k+n，第k时间戳的关键帧是被优化过，即认为其位姿是准确的，同时这里假设短时间内imu漂移不会太大，此时第k+n时间的位姿 = 积分k~k+n的位姿 +第k+n的位姿，这样就可以以更快的频率发布位姿，同时结果也比较准确，因为优化的位姿发布可能只有10 ~ 30 Hz ，这样用imu的频率发布有100Hz，里程计发布在 imuHandler 函数中

// 优化之后，根据最新的imu状态进行传播
prevStateOdom = prevState_;
prevBiasOdom  = prevBias_;
// first pop imu message older than current correction data
double lastImuQT = -1;
// 首先把lidar帧之前的imu状态全部弹出去
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
    lastImuQT = ROS_TIME(&imuQueImu.front());
    imuQueImu.pop_front();
}
// repropogate
// 如果有新于lidar状态时刻的imu
if (!imuQueImu.empty())
{
    // reset bias use the newly optimized bias
    // 这个预积分变量复位
    imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
    // integrate imu message from the beginning of this optimization
    // 然后把剩下的imu状态重新积分
    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;
    }
}

回到imuHandler函数

• 每来一个imu值就加入预积分状态中

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
// 每来一个imu值就加入预积分状态中
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);

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

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

注意这里的 prevStateOdom, prevBiasOdom 是 odometryHandler 函数优化进行传播赋值的，即第k时刻的优化值

• 发布位姿

// 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
// 将这个状态转到lidar坐标系下去发送出去
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);

关于高频发布odom时两个函数的配合

odometryHandler 和 imuHandler 至始至终都在处理，以下两个变量

imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);

两个区别在于
imuQueOpt 是用于处理两帧间积分的，而 imuQueImu 是用于处理优化后但还没到新关键帧来时的中间位姿积分，来进行高频发布odom计算用的

• 对这两个变量的实际操作都在 odometryHandler 中，其中 imuQueImu 在 odometryHandler函数 中计算预积分是因为两个函数在轮流调用，这样两个函数中都有预积分，而且都是用相同的接口 imuIntegratorImu_->integrateMeasurement 这样就可以确保积分是不间断的
• 而 imuHandler 函数只需要接着预积分即可，算完直接发布就行，因为是用同样的接口，有一部分预积分是在 odometryHandler 中算的
• 注意预积分接口有两个

imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization 

• imuIntegratorOpt_ 是在优化前进行预积分使用的，其使用的数据是imuQueOpt ，这个变量的预测结果是给优化当初始值的，预积分值给到因子图进行预积分约束

• 优化后将结果中的零偏 prevBias_ 给到 imuIntegratorImu_这个预积分接口进行复位，然后对 imuQueImu 数据进行预积分运算，再在 imuHandler 中用这个复位过的 imuIntegratorImu_预积分接口进行计算，最后再根据优化后的结果prevState_ ，prevBias_对当前状态进行预测，然后就能以高频发布比较准确的位姿了

odometryHandler中约束过多的处理

• 当isam优化器中加入了较多的约束后，为了避免运算时间变长，就直接清空，当有100帧时直接清空过去的所有约束，但是会保存最新时刻的位姿、速度、零偏及其协方差矩阵

        if (key == 100)
        {
            // get updated noise before reset
            // 取出最新时刻位姿 速度 零偏的协方差矩阵
            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)));
            // reset graph
            // 复位整个优化问题
            resetOptimization();
            // add pose
            // 将最新的位姿，速度，零偏以及对应的协方差矩阵加入到因子图中
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
            graphFactors.add(priorPose);
            // add velocity
            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
            graphFactors.add(priorVel);
            // add bias
            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
            graphFactors.add(priorBias);
            // add values
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);
            // optimize once
            optimizer.update(graphFactors, graphValues);
            graphFactors.resize(0);
            graphValues.clear();

            key = 1;
        }