LIO-SAM框架位姿融合輸出

在imu預(yù)積分的節(jié)點(diǎn)中，在main函數(shù)里面 還有一個類的實(shí)例對象，那就是TransformFusion TF。

其主要功能是做位姿融合輸出，最終輸出imu的預(yù)測結(jié)果，與上節(jié)中的imu預(yù)測結(jié)果的區(qū)別就是：

該對象的融合輸出是基于全局位姿的基礎(chǔ)上再進(jìn)行imu的預(yù)測輸出。全局位姿就是 經(jīng)過回環(huán)檢測后的lidar位姿。

建圖優(yōu)化會輸出兩種激光雷達(dá)的位姿：

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

其中l(wèi)idar 增量位姿就是 通過 lidar的匹配功能，優(yōu)化出的幀間的相對位姿，通過相對位姿的累積，形成世界坐標(biāo)系下的位姿

lidar全局位姿 則是在 幀間位姿的基礎(chǔ)上，通過 回環(huán)檢測，再次進(jìn)行優(yōu)化的 世界坐標(biāo)系下的位姿，所以對于增量位姿，全局位姿更加精準(zhǔn)

在前面提到的發(fā)布的imu的預(yù)測位姿是在lidar的增量位姿上基礎(chǔ)上預(yù)測的，那么為了更加準(zhǔn)確，本部分功能就預(yù)測結(jié)果，計算到基于全局位姿的基礎(chǔ)上面。首先看構(gòu)造函數(shù)

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

判斷l(xiāng)idar幀和baselink幀是不是同一個坐標(biāo)系，通常baselink指車體系，如果不是，查詢 一下 lidar 和baselink 之間的 tf變換 ros::Time(0) 表示最新的，等待兩個坐標(biāo)系有了變換，更新兩個的變換 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());

訂閱地圖優(yōu)化的節(jié)點(diǎn)的全局位姿 和預(yù)積分節(jié)點(diǎn)的 增量位姿

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

發(fā)布兩個信息 odomTopic ImuPath，然后看第一個回調(diào)函數(shù) 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格式轉(zhuǎn)換成 Eigen::Affine3f 的形式，將最新幀的時間保存下來，第二個回調(diào)函數(shù)是 imuOdometryHandler，imu預(yù)積分之后所發(fā)布的imu頻率的預(yù)測位姿

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

建圖的話 可以認(rèn)為 map坐標(biāo)系和odom坐標(biāo)系 是重合的（初始化時刻）

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

發(fā)布靜態(tài)tf，odom系和map系 他們是重合的

imuOdomQueue.push_back(*odomMsg);

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

if (lidarOdomTime == -1)            return;

如果沒有收到lidar位姿 就returen

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

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

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

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

Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;

增量補(bǔ)償?shù)絣idar的位姿上去，就得到了最新的預(yù)測的位姿

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

發(fā)送全局一致位姿的 最新位姿

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());            // 發(fā)布軌跡，這個軌跡實(shí)踐上是可視化imu預(yù)積分節(jié)點(diǎn)輸出的預(yù)測值            if (pubImuPath.getNumSubscribers() != 0)            {                imuPath.header.stamp = imuOdomQueue.back().header.stamp;                imuPath.header.frame_id = odometryFrame;                pubImuPath.publish(imuPath);            }        }    }

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