.5非线性优化
（0）回顾 SLAM问题的数学表达
定义：x：表示自身位置，x1，x2 … xk
y: 表示地图的路标，y1, y2 … yn
运动：从 k-1 时刻到 k 时刻，位置 x 如何变化，因此，运动方程即 k 时刻的位置需要借助 k-1 时刻的位置与输入uk确定。即：
其中uk为输入，wk为该过程中加入的噪声。
观测：k 时刻在 xk 处探测到某个路标 yj，产生的观测数据为zkj。即：
最基本的SLAM问题：已知运动测量读数 u 以及传感器读数 z 时，如何求解定位问题(估计 x )和建图问题(估计 y )。此时便将SLAM问题建模成了一个状态估计问题。
（1）状态估计问题
1)最大似然估计
运动和观察方程也可以写成：
让两个噪声项都满足均值为零的高斯分布（正态分布），即：
其中，R，Q为协方差矩阵。
a)整体的分析
定义：机器人的位姿和地标坐标在任何时候都是
u表示所有时刻的输入，z表示所有时刻的观测数据。在已知输入数据u和观测数据z的条件下，求x，y的条件概率分布：
应用贝叶斯规则：
首先明确，x与y均是假设的位置，并不一定就在那里
示例：机器人从位置到位置
原因：机器人在处，有
路标。
结果：因为在这个位置，机器人通过传感器测量了观测数据
，输入
进行下一步动作。
（1）：后验概率，知道结果推原因。看到正确的观测结果，对位置的看法（是否在这个位置）。
（2）：先验概率，知道原因推结果，在这个位置（假设）的概率（没有新证据之前）。
（3）：似然概率，在这个位置（假设）符合观测结果（证据）的比例
由于无法找到后验分布，我们选择找到一个状态的最优估计，使该状态下的后验概率最大化：
由贝叶斯法则，得求解最大后验概率等价于最大化似然和先验的乘积。但是我们不知道机器人位姿与路标，所以缺失先验，因此求解最大似然估计(MLE)：
至此，分析的都是所有时刻的x,y,u,z。
b)多维向量的正态分布模型
向量的正态分布称为多元正态分布，其概率密度函数为：
其中，μ为均值向量，Σ是一个半正定的对称矩阵称为协方差矩阵covariance matrix。
取负对数，我们得到：
它是关于将最大似然转换为最小化背后的二次形式。
2)最小二乘问题
c）局部分析
整体的x,y是由无数个独立的xi,yk构成的
我不知道如何在这里获得公式。
从，得到
，
然后让：
你可以得到：
（2）非线性最小二乘
牛顿法：
增量方程：
Guidance:
高斯牛顿法：
高斯-牛顿法的增量方程（程序中使用）：
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;
}
Summarize:
（1）进一步理解了slam问题的数学表达，引入状态估计，进而引出最小二乘法。
（2）主要了解求解最小二乘问题的方法：高斯牛顿法
（3）通过手动编写高斯牛顿法的求解程序简单了解运行步骤，简单了解优化库Ceres的使用。
（4）不足之处：需要补充一些C++的知识，主要是STL。
文章出处登录后可见！
