看问题维度不同，解决问题的方法就不同；看世界的维度不同，活出的人生就不一样。
我用三维软件查看模型，我是不是站在四维空间上（新增了时间）；如果，别人站在我们旁边看我们的操作，那别人是不是站在五维空间上（新增了）；

场景描述

机器人在运动或者静止过程中，处于什么状态，看到什么场景，是融为一体的。
SLAM

获取机器人姿态方法

基本方法：计算足够多的匹配点，不论是2D还是3D，获取机器人的位姿变化；
优化方法：通过大量的点计算最优化结果，得到位姿的最优解。

对比：基本方法需要进行外点剔除，或者筛选内点；优化方法，是一种大量数据最优化解，基本可以忽略场景变化。
其中，优化方法常用的方法有重投影误差和光度误差，这里涉及到今天的主题。

注意：1) 计算初始位姿方法；2)位置丢失状态；

场景路标点优化方法

这里需要保留多帧之间距离较近的特征点和3D点信息，关键问题是保留多少，保留什么。通常采用的两个原则：1）保证两个位姿对应的特征点之间有关联；2）而关联信息计算满足实时性。
这部分可以成为SLAM局部建图。
这里写图片描述

Local优化

这里写图片描述
注意：建立地图世界坐标系；

世界坐标系是系统的绝对坐标系，在没有建立用户坐标系之前画面上所有点的坐标都是以该坐标系的原点来确定各自的位置的。

由于摄像机可安放在环境中的任意位置，在环境中选择一个基准坐标系来描述摄像机的位置，并用它描述环境中任何物体的位置，该坐标系称为世界坐标系。摄像机坐标系与世界坐标系之间的关系可以用旋转矩阵与平移向量来描述。

g2o求解BA

下面来考虑如何使用g2o 求解这个BA 问题。g2o 使用图模型来描述问题的结构，所以我们要用节点来表示相机和路标，然后用边来表示它们之间的观测。
在使用g2o 时，求解的设置有以下几点：1. 使用何种方法（LM, DogLeg）等来定义非线性优化的下降策略；2. 使用哪类线性求解器。
注意：这里需要使用到稀疏性，所以必须选用稀疏的求解器。

Ceres求解BA

Ceres 是用Problem 类型来构建最终的目标函数。

后端优化

我们完全可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，我们就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系了。通过这种方式，我们省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图。

这里写图片描述

回环检测

回环检测对于SLAM 系统意义重大。它关系到我们估计的轨迹和地图在长时间下的正确性。另一方面，由于回环检测提供了当前数据与所有历史数据的关联，在跟踪算法丢失之后，我们还可以利用回环检测进行重定位。因此，回环检测对整个SLAM 系统精度与鲁棒性的提升，是非常明显的。甚至在某些时候，我们把仅有前端和局部后端的系统称为VO，而把带有回环检测和全局后端的称为SLAM。

至此，我们把SLAM前端和后端的实现过程完整的，发现机器人的定位信息，都是为后端优化做准备，最终完成SLAM的跟踪和建图问题。
这里写图片描述

设计过程

优化问题有三个最重要的因素：目标函数、优化变量、优化约束。一个简单的优化问题可以描述如下：

\begin{matrix} (1) & min_{x} F (x) \end{matrix}

$\begin{equation} \min\limits_{x} F(x) \end{equation}$

这里写图片描述
上述，是BA在SLAM的应用场景。Pose Graph应用模型如下：

BA和Pose Graph互补关系：

位姿图：对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的BA。

min E = \sum_{i, j} ‖ x_{i}^{*} - T_{i, j} x_{j}^{*} ‖_{2}^{2}

$\min E = \sum\limits_{i,j} \| x_i^* - T_{i,j} x_j^* \|_2^2$
不论是BA，还是Pose Graph，都可以用图的形式表示，本质问题还是优化问题，优化问题就可以表示成：

所有的位姿顶点和位姿~位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。那么总体目标函数为：

