.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
)
x_k=f(x_{k-1},u_k,w_k)
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
)
z_{k,j}=h(y_j,x_k,v_{k,j})
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
最基本的SLAM问题：已知运动测量读数 u 以及传感器读数 z 时，如何求解定位问题(估计 x )和建图问题(估计 y )。此时便将SLAM问题建模成了一个状态估计问题。
（1）状态估计问题
1)最大似然估计
运动方程与观测方程亦可以写成：
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
\begin{cases} x_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\\ \end{cases}
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
设两个噪声项均满足零均值的高斯分布(正态分布)，即：
w
k
∼
N
(
0
,
R
k
)
,
v
k
∼
N
(
0
,
Q
k
,
j
)
w_k \sim N(0,R_k) ,v_k \sim N(0,Q_{k,j})
w
k
∼
N
(
0
,
R
k
)
,
v
k
∼
N
(
0
,
Q
k
,
j
)
其中，R，Q为协方差矩阵。
a)整体的分析
定义：所有时刻的机器人位姿和路标点坐标为
x
=
{
x
1
,
x
2
,
⋯
x
k
}
,
y
=
{
y
1
,
y
2
,
⋯
y
N
}
x=\left\{\begin{matrix}x_1,x_2,\cdots x_k\end{matrix}\right\},y=\left\{\begin{matrix}y_1,y_2,\cdots y_N\end{matrix}\right\}
x
=
{
x
1
,
x
2
,
⋯
x
k
}
,
y
=
{
y
1
,
y
2
,
⋯
y
N
}
u表示所有时刻的输入，z表示所有时刻的观测数据。在已知输入数据u和观测数据z的条件下，求x，y的条件概率分布：
P
(
x
,
y
∣
z
,
u
)
P(x,y|z,u)
P
(
x
,
y
∣
z
,
u
)
应用贝叶斯法则：
P
(
x
,
y
∣
z
,
u
)
=
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
P
(
z
,
u
)
P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}
P
(
x
,
y
∣
z
,
u
)
=
P
(
z
,
u
)
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
首先明确，x与y均是假设的位置，并不一定就在那里
例：机器人从
x
i
x_i
x
i
位置，想到
x
i
+
1
x_{i+1}
x
i
+
1
位置
原因：机器人在
x
i
x_i
x
i
这个位置，此处有
y
j
y_j
y
j
路标。
结果：因为在
x
i
x_i
x
i
这个位置，机器人通过传感器测得了观测数据
z
i
z_i
z
i
，同时为接下来的运动输入了
u
u
u
（1）
P
(
x
,
y
∣
z
,
u
)
P(x,y|z,u)
P
(
x
,
y
∣
z
,
u
)
：后验概率，知道结果推原因。看到正确的观测结果，对位置的看法（是否在这个位置）。
（2）
P
(
x
,
y
)
P(x,y)
P
(
x
,
y
)
：先验概率，知道原因推结果，在这个位置（假设）的概率（没有新证据之前）。
（3）
P
(
z
,
u
∣
x
,
y
)
P(z,u|x,y)
P
(
z
,
u
∣
x
,
y
)
：似然概率，在这个位置（假设）符合观测结果（证据）的比例
因为无法求后验分布，所以选择求一个状态最优估计，使得在该状态下后验概率最大化：
(
x
,
y
)
M
A
P
∗
=
a
r
g
m
a
x
P
(
x
,
y
∣
z
,
u
)
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
(x,y)^*_{MAP}=arg\;max\;P(x,y|z,u) = arg\;max\;P(z,u|x,y)P(x,y)
(
x
,
y
)
M
A
P
∗
=
a
r
g
m
a
x
P
(
x
,
y
∣
z
,
u
)
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
由贝叶斯法则，得求解最大后验概率等价于最大化似然和先验的乘积。但是我们不知道机器人位姿与路标，所以缺失先验，因此求解最大似然估计(MLE)：
(
x
,
y
)
M
L
E
∗
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
(x,y)^*_{MLE}= arg\;max\;P(z,u|x,y)
(
x
,
y
)
M
L
E
∗
=
a
r
g
m
a
x
P
(
z
,
u
∣
x
,
y
)
至此，分析的都是所有时刻的x,y,u,z。
b)多维向量的正态分布模型
针对向量的正态分布称为多变量正态分布
x
∼
N
(
μ
,
Σ
)
x\sim N(\mu,\Sigma)
x
∼
N
(
μ
,
Σ
)
， 其概率密度函数为：
p
(
x
)
=
d
e
t
(
2
π
Σ
)
−
1
2
e
−
1
2
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
p(x)=det(2\pi\Sigma)^{-\frac{1}{2}}e^{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)}
p
(
x
)
=
d
e
t
(
2
π
Σ
)
−
2
1
e
−
2
1
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
其中，μ为均值向量，Σ是一个半正定的对称矩阵称为协方差矩阵
covariance matrix
。
取负对数，得：
−
ln
(
p
(
x
)
)
=
1
2
ln
(
2
π
d
e
t
(
Σ
)
)
+
1
2
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
-\ln(p(x)) = \frac{1}{2}\ln(2\pi\;det(\Sigma))+\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)
−
ln
(
p
(
x
)
)
=
2
1
ln
(
2
π
d
e
t
(
Σ
)
)
+
2
1
(
x
−
μ
)
T
Σ
−
1
(
x
−
μ
)
即将求最大似然转化为求最小化后面的二次型。
2)最小二乘问题
c）局部分析
整体的x,y是由无数个独立的xi,yk构成的
P
(
z
,
u
∣
x
,
y
)
=
∏
P
(
u
k
∣
x
k
−
1
,
x
k
)
∏
P
(
z
k
,
j
∣
x
k
,
y
i
)
P(z,u|x,y)=\prod P(u_k|x_{k-1},x_k)\prod P(z_{k,j}|x_k,y_i)
P
(
z
,
u
∣
x
,
y
)
=
∏
P
(
u
k
∣
x
k
−
1
,
x
k
)
∏
P
(
z
k
,
j
∣
x
k
,
y
i
)
此处的公式不知如何得到。
由
w
k
∼
N
(
0
,
R
k
)
,
v
k
∼
N
(
0
,
Q
k
,
j
)
w_k \sim N(0,R_k) ,v_k \sim N(0,Q_{k,j})
w
k
∼
N
(
0
,
R
k
)
,
v
k
∼
N
(
0
,
Q
k
,
j
)
，得
x
k
∼
N
(
f
(
x
k
−
1
,
u
k
)
,
R
k
)
x_k \sim N(f(x_{k-1},u_k),R_k)
x
k
∼
N
(
f
(
x
k
−
1
,
u
k
)
,
R
k
)
，
z
j
,
k
∼
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
z_{j,k} \sim N(h(y_j,x_k),Q_{k,j})
z
j
,
k
∼
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
且令：
e
u
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e_{u,k}=x_k-f(x_{k-1},u_k)
e
u
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e
z
,
j
,
k
=
z
k
,
j
−
h
(
x
k
,
y
i
)
e_{z,j,k}=z_{k,j}-h(x_k,y_i)
e
z
,
j
,
k
=
z
k
,
j
−
h
(
x
k
,
y
i
)
可以求得：
m
i
n
J
(
x
,
y
)
=
∑
e
u
,
k
T
R
k
−
1
e
u
,
k
+
∑
∑
e
z
,
j
,
k
T
Q
k
,
j
−
1
e
z
,
j
,
k
min \;J(x,y)= \sum e_{u,k}^TR_k^{-1}e_{u,k}+\sum \sum e_{z,j,k}^TQ_{k,j}^{-1}e_{z,j,k}
m
i
n
J
(
x
,
y
)
=
∑
e
u
,
k
T
R
k
−
1
e
u
,
k
+
∑
∑
e
z
,
j
,
k
T
Q
k
,
j
−
1
e
z
,
j
,
k
（2）非线性最小二乘
牛顿法：
增量方程：
Δ
x
∗
=
a
r
g
m
i
n
(
F
(
x
)
+
J
(
x
)
T
Δ
x
+
1
2
Δ
x
T
H
Δ
x
)
\Delta x^*=arg \; min(F(x)+J(x)^T\Delta x+\frac{1}{2}\Delta x^TH\Delta x)
Δ
x
∗
=
a
r
g
m
i
n
(
F
(
x
)
+
J
(
x
)
T
Δ
x
+
2
1
Δ
x
T
H
Δ
x
)
求导:
J
+
H
Δ
x
=
0
J+H\Delta x = 0
J
+
H
Δ
x
=
0
高斯牛顿法：
J
(
x
)
J
T
(
x
)
Δ
x
=
−
J
(
x
)
f
(
x
)
J(x)J^T(x)\Delta x=-J(x)f(x)
J
(
x
)
J
T
(
x
)
Δ
x
=
−
J
(
x
)
f
(
x
)
高斯牛顿法的增量方程(程序中应用的)：
(
∑
i
=
1
100
J
i
−
1
J
i
T
)
Δ
x
k
=
∑
i
=
1
100
−
J
i
−
1
e
i
(\sum_{i=1}^{100}J_i^{-1}J_i^T)\Delta x_k = \sum_{i=1}^{100}-J_i^{-1}e_i
(
i
=
1
∑
1
0
0
J
i
−
1
J
i
T
)
Δ
x
k
=
i
=
1
∑
1
0
0
−
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。