ORB_SLAM中的优化函数

最新推荐文章于 2024-05-20 17:10:24 发布
最新推荐文章于 2024-05-20 17:10:24 发布
阅读量1.5k 收藏 1
点赞数 1
分类专栏： slam 文章标签： SLAM
本文详细解析了ORB_SLAM中的五个关键优化函数：GlobalBundleAdjustment、PoseOptimization、LocalBundleAdjustment、OptimizeEssentialGraph及OptimizeSim3的工作原理。通过不同层次的优化，实现了从单帧到全局地图的精准定位。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

转载于：http://blog.youkuaiyun.com/qinruiyan/article/details/50918504

ORB_SLAM中使用到g2o进行优化的函数有四个: 
GlobalBundleAdjustemnt 
PoseOptimization 
LocalBundleAdjustment 
OptimizeEssentialGraph 
OptimizeSim3.

下面对这几个不同的函数进行逐一分析:

1. GlobalBundleAdjustment

这个函数的作用是对全局的地图点和keyframe位姿进行优化. 即大图优化, 非常费时. 
(1) 提取出所有的keyframes和所有的地图点. 
(2) 把keyframe设置为图中的节点 
(3) 把每一个地图点设置为图中的节点, 然后对于每一个地图点, 找出所有能看到这个地图点的keyframe. 
(4) 对每一个keyframe, 建立边: 
边的两端分别是地图点的SE3位姿与当前keyframe的SE位姿. 
边的观测值为该地图点在当前keyframe中的二维位置. 
信息矩阵(权重)是观测值的偏离程度, 即3D地图点反投影回地图的误差.

(5) 构图完成, 进行优化. 
(6) 把优化后的地图点和keyframe位姿全部放回原本的地图中.

2. PoseOptimization

对于每一个keyframe, 进行位姿的局部优化. 
(1) 直接把当前keyframe设为地图中的节点 
(2) 找出所有在当前keyframe中可见的的三维地图点, 对每一个地图点, 建立边: 
边的两端分别是keyframe的位姿与当前地图点为位姿. 
边的观测值为该地图点在当前keyframe中的二维位置. 
信息矩阵(权重)是观测值的偏离程度, 即3D地图点反投影回地图的误差. 
(3) 构图完成,进行优化. 这里的优化要做四次, 没搞明白为什么. 
(4) 把优化后的keyframe位姿放回去.

3. LocalBundleAdjustment

(1) 找到Local Keyframe, 即那些共享CovisbilityMap的Keyframes. 存入lLocalKeyFrames. 
(2) 找到所有Local Keyframes都能看到的地图点, 其实就是CovisbilityMap的地图点. 存入lLocalMapPoints. 
(3) 再找出能看到上面的地图点, 却不在Local Keyframe范围内的keyframe(为什么??). 存入lFixedCameras. 
(4) 把上面的Local Keyframe, Map Point, FixedCamera都设置为图节点. 
(5)对于lLocalMapPoints中的每一个地图点及能看到它的所有keyframes, 建立边: 
边的两端分别是keyframe的位姿与当前地图点为位姿. 
边的观测值为该地图点在当前keyframe中的二维位置. 
信息矩阵(权重)是观测值的偏离程度, 即3D地图点反投影回地图的误差. 
(6) 去除掉一些不符合标准的边. 
(7) 把优化后地图点和keyframe位姿放回去.

4. OptimizeEssentialGraph

这个函数应该是这几个之中最重要的一个. 
(1) 首先获取所有keyframes和地图点. 
(2) 把keyframe都设为图节点, 剔除不好的keyframe. 这里的frame位姿为Sim3. 
(3) 添加边 
<1> Loop edge: LoopConnections 是一个Map, Map中第一个元素是有loop的keyframe, 第二个元素是与第一个元素形成loop的keyframe集合. 给它们全部添加边进行连接. 
边的观测值是后一帧的SIM3位姿乘以前一帧的SIM3位姿的逆. (为什么??) 
<2> Normal edge: 遍历所有的keyframe, 
- 找到当前keyframe的parent keyframe, 建立边连接. 边的观测值为parent keyframe的位姿乘以keyframe位姿的逆(为什么??). 信息矩阵为单位矩阵. 
- 找到与当前keyframe形成Loop的所有keyframe, 如果找到成Loop的keyframe在当前keyframe之前, 则在两个keyframe之间添加一个边连接. 观测值为Loop keyframe的位姿乘以keyframe位姿的逆(为什么??). 信息矩阵为单位矩阵. 
- 找到当前keyframe的covisibility graph 中的每一个keyframe, 建立边连接. 观测值为covisibility graph keyframe位姿乘以keyframe位姿的逆(为什么??). 信息矩阵为单位矩阵.

