g2o是一个通用的图优化框架。
本文结合论文g2o: A general framework fo graph optimizaiton说明g2o的理论,然后编写一个示例程序,说明g2o的使用方法。
g2o理论:
许多问题可以建模为最小二乘优化的状态估计问题。问题形式如图
F(x)为目标函数,目的就是求解状态x,使目标函数最小。x是一个向量,向量中的每个元素也可以为一个参数块。zij是联系状态变量i和状态变量j的观测,数学上表现为一个约束方程。由于有测量噪声,采用误差向量e()评价xi和xj满足方程的程度。
论文中为了简化符号,采用下图描述误差向量
这里需要注意的是,状态变量xi和xj(左侧),已经变为了整个系统的状态x(右侧)。
这个误差向量可以表示为一个有向图,可以知道方程结构,便于可视化分析
这个图中,边的个数就是所有的约束,也就是方程个数。一个节点为一个状态变量,所有节点的结合组成了整个系统的状态变量。不要忘记我们的目标就是为了状态估计。
有了上述目标函数,我们就可以进行最小二乘优化。
论文中给出的是高斯牛顿法。
高斯牛顿法的思想是:对误差函数e采用一阶泰勒展开。然后构建一个新的误差函数,求这个误差函数的极小值,以极小值作为新的迭代点。重复迭代,直到满足终止条件。