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

2023-12-02 20:44
文章标签 代码 更新 浅析 位姿 dlio

本文主要是介绍代码浅析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。接下来将机体坐标系下的加速度转换到世界坐标系下，并使用加速度传播公式更新机器人的位置和速度。同时，将机器人的角速度更新为世界坐标系下的角速度。然后使用重力计传播公式更新四元数，确保其归一化。最后，将机器人的角速度和四元数更新到状态中。

…详情请参照古月居

这篇关于代码浅析DLIO(四)---位姿更新的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/446750。

相关文章

浅析Spring Security认证过程

浅析Spring Security认证过程

类图 为了方便理解Spring Security认证流程，特意画了如下的类图，包含相关的核心认证类 概述 核心验证器 AuthenticationManager 该对象提供了认证方法的入口，接收一个Authentiaton对象作为参数; public interface AuthenticationManager {Authentication authenticate(Authenti
阅读更多...
poj3468(线段树成段更新模板题)

poj3468(线段树成段更新模板题)

题意：包括两个操作：1、将[a.b]上的数字加上v；2、查询区间[a,b]上的和 下面的介绍是下解题思路： 首先介绍  lazy－tag思想：用一个变量记录每一个线段树节点的变化值，当这部分线段的一致性被破坏我们就将这个变化值传递给子区间，大大增加了线段树的效率。 比如现在需要对[a,b]区间值进行加c操作，那么就从根节点[1,n]开始调用update函数进行操作，如果刚好执行到一个子节点，
阅读更多...
hdu1394(线段树点更新的应用)

hdu1394(线段树点更新的应用)

题意：求一个序列经过一定的操作得到的序列的最小逆序数 这题会用到逆序数的一个性质，在0到n-1这些数字组成的乱序排列，将第一个数字A移到最后一位，得到的逆序数为res-a+(n-a-1) 知道上面的知识点后，可以用暴力来解 代码如下： #include<iostream>#include<algorithm>#include<cstring>#include<stack>#in
阅读更多...
hdu1689(线段树成段更新)

hdu1689(线段树成段更新)

两种操作：1、set区间[a,b]上数字为v；2、查询[ 1 , n ]上的sum 代码如下： #include<iostream>#include<algorithm>#include<cstring>#include<stack>#include<queue>#include<set>#include<map>#include<stdio.h>#include<stdl
阅读更多...
活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...
poj 1258 Agri-Net（最小生成树模板代码）

poj 1258 Agri-Net（最小生成树模板代码）

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码：效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n
阅读更多...
hdu 1754 I Hate It（线段树，单点更新，区间最值）

hdu 1754 I Hate It（线段树，单点更新，区间最值）

题意是求一个线段中的最大数。 线段树的模板题，试用了一下交大的模板。效率有点略低。 代码： #include <stdio.h>#include <string.h>#define TREE_SIZE (1 << (20))//const int TREE_SIZE = 200000 + 10;int max(int a, int b){return a > b ? a :
阅读更多...
AI行业应用(不定期更新)

AI行业应用(不定期更新)

ChatPDF 可以让你上传一个 PDF 文件，然后针对这个 PDF 进行小结和提问。你可以把各种各样你要研究的分析报告交给它，快速获取到想要知道的信息。https://www.chatpdf.com/
阅读更多...
GIS图形库更新2024.8.4-9.9

GIS图形库更新2024.8.4-9.9

更多精彩内容请访问 dt.sim3d.cn ，关注公众号【sky的数孪技术】，技术交流、源码下载请添加微信：digital_twin123 Cesium 本期发布了1.121 版本。重大新闻，Cesium被Bentley收购。 ✨ 功能和改进 默认启用 MSAA，采样 4 次。若要关闭 MSAA，则可以设置scene.msaaSamples = 1。但是通过比较，发现并没有多大改善。
阅读更多...
计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

🍊作者：计算机编程-吉哥 🍊简介：专业从事JavaWeb程序开发，微信小程序开发，定制化项目、 源码、代码讲解、文档撰写、ppt制作。做自己喜欢的事，生活就是快乐的。 🍊心愿：点赞 👍 收藏 ⭐评论 📝 🍅 文末获取源码联系 👇🏻 精彩专栏推荐订阅 👇🏻 不然下次找不到哟~Java毕业设计项目~热门选题推荐《1000套》 目录 1.技术选型 2.开发工具 3.功能
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2