slam 基本算法 --- 分别使用【高斯牛顿，g2o】进行曲线拟合（理论+实践）

本文链接：https://blog.youkuaiyun.com/CGJustDoIT/article/details/108005962

本文深入探讨并实践了SLAM领域中使用高斯牛顿法及g2o库进行曲线拟合的方法，通过具体实例讲解了两种算法的实现细节与优化流程。

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

slam 基本算法 --- 分别使用【高斯牛顿，g2o】进行曲线拟合（理论+实践）

一. 曲线拟合 --- 高斯牛顿
二. 曲线拟合 --- g2o

通过本次简单实践，详解高斯牛顿和g2o的应用
参照《slam14讲第二版》

一. 曲线拟合 — 高斯牛顿

1.1 问题描述

问题：求解一条满足以下方程的曲线
$y = exp(ax^2 +bx +c) + w$

$a, b, c$ 为待求的参数， $w$ 为高斯噪声，服从 $(0,\sigma^2)$ ;

构建最小二乘问题：
假设我们有 N 个关于 x, y 的观测数据点，想根据这些数据点求出曲线的参数。那么，可以求解下面的最小二乘问题以估计曲线参数：
$F=\min _{a, b, c} \frac{1}{2} \sum_{i=1}^{N}\left\|y_{i}-\exp \left(a x_{i}^{2}+b x_{i}+c\right)\right\|^{2}$

使用高斯牛顿法，那么我们的目标函数为 $f_i=e_i=y_{i}-\exp \left(a x_{i}^{2}+b x_{i}+c\right)$ ，即误差项；待优化的状态量为： $\chi=\begin{bmatrix}a\\b\\c\end{bmatrix}$ ；
求出每个误差项对状态变量的导数，即可以构成雅克比矩阵，维数为 $1\times3$ ,即：
$J_i=\begin{bmatrix}\frac{\partial e_i}{\partial a}\\\frac{\partial e_i}{\partial b}\\\frac{\partial e_i}{\partial c}\end{bmatrix}$
那么高斯牛顿法的增量方程为： $\left(\sum_{i=1}^NJ_i{(\sigma^2)}^{-1}J_i^T\right)\bigtriangleup\chi_k=\sum_{i=1}^N-J_i{(\sigma^2)}^{-1}e_i$

$N$ 为生成的噪声数据点数量，代码中取 100；
$\bigtriangleup\chi_k$ 表示第 $k$ 次迭代时，状态量的增量值，需要求出的；
$\sigma^2$ 为方差，是因为上面的观测噪声为 $w$ 为一位高斯分布，所以是方差；
然而slam问题中则是协方差矩阵 $\mathbf \Sigma$ ，为什么存在这个协方差矩阵呢？那是因为状态量与观测值之间是不相等的，并且其中的噪声为高维高斯分布，（后面对这个问题再详细分析，目前受知识所限）

1.2 代码

根据方程，生成噪声数据
给状态量设定初始值
高斯牛顿迭代，使用 $ldl^T$ 求解出线性方程 $\bigtriangleup\chi_k=b$ ，找到最优的状态量
最后使用 pcl plotter 画图

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/pcl_plotter.h>

using namespace std;
using namespace Eigen;
using namespace pcl;

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随机数产生器

  // 定义点云使用的格式：这里用的是XYZRGB
  // typedef pcl::PointXYZRGB PointT;
  // typedef pcl::PointCloud<PointT> PointCloud;

  // pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);

  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));

    // PointT p;
    // p.x = x;
    // p.y = exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma);
    // p.z = 1;

    // cloud->points.push_back(p);
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;          // 迭代次数
  double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++)
  {

    Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero(); // bias
    cost = 0;

    for (int i = 0; i < N; i++)
    {
      double xi = x_data[i], yi = y_data[i]; // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J;                                         // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);      // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);           // de/dc

      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;

      cost += error * error;
    }

    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0]))
    {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost)
    {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() << "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  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;

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;

  visualization::PCLPlotter *plot_(new visualization::PCLPlotter("curve fitting by gaussNewton"));
  plot_->setBackgroundColor(1, 1, 1);
  plot_->setTitle("curve fitting by gaussNewton");
  plot_->setXTitle("Elevation");
  plot_->setYTitle("Point number");

  vector<double> y_estimate_data; // 数据
  vector<double> y_truth_date;    // 数据
  for (int i = 0; i < x_data.size(); i++)
  {
    double xi = x_data[i]; // 第i个数据点
    double yi = exp(ae * xi * xi + be * xi + ce);
    y_estimate_data.push_back(yi);
    y_truth_date.push_back(exp(ar * xi * xi + br * xi + cr));
  }
  plot_->addPlotData(x_data, y_truth_date, "truth-line", vtkChart::LINE); //X,Y均为double型的向量

  plot_->addPlotData(x_data, y_estimate_data, "estimate-line", vtkChart::LINE); //X,Y均为double型的向量

  plot_->addPlotData(x_data, y_data, "rander-point", vtkChart::POINTS); //X,Y均为double型的向量
  plot_->plot();                                                        //绘制曲线
  return 0;
}

1.3 结果

在这里插入图片描述

二. 曲线拟合 — g2o

2.1 问题描述

g2o 是一个基于图优化的库,图优化是一种将非线性优化与图论结合起来的理论；
图优化，是把优化问题表现成图（Graph）（指图论中的图）的一种方式，一组优化变量和变量之间的误差项，使用图来图来描述，使它们之间的关系更加直观化；

