CN111915517A - 一种适用于室内光照不利环境下rgb-d相机全局定位方法 - Google Patents

Abstract

本发明涉及一种适用于室内光照不利环境下RGB‑D相机全局定位方法，具体包括以下步骤：步骤S1：获取RGB‑D传感器的深度图，投影至3D空间，去噪处理生成原始点云，根据Lidar点云数据生成点云地图，体素格网降采样得到采样点云；步骤S2：计算点云的法相，进行聚类分割，得到聚类子集；步骤S3：聚类子集平面拟合，得到所有平面，将被分裂的平面合并，根据平面的法向量的主方向不同，分为互相垂直的3组主方向平面；步骤S4：主方向平面关联匹配，求解得到初始位姿，进行ICP精匹配，计算总误差，选取总误差最小的主方向平面的初始位姿作为全局初始化的位姿。与现有技术相比，本发明具有使用成本低、充分利用RGB‑D相机获取的深度图数据，适用于光照不佳的场景等优点。

Description

一种适用于室内光照不利环境下RGB-D相机全局定位方法

技术领域

本发明涉及全局定位技术领域，尤其是涉及一种适用于室内光照不利环境下RGB-D相机全局定位方法。

背景技术

作为移动机器人基础的核心功能模块之一，精确的定位导航是机器人实现其他功能的基础，为完成特定的任务和服务提供保障。在基于已有地图的定位系统中，全局定位初始化是其基础核心的模块之一，通过全局定位初始化为后续基于当前帧与地图精匹配的定位提供初值，保证机器人能够实现连续的跟踪定位工作。当机器人在连续跟踪定位时，由于传感器遮挡、匹配失败或系统故障等其他不可抗因素而导致跟踪丢失后，需要使用全局定位算法恢复机器人在全局地图中的当前位置实现重定位。

在基于视觉定位方案中，主流的方法是基于词袋模型(BoW)实现重定位。通过基于词袋查找和特征匹配的方法构建当前帧与地图的观测约束，并求解相机位姿实现全局定位初始化。但是对于环境光照变化剧烈，纹理较少或重复纹理等场景，基于词袋模型的方法将失效，且需要针对不同的应用场景和不同的特征点描述子训练特定的词典，才能在实际的应用中获得较好的表现。

由于基于光学影像的视觉定位方案对于外界光照条件有较高要求，且对光照条件的变化比较敏感，人们发展出基于点云的定位方案，点云获取是不受外界光照影响的主动遥感方式，点云获取主要利用成本较高的LiDAR。在LiDAR的定位方案中，可使用点云的空间几何信息，将实时传感器数据与已知几何位置的参考点云地图(reference point cloud)匹配求解传感器在参考点云地图中的全局位姿。但由于点云的数据量较大，且传统的点云精匹配算法(ICP)对初值具有很强的依赖，因此直接使用精匹配的方法往往无法得到理想的结果，需要在精配准前确定实时获取的点云数据(real point cloud)与参考点云的粗略配准位置。现在的解决方法一种是利用移动平台上搭载的其他外部传感器获取的位姿信息，完成全局定位初始化。如GNSS、INS得到的位姿作为初值，但在室内环境下GNSS无法工作，而INS系统获取的位姿数据会随着时间的推移发生飘移。另一种解决方法是进行由粗到精的点云配准，在粗配准阶段影像数据可作为进行粗配准非常有价值的辅助数据，但对于光照不佳场所一般得到的RGB影像很难利用。现有技术中公开了一种用于室外道路环境的解决机器人“唤醒”和“被绑架”问题的全局定位算法，该方法除了考虑点云的几何信息，还利用了激光点云的反射强度信息，通过近邻点云强度的分布直方图编码点云描述子，使用卡方检验实现基于强度信息的粗配准，然后基于几何信息进行验证和精配准完成全局定位的初始化。这种方案对于激光雷达各通道反射强度的一致性具有很高的要求，而目前市面上的激光雷达还很难以满足，且激光点云的反射强度很大程度上受测量距离的影响，对不同材料的反射率差异不显著。

发明内容

