三维重建面试6：绑架问题/SensorFusion/IMU+CV-小尺度SLAM

前言

         机器人的“绑架”问题是指在缺少它之前的位置信息情况下，去确定机器人的当前位姿，例如当机器人被安置在一个已经构建好地图的环境中，但是并不知道它在地图中的相对位置，或者在移动过程中，由于传感器的暂时性功能故障或相机的快速移动，都导致机器人先前的位置信息的丢失，就像人质的眼睛被蒙上黑布条，拉上集装箱被运送到了未知的地方，此时，人质就无法给自己定位了。

        "绑架问题"可以分为初始化绑架和追踪丢失状态绑架。初始化绑架可以阐述为一种通常状况初始化问题，可使用蒙特卡洛估计器，即粒子滤波方法，重新分散粒子到三维位形空间里面，被里程信息和随机扰动不断更新，初始化粒子聚集到/收敛到可解释观察结果的区域。追踪丢失状态绑架，即在绑架发生之前，系统已经保存当前状态，则可以使用除视觉传感器之外的其他的传感器作为候补测量设备。

IMU-惯性传感器

        惯性传感器是检测和测量加速度、倾斜、冲击、振动、旋转和多自由度( DoF )运动的传感器。惯性传感器是解决导航、定向和运动载体控制的重要部件。    

        惯性传感器包括加速度计（或加速度传感计）和角速度传感器（陀螺仪）以及它们的单、双、三轴组合IMU（惯性测量单元），AHRS（包括磁传感器的姿态参考系统）。MEMS加速度计是利用传感质量的惯性力测量的传感器，通常由标准质量块（传感元件）和检测电路组成。IMU主要由三个MEMS加速度传感器及三个陀螺和解算电路组成，即九轴传感器。

        惯性传感器能够测量本体的角速度和线性加速度，可以通过标定+积分的方法辅助其他传感器得到进一步的测量信息。

      

IMU与视觉的对比

        使用视觉的方式，不管是稠密的、半稠密的、还是稀疏的，在相机位姿变化过快的时候都必然存在无法匹配的情景，导致先前的位置丢失，简称追踪失败。

        IMU可以测得角速度和加速度，由于物理理论基础和物理结构本身的原因，测得量都存在一定的误差。在短时间内能得到较为准确的位姿积分，但长时间积误差会逐渐累积，造成明显的漂移。

        相机的位姿转换是基于世界坐标系的，观察到的路标也会在世界坐标系中不断收敛，若不是计算错误，基本上不会产生漂移。而IMU是自身坐标系标定的，在转换到世界坐标系时候一定会产生误差，时序累积必定产生漂移。两种方式的优缺点互补，这就引发视觉SLAM的融合思想，既是SensorFusion。

      

SensorFusion松绑定和紧绑定

        对IMU在视觉融合中所占有的位置和状态变量的耦合程度，可以在算法中设定不同的观测和更新方式。

        松绑定：或许可以使用额外的同步时钟，对IMU进行特定短的时间进行积分，获取位姿转换。作为相机转换的初始位姿或者作为"绑架时间"内的视觉补充。在AR眼睛的SLAM算法中，我们使用了松绑定的方法。主要在策略层面进行改进，IMU的位姿积分作为相机的初始优化和转换位姿，并在“绑架时间”内进行单独位姿计算。

        紧绑定：或许可以使用另外的同步时钟，把IMU的状态和视觉状态参数进行融合，同步观测和更新。基于滤波的方式典型的紧绑定算法有著名的MSCKF算法。

MSCKF算法

         如有不适，请移步原文，或者联系删除

         参考文章：MSCKF算法流程框架

1. 初始化

         1.1 摄像机参数、噪声方差（图像噪声、IMU噪声、IMU的bias）、初始的IMU协方差、IMU和摄像机的外参数*、IMU和摄像机的时间偏移量*

         1.2 MSCKF参数：状态向量里滑动窗口大小的范围、空间点三角化误差阈值、是否做零空间矩阵构造和QR分解

         1.3 构造MSCKF状态向量