用顶点表示优化变量，用边表示误差项

在这里插入图片描述

三角形和圆圈，都是顶点，待优化的变量；
红色与蓝色的线，都是边，描述相连顶点优化变量关系，也就是误差项；

在这里插入图片描述

曲线拟合对应的图优化模型；
整个问题只有一个顶点：曲线模型的参数 a, b, c；而每个带噪声的数据点，构成了一个个误差项，也就是图优化的边；
它们是一元边，即只连接一个顶点；

图优化中一条边可以连接一个、两个或多个顶点，这主要反映在每个误差与多少个优化变量有关对应超边，与超图；

g2o求解步骤：

定义顶点和边的类型；
构建图；
选择优化算法；
调用 g2o 进行优化，返回结果。

2.2 代码

在g2o中，内置了三大类：

VertexSE3Expmap，这个表示李代数的位姿；顶点
VertexSBAPointXYZ，表示空间点的位置；顶点
EdgeProjectXYZ2UV，表示投影方程的边。边

没有和曲线拟合相关的，所以我们需要自定义顶点和边；内置类的定义可以去看源码；

#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;

定义顶点

要点：（初始值设置 setToOriginImpl，以及状态量更新方式 oplusImpl ）

class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d>
定义曲线拟合顶点；继承基础顶点类；3表示顶点代表的待优化变量为3维，数据类型是Eigen::Vector3d；
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
表示数据对齐，固定格式; 类成员变量如果是固定大小对象需要加上此句；
而对于动态变量(例如Eigen::VectorXd)会动态分配内存，因此会自动地进行内存对齐
setToOriginImpl()
设置顶点初始估计的值, 估计值变量为 _estimate；或是重置状态值；重载内置函数;
oplusImpl(const double *update)
重载状态量更新方式； _estimate += Eigen::Vector3d(update) ,这里为三维向量相加；在slam中则为：更新小量乘以估计值，左乘更新，如：
```
virtual void oplusImpl(const double* update_)  {
Eigen::Map<const Vector6d> update(update_);
setEstimate(SE3Quat::exp(update)*estimate()); }
```
读写操作按需自己定义；这里留空
bool read(std::istream& is);
bool write(std::ostream& os) const;

//顶点 // 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
  // 类成员变量如果是固定大小对象需要加上 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // 而对于动态变量(例如Eigen::VectorXd)会动态分配内存，因此会自动地进行内存对齐
  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 {}
};

定义边

要点：（初始值设置 setToOriginImpl，以及状态量更新方式 oplusImpl ）

class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>
定义曲线拟合边；继承一元边类；
1表示观测值是1维的，也就是误差项也是1维的，因为误差项也就是理论值/估计值减去实际观测值，当然和观测值维数一样；数据类型为 double；
CurveFittingVertex 表示边连接的顶点的数据类型；因为是一元边，所以只有一个顶点类型；
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
边的构造函数；给类成员变量_x 赋值；给每条边对象存储一个_x值；
computeError()函数
描述了error的计算过程，即观测的值，减去计算出的值；（实际观测值减去理论计算值）
_measurement 测量值变量；因为误差是一维的所以赋值时是： _error(0, 0)；
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *>(_vertices[0]) ;
_vertices[0] 对应 “class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>”绑定的第一个顶点 CurveFittingVertex；
在main函数中 edge->setVertex(0, v);：第一个参数 0 表示将此边连接到 CurveFittingVertex类型节点上，本问题中只有一个顶点v，后面就只接了 v；
linearizeOplus()
书写雅克比矩阵；
每个误差项对状态变量的导数，即可以构成雅克比矩阵，对于本问题雅克比矩阵维数为 $1\times3$ ,即： $e_i$ 实际测量值减去理论计算值
$J_i=\begin{bmatrix}\frac{\partial e_i}{\partial a}\\\frac{\partial e_i}{\partial b}\\\frac{\partial e_i}{\partial c}\end{bmatrix}=\begin{bmatrix}\frac{\partial(y_i-\exp\left(ax_i^2+bx_i+c\right))}{\partial a}\\\frac{\partial(y_i-\exp\left(ax_i^2+bx_i+c\right))}{\partial b}\\\frac{\partial(y_i-\exp\left(ax_i^2+bx_i+c\right))}{\partial c}\end{bmatrix}=\begin{bmatrix}-x_i^2\exp\left(ax_i^2+bx_i+c\right)\\-x_i\exp\left(ax_i^2+bx_i+c\right)\\-\exp\left(ax_i^2+bx_i+c\right)\end{bmatrix}$

如果绑定了两个顶点，在linearizeOplus()中则需要写清楚误差分别对于两个顶点的偏导数。第一个为 _jacobianOplusXi,第二个为 _jacobianOplusXj

//边 //误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

mian() —生成观测数据

生成100 个关于x，y的观测数据点

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));
		  }

构建图

要点：

solver 求解器

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
// 每个误差项优化变量维度为3，误差值维度为1；
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
// 线性求解器类型
g2o::OptimizationAlgorithmGaussNewton
// GN

图模型

g2o::SparseOptimizer optimizer;// 图模型

往图中增加顶点

 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);  // 添加
 }

		 // 构建图优化，先设定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);
		  }

执行优化和结果输出

要点：

初始化优化器，设置迭代次数

optimizer.initializeOptimization();
optimizer.optimize(10);
优化结果
Eigen::Vector3d abc_estimate = v->estimate();
Eigen::Vector3d 维数为 $\times 1$ ; 矩阵

		// 执行优化
		  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;
}