CN109255749B - 自主和非自主平台中的地图构建优化 - Google Patents

Abstract

本公开涉及自主和非自主平台中的地图构建优化。本公开的位置感知技术采用视觉惯性传感数据收集和分析硬件，参照特定示例实施方式实现了传感器、技术和硬件设计的使用方面的改进，其可以使特定实施例能够以提高的速度和准确度向机器提供位置感知。

Description

自主和非自主平台中的地图构建优化

与其他申请的交叉引用

本申请是2016年8月29日提交的名称为“用于自主和非自主地图构建的视觉惯性位置感知(Visual-Inertial Positional Awareness For Autonomous And Non-Autonomous Mapping)”的美国专利申请第15/250,581号的部分继续申请(代理人卷号：PERC1002-1)，该申请通过引用并入本文。

以下美国专利申请通过引用并入本文：2016年8月29日提交的名称为“用于自主和非自主设备的视觉惯性位置感知(Visual-Inertial Positional Awareness ForAutonomous And Non-Autonomous Device)”的美国非临时申请第15/250,419号(代理人卷号：PERC1000-1)，以及2016年8月29日提交的名称为“用于自主和非自主跟踪的视觉惯性位置感知(Visual-Inertial Positional Awareness For Autonomous And Non-AutonomousTracking)”的美国非临时申请第15/250,393号(代理人卷号：PERC1001-1)。

技术领域

所公开的技术总体上涉及检测移动设备的位置和定位，并且更具体地涉及视觉处理和惯性传感器数据在定位和引导技术中的应用。

背景技术

本节中讨论的主题不应仅仅由于在本节中提及而成为现有技术。类似地，不应假定在本节中提及的或与作为背景提供的主题相关联的问题先前已经在现有技术中认识到。本节中的主题仅表示不同的方法，其本身也可以对应于所要求保护的技术的实施方式。

自主机器人一直是科幻小说的素材。实现真正的自主机器人的一个技术挑战是机器人需要能够标识他们在哪里，他们曾经在哪里以及计划他们去哪里。传统的SLAM技术近年来有了很大的改进，然而，为机器人和自导式移动平台提供快速准确和可靠的位置感知仍然存在相当大的技术挑战。

随着近来诸如Oculus Rift^TM、PlayStation^TMVR、Samsung Gear^TMVR、HTC Vive^TM等虚拟现实耳机的广泛使用，一种新类型的设备已经兴起，该设备——不是自主的，而是由人类用户穿戴的——将从快速、准确和可靠的位置信息中受益。然而，在使机器和设备能够标识他们在何处，他们去过哪里以及计划他们将要去哪里的领域，仍然存在许多技术挑战。一个特别具有挑战性的领域涉及精确和快速地识别位置和障碍物。已经尝试了各种不同的方法。例如RFID/WiFi方法已被证明是昂贵的并且精度有限。已经发现基于深度传感器的方法成本高并且存在功耗和干扰问题。基于标记的方法要求将标记放置在工作区域内，这限制了设备可操作的有用区域。视觉方法目前速度很慢，导致在快速运动应用中使用时出现故障。这些方法也可能会受到模糊性的影响。然而，这些实施方案未能达到广泛采用所需的标准。

向迄今为止的设备提供快速、可靠、经济的位置感知的挑战在很大程度上仍未解决。

附图说明

在附图中，遍及不同的视图，相同的附图标记通常指代相似的部分。而且，附图不一定按比例绘制，而是通常将重点放在说明所公开的技术的原理上。在以下描述中，参考以下附图来描述所公开的技术的各种实施方式，其中：

图1示出了用于视觉惯性传感器的控制单元的示例。

图2示出了图1中的成像部件的示例。

图3示出了图1中的特征缓冲区的操作方法。

图4示出了图1中的惯性部件的示例。

图5示出了在其中可以实施本文描述的技术的成像配置的示例。

图6示出了视觉惯性传感系统的示例。

图7示出了实施跟踪过程的示例跟踪系统。

图8A示出了用于实施跟踪过程的跟踪系统的示例再定位过程。

图8B示出了在一种实施方式中再定位搜索策略选择过程的示例。

图9示出了在一种实施方式中占用网格地图的示例。

图10是一种在点云中改进3D点和关键帧姿态的代表性方法，以引导包括具有距离计算和多轴惯性测量单元(IMU)的相机的移动设备。

图11示出了重新投影误差的示例。

图12在其中可以实施一种实施方式的示例机器人引导应用。

图13在其中可以实施一种实施方式的示例VR应用。

图14在其中可以实施一种实施方式的示例AR应用。

图15是一种使用来自具有距离计算和多轴惯性测量单元(IMU)的相机的信息来引导移动设备的代表性方法。

图16是一种更新包括具有距离计算和多轴惯性测量单元(IMU)的相机的移动单元的位置的代表性方法。

图17是一种使用混合点网格来引导包括具有距离计算和多轴惯性测量单元(IMU)的相机的移动单元的代表性方法。

具体实施方式

参考附图进行以下详细描述。描述示例的实施方式来说明所公开的技术，而不是限制由权利要求限定的范围。本领域的普通技术人员将认识到以下描述中的各种等同变化。

本文件参考具体示例实施方式描述采用视觉惯性传感数据收集和分析硬件的位置感知技术。讨论组织如下。首先，将给出各种实施方式所解决的一些问题的介绍。然后，一个实施方式的高级描述将在体系结构级别进行讨论。接下来，讨论由一些实施方式所使用的过程，用以高效处理图像和惯性数据。最后，将参照(i)机器人和自导式自主平台，(ii)虚拟现实耳机和可穿戴设备，以及(iii)增强现实耳机和可穿戴设备的特定应用来说明所公开的技术。对具体示例的参考旨在说明而不是限制本文公开的方法。

传感器、技术和硬件设计的使用的改进可以使特定的实施方式能够提供改进的速度和准确度，然而，这种改进伴随着参数数量的增加并伴随着显著的存储器和计算要求。传统的自动引导的方法主要集中在单个传感器输入上。基于相机的方法相对精确，但受速度限制(大多数硬件提供30fps，最高60fps)，并且由于这些方法处理每个像素，而在计算上是昂贵的。基于惯性引导的方法存在零点或原点漂移的问题。此外，这些方法需要昂贵的硬件以获得有用的结果。存在基于旧技术的WIFI和RFID方法；然而，这些方法都以表明这些方法本身能力有限。基于深度传感器的方法是昂贵的。而且，这些方法需要主动感测，所以计算成本相对较高。最后，设备的主动感测可能会造成干扰问题。

为了克服一直处理大量图像数据的计算负担，可以使用惯性数据来估计由于引导下的机器的姿态变化而导致的环境变化。为了克服与惯性传感器相关的漂移问题，可以捕捉和处理图像以校正和更新基于惯性数据的姿态估计。此外，由RGB和灰度相机组合组成的立体成像传感器可以提供立体成像能力，成本低于立体RGB系统。更进一步地，使用低端传感器来构建传感器，例如，具有640×480分辨率的相机，消除了高端图像传感器的成本。更进一步地，使用低功率控制单元执行某些基于传感器的处理，而不是使用主机或处于引导下的机器的强大处理器，使得相对于传统方法以较低的成本使用系统。实施方式可以被部署在各种使用场景中，包括机器人或其他移动平台引导、虚拟现实/增强现实(VR/AR)耳机、眼镜或其他可穿戴设备等。

受益于采用如本文的位置感知技术的机器人应用的示例包括：

·看护者和服务机器人(在地面行走)

·吸尘/拖地/清洁地板的机器人。

·被命令在环境中携带对象的机器人。

·远程呈现机器人自动在远程环境中移动。

·跟随人员的机器人管家。

在上面列出的每个场景中，机器人利用本文描述的技术，从而跟踪其自己的位置并识别它遇到的对象。此外，由于机器人执行许多复杂的任务，每个任务都有实时限制，因此快速完成传感以加速感知流水线(perception pipeline)是有益的。为了克服由该处理施加的计算负担，实施方式将关于从主处理器到成像部件或数字信号处理器的图像处理的一些计算卸载。另外，由于它是一种携带有限电池的移动机器人，能耗是一项重大挑战。因此，一些实施方式将一些计算任务从主处理器卸载到低功率传感器模块，由此使得实施方式能够达到整体能量效率。因为降低机器人的成本使得机器人能够为更多的客户提供经济实惠，成本是移动机器人的一个问题，所以降低成本是传感器设计的另一个因素。因此，一些实施方式采用一个用于定位任务的低成本灰度传感器，以及一个用于识别任务的彩色传感器。这一设计点使这些实施方式能够在不牺牲性能的情况下相对于立体彩色传感器设计显著降低成本。

虚拟现实(VR)和增强现实(AR)场景需要可穿戴耳机跟踪其自己的位置，并且可能识别它遇到的对象。为了跟踪其位置，可穿戴耳机配备了位置自我感知设备，该设备通过立体惯性硬件传感器感测其自身的运动。因此，传感器产生可靠的惯性数据，以便后续的跟踪和地图构建流水线可以准确地推断设备的位置，从而确定耳机的位置。

在一个实施方式中，一个设备被嵌入另一个设备(例如，机器人、移动平台、可穿戴计算机、AR/VR耳机、眼镜、腕表或其他手表等)中，可用的计算资源是有限的，然而机器人引导或AR/VR处理的工作负荷需要实时性能，以及感测迅速完成以加速感知处理流水线(perception processing pipeline)。因此，一些实施方式通过将来自主处理器的一些计算卸载到传感器模块来实现这些目标。

另外，在AR/VR应用中，移动嵌入式设备承载有限的电池功率，使得能量消耗成为挑战。因此，一些实施方式将来自主处理器的一些计算卸载到低功率传感器模块，以便实现整体能量效率。

进一步地，成本是许多AR/VR应用中的问题，因为随着设备的成本降低，扩大了接触更多客户的可能性。因此成本是传感器模块设计的另一个因素。因此，一些实施方式使用一个低成本灰度传感器来进行定位任务，并且使用一个彩色传感器来进行识别任务。与立体彩色传感器设计相比，此设计可以显著降低成本，而不会牺牲性能。

在具有图像和惯性数据的机器人引导、VR和AR可穿戴设备的背景下，描述了根据所公开的实施方式的系统、装置和方法的示例。在其他情况下，所公开的技术可以应用于自主交通工具引导技术、VR和AR可穿戴设备、导航、电信系统、金融系统、安全交易、银行业务、商业智能、市场营销、采矿、能源等，并且使用声纳、音频和LIDAR数据。其他服务也是可能的，所以下面的示例不应被视为是在范围、背景或设置上起定义或限制作用。

所公开的技术涉及在单输入多数据(SIMD)体系结构内改进计算资源(例如，计算能力和存储器使用)在处理图像和惯性数据期间的利用。所公开的技术可以在包括精简指令集(RISC)的系统、模拟硬件环境等的任何计算机实施的系统的背景中实施。此外，该技术可以使用两个或更多独立且不同的计算机实施的系统来实施，这些系统彼此协作和通信。该技术可以以多种方式实施，包括作为过程、方法、装置、系统、设备、计算机可读存储介质(诸如存储计算机可读指令或计算机程序代码的计算机可读存储介质)，或者作为计算机程序产品，其包括其中包含有计算机可读程序代码的计算机可用存储介质。

所公开的技术可以在任何计算机实施的系统(诸如NEON ARM VFP9-S处理器、ARM核心处理器或兼容的处理器实施方式)的背景中实施。

另外，所公开的技术可以使用各种不同的成像传感器和技术来实施，包括RGB、灰度、二进制(例如，受制于阈值强度水平的数字图像)、IR、声纳、LIDAR或其组合。

系统概述

立体视觉惯性传感器

图1以框图格式示出了用于视觉惯性传感器的控制单元的示例。图1中的控制单元100可以耦合到外部存储器110、闪存(为了清楚起见，在图1中未示出)以及一个或多个永久存储器，例如HDD、光驱等(为了清楚起见，在图1中也未示出)。控制单元100包括存储器缓存108、USB I/O端口102、相机串行接口(CSI)I/O端口116、其便于通过成像部件118直接从相机(为了清楚起见，在图1中未示出)接收图像、内部集成电路(I2C)I/O端口104、具有单一指令多数据(SIMD)能力的处理器106、以及特征缓冲区120。控制单元100中的部件通过本地总线107相互耦合。在一个实施例中，外部存储器110是64位双倍数据速率(DDR)随机存取存储器(RAM)。在一个实施例中，具有SIMD能力的处理器106被实施为精简指令集计算机(RISC)体系结构。在一个实施例中，具有SIMD能力的处理器106被实施为NEON ARM VFP9-S。惯性部件112驻留在存储器缓存108内。