本发明的目的就是为了克服上述现有技术存在的成本较高、对光照不利场所得到的RGB影像无法提供初始位姿以及解算约束的应用场景，提供一种只利用RGB-D相机主动获取的深度图数据，不使用RGB光学影像数据，因此适用于室内光照不利环境下RGB-D相机全局位姿确定的方法。

本发明的目的可以通过以下技术方案来实现：

一种适用于室内光照不利环境下RGB-D相机全局定位方法，具体包括以下步骤：

步骤S1：获取RGB-D传感器得到的实时数据中的深度图，将所述深度图投影至3D空间，并进行去噪处理生成原始实时点云，同时根据多站Lidar点云数据进行拼接和裁剪生成参考点云(点云地图)，对以上两类点云进行体素格网降采样得到采样点云；

步骤S2：基于平滑度约束的聚类分割算法计算所述采样点云的法相，并对采样点云进行聚类分割，得到多个聚类子集；

步骤S3：对每个聚类子集进行平面拟合，得到场景数据中的所有平面，将由于过分割或隔断导致的被分裂的重叠的平面进行合并，并根据平面的法向量的主方向不同，将所有平面分为互相垂直的3组主方向平面；

步骤S4：所述3组主方向平面进行关联匹配，遍历所有可能的数据关联，得到存在关联性的若干数据关联组，每组关联求解得到每组的初始位姿，根据所述初始位姿进行ICP精匹配，计算对应的查找近邻点的总误差，选取总误差最小的一组主方向平面对应的初始位姿作为最终全局初始化的位姿。

所述全局定位方法中只采用RGB-D传感器获取的深度图数据，来进行后续的室内6-DOF全局定位，完全不使用其获取的光学RGB影像数据。

所述步骤S2中基于平滑度约束的聚类分割算法包括法相估计模块和区域生长模块，将完整的点云分割成若干个具有同类属性及真实意义的聚类子集。

进一步地，所述法相估计模块包括近邻点查找和局部平面拟合两个过程。

进一步地，所述近邻点查找过程具体为通过建立k维树，根据所述k维树K近邻算法或半径搜索算法查找近邻点，具有更高的效率且在使用中具有更好的灵活性，对于物体边界处的点，在查找邻域时剔除距离过大的异常点。

进一步地，所述局部平面拟合过程具体为拟合出一个当前位置处的局部估计平面，所有近邻点到所述局部估计平面距离的二范数和最小。

进一步地，所述区域生长模块具体执行步骤如下：

步骤S201：设置所述采样点云的残差阈值和平滑度阈值；

步骤S202：在当前未聚类的采样点云中选取残差值最小的采样点云作为当前种子点，若所有的采样点云都完成聚类，退出区域生长模块，否则转至步骤S203；

步骤S203：查找当前种子点的近邻点集，遍历所述近邻点集，若当前种子点与近邻点集的近邻点的法向量夹角满足平滑度阈值，则将对应近邻点归为当前聚类，并且若近邻点的残差值小于残差阈值，则将对应近邻点添加至种子点列表；

步骤S204：若所述种子点列表非空，则将下一个种子点设置为当前种子点，转至步骤S203，若遍历完所述种子点列表，转至步骤S202。

进一步地，所述区域生长模块的约束包括局部连续性和表面平滑性，局部连续性是指被分割为同一个聚类子集的点集在局部需要是连续的，表面平滑性是指聚类子集中的点构成的表面是连续光滑的。

所述步骤S3中根据每个聚类子集的点云数据集，根据所述点云数据集拟合估计出聚类子集对应平面的模型参数。

所述步骤S2中平面拟合的具体过程如下：

待拟合的平面模型的平面方程为：

ax+by＋cz＝d

其中，a、b、c、d均为平面方程的参数，用于构成平面的模型参数，a、b、c为平面法向量；

对应约束条件为：

a²+b²+c²＝1

设置所有点云的平均坐标为

满足：

矩阵形式表达为：

AX＝0

X＝[a,b,c]^T

目标函数为：

min‖AX‖

对应约束条件为：

‖X‖＝1

将矩阵A进行奇异值分解：

A＝U∑V^T

其中，∑是对角矩阵，U和V为酉矩阵，则：

‖AX‖＝‖U∑V^TX‖＝‖∑V^TX‖

