ORB_SLAM3_优化方法 InertialBA

2024-02-20 05:28

本文主要是介绍ORB_SLAM3_优化方法 InertialBA,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

FullInertialBA

FullInertialBA的主要作用是结合视觉和IMU信息进行联合优化,主要作用在LocalMapping中的initializeIMULoopClosing中的RunGlobalBundleAdjustment

  • 在初始化阶段,bias固定不变,通过先验,约束bias为一个微小量

在这里插入图片描述
在这里插入图片描述

输入

参数说明
pMap地图

初始化

    g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType *linearSolver;linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);solver->setUserLambdaInit(1e-5);optimizer.setAlgorithm(solver);optimizer.setVerbose(false);if (pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);

设置vertex

对于地图中的关键帧
  • VertexPose
VertexPose *VP = new VertexPose(pKFi);
VP->setId(pKFi->mnId);
pIncKF = pKFi;
bool bFixed = false;
if (bFixLocal)
{bFixed = (pKFi->mnBALocalForKF >= (maxKFid - 1)) || (pKFi->mnBAFixedForKF >= (maxKFid - 1));if (!bFixed)nNonFixed++;VP->setFixed(bFixed);
}
optimizer.addVertex(VP);
  • VertexVelocity
    需要该地图完成IMU初始化后才添加,也就是关键帧的bImutrue
VertexVelocity *VV = new VertexVelocity(pKFi);
VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
VV->setFixed(bFixed);
optimizer.addVertex(VV);
  • VertexGyroBias
    • 当有先验信息时,只添加最后一帧关键帧的bias
    • 当没有先验信息时,每一帧关键帧都添加bias
VertexGyroBias *VG = new VertexGyroBias(pKFi);
VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
VG->setFixed(bFixed);
optimizer.addVertex(VG);
  • VertexAccBias
    • 当有先验信息时,只添加最后一帧关键帧的bias
    • 当没有先验信息时,每一帧关键帧都添加bias
VertexAccBias *VA = new VertexAccBias(pKFi);
VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
VA->setFixed(bFixed);
optimizer.addVertex(VA);
对于地图中的地图点
  • VertexSBAPointXYZ
MapPoint *pMP = vpMPs[i];
g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(pMP->GetWorldPos().cast<double>());
unsigned long id = pMP->mnId + iniMPid + 1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);

设置edge

添加IMU边
  • EdgeInertial
    • 当有先验信息时,边的两个Vertex:VG1VA1为最后一帧关键帧的bias,同时添加先验边EdgePriorAcc和EdgePriorGyro
    • 当没有先验信息时,边的两个Vertex:VG1VA1为前一关键帧的bias,同时在前后两帧之间添加边EdgeGyroRW]和EdgeAccRW
// 3.1 根据上一帧的偏置设定当前帧的新偏置
pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
// 3.2 提取节点
g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1);g2o::HyperGraph::Vertex *VG1;
g2o::HyperGraph::Vertex *VA1;
g2o::HyperGraph::Vertex *VG2;
g2o::HyperGraph::Vertex *VA2;
// 根据不同输入配置相应的偏置节点
if (!bInit)
{VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3);VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2);VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3);
}
else
{VG1 = optimizer.vertex(4 * maxKFid + 2);VA1 = optimizer.vertex(4 * maxKFid + 3);
}g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1);// 3.3 设置边
EdgeInertial *ei = new EdgeInertial(pKFi->mpImuPreintegrated);
ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG1));
ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
ei->setRobustKernel(rki);
// 9个自由度的卡方检验(0.05)
rki->setDelta(sqrt(16.92));optimizer.addEdge(ei);
  • EdgeGyroRW
EdgeGyroRW *egr = new E

这篇关于ORB_SLAM3_优化方法 InertialBA的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

Java 方法重载Overload常见误区及注意事项

《Java方法重载Overload常见误区及注意事项》Java方法重载允许同一类中同名方法通过参数类型、数量、顺序差异实现功能扩展,提升代码灵活性,核心条件为参数列表不同,不涉及返回类型、访问修饰符... 目录Java 方法重载(Overload)详解一、方法重载的核心条件二、构成方法重载的具体情况三、不构

SQL中如何添加数据(常见方法及示例)

《SQL中如何添加数据(常见方法及示例)》SQL全称为StructuredQueryLanguage,是一种用于管理关系数据库的标准编程语言,下面给大家介绍SQL中如何添加数据,感兴趣的朋友一起看看吧... 目录在mysql中,有多种方法可以添加数据。以下是一些常见的方法及其示例。1. 使用INSERT I

Python中反转字符串的常见方法小结

《Python中反转字符串的常见方法小结》在Python中,字符串对象没有内置的反转方法,然而,在实际开发中,我们经常会遇到需要反转字符串的场景,比如处理回文字符串、文本加密等,因此,掌握如何在Pyt... 目录python中反转字符串的方法技术背景实现步骤1. 使用切片2. 使用 reversed() 函

Python中将嵌套列表扁平化的多种实现方法

《Python中将嵌套列表扁平化的多种实现方法》在Python编程中,我们常常会遇到需要将嵌套列表(即列表中包含列表)转换为一个一维的扁平列表的需求,本文将给大家介绍了多种实现这一目标的方法,需要的朋... 目录python中将嵌套列表扁平化的方法技术背景实现步骤1. 使用嵌套列表推导式2. 使用itert

Python使用pip工具实现包自动更新的多种方法

《Python使用pip工具实现包自动更新的多种方法》本文深入探讨了使用Python的pip工具实现包自动更新的各种方法和技术,我们将从基础概念开始,逐步介绍手动更新方法、自动化脚本编写、结合CI/C... 目录1. 背景介绍1.1 目的和范围1.2 预期读者1.3 文档结构概述1.4 术语表1.4.1 核

在Linux中改变echo输出颜色的实现方法

《在Linux中改变echo输出颜色的实现方法》在Linux系统的命令行环境下,为了使输出信息更加清晰、突出,便于用户快速识别和区分不同类型的信息,常常需要改变echo命令的输出颜色,所以本文给大家介... 目python录在linux中改变echo输出颜色的方法技术背景实现步骤使用ANSI转义码使用tpu

Conda与Python venv虚拟环境的区别与使用方法详解

《Conda与Pythonvenv虚拟环境的区别与使用方法详解》随着Python社区的成长,虚拟环境的概念和技术也在不断发展,:本文主要介绍Conda与Pythonvenv虚拟环境的区别与使用... 目录前言一、Conda 与 python venv 的核心区别1. Conda 的特点2. Python v

Spring Boot中WebSocket常用使用方法详解

《SpringBoot中WebSocket常用使用方法详解》本文从WebSocket的基础概念出发,详细介绍了SpringBoot集成WebSocket的步骤,并重点讲解了常用的使用方法,包括简单消... 目录一、WebSocket基础概念1.1 什么是WebSocket1.2 WebSocket与HTTP

SQL Server配置管理器无法打开的四种解决方法

《SQLServer配置管理器无法打开的四种解决方法》本文总结了SQLServer配置管理器无法打开的四种解决方法,文中通过图文示例介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的... 目录方法一:桌面图标进入方法二:运行窗口进入检查版本号对照表php方法三:查找文件路径方法四:检查 S

MyBatis-Plus 中 nested() 与 and() 方法详解(最佳实践场景)

《MyBatis-Plus中nested()与and()方法详解(最佳实践场景)》在MyBatis-Plus的条件构造器中,nested()和and()都是用于构建复杂查询条件的关键方法,但... 目录MyBATis-Plus 中nested()与and()方法详解一、核心区别对比二、方法详解1.and()