淦ORB-SLAM2源码 10--最小二乘问题（最速下降法，牛顿法，高斯牛顿，LM法，鲁棒核函数）

最小二乘问题

定义

最小二乘问题就是找到一个 n 维的变量 x∗ ∈ Rn，使得损失函数 F (x) 取局部最小值:

其中 fi 是残差函数， 比如测量值和预测值之间的差，且有 m ≥ n。局部最小值指对任意 ∥x − x∗∥ < δ 有 F (x∗) ≤ F (x)

损失函数泰勒展开

假设损失函数 F (x) 是可导并且平滑的，因此，二阶泰勒展开：

其中 J 和 H 分别为损失函数 F 对变量 x 的一阶导和二阶导矩阵。

忽略泰勒展开的高阶项，损失函数变成了二次函数，可以轻易得到如下性质：
• 如果在点 xs 处有导数为 0 ，则称这个点为稳定点。
• 在点 xs 处对应的 Hessian 为 H：
• 如果是正定矩阵，即它的特征值都大于 0，则在 xs 处有 F(x) 为局部最小值;
• 如果是负定矩阵，即它的特征值都小于 0，则在 xs 处有 F(x) 为局部最大值;
• 如果是不定矩阵，即它的特征值大于 0 也有小于 0 的，则 xs 处为鞍点

求解法

直接求解：线性最小二乘
迭代下降法：适用于线性和非线性最小二乘

最速下降法

迭代法初衷

找一个下降方向使损失函数随 x 的迭代逐渐减小，直到 x 收敛到 x∗：

分两步：第一，找下降方向单位向量 d，第二，确定下降步长 α.
假设 α 足够小，我们可以对损失函数 F(x) 进行一阶泰勒展开：

只需寻找下降方向，满足：

通过 line search 方法找到下降的步长：

最速下降法: 适用于迭代的开始阶段从下降方向的条件可知： Jd = ∥J∥ cos θ， θ 表示下降方向和梯度方向的夹角。当 θ = π ，有

即 梯度的负方向为最速下降方向。缺点：最优值附近震荡，收敛慢。

牛顿法

牛顿法：适用于最优值附近
在局部最优点 x∗ 附近，如果 x + ∆x 是最优解，则损失函数对 ∆x的导数等于 0，对公式 (2) 取一阶导有：

得到：

缺点：二阶导矩阵计算复杂。

阻尼法

将损失函数的二阶泰勒展开记作

求以下函数的最小化：

其中， µ ≥ 0 为阻尼因子
为惩罚项。对新的损失函数求一阶导，并令其等于 0 有：

高斯牛顿

为了公式约简，可将残差组合成向量的形式

则有:
同理，如果记：

则有:

残差函数 f(x) 为非线性函数，对其一阶泰勒近似有:

请特别注意，这里的 J 是残差函数 f 的雅克比矩阵。代入损失函数：

这样损失函数就近似成了一个二次函数，并且如果雅克比是满秩的，则 J⊤J 正定，损失函数有最小值。
另外, 易得：

令其的一阶导等于 0，得到：

上式就是通常论文里看到的

LM法

Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进，求解过程中引入了阻尼因子：

阻尼因子的作用

• µ > 0 保证 (J⊤J + µI) 正定，迭代朝着下降方向进行。
• µ 非常大，则接近最速下降法.
• µ 比较小，则 ∆xlm ≈ ∆xgn, 接近高斯牛顿法。

阻尼因子初始值的选取

阻尼因子 µ 大小是相对于 J⊤J 的元素而言的。半正定的信息矩阵J⊤J 特征值 {λj} 和对应的特征向量为 {vj}。 对 J⊤J 做特征值分解分解后有： J⊤J = VΛV⊤ 可得：

所以，一个简单的 µ0 初始值的策略就是：

对角元素的最大值，通常，按需设定 τ ∼ [10−8, 1]。

阻尼因子 µ 的更新策略

定性分析，直观感受阻尼因子的更新:
1 如果 ∆x → F (x) ↑ ，则 µ ↑→ ∆x ↓, 增大阻尼减小步长，拒绝本次迭代。
2 如果 ∆x → F (x) ↓ ，则 µ ↓→ ∆x ↑, 减小阻尼增大步长。加快收敛，减少迭代次数。

定量分析，阻尼因子更新策略通过比例因子来确定的：

其中：

通过计算ρ的大小来更新阻尼因子。
1963 年 Marquardt 提出了一个如下的阻尼策略:

但是目前g2o和ceres用的是Nielsen策略：

鲁棒核函数

最小二乘中遇到 outlier 怎么处理？核函数如何在代码中实现？
有多种方法， 这里主要介绍 g2o 和 ceres 中使用的 Triggs Correction

鲁棒核函数直接作用残差 fk(x) 上，最小二乘函数变成了如下形式：

将误差的平方项记作 sk = ∥fk(x)∥2, 则鲁棒核误差函数进行二阶泰勒展开有：

上述函数中 ∆sk 的计算稍微复杂一点：

代入上式有：

对上式求和后，对变量 ∆x 求导，令其等于 0 ，得到：

Example: Cauchy Cost Function
柯西鲁棒核函数的定义为：

其中 c 为控制参数。对 s 的一阶导和二阶导为：