CN112907644A - 一种面向机器地图的视觉定位方法 - Google Patents

Abstract

本发明涉及一种面向机器地图的视觉定位方法，属于机器定位技术领域。本发明充分考虑到机器地图的测图过程受到环境和自身位置的先验信息与用图任务反馈的双重约束，通过自身位姿和环境的先验分布以及视觉定位的状态向量建立先验约束和图任务反馈约束的视觉定位模型，利用该定位模型来实现机器地图的定位。本发明能够在自身的定位体系不发生改变的情况下，可应对不同的先验信息和反馈，可适应不同的应用场景。

Description

一种面向机器地图的视觉定位方法

技术领域

本发明涉及一种面向机器地图的视觉定位方法，属于机器定位技术领域。

背景技术

地图作为人类文化和文明的认知工具，所表达的是人类存在的物理空间，主要服务于人的环境感知与理解。随着人工智能、5G、物联网等技术的发展，智能机器与人共融共生态势已悄然显现，特别是在新冠肺炎的抗击、巡查与物资运送等单调重复、危险、环境未知的任务中已体现出一定的优势。与人类的认知过程类似，无人智能平台进行活动的前提是对所在环境的理解，不仅包含对于复杂、易变、高度动态的环境的准确感知过程，也牵涉到环境的表述与建模、空间推理与计算等诸多过程。如何针对无人智能平台的认知过程、“定制”一套适合其认知特点的专用地图，已经成为人工智能、大数据时代测绘科学与技术需要关注的新问题，同时也是人工智能领域的研究热点和难点问题。

围绕着为无人智能平台提供地理环境数据这一科学问题，近年来测绘领域已有专家学者进行了大量的研究工作并取得了突破，例如以面向自动驾驶应用场景的高精度地图(High Definition Maps，HD Maps)为代表的地图产品已经可获得厘米级的城市道路信息，对机器地图的地图模型研究有重要借鉴和启发。类似的，计算机视觉领域的同时定位与建图(Simultaneous Localization and Mapping,SLAM)技术的发展也为智能平台的地理环境感知、环境建模、控制决策准确性和稳健性的提升带来理论基础。然而，上述的研究中“测图”、“制图”和“用图”3个过程在时间、空间维度上仍然较为独立，尽管SLAM技术可以将“测图”、“制图”和“用图”进行算法层面的集成，但是缺乏三者之间的关系和相互作用的统一描述和系统研究，三者的相互支撑关系与制约关系并没有得到较好的解决。当三者分离时，无人智能平台需要根据任务预先制定测图、制图和用图任务，当应对复杂的、高动态的地理环境时缺乏有效的技术手段。因此，如何基于无人智能平台认知过程，构建具备“测图”、“制图”和“用图”一体功能(下文称“测制用一体”)的机器地图，是测绘科学与技术、人工智能、计算科学与技术等众学科发展至新时期所面临的共同挑战。

发明内容

本发明的目的是提供一种面向机器地图的视觉定位方法，以提高机器地图定位的应用场景的灵活性。

本发明为解决上述技术问题而提供一种面向机器地图的视觉定位方法，该定位方法包括以下步骤：

1)获取用于视觉定位的传感器所在环境的先验信息，构建传感器自身位姿和环境的先验分布；

2)获取用于视觉定位的传感器类型、观测数据类型和几何建模目标并以此得到视觉定位的状态向量；

3)根据先验分布和状态向量建立先验约束和图任务反馈约束的视觉定位模型，利用该视觉定位模型对传感器进行定位。

本发明充分考虑到机器地图的测图过程受到环境和自身位置的先验信息与用图任务反馈的双重约束，通过自身位姿和环境的先验分布以及视觉定位的状态向量建立先验约束和图任务反馈约束的视觉定位模型，利用该定位模型来实现机器地图的定位。本发明能够在自身的定位体系不发生改变的情况下，可应对不同的先验信息和反馈，可适应不同的应用场景。

进一步地，所述步骤3)中的视觉定位模型为：

其中z为给定的影像；u为其它观测数据；x为视觉定位的传感器自身位姿；y为环境信息；c为给定随机变量，由状态向量映射得到；P(x,y,c)为传感器自身位姿、环境和状态向量共同构成的联合概率；P(z,u│x,y,c)为影像、观测数据、其它给定随机变量已知时，传感器位姿与环境信息的条件概率；P(z,u,c)为传感器位姿、观测数据、给定随机变量的联合概率；P(x,y,z,u,c)为传感器位姿、环境信息、影像、观测数据和给定随机变量的联合概率，即所有随机变量的全概率。

