代码浅析DLIO(四)---位姿更新

0. 简介

我们刚刚了解过DLIO的整个流程，我们发现相比于Point-LIO而言，这个方法更适合我们去学习理解，同时官方给出的结果来看DLIO的结果明显好于现在的主流方法，当然指的一提的是，这个DLIO是必须需要六轴IMU的，所以如果没有IMU的画，那只有DLO可以使用了。

在这里插入图片描述

1. getNextPose–通过IMU + S2M + GEO获取下一个姿态

下面的函数作用主要是获取下一个姿态的函数，主要是通过IMU、S2M和GEO三种方式获取。首先，检查新子地图是否准备就绪，如果准备好了并且子地图发生了变化，则将当前全局子地图设置为目标点云，并设置子图的kdtree以及将目标云的法线设置为子地图的法线。接着，使用全局IMU变换作为初始猜测，将当前子地图与全局地图对齐，并在全局坐标系中获取最终变换。然后，更新下一个全局位姿，并进行几何观察器更新。最终，该函数返回下一个姿态。

/*** @brief 通过IMU + S2M + GEO获取下一个姿态**/
void dlio::OdomNode::getNextPose() {// 检查新子地图是否准备好可供使用this->new_submap_is_ready =(this->submap_future.wait_for(std::chrono::seconds(0)) ==std::future_status::ready); //等待子地图准备好if (this->new_submap_is_ready &&this->submap_hasChanged) { //如果子地图准备好了,并且子地图发生了变化// 将当前全局子地图设置为目标点云this->gicp.registerInputTarget(this->submap_cloud);// 设置子图的kdtree,之前就是直接从target_kdtree_拿出来的,这个有必要嘛?this->gicp.target_kdtree_ = this->submap_kdtree;// 将目标云的法线设置为子地图的法线this->gicp.setTargetCovariances(this->submap_normals);this->submap_hasChanged = false;}// 使用全局IMU变换作为初始猜测，将当前子地图与全局地图对齐pcl::PointCloud<PointType>::Ptr aligned(boost::make_shared<pcl::PointCloud<PointType>>());this->gicp.align(*aligned); // 设置对齐后的地图// 在全局坐标系中获取最终变换this->T_corr =this->gicp.getFinalTransformation(); // 根据对齐后的地图来校准转换this->T = this->T_corr * this->T_prior;// 更新下一个全局位姿,现在源点云和目标点云都在全局坐标系中，所以变换是全局的this->propagateGICP();// 几何观察器更新this->updateState();
}

2. updateState–更新GEO信息

这段代码是通过GEO更新状态的，主要分为以下几个步骤：

  1. 锁定线程以防止状态被PropagateState访问，保证线程安全。
  2. 获取机器人当前的位置、四元数和时间差。
  3. 通过拿到的四元数和预测的四元数构造误差的四元数，然后根据误差的四元数构建四元数校正量，对应公式7。
  4. 将误差的四元数转换到机器人的body坐标系下。
  5. 更新加速度偏差和陀螺仪偏差，同时限制偏差的最大值。
  6. 更新机器人的速度和位置，以及四元数，同时存储前一个姿态、方向和速度。
