论文笔记-ClusterSLAM：A SLAM Backend for Simultaneous Rigid Body Clustering and Motion Estimation

标题：ClusterSLAM: A SLAM Backend for Simultaneous Rigid Body Clustering and Motion Estimation
作者：Jiahui Huang Sheng Yang Zishuo Zhao Yu-Kun Lai Shi-Min Hu
来源：ICCV2019

摘要
提出了一种实用的双目视觉SLAM后端方法，它能够在动态场景中实现即时的单个刚体分割和运动估计。尽管当前基于因子图的状态优化算法通过将动态物体当作野点能够鲁棒地求解SLAM问题，然而动态运动估计很少涉及。本文利用从同一刚体上所提取特征点的3维运动一致性，使用统一方法来实现静态和动态物体的聚类与估计。特别地，本文的方法对特征点建立了一个感知噪声的运动一致性矩阵，而后采用聚类的方法，实现了特征点所属刚体划分。通过解耦因子图优化算法对特征点的坐标和刚体的位姿进行优化，从而能同时迭代更新聚类和运动估计。在合成数据集和KITTI数据集上的实验证明了本文方法的有效性，进一步的在线实验也展示了本文算法在同时进行运动估计和多物体跟踪上的有效性。

主要贡献

1. 与近期SLAM后端不同，该算法将场景中动态特征点聚类而非剔除，并进一步进行运动估计；
2. 在聚类和估计中均考虑关键点的测量不确定性，来提高聚类和运动属性估计的精度；
3. 使用大量输入帧来保持聚类一致性，以及解耦因子图优化方法来保持整个系统的效率。

目前，解决动态场景SLAM问题通常有两种思路：一是，将场景中的移动物体当作野点；二是，通过对场景的语义分割，将动态物体划分出来，实现对相机运动和移动物体运动的联合估计。第一种思路，通常采用运动一致性检测出运动的点，或者采用鲁棒估计算法，自动剔除异常点的影响，这类方法无法实现对移动物体的运动估计。第二种思路通常采用深度学习的方法，需要大量算力，通常难以应用在嵌入式平台上。
本文提出的方法，综合采用了以上两种方法的优势：首先，通过同一刚体上特征点的运动一致性这一先验信息，构造特征点的运动一致性矩阵，通过分层聚类，实现对不同刚体特征点的划分；另一方面，在得到刚体的特征点的划分后，对每个刚体（包括背景）使用解耦因子图优化方法进行运动估计和三维点坐标优化。通过交替进行上述两个步骤，实现了对聚类结果和运动估计的同时优化。

备注：
$x_t^i=(u_l,v_l,u_r)$：第 t 帧，第 i 个特征点
$(u_l,v_l)$为左图坐标，$(u_r)$为右图横坐标
$X_t^{\times ,i}\in R^3$：局部三维坐标
${\Sigma }_t^{\times ,i}\in R^{3\ast 3}$：第 t 帧坐标系中第 i 个特征点的不确定性
$f:x_t^i\to X_t^{c,i}$：反向投影函数，将坐标投影到相机坐标系 c 中
${\Sigma }_t^{c,i}=J_f{\Sigma }_t^i{J}_f^T$：观测误差，$J_f$ 投影函数f的雅克比矩阵，${\Sigma }_t^i$ 图像中观测点协方差
$P_t^q\in SE(3)$：第 t 帧聚类 q 的位姿
$P_t^c$：第t帧相机的位姿
指定静态特征点聚类 q=0，且 $P_t^0=I$

1. 特征点聚类
   
   $d^{ij}\in D$：特征点 i , j的运动不一致性，
   $||x|{|}_{\Sigma }^2\triangleq x^T{\Sigma }^{-1}x$：Mahalanobis distance马氏距离，使用同时包含特征点ij的图像来计算马氏距离
   第一项 $l_t^{ij}$表示在第 t 帧中，特征点 i 和特征点 j 的三维坐标的差值，$l_{\ast }^{ij}$为由于相机运动带来的点的三维坐标的变化，因此$l_t^{ij}-l_{\ast }^{ij}$ 即为由于特征点属于不同刚体带来的运动不一致性。第二项表示的是特征点在图像坐标系中的距离，其含义是，同一刚体上的点，在同一帧图像上的坐标也应该比较接近。
   由于特征点的三维坐标的估计存在一定的误差，这一误差也会给特征点的聚类带来误差，因此上式充分考虑了特征点坐标估计的不确定性，在后续过程中，通过点的坐标估计的优化，进一步更新上述不确定性矩阵$\sigma$和聚类的结果。

2. 刚体运动和特征点坐标估计
   在得到场景中特征点的聚类结果后，可以实现对每一类（每个刚体上的）特征点进行位姿估计和三维点坐标优化，该过程主要分为：初始化，刚体运动估计，特征点三维坐标优化三个步骤。首先，对于每个第一次检测到的刚体，初始化其刚体与相机坐标系的转换关系为 I（单位矩阵），对于其后的每一帧，估计该帧中刚体相对于第一次检测到该刚体的帧的相对位姿，由于双目相机可以恢复点的三维坐标，因此这一过程可以看做一个点云配准问题：
   
   随后，对每个特征点的在刚体坐标系下的三维坐标进行优化：
   
   第一项为相机坐标系三维点与刚体坐标系三维点的配准误差，第二项表示在刚体坐标系下，属于该刚体的点的三维坐标应该保持不变。通过求解上述优化问题，实现了对刚体特征点坐标的优化。
   上述过程将刚体的运动估计和特征点的坐标估计分开进行，会带来较大的误差，因此和其他SLAM系统一样，需要在后端对刚体运动和特征点坐标估计进行联合优化，即：
   

3. KITTI数据集测试
   
   转载自泡泡机器人SLAM，感兴趣可以多多关注
   https://mp.weixin.qq.com/s/X4sKR3WeGmldNi1V2zRwmw