【SLAM模块】g2o 曲线拟合实例

dulu_202

已于 2022-02-16 22:04:25 修改

阅读量2.9k

点赞数 3

分类专栏： SLAM 文章标签：计算机视觉算法人工智能 slam

于 2022-02-11 00:32:02 首次发布

本文链接：https://blog.youkuaiyun.com/dulu_301/article/details/122871382

版权

SLAM 专栏收录该内容

5 篇文章

订阅专栏

本文详细介绍了如何利用g2o库进行曲线拟合，首先阐述了g2o的基本概念和特点，然后逐步讲解了构建图优化模型的流程，包括自定义顶点和边类型、设置求解器、构建稀疏优化器以及添加顶点和边。通过一个具体的曲线y=exp(ax^2+bx+c)的优化实例，展示了如何定义顶点和边的计算函数，以及如何进行优化。最后，给出了C++代码实现和优化结果，并对比了使用scipy库的最小二乘法进行拟合的图像。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

【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<g2o::BlockSolverTraits<3, 1>> BlockSolverType;                   // 矩阵块求解器

2.2 创建线性方程求解器LinearSolver

typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;         // 线性求解器

2.3 创建总求解器solver，并从GN,LM,Dogleg优化算法中选一个，再用上述块求解器BlockSolver初始化

// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

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( $\mathbf{ax}^{2}+bx+c$ ) 进行优化，待估计参数为 a,b,c , 观测数据为 y, 误差函数为 e = y - exp( $\mathbf{ax}^{2}+bx+c$ )。
其中构成g2o的顶点为
a,b,c ，自定义顶点时， setToOriginImpl 函数设定被优化变量的原始值，为0,0,0，oplusImpl 函数对估计值进行更新调整。边为误差 e = y - exp( $\mathbf{ax}^{2}+bx+c)$ ，自定义边时，computeError 函数是使用当前顶点的值 a,b,c
计算的测量值 exp( $\mathbf{ax}^{2}+bx+c)$ 与真实的测量值 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$ ,
$\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$ ,
$\frac{\partial\mathbf{e}_{i}}{\partial c} = -exp(a\mathbf{x}_{i}^{2} + b\mathbf{x}_{i}+c) = = -y$ ,
于是雅克比矩阵 $\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}$ ，自定义边时，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 <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;

// 存取数据
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<const CurveFittingVertex *>(_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<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;
    }

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<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));
    }
    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<g2o::BlockSolverTraits<3, 1>> BlockSolverType;              // 矩阵块求解器
    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 *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<double, 1, 1>::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<double> time_used = chrono::duration_cast<chrono::duration<double>>(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、相关库的操作等，希望点个小心心，谢谢