前言

现有的位姿求解算法，有直接线性变换法、非线性优化法，其中最广为人知的是基于捆绑调整的非线性优化方法（Bundle Adjustment，BA）。g2o是一个基于图优化的非线性优化库，目前已经是包含COLMAP在内的大型SLAM算法的根基。g2o可以用于求解大规模的非线性优化问题，自然也适用捆绑调整。

g2o完全没有用户文档

并且，新版本的g2o与当前社区中流传的CSDN、博客园甚至著名的《视觉SLAM14讲》等文章介绍内容几乎都存在较大的差异（基本上都不能直接使用），毫无疑问增加了新手使用的探索成本。本文，将记录当前时间最新版本的g2o安装、使用的一些心得与避雷提示，希望读者能够通过本博客跑通示例程序，并大致知道如何根据自己需求修改例程。

安装g2o

首先请前往g2o官方仓库，下载最新版本的g2o至本地，进入该目录，首先需要注意的是，g2o安装前应当至少安装Eigen3库。当前网上教程大多直接推荐使用apt源安装，即有：

1	sudo apt-get install libeigen3-dev

这种方法方便快捷，但是也存在潜在的版本匹配问题。

另一个麻烦之处是如果使用Vscode等IDE，可能会出现头文件无法找到，从而不能自动补全的问题

因此，本文推荐使用手动安装Eigen3库，首先前往Eigen源码地址，将其克隆至本地，见于：

git clone https://gitlab.com/libeigen/eigen.git
cd eigen
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_PREFIX=/path/to/eigen # 注意这一步，请指定安装路径
make install

然后，在当前用户的.bashrc文件中添加如下内容：

1 2	export CPLUS_INCLUDE_PATH=/path/to/eigen/include:$CPLUS_INCLUDE_PATH export C_INCLUDE_PATH=/path/to/eigen/include:$C_INCLUDE_PATH

理论上，这样就可以使用cmake自动查找Eigen3库并编译g2o了。接下来，进入g2o目录，在这里我遇到了第二个重大BUG

主分支master的源代码有问题

具体体现在，使用主分支代码编译的动态链接库在编写g2o程序时，始终显示undefined reference错误，而且报错的对象是g2o::SparseOptimizitionGraph一类的核心函数，问题相当严重。查阅网上说法以及github的仓库的提交记录后得知，master分支太旧了，有很多不兼容BUG，此时应该使用最新版分支pymem，执行如下命令：

git checkout pymem
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_PREFIX=/path/to/g2o
make -j8
make install

应当会持续一段时间，最终在/path/to/g2o目录下生成include、lib等文件夹，即为安装成功。

同样地，请在.bashrc文件中添加环境变量。

1
2
3

export CPLUS_INCLUDE_PATH=/path/to/g2o/include:$CPLUS_INCLUDE_PATH
export C_INCLUDE_PATH=/path/to/g2o/include:$C_INCLUDE_PATH
export LD_LIBRARY_PATH=/path/to/g2o/lib:$LD_LIBRARY_PATH

这里多了一步，因为g2o编译将生成动态链接库，因此需要将其添加至动态链接库路径中。

验证安装

先别着急删除源代码（建议保留），接下来，打开g2o/examples目录，可以看到官方源码提供有一些示例程序，其中包含了CMakeLists.txt文件，可以直接使用cmake编译

这里千万不要强行照搬网上的教程，因为每个人的依赖安装环境都不一样，直接照搬极可能编译失败。

例如，打开最简单的曲线拟合例程curve_fit.cpp，输入命令：

mkdir build
cd build
cmake ..
make

不出意外的话，你将会遇到第一个bug：error: ‘string_view’ in namespace ‘std’ does not name a type。这是因为g2o的新版本使用了C++17的特性，而例程中提供的CMakeLists.txt文件并没有指定编译器版本，因此默认使用C++11，解决方法是在CMakeLists.txt文件中添加如下内容：

