【SLAM模块】g2o 曲线拟合实例
【SLAM模块】g2o 曲线拟合实例
概念
g2o是一个图优化库，其将非线性优化与图论相结合。
特点
- 顶点为优化变量，边为误差项。
- 待估计的参数构成顶点，观测数据构成了边。
- 误差定义在边内，边附着在顶点上。
- 误差关于顶点的偏导数定义在边里的雅克比矩阵J上，而顶点的更新通过内置的oplusImpl函数实现更新。
- 在视觉slam中，相机的位姿与观测的路标构成图优化的顶点，相机的运动，路标的观测构成图优化的边。
结构
流程
1. 定义顶点和边的类型
自定义顶点时，需要重写4个函数:
virtual bool read(std::istream& is); // 读操作
virtual bool write(std::ostream& os) const; // 写操作
virtual void oplusImpl(const number_t* update); // 顶点更新函数
virtual void setToOriginImpl(); // 顶点重置函数
其中read，write 函数可以不进行覆写，仅仅声明一下就可以，setToOriginImpl 设定被优化变量的原始值，oplusImpl 比较重要，我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要写对，否则会造成一直优化却得不到好的优化结果的现象。
自定义边时，需要重写4个函数:
virtual bool read(std::istream& is); // 读操作
virtual bool write(std::ostream& os) const; // 写操作
virtual void computeError(); // 误差计算函数
virtual void linearizeOplus(); // 计算雅克比矩阵
read 和 write 函数同上，computeError 函数是使用当前顶点的值计算的测量值与真实的测量值之间的误差，linearizeOplus 函数是在当前顶点的值下，该误差对优化变量的偏导数，即jacobian矩阵。
2. 构建图优化
设置线性求解器，求解求 *Δx=−b 的线性求解器，完全采用第三方的线性代数库,主要采用 Cholesky分解 和 PCG迭代:
2.1 创建矩阵块求解器BlockSolver
typedef g2o::BlockSolver> BlockSolverType; // 矩阵块求解器
2.2 创建线性方程求解器LinearSolver
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器
2.3 创建总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique(g2o::make_unique()));
2.4 创建稀疏优化器SparseOptimizer
g2o::SparseOptimizer optimizer; // 稀疏优化器，图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
2.5 往图中增加顶点和边，并添加到SparseOptimizer
// 往图中增加顶点
CurveFittingVertex *vertex = new CurveFittingVertex();
vertex->setEstimate(待优化值); // 设置优化初始值
vertex->setId(0); // 设置顶点ID
optimizer.addVertex(vertex); // 向稀疏优化器添加顶点
// 往图中增加边
for (int i = 0; i < N; i++){
CurveFittingEdge *edge = new CurveFittingEdge(边);
edge->setId(i);
edge->setVertex(0, vertex); // 设置连接的顶点
edge->setMeasurement(观测数值); // 设置观测数值
edge->setInformation(信息矩阵); // 设置信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge); // 向稀疏优化器添加边
}
3. 调用g2o进行优化，返回结果
设置初始值和优化次数，执行优化。
optimizer.initializeOptimization(); // 设置优化初始值
optimizer.optimize(10); // 设置优化次数
estimate = vertex->estimate(); // 执行优化
实例
对曲线 y = exp( a x 2 + b x + c \mathbf{ax}^{2}+bx+c ax2+bx+c) 进行优化，待估计参数为 a,b,c , 观测数据为 y, 误差函数为 e = y - exp( a x 2 + b x + c \mathbf{ax}^{2}+bx+c ax2+bx+c)。
其中构成g2o的顶点为
a,b,c ，自定义顶点时， setToOriginImpl 函数设定被优化变量的原始值 ，为0,0,0，oplusImpl 函数对估计值进行更新调整。边为误差 e = y - exp( a x 2 + b x + c ) \mathbf{ax}^{2}+bx+c) ax2+bx+c) ，自定义边时，computeError 函数是使用当前顶点的值 a,b,c
计算的测量值 exp( a x 2 + b x + c ) \mathbf{ax}^{2}+bx+c) ax2+bx+c) 与真实的测量值 y 之间的误差，每个误差项对于状态变量的导数，即边的误差对顶点的偏导数为
∂ e i ∂ a = − x i 2 e x p ( a x i 2 + b x i + c ) = − x i 2 y \frac{\partial\mathbf{e}_{i}}{\partial a} = -\mathbf{x}_{i}^{2}exp(a\mathbf{x}_{i}^{2} + b\mathbf{x}_{i}+c) = -\mathbf{x}_{i}^{2}y ∂a∂ei=−xi2exp(axi2+bxi+c)=−xi2y,
∂ e i ∂ b = − x i e x p ( a x i 2 + b x i + c ) = − x i y \frac{\partial\mathbf{e}_{i}}{\partial b} = -\mathbf{x}_{i}exp(a\mathbf{x}_{i}^{2} + b\mathbf{x}_{i}+c) = -\mathbf{x}_{i}y ∂b∂ei=−xiexp(axi2+bxi+c)=−xiy,
∂ e i ∂ c = − e x p ( a x i 2 + b x i + c ) = = − y \frac{\partial\mathbf{e}_{i}}{\partial c} = -exp(a\mathbf{x}_{i}^{2} + b\mathbf{x}_{i}+c) = = -y ∂c∂ei=−exp(axi2+bxi+c)==−y,
于是雅克比矩阵 J i = [ ∂ e i ∂ a , ∂ e i ∂ b , ∂ e i ∂ c ] T \mathbf{J}_{i} = \mathbf{[\frac{\partial\mathbf{e}_{i}}{\partial a}, \frac{\partial\mathbf{e}_{i}}{\partial b}, \frac{\partial\mathbf{e}_{i}}{\partial c}]}^{T} Ji=[∂a∂ei,∂b∂ei,∂c∂ei]T，自定义边时，linearizeOplus 函数是在当前顶点的值下，该误差对优化变量的偏导数，即 jacobian 矩阵。
/*
* @Author: LXJ
* @Date: 2021-10-19 23:23:33
* @LastEditTime: 2022-02-10 17:12:31
* @LastEditors: LXJ
* @Description: the curve fitting program by g2o.
* @FilePath: /vslam14/ch10/g2oCurveFitting.cpp
*/
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
// 存取数据
void write(double n)
{
ofstream outFile;
outFile.open("/home/lxj/vslam14/ch11/data.txt", ofstream::app);
outFile << n << ",";
outFile.close();
}
// 打印容器vector里的数据
void func(double n)
{
cout << n << ",";
}
//todo 1.定义顶点和边的类型
//todo 2.构建图
//todo 3.选择优化算法
//todo 4.调用g2o进行优化，返回结果
// 顶点为优化变量，边为误差项
// 待估计的参数构成顶点，观测数据构成了边（位姿R,t构成顶点，像素点p构成了边）
// 误差定义在边内，边附着在顶点上（e = x - f(x)）
// 误差关于顶点的偏导数定义在边里的雅克比矩阵J上，而顶点的更新通过内置的oplusImpl函数实现更新
// 在VSLAM里，相机的位姿与观测的路标构成图优化的顶点，相机的运动和路标的观测构成图优化的边
// ******************************************************************** /
// | 问题 | y = exp(a*x*x+b*x+c) | VSLAM |
// | 待估计参数 | a,b,c | 位姿R,t 路标点P |
// | 观测数据 | y | 像素点p |
// | 误差 | e = y-exp(a*x*x+b*x+c) | e = x - f(x) |
// ******************************************************************** /
// 拟合曲线模型的顶点，模板参数：优化变量（待估计参数）维度和数据类型
// 顶点需要继承BaseVertex，维度为3，数据类型为Vector3d
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d>{
public:
// 固定格式，表示数据对齐
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 读写操作，按需自己定义
// 存盘和读盘：留空
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
// 设置初始值
// 顶点重置函数
virtual void setToOriginImpl() override{
_estimate << 0, 0, 0;
}
// 更新方式
// 顶点更新函数
// 顶点的更新通过内置的oplusImpl函数实现更新
virtual void oplusImpl(const double *update) override{
_estimate += Eigen::Vector3d(update);
}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
// 边需要继承BaseUnaryEdge，维度为1，数据类型为double
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>{
public:
// 固定格式，表示数据对齐
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 读写操作，按需自己定义
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
// 误差计算函数
virtual void computeError() override{
const CurveFittingVertex *v = static_cast(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();// 待估计参数
// e = y - exp(a*x*x+b*x+c)
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// 计算雅克比矩阵
// 误差关于顶点的偏导数定义在边里的雅克比矩阵J上
virtual void linearizeOplus() override{
const CurveFittingVertex *v = static_cast(_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;
}
public:
double _x; // x 值， y 值为 _measurement
};
// 求解 y = exp(ax*x+bx+c)+w
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;
// 根据模型生成x,y真值
vector 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));
}
cout << "x = ";
for_each(x_data.begin(), x_data.end(), func);
for_each(x_data.begin(), x_data.end(), write);
cout << "\n";
cout << "y = ";
for_each(y_data.begin(), y_data.end(), func);
for_each(y_data.begin(), y_data.end(), write);
cout << "\n";
//for_each(x_data.begin(), x_data.end(), [](const double &i){cout << i << "," ;});
//cout << "\n";
//for_each(y_data.begin(), y_data.end(), [](const double &i){cout << i << ",";});
//cout << "\n";
// 1.创建矩阵块求解器BlockSolver
// 2.创建一个线性方程求解器LinearSolver
// 3.创建总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化
// 4.创建稀疏优化器SparseOptimizer
// 5.往图中增加顶点和边，并添加到SparseOptimizer
// 构建图优化，先设定g2o
typedef g2o::BlockSolver> BlockSolverType; // 矩阵块求解器
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));
g2o::SparseOptimizer optimizer; // 稀疏优化器，图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex *vertex = new CurveFittingVertex();
vertex->setEstimate(Eigen::Vector3d(ae, be, ce)); // 设置优化初始值
vertex->setId(0);
optimizer.addVertex(vertex);
// 往图中增加边
for (int i = 0; i < N; i++){
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
edge->setId(i);
edge->setVertex(0, vertex); // 设置连接的顶点
edge->setMeasurement(y_data[i]); // 观测数值
edge->setInformation(Eigen::Matrix::Identity() * 1 /(w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge);
}
// 执行优化start
cout << "-- start g2o 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 time_used = chrono::duration_cast>(t2 - t1);
cout << "-- solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值
Eigen::Vector3d abc_estimate = vertex->estimate();
cout << "-- estimated model: " << abc_estimate.transpose() << endl;
return 0;
}
程序结果如图所示：
C++程序里已经将曲线的数据存入txt文件，只需读入即可对曲线数据进行图像拟合（偷懒没写，直接copy数据），使用scipy库中的最小二乘函数leastsq(),程序如下：
# coding:UTF-8
'''
Author: LXJ
Date: 2021-10-19 21:28:29
LastEditTime: 2022-02-10 23:23:42
LastEditors: LXJ
Description: drawing the curve fitting picture by the functon leastsq
FilePath: /draw/draw.py
'''
import numpy as np
import scipy as sp
import matplotlib.pyplot as plt
from scipy.optimize import leastsq
#* 样本数据(xi, yi)
xi = np.array([0,0.01,0.02,0.03,0.04,0.05,0.06,0.07,0.08,0.09,0.1,0.11,0.12,0.13,0.14,0.15,0.16,0.17,0.18,0.19,0.2,
0.21,0.22,0.23,0.24,0.25,0.26,0.27,0.28,0.29,0.3,0.31,0.32,0.33,0.34,0.35,0.36,0.37,0.38,0.39,0.4,0.41,
0.42,0.43,0.44,0.45,0.46,0.47,0.48,0.49,0.5,0.51,0.52,0.53,0.54,0.55,0.56,0.57,0.58,0.59,0.6,0.61,0.62,
0.63,0.64,0.65,0.66,0.67,0.68,0.69,0.7,0.71,0.72,0.73,0.74,0.75,0.76,0.77,0.78,0.79,0.8,0.81,0.82,0.83,
0.84,0.85,0.86,0.87,0.88,0.89,0.9,0.91,0.92,0.93,0.94,0.95,0.96,0.97,0.98,0.99])
yi = np.array([2.71828,2.93161,2.12942,2.46037,4.18814,2.73368,2.42751,3.44729,3.72543,2.1358,4.12333,3.38199,4.81164,
1.62582,1.76862,3.21555,3.0922,5.82752,4.29855,2.74081,5.75724,3.53729,1.95514,2.99195,3.28739,4.70749,
6.24365,5.81645,4.88402,4.75991,7.25246,5.92933,7.00306,5.22286,5.16179,7.26191,6.40545,6.25549,6.56094,6.53523,
8.14891,7.77616,7.40141,8.75638,7.20606,7.57795,8.21564,9.84032,6.96725,9.90619,9.27125,9.87567,10.3412,9.55315,
11.3635,10.8815,13.0648,11.4756,11.337,13.2393,13.5299,14.0441,13.31,13.672,14.8504,14.2599,14.7724,17.4339,17.4632,
17.7598,16.8223,19.9468,20.5446,21.3767,20.1435,20.3088,23.2543,23.4349,22.8706,24.094,25.4183,25.5237,27.9738,28.5861,
29.5703,29.6744,32.667,34.2698,33.5124,36.1479,39.2485,40.988,41.5716,41.3686,44.285,42.8312,47.7941,48.5931,51.8487,51.0258])
print("xi_size = ", xi.size, "yi_size = ", yi.size)
# y = a*x*x+b*x+c
# 拟合函数
def function(p, x):
#k, b = p
#return k*x+b
a, b, c = p #! 待估计变量
return a*x*x+b*x+c
# 偏差函数
def error(p, x, y):
return function(p, x) - y
# 待估计变量的初值
p0 = [0, 0, 0]
# 最小二乘函数
Para = leastsq(error, p0, args=(xi, yi))# 把error函数中除了p0以外的参数打包到args中
# 读取结果
a, b, c = Para[0]
print("a = ", a, "b = ", b, "c = ", c)
print("cost: " + str(Para[1]))
print("y = "+str(round(a,2))+"x*x+"+str(round(b,2))+"x+"+str(round(c,2)))
# 画样本点
plt.figure(figsize = (8,6))# 指定图像比例
plt.scatter(xi, yi, color = "green", label = "data", linewidth = 2)
#plt.show()
x = np.linspace(0, 1.0, 100)
y = a*x*x+b*x+c # 函数方程
plt.plot(x, y, color = "red", label = "fitter", linewidth = 2)
plt.legend() # 绘制图例
plt.savefig("/home/lxj/vslam14/ch11/picture", dpi = 300)
plt.show()
拟合图像如下：
gitee链接：https://gitee.com/neu-real-lxj/slam_-moudle/tree/master/slam_moudle
视觉SLAM的一些模块实现，包括特征提取、特征匹配、三角化、点云配准、VO、相关库的操作等，希望点个小心心，谢谢