LIO-SAM代码逐行解读（4）-IMU预积分
我们在前述的博客中介绍了LIO-SAM的一些准备工作、点云预处理、特征点提取等内容。之后，整理为cloud_info消息类型进行发布，包括其中的面片点、角点两种类型的特征点。
详细参考：
LIO-SAM代码逐行解读（1）-准备工作
LIO-SAM代码逐行解读（2）-点云预处理
LIO-SAM代码逐行解读（3）-特征点提取
接着，本篇博客中将重点介绍IMU预积分的相关处理，与前部分的内容相对独立一些。其发布的IMU里程计数据主要用于后续匹配的初值、点云去除运动畸变（实际未使用）等方面。
目录
- 准备工作
- IMUPreintegration类
-
- 准备工作
- 构造函数
- 重设优化器、重设参数
- 订阅激光里程计数据，并处理
- 订阅原始IMU数据，并处理
- 错误检测
- TransformFusion类
-
- 准备工作
- 订阅LiDAR里程计数据并处理
- 订阅IMU里程计数据并处理
- Main函数
准备工作
- 引用头文件
/*************************************************
TransformFusion类
功能简介：
主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。
订阅：
1、订阅激光里程计，来自MapOptimization；
2、订阅imu里程计，来自ImuPreintegration。
发布：
1、发布IMU里程计，用于rviz展示；
2、发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。
--------------------------------------------------
IMUPreintegration类
功能简介：
1、用激光里程计，两帧激光里程计之间的IMU预积分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;
2、以优化后的状态为基础，施加IMU预积分量，得到每一时刻的IMU里程计。
订阅：
1、订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预积分量，预测每一时刻（IMU频率）的IMU里程计；
2、订阅激光里程计（来自MapOptimization），用两帧之间的IMU预积分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。
发布：
1、发布imu里程计；
**************************************************/
// 引用自定义的函数
#include "utility.h"
// gtsam相关
#include 此文件中主要包括TransformFusion与IMUPreintegration两个类，分别进行位姿坐标转换与预积分操作。
首先，我们先来看一下IMUPreintegration类中的相关操作：
IMUPreintegration类
准备工作
- IMUPreintegration类变量声明
/**
* @brief IMU预积分类
* 1、接收IMU原始数据，接收mapoptimization节点发送的lidar里程计incremental消息
* 2、分别设置两个预积分器，一个在imuhandler函数中不断添加IMU原始数据，以最新的优化后的lidar位姿为基础，同IMU原始数据相同频率发布
* 3、另外一个预积分器odometryHandler中进行处理，添加两帧LiDAR里程计优化后位姿之间的预积分量，噪声、零偏约束等因子，进行优化
* 4、对两个预积分器重新设置优化后的零偏，状态（pose, bias），预计分量。
*/
class IMUPreintegration : public ParamServer
{
public:
std::mutex mtx;
// 接听IMU消息数据，里程计数据
ros::Subscriber subImu;
ros::Subscriber subOdometry; // 增量式的lidar里程计
// 发布IMU里程计 "odometry/imu_incremental"
ros::Publisher pubImuOdometry;
// 系统是否进行了初始化
bool systemInitialized = false;
// 噪声协方差
// 初始的位姿、速度、零偏
gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
// 激光里程计的矫正噪声，如果激光里程计发生了退化则设置的比较高
// 若没有发生退化，则设置的比较小
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
// 零偏之间的噪声模型
gtsam::Vector noiseModelBetweenBias;
// 两个IMU预积分器，都存储IMU观测值
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
// 存储IMU消息队列
std::deque<sensor_msgs::Imu> imuQueOpt;
std::deque<sensor_msgs::Imu> imuQueImu;
// imu因子图优化过程中的状态变量
gtsam::Pose3 prevPose_;
gtsam::Vector3 prevVel_;
gtsam::NavState prevState_; // pose + vel
gtsam::imuBias::ConstantBias prevBias_;
// imu的状态
gtsam::NavState prevStateOdom;
gtsam::imuBias::ConstantBias prevBiasOdom;
// 是否完成了第一次优化
bool doneFirstOpt = false;
// 上一帧IMU、上一帧IMU优化的时间
double lastImuT_imu = -1;
double lastImuT_opt = -1;
// 定义ISAM2优化器 需要设置ISAM2Params参数
gtsam::ISAM2 optimizer;
// 定义一个factors以及一个values进行优化
gtsam::NonlinearFactorGraph graphFactors;
gtsam::Values graphValues;
// IMU数据与激光里程计数据之间的时间差，设置为0
const double delta_t = 0;
int key = 1;
// lidar与imu坐标系之间的相互转换 外参变换（只有平移没有旋转，参数文件中平移量设置的也为 [0.0, 0.0, 0.0]）
// 所以两个变换都没有作用
gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
构造函数
// 构造函数
IMUPreintegration()
{
// 接听IMU消息（imuTopic， /imu/data）
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅"lio_sam/mapping/odometry_incremental"消息（后端mapOptmization模块发出）
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 发布imu里程计， 预积分节点发布的增量位姿（"odometry/imu_incremental"）
pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
// 定义预积分相关参数 噪声协方差 定义重力方向ENU
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
// 假设初始零偏为0
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
// 噪声模型 如果激光里程计scan-to-map优化过程中发生退化，则选择一个较大的协方差correctionNoise2
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();
// 两个IMU预积分器
// imuIntegratorImu_用于预测每一时刻（imu频率）的imu里程计（转到lidar系了，与激光里程计同一个系）
imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
// imu预积分器，用于因子图优化
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
}
重设优化器、重设参数
/**
* @brief 重设ISAM2优化器相关参数
* 1、主要是optimizer优化器重设optParameters
* 2、重设graphFactors与graphValues
*/
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;
}
/**
* @brief 重设参数
* 包括上一帧接收的IMU时间 是否已经进行过激光里程计的优化 系统是否已经初始化
*/
void resetParams()
{
lastImuT_imu = -1;
doneFirstOpt = false;
systemInitialized = false;
}
订阅激光里程计数据，并处理
/**
* @brief 订阅激光里程计，来自mapOptimization节点
*
* 1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化
* 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
* 3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
*
* @param odomMsg
*/
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队列中有数据，进行预积分
if (imuQueOpt.empty())
return;
// 获取后端里程计位姿
// 经过scan-to-map匹配、因子图优化后等处理获取
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）
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)
{
// 重置ISAM2优化器
resetOptimization();
// pop old IMU message
// 将这个里程计消息之前的imu信息全部扔掉
while (!imuQueOpt.empty())
{
// delta_t = 0，弹出在当前激光里程计currentCorrectionTime时刻之前的IMU数据
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
// 取出imu队列中的第一个数据的时间
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// initial pose（添加里程计位姿先验因子）PriorFactor Pose3
// 将接收的激光里程计位姿转移到imu坐标系下
prevPose_ = lidarPose.compose(lidar2Imu);
// 设置其初始位姿和置信度
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
// 约束加入到因子中 添加里程计的位姿先验因子
graphFactors.add(priorPose);
// initial velocity（添加速度先验因子） PriorFactor Vector3
// 初始化速度，这里就直接赋0了
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
// 将对速度的约束也加入到因子图中 添加速度先验因子
graphFactors.add(priorVel);
// initial bias（添加imu零偏先验因子） PriorFactor imuBias::ConstantBias
// 初始化零偏
prevBias_ = gtsam::imuBias::ConstantBias(); // 初始为（0,0,0,0,0,0）
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
// 约束和状态量更新进isam优化器（优化一次）
optimizer.update(graphFactors, graphValues);
// 进优化器之后保存约束和状态量的变量就清零
graphFactors.resize(0);
graphValues.clear();
// 预积分的接口，使用初始零偏进行初始化（对两个预积分器分别重置零偏）
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1; // 索引
systemInitialized = true; // 完成系统初始化
return;
}
// reset graph for speed
// 当isam优化器中加入了较多的约束后，为了避免运算时间变长，就直接清空
// 每收到100个来自后端Lidar odometry数据重设一次
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;
}
// 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计
// 添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
// 1. integrate imu data and optimize
// 添加到上一帧激光里程计到当前帧时间戳之间的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预积分因子 ImuFactor
// 两帧间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 BetweenFactor
// 零偏的约束，两帧间零偏相差不会太大，因此使用常量约束
// 添加imu偏置因子，前一帧偏置，当前帧偏置，观测值，噪声协方差；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 添加位姿因子（后端LiDAR里程计传入的LiDAR位姿，转换到IMU坐标系下）PriorFactor
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
// lidar位姿转换到imu坐标系下，同时根据是否退化选择不同的置信度，作为这一帧的先验估计，如果degenerate为真，则调大噪声
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
// 加入因子图中去
graphFactors.add(pose_factor);
// 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态
// 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_);
// 当前帧IMU的零偏
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
// Reset the optimization preintegration object.
// 当前约束任务已经完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先决条件（更新零偏）
// reset零偏之后，之前的IMU预积分量就清零了
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
// 一个简单的失败检测
// imu因子图优化结果，速度或者偏置过大，认为失败。进行状态复位
if (failureDetection(prevVel_, prevBias_))
{
// 状态异常就直接复位了
resetParams();
return;
}
// 优化之后，执行重传播；
// 优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
// 2. after optiization, re-propagate imu odometry preintegration
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
// 这个预积分变量复位 （reset零偏之后，之前的IMU预积分量就清零了）
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;
}
}
++key;
doneFirstOpt = true;
}
订阅原始IMU数据，并处理
接听原始的IMU数据，包括加速度计的三轴线性加速度与陀螺仪的三轴旋转速度。添加这些原始的IMU测量值到imuQueOpt与imuQueImu中。以当前最新的IMU预积分器零偏值进行预测IMU里程计数据。
/**
* @brief 订阅IMU原始数据/imu/data
* 处理IMU原始数据
* 1、用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计
* 2、imu里程计位姿转到lidar系，发布里程计
* @param imu_raw
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
// 首先把imu的状态做一个简单的转换（utilty.h文件中的ParamServer类）
// shan ti xiao 使用的IMU加速度计、陀螺仪，与磁力计的坐标系朝向不同，进行一个转换
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
// 注意这里有两个imu的队列，作用不相同，一个用来执行预积分和位姿的优化，一个用来更新最新imu状态
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
// 如果没有发生过优化就return
// 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分重新计算了
if (doneFirstOpt == false)
return;
// 更新IMU时间，相邻帧之间的时间间隔
double imuTime = ROS_TIME(&thisImu);
// 如果上一帧IMU时间小于0（-1，更新激光帧之后的第一帧IMU数据）则dt=1/500(500hz)
// 反之若上一帧IMU时间大于0，则dt=当前帧IMU数据-上一帧IMU数据
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// integrate this single imu message
// 每来一个imu值就加入预积分状态中
// 注：这个预积分器的起始时刻是上一帧激光里程计时刻，而不是0时刻
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
// 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态
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
// 将这个状态转到lidar坐标系下去发送出去
// 增量位姿（"odometry/imu_incremental"） 从上一帧激光里程计到当前的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);
}
错误检测
检测优化的结果是否在合理范围内：
/**
* @brief 检测优化后的结果是否正确
* 检查优化后的速度prevVel_ 与 零偏 prevBias_是否在合理范围内
* @param velCur
* @param biasCur
* @return true
* @return false
*/
bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
{
// 速度
Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
// 如果当前速度大于30m/s，108km/h就认为是异常状态，
if (vel.norm() > 30)
{
ROS_WARN("Large velocity, reset IMU-preintegration!");
return true;
}
// 加速度计零偏 与 陀螺仪零偏
Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
// 如果零偏太大，那也不太正常
if (ba.norm() > 1.0 || bg.norm() > 1.0)
{
ROS_WARN("Large bias, reset IMU-preintegration!");
return true;
}
return false;
}
TransformFusion类
准备工作
- 变量定义
/**
* @brief 变换融合
* 1、接收预积分类发送的incremental消息，mapoptimization节点发送的lidar里程计消息
* 2、把最新的IMU预积分变换增量，叠加到lidar里程计上，发布/odometry/imu
* 3、以10hz的频率发布上一激光里程计到当前激光里程计之间的IMU轨迹，lio_sam/imu/path
*/
class TransformFusion : public ParamServer
{
public:
std::mutex mtx;
// 订阅地图优化节点的全局位姿和预积分节点的增量位姿
ros::Subscriber subImuOdometry; // 预积分节点的增量位姿 "odometry/imu_incremental"
ros::Subscriber subLaserOdometry; // 全局位姿 "lio_sam/mapping/odometry"
// 发布IMU里程计与轨迹
ros::Publisher pubImuOdometry;
ros::Publisher pubImuPath;
// lidar里程计，IMU里程计
Eigen::Affine3f lidarOdomAffine;
// 用于计算IMU数据的增量imuOdomAffineIncre
Eigen::Affine3f imuOdomAffineFront;
Eigen::Affine3f imuOdomAffineBack;
// 接听tf消息
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;
// lidar里程计时间
double lidarOdomTime = -1;
deque<nav_msgs::Odometry> imuOdomQueue;
- 构造函数
TransformFusion()
{
// 如果lidar帧和baselink帧不是同一个坐标系，则查询是否有tf消息发送两者之间的转换关系
// 通常baselink指车体系 参数文件中lidar与车体系相同，都是baselink
if(lidarFrame != baselinkFrame)
{
try
{
// 查询一下lidar和baselink之间的tf变换
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
}
// 订阅地图优化节点的全局位姿（lio_sam/mapping/odometry）， 和预积分节点的增量位姿（"odometry/imu_incremental"）
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
// 发布IMU里程计"odometry/imu" (最新的预测的位姿,lidarOdomAffine * imuOdomAffineIncre;)
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
// 发布IMU路径"lio_sam/imu/path"
pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);
}
- 常用自定义函数
// nav_msgs::Odometry格式转换为Eigen::Affine3f格式数据
Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
{
double x, y, z, roll, pitch, yaw;
x = odom.pose.pose.position.x;
y = odom.pose.pose.position.y;
z = odom.pose.pose.position.z;
tf::Quaternion orientation;
tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}
/**
* @brief 接听图优化节点的全局位姿 将全局位姿保存下来
*
* @param odomMsg
*/
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// lidar里程计数据（Affine3f格式）
lidarOdomAffine = odom2affine(*odomMsg);
// 记录lidar里程计消息的时间
lidarOdomTime = odomMsg->header.stamp.toSec();
}
订阅LiDAR里程计数据并处理
/**
* @brief 接听图优化节点的全局位姿 将全局位姿保存下来
*
* @param odomMsg
*/
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// lidar里程计数据（Affine3f格式）
lidarOdomAffine = odom2affine(*odomMsg);
// 记录lidar里程计消息的时间
lidarOdomTime = odomMsg->header.stamp.toSec();
}
订阅IMU里程计数据并处理
// 接听预积分节点的增量位姿 odomTopic+"_incremental"
/**
* 订阅imu里程计，来自本文件中的IMUPreintegration类
* 1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
* 2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径（只是最近一帧激光里程计时刻与当前时刻之间的一段）
*/
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
// static tf
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
// 发送静态tf，odom系和map系将他们重合（分别是”map“与”odom“，这里设置的变换为单位阵）
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
std::lock_guard<std::mutex> lock(mtx);
// imu得到的里程记结果送入这个队列中
imuOdomQueue.push_back(*odomMsg);
// get latest odometry (at current IMU stamp)
// 如果没有收到lidar位姿就return
if (lidarOdomTime == -1)
return;
// 弹出时间戳小于最新lidar位姿时刻之前的imu里程记数据
while (!imuOdomQueue.empty())
{
// 弹出比lidarOdomTime时间戳早的odomTopic+"_incremental"消息
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
// 计算最新队列里imu里程记的增量 Front对应于当前lidar里程计时间，back是最新的IMU数据
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
// 以当前Lidar里程计时间为基础，IMU的增量
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
// 增量补偿到lidar的位姿上去，就得到了最新的预测的位姿
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
float x, y, z, roll, pitch, yaw;
// 分解成平移+欧拉角的形式 把预测的位姿提取出xyz，欧拉角
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
// publish latest odometry
// 发送最新的IMU位姿
nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
// 最新的预测的位姿，发布为odomTopic（odometry/imu）
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
// 更新tf laserOdometry转换为tcur， lidar2Baselink为接听的两个坐标系之间的变换
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
// 把预测的位姿转换成tf
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
// 这里lidar与baselink的frame都是相同的
if(lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink; // 转换
// 更新odom到baselink的tf
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);
// publish IMU path
// 发送imu里程记的轨迹
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
// 控制一下更新频率，不超过10hz
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;
// 预测的位姿 laserOdometry = imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre
pose_stamped.pose = laserOdometry.pose.pose;
// 将最新的位姿送入轨迹中
imuPath.poses.push_back(pose_stamped);
// 把lidar时间戳之前的轨迹全部擦除
while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
// 发布轨迹，这个轨迹实际上是可视化imu预积分节点输出的预测值
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}
};
Main函数
初始化一个IMUPreintegration类与TransformFusion 类，并执行：
int main(int argc, char** argv)
{
ros::init(argc, argv, "roboat_loam");
/**
* @brief IMU预积分类
* 订阅IMU原始数据消息（imuTopic， /imu/data）
* 订阅后端mapOptmization模块发出的"lio_sam/mapping/odometry_incremental"消息
* 发布预积分计算的增量位姿（"odometry/imu_incremental"）从最近的LiDAR帧时刻到当前IMU帧时刻的里程计增量
*/
IMUPreintegration ImuP;
/**
* @brief 转换增量，与最新的LiDAR里程计结合，输出预测的位姿
* 订阅地图优化节点的全局位姿（lio_sam/mapping/odometry）
* 订阅本节点的增量位姿（"odometry/imu"+"_incremental"）
* 发布IMU里程计"odometry/imu" (最新的预测的位姿,lidarOdomAffine * imuOdomAffineIncre;)
* 发布组成的IMU路径"lio_sam/imu/path"
*/
TransformFusion TF;
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
// TransformFusion类有两个subscriber
// IMUPreintegration类有两个subscriber
// 所以共设置线程为4
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}