进一步地，所述的状态向量为：

其中s_i为用视觉定位的传感器类型，s_i∈N；d_j为观测数据类型，包括维度和数据密度；m_k为几何建模目标为向量。

进一步地，为了在适应环境先验信息无法直接获取的场景，当环境先验信息无法直接获取时，将包含地理坐标的已有的地理影像作为约束，通过当前传感器获取影像与已有的地理影像进行检索与匹配，将检索到的地理影像集作为环境的先验信息。

进一步地，为了方便实现不同密度的地图重建，当需要进行多密度三维重建时，将整个任务分解为抵达测区、重建初始化和稠密三维重建三个阶段，令三个阶段中状态向量中的数据密度依次增大。

进一步地，为了在适应环境先验信息完全未知的场景，当室内GNSS失效、环境信息完全未知时，令传感器自身位姿和环境的先验分布为常数。

附图说明

图1是本发明机器地图用图任务的反馈过程示意图；

图2是本发明中用图任务反馈约束的视觉定位原理示意图；

图3是本发明中状态向量与随机变量的满射关系示意图；

图4-a是本发明验证例的场景1中三维重建结果示意图；

图4-b是本发明验证例的场景1中语义三维重建结果示意图；

图4-c是本发明验证例的场景1中语义标签对应类别示意图；

图5-a是本发明验证例的场景1中得到的分辨率为4cm的语义占据栅格地图；

图5-b是本发明验证例的场景1中得到的分辨率为8cm的语义占据栅格地图；

图5-c是本发明验证例的场景1中得到的分辨率为12cm的语义占据栅格地图；

图6是本发明验证例的场景2中将基础地理影像作为先验的视觉定位流程图；

图7是本发明验证例的场景2中得到的地理影像序列与未知影像特征点匹配示意图；

图8是本发明验证例的场景2中地理影像作为先验约束生成场景稀疏点云结果示意图；

图9-a是本发明验证例的场景3中任务阶段示意图；

图9-b是本发明验证例的场景3中任务约束得到的机器地图三维重建结果示意图；

图9-c是本发明验证例的场景3中SLAM三维重建结果(1×δ)示意图；

图9-d是本发明验证例的场景3中SLAM三维重建结果(8×δ)示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步地说明。

对于人用地图而言，其测图、制图和用图环节相对分离，地图设计者和地图使用者通过地图语言完成信息交换与传递。智能平台作为机器地图的主体，就像人在实际环境中实施操作和作出决策一样，机器始终“置身”于事件循环系统进程之中，不断地与环境、人以及其他智能平台进行交互，实时计算进程或事件的影响结果，并激发新的进程或事件，如此周而复始，形成一个信息交互的回路。这种回路是一种以机器为认知主体的信息传输闭环，是一种类似“‘人’在回路中”的运作机制。在这种意义下，机器地图是无人智能平台感知环境要素、判断环境态势，进而辅助行动决策的认知系统。

从地图学的视角，地图在任务周期内可以区分为观测、地图制作和地图使用3个主要环节。类似的，对应到无人智能平台主体，地图在任务周期内可以分解为环境感知(测图)、环境建模(制图)和决策规划(用)这3个环节，以“用图”任务为主体，根据实时的反馈来动态的调整“制图”和“测图”任务的一个闭环系统，即“测制用一体”的动态运行机制。在该机制下，将测、制、用一般过程细化为用图任务驱动下的几何建模、场景解译、制图建模和任务评估过程，构成一个信息不断循环迭代、“机器在回路”的实时动态计算过程。

在这种情况下，机器地图与人用地图、导航地图、高精地图等最大的区别，是用图任务的反馈将动态地约束整个测图和制图过程。如图1所示，智能平台需要根据用图任务评估的结果，“反思”制图和测图过程的正确性与合理性，形成数据反馈、计算反馈来动态地调整测图和制图过程，加强所有与任务相关的数据参数和计算权重，弱化或抑制与任务不太相关的步骤和信息。这就决定了机器地图的视觉定位(或称“测图”)需要具备反馈与先验约束、适合复杂环境、动态调整测制策略等特性。本发明将具备此类特性的视觉定位方法定义为柔性视觉定位方法。