1	set(CMAKE_CXX_STANDARD 17)

然后重新编译，此时有两种情况，一种是直接编译成功，恭喜你直接可以进入下一步。如果编译失败也不用灰心，因为我也失败了，而且遇到的是这个错误：

1 2	/usr/bin/ld: cannot find -lcore /usr/bin/ld: cannot find -lsolver_dense

但是打开/path/to/g2o/lib目录，发现这两个库文件确实存在，这是因为g2o的新版本将库文件名字改为了libg2o_core.so和libg2o_solver_dense.so，因此需要在CMakeLists.txt文件中，将示例提供的内容：

1	target_link_libraries(curve_fit core solver_dense)

修改为如下：

1	target_link_libraries(curve_fit g2o_core g2o_solver_dense)

再编译，可能会报错未定义的符号，这是因为g2o的新版本修改了部分API的位置，此时根据CMake报错信息，在target_link_libraries中添加对应的库文件名，以我的程序为例，最终添加完的内容如下：

1	target_link_libraries(curve_fit g2o_core g2o_solver_dense g2o_stuff)

运行可执行文件./curve_fit，即可在控制窗口看到拟合结果。

g2o入门使用

g2o并没有官方文档，因此全部的参考只能通过博客或者例程，而博客又稍有点陈旧，大多不能直接使用，这里建议直接参考源码例程，比照源码，理论上两天内能够入门g2o的基本使用。是时候献上经典g2o的派生类关系图啦，请膜拜：
g2o类关系图

图中提到了解决一个非线性优化问题的全部步骤。如果要长期使用g2o，请牢记这个图。

当然，源码中也提供了尚未编译的原理文档（数学原理），可以在/path/to/g2o/doc目录下找到，直接用latex编译可能报错，推荐安装fig2dev工具，将*.fig文件转换为*.pdf文件，然后再编译。如果嫌麻烦，可以直接阅读这里提供的指导文档。

程序框架

推荐使用C++编写g2o程序（尽管g2o确实支持Python，但是其对象编程设计得相当糟糕，起不到任何帮助学习的作用，仍然必须先熟悉其C++使用后，才能转移至python脚本完成编写）。一个典型的g2o程序框架应当包括以下几个部分：

部分	功能	说明
顶点	存储待优化的变量	需要用到
边	存储观测量与顶点的关系	需要用到
优化图	用于求解非线性优化的图结构	不需要修改
优化图参数	存储一些不需要修改的参数	例如相机内参数

其中，如果是解决常见问题，例如PnP、ICP、SLAM2d和SLAM3d问题，g2o已经预置了相当多的顶点和边的类型，可以直接使用，按需要修改其Arguments即可，不需要自己定义新的边和顶点类型，这一部分见于顶点Vertex和边Edge。如果要解决一些特别的问题（例如曲线拟合、曲面拟合等），此时往往需要定义新的边和顶点。

顶点Vertex

直观地说，顶点是存储可优化变量的对象。以最典型的PnP求解位置、姿态问题为例，如果认为世界坐标系的三维点 $\mathbf{X}$ 和图像坐标系的二维点 $\mathbf{x}$ 都是准确的，那么求解位姿的问题是最小化重投影误差的问题，即：

$\argmin_{\mathbf{R},\mathbf{T}}\sum_{i=1}^{n}\left\|\mathbf{x}_i-\mathbf{K}\left(\mathbf{R}\mathbf{X}_i+\mathbf{T}\right)\right\|^2$

其中，能够被优化的变量仅有 $\mathbf{R}$ 和 $\mathbf{T}$ ，其余量都是不可修改的（请记住这里，后面程序中会用到）。在本问题中，顶点就应当定义为存储 $\mathbf{R}$ 和 $\mathbf{T}$ 的对象（使用李代数，可将这个对象描述为一个 $\mathbf{SE}(3)$ 的李群元素，即使用一个顶点就足以同时描述旋转和平移两种变换）。

