HoloLens中的视觉SLAM介绍

HoloLens是微软在2015年1月22日凌晨发布的一款头戴式的便携全息计算机设备。不同于VR眼镜，HoloLens直接在现实世界中呈现出一些虚拟的全息投影，即混合现实，不仅增强了真实感，还提供了一种新的人与现实世界交互的方式。

HoloLens与用户的交互主要有以下三种方式：
1、凝视（gaze）
HoloLens可以探测到用户视线的焦点所在从而做出一些判断和反应。
2、手势（gesture）
HoloLens可以通过识别用户的手势，如手指轻敲、张开等，来对全息影像进行交互操作。
3、声音（voice）
HoloLens内置了语音识别功能，用户可通过语音来进行一些操作。

本文主要介绍HoloLens上配齐的视觉导航的硬件以及使用的SLAM技术

HoloLens上的导航硬件配备

HoloLens上有4个环境感知摄像头、1个深度摄像头、1个IMU（惯性测量单元）

IMU指的是惯性测量单元。IMU大多用在需要进行运动控制的设备，如汽车和机器人上。也被用在需要用姿态进行精密位移推算的场合，如潜艇、飞机、导弹和航天器的惯性导航设备等。

惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

HoloLens的扫描范围在这个75°的圆锥里，近到0.8米、远到3.1米的范围内。（期待未来这个范围有质的突破）近于0.8米，远于3米范围的东西不会被扫到，hololens可以覆盖到的水平和垂直角度是120度

HoloLens自带CPU+GPU+HPU可以进行独立的计算和运行。

HoloLens在CV方面的局限性

目前HoloLens 的使用场景主要受限于现在还不成熟的CV技术，所以 HoloLens 只能在小型室内、中等光照、较简洁的空间里使用。HoloLens 的slam技术，官方叫 spatial mapping 。每秒钟会『拍』5帧画面，然后对周围的空间进行『学习』和『理解』。而 spatial mapping 的使用场景一般为 Room scan，也就是是在房间尺度内进行扫描。

HoloLens的局限

1、用户快速移动，比如猛地一回头；光线不足——因为黑暗吸收红外光，所以如果周围很暗，那就整个scanning 出错。或者某个黑暗的角落，吸收掉红外光导致信号衰弱（想象曝光不足）。

2、墙壁没有特殊颜色，惨白的一片，HoloLens无法找到interesting point或有特征的landmark点。

3、 多个重复的物体，这时想在空间中定位自己的位置就很难。

4、 RGB camera很难区分同一场景下不同的环境光，即使landmark是相同的地方也会被当做不同的地方，因为它们的色彩、光照、阴影、明暗面……看上去差异太大了。反光的镜子，导致mapping 出现幻象；

VSLAM技术

HoloLens的三维场景重建利用的是Richard Newcombe发明的Kinect Fusion

KinectFusion由四部分组成（图1）：首先，处理采集到的原始深度图，获取点云voxel的坐标以及法向量坐标；接着使用ICP算法，根据当前帧的点云和上一帧预测出的点云计算当前相机的位置姿态；然后，根据相机位置姿态更新TSDF值，融合点云；最后根据TSDF值估计出表面。

**KinectFusion算法运行流程**

首先对原始数据进行处理

首先对原始深度数据进行双边滤波处理，目的是保持清晰的边界。一般的滤波是在空间域做加权平均，像素越靠近中心点，权重越高。双边滤波是在空间域加权平均的基础上再对值域加权平均，即像素灰度值越靠近中心像素的灰度值，权重越高。在边界附近，灰度值差异很大，所以虽然边界两边的像素在空间域靠在一起，但是由于灰度值差别非常大，对于互相的权重很低，所以可以保持清晰的边界。拿到降噪后的深度图D_k之后，再根据相机内参K，可以反投影出每个像素点的三维坐标，这就是Vertex map V_k。公式中u是像素坐标，u˙是对应的齐次坐标。每个vertex的法向量可以很方便的通过相邻vertex用叉乘得到。然后对深度图降采样，行数、列数各减一半。降采样使用的是均值降采样，即深度图上四个相邻像素的深度值被平均成一个值。构建三层金字塔的目的是为了从粗到细地计算相机位置姿态，有加速计算的效果。

KinectFusion原始处理流程

相机的位姿估计

传统的三位重建中的相机位姿估计基于SfM(Structure from motion)，Kinect Fusion则采用ICP计算点云配齐。
ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。

ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对， 计算最优刚体变换，直到满足正确配准的收敛精度要求。

ICP 算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数R和平移参数 T，使得两点数据之间满足某种度量准则下的最优匹配。
假设给两个三维点集 X1 和 X2，ICP方法的配准步骤如下：

