视觉SLAM十四讲学习笔记——第六讲 非线性优化（1）

非线性优化算法在SLAM中主要解决的问题：如何在有噪声的数据中进行准确的状态估计。

1.最小二乘问题引出

        首先，将SLAM中的状态估计问题，从概率学的角度分析：已知输入数据u和观测数据z的条件下，求状态x,y的条件概率分布，由贝叶斯法则将条件概率分布表示为“后验概率=似然 * 先验概率”的形式：

        此时状态最优估计问题转化为了一个该状态下后验概率最大化（最大后验分布）问题：

         要最大化后验概率，也就是要最大化似然和先验概率的乘积，先验概率多数情况是很难知道的，因此只需要求最大似然估计：

        最大似然估计的核心思想是：概率最大的事件最有可能发生，在视觉SLAM中表现为：什么样的状态下，最可能产生现在观测到的数据。接下来是通过数学模型：假设噪声项符合零均值的正态分布，因此观测数据的条件概率也满足正态分布，并使用最小化负对数来求解一个高斯分布的最大似然。P123-124对上述过程有详细推导，这里不做赘述，最终得到一个最小二乘问题。至此SLAM中的状态估计问题的分析思路：

 2.最小二乘问题的唯一解？

         P125的批量状态估计得示例中，最后得到一个最小二乘问题，同时作者给出了这个问题的唯一解，在后文中我并没有找到和这一段呼应的内容，这里用自己的方法简单证明用一下：

         首先回顾一下各个变量的定义以及各个矩阵、向量的维度（这个我认为很重要），所有的定义方式和书上相同：

        这部分公式推导出现了一些问题，虽然结果看似是对的，但是事实上属于是强行解释了，主要问题出现在数值函数、向量函数、矩阵函数分别对数值、向量、矩阵的求导之间是不同的，此问题下属于是数值函数对向量求导，因此公式有些不同，这里补充正确的公式，就不在原过程中修改了（文件丢了，不想再敲一遍了...），带入进去还是比较好理解的，注意这里面加粗的是向量，未加粗的是数值函数。实际上公式里的f对应我们的目标函数F，向量函数g对应着误差e，待求的向量都是x。

 

3.手写高斯牛顿法

        这里使用了一个简单的曲线拟合的问题来演示，这部分有详细的分析过程，唯一让我比较困惑的是最终确定的牛顿增量方程：

         其中的是预设的高斯噪声的方差，这一步就显得很奇怪，首先等式左右两侧的对等式完全没有影响，完全抵消掉了；其次在整个高斯牛顿法的分析过程中并没有涉及到这样的内容；最后在实际问题的求解过程中，我们不可能预先知道噪声高斯分布的参数。总之，我个人认为这一步毫无意义，不知道是不是有什么我没有意识到的作用。。。

        最后附上手写高斯牛顿法的代码，也是在ch6/gaussNewton.cpp的基础上加了一些注释，同时测试了一下去掉的效果，结果是完全没有影响，甚至运算速度更快了一些...

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

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;            // Sigma的倒数
  cv::RNG rng;                                 // OpenCV随机数产生器
  //生成含有高斯噪声的数据
  //x_data:0-1 均匀分布
  //y_data:在真实值的基础上增加随机的高斯噪声
  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));
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的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、b
      H +=  J * J.transpose();
      b += - error * J;
      //H +=  J *inv_sigma*inv_sigma* J.transpose();
      //b += - error*inv_sigma*inv_sigma * J;
      // 代价函数
      cost += error * error;
    }

    // 求解线性方程 Hx=b :cholesky分解
    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;
  return 0;
}