在SLAM问题中，除了相机的位姿，地图点（即三维点 $\mathbf{X}$ ）也应当是可优化的变量，因此SLAM问题中还需要额外定义地图点 $\mathbf{X}$ 作为顶点，此时不需要使用李代数，普通的三维矢量即可充分描述其性质。

对于直接PnP的问题，顶点可以使用g2o::VertexSE3Expmap类，其源代码见于g2o/types/sba/types_six_dof_expmap.h文件中，另一种常见的顶点类型是g2o::VertexSBAPointXYZ，用于描述三维点，其源代码可见于g2o/types/slam3d/vertex_pointxyz.h文件中，亦如下：

namespace g2o {
/**
 * \brief Vertex for a tracked point in space
 */
class G2O_TYPES_SLAM3D_API VertexPointXYZ : public BaseVertex<3, Vector3> {
 public:
  VertexPointXYZ() = default;

  void oplusImpl(const VectorX::MapType& update) override {
    estimate_ += update.head<kDimension>();
  }
};
}  // namespace g2o

值得注意的是，在定义的顶点中，必须要求实现oplusImpl方法（旧版本要实现的setToOrigin等方法可以不需要了），建议在定义时加入关键字override以显式强调继承类型正确。oplusImpl方法描述的是如何利用增量更新顶点的值，即如下式：

$\mathbf{v}_n\leftarrow f(\mathbf{v}_{n-1},\Delta\mathbf{v})$

因此其传入的形参update即为增量 $\Delta\mathbf{v}$ ，而本例是三维点的顶点类型，因此可以直接相加，即 $f(\bullet,\bullet)$ 是矢量加法。而在g2o::VertexSE3Expmap类中，oplusImpl方法的实现如下：

1
2
3

void VertexSE3Expmap::oplusImpl(const VectorX::MapType& update) {
  setEstimate(SE3Quat::exp(update.head<kDimension>()) * estimate());
}

其不再是加法，而是相应的李代数乘法。

边Edge

从图论来讲，边是存储顶点与顶点关系的数学抽象。在g2o中，边是存储顶点与顶点之间的计算关系的对象，其含有求解误差、雅可比矩阵等成员方法，因此边是计算图的核心。仍然以PnP问题为例，三维空间点 $\mathbf{X}$ 和二维图像点 $\mathbf{x}$ 均是已知量，而现有的顶点只有相机位姿 $\xi\in\mathbf{SE}(3)$ ，因此这里的边应当连接顶点 $\xi$ ，而边的另一侧还是顶点 $\xi$ （闭环），边的计算关系即为重投影误差。在本例中，边可以使用如下定义的类：

namespace g2o {
class CameraParameters;

class G2O_TYPES_SBA_API EdgeProjectXYZ2UVOnlyPose
    : public BaseUnaryEdge<2, Vector5, VertexSE3Expmap> {
 public:
  EdgeProjectXYZ2UV();
  void computeError() override;
  void linearizeOplus() override;

  std::shared_ptr<CameraParameters> _cam;  
};

}  // namespace g2o

不要去找了，这个类是我自己写的，源码中没有

说明类模板的参数定义：

2：边的维度，即误差的维度，由于重投影误差是二维向量，因此这里是2
Vector5：边的观测量，这里是一个五维向量，分别是 $\mathbf{x}$ 和 $\mathbf{X}$ ，即图像点与其对应的三维点
VertexSE3Expmap：边的连接顶点，这里是相机位姿 $\xi$

此外，还有更加复杂的类模板，例如双边g2o::BaseBinaryEdge:

1 2	template <int D, typename E, typename VertexXi, typename VertexXj> class BaseBinaryEdge : public BaseFixedSizedEdge<D, E, VertexXi, VertexXj>

