欧美日韩一区二区三区免费,国产强奸乱伦中文字幕一区二区三区四区 ,91精品一区二区三区久久久久久

前言

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

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

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

Eigen::Affine3f

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

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

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

創建Eigen::Affine3f 對象a。

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

創建類型為Eigen::Quaternionf 四元數對象c，用來存儲旋轉變換；

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

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

仿射變換包括：平移、旋轉、放縮、剪切、反射

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

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

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

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

位姿融合輸出

在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("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());    subImuOdometry  = nh.subscribe(odomTopic+"_incremental",  2000, &TransformFusion::imuOdometryHandler,  this, ros::TransportHints().tcpNoDelay());

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

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

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

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

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

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

建圖的話可以認為 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預積分節點輸出的預測值

result