g2o 学习笔记 一
g2o
g2o 是ORB_SLAM 用到的后端优化库，这里我尝试将自己的学习过程记录下来。
以便自己日后复习，查漏补缺。如果有不正确的地方，请各位指正。
图优化
这个还好，我研究生阶段做的有点相关，例如社交网络的两个人，他们存在好友关系，那么两个人就是顶点，好友关系这个约束就是边。
我以前做的隐特征分析模型，用的是张量和矩阵对图网络进行建模，然后用的全梯度下降和其他东西做分解的。
而g2o也是要建模为矩阵，然后进行解算。
学习路上，多谢各位大佬的分享，以下是我看的一些资料：
g2o开源库 : https://github.com/RainerKuemmerle/g2o
g2o论文 : http://ais.informatik.uni-freiburg.de/publications/papers/kuemmerle11icra.pdf
深入理解图优化与g2o：g2o篇 : https://www.cnblogs.com/gaoxiang12/p/5304272.html
非线性优化库g2o使用教程 : https://blog.csdn.net/u011341856/article/details/112134763
g2o库简单入门 : https://blog.csdn.net/qq_34761779/article/details/127183682
非线性优化库之g2o : https://zhuanlan.zhihu.com/p/384569456
视觉惯性SLAM：理论与源码解析 程小六
g2o 库的文件树
- EXTERNAL ~~~~ 第三方库，最新的版本里面只有一个freeglut和CMakeLists.txt文件
- apps ~~~~~~~~~~~~~~ 这个是使用g2o的主要应用，包裹了命令行界面和GUI应用
- autodiff ~~~~~~~~~~~ 看名字应该是自动求导
- core ~~~~~~~~~~~~~~~ g2o的核心库，顶点、边和图结构的定义，算法的定义，求解器的定义都在这里
- examples ~~~~~~~~ 一些应用的例子，会构建出一些例子的可执行文件。
- solvers ~~~~~~~~~~~ 线性求解器
- stuff ~~~~~~~~~~~~~~~~ 一些工具，我们只是用g2o的话，好像用不上
- types ~~~~~~~~~~~~~~ 定义好的类型，一般用就行了，没有想我们需要的类型，需要自己定义
- what_is_in_these_directories.txt 该目录下的文件夹的解释，最好直接看这个，我的理解不一定对
g2o 框架
g2o最基本的类结构如下图：
emmmmmmmmm，好复杂，慢慢看吧。
图上所有箭头都是从SparseOptimizer上射出，所以这个就是我们的最终目的就是维护这个稀疏优化器。
SparseOptimizer是一个OptimizableGraph，OptimizableGraph是一个HyperGraph。然后HyperGraph有各种vertex和edge。
SparseOptimizer有一个OptimizationAlgorithm(优化算法)。而优化算法是OptimizationWithHessian(二阶算法)的一种，然后二阶算法包含了高斯牛顿法、LM法和狗腿法，同时二阶算法有一个Solver，Solver是一个BlockSolver，其包含了SparseBlockMatrix和LinearSolver。
也就是说，使用g2o其实可以按上图分为上下两个部分，按照例子examples/data_fitting/curve_fit.cpp，我们梳理一下，g2o的使用顺序可以写成下面这样。（自己看链接里的源码更好）
// 定义
typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver;
typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;// step 1 创建线性求解器
MyLinearSolver* linearSolver = new MyLinearSolver();// step 2 创建块求解器，并用上面定义的线性求解器初始化
MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);// step 3 创建总求解器(Solver)，GN、LM、DogLeg 中选择一个，再用上述块求解器初始化
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// step 4 创建稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer; // 创建优化器
optimizer.setAlgorithm( solver ); // 用前面定义好的求解器作为求解方法
optimizer.setVerbose( true ); // 在优化过程中输出调试信息// step 5 定义图的顶点，并添加到优化器中
CurveFittingVertex *v = new CurveFittingVertex(); // 向图中增加顶点
v->setId(0); // 设置顶点的ID
v->setEstimate ( Eigen::Vector3d(1, 1 , 1) ); // 设置顶点的观测值
optimizer.addVertex(v); // 往优化器里添加顶点// step 6 定义图的边，并添加到优化器中
for (int i i=0; i<N; i++)
{CurveFittingEdge *edge = New CureveFittingEdge( x_data[i] );edge->setId(i); // 设置边IDedge->setVertex(0, v); // 设置边连接的顶点edge->setMeasurement(y_data[i]); // 设置边的观测值edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity() * 1) / (w_sigma * w_sigma); // 设置信息矩阵optimizer.addEdge(edge); // 往优化器中添加边
}// step 7 设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100); // 最多迭代100次线性求解器
g2o中主要有这几种线性求解器
g2o::LinearSolverCholmod(); // 使用 sparse cholesky 分解法
g2o::LinearSolverCSparse(); // 使用 CSparse 法
g2o::LinearSolverPCG(); // 使用 preconditioned conjugate gradient 法
g2o::LinearSolverDense(); // 使用 dense cholesky 分解法
g2o::LinearSolverEigen(); // 使用 Eigen 中的 sparse Cholesky 分解法，依赖项只有 eigen
块求解器
创建块求解器需要用到定义好的线性求解器进行初始化。
块求解器有两种定义方式，一种是固定变量的求解器，定义如下:
using BlockSolverPL = BlockSolver<BlockSolverTraits<p, q>
其中，p和q表示边链接的两个顶点的维度。
此外，g2o还能用Eigen的动态矩阵来实现动态变量求解器，定义如下：
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>
block_solver.h里有一些预定义的变量
BlockSolver_6_3; // 用于3d SLAM, 位姿是6维，地图点是3维
总求解器
总求解器一共有三个优化方法: Gauss Newton 、Levenberg-Marquardt 和 Dog-Leg。这三个都继承了OptimizationWithHessian，其又继承了OptimizationAlgotithm。
g2o::OptimizationAlgorithmGaussNewton;
g2o::OptimizationAlgorithmLevenberg;
g2o::OptimizationAlgorithmDogleg;
稀疏优化器
用定义好的总求解器，创建一个稀疏优化器，然后设置一些稀疏优化器的参数。
顶点
文件在：core/base_vertex.h
g2o提供了一个比较通用的，适合大部分情况的模板BaseVertex，其继承了OptimizableGraph::Vertex。其中 int D 是表示顶点的最小维度，例如三维地图点的话，int D = 3。
template <int D, typename T>class BaseVertex : public OptimizableGraph::Vertex {public:typedef T EstimateType;typedef std::stack<EstimateType, std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > >BackupStackType;static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space...some code}
g2o里有许多已经定义好的顶点：
VertexSE2 : public BaseVertex<3, SE2> // SE表示的2D顶点（x, y, yaw）
VertexSE3 : public BaseVertex<6, Isometry3> // 等距变换的3D位姿（x, y, z, qx, qy, qz） 没有qwVertexPointXY : public BaseVertex<2, Vector2> // 2D 点
VertexPointXYZ : public BaseVertex<3, Vector3> // 3D 地图点
VertexSBAPointXYZ : public BaseVertex<3, Vector3> // 3D 地图点VertexSE3Expmap : public BaseVertex<6, SE3Quat> // SE3 顶点，内部用变换矩阵参数化，外部用指数映射参数化VertexCam : public BaseVertex<6, SBACam> // 位姿 SBACam顶点VertexSim3Expmap : public BaseVertex<7, Sim3> // Sim3 顶点
自定义顶点
如果说g2o的预定义顶点不满足我们的使用要求，还可以自定义顶点。根据3D/2D的应用场景，待优化变量: 位姿/三维地图点，不同的优化类型: 李群/李代数，可以定义自己的顶点。
模板如下：
class myVertex : public g2o::BaseVertex<Dim, Type>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWmyVertex();virtual void read(std::istream& is) {}virtual void write(std::osteam& os) const {}// 设置顶点的初始值virtual void setOriginImpl(){_estimate = Type();}// 顶点更新函数// 其实就是 x + δxvirtual void oplusImpl(const double* update) override{_estimate += some kind of (update);}
}
除了加法更新外，还存在乘法更新，例如 types_six_dof_expmap.h 中的SE(3) 位姿。
/* \\brief SE3 Vertex parameterized internally with a transformation matrixand externally with its exponential map*/
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat> // 6 是存储优化变量的维度，六维李代数； SE3Quat 是优化变量的类型，内部使用了四元数表达旋转，然后加上位移
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();bool read(std::istream& is);bool write(std::ostream& os) const;// 设置顶点的初始值virtual void setToOriginImpl() {_estimate = SE3Quat();}// 顶点更新函数// 乘法更新virtual void oplusImpl(const double* update_) {Eigen::Map<const Vector6d> update(update_);setEstimate(SE3Quat::exp(update)*estimate());}
};
向图中添加顶点
无论是哪种顶点，向图中添加的方式都是一模一样的：创建顶点->设置ID->设置观测值(初始值)->添加到优化器
CurveFittingVertex *v = new CurveFittingVertex(); // 新建顶点
v->setId(0); // 设置顶点的ID
v->setEstimate ( Eigen::Vector3d(1, 1 , 1) ); // 设置顶点的观测值
optimizer.addVertex(v); // 往优化器里添加顶点
还有是否需要边缘化的选项，我们用 VectorSBAPointXYZ 类型的三维地图点为例
int index = 0;
for (const Point3f p : points)
{g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ(); // 新建顶点point->setId(index++); // 设定顶点编号point->setEstimate(Eigen::Vector3d (p.x, p.y, p.z) ); // 设定观测值(初始值)point->setMarginalized( true ); // 设定是否需要边缘化optimizer.addVertex(point); // 将顶点添加到优化器中
}
边
g2o 中有三种类型的边，BaseUnaryEdge(一元边)、BaseBinaryEdge(二元边)、BaseMultiEdge(多元边)。
一条边可以连接多少个顶点，就叫几元边，我们常用的是二元边。
我们以三维点及其重投影误差作为例子，其两个顶点是三维地图点和位姿，边是重投影误差。
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
二元边的定义有4个参数，其中
- 2 --> 测量值的维度，这里是2维，也就是说图像的二维像素坐标
- Vertex2D --> 测量值的类型，Vertex2D
- VertexSBAPointXYZ --> 第一个顶点，地图三维点的类型
- VertexSE3Expmap --> 第二个顶点，李群位姿
我们可以看一下BaseBinaryEdge的定义
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWmyEdge();virtual bool read(istream& in) {} // 读virtual bool write(ostream& out) const {} // 写// 误差 = 测量值 - 估计值virtual void computeError() override{_error = _measurement - /*估计值*/}virtual void linearizeOplus() override {/*计算偏导数(雅克比矩阵)*/}}
看起来很简单，我们来看一个最小化重投影误差的问题，源码在：
edge_project_xyz2uv.h
// edge_project_xyz2uv.h#ifndef G2O_SBA_EDGEPROJECTXYZ2UV_H
#define G2O_SBA_EDGEPROJECTXYZ2UV_H#include "g2o/core/base_binary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o_types_sba_api.h"
#include "parameter_cameraparameters.h"
#include "vertex_se3_expmap.h"namespace g2o {class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjectXYZ2UV();bool read(std::istream& is); // 读bool write(std::ostream& os) const; // 写void computeError(); // 计算误差virtual void linearizeOplus(); // 计算增量public:CameraParameters* _cam; // 相机参数
};} // namespace g2o
#endif
edge_project_xyz2uv.cpp
// edge_project_xyz2uv.cpp#include "edge_project_xyz2uv.h"namespace g2o {EdgeProjectXYZ2UV::EdgeProjectXYZ2UV() : BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>()
{_cam = 0;resizeParameters(1);installParameter(_cam, 0);
}bool EdgeProjectXYZ2UV::read(std::istream& is)
{readParamIds(is);internal::readVector(is, _measurement);return readInformationMatrix(is);
}bool EdgeProjectXYZ2UV::write(std::ostream& os) const
{writeParamIds(os);internal::writeVector(os, measurement());return writeInformationMatrix(os);
}// 计算误差
void EdgeProjectXYZ2UV::computeError()
{const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); // 顶点1，李群位姿const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]); // 顶点2，三维地图点const CameraParameters* cam = static_cast<const CameraParameters*>(parameter(0)); // 相机参数_error = measurement() - cam->cam_map(v1->estimate().map(v2->estimate())); // 误差 = 测量值 - 估计值// 先用map，将顶点2的三维地图点转换到相机坐标系下// 后用cam_map，将三维地图点转换为像素坐标
}// 计算增量
void EdgeProjectXYZ2UV::linearizeOplus()
{VertexSE3Expmap* vj = static_cast<VertexSE3Expmap*>(_vertices[1]);SE3Quat T(vj->estimate());VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);Vector3 xyz = vi->estimate();Vector3 xyz_trans = T.map(xyz);number_t x = xyz_trans[0];number_t y = xyz_trans[1];number_t z = xyz_trans[2];number_t z_2 = z * z;const CameraParameters* cam = static_cast<const CameraParameters*>(parameter(0));// 重投影误差 对 三维点求偏导数的 雅克比矩阵// 2维像素点 3维地图点 --> 2x6矩阵Eigen::Matrix<number_t, 2, 3, Eigen::ColMajor> tmp;tmp(0, 0) = cam->focal_length;tmp(0, 1) = 0;tmp(0, 2) = -x / z * cam->focal_length;tmp(1, 0) = 0;tmp(1, 1) = cam->focal_length;tmp(1, 2) = -y / z * cam->focal_length;_jacobianOplusXi = -1. / z * tmp * T.rotation().toRotationMatrix();// 重头误差 对 相机位姿求偏导数的 雅克比矩阵// 2维像素点 6维位姿 --> 2x6矩阵_jacobianOplusXj(0, 0) = x * y / z_2 * cam->focal_length;_jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * cam->focal_length;_jacobianOplusXj(0, 2) = y / z * cam->focal_length;_jacobianOplusXj(0, 3) = -1. / z * cam->focal_length;_jacobianOplusXj(0, 4) = 0;_jacobianOplusXj(0, 5) = x / z_2 * cam->focal_length;_jacobianOplusXj(1, 0) = (1 + y * y / z_2) * cam->focal_length;_jacobianOplusXj(1, 1) = -x * y / z_2 * cam->focal_length;_jacobianOplusXj(1, 2) = -x / z * cam->focal_length;_jacobianOplusXj(1, 3) = 0;_jacobianOplusXj(1, 4) = -1. / z * cam->focal_length;_jacobianOplusXj(1, 5) = y / z_2 * cam->focal_length;
}} // namespace g2o定义完了，就是如何往图里添加边了。这个其实和添加点一样，是程式化的。
下面看一下添加重投影误差的二元边的例子
index = 1;for (const Point2f p : points)
{g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); // 新建边edge->setId(index); // 设置边的IDedge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(index))); // 设置第一个顶点，三维地图点edge->setVertex(1, pose); // 设置第二个顶点，位姿edge->setMeasurement(Eigen::Vector2d(p.x, p.y)); // 设置观测值（初始值），二维像素坐标edge->setInformation(Eigen::Matrix2d::Identity()); // 设置信息矩阵optimizer.addEdge(edge); // 将边添加到优化器中index++;
}
实际应用
TODO …
后面写第二篇
