LIO-sam源码分析：imuPreintegration.cpp

功能：联合优化激光odom和imu数据，估计IMU偏差，进行实时的里程计估算。

imuPreintegration.cpp有两个类，其中IMUPreintegration预积分发布的话题，会被TransformFusion类的订阅函数接收。

• IMUPreintegration的构造函数：

• 订阅:
  imuTopic：imu原始数据回调函数：imuHandler
  lio_sam/mapping/odometry_incremental -->里程计回调：odometryHandler
• 发布：
  odomTopic+"_incremental"

• TransformFusion的构造函数：

• 订阅：
  lio_sam/mapping/odometry -->激光里程计回调：lidarOdometryHandler
  odomTopic+"_incremental"–>imu里程计回调：imuOdometryHandler
• 发布：
  odomTopic
  lio_sam/imu/path ：rviz显示的路径

IMUPreintegration构造函数

按照顺序，先来看看IMUPreintegration类的构造函数，它定义了两个订阅器和一个发布器，然后其它变量为gtsam优化器的变量。

    IMUPreintegration()
    {
    
    
        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());

        pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);

        boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
        p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
        p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
        p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
        gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias

        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
        correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
        correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
        noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
        
        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        
    }

imuHandler回调函数

这里面包括了imu预积分以及发布IMU里程计的相关消息。

    void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
    {
    
    
        std::lock_guard<std::mutex> lock(mtx);

        坐标系转换  调用utility.h中函数
        sensor_msgs::Imu thisImu = imuConverter(*imu_raw);

        //转换后imu信息存入两个队列 后续使用
        imuQueOpt.push_back(thisImu);
        imuQueImu.push_back(thisImu);

        //检查有没有执行过一次优化  也就是说这里需要先在odomhandler中优化一次后再进行该函数后续的工作
        //odomhandler回调函数执行一次
        if (doneFirstOpt == false)
            return;

        double imuTime = ROS_TIME(&thisImu);
         //获得时间间隔 第一次为1/500 之后是两次imuTime间的差
        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
        //发布imu里程计 
        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
        // //预测值currentState获得imu位姿 再由imu到雷达变换 获得雷达位姿
        gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
        gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);

        //IMU里程计的相关数据填充 
        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);//发布
    }
};

odometryHandler回调函数

这个回调函数比较长，也是很重要的一个函数，主要是对里程计数据进行优化。

    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
    
    
        std::lock_guard<std::mutex> lock(mtx);

         //里程计消息的当前时间戳
        double currentCorrectionTime = ROS_TIME(odomMsg);

        // make sure we have imu data to integrate
        //保证有imu数据  两个回调函数是互有联系的  在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::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));


        // 0. initialize system
        //只执行一次 初始化系统
        if (systemInitialized == false)
        {
    
    
            //调用函数 优化参数重置
            resetOptimization();

            // pop old IMU message
             //推出相对较旧的imu消息 保证imu与odometry消息时间同步  因为imu是高频数据所以这是必要的
            //整个LIO-SAM中作者对时间同步这块的思想都是这样的
            while (!imuQueOpt.empty())
            {
    
    
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
    
    
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }
                else
                    break;
            }
            // initial pose
            //由激光里程计消息提供位姿  并转到imu坐标系下
            prevPose_ = lidarPose.compose(lidar2Imu);
            //PriorFactor 概念可看gtsam  包括了位姿 速度  bias 
            //加入PriorFactor在图优化中基本都是必需的前提
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
            graphFactors.add(priorPose);
            // initial velocity
            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);
            //除了因子外 还要有节点value
            // 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();

            //积分器重置
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
            //key =1用于计数 
            key = 1;
            systemInitialized = true;
            return;
        }


        // reset graph for speed
        if (key == 100)
        {
    
    
            //如果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;
        }


        // 1. integrate imu data and optimize
        //主要的优化过程 
        while (!imuQueOpt.empty())
        {
    
    
            // pop and integrate imu data that is between two optimizations
            //对两次优化间的imu数据进行积分
            //imuQueOpt中队列头部数据存入thisImu
            sensor_msgs::Imu *thisImu = &imuQueOpt.front();
            //imu时间戳
            double imuTime = ROS_TIME(thisImu);
             //imu时间早于当前激光里程计时间-delta
            //从k-1帧到k帧之间的imu的测量值 用于求出两帧间的相对位姿以及k帧时刻的状态预测值 
            if (imuTime < currentCorrectionTime - delta_t)
            {
    
    
                //dt的赋值 与imuHandler中类似 
                double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
                 //进行预积分得到新的状态值  注意用到的是imu数据的加速度 角速度  作者要求的9轴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);
                //在推出一次数据前保存上一个数据的时间戳
                lastImuT_opt = imuTime;
                imuQueOpt.pop_front();
            }
            else
                break;
        }
        // add imu factor to graph
        //利用两帧之间的IMU数据完成了预积分后增加imu因子到因子图中
        //这里具体涉及gtsam的内容 如果不熟悉需要学习下 官方有例程
        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
        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
        //还加入了pose factor 其实对应于作者论文中的因子图结构 就是与imu因子一起的 Lidar odometry factor
        gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
        gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
        graphFactors.add(pose_factor);
        // insert predicted values
        //插入预测的值 即因子图中x0 x1 x2 ……节点
        gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
        graphValues.insert(X(key), propState_.pose());
        graphValues.insert(V(key), propState_.v());
        graphValues.insert(B(key), prevBias_);
        // optimize
         //进行优化
        optimizer.update(graphFactors, graphValues);
        optimizer.update();
        //优化完成后重置
        graphFactors.resize(0);
        graphValues.clear();
        // Overwrite the beginning of the preintegration for the next step.
        //用这次的优化结果重写或者说是覆盖相关初始值 为下一次优化准备
        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));
        // Reset the optimization preintegration object.
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
        // check optimization
        //调用本程序文件内函数 检查是否有失败情况 如有则重置参数
        if (failureDetection(prevVel_, prevBias_))
        {
    
    
            resetParams();
            return;
        }


        // 2. after optiization, re-propagate imu odometry preintegration
        //重传播 
        /*为了维持实时性imuIntegrateImu就得在每次odom触发优化后立刻获取最新的bias,
        同时对imu测量值imuQueImu执行bias改变的状态重传播处理，这样可以最大限度的保证实时性和准确性。*/
        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        同样是推出时间上更早一些 过旧的数据
        double lastImuQT = -1;
        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
            //使用最新的优化后的bias更新bias值
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
            // integrate imu message from the beginning of this optimization
            //利用imuQueImu中的数据进行预积分 主要区别在于上一行的更新了bias 
            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;
            }
        }

        ++key;
        doneFirstOpt = true;
    }

