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

相关文章

postgresql使用UUID函数的方法

《postgresql使用UUID函数的方法》本文给大家介绍postgresql使用UUID函数的方法,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录PostgreSQL有两种生成uuid的方法。可以先通过sql查看是否已安装扩展函数,和可以安装的扩展函数

Java中Arrays类和Collections类常用方法示例详解

《Java中Arrays类和Collections类常用方法示例详解》本文总结了Java中Arrays和Collections类的常用方法,涵盖数组填充、排序、搜索、复制、列表转换等操作,帮助开发者高... 目录Arrays.fill()相关用法Arrays.toString()Arrays.sort()A

Nginx安全防护的多种方法

《Nginx安全防护的多种方法》在生产环境中,需要隐藏Nginx的版本号,以避免泄漏Nginx的版本,使攻击者不能针对特定版本进行攻击,下面就来介绍一下Nginx安全防护的方法,感兴趣的可以了解一下... 目录核心安全配置1.编译安装 Nginx2.隐藏版本号3.限制危险请求方法4.请求限制(CC攻击防御)

python生成随机唯一id的几种实现方法

《python生成随机唯一id的几种实现方法》在Python中生成随机唯一ID有多种方法,根据不同的需求场景可以选择最适合的方案,文中通过示例代码介绍的非常详细,需要的朋友们下面随着小编来一起学习学习... 目录方法 1:使用 UUID 模块(推荐)方法 2:使用 Secrets 模块(安全敏感场景)方法

MyBatis-Plus通用中等、大量数据分批查询和处理方法

《MyBatis-Plus通用中等、大量数据分批查询和处理方法》文章介绍MyBatis-Plus分页查询处理,通过函数式接口与Lambda表达式实现通用逻辑,方法抽象但功能强大,建议扩展分批处理及流式... 目录函数式接口获取分页数据接口数据处理接口通用逻辑工具类使用方法简单查询自定义查询方法总结函数式接口

MySQL深分页进行性能优化的常见方法

《MySQL深分页进行性能优化的常见方法》在Web应用中,分页查询是数据库操作中的常见需求,然而,在面对大型数据集时,深分页(deeppagination)却成为了性能优化的一个挑战,在本文中,我们将... 目录引言:深分页,真的只是“翻页慢”那么简单吗?一、背景介绍二、深分页的性能问题三、业务场景分析四、

JAVA中安装多个JDK的方法

《JAVA中安装多个JDK的方法》文章介绍了在Windows系统上安装多个JDK版本的方法,包括下载、安装路径修改、环境变量配置(JAVA_HOME和Path),并说明如何通过调整JAVA_HOME在... 首先去oracle官网下载好两个版本不同的jdk(需要登录Oracle账号,没有可以免费注册)下载完

Linux进程CPU绑定优化与实践过程

《Linux进程CPU绑定优化与实践过程》Linux支持进程绑定至特定CPU核心,通过sched_setaffinity系统调用和taskset工具实现,优化缓存效率与上下文切换,提升多核计算性能,适... 目录1. 多核处理器及并行计算概念1.1 多核处理器架构概述1.2 并行计算的含义及重要性1.3 并

Java中读取YAML文件配置信息常见问题及解决方法

《Java中读取YAML文件配置信息常见问题及解决方法》:本文主要介绍Java中读取YAML文件配置信息常见问题及解决方法,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要... 目录1 使用Spring Boot的@ConfigurationProperties2. 使用@Valu

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

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