1. Introducción general
g2o (General Graphic Optimization) es una biblioteca general de algoritmos de optimización de gráficos . La investigación actual de SLAM se basa básicamente en la optimización. Puede consultar el artículo original aquí .
Los puntos en la optimización de gráficos son poses de cámara y los bordes son relaciones de transformación entre poses , que generalmente representan términos de error.
diagrama de clases de g2o
Del análisis de todo el diagrama de estructura, el optimizador disperso SparseOptimizer es el núcleo de todo el g2o.
2. Pasos de construcción
Para usar g2o para resolver problemas de optimización, los pasos para construir el marco son los siguientes:
(1) Cree un solucionador lineal LinearSolver.
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
Hay principalmente cinco métodos de resolución proporcionados por g2o , y hay dos
heredados de LinearSolverCCS : ① LinearSolverCholmod , que usa el método de descomposición cholesky disperso, heredado de LinearSolverCCS; ② LinearSolverCSparse , que usa el método CSparse, heredado de LinearSolverCCS; hay tres heredados de LinearSolver : ① LinearSolverPCG , utilizando el método de gradiente conjugado precondicionado, heredado de LinearSolver. ② LinearSolverDense , utilizando el método de descomposición cholesky denso, heredado de LinearSolver. ③ LinearSolverEigen , solo depende de eigen, utiliza Cholesky disperso en eigen para resolver, por lo que puede usarse fácilmente en otros lugares después de la compilación, y su rendimiento es similar a CSparse, heredado de LinearSolver.
(2) Cree un solucionador de bloques BlockSolver e inicialícelo con el solucionador lineal definido anteriormente
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
Block* solver_ptr = new Block( linearSolver );
La definición de solucionador tiene dos tipos: variable fija y tamaño variable , y las definiciones son las siguientes:
// 固定维度，p为pose的维度，l表示landmark的维度
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
// 可边尺寸
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
Además, g2o también predefine los siguientes tres tipos de uso común: ① BlockSolver_6_3 , pose de 6 dimensiones + punto de referencia tridimensional, comúnmente utilizado en el ajuste de paquete. ② BlockSolver_7_3 tiene una dimensión de escala adicional basada en BlockSolver_6_3. ③ BlockSolver_3_2 , pose 3D + 2D Lankmark, a menudo utilizado en el mundo plano.
(3) Cree un solucionador de solucionador total y seleccione uno de GN, LM, Dogleg e inicialícelo con el solucionador de bloques BlockSolver mencionado anteriormente.
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
Hay tres estrategias de iteración para elegir: g2o:: OptimizationAlgorithmGaussNewton , g2o:: OptimizationAlgorithmLevenberg , g2o:: OptimizationAlgorithmDogleg .
(4) Crear el núcleo de la optimización de gráficos: Sparse Optimizer .
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm( solver ); // 设置求解方法
optimizer.setVerbose( true ); // 设置优化过程输出信息
(5) Defina los vértices y los bordes del gráfico y agréguelos a SparseOptimizer
CurveFittingVertex* v = new CurveFittingVertex();
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 );
}
(6) Establecer parámetros de optimización e iniciar la optimización
optimizer.initializeOptimization();
optimizer.optimize(100); //设置迭代次数
3. Vértices y aristas del gráfico
3.1 Vértices
El vértice definido en g2o tiene una plantilla de clase general: BaseVertex, que tiene dos parámetros D/T , D es un tipo int, que indica la dimensión mínima del vértice, T es el tipo de datos del vértice a estimar y es el estado en su espacio múltiple La representación más pequeña en . (en duda)
Algunos tipos de vértices de uso común :
ertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //Isometry3使欧式变换矩阵T，实质是4*4矩阵//6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>VertexPointXYZ : public BaseVertex<3, Vector3>VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential mapVertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotationVertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.VertexSim3Expmap : public BaseVertex<7, Sim3>
Al definir los vértices por su cuenta, debe reescribir las siguientes funciones:
class myVertex: public g2o::BaseVertex<Dim, Type>
{
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myVertex(){
}
// 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual void read(std::istream& is) {
}
virtual void write(std::ostream& os) const {
}
// 顶点重置函数，设定被优化变量的原始值。
virtual void setOriginImpl(){
_estimate = Type(); }
// 顶点更新函数，主要用于优化过程中增量△x 的计算，根据增量方程计算出增量后，通过这个函数对估计值进行调整
virtual void oplusImpl(const double* update) override{
_estimate += update; }
}
Agregue vértices al gráfico:
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) )；// 设定初始值v->setId(0); // 定义节点编号
optimizer.addVertex( v ); // 把节点添加到图中
3.2 Bordes
Las aristas definidas en g2o tienen arista de un elemento, arista de dos elementos y arista de varios elementos.Tomando la arista de dos elementos como ejemplo, sus parámetros son: D, E, VertexXi, VertexXj . D es un tipo int, que indica la dimensión del valor de medición; E indica el tipo de datos del valor de medición; VertexXi, VertexXj respectivamente indican los tipos de diferentes vértices. De esto podemos obtener la definición de un borde binario de la siguiente manera:
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
Al definir los bordes usted mismo, debe reescribir las siguientes funciones:
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){
}
virtual bool read(istream& in) {
}
virtual bool write(ostream& out) const {
}
// 是使用当前顶点值计算的测量值与真实测量值之间的误差
virtual void computeError() override {
// ...
_error = _measurement - Something;
}
// 是在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian矩阵
virtual void linearizeOplus() override // 求误差对优化变量的偏导数，雅克比矩阵
{
_jacobianOplusXi(pos, pos) = something; // ... /* _
jocobianOplusXj(pos, pos) = something; ... */
}
private:
data
}
Blog de referencia: