G2O使用笔记(面向VSLAM场景)(学习笔记+有待完善+可能有错)
G2O使用笔记<面向VSLAM场景> <学习笔记+有待完善+可能有错>
- 类型定义】通过类型模板BlockSolverTraits定义类型g2o::BlockSolverTraits<节点1的维度，误差/节点2的维度>，在此称之为Traits_a_b
- 类型定义】借助上一步定义的类型，通过类型模板BlockSolver定义类型BlockSolver< Traits_a_b >,在此称之为blocksolver_a_b
- 对象定义】定义指向blocksolver_a_b::LinearSolverType类型的实例 指针 ，称之为 linearsolver
- 对象定义】定义指向blocksolver_a_b类型的指针对象，称之为blocksolver_a_b_ptr
- 对象定义】 定义指向优化算法类型的指针对象opti_algorithm_ptr
- 对象定义】定义稀疏优化器SparseOptimizer的对象(不是指针对象)，称之为optimizer
- 创建顶点
- 创建相机位姿顶点
- 创建特征点(3d)顶点
- 如果有需要可以【自定义顶点】
- 创建相机模型
- 创建边
- 同时优化相机位姿和空间点的边
- 如果想仅优化相机位姿，而不优化空间点可以【自定义相应的边】
- 启动优化
- 结果
- 其它_1:关于节点成员函数setToOriginImpl()，oplusImpl()，setEstimate()的解析
- 其它_2:节点继承关系
- 其它_3:边的继承关系
动态内存问题还未优化，近期更新
类型定义】通过类型模板BlockSolverTraits定义类型g2o::BlockSolverTraits<节点1的维度，误差/节点2的维度>，在此称之为Traits_a_b
#include Traits_a_b中定义了 类型PoseMatrixType，LinearSolverType 等几个类型。
其中LinearSolverType是通过类型模板LinearSolver< PoseMatrixType >定义的一个类型，LinearSolver是linear_solver.h中定义的一个类
类型定义】借助上一步定义的类型，通过类型模板BlockSolver定义类型BlockSolver< Traits_a_b >,在此称之为blocksolver_a_b
#include blocksolver_a_b中 重新定义了 类型PoseMatrixType，LinearSolverType等在Traits_a_b已经定义的几个类型(实际上是将其引入了自己的作用域)。
blocksolver_a_b中定义了SparseBlockMatrix< PoseMatrixType >类型的几个成员，其中类模板SparseBlockMatrix定义在sparse_block_matrix.h中。
至此定义了类型(仅定义类类型且未完善，更未实例化)：
对象定义】定义指向blocksolver_a_b::LinearSolverType类型的实例 指针 ，称之为 linearsolver
#include 使其指向由LinearSolver派生出的类型LinearSolverDense<>/LinearSolverPCG<>/LinearSolverCSparse<>/LinearSolverCholmod<>中的某个类型的对象，传入类型的参数为blocksolver_a_b::PoseMatrixType
至此完成了：
对象定义】定义指向blocksolver_a_b类型的指针对象，称之为blocksolver_a_b_ptr
std::unique_ptr<blocksolver_a_b> blocksolver_a_b_ptr (new blocksolver_a_b( move(linearsolver) ));
blocksolver_a_b类型已经通过上述步骤完善，此步骤调用其构造函数
BlockSolver(LinearSolverType* linearSolver)定义指向该类型的指针，传入的参数是上一步中定义的指针 linearsolver
至此完成了：
也可以说是完成了：
对象定义】 定义指向优化算法类型的指针对象opti_algorithm_ptr
可选择三种优化算法：Gauss-Newton/Levenberg-Marquardt/Powell’dogleg 之一，其构造函数均需传入类型模板Solver定义的类型的指针作为参数。而BlockSolver类型是Solver的派生的类型模板，故上一步中定义的指针blocksolver_a_b_ptr可以传入。
#include //levenberg优化算法定义在此头文件中
g2o::OptimizationAlgorithmLevenberg* opti_algorithm_ptr( new g2o::OptimizationAlgorithmLevenberg( move( blocksolver_a_b_ptr) ) );
#include //GaussNewton优化算法定义在此头文件中
g2o::OptimizationAlgorithmGaussNewton* opti_algorithem_ptr( new g2o::OptimizationAlgorithmGaussNewton( move( blocksolver_3_2_ptr) ) );
#include //Dogleg优化算法定义在此头文件中
g2o::OptimizationAlgorithmDogleg* opti_algorithem_ptr( new g2o::OptimizationAlgorithmDogleg( move( blocksolver_3_2_ptr) ) );
至此完成了：
对象定义】定义稀疏优化器SparseOptimizer的对象(不是指针对象)，称之为optimizer
g2o::SparseOptimizer optimizer;
并将上一步定义的优化算法指针opti_algorithm_ptr 传给其成员函数setAlgorithm()
optimizer.setAlgorithm(opti_algrithm_ptr)；
至此完成了：
创建顶点
创建相机位姿顶点
常采用的节点类:VertexSE3Expmap.
VertexSE3Expmap类是以四元数和平移向量表达位姿的节点，其定义为：class VertexSE3Expmap : public BaseVertex<6, SE3Quat>。
其中SE3Quat是g2o自定义的表达位姿（变换）数据类型，包含该两个成员变量：Eigen::Quaterniond _r和Eigen::Vector3d _t。
#include 添加节点
optimizer.addVertex(pose);
创建特征点(3d)顶点
常采用的节点类型：g2o::VertexSBAPointXYZ;
//空间点一般是多个，采用for循环添加
for(int i = 0; i != 3d点数量； ++i ){
g2o::VertexSBAPoint* point = new g2o::VertexSBAPointXYZ;
point->setId(i+1)//因为已经添加了一个节点pose，故Id从1开始
point->setEstimate( 类型为Vector3d的空间点坐标 ）；
point->setMarginalised(true);//or false
optimizer.addVertex(point);
}
如果有需要可以【自定义顶点】
本部分内容来源于高翔博士《视觉SLAM十四讲》
自定义顶点要完成一下几项工作：
1.确定要继承的父类。一般是BaseVertex类
2.确定优化变量的维度。
3.确定优化变量的数据类型。
4.重写setToOriginImpl()函数。//该函数作用是更新顶点的估计值，保存到_estimate。
5.重写oplusImpl()函数。//该函数作用是重置顶点的估计值，保存到_estimate。
6.重写read和write函数。
自定义顶点的格式如下：
class MyVertex: public g2o::BaseVertex<优化变量的维度, 优化变量的类型>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//顶点估计值重置函数
virtual void setToOriginImpl()；
//顶点估计值更新函数
virtual void oplusImpl( const double* update )
// 存盘和读盘：可以留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
创建相机模型
CameraParameters定义在头文件types_six_dof_expmap.h中。
g2o::CameraParameters* camera = new g2o::CameraParameters(double focal_length, const Vector2D & principle_point, double baseline );
camera->setId ( 0 );//这里设置的id是该参数（相机）在optimizer._parameters中的索引值
optimizer.addParameter ( camera )//将该参数（相机)保存在optimizer的成员_parameters(类型是ParameterContainer)中
创建边
同时优化相机位姿和空间点的边
常采用的边类型g2o::EdgeProjectXYZ2UV;
//边也有多条，借助for循环进行添加
for(int i = 0; i != 边的数量； ++i ){
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV;
edge->setId(i);
edge->setVertex(0,dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(i+1)));
//其中的optimizer.vertex()是定义在optimizable_graph.h中的类型OptimizableGraph中的成员函数，
//它按照参数将添加到优化器(此处的optimizer)中的相应顶点(Id=参数)转化为Vertex*类型后返回，
//因此此处需要使用dynamic_cast<>()将其类型复原。
//该成员函数的声明与定义如下:
//inline Vertex* vertex(int id) { return reinterpret_cast(HyperGraph::vertex(id));}
edge->setVertex(1,pose);
edge->setMeasurement( 类型为Vector2d的像素坐标)；
edge->setParameterId(0,0);
//Edge中有成员变量std::vector _parameterIds，保存该边所用
//到的parameter在optimizer中添加的参数中索引值，即添加Parameter
//时用setId()添加的参数Id。edge->setParameterId(a,b)，a是Edge
//中_parameterIds的元素索引，b是optimizer中Parameter的Id。
edge->Information(Eigen::Matrix2d::Identity());
optimizer.addEdge(edge);
}
如果想仅优化相机位姿，而不优化空间点可以【自定义相应的边】
本部分内容来源于高翔博士《视觉SLAM十四讲》
得益于C++的继承机制，我们直接从g2o的已有边类类型中衍生出我们需要的边
自定义g2o的边，要进行以下几项工作：
1.确定继承的父类，你肯定不想从头写一个类类型吧
2. 确定观测值的维度
3. 确定观测值的类型，同时也是边的成员_error的数据类型
4. 确定连接节点的类型
5. 重写compute()函数。该函数作用是取出边所连接节点的估计值，根据模型计算观测值的比较值，并与观测值进行比较将结果保存到Edge的成员_error(_error=观测值向量-计算出的观测值的比较值向量)
6. 重写linearizeOplus()函数。该函数计算当前_error关于节点每个维度的的Jacobian矩阵_jacobianOplusXi(这是父类中预定义的成员)。该函数好像也可以省略，g2o会自动求导，但能够写出的话，建议提供。
8. 重写read和l write}函数
9. 添加一些需要用到的成员变量、成员函数
格式如下(一元边)
class MyEdge: public g2o::BaseUnaryEdge<观测值的维度, 观测值的类型(也是_error的类型)，连接节点的类型>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//误差计算函数
virtual void computeError();
//Jacobain矩阵计算函数
virtual void linearizeOplus();
//读取与写入函数
virtual bool read( std::istream& in ){}
virtual bool write(std::ostream& os) const {};
};
对于本例：
1.仅优化当前位姿，故只有一个节点，以BaseUnaryEdge为父类
2.观测值为像素坐标，维度为2
3.像素坐标采用Eigen::Vector2d类型
4.节点类型：g2o::VertexSE3Expmap
7.我们不进行读写操作，read和write函数留空
8.参加数据成员：Vector3d 空间点坐标、Camera* 相机模型方便计算。
class Edge_PoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//误差计算函数
virtual void computeError();
//Jacobain矩阵计算函数
virtual void linearizeOplus();
//读取与写入函数
virtual bool read( std::istream& in ){}
virtual bool write(std::ostream& os) const {};
public:
Eigen::Vector3d point_;
myslam::Camera::Ptr camera_;//Camera类是自定义的，这是个一般的过程，不再赘述
};
void Edge_PoseOnly::computeError()
{
const g2o::VertexSE3Expmap* v = static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
_error = _measurement - camera_->camera2pixel( v->estimate().map(point_));
}
void Edge_PoseOnly::linearizeOplus()
{
const g2o::VertexSE3Expmap* v = static_cast<const g2o::VertexSE3Expmap*>(_vertices[0] );
Eigen::Vector3d xyz = v->estimate().map( point_);
double x = xyz[0];
double y = xyz[1];
double z = xyz[2];
double z_2 = z*z;
_jacobianOplusXi ( 0,0 ) = x*y/z_2 *camera_->fx_;
_jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
_jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;
_jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;
_jacobianOplusXi ( 0,4 ) = 0;
_jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;
_jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
_jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;
_jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;
_jacobianOplusXi ( 1,3 ) = 0;
_jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;
_jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_;
}
启动优化
optimizer.setVerbose ( true );//输出信息
optimizer.initializeOptimization();//启动
optimizer.optimize ( 100 );//迭代次数100
结果
Eigen::Isometry3d ( pose->estimate() ).matrix()
其它_1:关于节点成员函数setToOriginImpl()，oplusImpl()，setEstimate()的解析
OptimizableGraph:: G2O_CORE_API Vertex : public HyperGraph::Vertex, public HyperGraph::DataContainer
void setToOrigin() { setToOriginImpl(); updateCache();}
virtual void oplusImpl(const double* v) = 0;//纯虚函数
virtual void setToOriginImpl() = 0;//纯虚函数
class BaseVertex : public OptimizableGraph::Vertex
void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}//不是虚函数故子类直接使用，无法重写，用于设定_estimate的值
BaseVertex没有声明定义父类oplusImpl()，setToOriginImpl两个纯虚函数
BaseVertex没有重写父类setToOrigin()函数
BaseVertex用作模板，不可直接使用
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>
virtual void setToOriginImpl() {_estimate = SE3Quat();}
//重写了从祖父继承来的setToOriginImpl()，用于初始化_estimate为0
virtual void oplusImpl(const double* update_) {
Eigen::Map update(update_);
setEstimate(SE3Quat::exp(update)*estimate());
}
//重写了从祖父继承来的soplusImp()，用于更新_estimate，其调用从父类继承来setEstimate()向_estimate写入值
其它_2:节点继承关系
{(G2O_CORE_API HyperGraphElement -> G2O_CORE_API HyperGraph::Vertex )+ G2O_CORE_API HyperGraph::DataContainer} -> G2O_CORE_API OptimizableGraph::Vertex -> BaseVertex ->{ G2O_TYPES_SBA_API VertexSE3Expmap / G2O_TYPES_SBA_API VertexSBAPointXYZ}
其它_3:边的继承关系
G2O_CORE_API HyperGraphElement -> G2O_CORE_API HyperGraph::Edge -> G2O_CORE_API optimizeble_graph BaseEdge -> BaseBinaryEdge -> G2O_TYPES_SBA_API EdgeProjectXYZ2UV