视觉SLAM十四讲作业练习（4）非线性优化部分

.5非线性优化

（0）回顾 SLAM问题的数学表达

定义：x：表示自身位置，x1，x2 … xk

​ y: 表示地图的路标，y1, y2 … yn

运动：从 k-1 时刻到 k 时刻，位置 x 如何变化，因此，运动方程即 k 时刻的位置需要借助 k-1 时刻的位置与输入uk确定。即：
$$x_k=f(x_{k-1},u_k,w_k)$$
其中uk为输入，wk为该过程中加入的噪声。

观测：k 时刻在 xk 处探测到某个路标 yj，产生的观测数据为zkj。即：
$$z_{k,j}=h(y_j,x_k,v_{k,j})$$
最基本的SLAM问题：已知运动测量读数 u 以及传感器读数 z 时，如何求解定位问题(估计 x )和建图问题(估计 y )。此时便将SLAM问题建模成了一个状态估计问题。

（1）状态估计问题

1)最大似然估计

运动方程与观测方程亦可以写成：
$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\end{array}$$
设两个噪声项均满足零均值的高斯分布(正态分布)，即：
$$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$$
其中，R，Q为协方差矩阵。

a)整体的分析

定义：所有时刻的机器人位姿和路标点坐标为
$$x=\left\{\begin{array}{c}{x}_1,x_2,\cdots x_k\end{array}\right\},y=\left\{\begin{array}{c}{y}_1,y_2,\cdots y_N\end{array}\right\}$$
u表示所有时刻的输入，z表示所有时刻的观测数据。在已知输入数据u和观测数据z的条件下，求x，y的条件概率分布：
$$P(x,y|z,u)$$
应用贝叶斯法则：
$$P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}$$
首先明确，x与y均是假设的位置，并不一定就在那里

例：机器人从 $x_i$ 位置，想到 $x_{i+1}$ 位置

原因：机器人在 $x_i$ 这个位置，此处有 $y_j$ 路标。

结果：因为在 $x_i$ 这个位置，机器人通过传感器测得了观测数据 $z_i$ ，同时为接下来的运动输入了 $u$

（1）$P(x,y|z,u)$：后验概率，知道结果推原因。看到正确的观测结果，对位置的看法（是否在这个位置）。

（2）$P(x,y)$：先验概率，知道原因推结果，在这个位置（假设）的概率（没有新证据之前）。

（3）$P(z,u|x,y)$：似然概率，在这个位置（假设）符合观测结果（证据）的比例

因为无法求后验分布，所以选择求一个状态最优估计，使得在该状态下后验概率最大化：
$$(x,y{)}_{MAP}^{\ast }=argmaxP(x,y|z,u)=argmaxP(z,u|x,y)P(x,y)$$
由贝叶斯法则，得求解最大后验概率等价于最大化似然和先验的乘积。但是我们不知道机器人位姿与路标，所以缺失先验，因此求解最大似然估计(MLE)：
$$(x,y{)}_{MLE}^{\ast }=argmaxP(z,u|x,y)$$
至此，分析的都是所有时刻的x,y,u,z。

b)多维向量的正态分布模型

针对向量的正态分布称为多变量正态分布$x\sim N(\mu ,\Sigma )$， 其概率密度函数为：
$$p(x)=det(2\pi \Sigma {)}^{-\frac{1}{2}}{e}^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}$$
其中，μ为均值向量，Σ是一个半正定的对称矩阵称为协方差矩阵covariance matrix。

取负对数，得：
$$-\ln (p(x))=\frac{1}{2}\ln (2\pi det(\Sigma ))+\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )$$
即将求最大似然转化为求最小化后面的二次型。

2)最小二乘问题

c）局部分析

整体的x,y是由无数个独立的xi,yk构成的
$$P(z,u|x,y)=\prod P(u_k|x_{k-1},x_k)\prod P(z_{k,j}|x_k,y_i)$$
此处的公式不知如何得到。

由$w_k\sim N(0,R_k),v_k\sim N(0,Q_{k,j})$，得$x_k\sim N(f(x_{k-1},u_k),R_k)$， $z_{j,k}\sim N(h(y_j,x_k),Q_{k,j})$

