参考：https://www.jianshu.com/p/e16ffb5b265d
参考：https://zhuanlan.zhihu.com/p/36889150
图是一种数据结构。在图优化中，用顶点(vertex) 表示优化变量，用边(edge) 表示误差项。优化时，顶点的值逐渐趋近最优值，边的作用是计算误差
下面是一张关于g2o的整体结构图：
模板参数为优化变量维度和数据类型；需要定义读写函数。
void oplus(double* v)则是节点的更新函数，值存储在变量_estimate(类型为给定的数据类型) 中；
void setToOriginImpl()定义节点的重置值；
定义节点时，利用setEstimate(type)函数来设定初始值；
setId(int)定义节点编号。
• 模版参数为观测值的维度，类型和连接的节点类型；需要重定义读写函数；可以有初始化函数；
• 函数void linearizeOplus() 计算雅可比矩阵；
• void computeError() 函数计算误差，观测值存储在_measurement 中，误差存储在_error 中，节点存储在_vertices[] 中。
• 定义边时，利用setId(int) 来定义边的编号（决定了在H矩阵中的位置）；
• setMeasurement(type) 函数来定义观测值；
• setVertex(int, vertex) 来定义节点；其中int指的是第几类参数，如边的定义如下：
EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>则第0类参数为空间点（VertexSBAPointXYZ,），第1类参数为位姿（VertexSE3Expmap>）
• setInformation() 来定义协方差矩阵的逆。
• BaseBinaryEdge：二元边需要多设定一个节点类型，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 与设定的int 有关（0 或1）。
块求解器，会构建好线性求解器所需要的矩阵块，其中包含了线性求解器。
1、选择一个线性方程求解器；
2、选择一个BlockSolver；
3、选择一个迭代策略，一般为GN或LM；
（1）定义顶点及边的模板
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>//继承自g2o的顶点构造函数
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update); //cc此处应该是为了说明优化变量的更新方式
}
// 存盘和读盘：留空
virtual bool read( istream& in ) {} //虚函数，在继承类中再定义
virtual bool write( ostream& out ) const {}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex> //cc继承自g2o的边构造函数
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};
关于其中的 static_cast关键字
static_cast <type-id> (expression)static_cast叫做编译时类型检查。该运算符把expression转换为type-id类型，这样做的目的是在程序还没有运行时进行类型检查，来保证转换的安全性。
而与之对应的，dynamic_cast关键字（运行时类型检查）主要用于类层次结构中父类和子类之间指针和引用的转换，由于具有运行时类型检查，因此可以保证下行转换的安全性。即转换成功就返回转换后的正确类型指针，如果转换失败，则返回NULL。
所谓上行转换是指由子类指针到父类指针的转换，下行转换是指由父类指针到子类指针的转换。
（2）调用
// 构建图优化，先设定g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 1、选择线性方程求解器
Block* solver_ptr = new Block( linearSolver ); //2、选择BlockSolver
//3、选择迭代策略： 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// 往图中增加边
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
// 执行优化
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
g2o::SparseOptimizer optimizer;//图模型
optimizer.setAlgorithm ( solver );//设置求解器
// vertex
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );//初始值
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (
R_mat,
Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
) );//经过R,t作用的估计值
optimizer.addVertex ( pose );
int index = 1;
for ( const Point3f p:points_3d ) // landmarks 空间点位置
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true ); // g2o 中必须设置 marg
optimizer.addVertex ( point );
}
// parameter: camera intrinsics //相机内参
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
camera->setId ( 0 );
optimizer.addParameter ( camera );
// edges
index = 1;
for ( const Point2f p:points_2d ) //测量得到的关键点坐标
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
optimizer.optimize ( 100 );
cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;