vins-fusion代码解读[二] 惯性视觉里程结果与GPS松耦合

2023-12-18 04:58

本文主要是介绍vins-fusion代码解读[二] 惯性视觉里程结果与GPS松耦合,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

转载自:https://blog.csdn.net/huanghaihui_123/article/details/87183055?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-12.control&dist_request_id=1328769.29632.16174541431940121&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-12.control

vins-fusion代码解读[] 惯性视觉里程结果与GPS松耦合

乌龟抓水母 2019-02-14 14:34:43 4579 收藏 23

分类专栏: SLAM

版权

感谢“slam萌新”,本篇博客部分参考:
https://blog.csdn.net/weixin_41843971/article/details/86748719
欢迎讨论。

惯性视觉里程结果与GPS松耦合:

github开源的是KITTI的数据集的测试代码。跟着程序走一遍。

KITTIGPSTest.cpp

主程序入口:
vins_estimator包下面的KITTIGPSTest.cpp,主要作用:
(1)开启estimator类,进行vio里程估计
(2)从文件中读取视频图片列表,读入estimator类中
(3)从文件中读取GPS数据列表,直接通过ROS发布出去

具体的,从文件中获取图像和GPS数据当前的时间戳信息。以第一帧图像和第一个GPS时间早的作为基准时间,之后,左右双目的图像通过estimator.inputImage()读入里程计中,里程计Estimator类内部会定时发送VIO计算的结果。同时main函数中会在图片读入里程计的时刻发布同一时刻的GPS信息。(每一帧图片都有对应一条GPS信息,时间戳设置为一致的)

GPS融合主要发生在global_fusion包中。

global_fusion

程序同步开启了global_fusion节点。

rosrun global_fusion global_fusion_node

程序入口globalOptNode.cpp
程序定义了一个GlobalOptimization类globalEstimator来进行融合处理。
程序有三个回调函数:
(1)publish_car_model():根据最终vio与GPS融合的定位结果,发布在特定位置的一个炫酷的小车模型。
(2)GPS_callback():通过globalEstimator.inputGPS(),放入全局融合器中。
(3)vio_callback():通过globalEstimator.inputOdom(),放入全局融合器中。并且从全局融合器globalEstimator中取出最终位姿融合的结果,调用publish_car_model()发布出来。

最重要类的核心为 GlobalOptimization类 和类内的optimize()函数。

GlobalOptimization类

类成员:
(1)map类型
localPoseMap中保存着vio的位姿
GPSPositionMap中保存着gps数据
globalPoseMap中保存着优化后的全局位姿

以上类成员中map的格式:

map<double,vector<double>> =<t,vector<x,y,z,qw,qx,qy,qz>>

(2)bool类型
initGPS:第一个GPS信号触发
newGPS:有新的GPS信号后触发

(3)GeographicLib::LocalCartesian 类型
geoconverter GPS经纬度信号转化为X,Y,Z用到的第三方库
当该类已进行初始化,就同步开启了新线程optimize(),对两个结果不断进行优化。

optimize()

(1)当有新的GPS信号到来时候,进行GPS与视觉惯性的融合
(2)建立ceres的problem

  • lossfunction 设置为Huberloss
  • addParameterBlock添加优化的变量 ,优化的变量是q_array以及t_array。即globalPoseMap保存下来的每帧图像的位姿信息。其中参数变量的多少是由localPoseMap来决定的。即VIO有多少个数据,全局也就有多少个。迭代器指向的first为时间,second为7变量的位姿。其中在添加q_array由于维度只有三维,因此增加了local_parameterization来进行约束。
  • 接着开始添加残差项,总共有两个残差项分别是:vio factor以及gps factor
    – vio factor:残差项的costfunction创建由 relativeRTError来提供。观测值由vio的结果提供。此时计算的是以i时刻作为参考,从i到j这两个时刻的位移值以及四元数的旋转值作为观测值传递进入代价函数中。 此时iPj代表了i到j的位移,iQj代表了i到j的四元数变换。添加残差项的时候,需要添加当前i时刻的位姿以及j时刻的位姿。即用观测值来估计i时刻的位姿以及j时刻的位姿。
    – gps factor:残差项的costfunction创建由 TError来提供。观测值由Gps数据的结果提供。添加残差项的时候,只需要添加当前i时刻的位姿。
  • 求解非线性优化方程
  • 求解出来后,把t_array和q_array(即两个优化的变量)赋值给globalposeMap。并且根据最新解算出来的结果(即i=length-1时刻最新的结果),跟新GPS到vio这两个独立体系之间坐标转换关系。

TError及RelativeRTError

