g2o简单测试

开始研究g2o,记录一些过程。
首先使用create_sphere可以生成一个数据文件。然后使用g2o_viewer就可以载入这个文件。
载入以后可以进行优化，优化后是一个球体。
这里g2o_viewer不是万能的可视化窗口，有许多已存在在g2o工程里的数据类型是不知显示的，比如

VertexSE3Expmap

因此如果希望使用g2o_viewer*来方便调试，有两个办法，第一个办法就是老老实实的使用一些已有的数据类型。比如在types/slam3d下面的那些数据类型，另外一种就是要自己去定义opengl的绘制方式，这个在说明的pdf里有讲到，不过我猜哪怕完成这一步，你也要重新编译整个g2o才行，至少g2o_viewer需要知道你自己新定义的数据该如如何画。这是我个人的见解，仅供参考。

第一次用这个可视化界面将那个球体优化出来时是很惊讶的，但怎么也不能和slam联系到一起。当看了create_sphere的源代码以后才理解，原来是创建了一个在球体上运动的机器人pose，然后均匀的撒了landmark在球体上，再加上里程计，最后才组成了这个球体。总之这些待优化的节点都在球体上，因此最后优化的结果就是一个球体。。。当然闲的无聊也可以在一个立方体上试试。

万事开头难，下面记录一个很简单的优化。
机器人在直线上行走，里程计组成了pose，当走到最后一格后，在添加一个首位相连的里程边（其实应该叫观测量），然后就可以开始优化了，我这儿一点噪声都没加，也没有设置哪个点是固定的，代码看懂了可以自己改，主要是为了能在可视化界面下学习。

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/Geometry>

#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/stuff/sampler.h"
#include "g2o/core/factory.h"

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/optimization_algorithm_levenberg.h"


#include "g2o/types/sba/typ