void dlio::OdomNode::updateState() {// 锁定线程以防止状态被PropagateState访问std::lock_guard<std::mutex> lock(this->geo.mtx);Eigen::Vector3f pin = this->lidarPose.p;              // 位置Eigen::Quaternionf qin = this->lidarPose.q;           // 四元数double dt = this->scan_stamp - this->prev_scan_stamp; // 时间差Eigen::Quaternionf qe, qhat, qcorr;qhat = this->state.q;// 构造误差的四元数qe = qhat.conjugate() * qin; //通过拿到的四元数和预测的四元数构造误差的四元数double sgn = 1.;if (qe.w() < 0) { // 如果误差的四元数的w小于0sgn = -1;}// 构建四元数校正量,对应公式7qcorr.w() = 1 - abs(qe.w());  // 误差的四元数的w部分qcorr.vec() = sgn * qe.vec(); // 误差的四元数的向量部分qcorr = qhat * qcorr;         // 误差的四元数Eigen::Vector3f err = pin - this->state.p;Eigen::Vector3f err_body;err_body =qhat.conjugate()._transformVector(err); // 误差的四元数转换到body坐标系下double abias_max = this->geo_abias_max_;double gbias_max = this->geo_gbias_max_;// 更新加速度偏差this->state.b.accel -= dt * this->geo_Kab_ * err_body;this->state.b.accel =this->state.b.accel.array().min(abias_max).max(-abias_max);// 更新陀螺仪偏差this->state.b.gyro[0] -= dt * this->geo_Kgb_ * qe.w() * qe.x();this->state.b.gyro[1] -= dt * this->geo_Kgb_ * qe.w() * qe.y();this->state.b.gyro[2] -= dt * this->geo_Kgb_ * qe.w() * qe.z();this->state.b.gyro =this->state.b.gyro.array().min(gbias_max).max(-gbias_max);// 更新速度和位置this->state.p += dt * this->geo_Kp_ * err;this->state.v.lin.w += dt * this->geo_Kv_ * err;this->state.q.w() += dt * this->geo_Kq_ * qcorr.w();this->state.q.x() += dt * this->geo_Kq_ * qcorr.x();this->state.q.y() += dt * this->geo_Kq_ * qcorr.y();this->state.q.z() += dt * this->geo_Kq_ * qcorr.z();this->state.q.normalize();// 存储前一个姿态、方向和速度this->geo.prev_p = this->state.p;this->geo.prev_q = this->state.q;this->geo.prev_vel = this->state.v.lin.w;
}

3. updateKeyframes–更新关键帧

在做完位置更新后就会更新关键帧，通过计算当前姿态与轨迹中所有关键帧姿态的差异，然后根据距离和旋转角度是否超过一定阈值来判断是否需要更新关键帧。具体实现过程如下：

首先，遍历轨迹中所有关键帧，计算当前姿态与每个关键帧之间的距离，并记录最近的关键帧的索引和距离。同时，计算当前姿态附近的关键帧数量。

然后，获取最近关键帧的姿态和旋转，并计算当前姿态与最近关键帧之间的距离和旋转角度。如果旋转角度超过了设定的阈值，或者距离超过了设定的阈值且附近关键帧数量小于等于1，则认为需要更新关键帧。

最后，如果需要更新关键帧，则将当前姿态和点云、时间戳、法向量、变换矩阵等信息存储到关键帧向量中。

整个函数实现了关键帧的自适应更新，可以在SLAM系统中提高精度和效率。

