目录
视觉 SLAM 里的 Bundle Adjustment 问题
最小二乘基础概念
迭代下降法求解：下降法
最速下降法和牛顿法
阻尼法
非线性最小二乘
Gauss-Newton 和 LM
鲁棒核函数的实现
VIO 残差函数的构建
视觉重投影误差
IMU 测量值积分
状态误差线性递推公式的推导
基于误差随时间变化的递推方程
视觉 SLAM 里的 Bundle Adjustment 问题
• 状态量初始值：特征点的三维坐标，
相机的位姿。
• 系统测量值：特征点在不同图像上的
图像坐标。
问题：如何估计状态量的最优值？
解决方式
构建误差函数，利用最小二乘得到状态量
的最优估计
最小二乘基础概念
找到一个 n 维的变量 x ∗ ∈ R n ，使得损失函数 F ( x ) 取局部最小值：
F ( x ) = 1 2 ∑ i =1 m ( f i ( x )) 2
其中 f i 是残差函数，比如测量值和预测值之间的差，且有 m ≥ n 。局
部最小值指对任意 ∥ x − x ∗ ∥ < δ 有 F ( x ∗ ) ≤ F ( x )
损失函数泰勒展开
假设损失函数 F ( x ) 是可导并且平滑的，因此，二阶泰勒展开：
F ( x + ∆ x ) = F ( x ) + J ∆ x + 1 2 ∆ x ⊤ H ∆ x + O ( ∥ ∆ x ∥ 3 ) (2)
其中 J 和 H 分别为损失函数 F 对变量 x 的一阶导和二阶导矩阵
损失函数泰勒展开性质
忽略泰勒展开的高阶项，损失函数变成了二次函数，可以轻易得到如
下性质：
• 如果在点 x s 处有导数为 0 ，则称这个点为稳定点。
• 在点 x s 处对应的 Hessian 为 H ：
• 如果是正定矩阵，即它的特征值都大于 0 ，则在 x s 处有 F ( x ) 为
局部最小值 ;
• 如果是负定矩阵，即它的特征值都小于 0 ，则在 x s 处有 F ( x ) 为
局部最大值 ;
• 如果是不定矩阵，即它的特征值大于 0 也有小于 0 的，则 x s 处
为鞍点。
迭代下降法求解：下降法
迭代法初衷
找一个下降方向使损失函数随 x 的迭代逐渐减小，直到 x 收敛到 x ∗ ：
F ( x k +1 ) < F ( x k )
分两步：第一，找下降方向 单位 向量 d ，第二，确定下降步长 α .
假设 α 足够小，我们可以对损失函数 F ( x ) 进行一阶泰勒展开：
F ( x + α d ) ≈ F ( x ) + α Jd
只需寻找下降方向，满足：
Jd < 0
通过 line search 方法找到下降的步长： α ∗ = argmin α> 0 { F ( x + α d ) }
最速下降法和牛顿法
最速下降法 : 适用于迭代的开始阶段
从下降方向的条件可知： Jd = ∥ J ∥ cos θ ， θ 表示下降方向和梯度方向
的夹角。当 θ = π ，有
d = − J ⊤ ∥ J ∥
即 梯度的负方向 为最速下降方向。缺点：最优值附近震荡，收敛慢
牛顿法：适用于最优值附近
在局部最优点 x ∗ 附近，如果 x + ∆ x 是最优解，则损失函数对
![洛谷P1107 [BJWC2008] 雷涛的小猫](http://pic.xiahunao.cn/洛谷P1107 [BJWC2008] 雷涛的小猫)
![[特殊字符] Excel 提取＋图片批量插入 | Python 自动化生成稽查报告 Word 模板](http://pic.xiahunao.cn/[特殊字符] Excel 提取＋图片批量插入 | Python 自动化生成稽查报告 Word 模板)
)
：下一代动态深度学习模型)