图2示出了图1中的成像部件118的示例。成像部件118包括直接存储器存取(DMA)202、图像去畸变处理器204、Shi-Tomasi处理器206、特征点去畸变处理器208、特征描述引擎210以及在成像引擎214控制下的光流特征对应处理器212。

如果通过相机串行接口(CSI)I/O端口116来自视觉传感器或相机的每个传入图像是至少3MB并且每秒捕捉30个图像，则每秒需要为传入的图像分配至少90MB的存储器。为了在短时间内处理相当大量的视觉数据，可以由成像部件118直接处理由单目辅助传感器中的相机捕捉的帧，以提取有用的角部特征并且为每个特征产生描述符。成像部件118可以被实施为数字信号处理器(DSP)。成像部件118直接处理传入图像而不涉及SIMD处理器106或将图像数据存储在缓存108中。在一些实施方式中，传入图像大小至少为2MB。在一些实施方式中，传入图像大小至少为1MB。在一些实施方式中，图像可以通过相机串行接口(CSI)I/O端口以每秒20个图像的速率到达。

在分析传入图像并且由成像部件118提取关键特征之后，特征缓冲区120存储所提取的特征。在一些实施方式中，特征缓冲区包括每个图像帧包含2-5KB特征数据的组(bank)。在一些实施方式中，特征缓冲区包括每个图像帧包含1-10KB特征数据的组。特征缓冲区120包括组一122、组二124和特征缓冲区控制器126。图3示出了特征缓冲区120的操作的方法300。成像部件118可以向组一(步骤302)或组二(步骤314)写入数据，但不是同时的。如果成像部件118当前正在将数据写入到组一并且组一变满(步骤304)，则特征缓冲区控制器126通知SIMD处理器106(步骤306)组一已满。同时，成像部件118开始将数据写入组二(步骤314)。SIMD处理器锁定组一(步骤308)，将组一中的数据复制到SIMD处理器内可用的L0缓存(步骤310)，并释放组一(步骤312)。如果组二变满(步骤316)，特征缓冲区控制器126通知SIMD处理器106关于组二已填满(步骤318)，并且成像元件118开始将数据写入组一(步骤302)。SIMD处理器锁定组二(步骤320)，将组二中的数据复制到SIMD处理器内可用的L0缓存(步骤322)，并释放组二(步骤324)。当然，对于本领域技术人员而言，使用其他组的其他实施方式将是清楚的。

图4示出了图1中的惯性部件112的示例。惯性部件112包括惯性测量引擎402，该惯性测量引擎402的实施中包括：时间戳处理器404，对来自惯性传感器(为了清楚起见，在图1中未示出)的惯性数据的集合施加时间戳；偏差校正处理器406，校正从带时间戳的惯性数据中读出的数据；比例校正处理器408，将存储的比例因子(scale factor)信息应用于校正的惯性数据；错位校正处理器410，校正惯性测量传感器的传感元件的错位；以及IMU图像坐标变换处理器412，计算描述惯性数据的参照系与图像数据的参照系之间的差异的变换。

视觉惯性传感器

图5示出了被配置用于确定位置信息的示例视觉惯性传感器实施方式。视觉惯性传感器500包括相机1 508、相机2 510、惯性测量单元(IMU)502和具有USB接口以向主机提供输出的计算单元(CU)100。相机508、510包括至少部分重叠的视场，以在视觉惯性传感器500的有效的景深范围内提供可立体视觉的部分514。使用相机508、510使视觉惯性传感器500能够产生图像深度信息，这对代理定位任务(包括跟踪、定位、地图产生和重定位)非常有用。在图5所示的代表性实施方式中，相机508和510主要用于从图像提取特征并提供多个功能的代理定位：首先，从代理定位中的图像提取特征，其次，提供用于基于深度学习的任务的原始信息，包括对象标识、对象跟踪、图像字幕等。

IMU502为代理定位流水线提供原始传感器数据，其使用高频率(>200Hz)的IMU数据以实时产生代理位置信息。在一个实施方式中，定位流水线将来自IMU 502与相机508、510的信息相组合，IMU 502以相对较高频率运行，以提供较不准确信息的频繁更新，相机508、510以较低频率(30Hz)运行，从而以较低频率提供更准确的信息。

控制单元100执行对传感器、IMU 502和相机508、510的控制，对来自传感器的传感器数据施加时间戳，执行预先计算以加速定位流水线，并且封装原始数据以用于通过USB102发送到主机。

USB接口102使视觉惯性传感器500能够与主机交互。主机(为了清楚起见，在图5中未示出)可以是使用由视觉惯性传感器500产生的数据的移动设备或台式/膝上型计算机、专用机器控制器、汽车控制模块、机器人控制器等。在各种实施方式中，主机可以执行额外的计算以实现代理定位和深度学习任务。在低功耗CU 100上执行数据预处理的实施方式可以将主处理器(与低功耗CU相比功耗要高得多)从执行这些任务中解脱出来。结果，这样的实施方式实现了提高的能量效率。

需要注意，一个实施方式平均了对齐的图像。在其他实施方式中，使用其他技术。还要注意的是，在另一个实施方式中，包括图像质量测量子步骤。因此，如果输出图像太暗或仍然不够锐利或清晰，则图像将被拒绝，而不会传递到流水线的其余部分。

传感器数据产生和处理

在一个实施例中，在CU 100上校正IMU原始数据，由此实现不需要来自主机处理器的额外处理，因此加速了传感器预处理流水线。

时间戳：

时间戳处理器404为控制单元100从IMU传感器502接收的每个集合的惯性测量数据施加时间戳，以确保视觉惯性传感器500保持时间精确的传感器数据流。这种对保持传感器数据流完整性的严格关注使实施方式能够提供可靠工作的代理定位。通过视觉惯性传感器对原始数据施加时间戳可以避免复杂的同步任务。

偏差校正：

偏差校正处理器406校正从带时间戳的惯性数据中读出的IMU数据。由于制造缺陷，IMU传感器通常存在偏差问题，因此其测量结果存在误差。如果不从测量中移除偏差误差，则偏差误差作为机械化过程的一部分被结合两次。在这种情况下，加速度中的恒定偏差(误差)成为速度的线性误差和位置的二次误差。姿态率(陀螺仪)中的恒定偏差成为速度的二次误差和位置的三次误差。该偏差可以从离线工厂传感器校准阶段获得。CU 100中的这个校准信息用于在CU 100上执行偏差校正任务。

比例校正：

比例校正处理器408将存储的比例因子信息应用于校正的惯性数据。比例因子误差是输入和输出之间的关系。如果输入为100％，则预期输出为100％。实际输出是线性效应的结果，其中输出与输入成比例但有缩放。例如，如果输入为10m/s2，但存在2％的比例因子误差，则输出测量值为10.2m/s2。比例因子可以从离线工厂传感器校准阶段获得。CU 100中的此校准信息用于在CU 100上执行比例校正任务。

错位校正：

错位校正处理器410校正惯性测量传感器的传感元件的错位。存在相互正交安装的三轴陀螺仪和三轴加速度计。然而，安装有误差，因此不完全在90度。这导致传感器之间的相关性。例如，假设一个轴完全指向上并且IMU是水平的。该轴上的加速度计正在测量重力。如果另外两个轴完全正交，它们不会测量任何重力的影响。如果存在非正交性，其他轴也会测量重力，从而导致测量值之间的相关性。非正交效应发生在传感器集合内(加速度计或陀螺仪之间)，传感器集合之间或传感器集合与外壳之间(封装错位)。仔细的制造和工厂校准可以帮助减少这种误差来源。系统运行期间的连续估计和校正也是一种用于最小化这种影响的方法。通过执行钻孔瞄准估算以确定IMU 502测量框架与传感器(物镜)框架之间的偏移，可以移除(IMU 502与外壳之间的)封装错位。错位数可以从离线工厂传感器校准阶段获得。CU 100中的该校准信息用于在CU 100上执行错位校正任务。

传感器视觉数据产生和处理

图像去畸变(Image Undistortion)：

图像去畸变处理器204校正捕捉的帧中的图像数据中的失真。图像失真通常指的是一种光学像差，它会使物理上的直线变形并弯曲，并使它们在图像中显得弯曲。由于光学设计而发生光学畸变。为了实现可靠的计算机视觉结果，图像去畸变处理器204可以在执行进一步处理之前校正图像失真。这可以通过使用传入图像大小的查找表并执行地图重新构建(remapping)操作来使整个图像不失真来实现。

特征点去畸变(Feature Undistortion)：

在处理流水线的其余部分不需要整个图像但仅需要图像内的特征点的情况下，特 征点去畸变处理器208对CU执行特征点去畸变操作。详细地说，这个操作在特征提取阶段之后运行，并且不破坏每个特征点。

特征检测：

Shi-Tomasi处理器206对图像帧执行特征检测。特征是图像的“有趣”部分。Shi-Tomasi特征点检测包括旨在计算图像信息的抽象并在每个图像点进行局部判定的方法，该判定涉及在该点是否存在给定类型的图像特征。得到的特征将是图像域的子集，通常以孤立点的形式出现。一些实施方式在CU 100上执行特征检测以将主机从执行此类任务中解脱出来并加速特征检测过程。因此，在一个实施方式中，处理包括：

·动作1：计算每个像素的特征值并确定(i)该特征是否被关注；

和(ii)对于被关注的特征，一种特征：

o两个小的特征值：特征不被关注

o一个小的，一个大的值：特征可能是边缘

o两个大的值：特征可能是角落

o其他类型的特征

·动作2：改进

o应用非最大抑制

o应用空间分级(spatial binning)

o应用启发式(heuristics)

o应用其他类型的改进

特征描述：

特征描述引擎210对检测到的特征执行特征描述。特征描述包括唯一地标识图像中每个检测点的方法。特征描述可用于比较和匹配不同图像之间的特征点。一些实施方式在CU 100上执行特征描述以将主机从执行这样的任务中解脱出来并加速特征描述过程。

特征描述引擎210的一个实施方式使用SIMD加速的ORB描述符来描述特征。对特征的描述可用于匹配目的并描述特征的唯一性。ORB描述符方法被选择是由于其相对旋转不变性和对高斯图像噪声的免疫性。ORB特征检测器和二进制描述符的一个例子可以在“ORB特征检测器和二进制描述符”中找到，http://scikitimage.org/docs/dev/auto_examples/plot_orb.html(上次访问时间：2016年8月17日)。有关ORB描述符的进一步信息，可以参考Ethan Rublee等人的“ORB：SIFT或SURF的高效替代物”，其通过引用并入本文用于所有目的。

2D特征对应产生：

光流特征对应处理器212为特征执行2D特征对应产生。特征对应计算用于标识出现在左侧和右侧相机中的特征点。一旦标识出任何两个特征点的特征对应，就可以将三角测量应用于特征点以获得空间点的深度。深度信息由稍后在定位流水线中的过程使用。一些实施方式在CU 100上执行特征对应产生以将主机从执行此类任务中解脱出来并加速特征对应产生。

一个光流特征对应处理器212的实施方式采用光流方法来计算两个图像帧之间的运动，其在每个体素位置(voxel position)的时间t和t+Δt处获得。一种称为微分方法的方法是基于图像信号的局部泰勒级数近似，使用关于空间和时间坐标的偏导数。因此，在一个实施方式中，处理包括：

·输入：最后一幅图像，当前图像，最后一幅图像中检测到的特征位置列表，

·输出：当前图像中最后一张图像的检测到的特征的位置列表

·假设：亮度一致，图像通过且仅通过运动变化

·动作1：通过假定没有运动来预测输出位置，以便当前位置与最后一帧的特征位置相同，或者使用从IMU检索的惯性信息来预测位置。

·动作2：通过在预测位置周围搜索，匹配色块并使用匹配分数确定精确位置来改进每个特征点的像素坐标。因此，所公开的技术可以通过使用来自IMU的惯性信息来提供具有通过缩小搜索范围和节省时间来获得提高的性能的实施方式。

IMU相机坐标变换：

在一些实施方式中，IMU 502和相机508、510不位于相同的物理位置处；在IMU 502和相机508、510之间存在距离。因此，为了使得定位流水线中的后续处理能够将IMU 502和相机508、510视为共处一处，在实现时确定IMU 502和相机508、510之间的变换矩阵，其可以从离线生产或后期生产校准阶段实现。在CU 100中，该变换矩阵被存储在本地，并被应用于IMU数据。该技术使后来的处理能够将IMU 502和相机508、510视为共处一处。

