slam-learning

SLAM

slam, (Simultaneous Localization and Mapping)

传感器数据 → 前端视觉里程计 → 后端非线性优化 → 建图
→ 回环检测 →

核心

运动方程

$x_k=f(x_{k-1},u_k,w_k)$

其中，u_k是运动传感器的参数，w_k是噪声

观测方程

$z_{k,j}=h(y_j,x_k,v_{k,j})$

y_j是路标点，z_k,j是观测数据
$a\times{b}=\begin{Vmatrix}i&j&k\\a_1&a_2&a_3\\b_1&b_2&b_3\end{Vmatrix}=\begin{bmatrix}{a_2}{b_3}-{a_3}{b_2}\\{a_3}{b_1}-{a_1}{b_3}\\{a_1}{b_2}-{a_2}{b_1}\\\end{bmatrix}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\\\end{bmatrix}b\triangleq{a\wedge{b}}$

特殊正交群
$SO(n)=\{R\in{R^{n\times{n}}|RR^T=I\}$

特殊欧式群
$SE(3)={T=\begin{bmatrix}R&t\\0^{T}&1\\\end{bmatrix}}$

罗德里格斯公式
$R=\cos\theta{I}+(1-\cos{\theta})nn^{T}+\sin\theta{n}^{\wedge}$

tr(R)=1+2cosθ
Rn=n

相似变换
$T_s=\begin{bmatrix}sR&t\\0^T&1\\\end{bmatrix}$

仿射变换
$T_A=\begin{bmatrix}A&t\\0^T&1\end{bmatrix}$

射影变换
$T_p=\begin{bmatrix}A&t\\a^T&v\end{bmatrix}$

李代数
$so(3)李代数$

$se(3)李代数$

SO(3)指数映射
$Z\begin{bmatrix}u\\v\\1\end{bmatrix}=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}X\\Y\\Z\end{bmatrix}\triangleq{KP}$

SE(3)指数映射
$ZP_{uv}=Z\begin{bmatrix}u\\v\\1\end{bmatrix}=K(RP_w+t)=KTP_w$

其中， $\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$

李代数求导&扰动模型
李代数求导：
$\frac{\partial(RP)}{\partial\phi}=(-Rp)^\wedge{J_l}$

扰动模型(左)：
$\frac{\partial(Rp)}{\partial\varphi}=-(Rp)^\wedge$

相机与图像

相机模型:
$\frac{Z}{f}=-\frac{X}{X'}=-\frac{Y}{Y'}$

$Z\begin{bmatrix}u\\v\\1\end{bmatrix}=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}X\\Y\\Z\end{bmatrix}\triangleq{KP}$

$ZP_{uv}=Z\begin{bmatrix}u\\v\\1\end{bmatrix}=K(RP_w+t)=KTP_w$

切向畸变&径向畸变
双目相机模型
$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$

得到， $z=\frac{fb}{d},d=u_L-u_R$

非线性优化

非线性最小二乘

$\begin{Vmatrix}f(x+\Delta{x})\end{Vmatrix}_2^2\approx\begin{Vmatrix}f(x)\end{Vmatrix}_2^2+J(x)\Delta{x}+\frac{1}{2}\Delta{x}^{T}H\Delta{x}$

保留一阶梯度
$\Delta{x}*=-J^T(x)$

保留二阶梯度
$H\Delta{x}=-J^T$

高斯牛顿法
$f(x+\Delta{x})\approxf(x)+J(x)\Delta{x}$

最小二乘:
$J(x)^{T}J(x)\Delta{x}=-J(x)^Tf(x)$

令左式系数为H，右边为g： $H\Delta{x}=g$

列文伯格-马尔夸尔特法
定义ρ来确定信赖区域范围
$\rho=\frac{f(x+\Delta{x}-f(x))}{J(x)\Delta{x}}$

第k次迭代求解：
$\min_{\Delta{x_k}}\frac{1}{2}\begin{Vmatrix}f(x_k)+J(x_k)\Delta{x_k}\end{Vmatrix}^{2},s.t.\begin{Vmatrix}D\Delta{x_k}\end{Vmatrix}\leqslant\mu{,}$

其中，D=I
解得： $(H+\lambda{I})\Delta{x}=g$

视觉里程计1

前端 → 后端,提供好的初始值;特征点（Feature）作为路标,由关键点&描述子组成。

特征提取

ORB

FAST角点提取
- 极大值抑制(NMS)
- 灰度质心法(Intensity Centroid)
BRIEF描述子
- 汉明距离(Hamming Distance)
- 快速近似最近邻(FLANN)

根据匹配点估计相机运动

对极几何
P=[X,Y,Z]^T,
s₁p₁=KP, s₂p₂=K(RP+t)
得到：Ｅ＝t^R, F=K^-TEK^-1, x_2^TEx₁=p₂^TFp₁=0
- 本质矩阵
  E=t^R
- 单应矩阵
  $n^Tp+d=0,p2=Hp1,H=K(R-\frac{tn^T}{d})K^{-1}p_1$
- 本质矩阵
  $s_1x_1=s_2Rx_2+t$

ICP(双目相机 OR RGB-D)
PnP(3D-2D)
重投影误差
$\xi^*=arg\min_{\xi}\frac{1}{2}\sum_{i=1}^n\begin{Vmatrix}u_i-\frac{1}{s_i}Kexp(\xi^\wedge)P_i\end{Vmatrix}_2^2$
关于相机位姿的一阶变化关系：
$\frac{\partial{e}}{\partial\delta\xi}=\lim_{\delta\xi\rightarrow{0}}\frac{e(\partial\xi\oplus\xi)}{\partial\xi}=\frac{\partial{e}}{\partial{P'}}\frac{\partial{P'}}{\partial\delta\xi}$
同理，得到关于位置的一阶变化关系
$\frac{\partial{e}}{\partial{P}}=\frac{\partial{e}}{\partial{P'}}\frac{\partial{P'}}{\partial{P}}$