\begin{matrix} (2) & min F (x) = \sum_{k = 1}^{n} e_{k} {(x_{k})}^{T} Ω_{k} e_{k} (x_{k}) \end{matrix}

$\begin{equation} \min F(x) = \sum\limits_{k = 1}^n {{e_k}{{\left( {{x_k}} \right)}^T}{\Omega _k}{e_k}\left( {{x_k}} \right)} \end{equation}$

最终，的求解方程为：

\begin{matrix} (3) & H_{k} Δ x = - b_{k} \end{matrix}

$\begin{equation} H_k \Delta x = -b_k \end{equation}$

图优化工具g2o

英文全称：General Graph Optimization，只要一个优化问题能够表达成图，那么就可以用g2o去求解它。常见的，比如bundle adjustment，ICP，数据拟合，都可以用g2o来做。
这里写图片描述
SparseOptimizer 是我们最终要维护接口函数。它是一个Optimizable Graph，从而也是一个Hyper Graph。一个 SparseOptimizer 含有很多个顶点（都继承自 Base Vertex）和很多个边（继承自 BaseUnaryEdge, BaseBinaryEdge或BaseMultiEdge）。这些 Base Vertex 和 Base Edge 都是抽象的基类，而实际用的顶点和边，都是它们的派生类。我们用 SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向一个图中添加顶点和边，最后调用 SparseOptimizer.optimize 完成优化。
同时，SparseOptimizer 拥有一个 Optimization Algorithm，继承自Gauss-Newton, Levernberg-Marquardt, Powell’s dogleg 三者之一（我们常用的是GN或LM）。同时，这个 Optimization Algorithm 拥有一个Solver，它含有两个部分。一个是 SparseBlockMatrix ，用于计算稀疏的雅可比和海塞；一个是用于计算迭代过程中最关键的一步：

\begin{matrix} (4) & H Δ x = - b \end{matrix}

$\begin{equation} H \Delta x = -b \end{equation}$
综上所述，在g2o中选择优化方法需要三个步骤：
1. 选择一个线性方程求解器。
2. 选择一个 BlockSolver 。
3. 选择一个迭代策略。

首先，我们以最小化重投影误差问题（Minimization of Reprojection error），采用Bundle Adjustment，表述成图优化的形式。

// 构造g2o中的图
// 先构造求解器
//SetMinimizerOptions(solver_ptr, params, optimizer);
g2o::OptimizationAlgorithmWithHessian* solver;
if(params.trust_region_strategy == "levenberg_marquardt"){
    solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
}
else if(params.trust_region_strategy == "dogleg"){
    solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
}
else {
    std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
    exit(EXIT_FAILURE);
}

optimizer->setAlgorithm(solver);


// 配置好求解模型，执行以下操作即可
// perform the optimizaiton 
optimizer.initializeOptimization();
optimizer.setVerbose(true);
optimizer.optimize(params.num_iterations);

（待续）

最优化是应用数学的一个分支，主要指在一定条件限制下，选取某种研究方案使目标达到最优的一种方法。最优化问题在当今的军事、工程、管理等领域有着极其广泛的应用。而且，随着科学技术的日益发展，许多工程的核心问题最终都归结为优化问题。

数学模型

最优化问题的共同特点是：求满足一定条件的变量x1，x2，…，xn，使某函数f(x1，x2，…，xn)取得最大值或者最小值。由于f(x1，x2，…，xn)的最大问题可以转化为-f(x1，x2，…，xn)的最小问题，所以较多时候只讨论最小问题。这里的函数f(x1，x2，…，xn)称为目标函数或者评价函数；变量x1，x2，…，xn称为决策变量；需要满足的条件称为约束条件；用以构成约束条件的函数称为约束函数。下面是图优化关于优化问题的定义：

常用方法

梯度下降法（Gradient Descent）
牛顿法和拟牛顿法（Newton’s method & Quasi-Newton Methods）
共轭梯度法（Conjugate Gradient）
启发式优化方法
解决约束优化问题——拉格朗日乘数法
Bundle Adjustment
单纯形法（Simplex Method）
interior-point法

问题分类

