深度干货：详解基于视觉+惯性传感器的空间定位方法
在今年的Oculus Connect大会上，扎克伯格公布了一款正在开发的VR一体机产品——Santa Cruz。细看报道你会发现，这款产品除了将计算单元都集成到头盔中，还在前面增加了4颗摄像头。这些摄像头的作用，其实是用来实现空间定位的，在Oculus Rift CV1上，空间定位系统需要通过一套外部传感器来实现，这显然不适合移动方案。
许多人都说VR一体机是未来，而移动VR要真正移动起来，就需要摆脱外部空间定位基站的束缚。那么如何用另一种方案来摆脱有线VR通常使用的Outside-in空间定位方案呢？那就是Inside-out空间定位技术，此类方案大都基于计算机视觉技术，包括微软、Google和Facebook在内的科技巨头都在进行相关研究。
所谓的Inside-out追踪到底是如何做的呢？它使用了哪些技术，又有什么难点，本期公开课将为您解答这些疑问。
嘉宾介绍
马赓宇，清华大学计算机系本科，硕士，以及博士。马博士于2015年9月加入uSens凌感科技，现任凌感科技技术副总裁和中国研发中心主任。
在清华，马博士在人工智能与媒体集成实验室主攻人机交互和人工智能。马博士曾任三星中国研究院（SAIT China）首席研究员与研发主任，是三维显示与人机交互技术以及虚拟现实技术的专家，为Timothy Sauer所著教科书《Numerical Analysis》的译者。
以下内容整理自马赓宇博士本次公开课演讲，您将看到以下内容：
大家好，本次讲座的主题是基于视觉+惯性传感器的空间定位方法，前半部分是基于视觉的定位方法，以前和学校及研究机构做得比较多。后半部分介绍惯性传感器在定位中的应用。惯性传感器是最近几年才兴起的，主要是因为手机传感器（这方面主要是iPhone）的发展，传感器的精度也在逐年提高。
手机里面的陀螺仪现在基本可以达到0.01度的误差，加速度计虽然误差稍微大一点，如果用来估计针尖运动也可以起到很大的辅助作用。
空间定位方法简介
这里列举了一些空间技术应用场景。据我了解空间定位是从军事上开始应用的，主要是导弹的定位，可以在没有GPS导航的情况下，根据自身的IMU和加速度计达到在几千公里外只差几百米的误差。然后像室外的自动驾驶、无人机，室内的机器人、吸尘器等都是需要SLAM技术的。
SLAM（simultaneous localization and mapping）就是同时进行场景的建模和相机自身位置的定位，它的用途就是估计一些设备在场景中的位置和相对运动轨迹。
在VR的场景里呢，因为需要模拟出头部运动后一些虚拟物体在场景中的位置，所以要实时得到头相对于世界的位置。图示中的头部位置有六个参数，三个表示旋转三个表示位置一共六个自由度。旁边有手和控制器，意思是在VR的应用里除了要知道这个设备相对于世界的位置还要知道手相对于设备、以及控制器相对于头盔或世界的位置。
然后我们在实验过程中发现，VR环境和其他一些应用环境相比，对SLAM的性能要求有些不同。在VR环境下对用户影响比较大的几个指标是：
延时，也就是说头运动之后一般二十毫秒之内就能把运动反馈到渲染部分；
相对位置精度，很小的运动都能识别；
稳定性，如果头不动，识别出来的结果不能左右抖动。
相对不太重要的指标，一是loop closing，比如说我在院子里走了一圈，回到起始位置时它识别的结果是否和刚才的起始位置重合。戴上VR头盔以后看不到世界场景，有可能一米的误差也感觉不到。二是绝对误差，戴上头盔以后头部转了100度，识别出来90度，实际用户也体验不到差别；那么第三个绝对位置也就好理解了。
下面介绍几种方向传感器，也就是陀螺仪的原理。
机械陀螺仪
可能我们最了解的是以前上物理课见过的机械陀螺仪。机械陀螺仪里面有一个转子，由于能量守恒的原因，整个设备高速旋转，而转子的方向是不变的，通过这种原理确定这个设备的方向。这种设备可能早期在航海里应用，但现在应该基本不用了。
激光陀螺仪
中间是现代高精度的激光陀螺仪，在军事上和一些高精度应用上使用。图中中间是激光发射模块，往两个方向发射激光，左上角的传感器会计算两束光路距离的差。当设备静止时，两边的光线长度一样；当设备旋转时，两边的光路长度会不一样，通过距离差来计算旋转速度。激光陀螺仪是很难小型化的。
MEMs陀螺仪
手机常用的是基于微机械结构的MEMs陀螺仪，它可以做到很小。两个黄色区域中间通过一个轴固定，利用科里奥利力——旋转物体在有径向运动时所受到的切向力，来计算出角速度。
图中是一个加速度计的示意图。基于弹簧的原理，一个重物两边有弹簧固定，如果有加速度，重物在弹簧上的位置就会改变，位置的改变用电信号输出就是它的加速度。对比之下，陀螺仪识别出来的是角速度的值，因为物体在旋转时会产生离心力，陀螺仪就是把离心力的大小输出出来了。本质上陀螺仪和加速度计识别的都是受力，等价于加速度。
这类磁场传感器是基于磁场的。比如说手机里的数字罗盘就是可以感应出地球磁场的方向。另外一种Razer Hydra遥控器，中间的球会产生一个强磁场，手柄上有一个传感器可以识别出磁场的强度和方向，以此来进行手柄位置的定位。
总结以上三种常见的传感器的优缺点，陀螺仪精度很高，手机上的采样率可达到100hz,一些先进的头盔可达到1khz的采样率。加速度计的精度比较低，从芯片的参数表来看，误差达到0.1米每秒方的加速度。如果用加速度进行积分，经过1秒钟的累计误差会到0.05m，误差较大。
Outside-in定位
介绍一些基于Outside-in定位的一些VR头盔设备，比如流行的Oculus、HTC VIVE、PSVR。主要讲讲HTC VIVE Lighthouse的工作原理。因为我们觉得他是性能最好的定位算法，而且设计很有意思，如果把它理解成一个图像的话，可以达到很高的分辨率。
发射器有两排灯，起始时会全部照亮以示初始时间，分别有竖直和水平的两束光线旋转扫描，头盔上的记录器会记录光线扫到的时刻。如果把时刻转换成图像坐标，那么简单理解就是每次扫描可以得到一张图像，图像记录的是每个红外感应点在图像上的XY坐标。这样有个好处就是如果时间很精确，图像的分辨率会很高。
现在的数码相机分辨率是有限制的，一般做SLAM的达到1080p就很高了。另外，每个LED的ID 都是已知的，不需要在图像上进行匹配或跟踪，就可以知道自己的图像坐标。
Inside-out定位
微软的Hololens基于四个鱼眼相机来进行slam定位，不需要在场景里设置传感器。高通发布VR820一体机头盔的参考设计，前面有两个摄像头，使用高通芯片的DSP来进行处理，也能达到inside-out的效果。我们公司出的Power Fingo也是通过两个红外摄像头进行跟踪，里面有自带的高通820来进行计算，也是Inside-Out的SLAM硬件。
上图是三种跟踪算法的比较
上图是比较误差对Outside-In 和Outside-Out的影响。Outside-In的方法是假设传感器是在外面的，认为外面有个摄像头，头盔上的Marker点在摄像头的成像算出二维或三维的位置，设备角度通过IMU来得到。如果角度有误差，位置是不会影响的。因为这些点的位置不变，如果角度变化最多识别出红框的结果，至少位置上是相对稳定变化不大的。
Inside-Out的方法是头盔上有摄像头来识别外部的点，假设外部点离设备一米远，如果识别出的角度差了1度，位置可能就会差几厘米。如果大家仔细体验Hololens就会发现，在凑近虚拟物体时，距离头盔一分米内的物体就会慢慢变黑不显示。因为它识别出的头盔位置的误差会有一些抖动，比如一米远的物体有稍微抖动，成像时显示出来可能就很小，如果物体就在眼前，显示出的抖动会很大，所以Hololens采用取折中方法，近处的物体不显示，以此减小SLAM误差的影响。这也说明Inside-out的方法对精度的要求会更高。
视觉和IMU融合的SLAM算法
下面介绍视觉和IMU融合的SLAM算法，包括有什么改进可以提高SLAM精度。先来看一些Demo。
第一个Demo是融合IMU数据和基于单个普通相机的PTAM算法。在笔记本的屏幕上可以看到桌面上有四个眼睛是虚拟的物体，整个场景是以AR的方式来显示场景，眼睛的位置和真实场景绑定。如果SLAM的效果好，那么当相机运动的时候，这4个眼睛的位置依然绑定在真实场景的物体上不会变。 在这个Demo中大家可以看到，头盔的运动（旋转）速度很快，这是在模拟真实的VR环境，人戴着头盔时一秒钟可以运动180度，有很快角速度。原始的PTAM算法在这种情况下会跟踪失败。 而这个Demo毫无问题，因为融合了IMU才能达到这么稳定的跟踪。
第二个Demo是在手机上运行的基于Mark点的SLAM算法，桌面上有4个红外反射点。这个初音跳舞的Demo，可以通过移动手机（通过头盔也可以）在不同的位置来看，这个就是和Gear VR最大的区别， Gear VR只能识别旋转，而前面运动场景是不会变的。
第三个Demo是Marker点SLAM算法的测试程序，手机在Mark前晃动，这个程序显示的红线是相机的运动轨迹，我们也模拟VR环境下头的快速运动，即使以很快的速度晃手机，也可以跟踪到很好的轨迹，不会丢失。
这些算法与网上一些SLAM定位算法或者基于手机的AR算法的最大区别就是，在快速运动或快速旋转时也都可以很好的跟踪到。在VR环境下必须保证SLAM永远是对的，必须保证即使多快的运动都不能丢失。否则如果丢失了再找回来，用户看到的就是场景跳了一下又回来了，体验是很差的。
基于视觉的3D立体几何的基本原理
这一页介绍一下基于视觉的3D立体几何的基本原理，需要了解的两个基础知识，一个是3D坐标变换，一个是针孔相机模型。左边是成像的基本原理，相机两个关键帧，一帧在c1位置，一帧在c2位置，同时观测到一个三维空间点，就是前面蓝色的点，这个点和相机光心的连线，这条连线和成像平面的交点，就是这个点在图像上的坐标，这就是基本的针孔相机模型。
因为这个3D点的位置在空间中是不变的，所以这两个相机的方向和位置以及3D点的位置就满足一个方程，简单说就是这两条射线要相交。
右边是对应的一个投影方程，XYZ就是3D点的空间坐标，T是相机的光心，XYZ减T再乘以R就是3D点在图像坐标系下的坐标。左边的矩阵一般叫K矩阵，F是相机的焦距，cx、cy是相机的光心，K乘以相机坐标系下的坐标就等价于得到了这个点的图像坐标。
这一页是方程的具体应用：
第一个应用是相机定位，就是在这个方程里已知3D点位置和投影点的图像坐标，求相机的位置和方向R，T；
第二个应用是已知好几个R和T，就是已知好几帧图像，以及这个图像3D点的投影位置，要求3D点的世界坐标，实际上就是刚刚那一页左图里边，已经两条射线的方向，要求它们的交点；
第三个应用是最复杂的，SLAM中Localization是相机定位，mapping求解3D坐标，这个公式里边如果相机位置和3D坐标都不知道的话，就需要多帧图像，只知道u、v，x、y、z都不知道的话，需要很多个这样的方程进行方程组求解，也是可以解出来这些未知数的。
还有一些进阶的技术知识，简单列举一下。第一是四元素，之所以用四元素表示旋转，是因为如果用欧拉角表示旋转，无法保证表达的连续性。还要了解各种非线性方程组求解的算法。虽然现在有些工具包是解SLAM问题的，但基本上都没有融合IMU，或者说融合方法千差万别。在VR需求下要求低延迟、相对位置准，对绝对值要求不高，方程都是需要改变的，对于改变的方程要求解的话就需要了解各种非线性议程组的求解方法，下面列了这些，常用的是最后这种。
Visual SLAM 算法
介绍一下Visual SLAM 算法，一些论文里边只用了视觉的方法（单目相机）进行SLAM，包含Localization和Mapping，这两块已经介绍了。Visual+IMU的融合，主要改进是在Localization的模块，Mapping是建立三维点坐标的过程，这是用不到IMU的，只是在Localization上要精确估计出相机的位置，才需要IMU的融合。
纯Visual SLAM的方法，主要是在场景中提取特征点，进行特征点匹配，但这种方法在室内场景，尤其是特征点比较少，主要特征都是线和边缘的场景，很容易跟踪失败。
最近几年有一些Direct Method，不是通过特征点匹配而是直接通过图像像素颜色匹配来优化相机位置，代表的一些论文算法包括LSD-SLAM和DSO-SLAM，下面有两个YouTube视频供参考。
他们的效果是基于纯视觉算法中最好的了，但是像刚刚那边快速头盔旋转，他们的Demo如果出现这种情况肯定会丢失。
这页是DSO算法的简单介绍，在图像management上要保留7个关键帧，新来的每帧图像会和最近的帧的特征进行比较，优化新图像的姿态（Localization）。到了一定范围之后就要添加新的关键帧，把所有关键帧一起优化（Mapping），来更新3D点的位置。右边是运行示意图，上边那些黑色的点是历史建出来的3D点的点云。
红色的一条线，是相机运动轨迹，蓝色的小框是选择的那些关键帧，下面这些图，上面右下角是最近的关键，后面是历史上的一些关键。在现在的场景，如何最远的那个关键关键帧中那些点太少，就会去掉，生成一个新的关键帧。
Vision+IMU融合
Vision+IMU融合的产品包括HoloLens、Tango和高通的一体机。它的好处包括：
通过IMU可以减小图像匹配的搜索范围，在VR环境下头快速旋转时可以达到每秒180度的速度，如果没有IMU的话，相邻图像里边搜索的范围可能会很大，比如当前这个点在图像的最左边，下一帧可能就移动到图像最右边了；
可以在很少或没有特征点时继续跟踪；Tango一个比较好的性能就是，在没有特征点，比如扫过一面白墙时，也能够基本正确地跟踪一两秒的时间。
可以在图像帧间预测位置，因为摄像头的采集速率一般比较低，可能每秒30、60帧，最快有300帧（Intel ，而IMU可以达到每秒1000帧或者更高，如果把IMU加到图像帧间预测，那么SLAM就可以以每秒1000帧的速率输出位置，而不是只在有图像的时间输出位置。高通的VR820就可以以800Hz的速度输出头部运动的结果。
手机里边有很多传感器，陀螺仪可以输出采样时的角速度，加速度计可以输出这个时刻的加速度。还有磁场传感器，GPS。有些手机里自带的融合算法，将磁场、陀螺仪和加速度计整合到一起输出一个方向，它输出的方向看上去误差很小。
IMU的速度很快，1000Hz，但是相机的速度很慢，需要有一个解决方法。
从Gear VR开始提出ATW这个概念后，现在大部分头盔都实现了这个功能。简单介绍一下，用户看到图像的时间，是经过了好几个处理，从图像采集加上图像处理加上渲染引擎渲染的时间，才是用户最终看到的时间。
所以SLAM需要输出的，不单是有图像时刻相机的R和P，还有图像之后通过IMU积分的，当前最新时刻的R和P，以及在当前时刻往后预测的渲染结束时刻的R和P，最后输出的预测R和P才是渲染引擎需要知道的相机位置和方向。
SLAM对硬件的要求是很高的，上面列举了一些。对摄像头的要求是全局快门，手机里是卷帘快门，它的图像扫描每一行曝光是不同的，手机快速运动时图像会变形，这种处理起来会增加很多难度，虽然很多paper也处理这种问题，但现有的所有SLAM算法并不考虑卷帘快门的影响。
相机需要固定的焦距、光圈，焦距固定可以通过预先的标定来标定相机的焦距和畸变；光圈的固定可以让相机在运动过程中场景的亮度不会变化，方便特征点的匹配和跟踪。
相机需要大的视角，比如鱼眼或全景，视角越大跟踪的效果越稳定。还有相机的快门速度足够高，这样在相机高速运动和旋转时图像不会产生运动模糊。相机的帧率要足够高，这样相对帧的图像差距比较小，方便跟踪。
传感器的要求，外置高性能IMU会比手机自带的精度要高。加速度计的精度需要过一两年能发明出更好的加速度计对SLAM性能的改进也会有很大提高。
这页是列了一些uSens的产品，右上角是双目的彩色摄像机，Color Fingo，可以进行RGB SLAM，以及AR see through显示。左下角是Fingo红外相机，可以识别手识，以及进行基于红外Marker的SLAM跟踪。右下角是Power Fingo，集合了两个RGB和两个红外摄像头，同时内置有820处理器，可以机器内部进行手势识别和SLAM跟踪。
我们做的Markerless SLAM相比现有SLAM算法有多种改进，在预测和优化阶段都结合了IMU的信息。
Markerless的算法计算量比较大，在手机上跑不到实时快速运动跟踪效果，所以出于实用的目的，uSens也开发了基于Marker点的SLAM算法。下面两个图，左图是桌面级别的SLAM，需要在桌面放四个Marker反射点。右图是房间级别的SLAM，每面墙放6、7个，房顶6、7个，大概一个房间需要30个Marker点，这些Marker点的坐标都可以事先通过离线方法计算出来，Marker点的ID通过点之间的位置来确定，这种基于Marker的算法可以直接在手机上实时跑起来，战资源比较小，在手机上每帧图像是5ms之下的处理时间。
Q&A环节
Q：Markerless SLAM定位怎么样可以做到低延迟、高精度，撸棒性也很好呢？
A：Markerless SLAM是在三个阶段都整合了IMU信息，第一个阶段是已知当前帧相机的位置，通过IMU信息来预测相机的位置，可以预测到3D的空间点在下一帧图像的位置，这样就可以提高特征点跟踪的稳定性。
然后是在没有图像时候，通过IMU来预测相机的位置，输出给渲染模块。
还有是在优化相机位置时，不光考虑图像特征点的投影坐标，也考虑帧间和加速度计的信息来进行统一优化，就是那一页讲得最复杂的议程。
Q：为什么HoloLens的定位可以做得这么好？
A：HoloLens的SLAM做得很好，它的SLAM过程中是没有用深度相机的，完全依靠左右两侧的4个鱼眼相机，深度相机只在场景建模时候用，它跟我们的思路一样，分两个阶段，一是把场景里3D点的位置确定出来，后面的跟踪就只用特征点的图像坐标和IMU来处理。
它的IMU精度还不确定，我们做过实验，比如在电梯里戴HoloLens，电梯一启动一停时，场景整个就飞到上面飞到下面，它首先是相信IMU的数据，其次才是相信图像的数据。
它里面有一个专门处理器HPU，专门处理特征点的识别匹配问题，这样就可以达到实时的效果了。最后，他们的位置跟踪误差其实挺大的，如果你凑近看一个物体可以发现抖动挺大，再近一点就直接不显示了。
雷锋网原创文章，网页转载请注明来自雷锋网，署名作者和原文链接。微信转载授权，请联系雷锋网公众号（微信ID：leiphone-sz），详情见转载须知。