imu预积分代码解析
Kong Liangqian Lv6
 2022-02-23 12:59:22  

LIOSAM一共四个src文件，本节我们要讨论的文件为imuPreintegration.cpp，

函数入口

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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");
    
    ros::MultiThreadedSpinner spinner(4);
    spinner.spin();
    
    return 0;
}

函数首先进入的是IMUPreintegration的默认构造函数

IMUPreintegration构造函数

该构造函数订阅了两条消息

  • IMU 的topic：回调函数为IMUPreintegration::imuHandler
  • Odometry：回调函数为IMUPreintegration::odometryHandler。 该Odom来自于mapping模块
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
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数据全部转换到lidar坐标系上，并且把收到的消息传入到两个队列中，一个用于预积分和位姿的优化，一个用于更新最新imu状态。

只有在odometry 的回调函数处理过后doneFirstOpt才会true。下面先看，在设置为true之前会发生什么

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
  void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
  {
      std::lock_guard<std::mutex> lock(mtx);
// 把imu转换到lidar坐标系上 
      sensor_msgs::Imu thisImu = imuConverter(*imu_raw);

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

      // 只有odometry handler之后才会true
      if (doneFirstOpt == false)
          return;

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

odometryHandler

流程

odometry的回调函数是最重要的回调函数，步骤为

  1. 取出里程计的位置和方向

  2. 判断里程计是否退化，这个会在后端中说明，什么时候会退化，退化的精度会下降

  3. 这帧雷达的pose转为gtsam

  4. 第一次进入，初始化系统：

    ​ 4.1 GTSAM初始化

    ​ 4.2 添加先验约束

    ​ 4.3 添加实际的状态量

    ​ 4.4 更新isam优化器

    ​ 4.5 预积分接口重置

  5. 加入imu数据，自动实现预积分量的更新以及协方差矩阵的更新

  6. 添加ImuFactor，零偏约束

  7. 添加雷达的位姿约束

  8. 失败检测（速度大于30，零偏太大）

  9. 根据最新的imu状态进行传播

代码

  1. 取出里程计的位置和方向
1
2
3
4
5
6
7
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;
  1. 判断里程计是否退化，这个会在后端中说明，什么时候会退化，退化的精度会下降
1
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
  1. 这帧雷达的pose转为gtsam
1
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
  1. 初始化系统
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
// 0. initialize system
if (systemInitialized == false)
{
    resetOptimization();
    /*
    void resetOptimization()
    {
        // GTSAM 初始化
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;
        optParameters.relinearizeSkip = 1;
        optimizer = gtsam::ISAM2(optParameters);

        gtsam::NonlinearFactorGraph newGraphFactors;
        graphFactors = newGraphFactors;

        gtsam::Values NewGraphValues;
        graphValues = NewGraphValues;
    }
    */

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

下面的内容为，添加因子图的信息

gtsam::NonlinearFactorGraph graphFactor： 类别为：gtsam::PriorFactor

  • 添加先验速度约束
  • 添加先验零偏约束
  • 添加先验位置约束

gtsam::Values graphValue

  • 插入位置
  • 插入速度
  • 插入零偏
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
    // initial pose
    // 把雷达的位姿转移到imu坐标系下
    prevPose_ = lidarPose.compose(lidar2Imu);
    // X(0)表示第一个位姿，有一个先验的约束。约束内容为，lidar到imu下的prevPose_这么一个位姿
    // 该约束的权重，置信度为priorPoseNoise，越小代表置信度越高
    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);
    // 添加完约束，需要添加实际的状态量
    // 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;
    systemInitialized = true;
    return;
}
  1. 加入imu数据

imuIntegratorOpt_->integrateMeasurement

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
while (!imuQueOpt.empty())
{
    // pop and integrate imu data that is between two optimizations
    sensor_msgs::Imu *thisImu = &imuQueOpt.front();
    double imuTime = ROS_TIME(thisImu);
    // imu时间上小于当前odom消息都取出来
    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);

        lastImuT_opt = imuTime;
        imuQueOpt.pop_front();
    }
    else
        break;
}
  1. 加ImuFactor，零偏约束

graphFactors 因子图因子添加

1
2
3
4
5
6
7
8
9
10
11
// add imu factor to graph
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)));
// 以上添加完预积分的约束
  1. 添加雷达的位姿约束，设定因子图的初值
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
// add pose factor
// 转换到IMU坐标系下
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
// 结合预积分的结果，对当前帧的位姿进行预测
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_);
  1. 失败检测
1
2
3
4
5
6
// check optimization
if (failureDetection(prevVel_, prevBias_))
{
    resetParams();
    return;
}
  1. 根据最新的imu状态进行传播
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// repropogate
if (!imuQueImu.empty())
{
    // reset bias use the newly optimized bias
    imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
    // integrate imu message from the beginning of this optimization
    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;
    }
}
 Comments