lidarOdometryHandler回调函数

    void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
    
    
        std::lock_guard<std::mutex> lock(mtx);

        //调用工具类函数 获得从地图优化程序发布的里程计消息的仿射变换矩阵的形式
        lidarOdomAffine = odom2affine(*odomMsg);

        //时间戳
        lidarOdomTime = odomMsg->header.stamp.toSec();
    }

imuOdometryHandler回调函数

    void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
    
    
        // static tf
        // odom map之间一开始的静态变换 
        static tf::TransformBroadcaster tfMap2Odom;
        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

        std::lock_guard<std::mutex> lock(mtx);

        //存入队列
        imuOdomQueue.push_back(*odomMsg);

        // get latest odometry (at current IMU stamp)
         //如果没有地图优化程序的里程计消息直接退出 如果有进行时间同步
        if (lidarOdomTime == -1)
            return;
        while (!imuOdomQueue.empty())
        {
    
    
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }
         //利用imu队列首尾之间的增量式变换获得最终里程计仿射矩阵（地图优化程序中发出的里程计*imu里程计增量）
        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
        
        // publish latest odometry发布最终的里程计
        nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
        laserOdometry.pose.pose.position.x = x;
        laserOdometry.pose.pose.position.y = y;
        laserOdometry.pose.pose.position.z = z;
        laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        pubImuOdometry.publish(laserOdometry);

        // publish tf
        static tf::TransformBroadcaster tfOdom2BaseLink;
        tf::Transform tCur;
        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
        if(lidarFrame != baselinkFrame)
            tCur = tCur * lidar2Baselink;
        tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
        tfOdom2BaseLink.sendTransform(odom_2_baselink);

        // publish IMU path
        //这就是在RVIZ中看到的红色短的轨迹 其实这样的方式容易给人造成困扰
         // 这里的路径是融合了之后的 不是单纯的Imu path
        static nav_msgs::Path imuPath;
        static double last_path_time = -1;
        double imuTime = imuOdomQueue.back().header.stamp.toSec();
        if (imuTime - last_path_time > 0.1)
        {
    
    
            last_path_time = imuTime;
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
            pose_stamped.header.frame_id = odometryFrame;
            pose_stamped.pose = laserOdometry.pose.pose;
            imuPath.poses.push_back(pose_stamped);
            while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
                imuPath.poses.erase(imuPath.poses.begin());
            if (pubImuPath.getNumSubscribers() != 0)
            {
    
    
                imuPath.header.stamp = imuOdomQueue.back().header.stamp;
                imuPath.header.frame_id = odometryFrame;
                pubImuPath.publish(imuPath);
            }
        }
    }
};

---

参考：
https://www.zhihu.com/people/szm-6-90
https://zhuanlan.zhihu.com/p/183190393