现在参考图6，其示出了可用于实现视觉惯性传感器500的视觉惯性定位系统600的简化框图。视觉惯性定位系统600包括处理器632、存储器634、惯性测量单元(IMU)502和一个或多个相机508和510，以及通信接口642。包括一个或多个附加I/O特征636以实施特定需求，例如视觉呈现接口678、音频呈现接口679、用于检测触觉输入680(例如，键盘、小键盘、触摸板、鼠标、轨迹球、操纵杆等)和非触觉输入682(例如，麦克风、声纳传感器以及类似的)的传感器。存储器634可以用于存储要由处理器632执行的指令以及与指令的执行相关联的输入和/或输出数据。具体而言，存储器634包含指令，其在概念上被展示为下面更详细描述的一组模块，其控制处理器632的操作及其与其他硬件部件的交互。操作系统指导执行底层的基本系统功能，例如内存分配、文件管理和海量存储设备的操作。操作系统可以是或包括多种操作系统，诸如Microsoft WINDOWS操作系统、Unix操作系统、Linux操作系统、Xenix操作系统、IBM AIX操作系统、Hewlett Packard UX操作系统、Novell NETWARE操作系统、Sun Microsystems SOLARIS操作系统、OS/2操作系统、BeOS操作系统、MACINTOSH操作系统、APACHE操作系统、OPENACTION操作系统、iOS、Android或其他移动操作系统或其他操作系统的平台。

计算环境还可以包括其他可移除/不可移除，易失性/非易失性计算机存储介质。例如，硬盘驱动器可以读取或写入不可移除的非易失性磁介质。磁盘驱动器可以读取或写入可移除的非易失性磁盘，并且光盘驱动器可以读取或写入诸如CDROM或其它光学介质的可移除非易失性光盘。可以在示例操作环境中使用的其他可移除/不可移除，易失性/非易失性计算机存储介质包括但不限于磁带盒、闪存卡、数字多功能盘、数字录像带、固态RAM、固态ROM等。存储介质通常通过可移除或不可移除的存储器接口连接到系统总线。

在一个实施例中，处理器632是NEON ARM处理器，其实现单输入多数据(SIMD)体系结构作为精简指令集计算机(RISC)体系结构。然而，取决于实施方式，处理器632可以替代地使用专用微控制器、外围集成电路元件、CSIC(客户专用集成电路)、ASIC(专用集成电路)、逻辑电路、数字信号处理器，诸如FPGA(现场可编程门阵列)、PLD(可编程逻辑设备)、PLA(可编程逻辑阵列)、RFID处理器、智能芯片之类的可编程逻辑设备或者任何其他设备或装置的布置能够实施所公开的技术的过程的动作。

通信接口642可以包括能够实现视觉惯性定位系统600和其他控制或启用用户硬件和应用(以下称为“主系统”或“主机”)的系统之间的通信的硬件和/或软件，例如，机器人或其他引导的移动平台、自主交通工具、虚拟现实增强现实可穿戴设备(VR/AR耳机)等(为了清楚起见，在图6中未示出)。相机508、510以及诸如IMU 502的传感器可以通过由硬件和软件组合实现的各种通信接口和协议耦合到处理器632。因此，例如，定位系统600可以包括一个或多个相机数据端口和/或运动检测器端口(为了清楚起见，在图6中未示出)，相机和运动检测器可以连接到该端口(通过传统插头和插孔)，以及在将信号作为输入提供给在处理器632上执行的快速准确的稳定自适应跟踪(“FASAT”)过程644之前修改从相机和运动检测器接收的数据信号(例如，减少噪声或重新格式化数据)的硬件和/或软件信号处理器。在一些实施方式中，视觉惯性定位系统600还可以向相机和传感器发送信号，例如激活或停用它们，以控制相机设置(帧速率、图像质量、灵敏度等)，控制传感器设置(校准、灵敏度等)等。这样的信号可以被传输，例如，响应于来自处理器632的控制信号，控制信号可以进而响应于用户输入或其他检测到的事件而产生。

定义FASAT过程644的指令被存储在存储器634中，并且这些指令在被执行时对由相机508、510捕捉的图像帧、由被连接到视觉惯性定位系统600的IMU 502捕捉的惯性数据进行分析。在一个实施方式中，FASAT过程644包括各种逻辑过程，诸如特征提取器652，其接收原始图像并确定图像中对象的显著点表示，从而表示对象从机器角度的几何理解。在一些实施方式中，特征提取器652分析图像(例如，经由相机508、510捕捉的图像帧)，以检测其中的对象的边缘和/或关于对象的位置的其他信息。传感器融合跟踪过程654使用特征提取结果和来自IMU 502的惯性数据来精确且快速地产生姿态。智能交互地图656使得能够使用已知的障碍物地图来定位传感器500。该地图使用地图构建过程692的地图构建功能来构建，其在下文中进一步详细描述。当设备失去对设备位置的跟踪时，重定位器过程658恢复设备位置感知。系统诊断和响应(SDAR)666管理设备的当前定位状态并提供响应策略。

地图构建过程692产生混合占用网格，其构建由特征提取器652识别的空间和对象的地图。混合占用网格包括(i)位于图像帧中的空间中点的点云表示，以及(ii)一个或多个xy平面占用网格，其布置在所提取的特征上的点相交的高度处。

