使用gtsam添加OrientedPlane3Factor平面约束因子
在基于地面约束的SLAM优化中,已知的地面信息(如 plan.pcd 文件中的地面模型)可以用作一个先验约束,以帮助优化位姿估计。具体而言,这个过程涉及将地面模型和每个帧的位姿结合,以创建一个因子模型,然后利用该因子模型在图优化过程中约束位姿。
以下是一个简化的工作流程,描述了如何使用已知地面模型对位姿进行优化:
-
地面模型的提取:
- 从
plan.pcd文件中提取地面的平面参数。通常,这包括平面方程的法向量和一个偏移量。这可以通过PCL(Point Cloud Library)的平面分割工具如RANSAC实现。
- 从
-
GTSAM因子图初始化:
- 使用GTSAM的因子图模型,比如
NonlinearFactorGraph,以构建整个SLAM问题。
- 使用GTSAM的因子图模型,比如
-
定义地面约束:
- 创建
OrientedPlane3Factor,这个因子将平面模型(提取自plan.pcd)与每个帧的位姿关联。它定义了地面应该如何与这些位姿对齐。
- 创建
-
添加因子到因子图中:
- 将每帧的位姿和对应的
OrientedPlane3Factor添加到因子图中。这个因子会考虑当前位姿与地面模型之间的偏差,并用来优化位姿。
- 将每帧的位姿和对应的
-
位姿优化:
- 利用GTSAM中的优化工具(如Levenberg-Marquardt优化器),根据定义的因子图对所有位姿进行优化。过程中的关键是通过地面因子提供的约束减少累积的位姿误差。
-
结果应用:
- 优化后,新生成的位姿序列会使得每个帧的位姿对齐到地面模型给予的参考系中。通常,这意味着消除由传感器噪声、漂移或累积误差引入的偏差。
通过以上过程,地面模型被用于约束和改善传感器估计的轨迹,提供一个更稳定和准确的位姿方案。通过这种综合约束模式,尤其是在运行中的环境几何形状已知的情况下,能显著提高定位的精度和鲁棒性。
OrientedPlane3Factor 是一种用于约束平面和位姿之间关系的因子,在图优化中用于减少位姿估计的漂移和误差。要理解这个因子如何计算位姿与地面模型之间的偏差,我们需要深入了解其工作机制和数学基础。
示例:
在slam中对一系列点云帧添加地平面约束,我们需要对每一帧都进行地面平面的提取,然后利用GTSAM来添加 OrientedPlane3Factor。以下是如何实现这一过程的代码示例,它将对1000帧点云进行处理,并对相应的位姿进行优化
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <iostream>
#include <string>// Function to extract plane coefficients from a PCD file
Eigen::Vector4f extractPlaneCoefficients(const std::string& file_path) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) {PCL_ERROR("Couldn't read file %s\n", file_path.c_str());exit(-1);}pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);pcl::SACSegmentation<pcl::PointXYZ> seg;seg.setOptimizeCoefficients(true);seg.setModelType(pcl::SACMODEL_PLANE);seg.setMethodType(pcl::SAC_RANSAC);seg.setDistanceThreshold(0.01);pcl::PointIndices::Ptr inliers(new pcl::PointIndices);seg.setInputCloud(cloud);seg.segment(*inliers, *coefficients);if (coefficients->values.size() != 4) {PCL_ERROR("Could not estimate a planar model for the given dataset.\n");exit(-1);}return Eigen::Vector4f(coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3]);
}int main() {// Assume poses is an array that holds the pose of each frame obtained from SLAMstd::vector<gtsam::Pose3> poses(1000);// Initialize the poses here or from your SLAM results. This is just a placeholder.for (int i = 0; i < 1000; ++i) {poses[i] = gtsam::Pose3(); // Replace with initial poses}// Create a factor graphgtsam::NonlinearFactorGraph graph;// Initialize valuesgtsam::Values initial;// Create a noise model for the plane factorauto noiseModel = gtsam::noiseModel::Isotropic::Sigma(3, 0.1);for (int i = 0; i < 1000; ++i) {// Extract plane coefficients for the ith point cloudstd::string file_path = "path/to/cloud/plane_" + std::to_string(i) + ".pcd";Eigen::Vector4f planeCoefficients = extractPlaneCoefficients(file_path);// Convert to GTSAM's OrientedPlane3gtsam::OrientedPlane3 groundPlane(planeCoefficients.head<3>(), planeCoefficients[3]);// Add the OrientedPlane3Factor for each posegraph.emplace_shared<gtsam::PriorFactor<gtsam::OrientedPlane3>>(i, groundPlane, noiseModel);// Insert the corresponding initial pose into the valuesinitial.insert(i, poses[i]);}// Optimize the graphgtsam::LevenbergMarquardtOptimizer optimizer(graph, initial);gtsam::Values result = optimizer.optimize();// Retrieve and print the optimized posesfor (int i = 0; i < 1000; ++i) {gtsam::Pose3 optimizedPose = result.at<gtsam::Pose3>(i);std::cout << "Optimized Pose " << i << ": " << optimizedPose << std::endl;}return 0;
}
OrientedPlane3和OrientedPlane3Factor
-
OrientedPlane3:
- 描述: 这是GTSAM中用于表示三维空间中平面的一种数据结构。一个平面通常通过其法向量 ( \mathbf{n} = (a, b, c) ) 以及距离原点的距离 ( d ) 来定义。
- 平面方程: ( ax + by + cz + d = 0 )
-
OrientedPlane3Factor:
- 作用: 这个因子将一个
OrientedPlane3与一个Pose3(三维位姿) 联系起来,用于检查位姿在优化过程中是否遵循平面约束。 - 目的: 确保优化后的位姿(例如相机或激光雷达的位姿)在空间中保持与已知平面之间的几何关系。
- 作用: 这个因子将一个
计算位姿与地面偏差
OrientedPlane3Factor 的目的在于计算当前位姿(如相机或激光雷达的坐标系)与定义的地面模型之间的几何误差,并将此误差用于优化流程中。
-
误差计算:
- 当一个
Pose3被应用到一个OrientedPlane3时,计算的目标是看看变换后的平面与其在地图坐标系中记录的模型间的差异。 - 这个误差可以看作是在当前位姿下,地图中的平面和传感器估计平面间的距离或角度偏差。
- 当一个
-
误差方程:

