使用C++实现最小二乘曲线拟合（使用g2o实现）
自学视觉SLAM14讲-6.3.3，自学笔记
理论部分
略
安装g2o（安装在ubuntu20.04环境下有效）
先安装依赖环境，终端输入：
sudo apt-get install qt5-qmake qt5-default libqglviewer-dev-qt5 libsuitesparse-dev libcxsparse3 libcholmod3
在slambook2/3rdparty/g2o文件夹下打开终端并输入：
mkdir build
cd build/
cmake ..
make -j4
sudo make install
安装方法也可以查看链接，视觉slam的环境部署都在这里：
视觉SLAM十四讲-环境部署
g2o ( General Graphic Optimization，G2O )。它是一个基于图优化的库。图优化是一种将非线性优化与图论结合起来的理论,因此在使用它之前,我们花一点篇幅介绍图优化理论。
我们已经介绍了非线性最小二乘的求解方式。它们是由很多个误差项之和组成的。然而，目标函数仅描述了优化变量和许多个误差项，但我们尚不清楚它们之间的关联（优化变量和误差项之间的关联）。例如，某个优化变量xi存在于多少个误差项中呢?我们能保证对它的优化是有意义的吗?进一步，我们希望能够直观地看到该优化问题长什么样。于是,就牵涉到了图优化。
图优化，是把优化问题表现成图的一种方式。这里的图是图论意义上的图。一个图由若干个顶点( Vertex )，以及连接着这些顶点的边(Edge )组成。进而，用顶点表示优化变量，用边表示误差项。于是,对任意一个上述形式的非线性最小二乘问题,我们可以构建与之对应的一个图我们可以简单地称它为图，也可以用概率图里的定义，称之为贝叶斯图或因子图。
图6-2是一个简单的图优化例子。我们用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点;同时，实线表示相机的运动模型，虚线表示观测模型，它们构成了图优化的边。此时，虽然整个问题的数学形式仍是式(6.13)那样，但现在我们可以直观地看到问题的结构了。如果希望，也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大)的顶点这样的改进。但是最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质做更好的优化。
g2o是一个通用的图优化库。“通用”意味着你可以在g2o里求解任何能够表示为图优化的最小二乘问题，显然包括上面谈的曲线拟合问题。下面我们来演示这个过程。
为了使用g2o，首先要将曲线拟合问题抽象成图优化。这个过程中，只要记住节点为优化变量，边为误差项即可。曲线拟合对应的图优化模型可以画成图6-3所示的形式。
在曲线拟合问题中，整个问题只有一个顶点:曲线模型的参数是a,b, c;而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边。但这里的边与我们平时想的边不太一样，它们是一元边(Unary Edge )，即只连接一个顶点——因为整个图只有一个顶点。所以在图6-3中我们只能把它画成自己连到自己的样子。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映每个误差与多少个优化变量有关。在稍有些玄妙的说法中，我们把它叫作超边( Hyper Edge )，整个图叫作超图（Hyper Graph)。
作为g2o的用户，我们要做的事主要包含以下步骤:
1.定义顶点和边的类型。
2．构建图。
3．选择优化算法。
4．调用g2o进行优化，返回结果。
程序注释1：
static_cast 是 C++ 中的一种类型转换运算符，用于在编译时进行类型转换。它可以用于将一个表达式或值转换为另一种相关类型，但需要保证所执行的转换是类型安全的。
static_cast 可以用于以下几种类型转换操作：
隐式转换：用于执行标准的隐式类型转换，例如将整数类型转换为浮点类型，将指针类型转换为 void* 等。
显式转换：用于执行显式类型转换，例如将浮点类型转换为整数类型，将指针类型转换为其他指针类型等。
类层次转换：用于在继承关系中进行基类指针或引用向派生类指针或引用的转换。
类型补偿转换：用于在无关类型之间进行转换，例如将指针类型转换为整数类型，将整数类型转换为枚举类型等。
static_cast<目标类型>(表达式或值)
其中，目标类型 是要转换的目标类型，而 表达式或值 则是要进行类型转换的表达式或值。
需要注意的是，static_cast 并不进行运行时的类型检查，因此在执行类型转换时需要确保转换是合法的。如果转换是非法的，可能会导致未定义的行为或错误的结果。
程序注释2：
当abc的类型是Eigen::Vector3d时，abc(0, 0)和abc(0)都是正确的，都表示向量的第一个索引处的元素。
程序注释3：
/**
* override 是 C++11 引入的一个关键字，用于显式地标记派生类中覆盖（重写）基类的虚函数。
* 在 C++ 中，通过使用虚函数可以实现多态性，派生类可以重写基类的虚函数来提供自己的实现。
* 然而，有时候我们可能会犯一些错误，如拼写错误或参数不匹配等，导致派生类的函数并没有真正重写基类的函数。
* 这样的情况下，编译器也不会给出警告或错误提示。
* 为了避免这种情况的发生，C++11 引入了 override 关键字。
* 当我们在派生类中使用 override 关键字来修饰一个函数时，编译器会检查该函数是否真正重写了基类的虚函数。
* 如果没有正确地重写基类的虚函数，编译器将会给出错误提示，帮助我们发现并纠正错误。
*/
上代码:
关键代码1：
//图的 顶点类的定义
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
// 它继承自 g2o::BaseVertex<3, Eigen::Vector3d>，
// 表示这是一个具有三维参数（估计值）并使用 Eigen::Vector3d 类型表示的顶点。
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //是一个宏，用于确保对齐内存分配器以适应 Eigen 库的需要。
// 重置
// 虚函数，用于将顶点的估计值重置为原点（0, 0, 0）。
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}
// 更新，虚函数，用于根据传入的更新向量来更新顶点的估计值。
// 传入的更新向量是一个 double 类型的指针 update，
// 通过将其转换为 Eigen::Vector3d 类型，并将其加到当前的估计值 _estimate 上来实现更新。
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
/**
* 这是 CurveFittingVertex 类的成员函数 read() 和 write() 的定义部分。
* 这两个函数用于从流（istream）中读取顶点的数据或将顶点的数据写入流（ostream）。
* 在这里，它们被留空，需要根据实际需求进行实现。
*/
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
关键代码2：
// 图的边的类的定义
// 图的边的类的定义
// 继承自g2o::BaseUnaryEdge，
// 误差模型 模板参数：观测值维度1，类型double，连接顶点类型CurveFittingVertex
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 是一个宏，用于确保对齐内存分配器以适应 Eigen 库的需要。
// 这是 CurveFittingEdge 类的构造函数。它接受一个参数 x，用于初始化成员变量 _x。
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
virtual void computeError() override {
// _vertices[0] 是 CurveFittingEdge 类的成员变量，它是一个指向顶点的指针数组。
// 在这段代码中，_vertices[0] 表示与当前边连接的第一个顶点。
/**
* 通过 _vertices[0] 获取的指针需要进行类型转换，将其转换为 CurveFittingVertex* 类型的指针。
* 这是因为 BaseUnaryEdge 类是模板类，其中的顶点类型参数可能不同，
* 而在 CurveFittingEdge 类中，将顶点类型指定为 CurveFittingVertex。
*/
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
/**
* 通过将 _vertices[0] 转换为 CurveFittingVertex* 类型的指针，可以访问和操作与当前边连接的顶点的成员变量和方法。
* 在这段代码中，通过指针 v 获取了顶点的估计值 abc，并在计算误差和雅可比矩阵时使用了这个估计值。
*/
const Eigen::Vector3d abc = v->estimate();
_error(0) = _measurement - std::exp(abc(0) * _x * _x + abc(1) * _x + abc(2));
}
// 计算雅可比矩阵
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[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
关键代码3
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
关键代码4
// 梯度下降方法，可以从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); // 打开调试输出
关键代码5
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);
// 往图中增加边
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);
}
关键代码6：
// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10); // 10是迭代次数
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
完整代码：
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}
// 更新
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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]);
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
// 构建图优化，先设定g2o
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); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);
// 往图中增加边
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);
}
// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0;
}
218
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