为此，本发明提供了一种面向机器地图的视觉定位方法，该方法根据当前用图任务状态(Mission State)的分解，依反馈与触发机制动态调整观测与生成。在测图、制图、用图和用图任务评估主要计算骤构成步的信息交互回路中，智能平台在任意t时刻的状态(State)S_t可以利用该时刻的表示为：

S_t+1＝S(g_t,u_t,c_t,k_t) (1)

其中：g_t、u_t、c_t分别表示t时刻测图、制图、用图的状态；k_t为智能平台对g_t、u_t、c_t状态下进行用图任务评估的参数化结果，可表示为k_t＝f(g_t,u_t,c_t)。

上式表明智能平台需要根据t时刻的测图、制图和用图状态来进行用图任务评估，并结合先验信息判定t时刻需采取的动作A_t(包括动作调整、硬件调整、算法调整及调整所带来的代价等)到达最优状态S_t+1，从而构成形成“测制用一体”的信息交互回路。此外，由于动作A_t是用图任务评估和先验约束的共同结果。

为使智能平台能够根据用图任务评估结果来定量地确定状态S_t+1，需将任务反馈k_t及其它先验约束参数化为行动的“价值”，并使智能平台通过对行动价值的学习来实现自身最优状态

的判定。为方便数值运算，本文将任务反馈、先验约束参数化为智能平台的行动回报(Rewards)矩阵R(S_t,A⁽ⁱ⁾ _t)，来表示在状态S_t下进行某种动作A⁽ⁱ⁾ _t所带来的价值，其中上标(i)表示t时刻智能平台可采取的第i个动作。此时，“测制用”一体的机制可利用数学语言描述为：依据t时刻的回报矩阵R(S_t,A⁽ⁱ⁾ _t)来判定智能平台的动作A⁽ⁱ⁾ _t来完成状态转移S_t→S_t+1过程，使得智能平台该动作和状态所带来的效益Q(S_t,A_t)最大。此处，效益Q(S_t,A_t)指S_t状态下智能平台的用图收益与动作A⁽ⁱ⁾ _t消耗的综合效益指标。

为建立合理的数学模型，先提出2个假设：

假设1：状态空间有限假设：由g_t,u_t,c_t,k_t所构成的状态S(g_t,u_t,c_t,k_t)有限多个，即空间L(S∈L)是元素有限的状态空间；

假设2：无后效性(或马尔可夫性)假设：在回报矩阵R(S_t,A⁽ⁱ⁾ _t)和动作A_t的作用下，任意时刻智能平台的状态S_t+1仅与上一时刻的状态S_t有关，而与t时刻之前的状态无关，即：

P(S_t+1|S_t)＝P(S_t+1|S_t,S_t-1,...,S₁) (2)

在以上两点假设的基础上，根据马尔可夫决策过程(Markov Decision Process,MDP)和Q-学习(Q-Learning，构造“测制用”一体机制的目标函数为：

(S_t,A_t)＝argmaxQ(S_t,A_t) (3)

效益函数Q(S_t,A_t)构造为：

Q(S_t,A_t)＝R(S_t,A_t)+γmax{Q(S⁽ⁱ⁾ _t+1,A⁽ⁱ⁾ _t+1)} (4)

式(3)和式(4)即为本发明所建立的“测制用一体”机制的数学模型，由该模型可知，智能平台通过回报矩阵R(S_t,A⁽ⁱ⁾ _t)使下一时刻效益达到最大化的max{Q(S⁽ⁱ⁾ _t+1,A⁽ⁱ⁾ _t+1)}和平衡参数γ来推断当前时刻应处于的状态和相应的动作。

在式(4)的数学模型下，智能平台利用效益函数与回报来寻求当前“测制用”状态的最优解，是一个在线学习的过程，但是该式并没有显式地给出测图过程是如何受到制图用图任务的影响的。因此，本发明需要将在假定用图、制图约束是可参数化的，并利用概率模型构建测图与用图。很显然，机器地图的测图过程受到环境和自身位置的先验信息与用图任务反馈的双重约束，因此本发明采用先验信息与用图任务反馈约束的视觉定位，并进一步构建二者共同约束下机器地图柔性定位的数学模型。

1)环境先验约束的视觉定位。

环境先验信息主要影响智能平台的观测过程，即在一定的先验信息、先验模型的约束下，寻求最优的环境估计量，在本质上是一个状态估计问题，即在给定的影像z与其它观测数据u的情况下，实现对智能平台自身运动状态x和环境y的估计，其概率模型可表述为：