直观上理解:
{0, 1,2,3,4, 5,6…}
要估计出这些时刻,每个时刻的位姿。
我有的是两个方面的观测值,一方面是GPS得到的每个时刻的位置(x,y,z)(并且GPS信号可以提供在该时刻得到这个位置的精度posAccuracy),没有累计误差,精度较低。另一方面是VIO得到的每个时刻的位置(x,y,z)以及对应的姿态四元数(w,x,y,z),存在累计误差,短时间内精度较高。为了得到更好的一个融合结果,因此我们采用这个策略:观测值中,初始位置由GPS提供,并且vio观测值信任的是i到j时刻的位移以及姿态变化量。 并不信任vio得到的一个绝对位移量以及绝对的旋转姿态量。只信任短期的i到j的变化量,用这个变化量作为整个代价函数的观测值,进行求解。
因此两个残差项TError及RelativeRTError分别对应的就是GPS位置信号以及vio短时间内估计的i到j时刻的位姿变化量对最终结果的影响。迭代求解的过程中均采用了AutoDiffCostFunction自动求解微分来进行迭代。
(1)TError
TError(x,y,z,accuracy),最后一项是定位精度,可以由GPS系统提供。残差除了直接观测值与真值相减以外,还除了这个accuracy作为分母。意味着精度越高,accuracy越小,对结果的影响就越大。
(2)RelativeRTError
RelativeRTError(dx,dy,dz,dqw,dqx,dqy,dqz,t_var,q_var),最后两项是位移以及旋转之间的权重分配比例,并且为了使得与TError对应。在程序中,应该是根据经验把最后两项设置成一个常值,分别对应0.1以及0.01。残差的得到就根据实际值与观测值之间的偏差直接得出。

这篇关于vins-fusion代码解读[二] 惯性视觉里程结果与GPS松耦合的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/507119

相关文章

解读GC日志中的各项指标用法

《解读GC日志中的各项指标用法》:本文主要介绍GC日志中的各项指标用法,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、基础 GC 日志格式(以 G1 为例)1. Minor GC 日志2. Full GC 日志二、关键指标解析1. GC 类型与触发原因2. 堆

Java设计模式---迭代器模式(Iterator)解读

《Java设计模式---迭代器模式(Iterator)解读》:本文主要介绍Java设计模式---迭代器模式(Iterator),具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,... 目录1、迭代器(Iterator)1.1、结构1.2、常用方法1.3、本质1、解耦集合与遍历逻辑2、统一

Java中调用数据库存储过程的示例代码

《Java中调用数据库存储过程的示例代码》本文介绍Java通过JDBC调用数据库存储过程的方法,涵盖参数类型、执行步骤及数据库差异,需注意异常处理与资源管理,以优化性能并实现复杂业务逻辑,感兴趣的朋友... 目录一、存储过程概述二、Java调用存储过程的基本javascript步骤三、Java调用存储过程示

Visual Studio 2022 编译C++20代码的图文步骤

《VisualStudio2022编译C++20代码的图文步骤》在VisualStudio中启用C++20import功能,需设置语言标准为ISOC++20,开启扫描源查找模块依赖及实验性标... 默认创建Visual Studio桌面控制台项目代码包含C++20的import方法。右键项目的属性:

MySQL之InnoDB存储页的独立表空间解读

《MySQL之InnoDB存储页的独立表空间解读》:本文主要介绍MySQL之InnoDB存储页的独立表空间,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录1、背景2、独立表空间【1】表空间大小【2】区【3】组【4】段【5】区的类型【6】XDES Entry区结构【

MySQL数据库的内嵌函数和联合查询实例代码

《MySQL数据库的内嵌函数和联合查询实例代码》联合查询是一种将多个查询结果组合在一起的方法,通常使用UNION、UNIONALL、INTERSECT和EXCEPT关键字,下面:本文主要介绍MyS... 目录一.数据库的内嵌函数1.1聚合函数COUNT([DISTINCT] expr)SUM([DISTIN

Java实现自定义table宽高的示例代码

《Java实现自定义table宽高的示例代码》在桌面应用、管理系统乃至报表工具中,表格(JTable)作为最常用的数据展示组件,不仅承载对数据的增删改查,还需要配合布局与视觉需求,而JavaSwing... 目录一、项目背景详细介绍二、项目需求详细介绍三、相关技术详细介绍四、实现思路详细介绍五、完整实现代码

Go语言代码格式化的技巧分享

《Go语言代码格式化的技巧分享》在Go语言的开发过程中,代码格式化是一个看似细微却至关重要的环节,良好的代码格式化不仅能提升代码的可读性,还能促进团队协作,减少因代码风格差异引发的问题,Go在代码格式... 目录一、Go 语言代码格式化的重要性二、Go 语言代码格式化工具:gofmt 与 go fmt(一)

MySQL主从复制与读写分离的用法解读

《MySQL主从复制与读写分离的用法解读》:本文主要介绍MySQL主从复制与读写分离的用法,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一、主从复制mysql主从复制原理实验案例二、读写分离实验案例安装并配置mycat 软件设置mycat读写分离验证mycat读

Python的端到端测试框架SeleniumBase使用解读

《Python的端到端测试框架SeleniumBase使用解读》:本文主要介绍Python的端到端测试框架SeleniumBase使用,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全... 目录SeleniumBase详细介绍及用法指南什么是 SeleniumBase?SeleniumBase