这是参考自清华大学的高翔所著的《视觉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是相机位姿，它本身不一定有加法运算。这时，就需要重新定义增量如何加到现有的估计上的行为。
• 顶点的重置函数:setToOriginImpl。这是平凡的，把估计值置0.
• 边的误差函数计算:computerError。该函数需要去除边所连接的顶点的当前估计置，根据曲线模型，与它的观测值比较。这和最小二乘问题中的误差模型是一致的。
• 存盘和读数函数:read和write。由于并不像进行读/写操作，所以留空。

---

在定义了顶点和边之后，在main函数里声明了一个图模型，然后按照生成的噪声数据，往图模型中添加顶点和边，最后调用优化函数进行优化。g2o会给出优化的结果。

Code:

---

- Red
+ Green
* Blue