流程：
1.每次添加边的时候，会对jacobian_workspace更新size，最终size等于所有边中顶点相关顶点数量最多的顶点数；这样做的原因是所有边求解雅克比矩阵，用的是同一个jacobian_workspace,所以分配内存分配所有边中需要内存最大的情况。
2.初始化：会迭代每个顶点判断顶点对应边的level是否是优化指定level，如果满足则放入auxEdgeSet中，该顶点放入_activeVertices中，如果对应所有边都不满足要求，则不放入；由于有些边对应多个顶点，因此前面auxEdgeSet是一个set，自动去重，最终转为vector类型的_activeEdges。接着对所有_activeVertices建立hessian索引，即在该容器中的索引，放在对应顶点v的_hessianIndex中。
3.优化：主要是三步：1) 初始化，主要是清空容器_algorithm->init(online);2)迭代求解， result = _algorithm->solve(i, online);3)计算目标函数并输出命令行_algorithm->printVerbose(cerr);
4.迭代求解：主要是四步：1) 建立结构，主要映射hessian矩阵对角块到顶点去，以及映射hessian矩阵非对角块（块数=边的顶点数*（顶点-1）/2）到边里去，bool ok = _solver.buildStructure();
2)对每条边求解相对各顶点的雅各比矩阵，并根据雅各比矩阵计算顶点涉及的hessian矩阵对角块，边涉及的hessian矩阵非对角块，_solver.buildSystem();3)调用线性求解器，求解Hpp*x = _b，bool ok2 = _solver.solve();4）求解得到的增量加到顶点去，_optimizer->update(_solver.x());
5.注意g2o里se3用的不是李群李代数，而是压缩四元数，残差是通过逆旋转矩阵乘以旋转矩阵，再把旋转矩阵差值转为四元数算得，因此残差和更新量x均是四元数，雅各比矩阵也是残差相对于四元数的求导
