g2o:一种图优化的C++框架

g2o: A general framework for graph optimization

原文发表于IEEE InternationalConference on Robotics and Automation (ICRA), Shanghai, China,May 2011

http://www.cnblogs.com/gaoxiang12/p/5244828.html 深入理解图优化与g2o：图优化篇

http://blog.csdn.NET/heyijia0327/article/details/50446140 李群、李代数在计算机视觉中的应用

建议结合本g2o论文和上面两篇文章看，以缓解理解压力。

摘要：

机器人和计算机视觉的很多问题，包括SLAM和BA，都可以归结为采用最小二乘法优化图像表示的非线性误差函数。本文描述了这些问题的结构，提出了g2o框架，一个开源C++ 框架，用于优化基于图像非线性误差函数。我们的系统容易扩展，新的方法可以直接添加到代码里面。目前提供了几种SLAM和BA方案。本文在真实环境下和模拟的数据集上对算法做了评估。结果表明g2o性能优良。

一简介

机器人和计算机视觉都有一个普遍性的问题，图优化的线性误差函数的最小化。比较典型的例子是SLAM和BA。解决这些问题的方法都是参数的配置或声明变量尽可能地解释清楚高斯噪声对测量的影响。比如，视觉SLAM的变量可能是机器人在环境中的位置或者路标在地图中的位置，可以通过机器人的传感器获得。因此可以通过两个变量的相对位置进行测量，比如里程计可以测量两个连续的位姿距离。类似的，在BA或基于路标的SLAM中，3D云点或路标可以通过观察到的云点在世界坐标系的位置和传感器的位置进行测量。

这些问题都可以通过图来表示。图的每一个节点（Node）都表示一个可以优化的变量，两个变量之间的每一个边缘（Edge）都代表了和它相连的两个成对的节点。在很多文献中都提到了这类问题。基本上都采用了标准的方法提供一个可接受的结果解决这样的应用问题，比如 Gauss-Newton, Levenberg-Marquardt (LM), Gauss-Seidel relaxation，或variants of gradient descent。但如果要获得更好的性能的话就需要专业知识并且持续努力。

本文描述了优化非线性最小二乘法问题的一般框架。我们称为g2o（general graph optimization）。图1是用g2o作为后端优化解决的各种问题。其性能与目前的最新的算法相媲美，可以处理非线性度量的一般形式。我们通过以下算法提高运行效率：

采用图的稀疏连接方式；
采用特殊的图结构处理上面提到的问题；
用一种新的方法处理离散线性系统；
有效利用现代处理器的指令系统SIMD优化缓存的使用。

除了高效之外，g2o也很通用，扩展性也很好：

2D SLAM算法的C++代码少于30行。用户只需要指定误差函数和参数就可以了。

我们将g2o应用于不同类型的最小二乘法优化问题中，用不同的算法解决问题比较了它们的性能。我们在真实环境中和模拟的数据集上进行了评估，在所有的试验中g2o性能与目前最新的算法相当，在几个例子中性能优于它们。

本文首先讨论了SLAM和BA的相关工作。在第3章中，我们详细描述了一个嵌入式的图优化问题，讨论了Gauss-Newton或LM的非线性最小二乘法。第4章中，讨论了程序的特点。最后，第5章，做实验评估了g2o和目前最新算法的比较。

二相关工作

图优化理论在机器人和机器视觉领域都有大量研究。论文19做出了开创性的工作，他们通过线性迭代进行图优化。当时，图优化被认为实时性能很差，但最近的出现的直接线性方法，如论文4，图优化被用于基于图的SLAM，取得了重大进展。例如，论文12采用了一个宽松的方式构建地图。论文6，采用高斯-塞德尔松散迭代法最小化误差（详见《数值分析》）。论文8介绍的多层松散迭代法，是高斯-塞德尔松散迭代法的一个变形，在不同的分辨率下使用。最近，论文22提出了一个梯度下降方法优化位置图。后来，论文4扩展了这种方法，他应用基于树的参数模型来增加融合速度。两种方法对初识的估计都具有鲁棒性，也很容易实现。然而，他们假设方差近似球形，因此在位姿图优化上会有困难，方差在特定值上为零度空间或有很大的差别。

图优化被认为是一个非线性最小二乘法问题（见《计算方法》），他通过构建一个当前状态的线性模型，进行迭代来解决问题。我们用于解决线性系统的方法是预处理共轭梯度方法 PCG，论文17和论文20也采用这种高效方法介绍大场景下松散位姿约束系统的问题。g2o具有较高效率，它包含了一个稀疏预处理共轭梯度方法 PCG，如论文13，采用block-Jacobi pre-conditioner（见《区域分解算法-算法与理论》）。

