前言

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

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

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

Eigen::Affine3f

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

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

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

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

創(chuàng)建類型為Eigen::Translation3f 對(duì)象b，用來(lái)存儲(chǔ)平移向量；

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

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

一個(gè)向量通過(guò)仿射變換時(shí)的方法是result_vector=test_affine*test_vector;

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

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

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

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

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

位姿融合輸出

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

TransformFusion TF

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

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

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

lidar 增量位姿

lidar 全局位姿

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

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

在前面提到的發(fā)布的imu的預(yù)測(cè)位姿是在lidar的增量位姿上基礎(chǔ)上預(yù)測(cè)的，那么為了更加準(zhǔn)確，本部分功能就預(yù)測(cè)結(jié)果，計(jì)算到基于全局位姿的基礎(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幀是不是同一個(gè)坐標(biāo)系，通常baselink指車體系，如果不是，查詢 一下 lidar 和baselink 之間的 tf變換 ros::Time(0) 表示最新的，等待兩個(gè)坐標(biāo)系有了變換，更新兩個(gè)的變換 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é)點(diǎn)的全局位姿 和預(yù)積分節(jié)點(diǎn)的 增量位姿

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

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

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

將全局位姿保存下來(lái)，將ros的odom格式轉(zhuǎn)換成 Eigen::Affine3f 的形式，將最新幀的時(shí)間保存下來(lái)，第二個(gè)回調(diào)函數(shù)是imuOdometryHandler，imu預(yù)積分之后所發(fā)布的imu頻率的預(yù)測(cè)位姿

  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)系 是重合的（初始化時(shí)刻）

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

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

imuOdomQueue.push_back(*odomMsg);

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

    if (lidarOdomTime == -1)      return;

如果沒(méi)有收到lidar位姿 就returen

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

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

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

計(jì)算最新隊(duì)列里imu里程計(jì)的增量

Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;

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

    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();    // 控制一下更新頻率，不超過(guò)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時(shí)間戳之前的軌跡全部擦除      while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)        imuPath.poses.erase(imuPath.poses.begin());      // 發(fā)布軌跡，這個(gè)軌跡實(shí)踐上是可視化imu預(yù)積分節(jié)點(diǎn)輸出的預(yù)測(cè)值      if (pubImuPath.getNumSubscribers() != 0)      {        imuPath.header.stamp = imuOdomQueue.back().header.stamp;        imuPath.header.frame_id = odometryFrame;        pubImuPath.publish(imuPath);      }    }  }

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

result

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

審核編輯：劉清