在imu預積分的節點中，在main函數里面 還有一個類的實例對象，那就是TransformFusion TF。

其主要功能是做位姿融合輸出，最終輸出imu的預測結果，與上節中的imu預測結果的區別就是：

該對象的融合輸出是基于全局位姿的基礎上再進行imu的預測輸出。全局位姿就是 經過回環檢測后的lidar位姿。

建圖優化會輸出兩種激光雷達的位姿：

• lidar 增量位姿
• lidar 全局位姿

其中lidar 增量位姿就是 通過 lidar的匹配功能，優化出的幀間的相對位姿，通過相對位姿的累積，形成世界坐標系下的位姿

lidar全局位姿 則是在 幀間位姿的基礎上，通過 回環檢測，再次進行優化的 世界坐標系下的位姿，所以對于增量位姿，全局位姿更加精準

在前面提到的發布的imu的預測位姿是在lidar的增量位姿上基礎上預測的，那么為了更加準確，本部分功能就預測結果，計算到基于全局位姿的基礎上面。首先看構造函數

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

判斷lidar幀和baselink幀是不是同一個坐標系，通常baselink指車體系，如果不是，查詢 一下 lidar 和baselink 之間的 tf變換 ros::Time(0) 表示最新的，等待兩個坐標系有了變換，更新兩個的變換 lidar2Baselink

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

訂閱地圖優化的節點的全局位姿 和預積分節點的 增量位姿

pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);        pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);

發布兩個信息 odomTopic ImuPath，然后看第一個回調函數 lidarOdometryHandler

void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)    {        std::lock_guard< std::mutex > lock(mtx);        lidarOdomAffine = odom2affine(*odomMsg);        lidarOdomTime = odomMsg- >header.stamp.toSec();    }

將全局位姿保存下來，將ros的odom格式轉換成 Eigen::Affine3f 的形式，將最新幀的時間保存下來，第二個回調函數是 imuOdometryHandler，imu預積分之后所發布的imu頻率的預測位姿

void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)    {

static tf::TransformBroadcaster tfMap2Odom;        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));

建圖的話 可以認為 map坐標系和odom坐標系 是重合的（初始化時刻）

tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg- >header.stamp, mapFrame, odometryFrame));

發布靜態tf，odom系和map系 他們是重合的

imuOdomQueue.push_back(*odomMsg);

imu得到的里程計結果送入到這個隊列中

if (lidarOdomTime == -1)            return;

如果沒有收到lidar位姿 就returen

while (!imuOdomQueue.empty())        {            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)                imuOdomQueue.pop_front();            else                break;        }

彈出時間戳 小于 最新 lidar位姿時刻之前的imu里程計數據

Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;

計算最新隊列里imu里程計的增量

Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;

增量補償到lidar的位姿上去，就得到了最新的預測的位姿

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

發送全局一致位姿的 最新位姿

static tf::TransformBroadcaster tfOdom2BaseLink;        tf::Transform tCur;        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);        if(lidarFrame != baselinkFrame)            tCur = tCur * lidar2Baselink;

更新tf

tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg- >header.stamp, odometryFrame, baselinkFrame);        tfOdom2BaseLink.sendTransform(odom_2_baselink);

更新odom到baselink的tf

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

發布imu里程計的軌跡，控制一下更新頻率，不超過10hz，將最新的位姿送入軌跡中，把lidar時間戳之前的軌跡全部擦除，發布軌跡，這個軌跡實踐上是可視化imu預積分節點輸出的預測值。