G2O使用要点总结

1.顶点定义：

1. 继承BaseVertex<定点byte数，顶点结构体>类
2. 实现setToRiginImpl()函数，里面给_estimate成员函数赋初值
3. _estimate的类型就是模板里指定的顶点结构体
4. 实现：oplusImpl(const double * update_)。update_是指向跟新值得指针，使用前需要转换成真实的结构类型。不一定是顶点类型。比如update_可以是李代数，但顶点类型是李群。
5. 可以使用esitimate()函数得到顶点的值，类型同上面的顶点结构体。

2.边的定义：

1. 继承BaseBinaryEdge<观察值byte数，观察值结构体，第一个顶点的类型，第二个顶点的类型>（两个顶点的边）
2. 或继承BaseUnaryEdge<观察值byte数，观察值结构体，顶点的类型>（一个顶点的边）
3. 实现函数computeError()，函数里面需要计算_error的值，其他顶点和测量值的成员变量参考其他例子。
4. 实现linearizeOplus()，单顶点的计算_jacobianOplusXi矩阵，双顶点的还要计算_jacobianOplusXj矩阵

3.SE3Quat

1. 使用map函数来变换一个3d点

4.SparseOptimizer

1. 这个应该是总的流程和数据管理器
2. setAlgorithm设置具体计算更新值得算法
3. addVertex加入顶点
4. addEdge()
5. addParameter()可以传入一些超参
6. initializeOptimization()给顶点填入初始值
7. optimize()启动优化流程
   1. 调用所有边的linearizeOplus()函数，得到每个边的雅克比矩阵，然后把矩阵连接成一个大矩阵
   2. 调用所有边的computeError()，得到误差矩阵
   3. 基于雅克比和误差，使用优化器计算更新值（矩阵求逆，主要时间花费在这里）
   4. 把拆分成各个顶点的值
   5. 调用顶点的oplusImpl函数，更新顶点的_estimate变量