本文为视觉 SLAM 学习总结。
在给定观测数据 z k , j z_{k,j} zk,j 时,如何估计状态变量 x k , y j x_k,y_j xk,yj,即同时定位和建图
本讲内容概要
- 最小二乘法的含义和处理方式
- Gauss-Newton,Levenburg-Marquadt 等下降策略
- Ceres 库和 g2o 库的基本使用方法
状态估计问题
最大后验与最大似然
回顾 SLAM 方程:
{ x k = f ( x k − 1 , u k ) + w k z k , j = h ( y j , x k ) + v k , j \begin{cases} x_k=f(x_{k-1},u_k)+w_k& \\ z_{k,j}=h(y_j,x_k)+v_{k,j}& \end{cases} {
xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
其中
- 相机位姿: x k = T k = e x p ( ξ k ) x_k=T_k=exp(ξ_k) xk=Tk=exp(ξk)
- 像素: s z k , j = K e x p ( ξ ^ ) y j sz_{k,j}=Kexp(\hat ξ)y_j szk,j=Kexp(ξ^)yj
- 噪声: w k ∼ ( 0 , R k ) , v k ∼ ( 0 , Q k , j ) w_k\sim(0,R_k),v_k\sim(0,Q_{k,j}) wk∼(0,Rk),vk∼(0,Qk,j)
最简单的情况是: f , g f,g f,g 都为线性函数,噪声服从高斯分布,这种情况在 CH10 中会讲到,使用 KEF 可以求得无偏的最优解。如果是非线性系统,和非高斯噪声,情况会变得很复杂,要分情况讨论。
以前通常用滤波器求解状态估计,该方法假设该系统具有马尔可夫性——系统下一时刻状态仅依赖于上一个状态。于是我们可以只维护上一个状态,当有新的输入进入时就更新一次状态估计。
近年来非线性优化已成为主流方法。
状态变量:所有待求解的量,即所有时刻的位姿和路标
x = { x 1 , … , x N , y 1 , … , y M } x=\{x_1,…,x_N,y_1,…,y_M\} x={
x1,…,xN,y1,…,yM}
状态估计实际上是求解条件分布: P ( x ∣ z , u ) P(x|z,u) P(x∣z,u),这里的 z , u z,u z,u 是统称。考虑最简单的情况,当只有观测时,该问题类似于求解一个 SfM(Structure from Motion) 问题。
由贝叶斯法则有:
P ( x ∣ z ) = P ( z ∣ x ) P ( x ) P ( z ) ∝ P ( z ∣ x ) P ( x ) P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x) P(x∣z)=P(z)P(z∣x)P(x)∝P(z∣x)P(x)
其中 P ( z ∣ x ) P(z|x) P(z∣x) 为似然, P ( x ) P(x) P(x) 为先验。
然而, P ( x ∣ z ) P(x|z) P(x∣z) 条件分布很难求解,但是我们可以求解:
-
最大后验估计(MAP)。因为分母与 x x x 无关直接省略。
x M A P ∗ = arg max P ( x ∣ z ) = arg max P ( z ∣ x ) P ( x ) x^*_{MAP}=\arg \max P(x|z)=\arg \max P(z|x)P(x) xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x) -
最大似然估计(MLE)。此时先验 x x x 未知
x M