行人运动约束的视觉惯性融合定位与建图方法和装置

1.本技术涉及行人导航技术领域，特别是涉及一种行人运动约束的视觉惯性融合定位与建图方法和装置。


背景技术：

2.尽管卫星导航技术广泛应用于我们生产生活的各个领域，但是卫星导航易受遮挡而无法应用于室内、密林、隧道、地下等复杂场景，更不能全面满足行人导航需求。并且，行人运动模式更为灵活自由，动态模式更为复杂多变，导航装备精度要求高，体积成本要求低，因此与车辆、无人机等载体的导航模式有着很大的不同。行人导航也就成为了导航领域公认的难题之一，也是当前研究的热点问题之一。
3.为了解决行人自主导航定位问题，hemin zhang等人提出了一种基于航位推算的定位解算方法，其主要利用行人步态模型结合加速度计输出估计步长，结合陀螺与磁强计来进行航位推算。skog等人足绑式mimu上，提出了shoe零速检测器来抑制低精度mimu惯导的捷联惯性导航解算中的误差发散问题，从一定程度上提高了定位精度与稳定性。但是这两者方法由于主要依赖惯性信息来对行人进行定位，难以实现长航时的高精度定位。andrei等人则提出了unavchip结合零速检测与协同导航方法实现行人的导航定位。随着slam相关技术的发展，基于视觉的方法为解决行人导航问题提供了重要的思路。视觉传感器信息源包含了大量的关于运动场景的集合信息，视觉里程计或者惯性里程计通过利用这些场景中的几何信息，大大提高了行人导航定位的精度。如vins、orb-slam3等视觉slam方案利用视觉、惯性、深度等信息对相机进行位姿解算，将其用于行人导航可提高定位的可靠性与精度。陈泽等人则提出了利用足部惯性导航模块来修正视觉惯性里程计的行人全源导航算法，但是这种方法以视觉惯性模块为核心，易受到环境干扰而失效，鲁棒性相对不高。
4.视觉三维重建算法对于行人导航有着重要意义。行人导航过程中进行稠密三维重建有助于提高行人与环境的交互感知能力，是增强现实，语义分割，路径规划，导航避障等下游技术的基础。在视觉slam领域，rgb-d方法可以提供稠密的深度图和运动轨迹，因此也是三维重建领域的重要方法。bylow等人和kerl等人主要利用rgb-d相机在高精度轨迹估计反面展开了深入的研究。相较于rgb-d建图，单目方法不论是在建图还是在跟踪方面都具有许多挑战。newcombe等人提出了dtam算法使用一个单目相机，在gpu上实时对基于光流的代价体对相机位姿和稠密地图进行联合估计。最近有许多基于深度学习的方法相较于经典方法有了更好的表现。这些方法利用深度神经网络来构建出三维模型，这些3d模型涵盖了体积表示，3d点云表示以及深度图集表示等多种表示形式。huang等人提出了deepmvs算法根据输入图像预测的二维深度特征图直接计算单个体积。在接下来的工作中，yao等人提出了mvsnet，利用一个循环网络来取代卷积神经网络进行深度预测。


技术实现要素：

