LIO-SAM

news2024/12/26 9:32:45

3D激光SLAM：位姿融合输出，LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。这里主要讲解如何通过imu来进行位姿融合输出的。

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

订阅地图优化节点的全局位姿和预积分节点的增量位姿：

        pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);        pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);

发布两个信息 odomTopic ImuPath，然后看第一个回调函数 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格式转换成 Eigen::Affine3f 的形式，将最新帧的时间保存下来，第二个回调函数是 imuOdometryHandler，imu预积分之后所发布的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));

发布静态tf，odom系和map系，他们是重合的：

imuOdomQueue.push_back(*odomMsg);

imu得到的里程计结果送入到这个队列中：

        if (lidarOdomTime == -1)            return;

如果没有收到lidar位姿就return：

        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：         whaosoft aiot  http://143ai.com

        static tf::TransformBroadcaster tfOdom2BaseLink;        tf::Transform tCur;        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);        if(lidarFrame != baselinkFrame)            tCur = tCur * lidar2Baselink;

更新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

 

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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/22055.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

修复微信小程序不能获取头像和昵称的bug，微信小程序新版头像昵称API使用

修复微信小程序不能获取头像和昵称的bug，微信小程序新版头像昵称API使用

导读： 大厂程序员都是有KPI绩效考核的，所以他们不能闲着，每天要想着怎么优化程序代码、怎么满足奇葩用户的需求，所以苦逼了我们这些小公司程序员，微信一个小小的API接口改动，可能就让一个小公司因此损失惨…
阅读更多...
人工智能——大白话熟悉目标检测基本流程

人工智能——大白话熟悉目标检测基本流程

👦👦一个帅气的boy，你可以叫我Love And Program 🖱 ⌨个人主页：Love And Program的个人主页 💖💖如果对你有帮助的话希望三连💨💨支持一下博主 大白话熟悉目标检测基本…
阅读更多...
天然产物化合物库在肥胖中的潜在靶点 | MedChemExpress

天然产物化合物库在肥胖中的潜在靶点 | MedChemExpress

脂肪有分类？都说“燃脂”，但很少有人了解脂肪组织。其实脂肪组织主要分为两类：白色脂肪组织 (WAT) 和棕色脂肪组织 (BAT)。白色脂肪以甘油三酯的形式储存多余的能量，而棕色脂肪则通过消耗能量产热，在保暖和抵抗肥胖中起…
阅读更多...
31.nacos集成Feign和Gateway实例（springcloud）

31.nacos集成Feign和Gateway实例（springcloud）

一、项目nacos-client-a 1.pom.xml文件 新增了springcloud的依赖 新增了springcloud的依赖管理 新增了feign依赖 <?xml version"1.0" encoding"UTF-8"?> <project xmlns"http://maven.apache.org/POM/4.0.0" xmlns:xsi"http://…
阅读更多...
ArcMap10.6以上版本添加天地图底图

ArcMap10.6以上版本添加天地图底图

文章目录 申请天地图服务Key在ArcMap10.7中添加天地图服务注意点 申请天地图服务Key 天地图API&#xff1a;http://lbs.tianditu.gov.cn/server/MapService.html 需要登录后进入控制台&#xff0c;申请免费的Key&#xff1a; 在ArcMap10.7中添加天地图服务 天地图API提供…
阅读更多...
（十）Spring之回顾反射机制

（十）Spring之回顾反射机制

文章目录反射机制四要素Spring反射机制底层原理上一篇&#xff1a;&#xff08;九&#xff09;Spring之Bean的循环依赖问题反射机制四要素 反射机制调用方法&#xff0c;一般涉及到4个要素&#xff1a; 调用哪个对象的哪个方法传什么参数返回什么值 一般分为这几个步骤&…
阅读更多...
Mysql语法二：表的增删改查（简单查询）

Mysql语法二：表的增删改查（简单查询）

目录 1.新增&#xff08;Create) C 1.1 单行数据全列插入 1.2&#xff1a;多行新增指定列插入 1.3&#xff1a;思考题 2.查询&#xff08;Retrieve&#xff09;R 简单查询 2.1&#xff1a;指定列查询 2.2&#xff1a;查询字段为表达式 2.3&#xff1a;别名 as 2.4&…
阅读更多...
计算机专业毕业设计演示视频（论文+系统）_kaic

计算机专业毕业设计演示视频（论文+系统）_kaic

演示链接https://ssm2.oss-cn-beijing.aliyuncs.com/jspSSM201%E5%A4%A7%E5%AD%A6%E7%94%9F%E7%AC%AC%E4%BA%8C%E8%AF%BE%E5%A0%82%E5%AD%A6%E5%88%86%E6%88%90%E7%BB%A9%E6%B4%BB%E5%8A%A8%E6%8A%A5%E5%90%8Dvue.mp4https://ssm2.oss-cn-beijing.aliyuncs.com/jspSSM205%E6%97…
阅读更多...
操作系统之进程

操作系统之进程

操作系统 操作系统图解 这个图详细说明了计算机整个框架&#xff0c;系统调用&#xff0c;操作系统内核和驱动程序三个统称为操作系统&#xff0c;应用程序通过操作系统提供的api来调用硬件设备&#xff0c;而对于硬件设别来说&#xff0c;每个计算机的硬件设备的种类和厂家不…
阅读更多...
RNA-seq 保姆教程：差异表达分析（一）

