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

相关文章

MySQL启动报错:InnoDB表空间丢失问题及解决方法

《MySQL启动报错:InnoDB表空间丢失问题及解决方法》在启动MySQL时,遇到了InnoDB:Tablespace5975wasnotfound,该错误表明MySQL在启动过程中无法找到指定的s... 目录mysql 启动报错:InnoDB 表空间丢失问题及解决方法错误分析解决方案1. 启用 inno

Python函数返回多个值的多种方法小结

《Python函数返回多个值的多种方法小结》在Python中,函数通常用于封装一段代码,使其可以重复调用,有时,我们希望一个函数能够返回多个值,Python提供了几种不同的方法来实现这一点,需要的朋友... 目录一、使用元组(Tuple):二、使用列表(list)三、使用字典(Dictionary)四、 使

Linux查看系统盘和SSD盘的容量、型号及挂载信息的方法

《Linux查看系统盘和SSD盘的容量、型号及挂载信息的方法》在Linux系统中,管理磁盘设备和分区是日常运维工作的重要部分,而lsblk命令是一个强大的工具,它用于列出系统中的块设备(blockde... 目录1. 查看所有磁盘的物理信息方法 1:使用 lsblk(推荐)方法 2:使用 fdisk -l(

使用Python获取JS加载的数据的多种实现方法

《使用Python获取JS加载的数据的多种实现方法》在当今的互联网时代,网页数据的动态加载已经成为一种常见的技术手段,许多现代网站通过JavaScript(JS)动态加载内容,这使得传统的静态网页爬取... 目录引言一、动态 网页与js加载数据的原理二、python爬取JS加载数据的方法(一)分析网络请求1

MySQL查看表的最后一个ID的常见方法

《MySQL查看表的最后一个ID的常见方法》在使用MySQL数据库时,我们经常会遇到需要查看表中最后一个id值的场景,无论是为了调试、数据分析还是其他用途,了解如何快速获取最后一个id都是非常实用的技... 目录背景介绍方法一:使用MAX()函数示例代码解释适用场景方法二:按id降序排序并取第一条示例代码解

Python中合并列表(list)的六种方法小结

《Python中合并列表(list)的六种方法小结》本文主要介绍了Python中合并列表(list)的六种方法小结,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋... 目录一、直接用 + 合并列表二、用 extend() js方法三、用 zip() 函数交叉合并四、用

Java 中的跨域问题解决方法

《Java中的跨域问题解决方法》跨域问题本质上是浏览器的一种安全机制,与Java本身无关,但Java后端开发者需要理解其来源以便正确解决,下面给大家介绍Java中的跨域问题解决方法,感兴趣的朋友一起... 目录1、Java 中跨域问题的来源1.1. 浏览器同源策略(Same-Origin Policy)1.

Java Stream.reduce()方法操作实际案例讲解

《JavaStream.reduce()方法操作实际案例讲解》reduce是JavaStreamAPI中的一个核心操作,用于将流中的元素组合起来产生单个结果,:本文主要介绍JavaStream.... 目录一、reduce的基本概念1. 什么是reduce操作2. reduce方法的三种形式二、reduce

MybatisX快速生成增删改查的方法示例

《MybatisX快速生成增删改查的方法示例》MybatisX是基于IDEA的MyBatis/MyBatis-Plus开发插件,本文主要介绍了MybatisX快速生成增删改查的方法示例,文中通过示例代... 目录1 安装2 基本功能2.1 XML跳转2.2 代码生成2.2.1 生成.xml中的sql语句头2

python3 pip终端出现错误解决的方法详解

《python3pip终端出现错误解决的方法详解》这篇文章主要为大家详细介绍了python3pip如果在终端出现错误该如何解决,文中的示例方法讲解详细,感兴趣的小伙伴可以跟随小编一起了解一下... 目录前言一、查看是否已安装pip二、查看是否添加至环境变量1.查看环境变量是http://www.cppcns