ORB-SLAM中优化使用g2o库,先复习一下g2o的用法,上类图

其中SparseOptimizer就是我们需要维护的优化求解器,他是一个优化图,也是一个超图(包含若干顶点和一元二元多元边),怎样定义图的顶点(优化变量_estimate)和边(误差项_error)是用户需要考虑的问题,可以从g2o/types查找是否已经有定义好的顶点或边,若没有,需要自己去实现。

自己实现的时候注意,基本都是去继承BaseVertex<D,T>; BaseUnaryEdge<D,E,VertexXi>...这几个模板类,根据需要实现(override)虚函数.

  • virtual void setToOriginImpl() 设定优化变量0点 _estimate = 0 ;
  • virtual void oplusImpl(const double* v)实现优化变量的增量计算,特别是优化变量不在欧式空间中,没有的"+"定义时。例如李代数中,需要使用左乘或者右乘定义而不是直接的加法;
  • void computeError() 计算估计值和测量值之间的误差项_error
  • virtual void linearizeOplus() 计算雅可比的解析形式_jacobianOplus[i],每一个雅可比的类型为MatrixX::MapType, 参考Eigen::Map类 https://eigen.tuxfamily.org/dox/classEigen_1_1Map.html,可以认为是将一块内存数据构造成一个矩阵,比较灵活地适应相对各个优化变量的雅可比形式。

这张类图的下半部分就是我们初始化优化求解器时需要指定的求解方法。

求解的梯度下降算法可以选择GN,LM(最常用),或者DogLeg;

算法求解器还包括两部分,

  • 计算目标函数雅可比和海塞(或者近似的海塞),以及执行Schur complement的BlockSolver,需要指定优化变量的维度,常见的有

    •   //variable size solver
      using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>; // solver for BA/3D SLAM
      using BlockSolver_6_3 = BlockSolverPL<, >; // solver fo BA with scale
      using BlockSolver_7_3 = BlockSolverPL<, >; // 2Dof landmarks 3Dof poses
      using BlockSolver_3_2 = BlockSolverPL<, >;

      可以设置为动态的BlockSolverX

  • 求解线性方程组 Hx = b (linear problem constructed from _jacobianOplus and _error)的线性求解器,完全采用第三方的线性代数库,主要采用Cholesky分解和PCG迭代,具体可以选择的有
    • Cholmod,CSparse (以上两者为比较著名的线性代数库),
    • PCG (pre-conditioner is block Jacobi),
    • Dense(dense Cholesky decomposition),
    • 或者ORB-SLAM中使用的Eigen(sparse Cholesky decoposition from Eigen)。

因此,ORB-SLAM中优化求解器初始化过程如下:

g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
// 线性方程求解器
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
// 稀疏矩阵块求解器
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// 梯度下降算法
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

ORB中使用的这些优化函数是非常重要的,在视觉SLAM中有很强的通用性,自己实现的时候完全可以参考其实现方法。分为:

1. BundleAdjustment()

  • GlobalBundleAdjustment():用于单目初始化的CreateInitialMapMonocular函数以及闭环优化的RunGlobalBundleAdjustment函数(在闭环结束前新开一个线程,全局优化,在此之前会OptimizeEssentialGraph,论文中说其实这里全局优化提升的精度有限)。
  • LocalBundleAdjustment():用于LocalMapping线程中剔除关键帧之前的局部地图优化。

2. PoseOptimization()

  • 只优化当前帧pose,地图点固定。
  • 用于LocalTracking中运动模型跟踪,参考帧跟踪,地图跟踪TrackLocalMap,重定位。

3. OptimizeEssentialGraph()

  • EssentialGraph包括所有的关键帧顶点,但是优化边大大减少,包括spanning tree(生成树),共视权重θ>100的边,以及闭环连接边。
  • 用于闭环检测Sim3调整后优化。

4. OptimizeSim3()

  • 在用RANSAC求解过Sim3,以及通过Sim3匹配更多的地图点后,对当前关键帧,闭环关键帧,以及匹配的地图点进行优化,获得更准确的Sim3位姿,再去下一步的闭环调整。

使用到的g2o顶点包括:

1. VertexSE3Expmap():SE(3)位姿

2. VertexSim3Expmap():Sim(3)位姿

3. VertexSBAPointXYZ():地图点坐标

使用到的g2o边包括:

1. EdgeSE3ProjectXYZ():BA中的重投影误差(3D-2D(u,v)误差),将地图点投影到相机坐标系下的相机平面。

2. EdgeSE3ProjectXYZOnlyPose():PoseEstimation中的重投影误差,将地图点投影到相机坐标系下的相机平面。优化变量只有pose,地图点位置固定,是一边元,双目中使用的是EdgeStereoSE3ProjectXYZOnlyPoze()。

3. EdgeSim3():Sim3之间的相对误差。优化变量只有Sim3表示的pose,用于OptimizeEssentialGraph。

4. EdgeSim3ProjectXYZ():重投影误差。优化变量Sim3位姿与地图点,用于闭环检测中的OptimizeSim3。