void dlio::OdomNode::updateKeyframes() {// 计算轨迹中所有姿态和旋转的差异float closest_d = std::numeric_limits<float>::infinity();int closest_idx = 0;int keyframes_idx = 0;int num_nearby = 0;for (const auto &k : this->keyframes) {// 计算当前姿态与关键帧中的姿态之间的距离,这里和更新submap的操作一样float delta_d = sqrt(pow(this->state.p[0] - k.first.first[0], 2) +pow(this->state.p[1] - k.first.first[1], 2) +pow(this->state.p[2] - k.first.first[2], 2));//计算当前姿态附近的数量if (delta_d <= this->keyframe_thresh_dist_ * 1.5) {++num_nearby;}// 将其存储到变量中if (delta_d < closest_d) {closest_d = delta_d;         //最近的距离closest_idx = keyframes_idx; //最近的关键帧的索引}keyframes_idx++;}// 获取最接近的姿势和相应的旋转Eigen::Vector3f closest_pose =this->keyframes[closest_idx].first.first; //最近的关键帧的位置Eigen::Quaternionf closest_pose_r =this->keyframes[closest_idx].first.second; //最近的关键帧的旋转// 计算当前姿势与最近的姿势之间的距离,和closest_d一致float dd = sqrt(pow(this->state.p[0] - closest_pose[0], 2) +pow(this->state.p[1] - closest_pose[1], 2) +pow(this->state.p[2] - closest_pose[2], 2));// 使用SLERP计算方向差异Eigen::Quaternionf dq;if (this->state.q.dot(closest_pose_r) <0.) { //如果两个四元数的点积小于0，说明两个四元数的方向相反Eigen::Quaternionf lq = closest_pose_r; //将最近的关键帧的旋转赋值给lqlq.w() *= -1.;lq.x() *= -1.;lq.y() *= -1.;lq.z() *= -1.;dq = this->state.q * lq.inverse(); //计算当前姿态与最近的姿态之间的旋转} else {dq = this->state.q *closest_pose_r.inverse(); //计算当前姿态与最近的姿态之间的旋转}double theta_rad =2. * atan2(sqrt(pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2)),dq.w()); //计算当前姿态与最近的姿态之间的旋转角度double theta_deg = theta_rad * (180.0 / M_PI); //将弧度转换为角度// 更新关键帧bool newKeyframe = false;if (abs(dd) > this->keyframe_thresh_dist_ ||abs(theta_deg) >this->keyframe_thresh_rot_) { //如果距离或者旋转角度超过阈值newKeyframe = true;}if (abs(dd) <= this->keyframe_thresh_dist_ &&abs(theta_deg) > this->keyframe_thresh_rot_ &&num_nearby <=1) { //如果距离小于阈值，但是旋转角度超过阈值，且附近的关键帧数量小于等于1newKeyframe = true;}if (abs(dd) <= this->keyframe_thresh_dist_) { //如果距离小于阈值newKeyframe = false;} else if (abs(dd) <= 0.5) {newKeyframe = false;}if (newKeyframe) {// 更新关键帧向量std::unique_lock<decltype(this->keyframes_mutex)> lock(this->keyframes_mutex);this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q),this->current_scan)); //将当前的姿态和点云压入到keyframes中this->keyframe_timestamps.push_back(this->scan_header_stamp); //将当前的时间戳压入到keyframe_timestamps中this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); //将当前的法向量压入到keyframe_normals中this->keyframe_transformations.push_back(this->T_corr); //将当前的变换矩阵压入到keyframe_transformations中lock.unlock();}
}

4. propagateState–传播GEO状态

然后我们整个基本都过完了，只有一个IMU的feedback中漏掉的propagateState函数。通过IMU测量值来更新机器人的状态。首先通过一个mutex锁来确保线程安全。然后获取IMU测量的时间间隔，并将当前状态下的四元数和角速度赋值给变量qhat和omega。接下来将机体坐标系下的加速度转换到世界坐标系下，并使用加速度传播公式更新机器人的位置和速度。同时，将机器人的角速度更新为世界坐标系下的角速度。然后使用重力计传播公式更新四元数，确保其归一化。最后，将机器人的角速度和四元数更新到状态中。

…详情请参照古月居

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

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

相关文章

[WP] ISCTF2023 Web 部分题解

[WP] ISCTF2023 Web 部分题解

圣杯战争!!! 反序列化伪协议读取 where_is_the_flag 环境变量根目录当前目录 绕进你的心里 利用正则最大回溯绕过 easy_website or select 用双写绕过 空格用/**/绕&#xff0c;报错注入 wafr codesystem(ca\t /f*) webinclude 扫描得到index.bak备份文件打开为加密的代码 写…
阅读更多...
1998-2021年全国各区县PM2.5平均浓度数据

1998-2021年全国各区县PM2.5平均浓度数据

1998-2021年全国各区县PM2.5平均浓度数据 1、时间&#xff1a;1998-2021年 2、指标&#xff1a;省、省代码、市、市代码、县代码、县、年份、均值、总和、最小值、最大值、标准差 3、来源&#xff1a;Washington university Atmospheric Composition Analysis Group 4、范围…
阅读更多...
2023年第十二届数学建模国际赛小美赛C题雪崩防范求解分析

2023年第十二届数学建模国际赛小美赛C题雪崩防范求解分析

2023年第十二届数学建模国际赛小美赛 C题 雪崩防范 原题再现&#xff1a; 雪崩是极其危险的现象。现在&#xff0c;我们对雪崩是如何形成的已经有了很好的理解&#xff0c;但是我们还不能详细地预测雪崩发生的原因、时间和地点。村庄和道路可以通过各种方式防止雪崩。避免在脆…
阅读更多...
Git Hooks实战：提交前检查修改文件中是否包含调试代码

