系列文章链接：

【SLAM】LIO-SAM解析——流程图(1)

【SLAM】LIO-SAM解析——数据预处理imageProjection(2)

【SLAM】LIO-SAM解析——特征提取featureTrack(3)

【SLAM】LIO-SAM解析——IMU预计分IMU-Preintegration(4)

【SLAM】LIO-SAM解析——后端优化MapOptimization(5)

【SLAM】LIO-SAM解析——里程计融合transformFusion(6)

知识点：

如何合并一个低频但高精度的里程计和一个高频但低精度的里程计，输出一个高频高精度的里程计。(虽然我觉得代码里IMUPreintegration输出的里程计已经很高频且高精度了)

这是LIO-SAM里最小的一个类，它虽然和IMUPreintegration在同一个文件下，但是这个类所处的环节是最靠后的，所以分开讲。
LIO-SAM涉及到多个不同的坐标系，首先是lidar系，大部分运算都是在这个系上；baselink是指body系，一般和IMU坐标系重合。

6.1 综述

TransformFusion订阅了来自mapOptimization的lidar里程计和来自IMUPreintegration的IMU里程计。它的作用是什么呢?

以当前低频lidar里程计为基准，找到与它最近的一个IMU里程计和时间最新的一个IMU里程计，用这两个IMU里程计求出位姿的变化量，再作用到lidar位姿上，得到一个高频的里程计。

这个高频的里程计在LIO-SAM内部没有被用到，它与IMU里程计的区别是什么呢？

还记得mapOptimization发布了2个里程计吗，一个是没做处理的，另一个是与IMU数据加权融合后的IMUPreintegration特供版里程计；
前者是在这里生成高频的lidar里程计，也就是TransformFusion的输出，
后者是在IMUPreintegration与高频的IMU数据进行预积分+图优化生成高频的IMU里程计，也就是IMUPreintegration的输出；

作者在代码里搞出了这么多种数值差不了太多的里程计，第一次看着实非常的晕...

它的构造函数：

    TransformFusion(){// 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的变换关系if(lidarFrame != baselinkFrame){try{// 等待3stfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));// lidar系到baselink系的变换tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());}}// 订阅激光里程计，来自mapOptimizationsubLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());// 订阅imu里程计，来自IMUPreintegrationsubImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());// 发布高频里程计pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);// 发布imu里程计轨迹pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);}

6.2 lidarOdometryHandler()

把订阅的lidar里程计保存在buffer里，这个位姿是lidar->world的表示：

    void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){std::lock_guard<std::mutex> lock(mtx);// 激光里程计对应变换矩阵lidarOdomAffine = odom2affine(*odomMsg);// 激光里程计时间戳lidarOdomTime = odomMsg->header.stamp.toSec();}

6.3 imuOdometryHandler()

当收到IMU里程计的时候，先放到buffer里去，在把比lidar里程计时间早的IMU里程计都删掉：

        // 添加imu里程计到队列imuOdomQueue.push_back(*odomMsg);// 从imu里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据if (lidarOdomTime == -1)return;while (!imuOdomQueue.empty()){if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)imuOdomQueue.pop_front();elsebreak;}

求出最新时刻的IMU里程计和与lidar里程计最近的一个IMU里程计之间的位姿变换

        // 最近的一帧激光里程计时刻对应imu里程计位姿Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());// 当前时刻imu里程计位姿Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());// imu里程计增量位姿变换Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;

把这个位姿变换作用到lidar里程计获得最新时刻的lidar里程计，此时lidar里程计变成高频的了：

        // 最近的一帧激光里程计位姿 * imu里程计增量位姿变换 = 当前时刻imu里程计位姿Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;float x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);

发布这个高频的lidar里程计，虽然作者管它也叫IMU里程计，但是我这里把它称为高频的lidar里程计，与之前的IMU里程计作为区别：

        // 发布当前时刻里程计位姿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)

由于刚刚发布的里程计是lidar->world的位姿，这里发布一个body->world的位姿：

        // 发布tf，当前时刻odom与baselink系变换关系static tf::TransformBroadcaster tfOdom2BaseLink;tf::Transform tCur;tf::poseMsgToTF(laserOdometry.pose.pose, tCur);if(lidarFrame != baselinkFrame)tCur = tCur * lidar2Baselink;tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);tfOdom2BaseLink.sendTransform(odom_2_baselink);

发布path：

        static nav_msgs::Path imuPath;static double last_path_time = -1;double imuTime = imuOdomQueue.back().header.stamp.toSec();// 每隔0.1s添加一个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);// 删除最近一帧激光里程计时刻之前的imu里程计while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)imuPath.poses.erase(imuPath.poses.begin());if (pubImuPath.getNumSubscribers() != 0){imuPath.header.stamp = imuOdomQueue.back().header.stamp;imuPath.header.frame_id = odometryFrame;pubImuPath.publish(imuPath);}}}
};

完结。

【SLAM】LIO-SAM解析——获得高频的lidar里程计TransformFusion(6)相关推荐

激光SLAM源码解析S-LOAM（三）里程计图优化
里程计,是通过累计帧间位姿变换得来的,因此会累积帧间误差. 如果想要纠正此累积误差,我们需要通过另一种方法得到可信位姿,以此校正相同时刻里程计位姿. SLAM图优化,是一种记录各帧时刻里程计,并通过可 ...
视觉和Lidar里程计
搬来一个自动驾驶相关我就一个打酱油小弟哈自动驾驶的发展需要研究和开发准确可靠的自定位方法.其中包括视觉里程计方法,在这种方法中,精度可能优于基于GNSS的技术,同时也适用于无GPS信号的区域.本文 ...
视觉和Lidar里程计SOTA方法一览!（Camera/激光雷达/多模态）
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[SLAM]技术交流群后台回复[SLAM综述]获取视觉SLAM.激光SLAM.RGBD ...
基于LiDAR里程计和先验地图的定位方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:robot L | 来源:知乎 https://zhuanlan.zhihu.com/p/157 ...
3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
3D激光SLAM:LeGO-LOAM---两步优化的帧间里程计及代码分析前言利用地面点优化利用角点优化代码部分 gazebo测试前言 LeGO-LOAM的全称是 Lightweight an ...
ICRA 2021|用于LiDAR里程计和建图的Poisson表面重建
Poisson Surface Reconstruction for LiDAR Odometry and Mapping 作者:Ignacio Vizzo, Xieyuanli Chen, Nive ...
激光SLAM理论与实践第二次作业（里程计标定）
1.章节: 1.激光SLAM理论与实践-第五期第一次作业(矩阵坐标变换) 2.激光SLAM理论与实践-第五期第二次作业(里程计标定) 3.激光SLAM理论与实践-第五期第三次作业(去运动畸变) ...
视觉SLAM十四讲学习笔记1——视觉里程计
一.基础概念 1.按照相机的工作方式,我们把相机分为单目(Monocular).双目(Stereo)和深度相机(RGB-D)三个大类. 2.照片,本质上是拍照时的场景(Scene),在相机的成像平面 ...
SLAM | 使用三维位姿图优化减少单目视觉里程计（3D Visual Odometry）定位轨迹的漂移（附源代码）
===================================================== github:https://github.com/MichaelBeechan CSDN: ...

【SLAM】LIO-SAM解析——获得高频的lidar里程计TransformFusion(6)

系列文章链接：

6.1 综述

6.2 lidarOdometryHandler()

6.3 imuOdometryHandler()

【SLAM】LIO-SAM解析——获得高频的lidar里程计TransformFusion(6)相关推荐

最新文章

热门文章