ORB-SLAM(十二)优化的更多相关文章

  1. SLAM中的优化理论(二)- 非线性最小二乘

    本篇博客为系列博客第二篇,主要介绍非线性最小二乘相关内容,线性最小二乘介绍请参见SLAM中的优化理论(一)-- 线性最小二乘.本篇博客期望通过下降法和信任区域法引出高斯牛顿和LM两种常用的非线性优化方 ...

  2. [十二省联考2019]字符串问题——后缀自动机+parent树优化建图+拓扑序DP+倍增

    题目链接: [十二省联考2019]字符串问题 首先考虑最暴力的做法就是对于每个$B$串存一下它是哪些$A$串的前缀,然后按每组支配关系连边,做一遍拓扑序DP即可. 但即使忽略判断前缀的时间,光是连边的 ...

  3. Mysql优化(出自官方文档) - 第十二篇(优化锁操作篇)

    Mysql优化(出自官方文档) - 第十二篇(优化锁操作篇) 目录 Mysql优化(出自官方文档) - 第十二篇(优化锁操作篇) 1 Internal Locking Methods Row-Leve ...

  4. 高博-《视觉SLAM十四讲》

    0 讲座 (1)SLAM定义 对比雷达传感器和视觉传感器的优缺点(主要介绍视觉SLAM) 单目:不知道尺度信息 双目:知道尺度信息,但测量范围根据预定的基线相关 RGBD:知道深度信息,但是深度信息对 ...

  5. 《SLAM十四讲》个人学习知识点梳理

    0.引言 从六月末到八月初大概一个月时间一直在啃SLAM十四讲[1]这本书,这本书把SLAM中涉及的基本知识点都涵盖了,所以在这里做一个复习,对这本书自己学到的东西做一个梳理. 书本地址:http:/ ...

  6. 视觉slam十四讲第七章课后习题6

    版权声明:本文为博主原创文章,转载请注明出处: http://www.cnblogs.com/newneul/p/8545450.html 6.在PnP优化中,将第一个相机的观测也考虑进来,程序应如何 ...

  7. 视觉slam十四讲第七章课后习题7

    版权声明:本文为博主原创文章,转载请注明出处:http://www.cnblogs.com/newneul/p/8544369.html  7.题目要求:在ICP程序中,将空间点也作为优化变量考虑进来 ...

  8. 《视觉SLAM十四讲》第2讲

    目录 一 视觉SLAM中的传感器 二 经典视觉SLAM框架 三 SLAM问题的数学表述 注:原创不易,转载请务必注明原作者和出处,感谢支持! 本讲主要内容: (1) 视觉SLAM中的传感器 (2) 经 ...

  9. 我的MYSQL学习心得(十二) 触发器

    我的MYSQL学习心得(十二) 触发器 我的MYSQL学习心得(一) 简单语法 我的MYSQL学习心得(二) 数据类型宽度 我的MYSQL学习心得(三) 查看字段长度 我的MYSQL学习心得(四) 数 ...

  10. 【腾讯Bugly干货分享】腾讯验证码的十二年

    本文来自于腾讯bugly开发者社区,未经作者同意,请勿转载,原文地址:http://dev.qq.com/topic/581301b146dfb1456904df8d Dev Club 是一个交流移动 ...

随机推荐

  1. yaml 配置文件

    https://www.cnblogs.com/xinjing-jingxin/p/9128293.html ---文件开始 - 列表 key: val 字典 注意:字典里面可以嵌套字典,列表等.(举 ...

  2. new ,malloc

    特征 new/delete malloc/free 分配内存的位置 自由存储区 堆 内存分配失败返回值 完整类型指针 void* 内存分配失败返回值 默认抛出异常 返回NULL 分配内存的大小 由编译 ...

  3. 禁用U盘的两种方法

    方法一:注册表 计算机\HKEY_LOCAL_MACHINE\SYSTEM\ControlSet003\Services\USBSTOR 更改值为4即可,恢复时同理 ,重启即可 方法二:组策略

  4. 我的第一个C++程序

    准备抽空学习C++了,不知道自己以后能不能坚持下去,去百度查了一下入门,大多数朋友都是选择用VC++或者VS,而我这里用的是C-Free 5 ,安装包也只有十几兆. 用起来也方便.对于初学者而言够用了 ...

  5. 跟着大神学Mongo,Mongodb主从复制本机简单操作总结

    原文链接:http://www.cnblogs.com/huangxincheng/archive/2012/03/04/2379755.html 本机安装MongoDB不在介绍,本文Mongo小菜鸟 ...

  6. 配置git

    https://blog.csdn.net/qq_34446663/article/details/81106018

  7. input输入框中list属性不能没有效果

    网页代码入下,就是出不来效果. 人物:zslsww 解决办法: 将option中的value属性去掉: 张山李四王五爪留 显示效果,问题得到解决.

  8. HTML中汉字空格占位符

    == 普通的英文半角空格   ==   ==   == no-break space (普通的英文半角空格但不换行)   == 中文全角空格 (一个中文宽度)   ==   == en空格 (半个中文 ...

  9. 也说java虚拟机

              学习java的人如果不了解java虚拟机,那真是白学了.    java为什么可以跨平台,就是因为虚拟机的作用,java虚拟机就相当于一个计算机,它有自己的内存结构,当java程序 ...

  10. 关于CoreLocation定位服务的简单使用

    在我们发微博,发表空间内容,以及在朋友圈发表动态的时候,会发现有一个位置信息的控件.iOS中是如何定位我们的位置信息的呢?基于此写一个小Demo,供大家参考使用. 在iOS中,用于定位时需要我们导入以 ...