第一步，计算X2中的每一个点在X1 点集中的对应近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步， 如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

最近点对查找：对应点的计算是整个配准过程中耗费时间最长的步骤，查找最近点，利用 k－d tree提高查找速度 K－d tree 法建立点的拓扑关系是基于二叉树的坐标轴分割，构造 k－d tree 的过程就是按照二叉树法则生成，首先按 X 轴寻找分割线，即计算所有点的x值的平均值，以最接近这个平均值的点的x值将空间分成两部分，然后在分成的子空间中按 Y 轴寻找分割线，将其各分成两部分，分割好的子空间在按X轴分割……依此类推，最后直到分割的区域内只有一个点。这样的分割过程就对应于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点。这样点的拓扑关系就建立了。

TSDF更新三维地图

SDF描述的是点到面的距离，在面上为0，在面的一边为正，另一边为负。TSDF(Truncated SDF)是只考虑面的邻域内的SDF值，邻域的最大值是max truncation的话，则实际距离会除以max truncation这个值，达到归一化的目的，所以TSDF的值在-1到+1之间

TSDF更新方法
TSDF利用GPU并行处理各个voxel。首先把每个voxel根据计算出的相机位置姿态投影到相机上，如果在相机的视椎内，则会有一个像素点和它对应， ()是这个像素点距离表面的实际测量值， − 则是voxel到相机的距离，两者的差就是SDF值。然后用max truncation归一化得到当前TSDF值。接着，用加权平均的方式更新TSDF值。voxel越正对着相机，越靠近相机，权重越大，用公式表示就是：()∝()/ ()，是的像。论文中也提到把权重全部设为1，对TSDF做简单的平均。

表面估计

更新完TSDF值之后，就可以用TSDF来估计voxel/normal map。这样估计出来的voxel/normal map比直接用RGBD相机得到的深度图有更少的噪音，更少的孔洞（RGBD相机会有一些无效的数据，点云上表现出来的就是黑色的孔洞）。估计出的voxel/normal map与新一帧的测量值一起可以估算相机的位置姿态。具体的表面估计方法叫Raycasting。这种方法模拟观测位置有一个相机，从每个像素按内参投射出一条射线，射线穿过一个个voxel，在射线击中表面时，必然穿过TSDF值为一正一负的两个紧邻的voxel（因为射线和表面的交点的TSDF值为0），表面就夹在这两个voxel里面。然后可以利用线性插值，根据两个voxel的位置和TSDF值求出精确的交点位置。这些交点的集合就呈现出三维模型的表面。

回环检测

KinectFusion中回环检测使用的冲定位方法为随机蕨法（Random ferns）

这种重定位方法将相机的每一帧压缩编码，并且有效的对不同帧之间相似性进行评估。而压缩编码的方式采用随机蕨法。在这个基于关键帧的重定位方法中，采用基于fern的帧编码方式：输入一个RGB-D图片，在图像的随机位置评估简单的二进制测试，将整个帧进行编码，形成编码块，每个fern产生一小块编码，并且编码连接起来可以表达一个紧凑的相机帧。每一个编码块指向一个编码表的一行，和具有等效的编码、存储着关键帧id的fern关联起来，编码表以哈希表的形式存储。

当不断采集新的图片时，如果不相似性大于阈值，新进来的帧的id将会被添加到行中。在跟踪恢复的时候，从哈希表中检索姿态，将最相似的关键帧关联起来。一个新的帧和之前所有编码帧之间的不相似程度通过逐块汉明距离（BlockHD）来度量。

当返回值是0时，两个编码块是相似的。当返回值是1时，代表至少有一位不同。因此，BlockHD代表不同编码块的个数。块的长短不同，会直接影响到BlockHD在找相似帧时的精度/召回性质。判断一张图片是否满足足够的相似性需要设定一个最小BlockHD, 对于每一张新来的帧，计算

κ_I表示新的一个帧提供了多少有用的信息，如果新的一帧κI值很低，代表该帧和之前的帧很相似，如果κI值高，表示这个姿态是从一个新的视角拍摄的，理应被存为关键帧。通过这样的观测，可以试试捕获追踪帧，并且自动决定哪些应该被存为关键帧。通过值κ_I和一个实现确定好的阈值t,可以决定新来的一帧是应该添加到哈希表中，还是被剔除。这种找到关键帧并检索位姿的方法可以有效的减少三维重建的时间，并且适用于目前开源的slam算法。

源代码连接：
https://github.com/Nerei/kinfu_remake

参考连接：
https://www.cnblogs.com/zonghaochen/p/8325905.html
https://blog.csdn.net/weixin_30699831/article/details/96659910
https://www.cnblogs.com/CV-life/p/11420320.html