1. 代码流程

2. 功能说明

这个cpp文件主要有两个类，一个叫IMUPreintegration类，一个叫TransformFusion类。

现在我们分开讲，先说IMUPreintegration类。

关于IMU原始数据，送入imuhandle中：

2.1. imuhandle

imu原始数据，会先被坐标系转换，通过调用头文件里的imuConverter函数，转换到一个“雷达中间系”中，其实和真正的雷达系差了一个平移。

转换后，会存入两个队列，一个imuQueOpt队列，一个imuQueImu队列。这两队列有什么区别和联系呢？这个主要在另一个回调函数odometryHandler会被处理，在那个地方我会讲。这里我们可以先理解为，imuQueImu是真正我们要用的数据，imuQueOpt是一个中间缓存的数据结构，用来优化imu的bias之类的东西。

在标志位doneFirstOpt为True的时候（注意这个标志位，这是一个很重要的变量，之后会再提到），每到一个imu数据，就用imuIntegratorImu_这个预积分器，把这一帧imu数据累积进去，然后预测当前时刻的状态：currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系统所保持的状态。

把currentState，通过imu2Lidar，从“中间系”给平移到真正的雷达系，然后发布出去。发布的话题就叫odometry/imu_incremental，这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。

2.2. odomHandle

这部分订阅的是/mapping/odometry_incremental，这个话题是由mapOptmization.cpp发布的，可以把它理解为激光里程计。同理，也不要被incremental误导，觉得好像是两帧激光之间的变换，可不是这样的啊。它和imu里程计性质类似，就是相对世界坐标系的位姿。

2.2.1. 初始化系统

从imuQueOpt队列里，删掉当前这帧激光里程计时间上之前的数据，然后把雷达的pose变换到“中间系”，保存为prevPose。图优化部分，加入乱七八糟各种东西，priorVel，priorBias，把两个预积分器imuIntegratorImu_，imuIntegratorOpt_给reset一下。（之后简称imu积分器和opt积分器）

这两个预积分器opt积分器和imu积分器有什么区别呢？马上就要讲，在1.2.3部分。

2.2.2. 清理缓存

100帧激光里程计数据了，就把优化器给reset一下（用前面的先验协方差给reset一下）。注意，1.2.1和1.2.2的主要区别在于，1.2.1中的乱七八糟协方差数据，是用构造函数中写入的一大堆（我认为是经验值），而1.2.2这里的协方差，用的是optimizer.marginalCovariance这个轮子算出来的先验协方差。

2.2.3. 正式处理

假设数据如下分布：

之前imu数据 ——————第一帧开始——————第二帧开始————之后imu数据

把“第一帧开始”——“第二帧开始”这个之间的imu数据拿出来，送入opt积分器。这样得到二者之间的预积分，构建imu因子。

然后把Xkey-1 到Xkey之间，加入这个imu因子以及激光里程计提供的pose因子，整体做一个优化。优化的结果就是bias，以及“第二帧开始”这个时刻的系统位姿。

把优化的结果（主要是bias），重置opt积分器和imu积分器。然后把当前帧（上图的“第二帧开始”）之前的数据给删掉，用imu积分器，从“第二帧开始”这里开始往后积分。（我们需要明确一点，在这个处理过程中，imu队列也在持续的进数据，（即1.1的imuhandle中）），这里处理完，那么就置donefirst=True，这样1.1.3部分，就可以无缝衔接接着在这里的基础上对imu积分器继续积分。（可以看出，这点处理，Tixiaoshan还是做的比较牛的）

回顾：在1.1.3部分，发布imu里程计，在这里我们可以的出结果，它并非是纯粹的imu里程计，因为时不时是要加入激光里程计的信息做因子来优化得到imu的bias等信息的。

2.3. TransformFusion类。

2.3.1 lidarOdometryHandler

这部分监听的是/mapping/odometry，（也就是激光雷达里程计）这个回调函数比较特殊，它并没有把雷达里程计的东西再像别的回调函数一样，时刻存到什么队列里去。而是就保存当前的雷达里程计的数据到lidarOdomAffine里面，把时间戳存到lidarOdomTime里面去。

注意，这里/mapping/odometry和/mapping/odometry_incremental有什么区别呢？我认为本质上区别不大，前者是激光里程计部分直接优化得到的激光位姿，后者相当于激光的位姿经过旋转约束和z轴约束的限制以后，又和原始imu信息里面的角度做了一个加权以后的位姿。

2.3.2 imuOdometryHandler

这部分监听的是/imu/odometry_incremental（也就是上面我一直在说的imu里程计），把imu里程计放到imuodomQueue里面保存，然后把lidarOdomTime之前的数据扔掉，用imu里程计的两时刻差异，再加上lidarOdomAffine的基础进行发布。

