这是参考自清华大学的高翔所著的《视觉SLAM十四讲》读后感,笔记和摘要。
使用G2O拟合曲线
首先,将曲线拟合问题抽象成图优化。在该问题中,只要记住节点为优化变量,边为误差项即可。
- 在曲线拟合问题中,整个问题只有一个定点:曲线模型的参数a,b,c;而各个噪声的数据点,构成了一个个误差项,也就是图优化的边。我们把边称为Hyper Edge,整个图叫做Hyper Graph 。
优化步骤
弄清楚这个模型后,接下来就是在G2O中建立该模型,进行优化了
- 定义顶点和边的类型
- 构建图
- 选择优化算法
- 调用g2o进行优化,返回结果
下面演示一下程序
std::cout<<"Hello World"<<std::endl;
在这个程序中,从g2o派生出了用于曲线拟合的图优化顶点和边:CurveFittingVertex和CurveFittingEdge,这实质上扩展了g2o的使用方式。在这两个派生类中,重写了重要的虚函数:
- 顶点的更新函数:oplusImpl。我们知道优化过程中最重要的是增量 Δx 的计算,而该函数处理的是 xk+1=xk+Δx 的过程。在曲线拟合的过程中,由于优化变量本身位于向量空间中,这个更新计算确实就是简单的加法。但是,当优化变量不在向量空间中,比如说 x <script type="math/tex" id="MathJax-Element-3">x</script>是相机位姿,它本身不一定有加法运算。这时,就需要重新定义增量如何加到现有的估计上的行为。
- 顶点的重置函数:setToOriginImpl。这是平凡的,把估计值置0.
- 边的误差函数计算:computerError。该函数需要去除边所连接的顶点的当前估计置,根据曲线模型,与它的观测值比较。这和最小二乘问题中的误差模型是一致的。
- 存盘和读数函数:read和write。由于并不像进行读/写操作,所以留空。
在定义了顶点和边之后,在main函数里声明了一个图模型,然后按照生成的噪声数据,往图模型中添加顶点和边,最后调用优化函数进行优化。g2o会给出优化的结果。
Code:
- Red
+ Green
* Blue