前言

LIO-SAM的全稱是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping，從全稱上可以看出，該算法是一個緊耦合的雷達(dá)慣導(dǎo)里程計（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM庫中的方法。

LIO-SAM 提出了一個利用GT-SAM的緊耦合激光雷達(dá)慣導(dǎo)里程計的框架。實現(xiàn)了高精度、實時的移動機器人的軌跡估計和建圖。在之前的博客講解了imu如何進行預(yù)積分，最終以imu的頻率發(fā)布了imu的預(yù)測位姿里程計。

本篇博客主要講解，最終是如何進行位姿融合輸出的

Eigen::Affine3f

其中功能的核心在于 位姿間的變換，所以要了解 Eigen::Affine3f 部分的內(nèi)容，Affine3f 是eighen庫的仿射變換矩陣

實際上就是：平移向量+旋轉(zhuǎn)變換組合而成，可以同時實現(xiàn)旋轉(zhuǎn)，縮放，平移等空間變換。

Eigen庫中，仿射變換矩陣的大致用法為：

創(chuàng)建Eigen::Affine3f 對象a。

創(chuàng)建類型為Eigen::Translation3f 對象b，用來存儲平移向量；

創(chuàng)建類型為Eigen::Quaternionf 四元數(shù)對象c，用來存儲旋轉(zhuǎn)變換；

最后通過以下方式生成最終Affine3f變換矩陣：a=b*c.toRotationMatrix();

一個向量通過仿射變換時的方法是result_vector=test_affine*test_vector;

仿射變換包括：平移、旋轉(zhuǎn)、放縮、剪切、反射

平移（translation）和旋轉(zhuǎn)（rotation）顧名思義，兩者的組合稱之為歐式變換（Euclidean transformation）或剛體變換（rigid transformation）；

放縮（scaling）可進一步分為uniform scaling和non-uniform scaling，前者每個坐標(biāo)軸放縮系數(shù)相同（各向同性），后者不同；

如果放縮系數(shù)為負(fù)，則會疊加上反射（reflection）——reflection可以看成是特殊的scaling；

剛體變換+uniform scaling 稱之為，相似變換（similarity transformation），即平移+旋轉(zhuǎn)+各向同性的放縮；

位姿融合輸出

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

TransformFusion TF

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

該對象的融合輸出是基于全局位姿的基礎(chǔ)上再進行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)檢測，再次進行優(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("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());    subImuOdometry  = nh.subscribe(odomTopic+"_incremental",  2000, &TransformFusion::imuOdometryHandler,  this, ros::TransportHints().tcpNoDelay());

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

    pubImuOdometry  = nh.advertise(odomTopic, 2000);    pubImuPath    = nh.advertise  ("lio_sam/imu/path", 1);

發(fā)布兩個信息 odomTopic ImuPath，然后看第一個回調(diào)函數(shù)lidarOdometryHandler

  void lidarOdometryHandler(const nav_msgs::ConstPtr& odomMsg)  {    std::lock_guard 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::ConstPtr& odomMsg)  {

    static tf::TransformBroadcaster tfMap2Odom;    static tf::Transform map_to_odom = 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;

增量補償?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ā)布軌跡，這個軌跡實踐上是可視化imu預(yù)積分節(jié)點輸出的預(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ā)布軌跡，這個軌跡實踐上是可視化imu預(yù)積分節(jié)點輸出的預(yù)測值

result

其中粉色的部分就是imu的位姿融合輸出path。

審核編輯：劉清