其中，V^TX为列矩阵，并且：

‖V^TX‖＝‖X‖＝1

∑的对角元素为奇异值，设最后一个对角元素为最小奇异值：

V^TX＝[0,0,…,1]^T

X＝V[0,0,…,1]^T＝[v₁,v₂,…,v_n][0,0,…,1]^T＝v_n

得到平面方程的参数a、b、c最优解为：

X＝(a,b,c)＝(v_n,1,v_n,2,v_n,3)

因此平面方程的参数d具体为：

在完成平面的模型参数拟合后，需要计算点到平面的距离和作为该点集平面拟合的残差，具体为：

当残差大于预设阈值，则表明输入的聚类子集中不存在理想平面，则删除平面拟合残差较大的聚类子集，仅保留近似为平面结构的聚类子集及其平面的模型参数。

所述步骤S4中每组主方向平面包括三个或三个以上互不平行的平面。

所述步骤S4中关联匹配的具体实现步骤如下：

步骤S401：输入当前实时帧和点云地图及其对应的平面；

步骤S402：对实时帧平面和点云地图平面进行分组，分别得到相应平面放入互相垂直的3组子平面；

步骤S403：根据相应子平面的残差值和包含的点数加权计算每个子平面的得分，并根据得分对实时帧平面的子平面进行排序，选取每个主方向上得分最高的子平面作为该主方向的最高子平面；

步骤S404：遍历点云地图平面，与实时帧平面的3个主方向的得分最高子平面建立3对关联数据，根据所述3对关联数据求解得到相机位姿，通过点云精匹配的方法计算得到所述相机位姿的置信度；

步骤S405：对点云地图平面上所有以3对关联数据作为一组的关联数据组进行朴素遍历，将置信度最高的关联数据组的相机位姿作为所述实时帧的全局定位初始化位姿。

根据关联数据求解得到相机位姿的具体过程如下：

平面参数方程表示为矩阵形式：

np＝d