最近，论文5提出SAM开方系统，他们采用论文4的稀疏直接线性方法。论文14介绍了iSAM方法，可以更新非线性最小二乘问题的线性矩阵来解决这个问题。论文16通过探索线性系统的典型稀疏结构来构建线性矩阵。然而，这些方法都受制于就2D位姿图。在g2o里面，我们采用了类似的方法。该系统可以应用于各种变形问题的SLAM和捆集调整优化问题，例如：基于路标的2D SLAM，单目相机捆集调整，立体视觉捆集调整。然而，通过对比实验，我们发现g2o的性能具有明显优势。

在计算机视觉中，论文27的稀疏捆集调整，是一个非线性最小二乘法，它采用地图云点和相机位姿的雅可比矩阵的稀疏性。就在最近，又出现了新的方法，论文15，论文13，提出了与稀疏线性解决方法类似的方法，如第3章D节，通过舒尔分解降维算法有效计算大的系统（约1亿个稀疏矩阵单元）。也有一些新的系统采用非线性共轭梯度法，而不是线性方法，如论文1和论文2；它的收敛速度会更慢一些，但可以用于超大数据集（约10亿矩阵单元）。本文对比了g2o和论文15中的SSBA系统，它被认为是目前性能最好最新的系统。

三最小二乘法的非线性图优化

很多机器人和计算机视觉问题，都可以通过寻找函数的最小值问题来解决：

这里，X=(X1T, … , XnT)T表示参数向量，每个Xi表示一个参数组，Zij和Ωij分别表示参数Xi和Xj相关的平均和系数矩阵，e(Xi, Xj, Zij)是误差函数，表示参数Xi和Xj对约束Zij的满足程度。当Xi和Xj完全满足约束时，它的值是0。

为了简化公式，我们将误差函数分解为：

每个误差函数，参数块，都占用不同的存储空间。这个问题可以通过图的形式来表示。图的节点i表示参数块Xi，节点i和j之间的边表示两个参数块Xi和Xj的约束关系。图2 显示了图和目标函数的对应关系。

A. 最小二乘法优化

已知参数的初始估计`X ，等式2的计算方法可以采用高斯-牛顿或者LM算法来实现（《[数值分析方法库].Cambridge.Press.Numerical.Recipes.3rd.Edition.pdf》）。这个方法是通过初始估计为`X的一阶泰勒级数展开来估计误差函数：

这里Jij是eij(X)的雅可比矩阵，通过`X和进行计算。代入公式5，结合公式1，我们可以得到：

根据局部估计（局部收敛，全局不一定收敛），我们重写公式1中的函数F（X）：

公式13中的二次项由公式12推导，设

通过解线性方程14取得最小的⊿X

H是方程的系数矩阵。可以用⊿X*作为初始估计来求解

高斯-牛顿算法的迭代线性方程13，方法是方程14，等式15为迭代步骤。每次迭代，之前的结果作为一个线性点输入，知道给定判别结束。（详见《计算方法》）

LM算法引入了阻尼参数来控制收敛，不同于公式14，LM的迭代算式：

λ是阻尼因子：λ越大，⊿X越小。这对控制非线性系统的步长非常有用。LM算法是动态控制阻尼因子的。每次迭代都会控制误差。如果新的误差低于之前的误差，λ就会变小。否则，λ就会变大。

B. 参数法

上面描述的计算过程的通常方法是最小化变量方程。设参数空间X是欧几里得空间，但SLAM和BA无法使用。为了将状态变量扩展到非欧空间，通常的方法是对于参数Xi的增量⊿Xi的用不同表示方法。

比如，在SLAM问题中，参数Xi由平移变量ti和旋转变量αi表示。平移变量ti是一个欧式空间，旋转变量αi扩展至2D或 3D旋转空间SO（2）或SO（3）。为了避免奇异化，向量空间通过旋转矩阵或四元数用过参数化方法来表示。直接使用公式15做过参数化表示会破坏这个约束。为了克服这个问题，可以使用最小表示旋转（如3D空间的欧拉角）。但这受制于奇异矩阵。

另一种方法是计算新的误差函数，⊿Xi根据`Xi变动。⊿Xi用一个最小值表示旋转，Xi采用过参数化方法。通常⊿Xi都比较小，但不是奇异点。变量Xi*优化后通过非线性运算获得：