P(x,y|z,u) (5)

依据贝叶斯公式，在观测量分布已知时，有：

因此，几何模型的测量问题与条件概率P(z,u│x,y)和传感器位置和环境的先验分布P(x,y)有关。其中先验分布P(x,y)与对传感器的运行轨迹、环境的先验知识有关，此时几何模型的测值问题为一个最大验后估计(Maximum A Posteriori,MAP)问题：

(x,y)_MAP＝argmaxP(x,y│z,u) ＝argmaxP(z,u|x,y)P(x,y) (7)

特殊的，当对环境和传感器的状态未知(如陌生环境)或测制过程处于初始化阶段而未获得反馈信息时，可令P(x,y)＝a，a为常数，则几何模型测量问题可转化为一个极大似然估计(Maximum Likehood Estimation,MLE)问题，即有怎样的传感器状态和环境，才最可能产生当前的影像和观测数据：

(x,y)_MLE＝argmaxP(z,u|x,y) (8)

此时，几何模型的测制问题等价于一个运动恢复结构(Structure from Motion,SfM)过程，可按照经典的数字摄影测量或多视几何学问题进行求解。

2)用图任务反馈约束的视觉定位。

用图任务主要影响智能平台的观测目标，要确定智能平台应使用何种传感器，通过怎样的观测量来生成怎样的几何模型问题，在本质上是一个行为建模的问题。

设智能平台视觉定位的传感器类型为标量s_i∈N(包括光学相机、深度相机、激光雷达和惯性等观测设备及其组合，共m种)，观测数据类型为向量d_j∈R^|d|(二维或三维数据、稠密或稀疏、效率优先或精度优先等，共n种)，几何建模目标为向量m_k∈N^|m|(占据栅格图、点云地图、拓扑地图、是否包含语义等，共l种)，则视觉定位的状态向量可以表示为:

在“状态空间有限假设”下，向量g∈R^1+|d|+|m|可表示为一个有限状态机(FiniteState Machine,FSM)，即智能平台可在用图任务反馈下，在m×n×l种状态间进行状态转移，其具体原理如图2所示。在实际运算过程中，如将用图任务反馈映射为图中状态转移的概率(或归一化得分)Prob_i，则图2状态转移问题转换为有限马尔科夫链(Finite MarkovChains，FKC)，可进一步结合随机过程等方法进行研究。

3)建立先验约束和图任务反馈约束的视觉定位模型。

式(6)和式(9)分别描述了视觉定位的状态估计和行为建模的问题，二者共同决定智能平台的视觉定位模型，即智能平台要需要在给定的最优状态p(i,j,k)下，结合环境先验信息按式(6)进行环境与自身位置的估计。这样的性质决定了智能平台的视觉定位可应对不同的先验信息和反馈，而自身的定位体系不发生改变。

为了建立统一的模型，本发明假设存在1个满射关系M：R^1+|d|+|m|→R，可将状态向量g(i,j,k)映射为某一随机变量c，且对应的分布函数P(c)(如图4所示)，则在环境先验、传感器位置、反馈约束下的视觉定位模型可以描述为：

其中P(x,y,c)为传感器自身位姿、环境和状态向量共同构成的联合概率；P(z,u│x,y,c)为影像、观测数据、其它给定随机变量已知时，传感器位姿与环境信息的条件概率；P(z,u,c)为传感器位姿、观测数据、给定随机变量的联合概率；P(x,y,z,u,c)为传感器位姿、环境信息、影像、观测数据和给定随机变量的联合概率，即所有随机变量的全概率。

由于随机变量c是由用图任务反馈经一系列规则转换后得到，而用图任务的评估依赖于给定的用图任务和上一时刻测图和用图的结果(如图3所示)，因此在一般情况下，随机变量c与智能平台自身运动状态x和环境y不满足独立条件，式(10)不能按照独立变量的联合分布性质进行分离。特别的，如令式(10)中变量c为常数，则可得转换得到式(6)，即常见的SLAM、SfM等视觉定位问题，是本发明所提出模型的特例。

验证例

机器地图是面向智能平台认知所提出的全新的概念，许多关键科学问题，譬如关系解析、要素解译、用图任务评估等尚处于研究阶段。为对本发明所提出模型构想进行验证，将设定3种典型的场景，并假定用图任务的反馈结果，结合实际环境先验信息进行柔性视觉定位的验证。