-
优化目标:
- 在图优化期间,
OrientedPlane3Factor被用来引导优化算法最小化以上误差。利用Levenberg-Marquardt等非线性优化方法,逐步调整各关键帧位姿,使得增量误差不断降低。
- 在图优化期间,
通过这种方式,OrientedPlane3Factor 提供了一种将全局或相对不变的地面模型信息引入到基于视觉或激光的SLAM系统中,以强化其对全局坐标的约束,而不仅仅依赖于相对运动估计。此优化处理将有助于减少累积误差, 提供更可靠的位姿估计。
相关文章:
使用gtsam添加OrientedPlane3Factor平面约束因子
在基于地面约束的SLAM优化中,已知的地面信息(如 plan.pcd 文件中的地面模型)可以用作一个先验约束,以帮助优化位姿估计。具体而言,这个过程涉及将地面模型和每个帧的位姿结合,以创建一个因子模型࿰…...
换了城市ip属地会变吗?为什么换了城市IP属地不变
当我们跨越城市的界限,从一个地方迁移到另一个地方时,许多日常使用的网络服务和应用程序都会感知到这种变化,其中一个显著的现象就是IP属地的变化。IP属地,即IP地址所在的地理位置信息,它通常与互联网服务提供商&#…...
移远通信多模卫星通信模组BG95-S5获得Skylo网络认证,进一步拓展全球卫星物联网市场
近日,全球领先的物联网整体解决方案供应商移远通信正式宣布,其支持“卫星蜂窝”多模式的高集成度NTN卫星通信模组BG95-S5已成功获得NTN网络运营商Skylo的网络认证。BG95-S5也成为了获得该认证的最新款移远卫星通信模组。 BG95-S5模组顺利获得Skylo认证&a…...
IntelliJ IDEA Type Hierarchy Scope Pattern 学习指南
IntelliJ IDEA Type Hierarchy Scope Pattern 学习指南 什么是 Type Hierarchy? Type Hierarchy 是 IntelliJ IDEA 提供的一个工具,允许开发者查看某个类的继承关系及其实现的接口结构。它是理解类关系的重要工具,尤其在处理复杂的继承体系…...
简聊MySQL并发事务中幻读、虚读问题的解决方案
在MySQL数据库中,事务的幻读和虚读问题是并发控制中的关键挑战。以下是针对这两个问题的解决方案及原理说明,并附上相关示例。 一、幻读问题及其解决方案 幻读问题的定义 幻读是指一个事务在前后两次查询同一个范围的时候,后一次查询看到了…...
【搭建JavaEE】(2)Tomcat安装配置和第一个JavaEE程序
Tomcat–容器(Container) 下载 Apache Tomcat - Welcome! 下载完成 请求/响应 结构 测试 查看Jdk版本 改端口号localhost8080–>8099 学学人家以后牛逼了可以用自己名字当文件夹名 配置端口8099 找到server文件 用记事本打开 再打开另一个logging文件 ”乱码解决“步骤&…...
【Qt】01-了解QT
踏入QT的殿堂之路 前言一、创建工程文件1.1 步骤介绍1.2 编译介绍方法1、方法2、编译成功 二、了解框架2.1 main.cpp2.2 .Pro文件2.2.1 注释需要打井号。2.2.2 F1带你进入帮助模式2.2.3 build文件 2.3 构造函数 三、编写工程3.1 main代码3.2 结果展示 四、指定父对象4.1 main代…...
websocket股票行情接口
股票行情区别 交易所出来的数据,不管通过什么渠道,延时一般都不会差太远,估计一般也就几十ms的差别。 但是如果是通过http轮询,不太可能几十ms全部轮询一次。所以,做量化的话,用http协议是最次的选择。 …...
朴素贝叶斯分类器
目录 一、生成模型(学习)(Generative Model) vs 判别模型(学习)(Discriminative Model) 1、官方说明 2、通俗理解 3、举例 二、生成学习算法 1、数学符号说明 2、贝叶斯公式 …...
智能化植物病害检测:使用深度学习与图像识别技术的应用
植物病害一直是农业生产中亟待解决的问题,它不仅会影响作物的产量和质量,还可能威胁到生态环境的稳定。随着人工智能(AI)技术的快速发展,尤其是深度学习和图像识别技术的应用,智能化植物病害检测已经成为一…...
vim基本命令(vi、工作模式、普通模式、插入模式、可视模式、命令行模式、复制、粘贴、插入、删除、查找、替换)
1. Vim的作用 1.1. 文本编辑 1.1.1. 基础文本编辑功能 Vim是一个功能强大的文本编辑器,它可以用来创建、修改和保存各种文本文件。无论是编写简单的文本笔记,还是复杂的代码文件,Vim都能胜任。例如,我们可以用它来编写Python脚…...
Qt 自动根据编译的dll或exe 将相关dll文件复制到目标文件夹
Qt 自动根据编译的dll或exe 将相关dll文件复制到目标文件夹 如果你在使用 windeployqt 时遇到错误 “windeployqt 不是内部或外部命令”,说明你的命令行环境没有正确配置 Qt 工具路径。windeployqt 是 Qt 工具的一部分,它用于自动将所有必要的 Qt 库和插…...
探索新能源汽车“芯”动力:AUTO TECH China 2025广州国际新能源汽车功率半导体技术展盛况空前
广州,2025年11月20日 —— 在全球新能源车市场蓬勃发展的背景下,AUTO TECH China 2025 广州国际新能源汽车功率半导体技术展览会将于2025年11月20-22日在广州保利世贸博览馆盛大开幕。此次展会作为亚洲领先的车用功率半导体技术专业盛会,本…...
Kafka权威指南(第2版)读书笔记
目录 Kafka生产者——向Kafka写入数据生产者概览创建Kafka生产者bootstrap.serverskey.serializervalue.serializer 发送消息到Kafka同步发送消息 Kafka生产者——向Kafka写入数据 不管是把Kafka作为消息队列、消息总线还是数据存储平台,总是需要一个可以往Kafka写…...
WORD转PDF脚本文件
1、在桌面新建一个文本文件,把下列代码复制到文本文件中。 On Error Resume Next Const wdExportFormatPDF 17 Set oWord WScript.CreateObject("Word.Application") Set fso WScript.CreateObject("Scripting.Filesystemobject") Set fdsf…...
electron 打包后的 exe 文件,运行后是空白窗口
一、代码相关问题 1. 页面加载失败 1.1 原因 在 Electron 应用中,若loadFile或loadURL方法指定的页面路径或 URL 错误,就无法正确加载页面,导致窗口空白。 1.2. 解决 仔细检查loadFile或loadURL方法中传入的路径或 URL 是否正确…...
数据库重连 - 方案
要解决 SQL Server 连接失效后导致的错误问题,可以考虑以下几种解决方案: 1. 连接池机制: 通过实现一个连接池,确保连接失效后可以重新建立连接,而不会直接导致整个程序出错。连接池可以帮助在连接中断时自动恢复连接,而不必每次手动重连。 例如,可以通过以下方式定期…...
从 PostgreSQL 中挽救损坏的表
~/tmp-dir.dab4fd85-8b47-4d9a-b15c-18312ef61075 pg_dump -U postgres -h locathost www_p1 > wow_p1.sqlpg_dump:错误:转储表 “page_views” 的内容失败:PQgetResult() 失败。pg_dump:详细信息:来自服务器的错误…...
【Vue3 入门到实战】1. 创建Vue3工程
目录 编辑 1. 学习目标 2. 环境准备与初始化 3. 项目文件结构 4. 写一个简单的效果 5. 总结 1. 学习目标 (1) 掌握如何创建vue3项目。 (2) 了解项目中的文件的作用。 (3) 编辑App.vue文件,并写一个简单的效果。 2. 环境准备与初始化 (1) 安装 Node.js 和 …...
rtthread学习笔记系列(10/11) -- 系统定时器
文章目录 10. 系统定时器10.1 跳跃表[定时器跳表 (Skip List) 算法](https://www.rt-thread.org/document/site/#/rt-thread-version/rt-thread-standard/programming-manual/timer/timer?id定时器跳表-skip-list-算法) 10.2 硬件定时器10.2.1 初始化&&删除10.2.2 sta…...
vue3 字体颜色设置的多种方式
在Vue 3中设置字体颜色可以通过多种方式实现,这取决于你是想在组件内部直接设置,还是在CSS/SCSS/LESS等样式文件中定义。以下是几种常见的方法: 1. 内联样式 你可以直接在模板中使用style绑定来设置字体颜色。 <template><div :s…...
反射获取方法和属性
Java反射获取方法 在Java中,反射(Reflection)是一种强大的机制,允许程序在运行时访问和操作类的内部属性和方法。通过反射,可以动态地创建对象、调用方法、改变属性值,这在很多Java框架中如Spring和Hiberna…...
linux 下常用变更-8
1、删除普通用户 查询用户初始UID和GIDls -l /home/ ###家目录中查看UID cat /etc/group ###此文件查看GID删除用户1.编辑文件 /etc/passwd 找到对应的行,YW343:x:0:0::/home/YW343:/bin/bash 2.将标红的位置修改为用户对应初始UID和GID: YW3…...
MySQL中【正则表达式】用法
MySQL 中正则表达式通过 REGEXP 或 RLIKE 操作符实现(两者等价),用于在 WHERE 子句中进行复杂的字符串模式匹配。以下是核心用法和示例: 一、基础语法 SELECT column_name FROM table_name WHERE column_name REGEXP pattern; …...
高防服务器能够抵御哪些网络攻击呢?
高防服务器作为一种有着高度防御能力的服务器,可以帮助网站应对分布式拒绝服务攻击,有效识别和清理一些恶意的网络流量,为用户提供安全且稳定的网络环境,那么,高防服务器一般都可以抵御哪些网络攻击呢?下面…...
OPENCV形态学基础之二腐蚀
一.腐蚀的原理 (图1) 数学表达式:dst(x,y) erode(src(x,y)) min(x,y)src(xx,yy) 腐蚀也是图像形态学的基本功能之一,腐蚀跟膨胀属于反向操作,膨胀是把图像图像变大,而腐蚀就是把图像变小。腐蚀后的图像变小变暗淡。 腐蚀…...
sipsak:SIP瑞士军刀!全参数详细教程!Kali Linux教程!
简介 sipsak 是一个面向会话初始协议 (SIP) 应用程序开发人员和管理员的小型命令行工具。它可以用于对 SIP 应用程序和设备进行一些简单的测试。 sipsak 是一款 SIP 压力和诊断实用程序。它通过 sip-uri 向服务器发送 SIP 请求,并检查收到的响应。它以以下模式之一…...
安宝特案例丨Vuzix AR智能眼镜集成专业软件,助力卢森堡医院药房转型,赢得辉瑞创新奖
在Vuzix M400 AR智能眼镜的助力下,卢森堡罗伯特舒曼医院(the Robert Schuman Hospitals, HRS)凭借在无菌制剂生产流程中引入增强现实技术(AR)创新项目,荣获了2024年6月7日由卢森堡医院药剂师协会࿰…...
LangChain知识库管理后端接口:数据库操作详解—— 构建本地知识库系统的基础《二》
这段 Python 代码是一个完整的 知识库数据库操作模块,用于对本地知识库系统中的知识库进行增删改查(CRUD)操作。它基于 SQLAlchemy ORM 框架 和一个自定义的装饰器 with_session 实现数据库会话管理。 📘 一、整体功能概述 该模块…...
SQL慢可能是触发了ring buffer
简介 最近在进行 postgresql 性能排查的时候,发现 PG 在某一个时间并行执行的 SQL 变得特别慢。最后通过监控监观察到并行发起得时间 buffers_alloc 就急速上升,且低水位伴随在整个慢 SQL,一直是 buferIO 的等待事件,此时也没有其他会话的争抢。SQL 虽然不是高效 SQL ,但…...