2.读取IMU数据，估计新的MSCKF状态变量和对应的协方差矩阵

3.图像数据处理

         3.1 MSCKF状态向量中增加当前帧的摄像机位姿；若位姿数大于滑动窗口大小的范围，去除状态变量中最早的视图对应的摄像机位姿

         3.2提取图像特征并匹配，去除外点。

         3.3 处理所有提取的特征。判断当前特征是否是之前视图中已经观察到的特征

         是：

前言

         机器人的“绑架”问题是指在缺少它之前的位置信息情况下，去确定机器人的当前位姿，例如当机器人被安置在一个已经构建好地图的环境中，但是并不知道它在地图中的相对位置，或者在移动过程中，由于传感器的暂时性功能故障或相机的快速移动，都导致机器人先前的位置信息的丢失，就像人质的眼睛被蒙上黑布条，拉上集装箱被运送到了未知的地方，此时，人质就无法给自己定位了。

        "绑架问题"可以分为初始化绑架和追踪丢失状态绑架。初始化绑架可以阐述为一种通常状况初始化问题，可使用蒙特卡洛估计器，即粒子滤波方法，重新分散粒子到三维位形空间里面，被里程信息和随机扰动不断更新，初始化粒子聚集到/收敛到可解释观察结果的区域。追踪丢失状态绑架，即在绑架发生之前，系统已经保存当前状态，则可以使用除视觉传感器之外的其他的传感器作为候补测量设备。

IMU-惯性传感器

        惯性传感器是检测和测量加速度、倾斜、冲击、振动、旋转和多自由度( DoF )运动的传感器。惯性传感器是解决导航、定向和运动载体控制的重要部件。    

        惯性传感器包括加速度计（或加速度传感计）和角速度传感器（陀螺仪）以及它们的单、双、三轴组合IMU（惯性测量单元），AHRS（包括磁传感器的姿态参考系统）。MEMS加速度计是利用传感质量的惯性力测量的传感器，通常由标准质量块（传感元件）和检测电路组成。IMU主要由三个MEMS加速度传感器及三个陀螺和解算电路组成，即九轴传感器。

        惯性传感器能够测量本体的角速度和线性加速度，可以通过标定+积分的方法辅助其他传感器得到进一步的测量信息。

      

IMU与视觉的对比

        使用视觉的方式，不管是稠密的、半稠密的、还是稀疏的，在相机位姿变化过快的时候都必然存在无法匹配的情景，导致先前的位置丢失，简称追踪失败。

        IMU可以测得角速度和加速度，由于物理理论基础和物理结构本身的原因，测得量都存在一定的误差。在短时间内能得到较为准确的位姿积分，但长时间积误差会逐渐累积，造成明显的漂移。

        相机的位姿转换是基于世界坐标系的，观察到的路标也会在世界坐标系中不断收敛，若不是计算错误，基本上不会产生漂移。而IMU是自身坐标系标定的，在转换到世界坐标系时候一定会产生误差，时序累积必定产生漂移。两种方式的优缺点互补，这就引发视觉SLAM的融合思想，既是SensorFusion。

      

SensorFusion松绑定和紧绑定

        对IMU在视觉融合中所占有的位置和状态变量的耦合程度，可以在算法中设定不同的观测和更新方式。

        松绑定：或许可以使用额外的同步时钟，对IMU进行特定短的时间进行积分，获取位姿转换。作为相机转换的初始位姿或者作为"绑架时间"内的视觉补充。在AR眼睛的SLAM算法中，我们使用了松绑定的方法。主要在策略层面进行改进，IMU的位姿积分作为相机的初始优化和转换位姿，并在“绑架时间”内进行单独位姿计算。

        紧绑定：或许可以使用另外的同步时钟，把IMU的状态和视觉状态参数进行融合，同步观测和更新。基于滤波的方式典型的紧绑定算法有著名的MSCKF算法。

