前言
LIO-SAM的全稱是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
從全稱上可以看出，該算法是一個緊耦合的雷達慣導里程計（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM庫中的方法。
LIO-SAM 提出了一個利用GT-SAM的緊耦合激光雷達慣導里程計的框架。
實現(xiàn)了高精度、實時的移動機器人的軌跡估計和建圖。
在之前的博客講解了imu如何進行預積分，最終以imu的頻率發(fā)布了imu的預測位姿里程計。
本篇博客主要講解，最終是如何進行位姿融合輸出的
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，前者每個坐標軸放縮系數(shù)相同（各向同性），后者不同；如果放縮系數(shù)為負，則會疊加上反射（reflection）——reflection可以看成是特殊的scaling；剛體變換+uniform scaling 稱之為，相似變換（similarity transformation），即平移+旋轉(zhuǎn)+各向同性的放縮；
位姿融合輸出
在imu預積分的節(jié)點中，在main函數(shù)里面 還有一個類的實例對象，那就是
TransformFusion TF
其主要功能是做位姿融合輸出
最終輸出imu的預測結果，與上節(jié)中的imu預測結果的區(qū)別就是：
該對象的融合輸出是基于全局位姿的基礎上再進行imu的預測輸出。全局位姿就是 經(jīng)過回環(huán)檢測后的lidar位姿。
該對象的本質(zhì)功能如下圖
建圖優(yōu)化會輸出兩種激光雷達的位姿：
- lidar 增量位姿
- lidar 全局位姿
其中l(wèi)idar 增量位姿就是 通過 lidar的匹配功能，優(yōu)化出的幀間的相對位姿，通過相對位姿的累積，形成世界坐標系下的位姿
lidar全局位姿 則是在 幀間位姿的基礎上，通過 回環(huán)檢測，再次進行優(yōu)化的 世界坐標系下的位姿，所以對于增量位姿，全局位姿更加精準
在前面提到的發(fā)布的imu的預測位姿是在lidar的增量位姿上基礎上預測的，那么為了更加準確，本部分功能就預測結果，計算到基于全局位姿的基礎上面。
首先看構造函數(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幀是不是同一個坐標系
通常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());
訂閱地圖優(yōu)化的節(jié)點的全局位姿 和預積分節(jié)點的 增量位姿
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預積分之后所發(fā)布的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));
發(fā)布靜態(tài)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里程計數(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的位姿上去，就得到了最新的預測的位姿
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預積分節(jié)點輸出的預測值
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}
發(fā)布imu里程計的軌跡文章來源：http://www.zghlxwxcb.cn/news/detail-419191.html
- 控制一下更新頻率，不超過10hz
- 將最新的位姿送入軌跡中
- 把lidar時間戳之前的軌跡全部擦除
- 發(fā)布軌跡，這個軌跡實踐上是可視化imu預積分節(jié)點輸出的預測值
result
其中粉色的部分就是imu的位姿融合輸出path文章來源地址http://www.zghlxwxcb.cn/news/detail-419191.html
到了這里，關于3d激光SLAM：LIO-SAM框架---位姿融合輸出的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