(4) 构图完成, 进行优化. 
(5) 更新EssentialGraph中的所有位姿.

5. OptimizeSim3

(1) 把输入的KF1到KF2的位姿变换SIM3加入图中作为节点0. 
(2) 找到KF1中对应的所有map点, 放在vpMapPoints1中. vpMatches1为输入的匹配地图点, 是在KF2中匹配上map点的对应集合. 
(3) Point1是KF1的Rotation matrix*map point1的世界坐标+KF1的Translation matrix. Point2是KF2的Rotation matrix*map point2的世界坐标+KF2的Translation matrix. 
把Point1 Point2作为节点加入图中 
(4) 在节点0与Point1, Point2之间都建立边连接. 测量值分别是地图点反投影在图像上的二维坐标, 信息矩阵为反投影的误差. 
(5) 图构建完成, 进行优化! 
(6) 更新两帧间转换的Sim3.

总结一下, ORB_SLAM中的优化分为多个层次, 从低到高为:

  1. PoseOptimization: 优化单帧位姿. 每个帧可见多个地图点, 可以建立多个边连接, 构成图进行优化. 优化单一帧的SE3位姿.
  2. OptimizeSim3: 优化两帧之间的位姿变换, 因为两帧之间可以看到多个相同的地图点, 可以构成一个超定方程组, 可以最小化误差优化. 优化帧间变化的SIM3位姿与地图的VertexSBAPointXYZ位姿
  3. LocalBundleAdjustment: 在一个CovisbilityMap内进行优化. 在一定范围的keyframe中,可以看到同一块地图, 即是CovisbilityMap. 连接CovisbilityMap内的每一个map点与可见它的所有keyframe, 放在一起进行优化. 这个优化是双向的, 既优化地图点的VertexSBAPointXYZ位姿, 又优化Camera的SE3位姿.
  4. OptimizeEssentialGraph: 加入Loop Closure的考虑, Covisbility图中的keyframe相互连起来, Keyframe之间有前后相连, 于是不同的Covisbility图也可以联系起来. 这是一个大范围的优化, 主要是加入了Loop Closure约束. 优化的是Camera的SIM3位姿.
  5. GlobalBundleAdjustment: 最大范围的优化, 优化所有Camera的SE3位姿与地图点的XYZ位姿.