为了方便论述，本验证例对视觉向量做如下约定：

传感器类型共3种，即s_i＝{0,1,2}，其中0代表光学相机，1代表深度相机，2代表两种相机的组合；观测数据类型向量是一个2维向量，即d_j＝[d_j0,d_j1]^T∈R²，其中d_j0＝{0,1}分别表示二维、三维点云数据；d_j1＝k×δ表示数据密度，δ为基准密度，k为常数；几何建模目标是2维向量，即m_k＝[m_k0,m_k1]^T∈R²，其中m_k0＝{0,1,2}分别表示拓扑地图、点云地图、占据栅格图；m_k1＝{0,1}分别为不测制语义、测制语义。

以下将设定3种场景对本发明柔性视觉定位模型进行验证。

(1)场景1：语义室内地图的实时构建。

在室内GNSS失效、环境信息完全未知时，智能平台需要快速完成室内场景的构建与理解，因而除了传统几何模型构建问题外，还需要快速获取室内环境的语义信息。此时P(x,y)未知，可令其为常数；面向室内场景智能平台需融合深度相机与光学相机，同步构建场景的三维栅格占据图与语义信息，因此在测制过程中设置控制向量为g＝[2 1 1×δ 21]^T，其中密度参数δ＝1/cm³，即每立方厘有1个栅格占据单元，且栅格边长为1cm。

在运行过程中，智能平台在构建三维栅格占据图的过程中，同步引入PSPNet线程对实时获取的光学图像进行语义分割以获取所摄影像的语义信息，之后引入最大值融合方法，将分割得到的语义信息投影至体素(Voxel)上，最终形成如图4-a和图4-b所示的几何三维重建与语义三维重建结果，语义标签对应类别如图4-c所示。此外，智能平台如面临不同的任务(如定位、重建、路径规划等)，可根据具体情况选择分别生成不同分辨率的占据栅格地图，其结果如图5-a、图5-b和图5-c所示。

很显然，在本例中由于智能平台需要理解环境，因此需要快速、同步构建环境的语义信息和几何信息，所以机器地图的视觉定位比传统的视觉定位方法的结果更加丰富，可根据任务需求进行“定制化”的测图。

(2)场景2：基于先验地理影像约束的视觉定位。

当智能平台缺少GNSS时，智能体依靠单目光学相机，依托SLAM等视觉定位方法虽然可以获得环境的相似模型，但是与实际环境之间存在尺度的差异，也难以获取给定时空基准下的地理坐标(如经纬度、UTM坐标等)。本发明将包含地理坐标的已有的地理影像(Geo-tagged Images)作为约束，通过当前传感器获取影像与已有的地理影像进行检索与匹配，将检索到的地理影像集I＝{I₀,I₁,...,I_n}作为环境的先验信息，构建传感器自身位姿和环境的先验分布P(x,y)＝F(I)，之后按照由粗到精(Coarse-to-fine)的策略进行视觉定位解算，准确估计传感器的位姿并对环境进行初步，主要流程如图6所示。整体的定位流程包括以下两个方面：

1)粗略定位阶段：该过程实质上是影像检索的过程，在基础地理影像中检索出未知影像的最相似帧。最相似帧与未知影像包含相同的场景内容，它们的拍摄点位置是邻近的，因此可以根据最相似帧位姿粗略估算未知影像位姿。在影像检索过程中，本发明根据实体在基础地理影像中的共现性建立实体间的空间邻近关系，构建影像空间关系匹配模型，使得影像检索算法具有更强的鲁棒性。

2)精确定位阶段：在获取最相似帧的基础上，在基础地理影像中截取该帧邻近影像作为参考序列，根据参考序列与未知影像的点对匹配关系精确解算未知影像位姿。图7所示为参考序列与未知影像点对匹配示意图，图中用于解算未知影像位姿的参考序列包含5幅影像(包括最相似帧)，

为参考序列与未知影像的一组匹配点(同名点)，它们对应空间中相同的三维点，由于参考序列中的影像位姿信息已知，结合场景稀疏重构原理可以计算该匹配点三维坐标p，进而生成未知影像二三维匹配点

为未知影像构建多组二三维匹配点，最终结合PnP算法精确解算未知影像位姿，利用位姿估计结果进一步生成稀疏点云的结果如图8所示。

