《视觉SLAM十四讲精品总结》4：非线性优化g2o(BA基础)

最新推荐文章于 2025-04-28 17:41:29 发布

原创

最新推荐文章于 2025-04-28 17:41:29 发布 · 2.6k 阅读

12 ·

CC 4.0 BY-SA版权

本文介绍了g2o图优化的基本概念，包括SparseOptimizer类结构，图中包含的顶点和边的抽象基类及其派生类。优化过程涉及选择求解器、迭代算法和线性方程求解器。优化流程包括添加节点和边，设置初值并迭代计算雅可比矩阵和海塞矩阵，通过线性方程求解器找到梯度方向，最终完成SLAM中的非线性优化。此外，还提及了代码解析部分，包括CurvFittingVertex和CurveFittingEdge的实现细节。

一、g2o图优化（General Graph Optimization）

g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？我们祭出一张图：

先看上半部分。SparseOptimizer 是我们最终要维护的东东。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点（都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。

在优化之前，需要指定我们用的求解器和迭代算法。从图中下半部分可以看到，一个 SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell's dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞；一个是用于计算迭代过程中最关键的一步

HΔx=−b

这就需要一个线性方程的求解器。而这个求解器，可以从 PCG, CSparse, Choldmod 三者选一