论文笔记VDO-SLAM: A Visual Dynamic Object-aware SLAM System

一.整体系统

先建立一个感性认识：
系统的输入：双目或单目的rgb图和深度图，像素级实例分割的结果，以及稠密光流。

1. 对静态背景进行特征追踪，使用求出的相机pose来求解运动物体pose并对其运动进行估计；
2. 用图优化使估计结果更加准确。在这个过程中会维护局部地图，并且随着每一个新帧都会更新。

最终系统的输出是：相机位姿与轨迹，动态物体位姿与轨迹，运动物体的速度，静态地图

二. 几何基础

1. 位姿表示

${}^0{X}_k\in SE(3)$ : 机器人/相机的3D位姿，0为世界坐标系，k是时间戳，也可以理解为是相机坐标系到世界坐标系下的转换矩阵
${}^0{L}_k\in SE(3)$ : 物体的3D位姿，即物体坐标系到世界坐标系下的转换矩阵

2. 3D点的坐标系转换

一个世界坐标系中的第i个3D点(齐次坐标)转换到某一帧k的相机坐标系：

$${}^{X_k}{m}_k^i{=}^0{X}_k^{-1}\ast {}^0{m}_k^i(1)$$
再把这个点从相机坐标系乘以内参矩阵转换到像素坐标系，得到齐次坐标：
$${}^{I_k}{p}_k^i=\pi {(}^{X_k}{m}_k^i)=K\ast {}^{X_k}{m}_k^i(2)$$

相机和环境中物体的运动都会产生2D光流，可以把从第k-1帧图像到第k帧图像的像素运动写成一个位移向量，这就是光流：
$${}^{I_k}{\varphi }^i{=}^{I_k}{\tilde{p}}_k^i{-}^{I_{k-1}}{p}_{k-1}^i(3)$$
相当于说，我们可以得到第k-1帧第i点的像素坐标，然后可以通过光流法找到第k帧中对应的像素点(带波浪线的项)。这篇论文就是使用光流法对相邻帧进行追踪。

3. 物体和3D点的运动

物体在相邻帧之间的位姿变换，可以用下式表示：
$${}_{k-1}^{L_{k-1}}{H}_k{=}^0{L}_{k-1}^{-1}\ast {}^0{L}_k(4)$$
个人理解：${}^0{L}_k$是第k帧时物体坐标系到世界坐标系下的转换矩阵；而${}^0{L}_{k-1}^{-1}$是第k-1帧时世界坐标系到物体坐标系的变换矩阵。所以求出的就是从第k帧到第k-1帧物体位姿的变换矩阵

在这个物体(刚体)上的点坐标系变换就如下，表示第k帧时这个点从世界坐标系转到这个物体坐标系的位置：
$${}^{L_k}{m}_k^i{=}^0{L}_k^{-1}\ast {}^0{m}_k^i(5)$$

那么，此时我们就可以得到物体上某一点在世界坐标系的位置和该点在这个物体坐标系位置的坐标转换关系(把式4代入式5)：
$${}^0{m}_k^i{=}^0{L}_k{\ast }^{L_k}{m}_k^i{=}^0{L}_{k-1}{\ast }_{k-1}^{L_{k-1}}{H}_k{\ast }^{L_k}{m}_k^i(6)$$
注意：对于刚体来说，刚体上的某一点相对这个刚体坐标系(以刚体上某一点作为坐标系原点)是固定的，也就是说${}^{L_k}{m}_k^i$在物体坐标系的坐标为定值${}^L{m}^i$,不管时间戳是多少，都不会变。所以说式6的最后一项为：（这里的n可以是任何整数）
Lkmki=Lmi=0Lk−1∗0mki=0Lk+n−1∗0mk+ni  (7)^{L_k}m_k^i =^Lm^i=^0L_k^{-1}*^0m_k^i=^{0}L_{k+n}^{-1} *^0m_{k+n}^i \ \ (7) Lk​mki​=Lmi=0Lk−1​∗0mki</