其中D仍然是边的维度，E是边的观测量，VertexXi和VertexXj分别是边的两侧顶点（这表明这种边类型一定连接两个不同的顶点）。这种边的类型是比较常用的类型。此外还有多边g2o::BaseFixedSizedEdge：

1 2	template <int D, typename E, typename... VertexTypes> class BaseFixedSizedEdge : public BaseEdge<D, E>

可以连接任意数量的顶点，（一般也用不到这么多，优化效果会越来越差，计算复杂度也随之上升）。三边是顶天的用法了。

对于边的实现，必须要实现方法computeError和linearizeOplus，前者描述了如何计算误差 $\Delta \mathbf{e}$ ，后者描述了如何计算雅可比矩阵 $\mathbf{J}$ （旧版本中要实现的read、write方法可以不需要实现）。

优化图Optimization Graph

优化图是计算非线性优化的核心，这部分我们不需要自定义修改，只需要拿来主义即可，需要保证以下步骤：

创建求解器对象。
选定求解算法。
向优化图中添加顶点和边。
调用optimize方法进行优化。
利用optimizer.vertices().find(int id)方法获取优化后的顶点与优化值。

一个典型的优化图的使用方法如下：

g2o::SparseOptimizer optimizer; // 创建优化图对象
optimizer.setVerbose(true); // 设置是否输出优化信息
g2o::OptimizationAlgorithmProperty solverProperty;
optimizer.setAlgorithm(
  g2o::OptimizationAlgorithmFactory::instance()->construct（("lm_fix6_3",                                              solverProperty));

// 添加顶点和边
g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
v->setId(0);
v->setEstimate(g2o::SE3Quat());
optimizer.addVertex(v);

g2o::EdgeProjectXYZ2UVOnlyPose* e = new g2o::EdgeProjectXYZ2UVOnlyPose();
e->setVertex(0, v);
e->setMeasurement(Vector5());
e->setInformation(Matrix2::Identity());
optimizer.addEdge(e);

optimizer.initializeOptimization();
optimizer.optimize(10);

优化图参数

典型的优化图参数是相机内参数，使用g2o::CameraParameters类，它是边类型的共享指针，可以在边的内部被访问，在计算误差、雅可比矩阵时常被使用，在边内部访问的方法如下：

1	const g2o::StereoCameraParameters &cam = static_cast<g2o::CameraParameters *>(parameter(0).get())->param();

将其传入优化图的方法如下：

optimizer.addParameter(cam_params)
/*
// 亦可使用下面的方法保证一定传入成功
  if (!optimizer.addParameter(cam_params))
  {
    assert(false);
  }
*/

例程

这里不提供使用的例程，完整的程序请参考源码的g2o/examples目录，基本上每个都尝试一遍，出现的bug都会遇到，解决完后g2o也入门了。

踩雷记录

override在编译时提示始终未覆盖基类方法，这是因为网上的版本大多是旧版本，新版本中API产生更改，不再适用。请参考源码中的例程，以保证正确性。
关于顶点的setFixed(bool)和setMarginalized(bool)方法，这里简要说明：
- setFixed方法用于固定顶点，即在优化过程中不对其进行优化（因此其值在优化前后不会改变！）。
- setMarginalized方法用于边的边缘化，即在优化过程中不对其进行优化，但是会将其边缘化，这在SLAM问题中也很常见，例如边缘化地图点，边缘化相机内参数等，详细参考这篇博客。
另外，本人还遇到一个问题，目前尚未解决（推测是CMake链接的问题），即将全部自定义的顶点、边和相机参数代码放在同一个.hpp文件中，编译为动态库，链接至主程序后可以正确运行并输出正确结果。而如果将声明代码放在.h文件，将实现代码放在.cpp文件中，编译为动态库，链接至主程序后，运行时发现相机参数类型g2o::CameraParameters无法正确传入，其内部参数似乎被清空了，导致程序无法正确运行（即使是自行重定义一个MyCameraParameters类型，也无法正确传入）。目前尚未解决，希望有大佬襄助。