ROS小车 Archives - 慢慢的回味

G2O优化解析-自动微分

原创文章，转载请注明： 转载自慢慢的回味

上文G2O优化解析-手动微分我们学过用G2O来实现拟合曲线，但是大多情况下微分并不好计算，因为我们可能根本就不知道目标函数是什么，而是只知道一些断断续续的约束函数。导数主要以梯度和黑森函数的形式存在于机器学习中，自动微分（AD），也称为算法微分或简称“自动微分”，是一系列技术，类似于反向传播，但比反向传播更为普遍，用于高效、准确地计算以计算机程序表示的数值函数的导数。

Content:

通过自动微分计算雅可比矩阵

接上篇G2O优化解析-手动微分，接着解析G2O框架对非线性优化的实现。
继续阅读本作品采用知识共享署名 4.0 国际许可协议进行许可。

G2O优化解析-手动微分

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: G2O优化解析-手动微分

G2O是一个开源的C++框架，用于优化基于图形的非线性误差函数。g2o被设计为易于扩展到各种各样的问题，一个新的问题通常可以在几行代码中指定。
当前的实现为SLAM和BA的几个变体提供了解决方案，机器人技术和计算机视觉中的一系列问题都涉及到最小化可以用图形表示的非线性误差函数，典型的例子是同步定位和映射（SLAM）或束调整（BA）。这些问题的总体目标是最大限度地找到能够解释受到高斯噪声影响后的一组测量数据的参数或状态变量的配置。通过把优化问题设计成图的模式：带优化的变量称为顶点，对于待优量的限制条件为边。限制条件可理解成损失函数，这个函数是优化的关键，我们要通过不断的迭代获取最小的损失，从而推断出优化后的顶点而求解。
G2O是一个开源的C++框架，用于解决非线性最小二乘问题。G2O的性能可与针对特定问题的最先进方法的实现相媲美。本文通过曲线上的点受高斯噪声影响后，利用G2O设计图模型，然后经过手动计算微分实现来重新拟合曲线。

Content:

接上篇非线性优化，接着解析G2O框架对非线性优化的实现，本文的微分计算为手动微分。
继续阅读本作品采用知识共享署名 4.0 国际许可协议进行许可。

非线性优化

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: 非线性优化

这儿转载书籍《视觉SLAM十四讲从理论到实践第2版》第6章的内容来学习非线性优化。在理解高斯牛顿法的时候需要注意如下理解。

#include <iostream>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <g2o/stuff/sampler.h>
 
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;            // 大于1的噪声可以让梯度下降慢一些
  g2o::Sampler::seedRand();
 
  vector<double> x_data, y_data;      // 数据，用于测试的100组数据
  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) + g2o::Sampler::gaussRand(0, 0.02));
  }
 
  // 开始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; // 雅可比矩阵，fx的一阶导数，注意误差函数的x为向量[a, b, c]，和这儿的xi没有关系。
      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();//海森矩阵H
      b += -inv_sigma * inv_sigma * error * J;//error为误差函数的值
 
      cost += error * error;
    }
 
    // 求解线性方程 Hx=b，dx为向量[a, b, c]，即待优化参数。
    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><double> time_used = chrono::duration_cast<chrono::duration</double><double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
 
  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}
</double></chrono></iostream>

#include <iostream> #include <chrono> #include <Eigen/Core> #include <Eigen/Dense> #include <g2o/stuff/sampler.h> 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; // 大于1的噪声可以让梯度下降慢一些 g2o::Sampler::seedRand(); vector<double> x_data, y_data; // 数据，用于测试的100组数据 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) + g2o::Sampler::gaussRand(0, 0.02)); } // 开始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; // 雅可比矩阵，fx的一阶导数，注意误差函数的x为向量[a, b, c]，和这儿的xi没有关系。 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();//海森矩阵H b += -inv_sigma * inv_sigma * error * J;//error为误差函数的值 cost += error * error; } // 求解线性方程 Hx=b，dx为向量[a, b, c]，即待优化参数。 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><double> time_used = chrono::duration_cast<chrono::duration</double><double>>(t2 - t1); cout << "solve time cost = " << time_used.count() << " seconds. " << endl; cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl; return 0; } </double></chrono></iostream>

继续阅读“非线性优化”本作品采用知识共享署名 4.0 国际许可协议进行许可。