VIO —— 融合算法
基于Bundle Adjustment 的VIO融合什么是Bundle AdjustmentBundle Adjustment的作用后端优化中重投影误差函数/BA问题 VIO信息融合考虑的问题最小二乘问题的求解定义：假设损失函数基础:最速下降法，牛顿法迭代下降法最速下降法----适用于迭代开始阶段牛顿法----适用于最优值附近阻尼法 进阶:高斯牛顿法，LM算法的具体实现非线性最小二乘法高斯牛顿法LM算法L-M 迭代法算法步骤 终极:鲁棒核函数的实现基于Bundle Adjustment 的VIO融合
什么是Bundle Adjustment
Bundle Adjustment( BA )，是指从视觉图像中提炼出最优的3D模型和相机参数(内参数和外参数)。考虑从任意特征点发射出来的几束光线( bundles of light rays),它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整( adjustment)各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心，就称为BA（光束法平差）。bundle adjustment 其本质还是离不开最小二乘原理(几乎所有优化问题其本质都是最小二乘)，目前bundle adjustment 优化框架最为代表的是ceres solver和g2o。
如上图所示相机分别分三次在P1；P2；P3处进行拍摄（三维空间中的物体在二维相机底片上的投影）那现在就该说下什么叫重投影误差了，重投影也就是指的第二次投影，那到底是怎么投影的呢？
我们来整理一下吧：
其实第一次投影指的就是相机在拍照的时候三维空间点投影到图像上，然后我们利用这些图像对一些特征点进行三角定位，利用几何信息构建三角形来确定三维空间点的位置P P P（相关内容请参考对极几何）；最后利用我们计算得到的三维点的坐标P P P（注意不是真实的）和我们计算得到的相机矩阵（当然也不是真实的）进行第二次投影，也就是重投影，然后通过计算得到 P ^ 2 \hat{P}_2 P^2坐标；现在我们知道什么是重投影了，那重投影误差到底是什么样的误差呢？
这个误差是指的真实三维空间点在图像平面上的投影P 1 P_1 P1（也就是图像上的像素点）和重投影P ^ 2 \hat{P}_2 P^2（其实是用我们的计算值得到的虚拟的像素点）的差值，因为种种原因计算得到的值和实际情况不会完全相符，也就是这个差值不可能恰好为0，此时也就需要将这些差值的和最小化获取最优的相机参数及三维空间点的坐标。
Bundle Adjustment的作用
BA不仅可以优化位姿（R和t），还可以优化特征点的空间位置。而我们又可以把BA看成是最小化重投影误差（Reprojection error）问题，同时这也是一个非线性最小二乘问题。BA属于SLAM中的后端，BA就是一个优化模型，其本质就是最小化重投影误差。
后端优化中重投影误差函数/BA问题
在视觉SLAM中，我们通过最小化路标点的重投影误差，实现对相机位姿的约束。
对于VSLAM中的BA问题，有以下已知条件：
状态量的初始值：特征点的三维坐标；相机的位姿系统测量值：特征点在不同图像上的像素坐标
BA问题就是如何利用测量值来实现对初始状态量的最优估计？
构建误差函数，利用最小二乘得到状态量的最优估计，使重投影误差最小。（理论投影值减去实际观测值）
arg min q , p , f ∑ i = 1 m ∑ j = 1 n ∥ π ( q w c i , p w c i , f j ) − z f j c i ∥ Σ i j \underset{\mathbf{q}, \mathbf{p}, \mathbf{f}}{\arg \min } \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\pi\left(\mathbf{q}_{w c_{i}}, \mathbf{p}_{w c_{i}}, \mathbf{f}_{j}\right)-\mathbf{z}_{f_{j}}^{c_{i}}\right\|_{\Sigma_{i j}} q,p,fargmini=1∑mj=1∑n∥∥∥π(qwci,pwci,fj)−zfjci∥∥∥Σij
符号定义:q: 旋转四元数p: 平移向量 f \mathrm{f} f : 特征点 3D 坐标 c i c_{i} ci : 第 i i i 个相机系 π ( ⋅ ) \pi(\cdot) π(⋅) : 投影函数 z f j c i : c i \mathbf{z}_{f_{j}}^{c_{i}}: c_{i} zfjci:ci 对 f j f_{j} fj 的观测 Σ i j : Σ \Sigma_{i j}: \Sigma Σij:Σ 范数
VIO信息融合考虑的问题
将IMU融入到视觉里程计中，除了要最小化特征点的重投影误差以外，还需要考虑如下问题：
相机与IMU的外参，包括旋转和平移相机和IMU的帧率不同，如何实现两者数据上的融合IMU从 b 1 b_1 b1到 b 2 b_2 b2 时，有测量值 z b 1 b 2 z_{b_1 b_2} zb1b2 ，如何构建IMU的测量误差函数位姿的来源有IMU测量和视觉测量，如何设定多个信息源权重对于融合信息后构建的误差函数，如何求解
最小二乘问题的求解
线性最小二乘求解:最速下降法和牛顿法
非线性线性最小二乘求解:Gauss-Newton 和 LM
最小二乘中遇到 outlier，通过添加鲁棒核函数
定义：
找到一个 n n n 维的变量 x ∗ ∈ R n \mathbf{x}^{*} \in \mathbb{R}^{n} x∗∈Rn ，使得损失函数 F ( x ) F(x) F(x) 取局部最小值：
F ( x ) = 1 2 ∑ i = 1 m ( f i ( x ) ) 2 F(\mathbf{x})=\frac{1}{2} \sum_{i=1}^{m}\left(f_{i}(\mathbf{x})\right)^{2} F(x)=21i=1∑m(fi(x))2
其中 f i f_{i} fi 是残差函数，比如测量值和预测值之间的差，且有 m ≥ n m \geq n m≥n 。局部最小值指对任意 ∥ x − x ∗ ∥ < δ \left\|\mathrm{x}-\mathrm{x}^{*}\right\|<\delta ∥x−x∗∥<δ 有 F ( x ∗ ) ≤ F ( x ) F\left(\mathrm{x}^{*}\right) \leq F(\mathrm{x}) F(x∗)≤F(x)
假设损失函数
F ( x + Δ x ) = F ( x ) + J Δ x + 1 2 Δ x ⊤ H Δ x + O ( ∥ Δ x ∥ 3 ) F(\mathbf{x}+\Delta \mathbf{x})=F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x}+O\left(\|\Delta \mathbf{x}\|^{3}\right) F(x+Δx)=F(x)+JΔx+21Δx⊤HΔx+O(∥Δx∥3)
其中 J \mathrm{J} J 和 H \mathrm{H} H 分别为损失函数 F F F 对变量 x \mathrm{x} x 的一阶导和二阶导矩阵。
忽略泰勒展开的高阶项，损失函数变成了二次函数，可以轻易得到如下性质：
如果在点 x s x_s xs；处有导数为0，则称这个点为稳定点。在点x,处对应的Hessian为H；如果是正定矩阵，即它的特征值都大于0，则在 x s x_s xs处有F(x)为
局部最小值；如果是负定矩阵，即它的特征值都小于0，则在 x s x_s xs处有F(x)为
局部最大值；如果是不定矩阵，即它的特征值大于0也有小于0的，则 x s x_s xs处
为鞍点。
基础:最速下降法，牛顿法
迭代下降法
找一个下降方向使损失函数随 x \mathrm{x} x 的迭代逐渐减小，直到 x \mathrm{x} x 收敛到 x ∗ \mathrm{x}^{*} x∗ ：
F ( x k + 1 ) < F ( x k ) F\left(\mathrm{x}_{k+1}\right)<F\left(\mathrm{x}_{k}\right) F(xk+1)<F(xk)
分两步: 第一，找下降方向单位向量 d \mathrm{d} d ，第二，确定下降步长 α \alpha α。
假设 α \alpha α 足够小，我们可以对损失函数 F ( x ) F(\mathbf{x}) F(x) 进行一阶泰勒展开：
F ( x + α d ) ≈ F ( x ) + α J d F(\mathbf{x}+\alpha \mathbf{d}) \approx F(\mathbf{x})+\alpha \mathbf{J d} F(x+αd)≈F(x)+αJd
只需寻找下降方向，满足：
J d < 0 \mathbf{J} \mathbf{d}<0 Jd<0
通过 line search 方法找到下降的步长： α ∗ = argmin α > 0 { F ( x + α d ) } \alpha^{*}=\operatorname{argmin}_{\alpha>0}\{F(\mathbf{x}+\alpha \mathbf{d})\} α∗=argminα>0{F(x+αd)}
最速下降法----适用于迭代开始阶段
从下降方向的条件可知： J d = ∥ J ∥ cos θ , θ \mathbf{J d}=\|\mathbf{J}\| \cos \theta, \theta Jd=∥J∥cosθ,θ 表示下降方向和梯度方向 的夹角。当 θ = π \theta=\pi θ=π ，有
d = − J ⊤ \mathbf{d}=-\mathbf{J}^{\top} d=−J⊤
即梯度的负方向为最速下降方向。
缺点：最优值附近震荡，收敛慢。
牛顿法----适用于最优值附近
在局部最优点x ∗ \mathrm{x}^{*} x∗ 附近，如果 x + Δ x \mathrm{x}+\Delta \mathrm{x} x+Δx 是最优解，则损失函数对 Δ x \Delta \mathrm{x} Δx 的导数等于 0 ，对损失函数取一阶导有：
∂ ∂ Δ x ( F ( x ) + J Δ x + 1 2 Δ x ⊤ H Δ x ) = J ⊤ + H Δ x = 0 \frac{\partial}{\partial \Delta \mathbf{x}}\left(F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x}\right)=\mathbf{J}^{\top}+\mathbf{H} \Delta \mathbf{x}=\mathbf{0} ∂Δx∂(F(x)+JΔx+21Δx⊤HΔx)=J⊤+HΔx=0
得到： Δ x = − H − 1 J ⊤ \Delta \mathrm{x}=-\mathrm{H}^{-1} \mathrm{~J}^{\top} Δx=−H−1J⊤ 。
缺点： 二阶导矩阵计算复杂。
阻尼法
将损失函数的二阶泰勒展开记作
F ( x + Δ x ) ≈ L ( Δ x ) ≡ F ( x ) + J Δ x + 1 2 Δ x ⊤ H Δ x F(\mathbf{x}+\Delta \mathbf{x}) \approx L(\Delta \mathbf{x}) \equiv F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x} F(x+Δx)≈L(Δx)≡F(x)+JΔx+21Δx⊤HΔx
求以下函数的最小化:
Δ x ≡ arg min Δ x { L ( Δ x ) + 1 2 μ Δ x ⊤ Δ x } \Delta \mathbf{x} \equiv \arg \min _{\Delta \mathbf{x}}\left\{L(\Delta \mathbf{x})+\frac{1}{2} \mu \Delta \mathbf{x}^{\top} \Delta \mathbf{x}\right\} Δx≡argΔxmin{L(Δx)+21μΔx⊤Δx}
其中， μ ≥ 0 \mu \geq 0 μ≥0 为阻尼因子， 1 2 μ Δ x ⊤ Δ x = 1 2 μ ∥ Δ x ∥ 2 \frac{1}{2} \mu \Delta \mathbf{x}^{\top} \Delta \mathbf{x}=\frac{1}{2} \mu\|\Delta \mathbf{x}\|^{2} 21μΔx⊤Δx=21μ∥Δx∥2 是惩罚项。 对新的损失函数求一阶导，并令其等于 0 有:
L ′ ( Δ x ) + μ Δ x = 0 ⇒ ( H + μ I ) Δ x = − J ⊤ \begin{aligned} \mathbf{L}^{\prime}(\Delta \mathbf{x})+\mu \Delta \mathbf{x} &=\mathbf{0} \\ \Rightarrow(\mathbf{H}+\mu \mathbf{I}) \Delta \mathbf{x} &=-\mathbf{J}^{\top} \end{aligned} L′(Δx)+μΔx⇒(H+μI)Δx=0=−J⊤
进阶:高斯牛顿法，LM算法的具体实现
非线性最小二乘法
1、符号说明
为了公式约简，可将残差组合成向量的形式。
f ( x ) = [ f 1 ( x ) … f m ( x ) ] \mathbf{f}(\mathbf{x})=\left[\begin{array}{c} f_{1}(\mathbf{x}) \\ \ldots \\ f_{m}(\mathbf{x}) \end{array}\right] f(x)=⎣⎡f1(x)…fm(x)⎦⎤
则有: f ⊤ ( x ) f ( x ) = ∑ i = 1 m ( f i ( x ) ) 2 \quad \mathbf{f}^{\top}(\mathbf{x}) \mathbf{f}(\mathbf{x})=\sum_{i=1}^{m}\left(f_{i}(\mathbf{x})\right)^{2} f⊤(x)f(x)=∑i=1m(fi(x))2
同理，如果记 J i ( x ) = ∂ f i ( x ) ∂ x \mathrm{J}_{i}(\mathrm{x})=\frac{\partial f_{i}(\mathrm{x})}{\partial \mathrm{x}} Ji(x)=∂x∂fi(x) 则有:
∂ f ( x ) ∂ x = J = [ J 1 ( x ) … J m ( x ) ] \frac{\partial \mathbf{f}(\mathbf{x})}{\partial \mathbf{x}}=\mathbf{J}=\left[\begin{array}{c} \mathbf{J}_{1}(\mathbf{x}) \\ \ldots \\ \mathbf{J}_{m}(\mathbf{x}) \end{array}\right] ∂x∂f(x)=J=⎣⎡J1(x)…Jm(x)⎦⎤
2、基础
残差函数f ( x ) \mathrm{f}(\mathrm{x}) f(x)为非线性函数，对其一阶泰勒近似有:
f ( x + Δ x ) ≈ ℓ ( Δ x ) ≡ f ( x ) + J Δ x \mathrm{f}(\mathrm{x}+\Delta \mathrm{x}) \approx \ell(\Delta \mathrm{x}) \equiv \mathrm{f}(\mathrm{x})+\mathrm{J} \Delta \mathrm{x} f(x+Δx)≈ℓ(Δx)≡f(x)+JΔx
请特别注意，这里的 J \mathrm{J} J 是残差函数 f \mathrm{f} f 的雅克比矩阵。代入损失函数:
F ( x + Δ x ) ≈ L ( Δ x ) ≡ 1 2 ℓ ( Δ x ) ⊤ ℓ ( Δ x ) = 1 2 f ⊤ f + Δ x ⊤ J ⊤ f + 1 2 Δ x ⊤ J ⊤ J Δ x = F ( x ) + Δ x ⊤ J ⊤ f + 1 2 Δ x ⊤ J ⊤ J Δ x (1) \begin{aligned} F(\mathbf{x}+\Delta \mathbf{x}) \approx L(\Delta \mathbf{x}) & \equiv \frac{1}{2} \boldsymbol{\ell}(\Delta \mathbf{x})^{\top} \boldsymbol{\ell}(\Delta \mathbf{x}) \\ &=\frac{1}{2} \mathbf{f}^{\top} \mathbf{f}+\Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \\ &=F(\mathbf{x})+\Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \end{aligned}\tag 1 F(x+Δx)≈L(Δx)≡21ℓ(Δx)⊤ℓ(Δx)=21f⊤f+Δx⊤J⊤f+21Δx⊤J⊤JΔx=F(x)+Δx⊤J⊤f+21Δx⊤J⊤JΔx(1)
这样损失函数就近似成了一个二次函数，并且如果雅克比是满秩的， 则 J ⊤ J \mathbf{J}^{\top} \mathbf{J} J⊤J正定，损失函数有最小值。
另外, 易得： F ′ ( x ) = ( J ⊤ f ) ⊤ F^{\prime}(\mathbf{x})=\left(\mathbf{J}^{\top} \mathbf{f}\right)^{\top} F′(x)=(J⊤f)⊤, 以及 F ′ ′ ( x ) ≈ J ⊤ J F^{\prime \prime}(\mathbf{x}) \approx \mathbf{J}^{\top} \mathbf{J} F′′(x)≈J⊤J
高斯牛顿法
令公式 (1) 的一阶导等于 0 ，得到：
( J ⊤ J ) Δ x g n = − J ⊤ f \left(\mathbf{J}^{\top} \mathbf{J}\right) \Delta \mathbf{x}_{\mathrm{gn}}=-\mathbf{J}^{\top} \mathbf{f} (J⊤J)Δxgn=−J⊤f
上式就是通常论文里看到的 看到的 H Δ x g n = b ，称其为normalequation. {\text { 看到的 } \mathbf{H} \Delta \mathbf{x}_{\mathrm{gn}}=\mathbf{b} \text { ，称其为 normal equation. }} 看到的HΔxgn=b，称其为normalequation.
LM算法
Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进， 求解过程中引入了阻尼因子:
( J ⊤ J + μ I ) Δ x l m = − J ⊤ f with μ ≥ 0 \left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}\right) \Delta \mathbf{x}_{\mathrm{lm}}=-\mathbf{J}^{\top} \mathbf{f} \quad \text { with } \mu \geq 0 (J⊤J+μI)Δxlm=−J⊤fwithμ≥0
疑问: LM 中阻尼因子有什么作用，它怎么设定呢?
1、阻尼因子的作用：
μ > 0 \mu>0 μ>0 保证 ( J ⊤ J + μ I ) \left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}\right) (J⊤J+μI) 正定，迭代朝着下降方向进行。 μ \mu μ 非常大，则 Δ x lm = − 1 μ J ⊤ f = − 1 μ F ′ ( x ) ⊤ \Delta \mathbf{x}_{\operatorname{lm}}=-\frac{1}{\mu} \mathbf{J}^{\top} \mathbf{f}=-\frac{1}{\mu} F^{\prime}(\mathbf{x})^{\top} Δxlm=−μ1J⊤f=−μ1F′(x)⊤, 接近最速下降法. μ \mu μ 比较小, 则 Δ x l m ≈ Δ x g n \Delta \mathrm{x}_{\mathrm{lm}} \approx \Delta \mathrm{x}_{\mathrm{gn}} Δxlm≈Δxgn, 接近高斯牛顿法。阻尼因子 μ \mu μ 大小是相对于 J ⊤ J \mathbf{J}^{\top} \mathbf{J} J⊤J 的元素而言的。半正定的信息矩阵
2、阻尼因子初始值选取
J ⊤ J \mathbf{J}^{\top} \mathbf{J} J⊤J 特征值 { λ j } \left\{\lambda_{j}\right\} {λj} 和对应的特征向量为 { v j } \left\{\mathbf{v}_{j}\right\} {vj} 。对 J ⊤ J \mathbf{J}^{\top} \mathbf{J} J⊤J 做特征值分解分解后有: J ⊤ J = V Λ V ⊤ \mathbf{J}^{\top} \mathbf{J}=\mathbf{V} \boldsymbol{\Lambda} \mathbf{V}^{\top} J⊤J=VΛV⊤ 可得:
Δ x l m = − ∑ j = 1 n v j ⊤ F ′ ⊤ λ j + μ v j \Delta \mathbf{x}_{\mathrm{lm}}=-\sum_{j=1}^{n} \frac{\mathbf{v}_{j}^{\top} \mathbf{F}^{\prime \top}}{\lambda_{j}+\mu} \mathbf{v}_{j} Δxlm=−j=1∑nλj+μvj⊤F′⊤vj
所以，一个简单的 μ 0 \mu_{0} μ0 初始值的策略就是:
μ 0 = τ ⋅ max { ( J ⊤ J ) i i } \mu_{0}=\tau \cdot \max \left\{\left(\mathbf{J}^{\top} \mathbf{J}\right)_{i i}\right\} μ0=τ⋅max{(J⊤J)ii}
通常，按需设定 τ ∼ [ 1 0 − 8 , 1 ] \tau \sim\left[10^{-8}, 1\right] τ∼[10−8,1] 。
工程上 μ 0 \mu_{0} μ0 常取与 J ⊤ J \mathbf{J}^{\top} \mathbf{J} J⊤J的最大特征值得数量级保持一致：对角线元素最大值的数量级之一致的
3、阻尼因子的更新策略
定性分析，直观感受阻尼因子的更新:
(1) 如果 Δ x → F ( x ) ↑ \Delta \mathrm{x} \rightarrow F(\mathrm{x}) \uparrow Δx→F(x)↑ ，则 μ ↑ → Δ x ↓ \mu \uparrow \rightarrow \Delta \mathrm{x} \downarrow μ↑→Δx↓, 增大阻尼减小步长，拒绝本次迭代。
(2) 如果 Δ x → F ( x ) ↓ \Delta \mathrm{x} \rightarrow F(\mathrm{x}) \downarrow Δx→F(x)↓, 则 μ ↓ → Δ x ↑ \mu \downarrow \rightarrow \Delta \mathrm{x} \uparrow μ↓→Δx↑, 减小阻尼增大步长。加快收敛，减少迭代次数。
定量分析，阻尼因子更新策略通过比例因子来确定的:
ρ = F ( x ) − F ( x + Δ x l m ) L ( 0 ) − L ( Δ x l m ) \rho=\frac{F(\mathbf{x})-F\left(\mathbf{x}+\Delta \mathbf{x}_{\mathrm{lm}}\right)}{L(\mathbf{0})-L\left(\Delta \mathbf{x}_{\mathrm{lm}}\right)} ρ=L(0)−L(Δxlm)F(x)−F(x+Δxlm)
其中:
L ( 0 ) − L ( Δ x lm ) = − Δ x lm ⊤ J ⊤ f − 1 2 Δ x lm ⊤ J ⊤ J Δ x lm = b = − J ⊤ f − 1 2 Δ x lm ⊤ ( − 2 b + ( J ⊤ J + μ I − μ I ) Δ x lm ) = 1 2 Δ x lm ⊤ ( μ Δ x lm + b ) \begin{aligned} L(\mathbf{0})-L\left(\Delta \mathbf{x}_{\operatorname{lm}}\right)=&-\Delta \mathbf{x}_{\operatorname{lm}}^{\top} \mathbf{J}^{\top} \mathbf{f}-\frac{1}{2} \Delta \mathbf{x}_{\operatorname{lm}}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x}_{\operatorname{lm}} \\ & \stackrel{\mathrm{b}=-\mathbf{J}^{\top} \mathbf{f}}{=}-\frac{1}{2} \Delta \mathbf{x}_{\operatorname{lm}}^{\top}\left(-\mathbf{2 b}+\left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}-\mu \mathbf{I}\right) \Delta \mathbf{x}_{\operatorname{lm}}\right) \\ &=\frac{1}{2} \Delta \mathbf{x}_{\operatorname{lm}}^{\top}\left(\mu \Delta \mathbf{x}_{\operatorname{lm}}+\mathbf{b}\right) \end{aligned} L(0)−L(Δxlm)=−Δxlm⊤J⊤f−21Δxlm⊤J⊤JΔxlm=b=−J⊤f−21Δxlm⊤(−2b+(J⊤J+μI−μI)Δxlm)=21Δxlm⊤(μΔxlm+b)
首先比例因子分母始终大于 0 , 如果:
ρ < 0 \rho<0 ρ<0, 则 F ( x ) ↑ F(\mathbf{x}) \uparrow F(x)↑ ，应该 μ ↑ → Δ x ↓ \mu \uparrow \rightarrow \Delta \mathbf{x} \downarrow μ↑→Δx↓, 增大阻尼减小步长。如果 ρ > 0 \rho>0 ρ>0 且比较大，减小 μ \mu μ, 让 LM 接近 Gauss-Newton 使得系 统更快收敛。反之，如果是比较小的正数，则增大阻尼 μ \mu μ ，缩小迭代步长。
1963 年 Marquardt 提出了一个如下的阻尼策略:
if ρ < 0.25 μ : = μ ∗ 2 elseif ρ > 0.75 μ : = μ / 3 \begin{aligned} \text { if } \rho &<0.25 \\ \mu &:=\mu * 2 \\ \text { elseif } \rho &>0.75 \\ \mu &:=\mu / 3 \end{aligned} ifρμelseifρμ<0.25:=μ∗2>0.75:=μ/3
L-M 迭代法算法步骤
L-M 优化公式: ( J T J + μ I ) ( X − X ∗ ) = − J T f \left(\mathbf{J}^{\mathrm{T}} \mathbf{J}+\mu \mathbf{I}\right)\left(\mathbf{X}-\mathbf{X}^{*}\right)=-\mathbf{J}^{\mathrm{T}} \mathbf{f} (JTJ+μI)(X−X∗)=−JTf
设定初值状态量初值, 并计算收敛阈值 ε \varepsilon ε 和阻尼因子 μ \mu μ使用系统所有的状态变量和测量数据分别计算残差 f p − J p X \boldsymbol{f}_{p}-\boldsymbol{J}_{p} \boldsymbol{X} fp−JpX 、 f I ( z I k − h I k ( X ) ) 、 f C ( z C k − h C k ( X ) ) f_{I}\left(z_{I}^{k}-h_{I}^{k}(X)\right) 、 f_{C}\left(z_{C}^{k}-h_{C}^{k}(X)\right) fI(zIk−hIk(X))、fC(zCk−hCk(X)), 并整合为 f f f计算残差函数计算关于状态向量 X X X 的雅克比矩阵 J \mathbf{J} J解方程 X − X ∗ = − ( J T J ) − 1 J T f \mathbf{X}-\mathbf{X}^{*}=-\left(\mathbf{J}^{\mathrm{T}} \mathbf{J}\right)^{-1} \mathbf{J}^{\mathrm{T}} \mathbf{f} X−X∗=−(JTJ)−1JTf, 获得 X ∗ X^{*} X∗计算实际下降与近似下降的比例因子 ρ \rho ρ, 判断此次迭代是成功, 如果 ρ > 0 \rho>0 ρ>0,
更新 v = 2 , μ = μ ∗ max ( 1 3 , 1 − ( 2 ρ − 1 ) 3 ) v=2, \mu=\mu^{*} \max \left(\frac{1}{3}, 1-(2 \rho-1)^{3}\right) v=2,μ=μ∗max(31,1−(2ρ−1)3), 否则 μ = μ × v , v = 2 × v \mu=\mu \times v, v=2 \times v μ=μ×v,v=2×v判断 ∥ X − X ∗ ∥ < ε \left\|\mathbf{X}-\mathbf{X}^{*}\right\|<\varepsilon ∥X−X∗∥<ε, 若成立则结束; 不成立, 重复 2-5 步
终极:鲁棒核函数的实现
引言：最小二乘中遇到 outlier 怎么处理? 核函数如何在代码中实现?
有多种方法 ，这里主要介绍 g2o 和 ceres 中使用的 Triggs Correction 。
Iriggs Correction
鲁棒核函数直接作用残差 f k ( x ) f_{k}(\mathbf{x}) fk(x) 上，最小二乘函数变成了如下形式:
min x 1 2 ∑ k ρ ( ∥ f k ( x ) ∥ 2 ) \min _{\mathbf{x}} \frac{1}{2} \sum_{k} \rho\left(\left\|f_{k}(\mathbf{x})\right\|^{2}\right) xmin21k∑ρ(∥fk(x)∥2)
将误差的平方项记作 s k = ∥ f k ( x ) ∥ 2 s_{k}=\left\|f_{k}(\mathbf{x})\right\|^{2} sk=∥fk(x)∥2, 则鲁棒核误差函数进行二阶泰勒 展开有:
1 2 ρ ( s ) = 1 2 ( const + ρ ′ Δ s + 1 2 ρ ′ ′ Δ 2 s ) \frac{1}{2} \rho(s)=\frac{1}{2}\left(\text { const }+\rho^{\prime} \Delta s+\frac{1}{2} \rho^{\prime \prime} \Delta^{2} s\right) 21ρ(s)=21(const+ρ′Δs+21ρ′′Δ2s)
上述函数中 Δ s k \Delta s_{k} Δsk 的计算稍微复杂一点:
Δ s k = ∥ f k ( x + Δ x ) ∥ 2 − ∥ f k ( x ) ∥ 2 ≈ ∥ f k + J k Δ x ∥ 2 − ∥ f k ( x ) ∥ 2 = 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x \begin{aligned} \Delta s_{k} &=\left\|f_{k}(\mathbf{x}+\Delta \mathbf{x})\right\|^{2}-\left\|f_{k}(\mathbf{x})\right\|^{2} \\ & \approx\left\|f_{k}+\mathbf{J}_{k} \Delta \mathbf{x}\right\|^{2}-\left\|f_{k}(\mathbf{x})\right\|^{2} \\ &=2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x} \end{aligned} Δsk=∥fk(x+Δx)∥2−∥fk(x)∥2≈∥fk+JkΔx∥2−∥fk(x)∥2=2fk⊤JkΔx+(Δx)⊤Jk⊤JkΔx
将上式代入上上式有:
1 2 ρ ( s ) ≈ 1 2 ( ρ ′ [ 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ] + 1 2 ρ ′ ′ [ 2 f k ⊤ J k Δ x + ( Δ x ) ⊤ J k ⊤ J k Δ x ] 2 + const ) ≈ ρ ′ f k ⊤ J k Δ x + 1 2 ρ ′ ( Δ x ) ⊤ J k ⊤ J k Δ x + ρ ′ ′ ( Δ x ) ⊤ J k ⊤ f k f k ⊤ J k Δ x + const = ρ ′ f k ⊤ J k Δ x + 1 2 ( Δ x ) ⊤ J k ⊤ ( ρ ′ I + 2 ρ ′ ′ f k f k ⊤ ) J k Δ x + const \begin{aligned} \frac{1}{2} \rho(s) \approx & \frac{1}{2}\left(\rho^{\prime}\left[2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}\right]\right.\\ &\left.+\frac{1}{2} \rho^{\prime \prime}\left[2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}\right]^{2}+\text { const }\right) \\ \approx & \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\frac{1}{2} \rho^{\prime}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\rho^{\prime \prime}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} f_{k} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\text { const } \\ =& \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\frac{1}{2}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top}\left(\rho^{\prime} I+2 \rho^{\prime \prime} f_{k} f_{k}^{\top}\right) \mathbf{J}_{k} \Delta \mathbf{x}+\text { const } \end{aligned} 21ρ(s)≈≈=21(ρ′[2fk⊤JkΔx+(Δx)⊤Jk⊤JkΔx]+21ρ′′[2fk⊤JkΔx+(Δx)⊤Jk⊤JkΔx]2+const)ρ′fk⊤JkΔx+21ρ′(Δx)⊤Jk⊤JkΔx+ρ′′(Δx)⊤Jk⊤fkfk⊤JkΔx+constρ′fk⊤JkΔx+21(Δx)⊤Jk⊤(ρ′I+2ρ′′fkfk⊤)JkΔx+const
对上式求和后，对变量 Δ x \Delta \mathrm{x} Δx 求导，令其等于 0 ，得到:
∑ k J k ⊤ ( ρ ′ I + 2 ρ ′ ′ f k f k ⊤ ) J k Δ x = − ∑ k ρ ′ f k ⊤ J k ∑ k J k ⊤ W J k Δ x = − ∑ k ρ ′ f k ⊤ J k \begin{aligned} \sum_{k} \mathbf{J}_{k}^{\top}\left(\rho^{\prime} I+2 \rho^{\prime \prime} f_{k} f_{k}^{\top}\right) \mathbf{J}_{k} \Delta \mathbf{x} &=-\sum_{k} \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \\ \sum_{k} \mathbf{J}_{k}^{\top} W \mathbf{J}_{k} \Delta \mathbf{x} &=-\sum_{k} \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \end{aligned} k∑Jk⊤(ρ′I+2ρ′′fkfk⊤)JkΔxk∑Jk⊤WJkΔx=−k∑ρ′fk⊤Jk=−k∑ρ′fk⊤Jk
Example Cauchy Cost Function
柯西鲁棒核函数的定义为:
ρ ( s ) = c 2 log ( 1 + s c 2 ) \rho(s)=c^{2} \log \left(1+\frac{s}{c^{2}}\right) ρ(s)=c2log(1+c2s)
其中 c c c 为控制参数。对 s s s 的一阶导和二阶导为:
ρ ′ ( s ) = 1 1 + s c 2 , ρ ′ ′ ( s ) = − 1 c 2 ( ρ ′ ( s ) ) 2 \rho^{\prime}(s)=\frac{1}{1+\frac{s}{c^{2}}}, \quad \rho^{\prime \prime}(s)=-\frac{1}{c^{2}}\left(\rho^{\prime}(s)\right)^{2} ρ′(s)=1+c2s1,ρ′′(s)=−c21(ρ′(s))2
