LIO-SAM框架位姿融合输出

在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预积分节点输出的预测值。