MSCKF算法

         如有不适，请移步原文，或者联系删除

         参考文章：MSCKF算法流程框架

1. 初始化

         1.1 摄像机参数、噪声方差（图像噪声、IMU噪声、IMU的bias）、初始的IMU协方差、IMU和摄像机的外参数*、IMU和摄像机的时间偏移量*

         1.2 MSCKF参数：状态向量里滑动窗口大小的范围、空间点三角化误差阈值、是否做零空间矩阵构造和QR分解

         1.3 构造MSCKF状态向量

2.读取IMU数据，估计新的MSCKF状态变量和对应的协方差矩阵

3.图像数据处理

          3.1 MSCKF状态向量中增加当前帧的摄像机位姿；若位姿数大于滑动窗口大小的范围，去除状态变量中最早的视图对应的摄像机位姿

          3.2提取图像特征并匹配，去除外点。

          3.3 处理所有提取的特征。判断当前特征是否是之前视图中已经观察到的特征

          是：

                  3.3.1 如果当前帧还可以观测到该特征，则加入该特征的track列表

                  3.3.2 如果当前帧观测不到该特征(Out_of_View)，将该特征的track加入到featureTracksToResidualize，用于更新MSCKF的状态变量

         否：

                  3.3.4 给该特征分配新的featureID，并加入到当前视图可观测特征的集合

            3.4 循环遍历featureTracksToResidualize中的track，用于更新MSCKF的状态变量

                   3.4.1 计算每个track对应的三维空间点坐标(利用第一幅视图和最后一幅视图计算两视图三角化，使用逆深度参数化和高斯牛顿优化求解)，若三角化误差小于设置的阈值，则加入map集合

                   3.4.2 计算视觉观测(即图像特征)的估计残差，并计算图像特征的雅克比矩阵

                   3.4.3 计算图像特征雅克比矩阵的左零空间矩阵和QR分解，构造新的雅克比矩阵

            3.5 计算新的MSCKF状态向量的协方差矩阵

                  3.5.1 计算Kalman增益

                  3.5.2 状态矫正

                  3.5.3 计算新的协方差矩阵

            3.6 状态变量管理

                   3.6.1 查找所有无feature track可见的视图集合deleteIdx

                   3.6.2 将deleteIdx中的视图对应的MSCKF中的状态去除掉

                   3.6.3 绘制运动轨迹

简单分析补充：

1. IMU读取数据时尽量接近当前摄像机图像获取的时间（额外的同步时钟），最好先离线标定IMU的bias（一般是这样的，工程化时应该使用相同批次标定的IMU）

2. 离线标定好摄像机内参数，必要的话把畸变因子也标定出来（内参矫正是必须的）。算法运行时，利用摄像机内参数将图像特征归一化到摄像机坐标系。（PNP）

3. 图像特征选择ORB之类的(提取和匹配速度快，且具有旋转尺度不变性)，特征匹配时使用快速鲁棒的方法去除外点（RANSAC）

4. 若当前视角下场景纹理不丰富，提取的特征较少，则调用另一个函数，只利用IMU的读数做EKF更新（绑架问题备选方案-只使用IMU作为追踪方案）

5. 根据摄像机能观察到的track个数，摄像机之间的base长度、角度，确定关键帧，并构造摄像机视图的graph，根据摄像机之间的关系，利用g2o等工具优化摄像机轨迹。（平差方法）。

6. 可以利用并行线程，实时构建地图，并构造DBOW，用于回环检测和重定位（场景识别—模式识别）。

后记：

       在整个场景重建中，不同尺度的SLAM有不同的意义。SLAM在局部的改进可以使用多传感器融合的思路，在更为宽广的范围内，语义SLAM变得重要，SLAM本身又成为一个小比例的问题。