比如，在3D SLAM中，⊿Xi用平移向量和归一化四元数表示。位姿Xi用平移向量和完全四元数。田操作符表示Xi的增量⊿Xi，使用相对运动的坐标复合运算符进行运算Å（具体请查询论文25，“Estimating uncertain spatial realtionships in robotics”），需要先将增量转换成与位姿的相同变量：

这样就可以定义一个新的误差函数：

`X 扩展到过参数化向量空间（过参数化拟合）。

雅可比矩阵Jij变为：

增量通过初始估计`X的欧几里得空间计算，通过操作符田来计算重影射到原始空间。

g2o框架可以非常容易地定义不同向量空间的增量和状态变量，可以直接支持任意参数化。尽管使用了参数化方法，海塞矩阵H仍然保留。

C.线性化系统架构

根据等式13，矩阵H和向量b可以通过以下方式计算。设,，那么：

这种结构是由误差函数的雅可比矩阵构成。误差函数取决于2个顶点的取值，等式5的雅可比矩阵，可表示为：

Aij和Bij是误差函数中⊿Xi和⊿Xj的偏移量。

等式9可以得到矩阵Hij：

为了简化，我们去掉0元素。大家可能会注意到矩阵H是图的相邻矩阵。所以，有非零分块是途中的边。这是典型的稀疏H矩阵。在g2o中，我们使用了H的特性，采用最新的方法解决等式14的线性系统。

D. 系统中的特殊结构

有的问题，比如捆集调整，H矩阵具有更多的特征。我们采用这些特征结构对性能进行优化。在BA中，有2种变量，相机位姿p和相机观察到的路标位姿l。重新排列等式14中的变量，相机位姿带有下标，那么等式就变为：

通过H矩阵的舒尔补降维：

Hll是分块对角矩阵，计算Hll-1就比较容易。等式25根据相机位姿增量⊿Xp*来计算，我们可以计算下面的式子：

⊿X1*是调整世界特征的。通常，世界坐标特征的数量比相机位姿多，那么等式25比等式14的计算更快，尽管等式左边的矩阵也要化很多时间计算。

四实施

我们的C++代码实现尽可能地快，但也非常通用。为了达成这一目标，我们采用对图的顶点和边采用抽象基类。两个类都提供了虚函数，内部操作使用了局部参数提高效率。我们使用Eigen工具包，用SSE指令进行优化，可以获得更高的性能。

图3是我们的系统设计。灰色框部分是用于优化问题。采用所提供的基类，派生出新的节点定义田操作符操作增量。连接两个顶点Xi，Xj的边需要一个误差函数eij(.)。雅可比矩阵Jij就需要具体数字，为了提高计算效率，用户可以通过重写虚拟类函数指定Jij。所以，解决优化问题或比较不同的过参数化问题，只需要写很少的代码就可以了。

H的计算采用大小不固定的矩阵。如果变量维度，比如Xi的维度，已知，我们的框架也可以采用固定维度的矩阵计算。如果已知矩阵维度，可以使用编译时优化，执行矩阵乘法。

特别注意，等式25的舒尔降维方法进行矩阵乘法。图的松散结构只能乘以非零的矩阵，产生Hpp。另外，我们计算矩阵的分块结构，相比矩阵数乘，在缓存中执行矩阵乘法。

我们的框架对被嵌入的线性方案是不可知的，所以，我们可以用合适的方法解决不同的问题。实验中主要有两种方法。

五实验

本章，我们用真实数据和合成的数据集做实验来比较g2o和其他优化方法。图4是3D位姿图数据集，图5是捆集调整数据集和 Keble College位姿图，它采用7自由度相似约束执行对尺度变换敏感的SLAM，图6是2D数据集。变量的数据和约束如表1所示。所有的实验都在单核 Intel i7-930,2.8GHz处理器上执行。

我们将g2o和 ,SPA,sSBA,RobotVision进行对比。这些方法只针对一种优化问题的子集，而g2o针对所有问题，也很容易扩展到新的问题。

A. Runtime比较

我们对比每种方案迭代一次的时间。每种方案都用全优化问题测试，测试10次迭代，计算每次迭代的平均时间。这个实验中，g2o采用乔里斯基分解处理线性系统CHOLMOD，我们拿它来做实验对比。因此，处理线性系统的时间类似，它们之间的差别反映的是构建线性系统的效率。结果如图7所示。

我们的系统g2o在所有的2维和3维数据集上比速度快。

参考文献

[1] S. Agarwal, N. Snavely, S. M. Seitz, and R. Szeliski, “Bundle adjustment in the large,” in Proc. of the European Conf. on Computer Vision (ECCV), 2010.

[2] M. Byrod and K. Astroem, “Conjgate gradient bundle adjustment,” in Proc. of the European Conf. on Computer Vision (ECCV), 2010.

