LIO-sam源码分析：imuPreintegration.cpp

最新推荐文章于 2025-09-25 02:42:21 发布

原创

最新推荐文章于 2025-09-25 02:42:21 发布 · 1.2k 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#激光slam #3D slam #lio-sam

本文详细介绍了激光IMU里程计融合的实现过程，包括IMUPreintegration类的构造函数，其中定义了imu和里程计的订阅及发布，以及imuHandler和odometryHandler回调函数。imuHandler负责预积分和发布imu里程计，而odometryHandler则进行里程计数据的优化。整个流程中，通过对imu和里程计数据的融合，实现了高精度的实时定位估计。

在这里插入图片描述
功能：联合优化激光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 =</