Reference:
- 高翔,张涛 《视觉SLAM十四讲》
相关文章跳转:
在前文中我们介绍了 Ceres,Ceres 在定义误差项求曲线拟合的问题上比较自然,因为它本身就是一个优化库。
相比之下,这里引入的 g2o 的做法就比较迂回,如果用 g2o 来拟合曲线,必须先把问题转换为图优化,定义新的顶点和边。
然而,在 SLAM 中更多的问题是,一个带有许多个相机位姿和许多个空间点的优化问题如何求解。特别地,当相机位姿以李代数表示时,误差项关于相机位姿的导数如何计算,将是一件值得详细讨论的事。g2o 提供了大量现成的顶点和边,非常便于相机位姿估计问题。而在 Ceres 中,我们不得不自己实现每一个 Cost Function,有一些不便。
1. Introduction
图优化,是把优化问题表现成图的一种方式。这里的图是图论意义上的图。一个图由若干个顶点(Vertex),以及连接着这些顶点的边(Edge)组成。进而,用顶点表示优化变量,用边表示误差项。
上图中给出了一个简单的图优化的例子:
- 图优化的顶点
三角形----相机的位姿节点;
圆形----路标点; - 图优化的边
实线----相机的运动模型
虚线----相机的观测模型
为了使用 g2o,首先要将拟合问题抽象成图优化。节点为优化变量,边为误差项。在 g2o 中,将边称做超边(Hyper Edge),而整个图被叫做超图(Hyper Graph)。
g2o 主要包含以下步骤:
- 定义顶点和边的类型
- 构建图
- 选择优化算法
- 调用 g2o 进行优化,返回结果
2. 示例:优化 exp(ax^2+bx+c)
现想要拟合曲线
e
a
x
2
+
b
x
+
c
e^{ax^2+bx+c}
eax2+bx+c,将曲线拟合问题抽象成图优化。在该曲线拟合问题中,整个问题只有一个顶点(有且仅需优化
a
,
b
,
c
a,b,c
a,b,c):曲线模型的参数是
a
,
b
,
c
a,b,c
a,b,c;而各个带噪声的数据点,构成了一个个误差项,也就是图优化的边。由图片可知,这里是一个一元边(Unary Edge)----整个图中只有一个顶点(因为在每一组数据中,数据本身并没有发生变化,仅仅是被施加了不同噪声的相同数据集,因为都是同一个顶点)。有多少组观测数据就有多少图优化的边?
- Step 1: 构建图优化,并选择优化算法
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 定义每个误差项优化变量维度为3,误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
这里首先创建了一个线性求解器。BlockSolver
在 Hessian 块上执行求解器。 由这里的 LinearSolverDense
可知,使用的是 Dense Cholesky 分解法。
这里的 OptimizationAlgorithmGaussNewton
表示求解器使用的是高斯牛顿来做非线性优化。SparseOptimizer
表示创建一个稀疏优化器。
- Step 2: 派生用于曲线拟合的图优化顶点
// 曲线模型的顶点,模板参数:优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 在new一个对象时会总是返回一个对齐的指针
// 重置
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}
// 更新
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}
};
1.这里 BaseVertex
的模板为 template <int D, typename T>,其中 D 表示顶点最小的维度;T 表示待估计 Vertex 类型;
2.setToOriginImpl
为顶点的重置函数,用来设定待优化变量的初始值;
3.oplusImpl
为顶点更新函数,主要用于在计算出增量
Δ
x
\Delta x
Δx 后,用来更新
x
k
+
1
=
x
k
+
Δ
x
x_{k+1}=x_k+\Delta x
xk+1=xk+Δx;这里的顶点更新看起来只是很简单的加法而已,为什么 g2o 不帮我们完成呢?在曲线拟合过程中,由于优化变量(曲线参数)本身位于向量空间中,这个更新计算就只是简单的加法。但是当优化变量不在向量空间时,如
x
x
x 是相机位姿(李代数表示位姿),它本身不一定有加法运算。这时,就需要重新定义增量如何加到现有的估计上的行为了。比如会使用左乘更新或右乘更新,而不是直接的加法。
- Step 3: 派生用于曲线拟合的图优化边
// 误差模型 模板参数:观测值维度,类型,连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
virtual void computeError() override {
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 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]); // exp(ax^2+bx+c)
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
public:
double _x; // x 值, y 值为 _measurement
};
1.computeError
为边的误差计算函数。该函数需要取出边所连接的顶点的当前估计值(即更新后的
Δ
x
\Delta x
Δx),根据曲线模型,与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的;
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
// e r r o r = y − e a x 2 − b x − c error = y- e^{ax^2 - bx - c} error=y−eax2−bx−c
2.linearizeOplus
为边的雅克比计算函数。这个函数计算了每条边相对于顶点的雅克比。其中:
- Step 4: 往图中添加顶点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);
1.setEstimate
用来设定初始值,需要先给定估计参数值,在该情境下估计的是
a
e
,
b
e
,
c
e
a_e,b_e,c_e
ae,be,ce;
2.setId
定义了节点的编号;
3.定义好的顶点,需要将它添加到 SparseOptimizer 中,即:optimizer.addVertex(v)
。
- Step 5: 往图中添加边
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);
}
1.
N
N
N 为观测数据点个数,这里设定的观测数据点为 100,则
N
=
100
N=100
N=100;
2.每条边都需要设定一个 id;
3.void setVertex(size_t i, Vertex* v)
将超边关联到第
i
i
i 个顶点。
- Step 6: 设置优化参数,执行优化
optimizer.initializeOptimization();
optimizer.optimize(10);
1.int optimize(int iterations, bool online = false)
中iterations为迭代次数;
Step 7: 输出优化值
Eigen::Vector3d abc_estimate = v->estimate(); // 估计值读取方式
cout << "estimated model: " << abc_estimate.transpose() << endl;