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

相关文章

SpringBoot3实现Gzip压缩优化的技术指南

《SpringBoot3实现Gzip压缩优化的技术指南》随着Web应用的用户量和数据量增加,网络带宽和页面加载速度逐渐成为瓶颈,为了减少数据传输量,提高用户体验,我们可以使用Gzip压缩HTTP响应,... 目录1、简述2、配置2.1 添加依赖2.2 配置 Gzip 压缩3、服务端应用4、前端应用4.1 N

Linux换行符的使用方法详解

《Linux换行符的使用方法详解》本文介绍了Linux中常用的换行符LF及其在文件中的表示,展示了如何使用sed命令替换换行符,并列举了与换行符处理相关的Linux命令,通过代码讲解的非常详细,需要的... 目录简介检测文件中的换行符使用 cat -A 查看换行符使用 od -c 检查字符换行符格式转换将

SpringBoot实现数据库读写分离的3种方法小结

《SpringBoot实现数据库读写分离的3种方法小结》为了提高系统的读写性能和可用性,读写分离是一种经典的数据库架构模式,在SpringBoot应用中,有多种方式可以实现数据库读写分离,本文将介绍三... 目录一、数据库读写分离概述二、方案一:基于AbstractRoutingDataSource实现动态

Java中的String.valueOf()和toString()方法区别小结

《Java中的String.valueOf()和toString()方法区别小结》字符串操作是开发者日常编程任务中不可或缺的一部分,转换为字符串是一种常见需求,其中最常见的就是String.value... 目录String.valueOf()方法方法定义方法实现使用示例使用场景toString()方法方法

Java中List的contains()方法的使用小结

《Java中List的contains()方法的使用小结》List的contains()方法用于检查列表中是否包含指定的元素,借助equals()方法进行判断,下面就来介绍Java中List的c... 目录详细展开1. 方法签名2. 工作原理3. 使用示例4. 注意事项总结结论:List 的 contain

Spring Boot + MyBatis Plus 高效开发实战从入门到进阶优化(推荐)

《SpringBoot+MyBatisPlus高效开发实战从入门到进阶优化(推荐)》本文将详细介绍SpringBoot+MyBatisPlus的完整开发流程,并深入剖析分页查询、批量操作、动... 目录Spring Boot + MyBATis Plus 高效开发实战:从入门到进阶优化1. MyBatis

MyBatis 动态 SQL 优化之标签的实战与技巧(常见用法)

《MyBatis动态SQL优化之标签的实战与技巧(常见用法)》本文通过详细的示例和实际应用场景,介绍了如何有效利用这些标签来优化MyBatis配置,提升开发效率,确保SQL的高效执行和安全性,感... 目录动态SQL详解一、动态SQL的核心概念1.1 什么是动态SQL?1.2 动态SQL的优点1.3 动态S

macOS无效Launchpad图标轻松删除的4 种实用方法

《macOS无效Launchpad图标轻松删除的4种实用方法》mac中不在appstore上下载的应用经常在删除后它的图标还残留在launchpad中,并且长按图标也不会出现删除符号,下面解决这个问... 在 MACOS 上,Launchpad(也就是「启动台」)是一个便捷的 App 启动工具。但有时候,应

SpringBoot日志配置SLF4J和Logback的方法实现

《SpringBoot日志配置SLF4J和Logback的方法实现》日志记录是不可或缺的一部分,本文主要介绍了SpringBoot日志配置SLF4J和Logback的方法实现,文中通过示例代码介绍的非... 目录一、前言二、案例一:初识日志三、案例二:使用Lombok输出日志四、案例三:配置Logback一

Python如何使用__slots__实现节省内存和性能优化

《Python如何使用__slots__实现节省内存和性能优化》你有想过,一个小小的__slots__能让你的Python类内存消耗直接减半吗,没错,今天咱们要聊的就是这个让人眼前一亮的技巧,感兴趣的... 目录背景:内存吃得满满的类__slots__:你的内存管理小助手举个大概的例子:看看效果如何?1.