RNA-seq 保姆教程：差异表达分析（一）

介绍 RNA-seq 目前是测量细胞反应的最突出的方法之一。RNA-seq 不仅能够分析样本之间基因表达的差异&#xff0c;还可以发现新的亚型并分析 SNP 变异。本教程[1]将涵盖处理和分析差异基因表达数据的基本工作流程&#xff0c;旨在提供设置环境和运行比对工具的通用方法。请注意&…
阅读更多...
L2搭载率连续两个月站上30%大关，车企加速产业链整合

L2搭载率连续两个月站上30%大关，车企加速产业链整合

进入新的行业发展周期&#xff0c;车企的智能化挑战越来越大&#xff0c;也催生新一轮整合热潮。对于全球数百家中小型智能汽车技术公司来说&#xff0c;「上岸」时机已经到来。 本周&#xff0c;全球第四大汽车制造商Stellantis宣布&#xff0c;收购总部位于匈牙利的人工智能…
阅读更多...
在 Solidity 中 ++i 为什么比 i++ 更省 Gas？

在 Solidity 中 ++i 为什么比 i++ 更省 Gas？

前言 作为一个初学者&#xff0c;“在 Solidity 中 i 为什么比 i 更省 Gas&#xff1f;” 这个问题始终在每个寂静的深夜困扰着我。也曾在网上搜索过相关问题&#xff0c;但没有得到根本性的解答。最终决定扒拉一下它们的字节码&#xff0c;从较为底层的层面看一下它们的差别究…
阅读更多...
多进程编程

多进程编程

系列文章目录 多进程编程 VS 多线程编程_crazy_xieyi的博客-CSDN博客 文章目录 前言一、进程创建二、进程等待前言 Java对操作系统提供的多进程编程接口这些操作进行了限制&#xff0c;最终给用户只提供了两个操作&#xff1a;进程创建和进程等待。 一、进程创建 创建出一个…
阅读更多...
Android 基础知识3-1项目目录结构

Android 基础知识3-1项目目录结构

上一章我们创建了Hello Word项目&#xff0c;代码是由ADT插件自动生成的&#xff0c;我们没有对其进行编码&#xff0c;所以没有对其框架进行分析。其实每一个平台都有自己的结构框架&#xff0c;所以我们对Android项目的结构也进行分析。 与一般的Java项目一样&#xff0c;src…
阅读更多...
Qt 学习(二) —— Qt工程基本文件详解

Qt 学习(二) —— Qt工程基本文件详解

目录1. pro文件内容解释2. main文件内容解释3. widget.cpp/widget.h文件内容解释4. ui_widget.h文件内容解释5. widget.ui文件内容解释以Widget窗口部件项目为例&#xff0c;新建的工程目录有如下几个文件&#xff1a; QtCreator软件将他们做了如下分组&#xff0c;包含三个文件…
阅读更多...
idea快捷搜索键

idea快捷搜索键

目录 1、shift shift 双击 2、Ctrl F在当前类中&#xff0c;页中进行查找相关方法等 3、CtrlShiftN按【文件名】搜索文件 4、CtrlH 查看类的继承关系 5、Alt F7 查看类在哪儿被使用 idea全局搜索的快捷键 1、shift shift 双击 可以搜索任何东西。类、资源、配置项…
阅读更多...
运行写在字符串中的Python代码 exec(‘‘‘print(1)‘‘‘)

运行写在字符串中的Python代码 exec(‘‘‘print(1)‘‘‘)

【小白从小学Python、C、Java】 【Python-计算机等级考试二级】 【Python-数据分析】 运行写在字符串中的Python代码 exec(print(1)) [太阳]选择题 请问对以下Python代码说法错误的是&#xff1f; print("【执行】exec(print(1))") exec(print(1)) myFuncsumab prin…
阅读更多...
CTF秀web2

CTF秀web2

CTF秀web21.分析题目2.解题2.1信息收集3.2获取数据库3.3获取数据库表3.3获取表信息3.uinon注入语句3.1 判断注入3.2 信息收集3.3注入语句1.分析题目 如上图所示&#xff0c;可以看到是sql注入的题目&#xff0c;进入题目看看&#xff0c;题目页面如下&#xff1a; 如上图所示&a…
阅读更多...
fastjson反序列化漏洞

fastjson反序列化漏洞

1.fastjson反序列化漏洞原理 我们知道fastjson在进⾏反序列化时会调⽤⽬标对象的构造&#xff0c;setter&#xff0c;getter等⽅法&#xff0c;如果这些⽅法内部 进⾏了⼀些危险的操作时&#xff0c;那么fastjson在进⾏反序列化时就有可能会触发漏洞。 我们通过⼀个简单的案例…
阅读更多...
kubernetes 资源管理

kubernetes 资源管理

kubernetes 资源管理 资源管理介绍 在kubernetes中&#xff0c;所有的内容都抽象为资源&#xff0c;用户需要通过操作资源来管理kubernetes。 kubernetes的本质上就是一个集群系统&#xff0c;用户可以在集群中部署各种服务&#xff0c;所谓的部署服务&#xff0c;其实就是在…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023