根据约束条件、目标函数或者变量类型有不同的分类方法，其中，图优化分为最小二乘法问题和线性规划问题。
数值分析中根据约束条件可划分为：
- 无约束问题
求x=(x1，x2，…，xn)T使函数f(x)=f(x1，x2，…，xn)达到最小值，记为min f(x)。
- 约束问题
- 1. 等式约束问题：求x=(x1，x2，…，xn)T使其在满足l个等式约束条件hj(x)=0，j=1，2，…，l的情况下，使函数f(x)=f(x1，x2，…，xn)达到最小值。
- 2. 不等式约束问题：求x=(x1，x2，…，xn)T 使其在满足m个不等式约束条件gi(x)≥0，i=1，2，…，m 的情况下，使函数f(x)=f(x1，x2，…，xn)达到最小值。

求解过程

局部优化
起始位置的 Initial guess
全局优化
worst-case analysis 或者 verification

数学概念

矩阵范数
常用的三种p-范数诱导出的矩阵范数是：
1-范数：║A║1 = max{ ∑|ai1|, ∑|ai2| ,…… ,∑|ain| }
2-范数：║A║2 = A的最大奇异值 = ( max{ λi(A^H*A) } ) ^{1/2} (欧几里德范数,谱范数,即A^H*A特征值λi中最大者λ1的平方根，其中A^H为A的转置共轭矩阵)；
∞-范数：║A║∞ = max{ ∑|a1j|, ∑|a2j| ,…, ∑|amj| } (行和范数，A每一行元素绝对值之和的最大值)

最大似然估计（Maximum Likelihood Estimation）
它用来求一个样本集的相关概率密度函数的参数。就是去找到那组参数估计值，使得前面已经实现的样本值发生概率最大。因为你手头上的样本已经实现了，其发生概率最大才符合逻辑。这时是求样本所有观测的联合概率最大化，是个连乘积，只要取对数，就变成了线性加总。此时通过对参数求导数，并令一阶导数为零，就可以通过解方程（组），得到最大似然估计值。
最小二乘
最小二乘：找到一个（组）估计值，使得实际值与估计值的距离最小。本来用两者差的绝对值汇总并使之最小是最理想的，但绝对值在数学上求最小值比较麻烦，因而替代做法是，找一个（组）估计值，使得实际值与估计值之差的平方加总之后的值最小，称为最小二乘。英文的字面意思是“平方最小”。这时，将这个差的平方的和式对参数求导数，并取一阶导数为零，就是OLSE。

arg min
In mathematics, arg max (or argmax) stands for the argument of the maximum, that is to say, the set of points of the given argument for which the value of the given expression attains its maximum value:
这里写图片描述
换句话说：

ICP算法（Iterative Closest Point迭代最近点算法）
ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换，直到满足正确配准的收敛精度要求。找到待配准点云数据与参考云数据之间的旋转参数R和平移参数 T，使得两点数据之间满足某种度量准则下的最优匹配。

MeshLab 是一个开源、可移植和可扩展的三维几何处理系统，主要用于交互处理和非结构化编辑三维三角形网格。该系统发布于2005年年底，旨在提供一整套三维扫描、编辑、清洗、拼合、检查、呈现和转换网格数据的工具。

参考资料

“常见的几种最优化方法” https://www.cnblogs.com/maybe2030/p/4751804.html

ORB-SLAM、ORB-SLAM2
PTAM、PTAMM
DSO
VINS
SVO、SVO2
g2o

https://blog.csdn.net/zkl99999/article/details/50863936 The Future of Real-Time SLAM and “Deep Learning vs SLAM klzhang的专栏

https://blog.csdn.net/mulinb/article/details/53421864 Visual SLAM算法笔记

http://www.52vr.com/article-1140-1.html 实时SLAM的未来以及深度学习与SLAM的比较

https://www.jianshu.com/p/f1d3906e4a3e 深入浅出最大似然估计

https://wenku.baidu.com/view/797a578af524ccbff121848b.html ICP算法原理

【CV. SLAM之完结篇】最优化