视觉Slam14讲笔记第6讲非线性优化

自然数e2025-10-18 8:50

一、g2o方法使用过程中的问题

由于自身编译器是c++14标准，但是g2o的版本是17标准。导致引用g2o库时有相关报错信息，比如：

bash 复制代码 
g2o/stuff/tuple_tools.h:41:71: error: type/value mismatch at argument 1 in template parameter list for 'template<long unsigned int _Num> using make_index_sequence = std::make_integer_sequence<long unsigned int, _Num>'
   41 |       f, t, i, std::make_index_sequence<std::tuple_size<std::decay<T>>>());​

等等。

可做如下几个操作，
1.在编译g2o库时强制使用c++14标准，并且指定相应的安装路径【如果环境只有一个g2o版本，可以不用这个操作】

bash 复制代码 
cmake ..   -DCMAKE_CXX_STANDARD=14   -DCMAKE_CXX_STANDARD_REQUIRED=ON   -DCMAKE_BUILD_TYPE=Release   -DCMAKE_INSTALL_PREFIX=【安装目录】/slambook2-master/3rdparty/g2o/install   -DG2O_USE_CXX11_ABI=ON   -DG2O_BUILD_EXAMPLES=OFF   -DG2O_BUILD_APPS=OFF

2.在编译使用g2o非线性优化的代码时，指定目录去寻找g2o以及相应的库

bash 复制代码 
project(g2oCurveFitting)

cmake_minimum_required(VERSION 3.10)

find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)


set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

set(G2O_INCLUDE_DIRS "--------/slam14/slambook2-master/3rdparty/g2o/install/include")
set(G2O_LIBRARIES 
    ---------/slam14/slambook2-master/3rdparty/g2o/install/lib/libg2o_core.so
    ---------/slam14/slambook2-master/3rdparty/g2o/install/lib/libg2o_stuff.so
)

include_directories(${EIGEN3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS} ${G2O_INCLUDE_DIRS})


add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_LIBRARIES})

3.相应的修改代码

最终执行代码如下：

cpp 复制代码 
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

#include <Eigen/Core>
#include <chrono>
#include <cmath>
#include <iostream>
#include <opencv2/core/core.hpp>

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::LinearSolverEigen<BlockSolverType::PoseMatrixType>
      LinearSolverType;  // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
      std::make_unique<BlockSolverType>(std::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;
}

执行结果为：

bash 复制代码 
start optimization
iteration= 0     chi2= 376785.128234     time= 0.00107027        cumTime= 0.00107027     edges= 100     schur= 0
iteration= 1     chi2= 35673.566018      time= 0.000846484       cumTime= 0.00191675     edges= 100     schur= 0
iteration= 2     chi2= 2195.012304       time= 0.000880363       cumTime= 0.00279711     edges= 100     schur= 0
iteration= 3     chi2= 174.853126        time= 0.000863566       cumTime= 0.00366068     edges= 100     schur= 0
iteration= 4     chi2= 102.779695        time= 0.000838804       cumTime= 0.00449948     edges= 100     schur= 0
iteration= 5     chi2= 101.937194        time= 0.000842615       cumTime= 0.0053421      edges= 100     schur= 0
iteration= 6     chi2= 101.937020        time= 0.000968364       cumTime= 0.00631046     edges= 100     schur= 0
iteration= 7     chi2= 101.937020        time= 0.000840048       cumTime= 0.00715051     edges= 100     schur= 0
iteration= 8     chi2= 101.937020        time= 0.000848594       cumTime= 0.00799911     edges= 100     schur= 0
iteration= 9     chi2= 101.937020        time= 0.000839935       cumTime= 0.00883904     edges= 100     schur= 0
solve time cost = 0.0102802 seconds. 
estimated model: 0.890912   2.1719 0.943629

二、手写高斯牛顿代码以及执行结果

cpp 复制代码 
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace Eigen;

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

  // 开始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
    Vector3d b = Vector3d::Zero();  // bias
    cost = 0;

    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];
      double error = yi - exp(ae * xi * xi + be * xi + ce);

      Vector3d J;  // 雅克比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);
      J[2] = -exp(ae * xi * xi + be * xi + ce);

      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 << endl;
      break;
    }

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

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose()
         << "\t\testimated parmas: " << 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 << "time used: " << time_used.count() << " seconds. " << endl;
  cout << "estimated params: " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

结果如下：

bash 复制代码 
total cost: 3.19575e+06,                update: 0.0455771  0.078164 -0.985329           estimated parmas: 2.04558,-0.921836,4.01467
total cost: 376785,             update:  0.065762  0.224972 -0.962521           estimated parmas: 2.11134,-0.696864,3.05215
total cost: 35673.6,            update: -0.0670241   0.617616  -0.907497                estimated parmas: 2.04432,-0.0792484,2.14465
total cost: 2195.01,            update: -0.522767   1.19192 -0.756452           estimated parmas: 1.52155,1.11267,1.3882
total cost: 174.853,            update: -0.537502  0.909933 -0.386395           estimated parmas: 0.984045,2.0226,1.00181
total cost: 102.78,             update: -0.0919666   0.147331 -0.0573675                estimated parmas: 0.892079,2.16994,0.944438
total cost: 101.937,            update: -0.00117081  0.00196749 -0.00081055             estimated parmas: 0.890908,2.1719,0.943628
total cost: 101.937,            update:   3.4312e-06 -4.28555e-06  1.08348e-06          estimated parmas: 0.890912,2.1719,0.943629
total cost: 101.937,            update: -2.01204e-08  2.68928e-08 -7.86602e-09          estimated parmas: 0.890912,2.1719,0.943629
cost: 101.937 >= last cost: 101.937
time used: 0.00635663 seconds. 
estimated params: 0.890912, 2.1719, 0.943629

