g2o学习

g2o学习

g2o是slam中常用的一种优化库，在视觉slam中主要用来优化位姿与3D点的。它主要分为以下几个步骤：
1.定义一个求解器
2.定义优化顶点
3.定义优化边

它的基本框架基本在下面的图中：

注意箭头的方向以及箭头的含义。

构建求解器

在求解器中最终的目标是构建一个SparseOptimizer稀疏优化器。

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>> Block;

//1.构建线性求解器

Block::LinearSolver *linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

//2.构建BlockSolver
Block *slover_ptr=new Block(linearSolver);

//3.构建solver，这边选择了G-N法，当然也能选择其他的如，L-M,DogLeg
g2o::OptimizationAlgorithmGaussNewTown *solver = new g2o::OptimizationAlgorithmGaussNewTown(solver_ptr);

//构建稀疏优化器
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);

g2o的使用：
1.初始化LinearSolver、BlockSolver、选择优化算法（L-M、G-N、DogLeg）、定义图模型、设置调节器
2.顶点(定义并加入图模型)、边（定义其连接的顶点、加入图模型）
3.开始优化
4.读取结果

构建顶点

g2o中有个适应大部分情况的模板BaseVertex<D,T>
这里的D表示的是优化顶点在流行空间中最小的维度
T表示顶点的类型。

g2o内部定义好的顶点有
结点1：相机位姿结点：g2o::VertexSE3Expmap，来自<g2o/types/sba/types_six_dof_expmap.h>；
结点2：特征点空间坐标结点：g2o::VertexSBAPointXYZ，来自<g2o/types/sba/types_sba.h>；
边：重投影误差：g2o::EdgeProjectXYZ2UV，来自<g2o/types/sba/types_six_dof_expmap.h>

当然，也可以自定义顶点，需要重写的函数如下

virtual bool read(std::istream& is);  
virtual bool write(std::ostream& os) const;  
virtual void oplusImpl(const number_t* update);//这个就是x+delta x，也就是用来更新
virtual void setToOriginImpl(); //这是个设置的初值

如何添加顶点：

CurveFittingVertex* v=new CureFittingVertex();
v->setEstimate(Eigen::Vector3d(0,0,0));//设置初值
v->setId(0);
optimizer.addVertex(v);//添加顶点

构建边

例如构建一个二元边BaseBinaryEdge<2,Vector2D,VertexSBAPointXYZ，VertexSE3Expmap>
2表示观测值是2维的，类型是Vector2D，边连接顶点的类型是三维点VertexSBAPointXYZ与相机位姿VertexSE3Expmap

对于边的自定义需要重写下面几个函数

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();//计算误差
virtual void linearizeOplus();//对该误差对优化变量的偏导数，即Jacobian

除此之外还有几个重要函数与成员变量

_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆

参考连接：
1.http://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486858&idx=1&sn=ce458d5eb6b1ad11b065d71899e31a04&chksm=97d7e81da0a0610b1e3e12415b6de1501329920c3074ab5b48e759edbb33d264a73f1a9f9faf&scene=21#wechat_redirect
2.http://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486992&idx=1&sn=ecb7c3ef9bd968e51914c2f5b767428d&chksm=97d7eb87a0a062912a9db9fb16a08129f373791fd3918952342d5db46c0bc4880326a7933671&scene=21#wechat_redirect
3.http://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247487082&idx=1&sn=d4a27e4c9a76760fffb571f57f4f7719&chksm=97d7ebfda0a062eba412877e9ecf5933f2051f0210c0d56f03267985512d97f2db434ab7356c&scene=21#wechat_redirect