最近在学习g2o库的使用,折腾了几天,虽然整体还不是很清楚,但有不少收获,先做总结。
1. 学习方法
学习资料比较杂乱,我个人感觉最佳的学习方法是:
- 了解图优化基本思想
- 大致阅读g2o的教程文档:g2o: A general Framework for (Hyper) Graph Optimization
- 清楚写一个g2o需要做什么
- 对照API文档,逐个学习真实代码中的细节是在干什么。
- 不断实践学习
2. 具体内容
2.1 理论学习
SLAM图优化相关博客:https://blog.csdn.net/heyijia0327/article/details/47686523
g2o文档:g2o: A general Framework for (Hyper) Graph Optimization
文档来源:1. g2o源码中:g2o/doc/g2o.pdf
2. 我上传了百度网盘:https://pan.baidu.com/s/1zk4CXWOjpnmYLORhxQxa0Q 提取码: v7tx
2.2 g2o的下载与安装
g2o github: https://github.com/RainerKuemmerle/g2o
下载下来以后,执行常规操作即可。
生成API文档:
g2o的源码自带了API文档,但需要doxygen生成,同时保证安装了dot这个东西。
- doxygen下载与安装:https://blog.csdn.net/tfb760/article/details/103593100
- 安装完成doxygen,需要安装graphviz/dot,方法如下:
sudo apt-get install graphviz
# 之后遇到报错: 依赖libcdt5...等balabala
sudo apt-get -f install
- 生成API: 进入g2o/doc/doxygen文件,按照readme.txt执行
doxygen doxy.config
之后生成了html文件,可以在这里下载(提取码efma)我自己生成的
2.3 使用g2o优化需要定义什么?
代码中使用g2o时,需要定义以下三项内容:
1. 定义边(edge)和顶点(vertex):包括边和顶点的数据类型,顶点的数据更新方式,边的误差计算方法;
2. 确定优化求解器,包括:线性方程求解器、BlockSolver和迭代策略
3. 将定义的边和顶点连接成图,并开始优化
3. 具体例子
下面使用《视觉SLAM十四讲》中第6讲非线性优化的代码,注释说明具体的使用方法。
完整代码下载:https://pan.baidu.com/s/1YAIMiWPmPV2CDL8MTNV4tA 提取码: evzt
STEP 1. 定义顶点
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
// 自定义一个Vertex类,继承基本的BaseVertex,3维向量,数据格式为Vector3d
public:
// 这里时Eigen的一个宏定义,声明后后面的new方法使用eigen自己定义的方法,避免系统new的一些问题
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 定义“设置为零的实现(implement)”,覆盖默认的虚函数
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}
// 定义Oplus(自定义加法)的实现,即顶点的更新方法
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}
virtual bool read(istream &in) {} // 读写函数不覆盖
virtual bool write(ostream &out) const {}
};
可以看出,需要重写四个函数,其中最重要的oplusImpl,表示更新方法。由于顶点数据可能是自己定义的,所对应的加法(写作" ⊕ \oplus ⊕",所以函数名是"o-plus",而Impl指implement,这个是自己推测的……)需要自己定义而不是简单的相加。但这里进行拟合只是1维数字,所以直接加就行。
STEP 2. 定义边
// 自定义一个Edge类,继承自BaseUnaryEdge,有1个变量,类型是double,连接的vertex(由于是unary所以只有一个)类型时CurveFittingVertex
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
virtual void computeError() override { // 重写computeError函数,用于计算误差
// 通过从API文档中翻阅,可以查到 _vertices 是一个 vecotr<Vertex*>,所以取[0]为第一个Vertex,强制转化成我们自定义的 CurveFittingVertex类。
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate(); // 读取当前边的估计值
// 计算误差,误差是由measurement减去的当前估计值算出来的结果,_error是由Eigen定义的一个向量,向量的维度和类型默认被父类定义好了
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// 计算雅可比矩阵,这一部分可以不写。如果不写,将采用内置的数值计算方法求解,但速度较慢。
// 如果具体写出了,将采用重写的方法进行计算,速度较快。
virtual void linearizeOplus() override {
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
_jacobianOplusXi[0] = -_x * _x * y; // _jacobianOplusXi是 JacobianXiOplusType,本质是一个Eigen::Matrix
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x;
};
STEP 3. 在main中定义优化器
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 简写BlockSolver,其由一个具体的BlockSolverTraits定义维度3x1.
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 定义先行求解器的简写,来自于BlockSolver的PoseMatrixType。
// 定义一个求解器,使用GN算法(只能是GN,LM,DogLeg的一种),具体定义形式参看API文档。自己还不是很清楚
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 定义图模型
optimizer.setAlgorithm(solver); // 选择求解算法,使用刚刚定义的求解器
STEP 4. 连接成图
// 首先增加节点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce)); // 设定初始值为估计值
v->setId(0); // 每个顶点需要确定唯一的id
optimizer.addVertex(v); // 将定义好的节点加到优化器中
// 再增加边
for (int i = 0; i < N; i++) { // 把所有观测数据加上,每个都是一条边
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
edge->setId(i); // 边也需要设定唯一的ID
edge->setVertex(0, v); // 设置连接的顶点,第一个0表示这个边的一个节点(如果是Binary的edge,则需要设定0和1两个节点,分别连接那个节点)
edge->setMeasurement(y_data[i]); // 将观测值设置为测量值
// 信息矩阵,用于加权,取协方差矩阵的倒数。具体的取值根据经验吧。
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma));
optimizer.addEdge(edge); // 把边加进去
}
STEP 5. 开始优化求解
optimizer.initializeOptimization(); // 初始化:
optimizer.optimize(10); // 设定迭代次数并开始迭代优化
4. 其他
可以参考g2o中example提供的更多例子。