G2O优化构建

2024-02-15 22:52

文章标签 构建优化 g2o

本文主要是介绍G2O优化构建，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

g2o是一个通用的图优化库，可以应用到任何能够表示成图优化的最小二乘问题。例如BA，icp，曲线拟合等。

构建G2O优化器

构建G2O优化是一种图优化，所以需要准备顶点和边

顶点：待优化的变量（状态）

边：顶点之间的约束关系，常用误差表示

针对一个曲线拟合的问题

顶点为我们所需要求解的a,b,c，边就为预测值与观测值之间的差值。

定义顶点和边

顶点继承自g2o::BaseVertex，边继承自public g2o::BaseUnaryEdge

边有一元边，二元边和多元边，分别需要继承自

g2o::BaseUnaryEdge<D, E, VertexXi>
g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
g2o::BaseMultiEdge<D, E>

其中D表示误差值的维度，E表示测量值的数据类型，VertexXi表示连接顶点类型。

核心需要重载实现computeError函数，用于计算误差

一元边：

多元边：

边的误差计算

    void computeError() override {const VertexParams *params = dynamic_cast<const VertexParams *>(vertex(0));//顶点const double &a = params->estimate()(0);const double &b = params->estimate()(1);const double &c = params->estimate()(2);// double fval = a * exp(-lambda * measurement()(0)) + b;double fval = sin(a * measurement()(0)) + cos(b * measurement()(0)) + c;_error(0) = std::abs(fval - measurement()(1));}

配置优化器

定义优化器

选择优化算法

构建图（添加顶点和边）

添加顶点

添加顶点的时候，每个顶点为设置一个ID，在添加边的时候可以使用ID来查找顶点。

在添加顶点的时候，会设置一个初始估计值，后续的误差计算会拿出顶点的当前估计值参与运算

VertexParams *params = new VertexParams();
params->setId(0);
//设置初始值
params->setEstimate(Eigen::Vector3d(0.7, 2.4, 2));//初始化顶点的估计值
optimizer.addVertex(params);

添加边

在添加边的时候会设置顶点和测量值。，一元边只需要设置一个顶点，二元边需要设置两个顶点。这一步设置的顶点，需要跟上一步添加的顶点一一对应起来，边添加完成后，顶点和边会构成一个图。顶点和测量值会在计算误差的时候参与运算。

for (int i = 0; i < numPoints; ++i) {EdgePointOnCurve *e = new EdgePointOnCurve;e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());e->setVertex(0, params);e->setMeasurement(points[i]);optimizer.addEdge(e);
}

执行优化

    optimizer.initializeOptimization();optimizer.computeInitialGuess();optimizer.computeActiveErrors();optimizer.setVerbose(false);optimizer.optimize(maxIterations);