其中，n为平面法相n(a,b,c)，p为平面上的点p(x,y,z）；

第i对关联数据的误差项为：

e_i＝n_i-Rn’_i

其中，e_i为误差项，R为旋转矩阵，n’_i为相机坐标系下平面的法相，n_i为地图坐标系下平面的法相；

通过最小二乘法，求解误差平方和达到最小的旋转矩阵，具体为：

展开关于旋转矩阵的误差项：

其中，第一项与旋转矩阵无关，第二项由于R^TR＝Ｉ，亦于旋转矩阵无关。因此实际上优化目标函数变为：

定义矩阵：

通过SVD求解出优化问题中旋转矩阵的最优解，具体为：

R＝UV^T

根据所述旋转矩阵继续求解相机位姿中的平移向量，所述平移向量满足：

p＝Rp’+t

nRp’+nt＝d

根据点云的平面n’p’＝d’得到：

(nR-n’)p’+nt＝d-d’

由于求解旋转矩阵时，将nR-n’优化为0，求得平移向量具体为：

t＝n^-1(d-d’)

在得到所述相机位姿之后，将采样点云转至新的坐标系下：

p”＝Rp’+t

然后查找p”中每个点在点云地图p中的最近点，并累加所有点到最近点距离平方和的倒数作为该位姿的置信度σ²：

σ²＝1/∑dis(p”_i→p_j)

选取置信度最高，即实时帧点云与地图点云距离平方和最小，所对应的相机位姿作为该帧全局定位初始化的位姿T_init。

与现有技术相比，本发明具有以下有益效果：

本发明将RGB-D传感器获取的实时数据中的深度图与多站Lidar点云数据进行拼接和裁剪生成的点云地图进行配准来进行全局定位初始化，实时传感器采用RGB-D深度相机具有如下优点：设备成本低，采集数据具有良好的有序性，深度图数据获取不依赖于外界光照条件，同时生成的点云数据较为稠密，可以较好还原场景的局部结构；结合基于平滑度约束的聚类分割算法，进行点云聚类分割、平面拟合与合并分组、关联匹配与位姿求解，将置信度最高的相机位姿作为该帧全局定位初始化的位姿，充分利用RGB-D传感器获取的深度图数据，而不利用RGB影像；由于深度图数据是主动获取的，其获取不依赖于外部光照条件，因此提高了光照不足场景下的定位精度，减小了位置误差。

附图说明

图1为本发明的流程示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示，一种适用于室内光照不利环境下RGB-D相机全局定位方法，具体包括以下步骤：

步骤S1：获取RGB-D传感器得到的实时数据中的深度图，将深度图投影至3D空间，并进行去噪处理生成原始实时点云，同时根据多站Lidar点云数据进行拼接和裁剪生成参考点云(点云地图)，对以上两类点云进行体素格网降采样得到采样点云；

步骤S2：基于平滑度约束的聚类分割算法计算采样点云的法相，并对采样点云进行聚类分割，得到多个聚类子集；

步骤S3：对每个聚类子集进行平面拟合，得到场景数据中的所有平面，将由于过分割或隔断导致的被分裂的重叠的平面进行合并，并根据平面的法向量的主方向不同，将所有平面分为互相垂直的3组主方向平面；

步骤S4：3组主方向平面进行关联匹配，遍历所有可能的数据关联，得到存在关联性的若干数据关联组，每组关联求解得到每组的初始位姿，根据初始位姿进行ICP精匹配，计算对应的查找近邻点的总误差，选取总误差最小的一组主方向平面对应的初始位姿作为最终全局初始化的位姿。

步骤S2中基于平滑度约束的聚类分割算法包括法相估计模块和区域生长模块，将完整的点云分割成若干个具有同类属性及真实意义的聚类子集。

法相估计模块包括近邻点查找和局部平面拟合两个过程。

近邻点查找过程具体为通过建立k维树，根据k维树K近邻算法或半径搜索算法查找近邻点，具有更高的效率且在使用中具有更好的灵活性，对于物体边界处的点，在查找邻域时剔除距离过大的异常点。

局部平面拟合过程具体为拟合出一个当前位置处的局部估计平面，所有近邻点到局部估计平面距离的二范数和最小。

平面拟合的残差来自噪声以及近邻点与局部估计平面模型的一致性。

区域生长模块具体执行步骤如下：

步骤S201：设置采样点云的残差阈值和平滑度阈值；

步骤S202：在当前未聚类的采样点云中选取残差值最小的采样点云作为当前种子点，若所有的采样点云都完成聚类，退出区域生长模块，否则转至步骤S203；

步骤S203：查找当前种子点的近邻点集，遍历近邻点集，若当前种子点与近邻点集的近邻点的法向量夹角满足平滑度阈值，则将对应近邻点归为当前聚类，并且若近邻点的残差值小于残差阈值，则将对应近邻点添加至种子点列表；

步骤S204：若种子点列表非空，则将下一个种子点设置为当前种子点，转至步骤S203，若遍历完种子点列表，转至步骤S202。

区域生长模块的约束包括局部连续性和表面平滑性，局部连续性是指被分割为同一个聚类子集的点集在局部需要是连续的，表面平滑性是指聚类子集中的点构成的表面是连续光滑的。

步骤S3中根据每个聚类子集的点云数据集，根据点云数据集拟合估计出聚类子集对应平面的模型参数。

步骤S2中平面拟合的具体过程如下：

待拟合的平面模型的平面方程为：

ax+by+cz＝d

其中，a、b、c、d均为平面方程的参数，用于构成平面的模型参数，a、b、c为平面法向量；

对应约束条件为：

a²+b²+c²＝1

设置所有点云的平均坐标为

满足：

矩阵形式表达为：

AX＝0

X＝[a,b,c]^T

目标函数为：

min‖AX‖

对应约束条件为：

‖X‖＝1

将矩阵A进行奇异值分解：

A＝U∑V^T

其中，∑是对角矩阵，U和V为酉矩阵，则：

‖AX‖＝‖U∑V^TX‖＝‖∑V^TX‖

其中，V^TX为列矩阵，并且：

‖V^TX‖＝‖X‖＝1

∑的对角元素为奇异值，设最后一个对角元素为最小奇异值：

V^TX＝[0,0,…,1]^T

X＝V[0,0,…,1]^T＝[v₁,v₂,…,v_n］[0,0,…,1]^T＝v_n

得到平面方程的参数a、b、c最优解为：

X＝(a,b,c)＝(v_n,1,v_n,2,v_n,3)

因此平面方程的参数d具体为：

在完成平面的模型参数拟合后，需要计算点到平面的距离和作为该点集平面拟合的残差，具体为：

当残差大于预设阈值，则表明输入的聚类子集中不存在理想平面，则删除平面拟合残差较大的聚类子集，仅保留近似为平面结构的聚类子集及其平面的模型参数。

步骤S4中每组主方向平面包括三个或三个以上互不平行的平面。

步骤S4中关联匹配的具体实现步骤如下：

步骤S401：输入当前实时帧和点云地图及其对应的平面；

步骤S402：对实时帧平面和点云地图平面进行分组，分别得到相应平面放入互相垂直的3组子平面；

步骤S403：根据相应子平面的残差值和包含的点数加权计算每个子平面的得分，并根据得分对实时帧平面的子平面进行排序，选取每个主方向上得分最高的子平面作为该主方向的最高子平面；

步骤S404：遍历点云地图平面，与实时帧平面的3个主方向的得分最高子平面建立3对关联数据，根据3对关联数据求解得到相机位姿，通过点云精匹配的方法计算得到相机位姿的置信度；

步骤S405：对点云地图平面上所有以3对关联数据作为一组的关联数据组进行朴素遍历，将置信度最高的关联数据组的相机位姿作为实时帧的全局定位初始化位姿。

根据关联数据求解得到相机位姿的具体过程如下：

平面参数方程表示为矩阵形式：

np＝d

其中，n为平面法相n(a,b,c)，p为平面上的点p(x,y,z)；

第i对关联数据的误差项为：

e_i＝n_i-Rn′_i

其中，e_i为误差项，R为旋转矩阵，n’_i为相机坐标系下平面的法相，n_i为地图坐标系下平面的法相；

通过最小二乘法，求解误差平方和达到最小的旋转矩阵，具体为：

展开关于旋转矩阵的误差项：

其中，第一项与旋转矩阵无关，第二项由于R^TR＝I，亦于旋转矩阵无关。因此实际上优化目标函数变为：

定义矩阵：

通过SVD求解出优化问题中旋转矩阵的最优解，具体为：

R＝UV^T

根据旋转矩阵继续求解相机位姿中的平移向量，平移向量满足：

p＝Rp’+t

nRp’+nt＝d

根据点云的平面n’p’＝d’得到：

(nR-n’)p’+nt＝d-d’

由于求解旋转矩阵时，将nR-n’优化为0，求得平移向量具体为：

t＝n^-1(d-d’)

在得到相机位姿之后，将采样点云转至新的坐标系下：

p”＝Rp’+t

然后查找p”中每个点在点云地图p中的最近点，并累加所有点到最近点距离平方和的倒数作为该位姿的置信度σ²：

σ²＝1/∑dis(p”_i→p_j)

选取置信度最高，即实时帧点云与地图点云距离平方和最小，所对应的相机位姿作为该帧全局定位初始化的位姿T_init。

选取ICL_NUIM数据集中的办公室和客厅以及自采数据的会议室共三个场景的数据，从三个场景的数据中分别随机选取500帧RGB-D图像作为实验数据，测试使用这些数据进行全局定位初始化的成功率，并统计初始化成功帧的平均时间消耗和精度信息，实验结果如表3.1所示：

表3.1全局定位初始化算法在不同场景表现

其中自采会议室由于无轨迹真值，因此不进行定位精度评价，在共选取的1500帧数据中，平均初始化成功率约为90％，初始化算法的平均耗时约为800ms。

在定位精度方面，由于自采数据无轨迹真值，因此仅对ICL_NUIM数据集的两个场景的数据进行定量评价，其中位置误差小于0.05m，角度误差小于0.5°，满足后续基于点云地图的实时重定位的需要。

此外，需要说明的是，本说明书中所描述的具体实施例，所取名称可以不同，本说明书中所描述的以上内容仅仅是对本发明结构所做的举例说明。凡依据本发明构思的构造、特征及原理所做的等效变化或者简单变化，均包括于本发明的保护范围内。本发明所属技术领域的技术人员可以对所描述的具体实例做各种各样的修改或补充或采用类似的方法，只要不偏离本发明的结构或者超越本权利要求书所定义的范围，均应属于本发明的保护范围。

Claims (10)

1.一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，具体包括以下步骤：

步骤S1：获取RGB-D传感器上传的实时数据中的深度图，将所述深度图投影至3D空间，并进行去噪处理生成实时原始点云，同时根据多站Lidar点云数据进行拼接和裁剪生成参考点云，对以上两类点云进行体素格网降采样得到采样点云；

步骤S2：基于平滑度约束的聚类分割算法计算所述采样点云的法相，并对采样点云进行聚类分割，得到多个聚类子集；

步骤S3：对每个聚类子集进行平面拟合，得到场景数据中的所有平面，将由于过分割或隔断导致的被分裂的重叠的平面进行合并，并根据平面的法向量的主方向不同，将所有平面分为互相垂直的3组主方向平面；

步骤S4：所述3组主方向平面进行关联匹配，遍历所有存在关联性的数据关联，得到可能的若干数据关联组，每个关联组求解得到该组的初始位姿，根据该初始位姿进行ICP精匹配，计算对应的查找近邻点的总误差，选取总误差最小的一组主方向平面对应的初始位姿作为最终全局初始化的位姿。

2.根据权利要求1所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述步骤S2中基于平滑度约束的聚类分割算法包括法相估计模块和区域生长模块。

3.根据权利要求2所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述法相估计模块包括近邻点查找和局部平面拟合两个过程。

4.根据权利要求3所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述近邻点查找过程具体为通过建立k维树，根据所述k维树K近邻算法或半径搜索算法查找近邻点。

5.根据权利要求3所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述局部平面拟合过程具体为拟合出一个当前位置处的局部估计平面，所有近邻点到所述局部估计平面距离的二范数和最小。

6.根据权利要求2所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述区域生长模块具体执行步骤如下：

步骤S201：设置所述采样点云的残差阈值和平滑度阈值；

步骤S202：在当前未聚类的采样点云中选取残差值最小的采样点云作为当前种子点，若所有的采样点云都完成聚类，退出区域生长模块，否则转至步骤S203；

步骤S203：查找当前种子点的近邻点集，遍历所述近邻点集，若当前种子点与近邻点集的近邻点的法向量夹角满足平滑度阈值，则将对应近邻点归为当前聚类，并且若近邻点的残差值小于残差阈值，则将对应近邻点添加至种子点列表；

步骤S204：若所述种子点列表非空，则将下一个种子点设置为当前种子点，转至步骤S203，若遍历完所述种子点列表，转至步骤S202。

7.根据权利要求2所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述区域生长模块的约束包括局部连续性和表面平滑性。

8.根据权利要求1所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述步骤S3中根据每个聚类子集的点云数据集，根据所述点云数据集拟合估计出聚类子集对应平面模型的平面方程的参数。

9.根据权利要求1所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述步骤S4中每组主方向平面包括三个或三个以上互不平行的平面。

10.根据权利要求1所述的一种适用于室内光照不利环境下RGB-D相机全局定位方法，其特征在于，所述步骤S4中关联匹配的具体实现步骤如下：

步骤S401：输入当前实时帧和点云地图及其对应的平面；

步骤S402：对实时帧平面和点云地图平面进行分组，分别得到相应平面放入互相垂直的3组子平面；

步骤S403：根据相应子平面的残差值和包含的点数加权计算每个子平面的得分，并根据得分对实时帧平面的子平面进行排序，选取每个主方向上得分最高的子平面作为该主方向的最高子平面；

步骤S404：遍历点云地图平面，与实时帧平面的3个主方向的得分最高子平面建立3对关联数据，根据所述3对关联数据求解得到相机位姿，通过点云精匹配的方法计算得到所述相机位姿的置信度；

步骤S405：对点云地图平面上所有以3对关联数据作为一组的关联数据组进行朴素遍历，将置信度最高的关联数据组的相机位姿作为所述实时帧的全局定位初始化位姿。

Images