且令：
$$e_{u,k}=x_k-f(x_{k-1},u_k)$$

$$e_{z,j,k}=z_{k,j}-h(x_k,y_i)$$

可以求得：
$$minJ(x,y)=\sum e_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum \sum e_{z,j,k}^T{Q}_{k,j}^{-1}{e}_{z,j,k}$$

（2）非线性最小二乘

牛顿法：

增量方程：
$$\Delta x^{\ast }=argmin(F(x)+J(x{)}^T\Delta x+\frac{1}{2}\Delta x^{T}H\Delta x)$$
求导:
$$J+H\Delta x=0$$

高斯牛顿法：

$$J(x)J^T(x)\Delta x=-J(x)f(x)$$

高斯牛顿法的增量方程(程序中应用的)：
$$(\sum _{i=1}^{100}{J}_i^{-1}{J}_i^T)\Delta x_k=\sum _{i=1}^{100}-J_i^{-1}{e}_i$$

6.曲线拟合实验

（1）手写高斯牛顿

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

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    // 1.定义真实的参数值与估计值
    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值
    cv::RNG rng;                                 // OpenCV随机数产生器

    // 2.生成一系列的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));
    }

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

    for (int iter = 0; iter < iterations; iter++) {
        //3.1明确目标，求出H与b
        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;
        //3.2根据高斯牛顿算法求
        //每一次迭代的x，y都不同因为ae,be,ce不同
        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            double error = 0;   // 第i个数据点的计算误差
            double ye = (exp(ae*xi*xi + be*xi +ce));
            error = yi - ye; // 填写计算error的表达式 真实值的减去估计值
            Vector3d J; // 雅可比矩阵
            J[0] = -xi*xi*ye;  // de/da
            J[1] = -xi*ye;  // de/db
            J[2] = -ye;  // de/dc

            //H与b都是一个整体的概念
            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
 	// start your code here
        Vector3d dx;
        dx = H.ldlt().solve(b);
		if (dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2] < 0.00001) {
			cout << "Not much improvement. Stop here." << endl;
			break;
		}
	// end your code here

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 3.3迭代的目的是更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

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

（2）使用Ceres库

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>

using namespace std;

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

int main(int argc, char **argv) {
    double a = 1.0, b = 2.0, c = 1.0;     // 真实参数值
    int N = 100;                          // 数据点
    double w_sigma = 1.0;                 // 噪声Sigma值
    cv::RNG rng;                          // OpenCV随机数产生器
    double abc[3] = {0, 0, 0};            // abc参数的估计值

    vector<double> x_data, y_data;        // 数据

    cout << "generating data: " << endl;
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;             // 其实可以改为double x=i/double(N)
        x_data.push_back(x);
        y_data.push_back(exp(a * x * x + b * x + c) + rng.gaussian(w_sigma));
        cout << x_data[i] << " " << y_data[i] << endl;
    }

    // 2.构建最小二乘问题
    ceres::Problem problem;
    for (int i = 0; i < N; i++) {
        problem.AddResidualBlock(     // 向问题中添加误差项
            // 使用自动求导, 模板参数：误差类型, 输出维度, 输入维度(待估计参数维度), 维数要与前面struct中一致
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
                new CURVE_FITTING_COST(x_data[i], y_data[i])
            ),
            nullptr,                 // 核函数, 这里不使用, 为空
            abc                      // 待估计参数
        );
    }

    // 3.配置求解器
    ceres::Solver::Options options;                // 这里有很多配置项可以填
    options.max_num_iterations = 100;
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    ceres::Solve(options, &problem, &summary);     // 开始优化 求解

    // 输出结果
    cout << summary.BriefReport() << endl;
    cout << "estimated a,b,c = ";
    for (auto a:abc) cout << a << " ";
    cout << endl;

    return 0;
}

总结：

​ （1）进一步理解了slam问题的数学表达，引入状态估计，进而引出最小二乘法。

​ （2）主要了解求解最小二乘问题的方法：高斯牛顿法

​ （3）通过手动编写高斯牛顿法的求解程序简单了解运行步骤，简单了解优化库Ceres的使用。

​ （4）不足之处：需要补充一些C++的知识，主要是STL。