[3] Y. Chen, T. A. Davis, W. W. Hager, and S. Rajamanickam, “Algorithm 887: Cholmod, supernodal sparse cholesky factorization and update/downdate,” ACM Trans. Math. Softw., vol. 35, no. 3, pp. 1–14, 2008.

[4] T. A. Davis, Direct Methods for Sparse Linear Systems. SIAM, 2006, part of the SIAM Book Series on the Fundamentals of Algorithms.

[5] F. Dellaert and M. Kaess, “Square Root SAM: Simultaneous localization and mapping via square root information smoothing,” Int. Journal of Robotics Research, vol. 25, no. 12, pp. 1181–1204, Dec 2006.

[6] T. Duckett, S. Marsland, and J. Shapiro, “Fast, on-line learning of globally consistent maps,” Autonomous Robots, vol. 12, no. 3, pp. 287 – 300, 2002.

[7] U. Frese, “A proof for the approximate sparsity of SLAM information matrices,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2005.

[8] U. Frese, P. Larsson, and T. Duckett, “A multilevel relaxation algorithm for simultaneous localisation and mapping,” IEEE Transactions on Robotics, vol. 21, no. 2, pp. 1–12, 2005.

[9] G. Grisetti, R. K¨ummerle, C. Stachniss, U. Frese, and C. Hertzberg, “Hierarchical optimization on manifolds for online 2d and 3d mapping,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2010.

[10] G. Grisetti, C. Stachniss, and W. Burgard, “Non-linear constraint network optimization for efficient map learning,” IEEE Trans. on Intelligent Transportation Systems, 2009.

[11] G. Guennebaud, B. Jacob, et al., “Eigen v3,” http://eigen.tuxfamily.org, 2010.

[12] A. Howard, M. Matari´c, and G. Sukhatme, “Relaxation on a mesh: a formalism for generalized localization,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2001.

[13] Y. Jeong, D. Nister, D. Steedly, R. Szeliski, and I. Kweon, “Pushing the envelope of modern methods for bundle adjustment,” in Proc. of the IEEE Conf. on Comp. Vision and Pattern Recognition (CVPR), 2010.

[14] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smoothing and mapping,” IEEE Trans. on Robotics, vol. 24, no. 6, pp. 1365–1378, Dec 2008.

[15] K. Konolige, “Sparse sparse bundle adjustment,” in Proc. of the British Machine Vision Conference (BMVC), 2010.

[16] K. Konolige, G. Grisetti, R. K¨ummerle, W. Burgard, B. Limketkai, and R. Vincent, “Sparse pose adjustment for 2d mapping,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2010.

[17] K. Konolige, “Large-scale map-making,” in Proc. of the National Conference on Artificial Intelligence (AAAI), 2004.

[18] M. A. Lourakis and A. Argyros, “SBA: A Software Package for Generic Sparse Bundle Adjustment,” ACM Trans. Math. Software, vol. 36, no. 1, pp. 1–30, 2009.

[19] F. Lu and E. Milios, “Globally consistent range scan alignment for environment mapping,” Autonomous Robots, vol. 4, pp. 333–349, 1997.

[20] M. Montemerlo and S. Thrun, “Large-scale robotic 3-d mapping of urban structures,” in Proc. of the Int. Symposium on Experimental Robotics (ISER), 2004.

[21] K. Ni and F. Dellaert, “Multi-level submap based SLAM using nested dissection,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2010.

[22] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of pose graphs with poor initial estimates,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2006, pp. 2262–2269.

[23] W. Press, S. Teukolsky, W. Vetterling, and B. Flannery, Numerical Recipes, 2nd Edition. Cambridge Univ. Press, 1992.

[24] M. Smith, I. Baldwin, W. Churchill, R. Paul, and P. Newman, “The new college vision and laser data set,” Int. Journal of Robotics Research, vol. 28, no. 5, pp. 595–599, May 2009.

[25] R. Smith, M. Self, and P. Cheeseman, “Estimating uncertain spatial realtionships in robotics,” in Autonomous Robot Vehicles, I. Cox and G. Wilfong, Eds. Springer Verlag, 1990, pp. 167–193.

[26] H. Strasdat, J. M. M. Montiel, and A. Davison, “Scale drift-aware large scale monocular SLAM,” in Proc. of Robotics: Science and Systems (RSS), 2010.

[27] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzibbon, “Bundle adjustment - a modern synthesis,” in Vision Algorithms: Theory and Practice, ser. LNCS. Springer Verlag, 2000, pp. 298–375.