三、用ceres库实现非线性优化

代码

cpp 复制代码 
#include <ceres/ceres.h>

#include <chrono>
#include <iostream>
#include <opencv2/core/core.hpp>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
  // 残差的计算
  template <typename T>
  bool operator()(const T* const abc, T* residual) const {
    // y-exp(ax^2+bx+c)
    residual[0] =
        T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
    return true;
  }

  const double _x, _y;  // 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));
  }

  double abc[3] = {ae, be, ce};

  // 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < N; i++) {
    // 使用自动推导，模板参数：误差类型、输出维度、输入维度，维数要与前面struct一致
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
            new CURVE_FITTING_COST(x_data[i], y_data[i])),
        nullptr, abc);
  }

  // 配置求解器
  ceres::Solver::Options options;
  // 增量方程如何求解
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);  // 开始优化

  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 << summary.BriefReport() << endl;
  cout << "estimated a, b, c = ";
  for (auto a : abc) cout << a << " ";
  cout << endl;

  return 0;
}

执行结果：

bash 复制代码 
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.597873e+06    0.00e+00    3.52e+06   0.00e+00   0.00e+00  1.00e+04        0    4.80e-04    1.00e-03
   1  1.884440e+05    1.41e+06    4.86e+05   9.88e-01   8.82e-01  1.81e+04        1    8.84e-04    1.91e-03
   2  1.784821e+04    1.71e+05    6.78e+04   9.89e-01   9.06e-01  3.87e+04        1    4.86e-04    2.40e-03
   3  1.099631e+03    1.67e+04    8.58e+03   1.10e+00   9.41e-01  1.16e+05        1    5.10e-04    2.92e-03
   4  8.784938e+01    1.01e+03    6.53e+02   1.51e+00   9.67e-01  3.48e+05        1    4.62e-04    3.38e-03
   5  5.141230e+01    3.64e+01    2.72e+01   1.13e+00   9.90e-01  1.05e+06        1    4.67e-04    3.85e-03
   6  5.096862e+01    4.44e-01    4.27e-01   1.89e-01   9.98e-01  3.14e+06        1    4.63e-04    4.32e-03
   7  5.096851e+01    1.10e-04    9.53e-04   2.84e-03   9.99e-01  9.41e+06        1    4.62e-04    4.78e-03
solve time cost = 0.00486601 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a, b, c = 0.890908 2.1719 0.943628
相关推荐
点云SLAM12 小时前
弱纹理图像特征匹配算法推荐汇总
人工智能·深度学习·算法·计算机视觉·机器人·slam·弱纹理图像特征匹配
放羊郎15 小时前
基于萤火虫+Gmapping、分层+A*优化的导航方案
人工智能·slam·建图·激光slam
放羊郎1 天前
基于ROS2的语义格栅地图导航
人工智能·slam·建图·激光slam
WWZZ20257 天前
快速上手大模型：深度学习3（实践：线性神经网络Softmax）
人工智能·深度学习·神经网络·机器人·大模型·slam·具身感知
WWZZ20257 天前
快速上手大模型：深度学习４（实践：多层感知机）
人工智能·深度学习·计算机视觉·机器人·大模型·slam·具身智能
放羊郎9 天前
SLAM各类算法特点对比
人工智能·算法·slam·视觉slam·建图·激光slam
放羊郎11 天前
基于三维点云图的路径规划
人工智能·动态规划·slam·点云·路径规划·激光slam
求索小沈11 天前
ubuntu22.04 ros2 fast_lio2 复现
slam·ros2·重定位·建图·mid360·fast_lio2
WWZZ202512 天前
快速上手大模型：深度学习2（实践：深度学习基础、线性回归）
人工智能·深度学习·算法·计算机视觉·机器人·大模型·slam
WWZZ202517 天前
快速上手大模型：机器学习6（过拟合、正则化）
人工智能·算法·机器学习·计算机视觉·机器人·slam·具身感知
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03BongoCat - 跨平台键盘猫动画工具04综合整理：pdf预览显示：你尝试预览的文件可能对你的计算机有害。如果你信任此文件以及其来源，请打开此文件以看其内容，如何解决以正常预览文件05Linux下V2Ray安装配置指南06jdk21下载、安装（Windows、Linux、macOS）07安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）08npm使用国内淘宝镜像的方法09PyCharm 社区版全平台安装指南10NVIDIA显卡驱动、CUDA、cuDNN 和 TensorRT 版本匹配指南