在缺少环境的先验的约束下，本发明通过已有的地理影像序列和传感器拍摄影像之间的匹配点实现了已知影像与未知影像的数据关联，进而解算得到传感器的位姿与环境先验信息，生成包含地理坐标稀疏点云，在给定时空基准下完成视觉定位与环境理解过程。

(3)场景3：任务约束的多密度三维重建。

经典的SLAM问题需要同时解算传感器自身运动与环境三维点云，以间接法ORB-SLAM2框架为例，其点云的密度通常依赖于预先给定的参数、局部特征特性，视觉定位框架特性，生成的点云密度相对均匀，难以根据任务的约束来调整几何模型的测制结果。而机器地图的视觉定位问题中，几何模型的测制应当受到任务的约束，根据任务的不同阶段来调整自身的测制策略。

为对某区域环境感知并对感知结果进行三维重建，本发明将整个任务分解为抵达测区(阶段1)、重建初始化(阶段2)，稠密三维重建(阶段3)这3个阶段，具体情况如图9-a所示。本发明将这3个阶段的状态向量分别设置为g₀＝[0 2 1×δ 1 0]^T、g₁＝[0 2 3×δ 1 0]^T和g₀＝[0 2 8×δ 1 0]^T,并利用ORB-SLAM2和DSO融合的SLAM框架进行三维重建，其中将密度参数δ＝1000/image为基于影像梯度选取的地图点数量，可控制生成点云的稠密程度。根据以上设定，任务约束的机器地图三维重建结果如图9-b所示，使用固定密度的SLAM框架所生成三维点云结果如图9-c和图9-d所示。

显然，在任务的约束下，机器地图视觉定位所生成点云地图实现了对测区目标的重点观测，主要运算资源集中在目标任务当中，而在非主要任务上仅保留基本的视觉定位能力，从而实现了对目标区域的稠密重建、非目标区域的稀疏定位，形成了密度可变的三维重建结果。对比之下，使用传统SLAM进行三维重建，因缺少任务约束，智能生成密度相对均匀的重建结果，会出目标区域现过于稀疏(图9-c)或者全局稠密(图9-d)的情况，难以直接针对任务进行动态调整。

Claims (6)

1.一种面向机器地图的视觉定位方法，其特征在于，该定位方法包括以下步骤：

1)获取用于视觉定位的传感器所在环境的先验信息，构建传感器自身位姿和环境的先验分布；

2)获取用于视觉定位的传感器类型、观测数据类型和几何建模目标并以此得到视觉定位的状态向量；

3)根据先验分布和状态向量建立先验约束和图任务反馈约束的视觉定位模型，利用该视觉定位模型对传感器进行定位。

2.根据权利要求1所述的面向机器地图的视觉定位方法，其特征在于，所述步骤3)中的视觉定位模型为：

其中z为给定的影像；u为其它观测数据；x为视觉定位的传感器自身位姿；y为环境信息；c为给定随机变量，由状态向量映射得到；P(x,y,c)为传感器自身位姿、环境和状态向量共同构成的联合概率；P(z,u│x,y,c)为影像、观测数据、其它给定随机变量已知时，传感器位姿与环境信息的条件概率；P(z,u,c)为传感器位姿、观测数据、给定随机变量的联合概率；P(x,y,z,u,c)为传感器位姿、环境信息、影像、观测数据和给定随机变量的联合概率，即所有随机变量的全概率。

3.根据权利要求1所述的面向机器地图的视觉定位方法，其特征在于，所述的状态向量为：

其中s_i为用视觉定位的传感器类型，s_i∈N；d_j为观测数据类型，包括维度和数据密度；m_k为几何建模目标为向量。

4.根据权利要求1或2所述的面向机器地图的视觉定位方法，其特征在于，当环境先验信息无法直接获取时，将包含地理坐标的已有的地理影像作为约束，通过当前传感器获取影像与已有的地理影像进行检索与匹配，将检索到的地理影像集作为环境的先验信息。

5.根据权利要求3所述的面向机器地图的视觉定位方法，其特征在于，当需要进行多密度三维重建时，将整个任务分解为抵达测区、重建初始化和稠密三维重建三个阶段，令三个阶段中状态向量中的数据密度依次增大。

6.根据权利要求1或2所述的面向机器地图的视觉定位方法，其特征在于，当室内GNSS失效、环境信息完全未知时，令传感器自身位姿和环境的先验分布为常数。

Images