imuPreintegration.cpp
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");
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}
2.IMUPreintegration类
2.1 成员变量
class IMUPreintegration : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImu; // imu信息订阅器
ros::Subscriber subOdometry; // 最终优化后的里程计增量信息(用来矫正imu的偏置)
ros::Publisher pubImuOdometry; // 估计的imu里程计信息发布器(其实是通过imu估计的雷达里程计信息)
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;
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
std::deque<sensor_msgs::Imu> imuQueOpt;
std::deque<sensor_msgs::Imu> imuQueImu;
gtsam::Pose3 prevPose_; // 上一时刻估计imu的位姿信息
gtsam::Vector3 prevVel_; // 上一时刻估计imu的速度信息
gtsam::NavState prevState_;
gtsam::imuBias::ConstantBias prevBias_;
gtsam::NavState prevStateOdom;
gtsam::imuBias::ConstantBias prevBiasOdom;
bool doneFirstOpt = false;
double lastImuT_imu = -1;
double lastImuT_opt = -1;
gtsam::ISAM2 optimizer;
gtsam::NonlinearFactorGraph graphFactors;
gtsam::Values graphValues;
const double delta_t = 0;
int key = 1;
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()));
2.2 IMUPreintegration构造函数
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);
// 定义进行imu积分的imu传感器信息
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // 加速度计的白噪声
p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2); // 陀螺仪的白噪声
p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); // 通过速度积分位置信息引入的噪声
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // 初始化imu偏置信息
priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // 初始化位姿的噪声
priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // 初始化速度的噪声
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 初始化偏置的噪声
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预积分器，一个用于imu信息处理线程，一个用于优化线程
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
}
2.3 resetOptimization()
void resetOptimization()
{
// 重置isam2优化器
gtsam::ISAM2Params optParameters;
optParameters.relinearizeThreshold = 0.1;
optParameters.relinearizeSkip = 1;
optimizer = gtsam::ISAM2(optParameters);
// 重置初始化非线性因子图
gtsam::NonlinearFactorGraph newGraphFactors;
graphFactors = newGraphFactors;
gtsam::Values NewGraphValues;
graphValues = NewGraphValues;
}
2.4 resetParams()
// 对这几个变量进行重置
void resetParams()
{
lastImuT_imu = -1;
doneFirstOpt = false;
systemInitialized = false;
}
2.5 imuHandler()
void imuHandler(const sensor_msgs::Imu::ConstPtr &imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
sensor_msgs::Imu thisImu = imuConverter(*imu_raw); // 将imu信息转换到雷达坐标系下表达,其实也就是获得雷达运动的加速度、角速度和姿态信息
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu); // 获取相邻两帧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);
// predict odometry
// 利用上一时刻的imu里程计状态信息PVQ和偏置信息，预积分当前时刻imu里程计状态信息PVQ
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); // 获得估计的雷达位姿信息
// 发布通过imu估计的雷达里程计信息(后面都称为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);
}
2.6 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数据积分了
if (imuQueOpt.empty())
return;
// 转换消息数据为gtsam的3d位姿信息
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(); // 初始化isam2优化器及非线性因子图
// pop old IMU message
// 丢弃老的imu信息
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// initial pose
// 通过最终优化过的雷达位姿初始化先验的位姿信息并添加到因子图中
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
// 初始化先验偏置信息为0并添加到因子图中
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
// 将因子图更新到isam2优化器中
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return;
}
// reset graph for speed
// 当isam2规模太大时，进行边缘化，重置优化器和因子图
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
// 重置isam2优化器和因子图
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
// 并将最新初始化的因子图更新到重置的isam2优化器中
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1; // 重置关键帧数量
}
// 1. integrate imu data and optimize
// 1. 预积分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);
if (imuTime < currentCorrectionTime - delta_t)
{
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
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因子添加到因子图中
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
// 将imu偏置因子添加到因子图中
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);
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
// 将最新关键帧相关的因子图更新到isam2优化器中，并进行优化
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.
// 利用优化后的imu偏置信息重置imu预积分对象
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
// 对优化结果进行失败检测: 当速度和偏置太大时，则认为优化失败
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}
// 2. after optiization, re-propagate imu odometry preintegration
// 2. 优化后，重新对imu里程计进行预积分
// 利用优化结果更新prev状态
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
// 丢弃早于矫正时间的imu帧
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
// 将优化后的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;
}
2.7 failureDetection()
bool failureDetection(const gtsam::Vector3 &velCur, const gtsam::imuBias::ConstantBias &biasCur)
{
Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
// 当速度太大，则认为失败
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;
}
3 TransformFusion类
3.1 成员变量
/* 将最终优化过的里程计信息添加上后面imu里程计增加的里程计信息构成最新的imu里程计信息 */
class TransformFusion : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImuOdometry; // 通过imu积分估计的雷达里程计信息订阅器
ros::Subscriber subLaserOdometry; // 最终优化后的里程计信息订阅器
ros::Publisher pubImuOdometry; // imu里程计信息发布器
ros::Publisher pubImuPath; // imu路径发布器
Eigen::Affine3f lidarOdomAffine;
Eigen::Affine3f imuOdomAffineFront;
Eigen::Affine3f imuOdomAffineBack;
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;
double lidarOdomTime = -1;
deque<nav_msgs::Odometry> imuOdomQueue;
3.2 构造函数
TransformFusion()
{
// 如果雷达坐标系和基座标系不一致，则获取雷达坐标系相对于基座标系的转换关系
if (lidarFrame != baselinkFrame)
{
try
{
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());
}
}
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());
// TransformFusion这个类产生的数据没有被其它节点使用，只是单纯的为了rviz显示用，所以这个类可以去掉，不影响最后的建图结果
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000); // 该话题没有被任何其它节点利用
pubImuPath = nh.advertise<nav_msgs::Path>("lio_sam/imu/path", 1); // 该话题只为显示用
}
3.3 odom2affine()
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);
}
3.4 lidarOdometryHandler()
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
lidarOdomAffine = odom2affine(*odomMsg);
lidarOdomTime = odomMsg->header.stamp.toSec();
}
3.5 imuOdometryHandler()
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));
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
std::lock_guard<std::mutex> lock(mtx);
imuOdomQueue.push_back(*odomMsg); // 记录通过imu估计的雷达里程计信息(后面简称imu里程计信息)
// get latest odometry (at current IMU stamp)
// 当没有订阅到最终优化后的里程计信息时，直接返回
if (lidarOdomTime == -1)
return;
// 当订阅到最终优化后的里程计信息时，剔除掉比该帧还老的imu里程计信息帧
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front()); // 获取最老的imu里程计信息
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back()); // 获取最新的imu里程计信息
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack; // 获取最老最新帧之间的位姿增量
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; // 在最新的最终优化后的里程计信息基础上叠加imu里程计位姿增量，获得最新的imu里程计信息
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw); // 通过仿射变换提取出位置信息和用欧拉角表示的姿态信息
// publish latest odometry
// 发布最新的imu里程计信息
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
// 发布最新的odom与base_link之间的转换关系，为了rviz显示imu里程计路径用
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
// 发布imu对应的路径信息
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);
}
}
}
总结
IMUPreintegration 订阅 lio_sam/mapping/odometry_incremental 和 imu_correct , 使用gtsam进行imu与激光里程计的紧耦合优化，发布优化后的 odometry/imu_incremental。
TransformFusion 订阅 IMUPreintegration发布的odometry/imu_incremental，以及激光里程计lio_sam/mapping/odometry 。
imuOdometryHandler 只是将 odometry/imu_incremental 中的xyz值，使用lio_sam/mapping/odometry中的值，乘以相应时间内的odometry/imu_incrementa的变化量，得到新的xyz进行替代; 并发布map->odom->base_link的tf。