Git Hooks实战：提交前检查修改文件中是否包含调试代码

说在前面 不知道大家有没有遇到这样一种情况&#xff0c;平时在写代码调试时有时候会使用到debugger&#xff0c;可能大部分时间在提交代码前会记得把debugger先删除&#xff0c;但可能也会存在将debugger提交上去的情况&#xff0c;那我们该怎么防止出现这种情况呢&#xff1…
阅读更多...
css如何设置文本添加下划线

css如何设置文本添加下划线

css文本添加下划线 text-decoration: underline;text-decoration相关属性参数 参数描述none默认。定义标准的文本。underline定义文本下的一条线。overline定义文本上的一条线。line-through定义穿过文本下的一条线。blink定义闪烁的文本。inherit规定应该从父元素继承 text-…
阅读更多...
享元设计模式

享元设计模式

package com.jmj.pattern.flyweight;public abstract class AbstractBox {//获取图形的方法public abstract String getShape();//显示图形及颜色public void diplay(String color){System.out.println("方块形状:"getShape()",颜色:"color);}}package com…
阅读更多...
基于SpringBoot实现SSMP整合

基于SpringBoot实现SSMP整合

&#x1f648;作者简介&#xff1a;练习时长两年半的Java up主 &#x1f649;个人主页&#xff1a;程序员老茶 &#x1f64a; ps:点赞&#x1f44d;是免费的&#xff0c;却可以让写博客的作者开心好久好久&#x1f60e; &#x1f4da;系列专栏&#xff1a;Java全栈&#xff0c;…
阅读更多...
23.Python 图形化界面编程

23.Python 图形化界面编程

目录 1.认识GUI和使用tkinter2.使用组件2.1 标签2.2 按钮2.3 文本框2.4 单选按钮和复选按钮2.5 菜单和消息2.6 列表框2.7 滚动条2.8 框架2.9 画布 3. 组件布局4.事件处理 1.认识GUI和使用tkinter 人机交互是从人努力适应计算机&#xff0c;到计算机不断适应人的发展过程&#…
阅读更多...
[ 蓝桥杯Web真题 ]-年度明星项目

[ 蓝桥杯Web真题 ]-年度明星项目

目录 引入 介绍 准备 目标 效果 规定 思路 知识补充 解答参考 引入 hello&#xff0c;大家好&#xff01;我注意到了之前发的一篇蓝桥杯Web应用开发的文章是关注度最高的&#xff0c;可能大部分关注我的小伙伴对蓝桥杯Web应用开发比较感兴趣&#xff0c;或者想要参加…
阅读更多...
Flink（九）【时间语义与水位线】

Flink（九）【时间语义与水位线】

前言 2023-12-02-20:05&#xff0c;终于写完啦&#xff0c;最近状态不错。刚写完又收到了她的消息哈哈哈哈&#xff0c;开心。 再去全力打拼一次&#xff0c;奋战一场&#xff0c;就算最后打了败仗也无所谓&#xff0c;至少你留下了足迹。 《解忧杂货店》 1、时间语义 …
阅读更多...
出口贸易媒体发稿7种方法提升转化率的秘密武器解析-华媒舍

出口贸易媒体发稿7种方法提升转化率的秘密武器解析-华媒舍

出口贸易成为了许多企业发展的重要方向。在这个竞争激烈的市场中&#xff0c;如何让自己的产品脱颖而出&#xff0c;吸引更多客户并提高转化率&#xff0c;成为了每个企业家都面临的挑战。本文将向大家介绍7种提升转化率的秘密武器&#xff1a;出口贸易媒体发稿方法。 1. 出口贸…
阅读更多...
LeetCode刷题---合并两个有序链表

LeetCode刷题---合并两个有序链表

个人主页&#xff1a;元清加油_【C】,【C语言】,【数据结构与算法】-CSDN博客 个人专栏&#xff1a;http://t.csdnimg.cn/ZxuNL http://t.csdnimg.cn/c9twt 前言&#xff1a;这个专栏主要讲述递归递归、搜索与回溯算法&#xff0c;所以下面题目主要也是这些算法做的 我讲述…
阅读更多...
基于深度学习的肺炎CT图像检测诊断系统

基于深度学习的肺炎CT图像检测诊断系统

欢迎大家点赞、收藏、关注、评论啦 &#xff0c;由于篇幅有限&#xff0c;只展示了部分核心代码。 文章目录 一项目简介 二、功能三、系统四. 总结 一项目简介 深度学习在肺炎CT图像检测诊断方面具有广泛的应用前景。以下是关于肺炎CT图像检测诊断系统的介绍&#xff1a; 任务…
阅读更多...
基于PHP的高中生物学习平台

基于PHP的高中生物学习平台

有需要请加文章底部Q哦 可远程调试 基于PHP的高中生物学习平台 一 介绍 此高中生物学习平台基于原生PHP开发&#xff0c;数据库mysql。系统角色分为用户和管理员。(附带参考设计文档) 技术栈&#xff1a;phpmysqlphpstudyvscode 二 功能 学生 1 注册/登录/注销 2 个人中心 …
阅读更多...
人工智能时代：AIGC的横空出世

人工智能时代：AIGC的横空出世

&#x1f308;个人主页&#xff1a;聆风吟 &#x1f525;系列专栏&#xff1a;数据结构、网络奇遇记 &#x1f516;少年有梦不应止于心动&#xff0c;更要付诸行动。 文章目录 &#x1f4cb;前言一. 什么是AIGC?二. AIGC的主要特征2.1 文本生成2.2 图像生成2.3 语音生成2.4 视…
阅读更多...
蓝桥杯第198题 人物相关性分析 C++ 模拟 字符串 双指针

蓝桥杯第198题 人物相关性分析 C++ 模拟 字符串 双指针

题目 思路和解题方法 程序首先定义了一个函数check&#xff0c;用于判断一个字符是否为字母。接下来&#xff0c;程序读取输入的整数k和一行字符串str。定义了两个空的向量a和b&#xff0c;用于存储满足条件的子串的起始位置。使用for循环遍历字符串str的每个字符&#xff0c;检…
阅读更多...
string的模拟

string的模拟

> 作者简介&#xff1a;დ旧言~&#xff0c;目前大二&#xff0c;现在学习Java&#xff0c;c&#xff0c;c&#xff0c;Python等 > 座右铭&#xff1a;松树千年终是朽&#xff0c;槿花一日自为荣。 > 目标&#xff1a;能手撕模拟string类 > 毒鸡汤&#xff1a;时间…
阅读更多...
MySQL-视图

MySQL-视图

一、&#xff1f;看一个需求 emp表的列信息很多&#xff0c;有些信息是个人重要信息(比如 sal,comm,mgr,hiredate),如果我们希望某个用户只能查询emp表的(empno、ename,job和deptno)信息,有什么办法? 》视图 二、基本概念 视图 视图是一个虚拟表&#xff0c;其内容由查…
阅读更多...
Linux基础项目开发1:量产工具——UI系统(五)

Linux基础项目开发1:量产工具——UI系统(五)

前言&#xff1a; 前面我们已经把显示系统、输入系统、文字系统搭建好了&#xff0c;现在我们就要给它实现按钮操作了&#xff0c;也就是搭建UI系统&#xff0c;下面让我们一起实现UI系统的搭建吧 目录 一、按钮数据结构抽象 ui.h 二、按键编程 1.button.c 2.disp_manager…
阅读更多...
查找算法及哈希表

查找算法及哈希表

1 二分查找 1.1 重要概念 拟解决的问题&#xff1a;判断某个区间是否包含某个元素&#xff0c;无法确定区间中包含重复元素的具体位置&#xff1b;使用条件&#xff1a;查找的区间必须符合单调性&#xff1b;本质&#xff1a;采用分治思想&#xff0c;将某个单调区间一分为二…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023