在一些实施方式中，其他处理694分析音频或超声信号(例如，经包括非触觉输入682的由声纳或音频传感器捕捉的音频信号)以通过例如到达时间间隔、多点定位(multilateration)或类似的(多点定位是一种导航技术，它基于测量已知位置处两个或更多站点的距离差异，这些站点可以在已知时间广播信号。请参见Wikipedia，网址为<http://en.wikipedia.org/w/index.php？title＝Multilateration&oldid＝523281858>,on Nov.16,2012,06:07UTC)来定位对象和障碍物。音频信号将对象放置在已知的表面上，并且可以使用信号的强度和变化来检测对象的存在。如果音频和图像信息同时可用，则可以分析和调和这两种类型的信息，以产生更详细和/或准确的路径分析。

在一些实施方式中，其他处理694基于由地图构建过程692产生的混合占用网格来确定跟踪和预测空间中的设备移动的路径。一些实施方式694包括增强现实(AR)/虚拟现实(VR)环境，其提供反映真实对象的虚拟对象的集成(例如，图13中朋友1315的虚拟存在)，以及合成图14中的对象1415，以通过呈现接口678向图13中设备1325的用户展示，以提供个人虚拟体验。一个或多个应用696可以被加载到存储器634中(或者以其他方式使其可用于处理器632)以增强或定制设备500的功能，从而使得系统600能够用作平台。在像素级分析连续的相机图像以提取对象移动和速度。在一些实施方式中，呈现接口678包括视频反馈集成器，其提供来自相机508、510的实况视频馈送和一个或多个虚拟对象的集成。视频馈送集成器管理来自不同类型的相机508、510的视频信息的处理。例如，从提供单色成像的像素和从提供彩色成像的像素(例如，RGB)接收的信息可以由积分器分开并被不同地处理。来自视觉传感器的图像信息主要用于代理定位，其从图像中提取特征并提供多种功能：首先，在代理定位中提取图像中的特征(类似于灰度相机的使用)，其次，提供用于基于深度学习的任务的原始信息，包括对象识别、对象跟踪、图像字幕等。来自一种类型的传感器的信息可用于增强、纠正和/或确认来自另一种类型的传感器的信息。来自一种类型的传感器的信息可以在某些类型的情景或环境条件(例如，低光、雾、明亮的光等)中有利。设备可以自动地或通过接收来自用户的选择在基于一种或其他类型的图像信息提供呈现输出之间进行选择。成像集成器可以与AR/VR环境一起使用，控制通过呈现接口678呈现给用户的环境的创建。

呈现接口678、音频呈现679、非触觉输入682和通信接口642可以用于促进经由具有视觉惯性定位系统600的设备500的用户交互。这些部件可以是高度定制的设计，通常传统设计或其组合来提供任何类型的用户交互。在一些实施方式中，使用惯性测量单元502和相机508、510以及FASAT程序652分析捕捉的图像的结果可以被解释为表示3D空间中的对象和障碍物。例如，配备有视觉惯性传感器500的机器人可以在已经使用FASAT程序652分析的表面上执行路径规划和/或障碍物避免，并且该分析的结果可以被一些其他在处理器632(例如，运动计划器、定位和跟踪过程或其他应用程序)上执行的程序解释为占用地图。因此，作为说明，机器人可以使用相机508、510扫掠房间，以便将当前成像的空间通过呈现接口678“映射”到主机设备可以使用的混合点网格，该主机设备例如监视器、VR耳机或者类似的，从而提供机器人正在“看到”的区域的视觉输入。智能交互地图656可以使用通过地图构建692构建的空间的表示来规划机器人或移动平台通过该空间的路径，例如以改善机器人或平台穿过该空间的定位和跟踪。

应该理解，视觉惯性定位系统600是说明性的，并且变化和修改是可能的。视觉惯性定位系统可以以各种形式因素来实现，包括服务器和网络的“云”计算系统、台式系统、膝上型系统、平板电脑、智能电话或个人数字助理等等。为了清楚起见，特定的实施方式可以包括在此没有描述的其他功能。在一些实施方式中，一个或多个相机和两个或更多个麦克风可以被内置到视觉惯性系统600中，或者可以作为单独的部件被提供。此外，图像或音频分析器可以仅使用视觉惯性定位系统600部件的子集(例如，作为执行程序代码的处理器、ASIC或固定功能数字信号处理器、具有合适的I/O接口接收图像数据并输出分析结果)。

虽然本文参照特定框图描述了视觉惯性定位系统600，但应理解，框图是为了描述方便而定义的，并且不旨在暗示部件部分的特定物理布置。此外，框图不需要对应于物理上不同的部件。就使用物理上不同的部件而言，部件之间的连接(例如，用于数据通信)可以根据需要有线和/或无线。因此，例如，处理器632执行特征提取器652可以使得处理器632操作惯性测量单元502和相机508、510来捕捉穿过并接触表面的对象的图像和/或音频信号，以检测其进入分析图像和/或音频数据。

跟踪

跟踪是指由系统硬件和功能过程所呈现的能力，其使得受控设备(机器人、移动平台或VR/AR耳机、眼镜或其他硬件)能够连续定位自身并具有位置感知，例如传感器系统可以确定它在现实世界中的位置。

跟踪系统的架构

图7示出了一种跟踪系统实施方式的示例架构。如图7所示，部件和在图7的一个示例的快速准确稳定自适应跟踪(“FASAT”)过程644的部件之间的信息流由特征提取器(FE)702、基于传感器融合的跟踪器(SFT)711、与地图智能交互(SIM)的处理器714、系统诊断和响应(SDAR)处理器703和再定位器(RELOC)722来图示。下面将描述这些部件的一个示例实施方式的数据流和操作。

特征提取

特征提取器(FE)702表示根据机器的透视图对空间的几何理解。特征提取器702从控制单元100接收原始图像信息，并向传感器融合跟踪器(SFT)711提供显著点表示混合点云图。特征提取器702的一个实施方式执行各种动作，包括图像预处理、特征检测和特征描述符准备。

图像处理任务包括对原始图像数据执行高斯模糊和伽马校正。

特征检测：光流

光流给出先前图像和当前图像之间的2D-2D对应关系。特征提取器702产生层次结构；每个层级的产生与光流需求一致，因此只需计算一次。

处理的图像被存储在多层次补丁(patch)和/或未畸变的补丁中。在示例的实施方式中，随着层次结构中层级增加，图像中存在的模糊越多。在层次结构中的每个层级，图像的尺寸被缩小到其所来自的图像尺寸(例如前一个层级中图像)的尺寸的1/4。例如，如果第一个(“零级”)上的图像的尺寸为640×480，则下一个较高级别(“第一级”)上的对应图像的尺寸为320×240，并且在更高一级(“第二级”)上的下一个对应图像的尺寸为160×120。如果相同的特征点出现在多个层级上，则该特征点由系统确定为强特征点。在前述示例中，使用比例因子2，然而，可以选择任何比例因子，例如1.6的比例因子为原始640×480的图像提供了具有400×300分辨率的下一层级图像。

一个特征提取器702的实施方式采用光流方法来计算两个图像帧之间的运动，其在每个体素位置的时间t和t+Δt处获得。一种称为微分方法的方法是基于图像信号的局部泰勒级数近似，使用关于空间和时间坐标的偏导数。因此，在一个实施方式中，处理包括：

·输入：最后一幅图像、当前图像、最后一幅图像中检测到的特征位置列表，

·输出：当前图像中最后一张图像的检测到的特征的位置列表

·假设：亮度一致，图像通过且仅通过运动变化

·动作1：通过假定没有运动来预测输出位置，以便当前位置与最后一帧的特征位置相同，或者使用从IMU检索的惯性信息来预测位置。

·动作2：通过在预测位置周围搜索，匹配色块并使用匹配分数确定精确位置来改进每个特征点的像素坐标。因此，所公开的技术可以通过使用来自IMU的惯性信息来提供具有通过缩小搜索范围和节省时间来获得提高的性能的实施方式。

特征检测：Shi-Tomasi

特征提取处理器702的一个实施方式使用具有空间分级(spatial binning)的NEON加速的Shi-Tomasi特征点检测，以便从尽可能多地覆盖整个图像的区域获得特征。特征提取器702使用IMU和从传感器融合跟踪器711接收的姿态信息，以便动态地决定要跟踪的区域和要使用的参数。特征是图像的“有趣”部分。因此，在一个实施方式中，处理包括：

·动作1：计算每个像素的特征值并确定(i)该特征是否被关注；

和(ii)对于被关注的特征，一种特征：

o两个小的特征值：特征不被关注

o一个小的，一个大的值：特征可能是边缘

o两个大的值：特征可能是角落

o其他类型的特征

·动作2：改进

o应用非最大抑制

o应用空间分级(spatial binning)

o应用启发式(heuristics)

o应用其他类型的改进

特征描述符：ORB描述符

特征提取器702的一个实施方式使用SIMD加速的ORB描述符来描述特征。对特征的描述可用于匹配目的并描述特征的唯一性。ORB描述符方法被选择是由于其相对旋转不变性和对高斯图像噪声的免疫性。ORB特征检测器和二进制描述符的一个例子可以在“ORB特征检测器和二进制描述符”中找到，http://scikitimage.org/docs/dev/auto_examples/plot_orb.html(上次访问时间：2016年8月17日)。有关ORB描述符的进一步信息，可以参考Ethan Rublee等人的“ORB：SIFT或SURF的高效替代物”，其通过引用并入本文用于所有目的。

字词袋(Bag-of-words)

特征提取器702的一个实施方式执行字词袋提取，从一个图像确定字节数组、词汇表的稀疏直方图。因此，在一个实施方式中，字词袋处理包括使用一个图像中的特征描述符列表来提取一个图像的字词袋特征。例如，在包含具有400个对应描述符的400个特征的图像中，以及具有256位大小的字词袋特征。由此产生的字词袋是一个256位数组，而且一个图像只需要/有一个256位数组。字词袋在进行如下文更详细描述的再定位方面是有用的。字词袋可以指示A、B、C图像中，图像D具有单独的DA、DB、DC的匹配得分。举例来说，DB得分最高，则拍摄图像D的相机最接近拍摄图像B时使用的相机姿态，而不是A或C。

特征提取器702的一个实施方式自适应地调整应用于为了系统跟踪运动对象所需的多个特征的阈值。这种智能阈值包括基于设备移动、传感器读数、情境或环境变量(例如，低光、雾、明亮的光等)或其组合而适应性地调整的阈值。一旦标识出足够数量的特征(例如，高于阈值)，系统将继续跟踪对象。否则，系统不会跟踪该对象。特征提取器702将基于设备移动和其他传感器级别自适应地调整阈值。

SFT：从特征提取结果产生姿态信息以及来自辅助传感器的数据

基于传感器融合的跟踪器(SFT)711确定如何巧妙地使用来自IMU 502的特征提取结果和惯性数据来精确且快速地产生姿态。基于传感器融合的跟踪器711针对系统状态，特征提取器初始状态，与地图状态的交互以及其他状态执行初始化。基于传感器融合的跟踪器711具有决定如何初始化状态的逻辑，包括使用多少个第一样本IMU、使用跳过多少图像以及针对特定硬件的任何其他初始化步骤。

在一个实施方式中，基于传感器融合的跟踪器711对IMU执行缓冲区管理。IMU数据帧的时间戳被校正并对齐。另外，尚未处理的IMU信息被保存在缓冲区中。

传播(Propagation)：

实施方式可以采用通过方程(1)以一般非线性模型形式表示的扩展卡尔曼滤波(EKF)，以使用来自多轴IMU的惯性数据来外推初始姿态以产生传播姿态：

z＝h(x)+n (1)

其中x表示状态矢量，f表示非线性函数，h表示观测非线性函数，n表示测量噪声，z表示观测矢量，w表示噪声矢量。

方程(1)的离散时间模型的线性版本可用于协方差传播和更新，并且增益计算由方程(2)表示：

在一个实施方式中，仅使用IMU数据来执行EKF的传播。这使得某些实施方式能够以相对较低的计算成本提供快速结果。采用下面描述的更新过程来校正当在EKF中仅使用惯性信息来确定姿态时发生的“漂移”。传播可以通过多种方式进行：(i)传播在IMU进行惯性测量后立即发生；或者(ii)在视觉传感器拍摄的图像到达之后但在执行下面描述的更新过程之前立即发生延迟传播。当然，轮里程和/或用户定义控制也可用于执行传播。在一个实施方式中，“即插即用”决策引擎将决定是否进行多种类型的传播，以及何时和如何进行多种类型的传播。

更新：

在一个实施方式中，使用图像信息连同IMU缓冲区样本来执行更新过程。在单个观察实施方式中，基于当前图像观察，准备传播所需的信息，例如特征、姿态，地图点等。然后，通过直接使用来自光流跟踪结果的2D-2D对应来获得用于光流跟踪2D特征的2D-3D对应信息。对于其余的3D地图点，通过以下方式智能地在具有自适应参数的3D上进行搜索：(i)如果剩余点数很少(例如低于阈值)，则执行线性搜索，否则，(ii)如果剩余点数是相当大的，例如，高于阈值，使用kd树或八叉树执行log(n)搜索。接下来，执行更新，并仅使用“优秀”点为当前帧产生姿态。或者，也可以执行两次更新：第一次只使用高权重“优秀”点，然后使用所有地图点。如本文所使用的，术语“优秀”点表示高质量点，例如具有较小再投影误差和/或以良好状态被多次跟踪的点，其可通过应用一些启发法来选择

在多个观察实施方式中，基于多个先前的图像观察，来自多个姿态的信息被用在本文中参照方程(1)和(2)描述的扩展卡尔曼滤波器中。扩展卡尔曼滤波器使用每个功能的整个生命周期。

SIM：智能地图交互和定位

与地图智能交互(SIM)的处理器714使得能够通过利用已知地图被引导的设备(即，从视觉惯性传感器系统600接收位置感知信息)进行智能交互，以便自身定位。相应地，与地图智能交互(SIM)的处理器714的一个实施方式包括：

·根据需要和当前状态执行地图查询。特定实施方式中包含的某些查询组件是地图点(xyz，不确定性，平均重新投影误差等)，关键帧的姿态，2D3D约束信息和占用网格(一层或多层)。如这里所使用的，术语关键帧表示接收的图像数据的集合。在一些实施方式中，图像数据的集合包括特征点和姿态信息。姿态信息包括移动设备的位置和捕捉图像数据的相机的视图。这些图像数据的集合被统称为关键帧。

·如果整个地图不是很大，例如满足特定的阈值大小，则返回整个地图。

·否则，如果地图很大(例如，达到或超过特定阈值大小并覆盖大面积，例如空间区域达到或超过特定阈值)，则基于设备的当前位置巧妙地抓取地图的局部区域。

·如果地图的某些区域已知质量较差，则不使用它。

·基于系统性能根据自适应参数适应性地下采样地图。例如：(i)如果基于3D特征的跟踪过程是准确的，例如，低于阈值数量的错误或调用重定位处理以再定位丢失其位置的设备，则减小样本大小以节约资源，否则，(ii)如果设备频繁丢失，例如达到或超过阈值，则增加样本大小以提供更好的跟踪。

SDAR：管理本地化状态和响应策略

系统诊断和响应(SDAR)处理器703执行视觉惯性传感器500的当前定位状态的系统级管理并提供响应策略。在一个实施方式中，系统诊断和响应处理器703监视跟踪的地图点和相应的再投影误差，监视未跟踪的地图点的寿命，并且建议动作给地图构建模块，监视设备速度/加速度/抖动/漂移，(例如，它是否定期更新？)，执行新的添加地图对整体地图构建的贡献是否足够(即，因此它没有进行无用的悬停？)，监视关键帧拓扑结构(即伸展足够多还是太多？)，建议该设备何时何地应该添加关键帧，并监视特征提取流水线。

再定位丢失的设备

再定位器722恢复已从该状态失去其位置的跟踪的设备。一旦再定位器722将丢失的设备返回正轨，该设备可以恢复和与地图智能交互的处理器714的正常交互，和/或可以使用以下在此描述的技术来帮助构建地图。

再定位

图8A图示了一个实施方式中的再定位过程的示例。因此，再定位处理800A的一个实施方式包括：

·在动作810中，使用情景启发式来选择搜索策略。例如，如果有许多关键帧(例如，>50)并且需要快速恢复，然后选择在关键帧之间执行最佳匹配(关于字词袋特征)的搜索策略，并且使用最佳匹配关键帧的姿态作为最终结果。在另一个例子中，如果确定系统在丢失之前跟踪良好，则选择仅通过本地(例如，最近的x个关键帧)关键帧进行搜索，以快速恢复。在进一步的示例中，如果系统在进一步细化之前需要多个假设，则选择包括使用少数几个最佳匹配(关于字词袋特征)的搜索策略。其他类型的搜索策略可以使用指示搜索策略与一个或多个环境变量(例如，照明变化，由于雾，蒸汽或雾等导致的可见性变化等)或一个或多个状态变量(例如，以前在丢失之前很好地跟踪，刚刚执行过以前失败的再定位尝试等)之间的对应程度。

·在动作820中，使用所选择的搜索策略来搜索关键帧，以便在关键帧中找到具有与当前图像的字词袋描述最接近的字词袋描述的关键帧。

·在动作830，确定匹配质量是否足够。这可以通过将阈值应用于匹配指示的质量或通过其他手段来确定。例如，匹配质量指标的范围从0(表示完全差异)变化到1(表示完全匹配)。相应的匹配质量阈值可以是0.85，指示最小通过匹配质量。如果匹配质量足够，则继续执行动作840。否则，继续执行动作890以检查是否可以使用替代搜索策略，或者如果可用的适用搜索策略已经用尽，并且继续在替代搜索的动作810中处理策略或动作895返回指示再定位失败的错误条件。

·在动作840中，将当前图像的特征点与来自在动作820中选择的关键帧的地图构建点相关，产生<2D,3D>点配对。点配对表示2D当前图像与所选关键帧的3D地图之间的关系。

·在动作850中，确定点的数量是否足够。这可以通过将阈值应用于与选定关键帧的地图中的3D点具有对应关系的2D图像点的数量来确定。例如，在一个实施方式中，6个或更多点就足够了；而在另一个实施方式中则需要20-30个。如果点数量足够，则继续采取动作860。否则，继续采取动作890以检查是否可以采用替代搜索策略，或者如果可用的适用搜索策略已经用尽，并继续在动作810中采用替代方案搜索策略或在动作895中返回指示再定位失败的错误条件。

·在动作860中，根据最佳匹配的关键帧对姿态进行改进，并执行透视n点(Perspective n Point，PnP)或其他姿态评估方法，该方法根据2个二维图像特征的集合计算姿态。尽管可以使用各种PnP实施方式中的任何一种来改进姿态信息，但是OpenCV里的solvePnPRansac API在一些示例中的表现是可接受的。solvePnPRansac API的进一步描述可以参考“Camera Calibration and 3D Reconstruction”http://docs.opencv.org/master/d9/d0c/group__calib3d.html#gsc.tab＝0(上次访问2016年8月22日)。

·在动作870中，确定改进的姿态是否合理。在一个实施方式中，如果改进的姿态符合标准：(i)改进的姿态在位置和视角上与根据在行动820中的匹配选定的标识关键帧确定的姿态足够类似(例如，在容差内或在阈值以下)；(ii)在动作840中标识的<2D,3D>点对中的3D点的重新投影在2D图像平面上产生新的2D点，其中新2D点与<2D,3D>点配对的原始2D点具有足够类似的坐标(例如，在容差内或在阈值以下)，则改进的姿态被认为是合理的。如果定义的姿态是合理的，则继续执行动作880。否则，继续执行动作890以检查是否可以采用替代搜索策略，或者如果可用的适用搜索策略已经用尽，并使用可替代搜索策略在动作810中继续处理、或在动作895中返回指示再定位失败的错误条件。

·在动作880中，开始跟踪改进的姿态。另外，记录整个恢复信息，并且：

o使用改进的姿态信息将机器状态更新为当前姿态

o使设备重新回到轨道上并带有一些不确定性。

图8B示出了一个实施方式中的再定位搜索策略选择过程的示例。因此，过程800B的一个实施方式提供了使用情景启发式来选择搜索策略，其包括：

·在动作815中，确定是否有许多关键帧(例如，>50)以及是否需要快速恢复。如果是这种情况，则在动作825中，选择在关键帧之间执行最佳匹配(关于字词袋特征)的搜索策略，并且使用匹配最佳的关键帧的姿态作为最终结果。

·在动作835中，确定系统在丢失之前是否运行良好。如果是这种情况，则在动作845中，选择只在本地(例如，最近的X个关键帧)关键帧中搜索，以便快速恢复。

·在动作855中，确定系统在进一步改进之前是否需要多个假设。如果是这种情况，那么在步骤865中，选择包括使用少数几个最佳匹配(关于字词袋特征)的搜索策略。

·在动作875中，如果遇到不符合至少一个启发式规定的标准的情况，则选择默认搜索策略。其他类型的搜索策略可以使用指示搜索策略与一个或多个环境变量(例如，照明变化，由于烟、蒸汽或雾等导致的可见性变化等)或一个或多个状态变量(例如，此前在失踪之前很好地跟踪，刚刚执行过失败的再定位尝试等)。

通过基于3D特征的跟踪获得的几何信息被用于构建表示地图中用于引导移动设备的地图中的特征的混合网格。接下来参考图12、13和14描绘的示例描述执行该任务的过程。

地图构建

在实施方式中，由基于3D特征的跟踪过程所获得的几何信息使用描述性点云表示法来表示。描述性点云是稀疏的3D点的集合，其中每个点编码3D几何位置、几何位置的不确定性，以及2D位置加上基于外观的描述符的集合，每个描述符都由关键帧观察。关键帧包含(i)时间戳，其中捕捉关键帧中的图像；(ii)姿态(即，从视觉惯性传感器500坐标到当前时间的地图坐标的变换)及其不确定性；(iii)从时间戳之前的第一时间点开始到时间戳之后的第二时间点的IMU读数的序列。一些实施方式还包括标志，以指明视觉惯性传感器500是移动的还是静止的。该标志可以从IMU读数获得的信息中设置。

混合表示

实施方式表示使用混合点网格表示由基于3D特征的地图构建系统和可选地有源里程传感器(与地平面平行放置)感知的空间信息。混合点网格表示的一个示例实施方式组合了描述性点云和多层2D占用网格地图。

占用网格地图可以是基本垂直于重力方向定向的平面。每一层都有一个特定的高度。(按照惯例，地板上的层通常为0)。图9图示了一个实施方式中的占用网格地图的示例。图9的占用网格900指示视觉惯性传感器500的实施方式所映射的单层，例如底层902。占用网格900的白色部分表示空的空间，换句话说，由视觉惯性传感器500确定为未被占用的空间。例如，当视觉惯性传感器500能够透过房间门和窗户看到房间墙壁以外的空间未被占用时，这些区域在占用网格900中被相应地标记。实心黑色部分表示空间是被对象或障碍物占用。灰色部分表示视觉惯性传感器500尚未确定这些空间是否被占用或是空的。

将描述性点云坐标与基于重力的坐标相关联的3自由度变换。例如，如果地平面的法向矢量(即重力)是已知的，则首先旋转点云以使点云的z轴平行于重力，使用3x3旋转矩阵R＝R_pitch*R_roll。然后，(可选地)通过[0,0.T_z]将点转换，以使z＝0位于地平面上。请注意，正常的3x3旋转矩阵有3个自由度(R_yaw，*R_pitch*R_roll)，但在这种情况下只有2个。正常的3x1转换有3个自由度([t_x，t_y，t_z])，但这里只有1个。

为特征表示创建点云

实施方式通过参照标准添加关键帧，根据基于3D特征的跟踪结果递增地创建描述性点云，例如在以下示例中。在包含特征点和姿态信息的图像数据的集合中接收基于3D特征的跟踪结果。姿态信息包括移动设备的位置和捕捉图像数据的相机的视图。这些集合统称为关键帧。因此，实施方式通过使用一个或多个标准选择关键帧来构建描述性点云，例如以下示例：

·第一个图像帧被选为关键帧，并且该时间戳处的设备坐标系成为描述性点云的坐标。这建立了一个参照系。

·最新的关键帧的时间戳在时间阈值(通常为2秒)内小于当前时间戳。

·当前跟踪的姿态与最新的关键帧的位置之间的距离大于阈值(例如，x米)。

·当前跟踪姿态与最新的关键帧指向角之间的距离大于阈值(例如y度)

·当前图像包含一定比例的未包含在当前描述性点云中的特征。

·设备目前是静态的。这个信息可以从IMU的信号中的惯性信息获得。

如果正在考虑添加的关键帧包含当前描述性点云中未包括的特征，则在此时间戳处根据从设备捕捉的图像对新特征进行三角测量以获得设备坐标系中的点。通过将设备坐标系中的点转换为地图的坐标系，将新点添加到地图中。值得注意的是，一些实施方式包括以下中的一个或多个：(i)对来自当前/相同关键帧(例如，左和右相机之间)的跨图像新特征点进行三角化，(ii)对来自两个不同关键帧的跨图像新特征点进行三角化，两个不同的关键帧不一定是顺序的(例如，从关键帧1的左侧相机到从关键帧10的左侧相机)，以及(iii)对基于标准所选择的关键帧中的图像中的新的特征点进行三角化(例如，智能选择“最佳”关键帧)。转换是使用基于特征的跟踪或其他方法确定的。

改进点云

在一个实施方式中，来自基于3D特征跟踪的描述性点云被改进，通过获得“最佳”的(i)每个关键帧的姿态和(ii)所有点的3D位置，使得投影到其观察到的关键帧的所有点的重新投影误差的加权和最小化，以及(iii)可选地，来自具有静态标志的关键帧的加速度计读数必须为[0,0,1]，以迫使地图坐标的z轴更接近重力方向，同时满足以下约束(如果适用)：姿态沿平面移动(如果设备以平面运动安装在交通工具上)。

在一个实施方式中，通过以下处理利用有限的计算资源来执行从基于3D特征的跟踪来改进描述性点云：

·从关键帧的整个集合中随机抽取N个关键帧。

·通过最小化(i)投影到其观察关键帧的所有点的重新投影误差的加权和，精确地改进在这些关键帧中观察到的关键帧样本集合的姿态和3D点，并且(ii)可选地，来自具有静态标志的关键帧的加速度计读数必须为[0,0,1]以迫使地图坐标的z轴更接近重力方向，同时满足以下约束(如果适用)：即姿态沿平面移动(如果该设备以平面运动被安装在交通工具上)。

·重复采样并最小化，除了修复从之前迭代中已经优化过的3D点和姿态。

·迭代，直到关键帧和点的期望的百分比达到最优。

在一个实施方式中，通过以下处理利用有限的计算资源来执行从基于3D特征的跟踪来改进描述性点云：

·从关键帧整个集合中随机选择第一关键帧子集，并将第一关键帧子集中的关键帧分类为自由关键帧。

·选择第二关键帧子集，该第二关键帧子集与自由关键帧的第一子集中的关键帧共享视场。

·将不包括在自由关键帧的第一子集和第二子集中的任何关键帧分类为固定关键帧。

·将3D点分为3类：(i)自由3D点：在两个以上的关键帧的视点内的3D点，包括自由关键帧的第一子集和第二子集中的至少一个自由关键帧，(ii)半自由3D点：在自由关键帧的第一子集和第二子集中的一个自由关键帧的视点内的3D点，以及(iii)固定3D点：仅在固定关键帧的视点内的3D点。

·通过优化自由3D点来改进点云。

图10是改进3D点和点云中关键帧的姿态来引导包括具有距离计算的相机和多轴惯性测量单元(IMU)的移动设备的代表性方法1000。流程图1000可以至少部分地用计算机或其他数据处理系统来实现，例如通过配置成接收或检索信息、处理信息、存储结果和传输结果的一个或多个处理器。其他实施方式可以以不同的次序执行动作和/或以不同于图10中所示的那些动作的更少或更多的动作来执行动作。在一些实施方式中可以组合多个动作。为了方便起见，参考执行方法的系统来描述该流程图。该系统不一定是该方法的一部分。

所公开的技术的该部分和其他部分中描述的方法可以包括结合所公开的附加方法描述的以下特征和/或特征中的一个或多个。为了简洁起见，在本申请中公开的特征的组合未被单独枚举，并且不针对每个基本特征的集合重复。读者将会理解这种方法中标识出的特征如何能够很容易地与标识为诸如系统概述、跟踪、地图构建、机器人实现、VR/AR实现等实施方式的基本特征的集合相结合。

在动作1010中，接收包括3D特征点和姿态信息的图像数据集合。姿态信息包括移动设备的定位和捕捉图像数据的相机的视图。这些图像数据集合被统称为关键帧。视觉传感器设置在移动平台上，例如机器人、自主车辆或其他自主或被引导的移动平台。

在动作1020中，回顾关键帧的集合，以选择包含在3d点地图中的关键帧内容。

在动作1030中，从关键帧的整个集合中随机选择第一关键帧子集，并将第一关键帧子集中的关键帧分类为自由关键帧。

在动作1040中，选择第二关键帧子集，该第二关键帧子集与自由关键帧的第一子集中的关键帧共享视场，以及将第二关键帧子集中的关键帧分类为自由关键帧。

在动作1050中，将不包括在自由关键帧的第一子集和第二子集中的任何关键帧分类为固定关键帧。

在动作1060中，将3D点分为3类：(i)自由3D点：在两个以上的关键帧的视点内的3D点，包括自由关键帧的第一子集和第二子集中的至少一个自由关键帧，(ii)半自由3D点：在自由关键帧的第一子集和第二子集中的一个自由关键帧的视点内的3D点，以及(iii)固定3D点：仅在固定关键帧的视点内的3D点。

在动作1070中，优化自由3D点，以得到经改进的点云。

本部分中描述的方法的其他实施方式可以包括存储指令的非暂时性计算机可读存储介质，指令可由处理器执行以执行上述任何方法。本部分中描述的方法的又一个实施方式可以包括包括存储器和可操作以执行存储在存储器中的指令以执行上述任何方法的一个或多个处理器的系统。

在一个实施方式中，可以每N秒执行从基于3D特征的跟踪改进描述性点云。在一个实施方式中，N可以是1秒。

在一个实施方式中，当移动设备的当前位置与最后触发姿态之间的差异大于阈值时，即当设备已经移动大于X米或者取向方向已经改变大于Y度时，执行从基于3D特征的跟踪改进描述性点云。在一个实施方式中，X可以为0.5米。在一个实施方式中，X可以大于0.5米。在一个实施方式中，Y可以大于40度。

在一个实施方式中，当当前姿态与地图中的多个关键帧相比足够明显区别时，执行从基于3D特征的跟踪改进描述性点云。在一个实施方式中，足够明显区别包括移动单元行进超过0.5米。在一个实施方式中，足够明显区别包括移动单元旋转超过40度。

创建混合点网格

在一个实施方式中，使用以下处理从描述性点云创建初始混合点网格表示：

·将描述性点云从其原始坐标系转换为z轴沿重力方向的坐标系。这种转换具有2个自由度(旋转)。

·为覆盖描述性点云中xy点的层初始化2D占用网格图。

·使用射线跟踪过程，占用网格地图的每一层都由高度范围内的点(在描述性点云中)填满。

改进混合点网格

在一个实施方式中，用来自有源传感器的信息来改进混合点网格表示，采用以下过程：

·从地图坐标系中的设备的轨迹中获取在其中每个有源传感器的信号被捕捉的姿态(从设备坐标转换到地图坐标)。

·使用射线跟踪过程更新2D占用网格图。

在一个实施方式中，从更新的描述性点云的改进混合点网格表示，采用以下过程：

·将描述性点云坐标系转换为基于重力的坐标系。

·使用射线跟踪过程来确定要从描述性点云中的每个点更新哪些单元格。

估计被导向的设备的轨迹

在一个实施方式中，使用以下处理来估计描述性点云中的两个连续的关键帧(即，从关键帧k到关键帧k+1)之间的设备的离线轨迹：

·初始化创建关键帧k的图像帧，使其处于存储在描述性点云中的姿态。

·使用本文描述的标题为“跟踪”的部分中的“基于3D特征的跟踪过程”来跟踪两个关键帧之间的图像帧。每个图像帧具有6个自由度姿态，以及3D地图点到2D图像观察位置的对应关系的集合。

·迫使创建关键帧k+1的图像帧处于被存储在描述性点云中的姿态。

·通过最小化所有观察位置的重新投影误差总和来改进所有姿态。重新投影误差是相机坐标系中的重新投影点与原点之间的误差。图11示出重新投影1100的例子。在图11中，相机1102的参考系中的一些点用于对世界坐标系中的一个或多个新3D点P1104进行三角化。由于相机1102的校准误差，点P1104的空间位置将不是完全准确的。重新投影误差1106可以根据重新投影到相机1102的坐标中的生成的3D点P 1104(使用相机的校准数据)来确定，从而获得原始投影p1110附近的新点间1108。重新投影误差1106是原点p1110与重新投影点性1108之间的直线距离。

估计描述性点云和基于重力的帧的变换

在一个实施方式中，使用以下任何方法估计描述性点云到z轴沿重力方向的坐标之间的2自由度坐标变换：

·在IMU(加速度计)静止时由IMU(加速度计)测量重力方向，并找到将重力方向转换为[0,0,1]的旋转矩阵。

·从描述性点云在地平面上至少获得3个点，以估计原始坐标中地平面的法线。找到一个旋转矩阵，将地平面的法线转换为[0,0,1]或[0,0,1]。

·通过在地面放置已知的平面图案来确定旋转矩阵，离线校准视觉惯性传感器500与地面之间的关系。

机器人实施方式

图12示出了使用本文描述的图像和惯性信息技术的机器人引导的示例模型。受益于采用如本文所述的位置感知技术的机器人应用的示例包括：

·看护者和服务机器人(在地面行走)

·吸尘/拖地/清洁地板的机器人。

·被命令在环境中携带对象的机器人。

·远程呈现机器人自动在远程环境中移动。

·跟随人员的机器人管家。

在上面列出的每个场景中，机器人利用本文公开的技术来跟踪其自己的位置并识别它遇到的对象。此外，由于机器人执行许多复杂的任务，每个任务都有实时限制，因此快速完成感测以加速感知流水线是有益的。在实施方式中，这可以通过将一些计算从主处理器卸载到视觉惯性传感器500来实现。另外，由于它是一种移动机器人，它携带的电池容量有限，所以能耗是一个设计要点。在实施方式中，一些计算任务从主处理器卸载到视觉惯性传感器500以降低功耗，由此使得实施方式能够实现整体能量效率。成本是移动机器人的一个问题，因为降低机器人的成本使得机器人能够为更多的客户提供经济实惠。因此成本可能是传感器和引导系统设计的另一个因素。在实施方式中，一个低成本灰度传感器用于定位任务，一个彩色传感器用于识别任务。这一设计点使这些实现能够在不牺牲性能的情况下显著降低立体彩色传感器设计的成本。

在图12中，房间1200的墙壁、角落和门1223以及服务机器人1225在房间1200的地板上的行程反映在混合点网格中，其由描述性点云1245和占用网格1255组成，由视觉惯性传感器500使用上述地图构建部分中描述的技术来改良。占用网格1255是混合点网格的一部分，其是地图构建部分中描述的多层2D占用网格地图的层。为了构建未知(新暴露的)环境的地图，配备有视觉惯性传感器500的机器人1225使用上文在跟踪部分中描述的技术跟踪其姿态，同时使用上文的技术递增地构建初始描述性点云在地图构建部分。然后，机器人1225使用上文在地图构建部分中描述的技术来建立占用网格1255，以从初始描述性点云1245完成混合点网格。

获取实时图像和惯性信息

为了跟踪其位置，机器人通过视觉惯性传感器500感测其自身的移动。视觉惯性传感器500产生可靠惯性数据，以便跟踪可以准确地推断出机器人的位置的流水线并为其构建地图。图12示出了可以实现一种实施方式的示例机器人引导应用。如图12所示，机器人1225实施视觉惯性传感器500，以在房间1200内自我定位。图12中的机器人1225使用视觉传感器500的相机508、510以捕捉房间1200的周围环境的图像帧。根据以上在传感器可视数据产生和处理部分下所公开的技术处理图像，如下：

检测并描述捕捉图像中的特征

视觉惯性传感器500根据捕捉的图像帧中的信息确定房间1200的墙壁、角落和门1223的特征点1201、1211、1241、1251、1222等。在一些实施方式中，采用Shi-Tomasi特征点检测来从图像帧确定特征点1201、1211、1241、1251、1222。使用ORB特征描述为特征分配描述符。光流技术被用于确定图像中的2D对应关系，使得能够将不同图像中的特征匹配在一起。

将特征描述符映射(map)到混合点网格

配备有视觉惯性传感器500的机器人1225可以在房间1200中建立障碍物的描述性点云1245，使得机器人1225能够环绕障碍物并且在房间1200内自我定位。视觉惯性传感器500使用在上文中公开在地图构建部分下的技术使用针对由点1201、1211、1241、1251、1222指示的房间特征所确定的特征描述符来创建、更新并改进描述性点云1245。如图12中示意性所示，描述性点云1245包括对应于房间1200的特征点1201、1211、1241、1251、1222的坐标和特征描述符。视觉惯性传感器500通过重新投影特征点1201、1211、1241、1251、1295、1222到与房间1200的地板对应的2D层上，来准备占用地图1255。在一些实施方式中，在机器人1225的不同高度处产生第二个并且可能更大的占用地图，使得机器人1225能够在房间1200周围导航而不会将其头部撞到门下面或地板上方的其他障碍物。

现在重新参考图11，其示出了占用网格1100的示例，白色部分指示空白空间，换句话说，由视觉惯性传感器500确定为未被占用的空间。黑色实心部分表示对象或障碍物占用的空间。灰色部分表示机器人1225的视觉惯性传感器500尚未确定这些空间是否被占用或空的空间。

描述性点云1245和占用网格1255包括混合点网格，其使得机器人1225能够使用占用网格1255来规划穿过房间1200的路径，并且使用描述性点云1245相对于房间1200中的特征进行自定位。

使用占用网格和路径规划

当机器人在先前映射的环境中被激活时，机器人使用上文在跟踪部分中描述的技术来在描述性点云1245中自我定位。描述性点云1245和占用网格1255包括混合点网格表示，其对于使用无源传感器来实现机器人动作(即，在地板上移动)是关键的，因为机器人使用占用网格1255，以便使用上文在地图构建部分的技术将轨迹1256从其当前位置规划到地图中的另一个位置。人或实体也可以命令机器人去到占用网格1255中的特定点。在行进期间，机器人使用描述性点云1245以如上文在跟踪部分中所描述地在地图内定位自身。机器人可以使用上述地图构建部分中描述的技术来更新地图。此外，配备主动传感器(例如声纳，LIDAR)的一些实施方式也可以使用来自这些传感器的信息来更新地图。

VR/AR实现

在AR和VR使用情景下，耳机、眼镜或其他VR/AR呈现设备都采用视觉惯性传感器500来跟踪其自己的位置，并且在一些应用中识别它遇到的对象。为了跟踪其位置，VR/AR呈现设备需要感测其自身的移动，这是使用在本文中描述的视觉惯性传感器500技术来完成的。因此，视觉惯性传感器500产生可靠的惯性数据，使得随后的跟踪和地图构建流水线可以准确地推断出VR/AR呈现设备的位置。图13示出了可以实现一个实施方式的示例VR应用。

在图13中，用户穿戴VR/AR呈现设备——耳机1325。耳机1325配备有视觉惯性传感器500，该视觉惯性传感器500使用上文中在地图构建部分下面公开的技术，并且使用针对由房间1300的点1301、1311、1341、1351、1322以及在房间1300内的对象的点1316表示的房间特征而确定的特征描述符，来创建、更新和改进描述性点云1345。如图13中示意性所示，描述性点云1345包括对应于房间1300的特征点1301、1311、1341、1351、1322和在房间1300内的对象的点1316的坐标和特征描述符。视觉惯性传感器500通过重新投影特征点1301、1311、1341、1351、1322和1316到与在房间1300内发现特征点的地板上方(并包括地板)的各种高度对应的2D层上，来准备房间1300映射空间的混合点网格1355。因此，混合点网格1355通过堆叠的2D层来提供房间1300中的空间映射，指示对应于房间1300中的边界和对象的特征点。在一些实施方式中，自由空间1356可以由视觉惯性传感器500标识为可用于显示VR呈现1315，并且可以与耳机1325的VR呈现机制相通信，以使得耳机1325能够适当地使得VR呈现1315在空间1356中呈现给穿戴者。

耳机1325可以通信地耦合到内容源以将VR呈现1315呈现给穿戴者。尽管被描绘为耦合到网络网关的热点的无线连接，该网络网关进一步耦合到用于VR呈现1315的内容源，但是耳机1325可以使用有线或无线的各种机制中的任何一种来耦合到呈现内容。此外，用于呈现VR呈现1315的呈现内容可以被下载并存储在一些耳机1325的实施方式中。

上述操作原理易于应用于其他常见VR应用，例如游戏和社交使能体验。例如，在游戏中，用户在穿戴VR/AR呈现设备时进行游戏。VR/AR呈现设备将该游戏显示给用户(“玩家”)。配备VR/AR呈现设备的视觉惯性传感器500进行定向跟踪和玩家移动的变换，该移动可以被反映到由VR/AR呈现设备显示的游戏，以使得玩家能够在周围空间的特定区域内自由移动。配备有视觉惯性传感器500的VR/AR呈现设备为移动VR游戏提供了这样的一种“从内到外”，例如，集成在VR/AR呈现设备中，由此可以在跟踪时避免对一些种类的外部定位传感器的需要，例如，像传统的VR/AR耳机所需要的那样。此外，如果玩家经常在相同区域(例如，玩家的起居室)中使用VR/AR呈现设备，则视觉惯性传感器500可以巧妙地重新使用或更新现有的该空间的地图(例如，先前由视觉惯性传感器500创建的地图)以提供位置跟踪。

例如，在社交使能的体验中，名人/体育明星/歌手(创作者)在立体视频摄像机前面进行现场直播，该立体视频摄像机将直播视频流作为内容提供给粉丝。穿戴配备有视觉惯性传感器500(附加或嵌入)的VR/AR呈现设备的粉丝可以将此实况视频馈送作为VR呈现使用。当粉丝们兴奋/悲伤/快乐时，他们就像亲临现场的体验一样。视觉惯性传感器500执行位置跟踪，并向VR/AR呈现设备提供输入，从而能够基于位置跟踪相应地调整实况视频馈送。因此，粉丝和创作者可以借助所公开的技术“互动”。

图14示出了在其中可以实现一种实施方式的示例AR应用。在图14中，有两个用户穿戴VR/AR呈现设备——耳机1405、1425。耳机1405、1425配备有视觉惯性传感器500，该视觉惯性传感器500使用上文中在地图构建部分下面公开的技术，并且使用针对由房间1400的点1401、1411、1441、1451、1422表示的房间特征而确定的特征描述符，来创建、更新和改进描述性点云1445。如图14中示意性所示，描述性点云1445包括对应于房间1400的特征点1401、1411、1441、1451、1422的坐标和特征描述符。视觉惯性传感器500通过重新投影房间1400的特征点1401、1411、1441、1451、1422到与在房间1400内发现特征点的地板上方(并包括地板)的各种高度对应的2D层上，来准备房间1400映射空间的混合点网格1455。因此，混合点网格1455通过堆叠的2D层来提供房间1400中的空间映射，指示对应于房间1400中的边界和对象的特征点。在一些实施方式中，对应于穿戴者的对象1456可以由其他穿戴者的视觉惯性传感器500标识，并且在混合点网格1455中反映。此外，对应于穿戴者的对象1456之间的自由空间可用于显示AR呈现1415，在这种情况下，对于一个穿戴者在商业会议中讨论的项目计划的虚拟化渲染，以及与耳机1405、1425的VR/AR呈现机制的通信，使得耳机1405、1425能够适当地使得AR呈现1415在对应于穿戴者的对象1456之间的空间中出现。

耳机1405、1425可以通信地耦合到内容源，用于向(多个)穿戴者呈现AR呈现1415。虽然被描绘为耦合到网络网关的热点的无线连接，其进一步耦合到AR呈现1415的内容源，但是耳机1405、1425可以使用有线或无线的各种机制中的任何一种耦合到呈现内容。此外，用于呈现AR呈现1415的呈现内容可以被下载并存储在一些耳机1405、1425的实施方式中。

上述操作原理容易应用于其他常见的AR应用，例如游戏，社交使能体验和工业应用。就像VR游戏一样，在AR游戏中，玩家在穿着VR/AR呈现设备时进行游戏。玩家可以在配备有VR/AR呈现设备的视觉惯性传感器500跟踪玩家位置的同时在室内自由移动。由于视觉惯性传感器500提供的真实世界环境的精确映射，VR呈现设备能够在真实世界环境中定位虚拟对象。因此，AR玩家可以参与他们自己的客厅中的虚拟战斗，带有18世纪厨房用品的虚拟覆盖物的厨房等等。使用由虚拟惯性传感器500提供的位置和定位信息，可以将每个虚拟物品准确且真实地定位到玩家的真实世界中。

在例如社交使能的体验中，用于VR的视频聊天应用程序可以使得用户能够与他们最好的朋友汤姆进行视频聊天，并且虚拟的汤姆会出现在用户家中的某个位置(通过使用AR眼镜、VR耳机、机器人投射或者其他的呈现设备进行显示)。用户可以在虚拟汤姆周围走动，改变呈现——使他变得更大或更小，甚至可以完全改变渲染效果，使他穿戴拿破仑常戴的帽子和套装。

存在配备虚拟惯性传感器500的VR/AR呈现设备的许多其他应用。虚拟购物旅行、远程房地产放映、在室内骑自行车活动期间捕获并重放的自行车、虚拟飞行指导、远足的室内/室外位置跟踪、自行车骑行、摩托车旅行等等。

在上面列出的每个场景中，VR/AR呈现设备利用本文公开的技术来跟踪其自己的位置并识别它遇到的对象。此外，由于VR/AR呈现设备执行许多复杂的任务，每个任务都有实时限制，因此快速完成感测以加速感知流水线是有益的。在实施方式中，这可以通过将一些计算从主处理器卸载到视觉惯性传感器500来实现。另外，由于它是一种移动VR/AR呈现设备，它携带的电池容量有限，所以能耗是一个设计要点。在实施方式中，一些计算任务从主处理器卸载到视觉惯性传感器500以降低功耗，由此使得实施方式能够实现整体能量效率。成本是移动VR/AR呈现设备的一个问题，因为降低VR/AR呈现设备的成本使得VR/AR呈现设备能够为更多的客户提供经济实惠。因此成本可能是传感器和引导系统设计的另一个因素。在实施方式中，一个低成本灰度传感器用于定位任务，一个彩色传感器用于识别任务。这一设计点使这些实现能够在不牺牲性能的情况下显著降低立体彩色传感器设计的成本。

过程

图15是使用来自具有距离计算的相机和多轴惯性测量单元(IMU)的信息引导移动设备的代表性方法1500。流程图1500可以至少部分地用计算机或其他数据处理系统来实现，例如通过被配置成接收或检索信息、处理信息、存储结果和传输结果的一个或多个处理器。其他实施方式可以以不同的次序执行动作和/或以不同于图15中所示的那些动作的更少或更多的动作来执行动作。在一些实施方式中可以组合多个动作。为了方便起见，参考执行方法的系统来描述该流程图。该系统不一定是该方法的一部分。

所公开的技术的该部分和其他部分中描述的方法可以包括结合所公开的附加方法描述的以下特征和/或特征中的一个或多个。为了简洁起见，在本申请中公开的特征的组合未被单独枚举，并且不针对每个基本特征集合重复。读者将会理解这种方法中标识出的特征如何能够很容易地与标识为诸如系统概述、跟踪、地图构建、机器人实现、VR/AR实现等实施方式的基本特征集合相结合。

图15包括从动作1510开始的过程1500，其中来自视觉传感器的图像集合被缓冲。在一些实施方式中，从包括至少一个能够感测RGB的相机和至少一个其他灰度相机的视觉传感器接收图像集合。RGB相机和灰度相机可以相对于彼此以一定距离设置，以形成其中视场至少部分重叠的区域，由此提供立体成像能力。视觉传感器被设置在移动平台上，例如机器人、自主车辆或其他自主或被引导的移动平台。

过程1500在动作1520继续，其中至少能够提供加速度测量的来自多轴惯性测量单元(IMU)的惯性测量被缓冲。惯性测量单元可以与视觉传感器配置在一起，或者可以与其偏置设置。

在动作1530中，在视觉惯性控制单元处接收图像数据集合。

在动作1540中，在视觉惯性控制单元处接收来自多轴IMU的传感器数据集合。

在动作1550中，所接收的惯性数据被施加时间戳。可以通过惯性测量引擎施加该时间戳。

在动作1560中，对被施加时间戳的惯性数据中的惯性读数进行偏差校正。

在动作1570中，使用存储的比例因子对校正的惯性读数进行缩放。

在动作1580中，对被缩放的惯性数据针对IMU中的错位进行校正，以形成定位数据。

在动作1590中，对这些图像数据集合执行成像去畸变。

在动作1595中，跨越通信接口向控制移动平台的主机提供定位数据和未失真的图像数据的集合。

本部分中描述的方法的其他实施方式可以包括存储可由处理器执行以执行上述任何方法的指令的非暂时性计算机可读存储介质。本部分中描述的方法的又一个实施方式可以包括具有存储器和可操作以执行存储在存储器中的指令以执行上述任何方法的一个或多个处理器的系统。

图16是更新包括具有距离计算的相机和多轴惯性测量单元(IMU)的移动设备的位置的代表性方法1600。流程图1600可以至少部分地用计算机或其他数据处理系统来实现，例如通过被配置成接收或检索信息、处理信息、存储结果和传输结果的一个或多个处理器。其他实施方式可以以不同的次序执行动作和/或以不同于图16中所示的那些动作的更少或更多的动作来执行动作。在一些实施方式中可以组合多个动作。为了方便起见，参考执行方法的系统来描述该流程图。该系统不一定是该方法的一部分。

所公开的技术的该部分和其他部分中描述的方法可以包括结合所公开的附加方法描述的以下特征和/或特征中的一个或多个。为了简洁起见，在本申请中公开的特征的组合未被单独枚举，并且不针对每个基本特征的集合重复。读者将会理解这种方法中标识出的特征如何能够很容易地与标识为诸如系统概述、跟踪、地图构建、机器人实现、VR/AR实现等实施方式的基本特征的集合相结合。

图16包括从动作1610开始的过程1600，其中移动单元的位置和相机的视角(包括视图方向)被统称为初始姿态。在一些实施方式中，初始姿态表示包括坐标，例如，映射到设置在移动平台(例如机器人、自主驾驶车辆或其他自主或被引导移动平台)上的视觉惯性传感器的坐标系的点(x，y，z)。

过程1600在动作1620处继续，其中在连续相机帧之间等待新帧的同时，使用来自多轴IMU的惯性数据来更新初始姿态，以产生传播姿态。

在动作1630中，在基于惯性数据的传播姿态与新姿态的实际视角之间的漂移使用相机捕捉的新帧来校正。此动作还可以包括许多动作：

在动作1640中，使用传播姿态，估计连续相机帧之间的重叠，以减少计算需求，并且从估计的重叠开始，通过连续相机帧的2D比较，将新帧与先前帧相关。

在动作1650中，使用传播的姿态从3D地图中检索传播姿态的视场内的至少一些特征点。

在动作1660中，从新帧提取新特征。

在动作1670中，基于(1)来自先前帧的匹配特征的重用以及(2)从传播姿态的视角，新帧中的特征与从3D地图向2D视图上的重新投影特征位置的匹配，将提取的新特征与检索到的特征点相匹配，以产生匹配特征的列表。

在动作1680中，使用匹配特征列表中的匹配特征的定位来计算视觉校正姿态，以确定相机观察新帧的视角。

在动作1690中，响应于对移动单元的定位和/或相机的视角的请求，提供基于传播姿态、惯性数据和视觉矫正姿态中的一者或两者的数据。

本部分中描述的方法的其他实施方式可以包括存储可由处理器执行以执行上述任何方法的指令的非暂时性计算机可读存储介质。本部分中描述的方法的又一个实施方式可以包括具有存储器和可操作以执行存储在存储器中的指令以执行上述任何方法的一个或多个处理器的系统。

图13是使用混合点网格来引导包括具有距离计算的相机和多轴惯性测量单元(IMU)的移动设备的代表性方法1300。流程图1300可以至少部分地用计算机或其他数据处理系统来实现，例如通过配置成接收或检索信息、处理信息、存储结果和传输结果的一个或多个处理器。其他实施方式可以以不同的次序执行动作和/或以不同于图13中所示的那些动作的更少或更多的动作来执行动作。在一些实施方式中可以组合多个动作。为了方便起见，参考执行方法的系统来描述该流程图。该系统不一定是该方法的一部分。

所公开的技术的该部分和其他部分中描述的方法可以包括结合所公开的附加方法描述的以下特征和/或特征中的一个或多个。为了简洁起见，在本申请中公开的特征的组合未被单独枚举，并且不针对每个基本特征的集合重复。读者将会理解这种方法中标识出的特征如何能够很容易地与标识为诸如系统概述、跟踪、地图构建、机器人实现、VR/AR实现等实施方式的基本特征的集合相结合。

图17包括从动作1710开始的过程1700，其中图像数据的集合被接收。在一些实施方式中，图像数据的集合包括特征点和姿态信息。姿态信息包括移动设备的位置和捕捉图像数据的相机的视图。这些图像数据的集合被统称为关键帧。

过程1700在动作1720处继续，其中基于关键帧内容与经受一个或多个智能阈值的其他所选关键帧的内容的比较，回顾关键帧以选择关键帧内容以包括在特征点云中。如本文所使用的，智能阈值包括基于设备移动、传感器读数、情景或环境变量(例如，低光，雾，亮光等)或其组合而适应性地调整的阈值。

在动作1730中，对于选定的关键帧，(a)使用先前被添加到特征点云中的关键帧的特征点来对关键帧中的新特征点进行三角化，以获得设备的坐标系中的特征点，以及(b)特征点云中的特征点坐标与具有与重力对齐的z轴的坐标系对齐。

在动作1740中，使用多层2D占用网格的至少一个层根据为特征点云选择的特征点创建多层混合点网格，包括以下步骤：

在动作1750中，与从覆盖特征点云中的特征点的多个x-y层中选择的一个相对应的2D占用网格被初始化。

在动作1760中，利用从对齐的关键帧中的点的观察位置到占用网格中的对应点的射线跟踪，利用来自高度范围内的特征点云的点填充占用网格的至少一个层网格和重新投影在占用网格层上的对应点的位置。

在动作1770中，找到沿着对齐的观察点和在层上重新投影的对应点之间的射线的单元格，并将找到的单元格标记为空。

在动作1780中，响应于接收到行进到位置的命令，使用占用网格来规划到命令的位置的行进路径，并且在行进计划路径时同时使用描述性点云，以避免与障碍物碰撞。

本部分中描述的方法的其他实施方式可以包括存储指令的非暂时性计算机可读存储介质，指令可由处理器执行以执行上述任何方法。本部分中描述的方法的又一个实现可以包括具有存储器和可操作以执行存储在存储器中的指令以执行上述任何方法的一个或多个处理器的系统。

结论和其他实施方式

我们描述了用于引导包括具有距离计算的相机和多轴惯性测量单元(IMU)的移动设备的系统和各种实施方式。

在下面的讨论中描述了一些特定的实施方式和特征。

在一个实施方式中，描述了一种系统，该系统包括至少一个相机和多轴惯性测量单元(IMU)，以及到主机的接口，主机包括被耦合到存储计算机指令的存储器的一个或多个处理器，计算机指令校正定义地图的至少一些3D点的至少一些被估计的姿态和位置，地图被用于向包括具有距离计算的相机和多轴惯性测量单元(IMU)移动平台提供引导。计算机指令当在处理器上被执行时实施动作，该动作由以下组成：接收包括2D特征点和姿态信息的图像数据的集合，姿态信息包括移动平台的位置和捕捉图像数据的集合的相机的视图，其中由图像数据和姿态信息的组成的集合被统称为关键帧(keyrig)；基于关键帧内容与其他所选择的关键帧的内容的比较，通过在关键帧中分配自由度来回顾关键帧，以选择包括在从3D点定义的地图中的关键帧内容，该回顾通过以下方式实现：通过从关键帧中选择可以被改进的自由关键帧的集合而在关键帧中分配自由度，该自由关键帧的集合包括随机选择的第一关键帧子集和第二关键帧子集，第二关键帧子集(i)位于第一关键帧子集中的至少一个第一关键帧子集的半径内、并且(ii)与第一关键帧子集中的至少一个第一关键帧子集共享视场；其中未被选择的关键帧包括不可用来被改进的固定关键帧的集合；以及在地图中以及在至少一个关键帧的视场内确定选择3D点，组标识包括从以下集合中选择的一个集合：(i)可以改进的自由点；其中在两个以上的关键帧的视点内、包括至少一个自由的关键帧的3D点是自由点；(ii)不可改进但可在改进自由点的过程中改变位置的半自由点；其中在至多一个自由的关键帧的视点内的3D点是半自由点；和(iii)不可改变的固定点；其中在固定的关键帧的视点内的3D点是固定点；以及改进地图以校正该自由点的至少一些自由点的至少一些被估计的姿态和位置，并且使用校正的被估计的姿态和位置来向移动平台提供引导。

在一个实施方式中，系统进一步实施包括通过以下方式来改进从对应于2D特征的3D点定义的地图的动作，即：选择由3D点定义的地图的图像数据和姿态信息中的至少一些图像数据和姿态信息的集合；使用姿态信息将3D点从地图重新投影到2D图像平面上的重新投影点；计算2D图像平面上的重新投影点的图像坐标，并且将针对2D图像平面上的重新投影点计算出的图像坐标与直接从所选择的图像数据中获得的对应观察点的实际坐标进行比较，以计算多个重新投影误差；并且针对所选择的图像数据和姿态信息中的至少一些图像数据和姿态信息的集合，最小化重新投影误差的加权和。

其他实施方式包括包括执行系统的动作的方法，存储实现这些方法的程序逻辑的非暂时性机器可读存储介质、替代物和部件，以及被并入前述内容的任何组合或组合的设备。

在一个实施方式中，可以每N秒执行一次从基于3D特征的跟踪来改进地图。在一个实施方式中，N可以是1秒。

在一个实施方式中，当移动设备的当前位置与最后触发姿态之间的差异大于阈值时，即当设备已经移动大于X米或者取向方向已经改变大于Y度时，执行从基于3D特征的跟踪改进描述性点云。在一个实施方式中，X可以为0.5米。在一个实施方式中，X可以大于0.5米。在一个实施方式中，Y可以大于40度。

在一个实施方式中，在当前姿态与地图中的多个关键帧相比足够明显区别时，执行从基于3D特征的跟踪改进描述性点云。在一个实施方式中，足够明显区别包括移动单元行进超过0.5米。在一个实施方式中，足够明显区别包括移动单元旋转超过40度。

这里使用的术语和表达被用作描述的术语和表达而不是限制，并且在使用这样的术语和表达时，不打算排除所示出和描述的特征的任何等同物或其部分。另外，已经描述了所公开的技术的某些实施方式，对于本领域的普通技术人员来说显而易见的是，可以使用并入在此公开的概念的其它实施方式而不脱离所公开的技术的精神和范围。因此，所描述的实施方式在所有方面都被认为仅是说明性的而非限制性的。

Claims (23)

1.一种系统，包括：

移动平台，具有被设置在其上的：

至少一个相机；

多轴惯性测量单元(IMU)；以及

到主机的接口，所述主机包括被耦合到存储计算机指令的存储器的一个或多个处理器，所述计算机指令校正定义地图的至少一些3D点的至少一些被估计的姿态和位置，所述地图被用于向包括具有距离计算的所述相机和所述多轴惯性测量单元(IMU)的所述移动平台提供引导，所述计算机指令当在所述处理器上被执行时实施动作，所述动作包括：

接收包括2D特征点和姿态信息的图像数据的集合，所述姿态信息包括所述移动平台的位置和捕捉图像数据的所述集合的所述相机的视图，其中由图像数据和姿态信息组成的集合被统称为关键帧；

基于关键帧内容与其他所选择的关键帧的内容的比较来回顾关键帧，以选择包括在从3D点定义的地图中的关键帧内容，所述回顾通过如下方式实现：

通过从关键帧中选择能够被改进的自由关键帧的集合而在关键帧中分配自由度，所述自由关键帧的集合包括随机选择的第一关键帧子集并包括第二关键帧子集，所述第二关键帧子集(i)位于所述第一关键帧子集中的至少一个第一关键帧子集的半径内、并且(ii)与所述第一关键帧子集中的至少一个第一关键帧子集共享视场；其中未被选择的关键帧包括不能够被改进的固定关键帧的集合；以及

在所述地图中以及在至少一个关键帧的视场内确定选择3D点，组标识包括从下列集合中选择的一个集合：(i)能够被改进的自由点；其中在两个以上的关键帧的视点内的、包括至少一个自由的关键帧的3D点是自由点；(ii)不能够被改进、但是能够在改进自由点的过程中改变位置的半自由点；其中在至多一个自由的关键帧的视点内的3D点是半自由点；以及(iii)不能够被改变的固定点；其中在固定的关键帧的视点内的3D点是固定点；以及

改进所述地图以校正所述自由点的至少一些自由点的至少一些被估计的姿态和位置，并且使用所校正的被估计的姿态和位置来向所述移动平台提供引导。

2.根据权利要求1所述的系统，还实施包括通过以下方式改进从对应于2D特征的3D点定义的所述地图的动作，即：

选择由3D点定义的所述地图的图像数据和姿态信息中的至少一些图像数据和姿态信息的集合；

使用所述姿态信息将3D点从所述地图重新投影到2D图像平面上的重新投影点；

计算所述2D图像平面上的所述重新投影点的图像坐标，并且将针对所述2D图像平面上的所述重新投影点计算出的所述图像坐标与直接从所选择的图像数据中获得的对应观察点的实际坐标进行比较，以计算多个重新投影误差；并且

针对所选择的图像数据和姿态信息中的至少一些图像数据和姿态信息的所述集合，最小化所述重新投影误差的加权和。

3.根据权利要求1所述的系统，其中固定关键帧的所述集合需要维持用于所述地图的全局坐标系统的至少一个固定关键帧。

4.根据权利要求1所述的系统，还包括：每当新的改进过程被应用来调整所述地图的至少一些部分时，在关键帧中将自由度分配到“固定”和“自由”子集中。

5.根据权利要求1所述的系统，还包括：以N秒的间隔触发改进所述地图。

6.根据权利要求5所述的系统，其中N包括1秒。

7.根据权利要求1所述的系统，还包括：在当前位置或取向和最后触发的姿态大于阈值时，触发改进所述地图。

8.根据权利要求7所述的系统，其中位置或取向和最后触发的姿态大于阈值包括：设备已经移动超过X米或已经改变取向方向超过Y度。

9.根据权利要求8所述的系统，其中X为0.5米。

10.根据权利要求1所述的系统，还包括：在当前姿态与所述地图中的多个关键帧相比足够明显区别时，触发改进所述地图。

11.根据权利要求10所述的系统，其中足够明显区别包括大于0.5米。

12.根据权利要求10所述的系统，其中足够明显区别包括大于40度。

13.一种存储指令的非暂时性计算机可读存储介质，所述指令用于校正定义地图的至少一些3D点的至少一些被估计的姿态和位置，所述地图被用于向包括具有距离计算的相机和多轴惯性测量单元(IMU)的移动平台提供引导，所述指令当在一个或多个处理器上被执行时实施动作，所述动作包括：

接收包括2D特征点和姿态信息的图像数据的集合，所述姿态信息包括所述移动平台的位置和捕捉图像数据的所述集合的所述相机的视图，其中由图像数据和姿态信息组成的集合被统称为关键帧；

基于关键帧内容与其他所选择的关键帧的内容的比较，回顾关键帧以选择包括在从3D点定义的地图中的关键帧内容，所述回顾通过如下方式实现：

通过从关键帧中选择能够被改进的自由关键帧的集合而在关键帧中分配自由度，自由关键帧的所述集合包括随机选择的第一关键帧子集并包括第二关键帧子集，所述第二关键帧子集(i)位于所述第一关键帧子集中的至少一个第一关键帧子集的半径内、并且(ii)与所述第一关键帧子集中的至少一个第一关键帧子集共享视场；其中未被选择的关键帧包括不能够被改进的固定关键帧的集合；以及

在所述地图中以及在至少一个关键帧的视场内确定选择3D点，组标识包括从下列集合中选择的一个集合：(i)能够被改进的自由点；其中在两个以上的关键帧的视点内的、包括至少一个自由的关键帧的3D点是自由点；(ii)不能够被改进、但是能够在改进自由点的过程中改变位置的半自由点；其中在至多一个自由的关键帧的视点内的3D点是半自由点；以及(iii)不能够被改变的固定点；其中在固定的关键帧的视点内的3D点是固定点；以及

改进所述地图以校正所述自由点的至少一些自由点的至少一些被估计的姿态和位置，并且使用所校正的被估计的姿态和位置来向所述移动平台提供引导。

14.根据权利要求13所述的非暂时性计算机可读存储介质，还实施包括通过以下方式改进从对应于2D特征的3D点定义的所述地图的动作：

选择由3D点定义的所述地图的图像数据和姿态信息中的至少一些图像数据和姿态信息的集合；

使用所述姿态信息将3D点从所述地图重新投影到2D图像平面上的重新投影点；

计算所述2D图像平面上的所述重新投影点的图像坐标，并且将针对所述2D图像平面上的所述重新投影点计算出的所述图像坐标与直接从所选择的图像数据中获得的对应观察点的实际坐标进行比较，以计算多个重新投影误差；并且

针对所选择的图像数据和姿态信息中的至少一些图像数据和姿态信息的所述集合，最小化所述重新投影误差的加权和。

15.根据权利要求13所述的非暂时性计算机可读存储介质，其中固定关键帧的所述集合需要维持用于所述地图的全局坐标系统的至少一个固定关键帧。

16.根据权利要求13所述的非暂时性计算机可读存储介质，还实施包括每当新的改进过程被应用来调整所述地图的至少一些部分时在关键帧中将自由度分配到“固定”和“自由”子集中的动作。

17.根据权利要求13所述的非暂时性计算机可读存储介质，还实施包括以N秒的间隔触发改进所述地图的动作。

18.根据权利要求17所述的非暂时性计算机可读存储介质，其中N包括1秒。

19.根据权利要求13所述的非暂时性计算机可读存储介质，还实施包括在当前位置或取向和最后触发的姿态大于阈值时触发改进所述地图的动作。

20.根据权利要求19所述的非暂时性计算机可读存储介质，其中位置或取向和最后触发的姿态大于阈值包括：设备已经移动超过X米或已经改变取向方向超过Y度。

21.根据权利要求13所述的非暂时性计算机可读存储介质，还实施以下动作：当与所述地图中的多个关键帧相比，当前姿态为大于0.5米和大于40度中的至少一个时，触发改进所述地图。

22.一种用于校正定义地图的至少一些3D点的至少一些被估计的姿态和位置的方法，所述地图被用于向包括具有距离计算的相机和多轴惯性测量单元(IMU)的移动平台提供引导，所述方法包括：

接收包括2D特征点和姿态信息的图像数据的集合，所述姿态信息包括所述移动平台的位置和捕捉图像数据的所述集合的所述相机的视图，其中由图像数据和姿态信息组成的集合被统称为关键帧；

基于关键帧内容与其他所选择的关键帧的内容的比较来回顾关键帧，以选择包括在从3D点定义的地图中的关键帧内容，所述回顾通过如下方式实现：

通过从关键帧中选择能够被改进的自由关键帧的集合而在关键帧中分配自由度，所述自由关键帧的集合包括随机选择的第一关键帧子集并包括第二关键帧子集，所述第二关键帧子集(i)位于所述第一关键帧子集中的至少一个第一关键帧子集的半径内、并且(ii)与所述第一关键帧子集中的至少一个第一关键帧子集共享视场；其中未被选择的关键帧包括不能够被改进的固定关键帧的集合；以及

在所述地图中以及在至少一个关键帧的视场内确定选择3D点，组标识包括从下列集合中选择的一个集合：(i)能够被改进的自由点；其中在两个以上的关键帧的视点内的、包括至少一个自由的关键帧的3D点是自由点；(ii)不能够被改进但是能够在改进自由点的过程中改变位置的半自由点；其中在至多一个自由的关键帧的视点内的3D点是半自由点；以及(iii)不能够被改变的固定点；其中在固定的关键帧的视点内的3D点是固定点；以及

改进所述地图以校正所述自由点的至少一些自由点的至少一些被估计的姿态和位置，并且使用所校正的被估计的姿态和位置来向所述移动平台提供引导。

23.根据权利要求22所述的方法，还包括通过以下方式改进从对应于2D特征的3D点定义的所述地图，即：

选择由3D点定义的所述地图的图像数据和姿态信息中的至少一些图像数据和姿态信息的集合；

使用所述姿态信息将3D点从所述地图重新投影到2D图像平面上的重新投影点；

计算所述2D图像平面上的所述重新投影点的图像坐标，并且将针对所述2D图像平面上的所述重新投影点计算出的所述图像坐标与直接从所选择的图像数据中获得的对应观察点的实际坐标进行比较，以计算多个重新投影误差；并且

针对所选择的图像数据和姿态信息中的至少一些图像数据和姿态信息的所述集合，最小化所述重新投影误差的加权和。

Images