5.基于此，有必要针对上述技术问题，提供一种行人运动约束的视觉惯性融合定位
与建图方法和装置。
6.一种行人运动约束的视觉惯性融合定位与建图方法，应用于行人导航中，其中足绑式imu设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，所述方法包括：对所述足绑式imu进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和imu因子节点，所述偏差因子节点连接相邻的偏差变量，所述imu因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；根据所述足绑式imu的速度预测值以及零速之间的误差，构建零速修正因子；根据所述视觉惯性里程计观测的位置信息与所述足绑式imu观测的位置信息，构建视觉惯性里程计因子；将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；根据所述视觉传感器采集的图像数据以及通过足绑式imu的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
7.在其中一个实施例中，还包括：构建坐标系方向为右-前-上的载体坐标系以及东-北-天的导航坐标系，通过对所述足绑式imu进行初始化；利用所述足绑式imu静止时的三轴加速度测量得到横滚角和俯仰角；根据所述横滚角和所述俯仰角，将足绑式imu的测量数据从载体坐标系转化至导航坐标系，并计算得到在导航坐标系中的初始航向角。
8.在其中一个实施例中，还包括：通过足绑式imu进行零速检测之后，进行足绑式imu和所述视觉惯性里程计时间同步的步骤包括：将足绑式imu和所述视觉惯性里程计的传感器信息通过话题通讯机制上传至数据处理中心，并通过数据处理中心对所述传感器数据打上时间戳；进行足绑式imu和所述视觉惯性里程计空间上同步的步骤包括：确定足绑式imu和所述视觉惯性里程计在水平方向上初始航向角偏差值，根据所述初始航向角偏差值，对足绑式imu和所述视觉惯性里程计进行空间对齐。
9.在其中一个实施例中，还包括：对所述视觉惯性里程计进行因子图建模，得到定位因子图；所述定位因子图中的因子包括：视觉因子和惯性因子；所述视觉因子是根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差确定的；所述惯性因子是通过预积分方式计算得到的；对所述定位因子图进行计算，得到视觉惯性里程计的定位数据。
10.在其中一个实施例中，还包括：根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差，得到视觉因子为：其中，表示运动对象从第i个图像运动到第j个图像时，像素点在导航坐标系
中位置，表示运动对象从第i个图像运动到第j个图像时，像素点在世界坐标系中位置；其中，，表示投影方程；表示测量误差。
11.在其中一个实施例中，还包括：对所述足绑式imu状态随时间变化的过程建模为：其中，表示导航状态变量，表示imu的误差模型，与分别表示导航状态变量与误差的非线性模型；定义与表示时刻的状态变量与误差变量，表示imu测量值，离散化得到：其中，表示imu因子节点与表示偏差因子节点。
12.在其中一个实施例中，还包括：根据所述足绑式imu的速度预测值以及零速之间的误差，构建零速修正因子为：正因子为：表示载体坐标系下足绑式imu的零速，表示载体坐标系下足绑式imu的速度预测值，为系统的速度观测函数。
13.在其中一个实施例中，还包括：根据所述视觉惯性里程计观测的位置信息与所述足绑式imu观测的位置信息，构建视觉惯性里程计因子为：其中，视觉惯性里程计提供的位置信息，为系统的位置观测函数，用于计算所述足绑式imu观测的位置信息。
14.在其中一个实施例中，还包括：根据足绑式imu的零速检测结果，剔除处于同一步态范围内的关键帧，得到不同步态中的关键帧；将所述关键帧以及所述关键帧对应的全局位姿作为输入，输入预先构建的cva-mvsnet模型进行分层深度估计，聚合所有关键帧的深度特征，得到深度估计结果；其中，聚
合所有关键帧的深度特征包括：单阶段深度估计过程和分层深度估计过程；所述单阶段深度估计过程为：采用自适应视图分级的方式构建成代价体为：。
15.其中，是大小为的视图聚合权重，是广播的元素级乘法；将所述代价体输入3d u-net模型进行正则化并最终经过softmax非线性层输出估计深度值为：其中，表示每个像素的深度假设。
16.一种行人运动约束的视觉惯性融合定位与建图装置，应用于行人导航中，其中足绑式imu设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，所述装置包括：因子图构建模块，用于对所述足绑式imu进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和imu因子节点，所述偏差因子节点连接相邻的偏差变量，所述imu因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；约束构建模块，用于根据所述足绑式imu的速度预测值以及零速之间的误差，构建零速修正因子；根据所述视觉惯性里程计观测的位置信息与所述足绑式imu观测的位置信息，构建视觉惯性里程计因子；图优化模块，用于将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；定位与建图模块，用于根据所述视觉传感器采集的图像数据以及通过足绑式imu的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
17.上述行人运动约束的视觉惯性融合定位与建图方法和装置，首先，基于足绑式imu进行因子图建模，然后基于行人上设置的视觉惯性里程计，构建了视觉惯性里程计因子，虽然两个传感器的设置在不同位置，但是在行动过程中，二者属于柔性连接，因此，通过找到足绑式imu与视觉惯性里程计的运动关系，构建了视觉惯性里程计因子，另外还构建了零速修正因子，共同作为约束条件，对因子图进行优化，并且建图。从而，本发明可以不依赖于视觉数据，即使在复杂的环境，依然可以进行准确的行人导航与建图。
附图说明
18.图1为一个实施例中行人运动约束的视觉惯性融合定位与建图方法的流程示意图；
图2为一个实施例中行人运动约束的视觉惯性融合定位与建图方法的框架示意图；图3为一个实施例中优化因子图的框架图；图4为一个实施例中行人运动约束的视觉惯性融合定位与建图装置的结构框图。
具体实施方式
19.为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
20.在一个实施例中，如图1所示，提供了一种行人运动约束的视觉惯性融合定位与建图方法，包括以下步骤：步骤102，对所述足绑式imu进行因子图建模，得到因子图。
21.因子图中的因子节点包括：偏差因子节点和imu因子节点，所述偏差因子节点连接相邻的偏差变量，imu因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量。
22.步骤104，根据足绑式imu的速度预测值以及零速之间的误差，构建零速修正因子。
23.步骤106，根据视觉惯性里程计观测的位置信息与足绑式imu观测的位置信息，构建视觉惯性里程计因子。
24.步骤108，将零速修正因子和视觉惯性里程计因子作为状态变量的约束条件，对因子图进行优化，得到优化因子图。
25.步骤110，根据视觉传感器采集的图像数据以及通过足绑式imu的零速检测结果，筛选关键帧图像，根据关键帧图像进行稠密重建，得到建图结果。
26.步骤112，对优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
27.上述行人运动约束的视觉惯性融合定位与建图方法，框架图如图2所示，首先，基于足绑式imu进行因子图建模，然后基于行人上设置的视觉惯性里程计，构建了视觉惯性里程计因子，虽然两个传感器的设置在不同位置，但是在行动过程中，二者属于柔性连接，因此，通过找到足绑式imu与视觉惯性里程计的运动关系，构建了视觉惯性里程计因子，另外还构建了零速修正因子，共同作为约束条件，对因子图进行优化，并且建图。从而，本发明可以不依赖于视觉数据，即使在复杂的环境，依然可以进行准确的行人导航与建图。
28.在其中一个实施例中，构建坐标系方向为右-前-上的载体坐标系以及东-北-天的导航坐标系，通过对足绑式imu进行初始化；利用足绑式imu静止时的三轴加速度测量得到横滚角和俯仰角；根据横滚角和所述俯仰角，将足绑式imu的测量数据从载体坐标系转化至导航坐标系，并计算得到在导航坐标系中的初始航向角。
29.具体的，在系统初始化部分，主要利用磁力计进行初始航向估计，以便于后续实现空间对齐。首采集磁传感器在各个方向上的数据，即不断旋转磁传感器以对各个方向的产生充分激励来对磁传感器进行校准。之后将包含了磁传感器的足绑式imu安装于足部，之后静止20s采集静止数据。系统载体坐标系方向为右-前-上坐标系，导航坐标系为东-北-天坐标系。利用静止时的三轴加速度数据用来测量横滚角，俯仰角
(pitch)：其中，表示重力的大小。
30.为了计算初始航向角，我们利用校准后的磁传感器数据，将其投影至导航坐标系得到：则初始航向角为：其中一个实施例中，足绑式imu进行零速检测步骤包括：在行人正常步态周期中，采用广义似然比检验的方式来对零速状态进行检测。首先将k时刻的足绑式imu输出的比力表示为，其中包括k时刻的加速度计输出的角速率与陀螺的输出，即对于零速检测来说，主要的目的是检测出在时长为w的滑动时间窗口
内，imu是否处于零速状态。记。因此，可以将零速检测看作二元假设检验问题，原假设与备择假设分别定义如下：：imu移动：imu静止将虚警概率表示为 (即imu在移动时被检测为静止的概率)，零速检测正确概率表示为。根据静止时陀螺与加速度计的输出，可以得到以下关系：其中表示时长为w的滑动时间窗口，对于静止时刻的，满足。即imu静止时，为方向不确定的单位向量。
31.对于足绑式imu，我们可以建模为其中，这里的，分别表示imu比力与角速率的真实值。，分别表示加速度计与陀螺的噪声，二者独立同分布，可建模为零均值高斯噪声，协方差矩阵根据上述对静止时足绑式imu的陀螺与加表输出我们可以得出。即在静止时刻下imu的模型参数为方向未知的单位向量。
32.可以把加速度计与陀螺的概率密度函数()写作：
当时，即imu处于运动状态时，上式关于的最大似然估计为，带入上式可得当时，即imu处于静止状态时，上式关于的最大似然估计为，带入上式可得：则
其中，因此，将带回原式可得
根据广义似然比检验条件我们可以得到根据广义似然比检验条件我们可以得到简化可得：其中。即当滑动时间窗口内加速度计与陀螺的输出满足上式时，将其判定为零速状态，否则不是零速状态。
33.在另一个实施例中，通过足绑式imu进行零速检测之后，进行足绑式imu和视觉惯性里程计时间同步的步骤包括：将足绑式imu和视觉惯性里程计的传感器信息通过话题通讯机制上传至数据处理中心，并通过数据处理中心对传感器数据打上时间戳；进行足绑式imu和视觉惯性里程计空间上同步的步骤包括：确定足绑式imu和视觉惯性里程计在水平方向上初始航向角偏差值，根据初始航向角偏差值，对足绑式imu和所述视觉惯性里程计进行空间对齐。
34.具体的，在将足绑式imu数据进行零速状态检测后，由于足绑式imu与视觉惯性里程计模块是不同的传感器源，借助ros操作系统的话题通讯机制将不同的传感器信息实时上传至数据处理单元并将其附上时间戳，进而实现不同传感器源的时间同步。
35.在实际用用过程中，视觉惯性里程计位于胸前，而足绑式imu位于脚步，二者之间初始状态在水平方向上往往会相差一个航向角。为了实现两个传感器的空间对准，需要将视觉定位模块的初始航向减去二者之间的航向角偏差，即其中表示足绑式imu的初始航向，表示视觉惯性里程计模块的初始航向。从而实现了两个不同传感器的空间对准。
36.在其中一个实施例中，对视觉惯性里程计进行因子图建模，得到定位因子图；定位因子图中的因子包括：视觉因子和惯性因子；视觉因子是根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差确定的；惯性因子是通过预积分方式计算得到的；对定位因子图进行计算，得到视觉惯性里程计的定位数据。
37.具体的，视觉惯性里程计定位主要采用现有的视觉里程计定位模块。在本示例中，采用基于vins-mono方法的视觉惯性里程计。在该模块中，利用相机作为视觉前端，与固联的imu模块紧耦合形成视觉惯性里程计。并利用因子图优化的方法来处理这个问题。
38.视觉惯性里程计的滑动窗口内待估计状态为:式中，是 mimu 状态变量，由位置、姿态、速度、加速度计零偏和陀螺零偏组成，优化窗口内共有个；是图像中特征点的逆深度，优化窗口中共有个。
39.1、视觉因子外部某个特征点在世界坐标系w系的坐标为，特征点直接通过相机测量在第张和第张图像中的像素坐标分别为和。视觉系统经过运动，第张图的特征点像素坐标通过转换到第张图的像素坐标为，与之间的误差称之为重投影误差。视觉因子代价函数为：为：与的关系为：式中，表示投影方程；表示测量误差。特征点在第张图和第张图的视觉因子代价函数 可以扩展为：2、视觉惯性里程计中的惯性因子视觉系统中，mimu 的采样频率一般都在100hz以上，远高于相机的采样频率。为了避免优化中对 mimu 状态的重复计算，构建视觉惯性因子采用经典的预积分方法。mimu 在b系中测量的加速度和角速度为：
式中，和为实际运动的加速度和角速度；为系中的重力向量；、、和为高斯噪声。
40.从时间点到时间点区间中，mimu 的预积分为：式中，代表时的b系；、和分别代表位置、速度和姿态预积分。
41.视觉惯性里程计中惯性因子代价函数为：
式中，为系到w系四元数；为四元数乘法。
42.因此，视觉惯性里程计的优化目标函数可以写为：之后我们可以用ceressolver来优化这个因子图。
43.在其中一个实施例中，对足绑式imu状态随时间变化的过程建模为：其中，表示导航状态变量，表示imu的误差模型，与分别表示导航状态变量与误差的非线性模型；定义与表示时刻的状态变量与误差变量，表示imu测量值，离散化得到：其中，表示imu因子节点与表示偏差因子节点。
44.imu因子所表示的代价函数可以表示为：具体的，根据足绑式imu特点，构建以足绑式imu为核心的全源导航因子图框架。表示因子节点的集合，表示变量节点的集合，表示
因子节点与变量节点之间的边。与因子相邻的变量节点集合记作，并且我们将对这个变量集合赋值写作。则该因子图可以表示为作用于全体变量函数的分解：即每一个因子节点都可以看做是作用于相邻变量节点的函数，因子图即为因子节点的乘积。
45.在行人导航中，需要对足绑式imu的状态变量进行估计。其中由位置、速度与姿态组成。因子图优化的过程可以看做是给定观测量利用非线性优化方法求解关于变量的最大后验估计的过程。即在因子图中，对于给定的观测量，归一化项与最大后验估计无关，因此，不去显示的表示观测量。同时为了强调最大后验估计是关于变量的函数，可以用似然估计函数来表示相应的因子节点。即以下对各因子进行描述，1、零速修正因子根据所述足绑式imu的速度预测值以及零速之间的误差，构建零速修正因子为：子为：表示载体坐标系下足绑式imu的零速，表示载体坐标系下足绑式imu的速度预测值，为系统的速度观测函数。
46.2、视觉惯性里程计因子根据视觉惯性里程计观测的位置信息与足绑式imu观测的位置信息，构建视觉惯性里程计因子为：其中，视觉惯性里程计提供的位置信息，为系统的位置观测函数，用于计算所述足绑式imu观测的位置信息。
47.综上，优化因子图如图3所示，最大后验估计可以写作：估计可以写作：在其中一个实施例中，根据足绑式imu的零速检测结果，剔除处于同一步态范围内的关键帧，得到不同步态中的关键帧；将关键帧以及所述关键帧对应的全局位姿作为输入，输入预先构建的cva-mvsnet模型进行分层深度估计，聚合所有关键帧的深度特征，得到深度估计结果；其中，聚合所有关键帧的深度特征包括：单阶段深度估计过程和分层深度估计过程；单阶段深度估计过程为：采用自适应视图分级的方式构建成代价体为：。
48.其中，是大小为的视图聚合权重，是广播的元素级乘法；将代价体输入3d u-net模型进行正则化并最终经过softmax非线性层输出估计深度值为：其中，表示每个像素的深度假设。
49.具体的，由于相机图像频率相对较高，相邻两帧图像数据之间的重叠度往往较高，选取关键帧进行三维场景重建。首先根据行人运动约束，剔除掉处于同一个零速步态范围
内的关键帧。即通过零速检测我们对足绑式imu数据进行步态分割后，将剔除掉同一步内处于零速静止状态下的重复关键帧，增大不同关键帧之间的视差，提高建图效率。
50.在建图过程中，利用基于深度学习的方法对关键帧进行深度估计。这里神经网络的输入为关键帧的图像及其相应的全局位姿。其中，表示第一个关键帧大小为的图像，表示第帧图像的全局位姿。利用改进的cva-mvsnet来进行深度估计，采用级联式的代价量来进行分层深度估计，之后用自适应视图聚合模块有效地聚合所有关键帧的深度特征，从而克服了深度mvs网络对内存的限制性要求，有效提高深度估计的性能。
51.关键帧的多尺度深度特征首先通过共享权重的2d u-net网络来进行特征提取，其中是特征信息的空间纬度，是尺度信息。因此是大小为的矩阵，其中是尺度因子为的特征维度，，。参考帧的深度图则分为三个阶段进行分层估计，每个阶段将特征集作为输入并预测图像的深度值，其中的大小为。下面对单阶段估计和分层多阶段估计进行分别说明。
52.1、单阶段深度估计算法在每个阶段都会利用深度特征构建匹配代价函数。对于参考帧的每个像素点，我们假设深度值为，得到一个大小为的。基于深度假设、相对位姿和相机内参，将每一帧图像的深度特征通过可微单应变换，扭曲至参考图像相机前的这些平行面上。每一帧图像的深度特征可以被表示为大小为的特征卷。为了将多视图特征体的信息聚合到一个代价体中，大多数以前的深度mvs方法都将不同的视图视为等同的，并使用基于差异的指标：其中，但是在滑窗slam系统中，关键帧在优化窗口中的分布并不均匀，甚至有的滑窗中
并没有关键帧，通常新的关键帧之间的距离要小于旧的关键帧。这会引起阻塞和无重叠的图像帧。因此，基于变量的代价指标是不合适的，因为它对不同的视图具有同等的权重。为了解决这个问题，我们采用自适应视图分级的方式来构建成代价体：其中，是大小为的视图聚合权重，是广播的元素级乘法。以作为输入，通过浅层的卷积神经网络可以估计出相对于每个的视图权重。该聚合模块可以令网络自适应地降低错误信息的权重。
53.代价体通过3d u-net进行正则化并最终经过非线性层来获取大小为的概率体，给定每个像素的深度假设，估计深度值为：2、分层深度估计网络利用先前阶段的深度估计来定义一个精确的深度假设张量。由于第一阶段之前没有阶段，深度图的每个像素具有相同的深度范围。之后的阶段深度经过上采样并作为先验来定义。特别的，对于位于处的像素，被定义为利用上采样的作为中心。然后，用预先定义的起点来围绕这个中心继续进行采样。这样，更高的阶段则需要更少的深度信息。之后，再使用适用于所有三个阶段的真实深度的l1损失函数来训练网络，并使用总和作为最终损失函数。从而获得关键帧图像的深度估计。
54.应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
55.在一个实施例中，如图4所示，提供了一种行人运动约束的视觉惯性融合定位与建图装置，应用于行人导航中，其中足绑式imu设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，包括：因子图
构建模块402、约束构建模块404、图优化模块406和定位与建图模块408，其中：因子图构建模块，用于对所述足绑式imu进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和imu因子节点，所述偏差因子节点连接相邻的偏差变量，所述imu因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；约束构建模块，用于根据所述足绑式imu的速度预测值以及零速之间的误差，构建零速修正因子；根据所述视觉惯性里程计观测的位置信息与所述足绑式imu观测的位置信息，构建视觉惯性里程计因子；图优化模块，用于将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；定位与建图模块，用于根据所述视觉传感器采集的图像数据以及通过足绑式imu的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
56.关于行人运动约束的视觉惯性融合定位与建图装置的具体限定可以参见上文中对于行人运动约束的视觉惯性融合定位与建图方法的限定，在此不再赘述。上述行人运动约束的视觉惯性融合定位与建图装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
57.本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（rom）、可编程rom（prom）、电可编程rom（eprom）、电可擦除可编程rom（eeprom）或闪存。易失性存储器可包括随机存取存储器（ram）或者外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram（sram）、动态ram（dram）、同步dram（sdram）、双数据率sdram（ddrsdram）、增强型sdram（esdram）、同步链路（synchlink） dram（sldram）、存储器总线（rambus）直接ram（rdram）、直接存储器总线动态ram（drdram）、以及存储器总线动态ram（rdram）等。
58.以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
59.以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要求为准。