【ORB_SLAM2源码解读】EuRoC双目数据集跑通ORB_SLAM2
小秋 AI SLAM 入门实战教程
04-08 3756
Tracking-跟踪Local Mapping-地图Loop Closing-回环ORB_SLAM2与传感器之间是相互独立，提取特征之后，图像不需要被保存而是直接丢弃。ORB_SLAM2将编译出libORB_SLAM2 . so安装在lib目录中并且生成可执行的数据集测试样例。
【逐函数讲解ORB_SLAM2源码】3.ORB_SLAM2系统初始化
小秋 AI SLAM 入门实战教程
04-11 711
文章目录视频讲解mpVocabulary = new ORBVocabulary();mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);mpMap = new Map();mpFrameDrawer = new FrameDrawer(mpMap);mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);mpTracker = new Tracking 创建tracking对象并执行trackin
ORB-SLAM2源码解读（3）：LocalMapping
try_again_later的博客
11-15 2016
ORB-SLAM2的前端VO部分分为：Tracking和LocalMapping， Tracking线程负责根据输入的Frame恢复出相机位姿T并跟踪局部地图，最后生成关键帧传给LocalMapping线程。 LocalMapping线程负责对新加入的KeyFrames和MapPoints筛选融合，剔除冗余的KeyFrames和MapPoints，维护稳定的KeyFrame集合，传给后续的Lo...
【SLAM】位姿图优化的入门与实践
heroybc的博客
05-26 7895
1.位姿图 当把BA优化问题中的位姿图的问题简化要怎么做？ BA优化中的计算量很大很大，为了减少计算量，我已经介绍过很多方法了，这里，我们可以对位姿图进行优化，要如何做到，其实也很简单，只需要把相机观察的三维点去掉，在g2o优化问题上，只使用相机顶点，以及相邻相机顶点的关系描述即可。 这样，我们就大大简化了计算量，为啥这就简化计算量了呢，因为我们只考虑相机顶点，以及相邻相机顶点的关系，不考虑三维点了，我们知道，三维点的数量一定远远大于相机位姿顶点的数量。 这就是位姿图，在g2o优化问题上，只使用相机顶点，
ORB-SLAM（十二）优化
weixin_34143774的博客
03-01 222
ORB-SLAM中优化使用g2o库，先复习一下g2o的用法，上类图 其中SparseOptimizer就是我们需要维护的优化求解器，他是一个优化图，也是一个超图（包含若干顶点和一元二元多元边），怎样定义图的顶点（优化变量_estimate）和边（误差项_error）是用户需要考虑的问题，可以从g2o/types查找是否已经有定义好的顶点或边，若没有，需要自己去实现。 自己实现的时候注意，基...
ORB-SLAM2代码阅读笔记（十）：sim3求解
06-22 1万+
Table of Contents 1.sim3的简单概述 2.sim3算法介绍 1)3对匹配的3D点建立坐标系 2)旋转矩阵计算 3）平移向量计算 4）尺度计算 3.代码解析 1）sim3求解器构造函数Sim3Solver 2）sim3迭代求解Sim3Solver::iterate 本篇笔记对 ORB-SLAM2代码阅读笔记（八）：LoopClosing线程 中提到的s...
Bundle adjustment 总结
pikachu_777的专栏
04-04 1026
BA 最小化重投影误差： X=argmin∑i=1Ni∑j=1Nj∣∣ei,j(X)∣∣X=argmin\sum_{i=1}^{N_i}\sum_{j=1}^{N_j}||e_{i,j}(X)||X=argmini=1∑Ni​​j=1∑Nj​​∣∣ei,j​(X)∣∣ 求解方法： 使用LM算法优化求最优解。每一次迭代中的增量求解为： δX∗=argmin∣∣J(X)δX+E(X)∣∣+λ∣∣D(X...
ORB_SLAM2位姿发布所需的文件
04-22
在给定的压缩包文件中，我们重点关注的是ORB_SLAM2用于发布位姿的相关文件。 ORB_SLAM2的核心功能是通过处理来自相机的图像序列，实时估计相机的位姿（位置和姿态）并构建环境地图。位姿发布是系统的一个关键组件，...
ORB_SLAM2、ORB_SLAM3跑EuRoc&KITTI数据集及evo评估
weixin_59058976的博客
12-04 1万+
ORB_SLAM2 ORB_SLAM3 为使结果能在evo中进行评估，需要修改文件夹下system.cc文件中的第638行和第647行，把1e9*pKF改成pKF,改完示例如下： f << setprecision(6) << pKF->mTimeStamp << " " << setprecision(9) << t.at<float>(0) << " " << t.at<float&
ORB_SLAM2中从特征点到地图点完整PipeLine介绍
yzhhhh5068的博客
05-20 1946
在 `ORB_SLAM2` 中，特征点与地图点是整个 `SLAM` 系统的关键，所以整个 Pipeline 贯穿了 `Tracking` 和 `LocalMapping` 两大线程，本文将从特征点产生开始， 探讨它是怎么一步步经过删除和融合最终变成 `MapPoint` 地图点的。
SLAM笔记（五）光束平差法(Bundle Adjustment)
一只飞鱼的博客
04-09 2万+
1.什么是光束平差法 前边的八点法，五点法等可以求出闭式解的前提是已经知道确切的点对。但实际情况中往往存在大量的噪声，点与点不是精确地对应甚至出现一些错误匹配。 光束平差法由Bundle Adjustment翻译得来，有两层意思： 对场景中任意三维点P，由从每个视图所对应的的摄像机的光心发射出来并经过图像中P对应的像素后的光线，都将交于P这一点，对于所有三维点，则形成相当多的光束（bundl...
bundle adjustment算法学习
Where there is life, there is hope
11-27 1万+
今天学习了稀疏的光束平差法，基于上一篇博文Levenberg–Marquardt算法学习，这里对学习内容做一个理论梳理。本次内容包括：BA简介BA迭代步长的数学推导稀疏BA迭代步长的算法求解过程1.BA简介   摄像机在静态环境中移动，得到不同时刻拍摄的多幅图像。假设这些图像是同一刚性物体的投影，则可由图像特征对应关系估计出摄像机的运动参数。在计算机视觉中 ，这一过程称为运动分析或由运动重建物体结
Sim3相似变换
sdw8855的博客
11-10 2292
通过sim3变换解出当前关键帧和闭环候选帧的匹配MapPoint之间的旋转矩阵R、平移向量t、尺度变换s，也就得到了当前关键帧到闭环关键帧之间的sim3变换gScm。使用这个sim3变换gScm乘上闭环关键帧的sim3位姿gSmw，mg2oScw=gScm*gSmw的乘积mg2oScw就是当前关键帧的sim3位姿，之后在闭环校正中就可以使用这个sim3位姿转换为SE3位姿后对当前关键帧进行位姿校正。上理解：两组三维点，能构成6个方程，解6个未知数；在相似变换中，通过增加一个点，三个点构成三角形，通过。
Bundle Adjustment简述
热门推荐
记起来就随便写写
03-21 6万+
关于Bundle Adjustment简要介绍
covisibility graph 和 essential graph 的理解
u014488388的专栏
03-23 6926
参考：https://www.zhihu.com/question/42050992/answer/93631387 根据半闲居士的回答，我的理解如下： #1 首先“图”的概念：应该是由顶点和边组成的。顶点数量不限，每个顶点连接的边的数量不限。 #2 顶点：待优化的变量（举例：相机的pose） #3 边：误差项（举例：pose-pose之间的位置关系） #4 权值：每条边会有一个
认真的虎ORBSLAM2源码解读(三)：Optimizer
认真的虎的博客
05-18 2112
目录
SLAM流程之优化后端
Fredric18的博客
07-09 4566
优化后端： SLAM的优化后端完成工作主要是对视觉前端得到的不够准确的相机位姿和重建地图进行优化微调。在视觉前端中，不管是进行位姿估计还是建图，都是利用相邻帧之间的关系来完成的，这种依赖局部约束且不停地链式进行的算法，必将导致优化误差逐帧累积，最终产生一个较大的误差漂移。因此，后端优化的思路就是从全局（整个相机运动过程）中选取一些关键帧，利用这些关键帧之间的关系建立起时间和空间跨度更大的、需要同时...
ORB-SLAM2 闭环检测 computeSim3和CorrectLoop 流程图
qq_28448117的博客
05-31 3105
接上一篇博客：第四阶段：紫色线，计算当前关键帧mpCurrentKF共视的关键帧mvpCurrentConnectedKFs的纠正位姿CorrectedSim3和未纠正位姿NonCorrectedSim3，并用纠正后的位姿计算mvpCurrentConnectedKFs和mvpCurrentConnectedKFs的地图点vpMPsi的worldpose。第五阶段：蓝色线，用当前关键帧mpCurr...
视觉slam14讲之后端优化
入山砍柴郎
04-12 6105
&nbsp; &nbsp; &nbsp;前端视觉里程计能给出一个短时间内的轨迹和地图，但不可避免的误差累积，这个地图在长时间内是不准确的。要想构建一个尺度，规模更大的优化问题，以考虑长时间内的最优轨迹和地图，这时后端优化就出现了。 1.滤波器模型 &nbsp; &nbsp; &nbsp;a. 马尔可夫性，当前状态只和上一个时刻有关。 P(Xk| Xk-1,X0, U1:k,Z1:k-1) ...
ORB_SLAM2代价函数
最新发布
03-29
<think>好的，我需要回答用户关于ORB_SLAM2中使用的代价函数及其在SLAM过程中的实现和应用的问题。首先，我得回忆一下ORB_SLAM2的相关知识，确保信息的准确性。ORB_SLAM2是一个基于特征的视觉SLAM系统，主要使用ORB特征，所以代价函数应该和特征匹配、位姿优化有关。 用户提到了参考内容，其中引用了视觉词汇表、k-medians聚类、词汇树结构等内容，还有提到全局图像描述符的改进。这些可能和回环检测或者特征匹配有关，但用户的问题是关于代价函数的，我需要集中在优化部分的代价函数上。 ORB_SLAM2中的代价函数主要出现在跟踪（Tracking）、局部建图（Local Mapping）和回环检测（Loop Closing）这三个模块中。特别是跟踪和局部优化时，会用到重投影误差作为代价函数。重投影误差是Bundle Adjustment（BA）中的核心概念，用来优化相机位姿和地图点的位置。 接下来需要详细说明代价函数的具体形式。重投影误差通常是将3D地图点投影到图像平面，与检测到的特征点坐标之间的误差。数学表达式应该是非线性最小二乘问题，比如最小化误差平方和。这时候需要用李代数表示相机的旋转和平移，以方便优化。例如，代价函数可能写作： $$\sum_{i} \left\| \pi(T \cdot X_i) - x_i \right\|^2$$ 其中，$T$是相机的位姿（用SE3表示），$X_i$是3D点，$x_i$是观测到的2D特征点，$\pi$是投影函数。 然后，在ORB_SLAM2的实现中，可能使用了g2o或Ceres这样的优化库来处理这个非线性优化问题。需要提到使用了鲁棒核函数，比如Huber核，来减少异常值的影响。此外，ORB_SLAM2可能采用了协方差加权，根据特征点的尺度层级赋予不同的权重，比如高层级的特征点具有更低的权重，因为它们的定位可能不够精确。 在局部建图和全局BA中，代价函数的作用是优化相机位姿和地图点，确保全局一致性。回环检测时，通过优化位姿图来校正累积误差，这时代价函数可能涉及位姿之间的约束，比如闭环边带来的约束。 还要考虑参考内容中的视觉词汇树，可能与回环检测中的相似性计算有关，但可能不直接涉及代价函数。不过，特征匹配的准确性会影响重投影误差的计算，所以间接相关。 需要确认ORB_SLAM2是否使用其他代价函数，比如光度误差，但ORB是二进制描述子，可能更侧重几何误差而不是光度一致性，所以应该是重投影误差为主。 总结结构：先介绍代价函数在SLAM中的总体作用，再分模块（跟踪、局部建图、回环）说明，然后详细讲解重投影误差的数学形式，优化方法，鲁棒核函数，协方差加权等实现细节，最后提到其在全局优化中的应用。 还要注意用户提到的引用，比如引用[1]中的词汇树可能在回环检测中使用，而引用[2]中的K-Max池可能不直接相关，但特征描述子的处理可能影响匹配，从而影响代价函数的输入。不过主要回答应聚焦在代价函数本身。 可能需要查阅ORB_SLAM2的论文或源码来确认细节，比如是否使用g2o，是否确实应用了协方差加权，以及鲁棒核的类型。根据记忆，ORB_SLAM2确实使用g2o进行优化，重投影误差是主要代价函数，并应用了Huber核。 最后，生成相关问题，如代价函数的选择原因，优化方法，鲁棒核的作用，如何提高稳定性等。</think>ORB-SLAM2中使用的代价函数主要基于**重投影误差**，在SLAM过程中用于优化相机位姿与地图点位置。其核心实现可分为以下几个部分： --- ### 一、代价函数的核心形式 **重投影误差**定义为3D地图点$X_w$投影到图像平面后与观测特征点$u_{obs}$的欧氏距离： $$ e = \| \pi(R \cdot X_w + t) - u_{obs} \|_2 $$ 其中： - $R, t$为相机位姿（旋转矩阵与平移向量） - $\pi(\cdot)$为相机投影函数（含内参矩阵$K$） - 优化目标是最小化所有匹配点的误差平方和：$$\min_{R,t,X_w} \sum \rho(e^2)$$ $\rho(\cdot)$为鲁棒核函数（如Huber核），用于抑制异常值[^1]。 --- ### 二、在SLAM流程中的具体应用 1. **跟踪（Tracking）** - 通过**PnP算法**实时优化当前帧位姿，代价函数仅优化$R,t$，固定地图点$X_w$。 - 使用RANSAC剔除误匹配，确保仅内点参与优化。 2. **局部建图（Local Mapping）** - **局部Bundle Adjustment**同时优化局部关键帧位姿$\{T_i\}$和地图点$\{X_j\}$： $$ \min_{\{T_i\},\{X_j\}} \sum_{i,j} \rho\left( \|\pi(T_i \cdot X_j) - u_{ij}\|^2 \right) $$ - 引入**协方差加权**，根据ORB特征点的尺度层级调整权重（高层级特征权重更低）。 3. **回环检测（Loop Closing）** - **位姿图优化**（Pose Graph Optimization）优化关键帧位姿$\{T_k\}$，最小化相邻帧的相对运动误差与回环约束误差： $$ \min_{\{T_k\}} \sum_{(i,j)} \| \log(T_{ij}^{-1} \cdot T_i^{-1} \cdot T_j) \|^2 $$ 其中$T_{ij}$为帧$i,j$间的测量约束。 --- ### 三、代码实现关键点（基于g2o库） ```cpp // 定义重投影误差边（EdgeSE3ProjectXYZ） g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ(); e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose_vertex)); // 位姿顶点 e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(point_vertex)); // 地图点顶点 e->setMeasurement(obs); // 观测特征点坐标 e->setInformation(Matrix2d::Identity() * inv_sigma2); // 信息矩阵（协方差加权） e->setRobustKernel(new g2o::RobustKernelHuber()); // Huber核函数 ``` --- ### 四、作用总结 1. **位姿估计**：通过最小化重投影误差，优化相机运动轨迹。 2. **地图一致性**：联合优化位姿与地图点，提升全局一致性。 3. **鲁棒性增强**：Huber核与RANSAC抑制误匹配影响。 4. **实时性保障**：分层优化（局部BA与位姿图优化）平衡精度与效率[^2]。 ---
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值