实际上，/imu/odometry_incremental本身就是雷达里程计基础上imu数据上的发布，而在现在这里，也是雷达里程计在“imu里程计”上的一个“再次精修”。最后会发布两个内容，一个是odometry/imu，但是这个实际上是无人监听的，我觉得作者主要是发布tf变换，顺手给它发布了。当然我觉得用户可以监听这个数据，我觉得这个应该是频率上最高的一个里程计数据了。

另外还会发布一个path，用来rviz显示，名字叫lio-sam/imu/path。

3. 代码

#include "utility.h"#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)class TransformFusion : public ParamServer
{
public:std::mutex mtx;ros::Subscriber subImuOdometry;ros::Subscriber subLaserOdometry;ros::Publisher pubImuOdometry;ros::Publisher pubImuPath;Eigen::Affine3f lidarOdomAffine;Eigen::Affine3f imuOdomAffineFront;Eigen::Affine3f imuOdomAffineBack;tf::TransformListener tfListener;tf::StampedTransform lidar2Baselink;double lidarOdomTime = -1;deque<nav_msgs::Odometry> imuOdomQueue;/*** 构造函数*/TransformFusion(){// 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的变换关系if(lidarFrame != baselinkFrame){try{// 等待3stfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));// lidar系到baselink系的变换tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());}}// 订阅激光里程计，来自mapOptimizationsubLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());// 订阅imu里程计，来自IMUPreintegrationsubImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());// 发布imu里程计，用于rviz展示pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);// 发布imu里程计轨迹pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);}/*** 里程计对应变换矩阵*/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);}/*** 订阅激光里程计，来自mapOptimization*/void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){std::lock_guard<std::mutex> lock(mtx);// 激光里程计对应变换矩阵lidarOdomAffine = odom2affine(*odomMsg);// 激光里程计时间戳lidarOdomTime = odomMsg->header.stamp.toSec();}/*** 订阅imu里程计，来自IMUPreintegration* 1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿* 2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段*/void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){// 发布tf，map与odom系设为同一个系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);// 添加imu里程计到队列imuOdomQueue.push_back(*odomMsg);// 从imu里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据if (lidarOdomTime == -1)return;while (!imuOdomQueue.empty()){if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)imuOdomQueue.pop_front();elsebreak;}// 最近的一帧激光里程计时刻对应imu里程计位姿Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());// 当前时刻imu里程计位姿Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());// imu里程计增量位姿变换Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;// 最近的一帧激光里程计位姿 * imu里程计增量位姿变换 = 当前时刻imu里程计位姿Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);// 发布当前时刻里程计位姿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);// 发布tf，当前时刻odom与baselink系变换关系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);// 发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段static nav_msgs::Path imuPath;static double last_path_time = -1;double imuTime = imuOdomQueue.back().header.stamp.toSec();// 每隔0.1s添加一个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);// 删除最近一帧激光里程计时刻之前的imu里程计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);}}}
};class IMUPreintegration : public ParamServer
{
public:std::mutex mtx;// 订阅与发布ros::Subscriber subImu;ros::Subscriber subOdometry;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预积分器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_;gtsam::imuBias::ConstantBias prevBias_;// imu状态gtsam::NavState prevStateOdom;gtsam::imuBias::ConstantBias prevBiasOdom;bool doneFirstOpt = false;double lastImuT_imu = -1;double lastImuT_opt = -1;// ISAM2优化器gtsam::ISAM2 optimizer;gtsam::NonlinearFactorGraph graphFactors;gtsam::Values graphValues;const double delta_t = 0;int key = 1;// imu-lidar位姿变换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原始数据，用下面因子图优化的结果，施加两帧之间的imu预计分量，预测每一时刻（imu频率）的imu里程计subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());// 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());// 发布imu里程计pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);// imu预积分的噪声协方差boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuousp->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuousp->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocitiesgtsam::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, mpriorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/spriorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good// 激光里程计scan-to-map优化过程中发生退化，则选择一个较大的协方差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, mcorrectionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, mnoiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();// imu预积分器，用于预测每一时刻（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        }/*** 重置ISAM2优化器*/void resetOptimization(){gtsam::ISAM2Params optParameters;optParameters.relinearizeThreshold = 0.1;optParameters.relinearizeSkip = 1;optimizer = gtsam::ISAM2(optParameters);gtsam::NonlinearFactorGraph newGraphFactors;graphFactors = newGraphFactors;gtsam::Values NewGraphValues;graphValues = NewGraphValues;}/*** 重置参数*/void resetParams(){lastImuT_imu = -1;doneFirstOpt = false;systemInitialized = false;}/*** 订阅激光里程计，来自mapOptimization* 1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化* 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态* 3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿*/void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){std::lock_guard<std::mutex> lock(mtx);// 当前帧激光里程计时间戳double currentCorrectionTime = ROS_TIME(odomMsg);// 确保imu优化队列中有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::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));// 0. 系统初始化，第一帧if (systemInitialized == false){// 重置ISAM2优化器resetOptimization();// 从imu优化队列中删除当前帧激光里程计时刻之前的imu数据while (!imuQueOpt.empty()){if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t){lastImuT_opt = ROS_TIME(&imuQueOpt.front());imuQueOpt.pop_front();}elsebreak;}// 添加里程计位姿先验因子prevPose_ = lidarPose.compose(lidar2Imu);gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);graphFactors.add(priorPose);// 添加速度先验因子prevVel_ = gtsam::Vector3(0, 0, 0);gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);graphFactors.add(priorVel);// 添加imu偏置先验因子prevBias_ = gtsam::imuBias::ConstantBias();gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);graphFactors.add(priorBias);// 变量节点赋初值graphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// 优化一次optimizer.update(graphFactors, graphValues);graphFactors.resize(0);graphValues.clear();// 重置优化之后的偏置imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);key = 1;systemInitialized = true;return;}// 每隔100帧激光里程计，重置ISAM2优化器，保证优化效率if (key == 100){// 前一帧的位姿、速度、偏置噪声模型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)));// 重置ISAM2优化器resetOptimization();// 添加位姿先验因子，用前一帧的值初始化gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);graphFactors.add(priorPose);// 添加速度先验因子，用前一帧的值初始化gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);graphFactors.add(priorVel);// 添加偏置先验因子，用前一帧的值初始化gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);graphFactors.add(priorBias);// 变量节点赋初值，用前一帧的值初始化graphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// 优化一次optimizer.update(graphFactors, graphValues);graphFactors.resize(0);graphValues.clear();key = 1;}// 1. 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态while (!imuQueOpt.empty()){// 提取前一帧与当前帧之间的imu数据，计算预积分sensor_msgs::Imu *thisImu = &imuQueOpt.front();double imuTime = ROS_TIME(thisImu);if (imuTime < currentCorrectionTime - delta_t){// 两帧imu数据时间间隔double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);// imu预积分数据输入：加速度、角速度、dtimuIntegratorOpt_->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;// 从队列中删除已经处理的imu数据imuQueOpt.pop_front();}elsebreak;}// 添加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);// 添加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)));// 添加位姿因子gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);graphFactors.add(pose_factor);// 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);// 变量节点赋初值graphValues.insert(X(key), propState_.pose());graphValues.insert(V(key), propState_.v());graphValues.insert(B(key), prevBias_);// 优化optimizer.update(graphFactors, graphValues);optimizer.update();graphFactors.resize(0);graphValues.clear();// 优化结果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));// 重置预积分器，设置新的偏置，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);// imu因子图优化结果，速度或者偏置过大，认为失败if (failureDetection(prevVel_, prevBias_)){// 重置参数resetParams();return;}// 2. 优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿prevStateOdom = prevState_;prevBiasOdom  = prevBias_;// 从imu队列中删除当前激光里程计时刻之前的imu数据double lastImuQT = -1;while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t){lastImuQT = ROS_TIME(&imuQueImu.front());imuQueImu.pop_front();}// 对剩余的imu数据计算预积分if (!imuQueImu.empty()){// 重置预积分器和最新的偏置imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);// 计算预积分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因子图优化结果，速度或者偏置过大，认为失败*/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;}/*** 订阅imu原始数据* 1、用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计* 2、imu里程计位姿转到lidar系，发布里程计*/void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw){std::lock_guard<std::mutex> lock(mtx);// imu原始测量数据转换到lidar系，加速度、角速度、RPYsensor_msgs::Imu thisImu = imuConverter(*imu_raw);// 添加当前帧imu数据到队列imuQueOpt.push_back(thisImu);imuQueImu.push_back(thisImu);// 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分重新计算了if (doneFirstOpt == false)return;double imuTime = ROS_TIME(&thisImu);double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);lastImuT_imu = imuTime;// imu预积分器添加一帧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);// 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);// 发布imu里程计（转到lidar系，与激光里程计同一个系）nav_msgs::Odometry odometry;odometry.header.stamp = thisImu.header.stamp;odometry.header.frame_id = odometryFrame;odometry.child_frame_id = "odom_imu";// 变换到lidar系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);}
};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;
}