CN115578426A - 一种基于稠密特征匹配的室内服务机器人重定位方法 - Google Patents

Abstract

一种基于稠密特征匹配的室内服务机器人重定位方法，为解决基于关键帧的重定位算法的室内服务机器人在恶劣环境中无法精确定位的问题，根据室内场景的RGB图像和深度图像获取室内服务机器人的位姿，建立关键帧数据库，利用网络提取每张RGB图像的全局描述子组成全局描述子数据库；利用网络提取获得待查询RGB图像的全局描述子，计算全局描述子与全局描述子数据库的余弦相似度，将相似度最高的RGB图像作为检索图像；利用稠密特征匹配网络建立待查询RGB图像和检索图像特征点的关系；根据检索图像对应的位姿计算检索图像的特征点对应的路标点，建立待查询RGB图像特征点与路标点的关系；获得待查询RGB图像对应的室内服务机器人的位姿。

Description

一种基于稠密特征匹配的室内服务机器人重定位方法

技术领域

本发明涉及一种机器人重定位方法，属于机器人位姿估计领域。

背景技术

随着人工智能技术的日渐成熟，要求室内服务机器人具有优秀的环境感知能力。重定位技术作为室内服务机器人重要的功能模块，决定了机器人在开机或重启后能否确定其在全局地图中的位置。视觉重定位技术凭借传感器信息量丰富、价格低廉等优点逐渐成为计算机视觉领域的研究热点。其中，基于关键帧的视觉重定位算法实现了较高的定位精度，

基于关键帧的重定位算法主要包含图像检索技术与特征匹配技术，目前常用的图像检索技术主要包括词袋模型、基于内容的图像检索(CBIR)与基于深度学习的图像检索。其中，基于深度学习的图像检索技术通过卷积神经网络(CNN)提取图像的高维张量作为鲁棒的全局描述符，实现了最优的检索精度，逐渐成为主流发展趋势。Relja等提出的NetVLAD，使用AlexNet与VGG-16作为主干网络，通过弱监督方式进行训练，提取了输入图像的全局描述符，解决了大规模场景中的图像索引问题。特征匹配技术根据实现原理可分为基于人工描述符的特征匹配、基于CNN的特征匹配与基于图网络的特征匹配。其中，基于图网络的特征匹配方式实现了最高的匹配精度。SuperGlue使用SuperPoint提取特征点，并利用Transformer模型中的自注意力与交叉注意力机制整合图像内与图像间的全局信息，获得增强后的特征描述符，据此计算得分矩阵，再使用Sinkhorn算法处理得分矩阵获得软分配矩阵，据此获得匹配结果。

然而，在恶劣场景(如重复纹理区域、视角变化巨大的场景)中，传统的特征匹配技术(如ORB、SIFT)的失效使得室内服务机器人重定位失败。此外，现有的基于深度学习的特征匹配技术(如LoFTR)虽在恶劣场景中实现了精确的匹配效果，但由于高额的计算量，限制了其应用范围。本发明针对特征匹配技术在恶劣场景中失效且计算量大这一问题，对基于关键帧的重定位算法进行改进，提升了室内服务机器人的重定位精度。

发明内容

本发明为了解决基于关键帧的重定位算法的室内服务机器人在恶劣环境中开机后无法精确定位的问题，进而提出了一种基于稠密特征匹配的室内服务机器人重定位方法。

本发明采取的技术方案是：

它包括以下步骤：

步骤一、采集室内场景的RGB图像和深度图像；

步骤二、利用ORB-SLAM2算法得到RGB图像和深度图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿；

步骤三、建立ResNet-101网络，利用数据集ImageNet对建立的ResNet-101网络进行训练，得到训练好的ResNet-101网络；

利用训练好的ResNet-101网络提取关键帧数据库中每张RGB图像的特征向量，将所述特征向量作为全局描述子，重复上述操作，得到关键帧数据库中所有RGB图像的全局描述子，将所有全局描述子合并，得到全局描述子数据库；

步骤四、建立稠密特征匹配网络，利用Scannet数据集对稠密特征匹配网络进行训练，得到训练好的稠密特征匹配网络；

步骤五、将室内场景中的室内服务机器人开机，获得室内场景的待查询RGB图像，利用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的全局描述子，根据MegLoc计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，将相似度最高的RGB图像作为检索图像；

步骤六、利用步骤四训练好的稠密特征匹配网络建立待查询RGB图像和检索图像之间2D特征点的对应关系；

步骤七、根据检索图像对应的关键帧数据库中的室内服务机器人的位姿计算检索图像的2D特征点对应的3D路标点，再根据待查询RGB图像与检索图像之间2D特征点的对应关系和所述3D路标点建立待查询RGB图像的2D特征点与所述3D路标点的对应关系；

步骤八、根据待查询RGB图像的2D特征点与3D路标点间的对应关系，依次利用RANSAC和PNP算法计算待查询RGB图像对应的室内服务机器人的位姿。

进一步地，所述步骤一中采集室内场景的RGB图像和深度图像，具体过程为：

利用数据采集设备采集室内场景的RGB图像和深度图像，采集的RGB图像和深度图像根据采集时间自动对齐。

进一步地，所述步骤二中利用ORB-SLAM2算法得到RGB图像和深度图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿，具体过程为：

根据步骤一中对齐后的图像利用ORB-SLAM2算法得到每张图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿，将对齐后的第一张图像对应的室内服务机器人位置作为世界坐标系原点。

进一步地，所述步骤四中建立稠密特征匹配网络，利用Scannet数据集对稠密特征匹配网络进行训练，得到训练好的稠密特征匹配网络，具体过程为：

利用Scannet数据集对稠密特征匹配网络进行训练，设置训练次数为40，每结束一次训练均用当前稠密特征匹配网络进行一次测试，若当前测试结果优于上一轮训练得到的测试结果，则将当前稠密特征匹配网络的参数保存为最优参数，继续训练，直至训练结束，得到最优的稠密特征匹配网络，将最优的稠密特征匹配网络作为训练好的稠密特征匹配网络。

进一步地，所述稠密特征匹配网络依次包括特征提取器、锚点选择器、匹配生成模块、匹配细化模块。

进一步地，所述步骤五中将室内场景中的室内服务机器人开机，获得室内场景的待查询RGB图像，利用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的全局描述子，根据MegLoc计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，将相似度最高的待查询RGB图像作为检索图像，具体过程为：

将室内服务机器人在室内场景的任意位置开机后，获得室内场景的一张待查询RGB图像I_A，使用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的512维特征向量，即得到了对应的全局描述子v_A，计算全局描述子v_A与全局描述子数据库V_train中所有全局描述子的余弦相似度c_i：

其中，v_i表示全局描述子数据库中第i个全局描述子，i＝1,2,…,m；<·,·>表示向量点乘；|·|表示绝对值；

选取全局描述子数据库V_train中与全局描述子v_A余弦相似度c_i最高的全局描述子对应的RGB图像作为检索图像I_B。

进一步地，所述步骤六中利用步骤四训练好的稠密特征匹配网络建立待查询RGB图像和检索图像之间2D特征点的对应关系，具体过程为：

a、利用稠密特征匹配网络的特征提取器分别提取待查询RGB图像和检索图像的低阶特征图和高阶特征图，具体过程为：

利用特征金字塔网络提取待查询RGB图像的低阶特征图

和高阶特征图

以及检索图像的低阶特征图

和高阶特征图

定义N＝H/8×W/8，其中，H表示待查询RGB图像或检索图像的高；W表示待查询RGB图像或检索图像的宽；

表示低阶特征图的特征维度；

表示高阶特征图的特征维度；

b、利用稠密特征匹配网络的锚点选择器选取每张高阶特征图的锚点，根据选取的所有高阶特征图的锚点构建稀疏图神经网络，锚点选择器依次包括多尺度融合模块、位置编码模块、锚点选择模块，具体过程为：

1)、将每张高阶特征图输入多尺度融合模块内，多尺度融合模块使用三个不同大小的卷积核提取每张高阶特征图中以每个像素为中心的不同子窗口的特征，并在通道维度对特征进行拼接，获得上下文信息，得到包含多尺度上下文信息的高阶特征图

以及每张高阶特征图的一维特征X＝[0,1,…,W-1]和Y＝[0,1,…,H-1]；

其中，

表示待查询RGB图像I_A或检索图像I_B的高阶特征图，I∈{A,B}；

表示使用1×1的卷积核将特征维度压缩为原来的1/2；

表示使用3×3的卷积核将特征维度压缩为原来的1/4；

表示使用5×5的卷积核将特征维度压缩为原来的1/4；

2)、位置编码模块利用两个多层感知器分别将1)中得到的每张高阶特征图的一维特征X＝[0,1,…,W-1]和Y＝[0,1,…,H-1]投影到两个高维向量中，得到位置编码图，将位置编码图嵌入到包含多尺度上下文信息的高阶特征图

中，得到包含多尺度上下文信息和位置信息的高阶特征图

将包含多尺度上下文信息和位置信息的高阶特征图

中的每个像素点作为特征点，每个特征点对应的

维特征作为所述特征点的视觉描述子，则特征点为：

K_I＝(P_I,F_I)

其中，P_I表示特征点在步骤五得到的待查询RGB图像或检索图像中的像素坐标，维度为R^N×2；F_I表示特征点的视觉描述子，维度为

3)、将包含多尺度上下文信息和位置信息的高阶特征图

输入锚点选择模块，在锚点选择模块内以高阶特征图

中的每个像素点为中心，分别裁剪出尺寸为1×1和3×3的局部窗口，获得两组特征序列

I∈{A,B}，

根据特征序列

定义特征点对应的显著性张量为：

其中，

表示显著性张量，

表示

中的每个张量

与

相减；

根据显著性张量

依次利用一个全局平均池化层和多个卷积层提取每个特征点的显著性得分

选取显著性得分最大的T个特征点作为锚点，T≥1，则锚点为：

其中，

表示锚点在步骤五得到的待查询RGB图像或检索图像中的像素坐标，维度为R^T×2；

表示锚点的视觉描述子，维度为

根据上述方法得到所有锚点，利用所有的锚点构建稀疏图神经网络，并计算得到软分配矩阵；

c、利用稠密特征匹配网络的匹配生成模块对构建的稀疏图神经网络进行特征增强，获得粗匹配结果，所述匹配生成模块依次包括全局信息整合模块和粗匹配模块，具体过程为：

1)、全局信息整合模块利用线性Transformer模型中的自注意力与交叉注意力机制构建编码器Enc，将全局信息整合至稀疏图神经网络的每个锚点的视觉描述子中，得到新的视觉描述子；

线性Transformer模型：

LAtt(Q,K,V)＝φ(Q)(φ(K)^TV)

φ(·)＝elu(·)+1

其中，Q表示查询张量；K表示指键张量；V表示值张量；elu(·)表示指数线性激活函数；φ(Q)、φ(K)表示函数形式；

编码器Enc：

U′＝Enc(U,R)＝U+MLP([U||MLP(Λ)])

Λ＝LAtt(Q,K,V)

其中，U表示稀疏图神经网络中被更新的锚点；R表示检索锚点；[·||·]表示沿通道维度拼接操作；

在将全局信息整合至稀疏图神经网络的每个锚点的视觉描述子的过程中，在线性Transformer模型中的自注意力机制层，锚点U和锚点R是相同的；在线性Transformer模型中的交叉注意力层，锚点U和锚点R是不同的；

2)、在粗匹配模块内根据新的视觉描述子利用向量点乘计算得分矩阵，得分矩阵中的每个元素表示特征点间的相似度得分，分别使用softmax对得分矩阵的行和列进行归一化，得到软分配矩阵，根据软分配矩阵选择相似度得分大于阈值，且满足相互最近原则的匹配为预测匹配点，再获得预测匹配点在锚点中相应的索引，得到N^f对粗匹配结果

d、利用稠密特征匹配网络的匹配细化模块优化粗匹配结果，获得细匹配结果，具体过程为：

1)、将N^f对粗匹配结果输入匹配细化模块内，确定每一对粗匹配结果在低阶特征图中的位置，并以所述位置为中心对低阶特征图进行裁剪，得到一组尺寸为w×w的局部窗口，即得到每一对粗匹配结果的局部特征；

2)、将每一对粗匹配结果的局部特征在通道维度进行拼接，得到拼接后的局部特征；

3)、将拼接后的局部特征利用全局平均池化层和卷积层转换为一维张量，再利用两个单独的1×1卷积层得到偏移量δ与置信度c，

4)、利用sigmoid激活函数将置信度c归一化，得到N^f对细匹配结果

其中，

表示待查询RGB图像的细匹配结果，

表示检索图像的细匹配结果，即得到待查询RGB图像和检索图像之间2D特征点的对应关系。

进一步地，所述步骤七中根据检索图像对应的关键帧数据库中的室内服务机器人的位姿计算检索图像的2D特征点对应的3D路标点，再根据待查询RGB图像与检索图像之间2D特征点的对应关系和所述3D路标点建立待查询RGB图像的2D特征点与所述3D路标点的对应关系，具体过程为：

针对检索图像，根据检索图像的细匹配结果

和已知的检索图像在关键帧数据库中相应的深度图像，以及检索图像在世界坐标系中的机器人位姿TR_wc ^B，利用针孔模型得到检索图像的细匹配结果

对应的3D路标点的世界坐标P_w：

其中，K表示相机内参；z_c表示检索图像中匹配点所对应的3D路标点的世界坐标的深度值；

针对待查询RGB图像，根据待查询RGB图像与检索图像之间2D特征点的对应关系和3D路标点的世界坐标P_w得到待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系，按照上述方法，得到N^f组待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系。

进一步地，所述步骤八中根据待查询RGB图像的2D特征点与3D路标点间的对应关系，依次利用RANSAC和PNP算法计算室内服务机器人的位姿，具体过程为：

随机选取N^o组待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系优化重投影误差，优化公式为：

其中，d_i表示第i个3D路标点在相机坐标系中的深度值；TR表示优化后的机器人位姿，机器人位姿初值为

根据优化后的相机位姿TR，将N^o个3D路标点的世界坐标P_w投影至待查询RGB图像中，得到投影点，根据投影点与相应的2D特征点

计算重投影误差，若所有3D路标点的重投影误差均小于10像素，则优化结束，获得室内服务机器人位姿TR；若存在外点，则重复以上步骤，当循环设定次数后仍未满足以上条件，则求解机器人位姿失败。

进一步地，稠密特征匹配网络在训练过程中的损失函数为：

(1)利用Focal Loss计算粗匹配损失L^c：

其中，N^cp表示匹配的锚点数量；N^cn表示未匹配的锚点数量；α表示Focal Loss中的权重系数；γ表示Focal Loss中的聚焦系数；

(2)利用欧式距离计算回归损失L^reg：

其中，N^f表示粗匹配点数量；将待查询RGB图像中预测的粗匹配点投影到检索图像中，得到真值像素坐标

计算真值偏移量

当真值偏移量小于设定的阈值时计算相应的回归损失；

(3)利用二分类交叉熵损失分类损失L^cla：

(4)总损失L^a：

L^a＝L^c+ψL^reg+βL^cla

其中，ψ表示回归损失权重；β表示分类损失权重。

有益效果：

本发明根据采集的室内场景的RGB图像和深度图像利用ORB-SLAM2算法获取对应的室内服务机器人的位姿，建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿；利用经过数据集ImageNet训练得到的ResNet-101网络提取关键帧数据库中每张RGB图像的全局描述子，得到关键帧数据库中所有RGB图像的全局描述子，组成全局描述子数据库；利用Scannet数据集训练得到的稠密特征匹配网络；获得室内场景的待查询RGB图像，利用ResNet-101网络提取所述待查询RGB图像的全局描述子，根据MegLoc计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，将关键帧数据库中相似度最高的RGB图像作为检索图像；利用稠密特征匹配网络建立待查询RGB图像和检索图像之间2D特征点的对应匹配关系；根据检索图像对应的关键帧数据库中的室内服务机器人的位姿计算检索图像的2D特征点对应的3D路标点，再根据待查询RGB图像与检索图像之间2D特征点的对应关系和所述3D路标点建立待查询RGB图像的2D特征点与所述3D路标点的对应关系；根据待查询RGB图像的2D特征点与3D路标点间的对应关系，依次利用RANSAC和PNP算法计算待查询RGB图像对应的室内服务机器人的位姿。

本发明对传统的基于关键帧的重定位算法中的特征匹配算法进行了改进，本发明使用ResNet-101提取RGB图像的高维特征作为全局描述子，以实现图像检索。此外，还提出了一种基于锚点的稠密特征匹配网络(RAFMP)，稠密特征匹配网络将特征匹配问题视为图匹配问题，从特征点中选出锚点，以此作为节点构建图神经网络，以减少训练过程中的显存占用问题。稠密特征匹配网络依次包括特征提取器、锚点选择器、匹配生成模块、匹配细化模块。锚点选择器通过提取所需的特征点(不是所有的特征点)作为锚点构建稀疏图神经网络，显著地减少了稀疏图神经网络在训练过程中的GPU占用，解决了传统的基于稀疏图神经网络的特征匹配算法占用显存大的缺点。匹配细化模块对粗匹配结果进行优化，获得最终的匹配结果，解决了近似匹配真值影响的缺点，间接的提高了室内服务机器人在恶劣场景中的开机定位精度。且根据实施例的结果表明，本发明的重定位算法极大地提高了室内服务机器人在恶劣场景中的开机定位精度，实现了0.089m的位置偏差与1.374°的角度偏差。

附图说明

图1是本发明的结构示意图；

图2是步骤三中利用ResNet-101网络提取每张RGB图像特征向量的示意图；

图3是稠密特征匹配网络的示意图；

图4是稠密特征匹配网络中位置编码模块过程示意图；

图5是稀疏图神经网络的示意图；

图6是稠密特征匹配网络中全局信息整合模块中特征更新流程示意图；

图7是稠密特征匹配网络中近似匹配真值示意图；

图8是实施例所提供的真实场景中稠密特征匹配网络所预测的匹配效果；

图9是实施例所提供的真实实验场景与机器人运行轨迹示意图；

图10是实施例所提供的不同重定位算法对比效果；

具体实施方式

具体实施方式一：结合图1-图7说明本实施方式，本实施方式所述一种基于稠密特征匹配的室内服务机器人重定位方法，它包括以下步骤：

步骤一、采集室内场景的RGB图像和深度图像，具体过程为：

利用数据采集设备采集室内场景的RGB图像和深度图像，采集的RGB图像和深度图像根据采集时间自动对齐。

本发明使用移动底盘型号为SCOUT MINI Lite&Pro Kit，深度相机型号为IntelRealSense D435的数据采集设备根据应用的实际室内环境采集实际室内场景的RGB图像和深度图像，所采集的图像中需包含采集场景中大部分具有代表性的物体。在图像采集过程中使用的操作系统为ROS melodic，所述数据采集设备包括但不限于上述设备，只要可以实现室内场景的RGB图像和深度图像的采集即可。

首先，使用数据采集设备的遥控器控制移动底盘运动，在ROS melodic系统下，通过Intel RealSense D435深度相机提供的/camera/color/image_raw与/camera/aligned_depth_to_color/image_raw话题以10帧/s的速度获取室内场景的RGB图像和深度图像，得到的RGB图像和深度图像根据采集的时间戳会自动对齐，将对齐后的RGB图像和深度图像分别保存到数据采集设备的机械硬盘的两个文件夹内。

步骤二、利用ORB-SLAM2算法得到RGB图像和深度图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿，具体过程为：

根据步骤一中对齐后的图像(室内场景的RGB图像和深度图像)利用开源的ORB-SLAM2算法求解得到每张图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括大量的RGB图像、深度图像和对应的室内服务机器人的位姿，将对齐后的第一张图像对应的室内服务机器人位置作为世界坐标系原点。

步骤三、建立ResNet-101网络，利用数据集ImageNet对建立的ResNet-101网络进行训练，得到训练好的ResNet-101网络；

利用训练好的ResNet-101网络提取关键帧数据库中每张RGB图像的特征向量，将所述特征向量作为全局描述子，重复上述操作，得到关键帧数据库中所有RGB图像的全局描述子，将所有全局描述子合并，得到全局描述子数据库。

利用大规模图像分类数据集ImageNet对ResNet-101网络进行训练，得到训练好的ResNet-101网络，ResNet-101的结构如图2所示。利用训练好的ResNet-101网络提取步骤二中建立的关键帧数据库中每张RGB图像的512维特征向量，将提取的特征向量作为所述RGB图像的全局描述子，重复上述操作，得到关键帧数据库中所有RGB图像的特征向量，即得到所有RGB图像的全局描述子，将所有全局描述子合并，得到全局描述子数据库。假设关键帧数据库中RGB图像数量为m，经过ResNet-101提取每张RGB图像所对应的512维特征向量后，则获得特征向量集合V_train＝{v₁,v₂,v₃,…,v_m}，从而建立全局描述子数据库。

步骤四、建立稠密特征匹配网络，利用Scannet数据集对稠密特征匹配网络进行训练，得到训练好的稠密特征匹配网络，具体过程为：

利用Scannet数据集对稠密特征匹配网络RAFMP进行训练，设置训练次数为40，每结束一次训练均用当前稠密特征匹配网络RAFMP进行一次测试，若当前测试结果优于上一轮训练得到的测试结果，则将当前稠密特征匹配网络RAFMP的参数保存为最优参数，即为最优的模型，继续训练，直至训练结束，得到最优的稠密特征匹配网络RAFMP作为本发明所需的稠密特征匹配网络，利用最优的稠密特征匹配网络预测待查询RGB图像和RGB图像间的稠密特征匹配。本发明利用针对室内场景的，包含多达1.2TRGB图像的Scannet数据集对稠密特征匹配网络RAFMP进行训练，得到的稠密特征匹配网络RAFMP具有良好的泛化性，可泛化到未知室内场景中。稠密特征匹配网络的结构如图3所示。

所述稠密特征匹配网络依次包括特征提取器、锚点选择器、匹配生成模块、匹配细化模块。

特征提取器(基于CNN的特征提取器)用于提取每张RGB图像的低阶特征图(分辨率为原图的1/2)与高阶特征图(分辨率为原图的1/8)，每张RGB图像的低阶特征图与高阶特征图为相对应关系；锚点选择器用于选取并得到每张高阶特征图内的锚点，根据锚点建立稀疏图神经网络；匹配生成模块用于获取稀疏图神经网络的粗匹配结果；匹配细化模块用于优化粗匹配结果；

步骤五、将室内场景中的室内服务机器人开机，获得室内场景的待查询RGB图像，利用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的全局描述子，根据MegLoc(来源论文：MegLoc:A Robust and Accurate Visual Localization Pipeline)计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，将关键帧数据库中相似度最高的RGB图像作为检索图像，具体过程为：

将室内服务机器人在室内场景的任意位置开机后，获得此时室内场景的一张待查询RGB图像I_A，使用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的512维特征向量，即得到了对应的全局描述子v_A，计算全局描述子v_A与全局描述子数据库V_train中所有全局描述子的余弦相似度c_i：

其中，v_i表示全局描述子数据库中第i个全局描述子，i＝1,2,…,m；

<·,·>表示向量点乘，|·|表示绝对值。

其中，余弦相似度c_i描述了全局描述子在高维空间中夹角的余弦值。选取全局描述子数据库V_train中与全局描述子v_A余弦相似度c_i最高的全局描述子对应的一张RGB图像(即位于关键帧数据库中的RGB图像)作为检索图像I_B，检索图像I_B对应的全局描述子为v_B。

步骤六、利用步骤四训练好的稠密特征匹配网络建立待查询RGB图像和检索图像之间2D特征点的对应关系，从而实现稠密特征匹配，具体过程为：

a、利用稠密特征匹配网络的特征提取器分别提取待查询RGB图像和检索图像的低阶特征图和高阶特征图，具体过程为：

利用特征金字塔网络提取待查询RGB图像的低阶特征图

和高阶特征图

以及检索图像的低阶特征图

和高阶特征图

定义N＝H/8×W/8，其中，H表示待查询RGB图像或检索图像的高；W表示待查询RGB图像或检索图像的宽；

表示低阶特征图的特征维度；

表示高阶特征图的特征维度。

特征金字塔网络(FPN)能够有效地整合低阶几何信息与高阶语义信息，生成更具描述力的特征图。因此，如图3中(a)所示，对于输入的待查询RGB图像I_A与检索图像I_B，本发明使用特征金字塔网络(FPN)提取低阶特征图

与高阶特征图

为方便起见，本发明定义N＝H/8×W/8。

b、利用稠密特征匹配网络的锚点选择器选取每张高阶特征图的锚点，根据选取的所有高阶特征图的锚点构建稀疏图神经网络，具体过程为：

锚点选择器依次包括多尺度融合模块、位置编码模块、锚点选择模块；

锚点选择器(APS)由三部分组成：

(1)多尺度融合模块(MSEL)，用于将多尺度上下文信息整合到高阶特征图中，多尺度指的是使用3个不同大小的卷积核进行信息提取，上下文信息指的是卷积核将窗口中的特征进行整合，从而获得的整合信息。如图3中(b)所示，将每张高阶特征图输入多尺度融合模块内，多尺度融合模块使用三个不同大小的卷积核(1×1，3×3，5×5)提取每张高阶特征图中以每个像素为中心的不同子窗口的特征，并在通道维度进行拼接，获得更具描述力的特征表示。其中，不同子窗口的特征指的是卷积核以每个像素为中心，将1×1或3×3或5×5窗口中的特征整合为一个特征。为减少算法的计算量，本发明需要考虑的是如何分配每个卷积核输出的特征维度。由于卷积操作的计算量与卷积核尺寸的平方以及输出特征维度成正比。因此，本发明使用大卷积核将高阶特征图投影到维度较低的特征空间，使用小卷积核投影到维度较高的特征空间。具体而言，当高阶特征图的维度为256时，本发明使用1×1卷积核将高阶特征图投影到128维特征空间中，使用3×3和5×5卷积核将高阶特征图分别投影到两个64维特征空间中。相比于平均分配特征维度，本发明分别为三个卷积核分配了

的特征维度。这一过程可表示为：

其中，

表示包含多尺度上下文信息的高阶特征图；

表示待查询RGB图像I_A或检索图像I_B的高阶特征图，I∈{A,B}；

表示使用1×1的卷积核将特征维度压缩为原来的1/2；

表示使用3×3的卷积核将特征维度压缩为原来的1/4；

表示使用5×5的卷积核将特征维度压缩为原来的1/4。

(2)位置编码模块(DPE)，用于将高阶特征图的像素的位置信息整合到高阶特征图中。本发明提出一种简单但有效的解耦位置编码方式，即位置编码模块(DPE)使用两个独立的多层感知器(MLP)分别将每张高阶特征图的一维特征X＝[0,1,…,W-1]与Y＝[0,1,…,H-1]投影到两个独立的高维(256维)向量中，得到位置编码图，该过程如图4所示。将位置编码图嵌入到包含多尺度上下文信息的高阶特征图

中，得到包含多尺度上下文信息和位置信息的高阶特征图

经过DPE模块之后，本发明获得了包含多尺度上下文信息和位置信息的高阶特征图

记载了多尺度上下文信息与位置信息，实现了更好的场景理解。将包含多尺度上下文信息和位置信息的高阶特征图

中的每个像素点作为特征点，每个特征点对应的

维特征作为所述特征点的视觉描述子。因此，本发明定义特征点为：

K_I＝(P_I,F_I)

其中，P_I表示特征点在步骤五得到的待查询RGB图像或检索图像中的像素坐标，维度为R^N×2；F_I表示特征点的视觉描述子，维度为

(3)锚点选择模块(SSB)，根据每个特征点与周围的差异生成显著性得分，选取得分最高的T(T≥1)个特征点作为锚点，根据锚点构建稀疏图神经网络。

如图3中(b)的锚点选择模块所示，以包含多尺度上下文信息和位置信息的每张高阶特征图

中的每个像素点为中心，分别裁剪出两组尺寸为1×1和3×3的局部窗口，获得两组特征序列

其中I∈{A,B}，

根据特征序列，本发明定义特征点对应的显著性张量为：

其中，

表示

中的每个张量

与

相减。针对显著性张量

本发明依次使用一个全局平均池化层与多个卷积层提取每个特征点的显著性得分

显著性得分越大，则该特征点与周围像素的差异越大，该特征点可以提供包含丰富场景信息的视觉描述子，有利于进行匹配搜索。因此，本发明保留显著性得分最大的T(T≥1)个特征点作为锚点，并遗弃剩余的特征点。本发明将锚点定义为：

其中，

表示锚点在步骤五得到的待查询RGB图像或检索图像中的像素坐标，包括

维度为R^T×2；

表示锚点的视觉描述子，维度为

根据所有的锚点构建稀疏图神经网络，并通过计算生成软分配矩阵M。稀疏图神经网络示意图如图5所示，其中每个节点表示提取出的锚点。

锚点选择器将高阶特征图中的每个像素点作为特征点，根据每个特征点与周围信息的差异为每个特征点预测一个显著性得分，令得分最高的T(T≥1)个特征点作为锚点。随后，本发明将锚点作为节点构建图神经网络，而不是利用所有的特征点构建图神经网络，这显著地减少了GPU占用与不必要的信息交流。

c、利用稠密特征匹配网络的匹配生成模块对构建的稀疏图神经网络进行特征增强，获得粗匹配结果，具体过程为：

匹配生成模块利用Transformer模型中的自注意力与交叉注意力机制构建编码器Enc，将全局上下文信息传播到整个稀疏图神经网络中进行特征增强，获得粗匹配结果。其中，全局上下文信息指的是Transformer通过全局感受野提取出的整张图像的信息。

匹配生成模块(CPB)依次包括全局信息整合模块和粗匹配模块。

粗匹配模块通过求解软分配矩阵M获得像素级粗匹配结果。

(1)全局信息整合模块(GIAB)

如图3中(c)全局信息整合模块所示，本发明将特征匹配问题视为图匹配问题，使用Transformer模型进行全局信息交流与特征增强。全局信息整合模块通过执行多次Transformer模型中的自注意力与交叉注意力机制构建编码器Enc，将全局信息整合至稀疏图神经网络的每个锚点的视觉描述子中，得到新的视觉描述子。

对于张量Q∈R^m×d，K∈R^n×d，V∈R^n×d，传统的Transformer模型首先通过Q，K计算注意力矩阵，据此从V中整合信息：

其中，Q表示查询张量，K，V分别指键张量、值张量，由于传统的Transformer模型中的矩阵乘法使Transformer模型难以处理长序列特征，同时考虑到

的序列长度，本发明使用线性Transformer模型，利用矩阵乘法的结合律，将计算复杂度从

降为

线性Transformer模型的表达式为：

LAtt(Q,K,V)＝φ(Q)(φ(K)^TV)

φ(·)＝elu(·)+1

其中，elu(·)表示指数线性激活函数，φ(Q)、φ(K)表示函数形式。

在稀疏图神经网络结构中，图节点即为锚点。在信息传递时，对锚点的视觉描述子进行更新，视觉描述子更新过程如图6所示。定义稀疏图神经网络结构中，被更新的锚点为U，检索锚点为R，利用检索锚点R更新锚点为U，编码器Enc定义为：

U′＝Enc(U,R)＝U+MLP([U||MLP(Λ)])

Λ＝LAtt(Q,K,V)

其中，[·||·]表示沿通道维度拼接操作。参考人在匹配两张图像中的特征点时来回观察两张图像，Transformer模型利用自注意力机制整合两张图像内的信息，利用交叉注意力机制整合两张图像间的信息。对于自注意力机制层，节点U，节点R是相同的(均为

或

)。对于交叉注意力层，节点U，R是不同的((

或

)或(

或

))。自注意力层与交叉注意力层循环L次，在第l层中，视觉描述子的增强过程可定义为：

其中，I，J∈{A,B}，I≠J。

最终整合全局信息后，本发明获得更具描述力的新的视觉描述子

(2)粗匹配模块(CCL)

如图3中的粗匹配模块所示，根据新的视觉描述子

利用向量点乘计算得分矩阵S∈R^T×T，得分矩阵中的每个元素表示特征点间的相似度得分。随后，分别使用softmax(即dual-softmax)对得分矩阵S的行和列进行归一化操作，得到软分配矩阵M∈R^{T ×T}。

M(i,j)＝softmax(S(i,·))·softmax(S(·,j))

其中，

表示待查询RGB图像I_A中第i个锚点所对应的视觉描述子；

表示检索图像I_B中第j个锚点所对应的视觉描述子；<·>表示向量间的内积计算，softmax(S(i,·))表示对得分矩阵S的第i行进行softmax操作；softmax(S(·,j))表示对得分矩阵S的第j列进行softmax操作。

根据软分配矩阵M选择相似度得分大于指定阈值λ，且满足相互最近原则(MNN，即相似度得分在软分配矩阵M对应的行和列中均为最大值)的匹配为预测匹配点，再通过下式获得预测匹配点在锚点中相应的索引D。至此，粗匹配模块CCL最终获得N^f对或组粗匹配结果

其中，

指的是待查询RGB图像I_A的锚点的像素坐标，

指的是图像I_B的锚点的像素坐标。

d、利用稠密特征匹配网络的匹配细化模块优化粗匹配结果，获得最终匹配，具体过程为：

匹配细化模块使用粗匹配结果作为可靠的局部先验以压缩搜索空间，并通过预测偏移量对粗匹配结果进行优化，获得最终匹配。此外，本发明使用线性Transformer模型进一步降低复杂度。相比于使用所有特征点在稀疏图神经网络结构中传递全局信息，本发明所提出的RAFMP网络将特征匹配的计算复杂度从O(N²C)降至O(TC²)，其中N为特征点的数量，T为锚点数量，C为描述子的维度，一般情况下，C＜＜T。

如图7所示，在制作真值匹配时，本发明遵循相互最近邻原则(MNN)，即左图中的特征点A通过两帧间的相对位姿投影到右图中，获得点C。在右图的所有特征点中，点B距离点C最近，则点A与点B视为一组候选匹配。同理，右图中的点B投影到左图中获得点D，与点A最近，则点B与点A视为一组候选匹配。点A与点B满足MNN原则，视为一组匹配真值。然而，点A真实的匹配点为点C，与点B并不完全重合，从而导致训练时使用的是近似匹配(点A与点B)，使得网络无法向最优方向优化。

如图3中的模块(d)匹配细化模块(CRB)所示，对于每一对粗匹配结果，首先确定它在低阶特征图

中的位置，并以所述位置为中心在对低阶特征图上裁剪出一组尺寸为w×w的局部窗口，从而获得局部特征

随后，两组局部特征在通道维度进行拼接作为匹配细化模块中匹配细化网络的输入。随后，本发明使用一个全局平均池化层和一组卷积层将拼接后的局部特征转换为一个一维张量，并使用两个单独的1×1卷积层预测偏移量δ与置信度c，

用于优化粗匹配结果。此外，本发明使用sigmoid激活函数将置信度

归一化。通过使用预测的偏移量，本发明最终获得细匹配结果

定义为：

为了更好地展示匹配细化模块的有效性，通过使用预测偏移量δ优化粗匹配点，使得细匹配点更加接近于匹配真值，这证明了本发明所提出的CRB模块的有效性。

步骤七、根据检索图像对应的关键帧数据库中的室内服务机器人的位姿计算检索图像的2D特征点对应的3D路标点，再根据待查询RGB图像与检索图像之间2D特征点的对应关系和所述3D路标点建立待查询RGB图像的2D特征点与所述3D路标点的对应关系，具体过程为：

针对输入的待查询RGB图像I_A与检索图像I_B，通过基于锚点的稠密特征匹配网络(RAFMP)获得的预测匹配的像素坐标为

针对检索图像I_B，已知其相应的深度图像以及在世界坐标系中的机器人位姿TR_wc ^B，根据针孔模型可计算出

对应的3D路标点的世界坐标P_w：

其中，K表示相机内参，z_c表示检索图像中匹配点所对应的3D路标点的世界坐标的深度值。

据此，针对待查询RGB图像I_A中的特征点，通过本发明所提出的稠密特征匹配网络建立了

间的匹配关系，同时已知

对应的路标点的世界坐标P_w，从而建立了待查询RGB图像的匹配点的像素坐标

(2D特征点)与3D路标点的世界坐标P_w间的对应联系，按照上述方法，得到N^f组待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系。

步骤八、根据待查询RGB图像的2D特征点与3D路标点间的对应关系，依次利用RANSAC和PNP算法计算待查询RGB图像对应的室内服务机器人的位姿，具体过程为：

本发明使用RANSAC与PNP算法求解待查询RGB图像对应的室内服务机器人位姿，RANSAC作为鲁棒算法，用于降低预测的错误匹配对于机器人位姿估计精度的影响。具体而言，在步骤七中建立的N^f组待查询RGB图像的2D特征点

与3D路标点P_w对应关系中，随机选取N^o组优化重投影误差。在优化过程中，机器人位姿初值TR_wc ^B定为检索图像I_B所对应的位姿：

重投影误差的优化公式为：

其中，d_i是第i个路标点在相机坐标系中的深度值；TR是优化后的机器人位姿，优化初值为

根据优化后的相机位姿TR，将所选的N^o个路标点的世界坐标P_w投影至待查询RGB图像中，获得投影点，随后将投影点与相应的2D特征点

计算重投影误差。若所有路标点的重投影误差均小于10像素，则优化结束，获得最终的室内服务机器人位姿TR。若存在外点，则重复以上步骤。当循环设定次数(10000次)后仍未满足以上条件，则认为PNP算法求解机器人位姿失败。

考虑到室内服务机器人的开机定位精度要求，性能评价主要有六个指标：平移误差中值Δt、平移误差的均方根误差(RMSE)、旋转误差中值ΔR、旋转误差的均方根误差(RMSE)、准确率、召回率。相比其他方法，本发明实现了0.089m的平移偏差、0.140的平移误差RMSE、1.374°的旋转偏差、2.033的旋转误差RMSE、82.11％的准确率与98.58％的召回率，优于其他方法。

稠密特征匹配网络RAFMP在训练过程中的损失函数：

稠密特征匹配网络RAFMP使用的损失函数L^a包括三部分：

(1)粗匹配损失L^c

本发明所提出的RAFMP网络通过匹配真值D^gt＝{(i,j)}进行监督学习。粗匹配表示了锚点间的对应关系，其中每个锚点代表每张原图(待查询RGB图像I_A和检索图像I_B)中每个8×8的网格。为获得匹配真值D^gt，本发明首先将每张原图(待查询RGB图像I_A和检索图像I_B)划分为多个8×8的网格，每个网格的中心像素均表示一个特征点，在所有特征点中选出锚点，对于待查询RGB图像I_A中的锚点，本发明根据两帧间的相对位姿将其投影到检索图像I_B中，并选择距离投影点最近的锚点作为候选匹配点。对于检索图像I_B中的锚点，根据两帧间的相对位姿将其投影到待查询RGB图像I_A中，并选择距离投影点最近的锚点作为候选匹配点。在获得两组候选匹配点后，本发明选择满足相互最近邻原则(MNN)的特征点作为匹配真值，并将匹配真值中锚点的索引定义为D^gt＝{(i,j)}。已知匹配真值索引D^gt与软分配矩阵M，考虑到正负样本失衡问题(如，2000组锚点中，匹配上的点对可能只有200组，远小于未匹配的1800组)，本发明使用Focal Loss计算粗匹配损失L^c：

其中，N^cp表示制作匹配真值时匹配上的锚点数量；N^cn表示制作匹配真值时未匹配上的锚点数量；α表示Focal Loss中的权重系数；γ表示Focal Loss中的聚焦系数。

(2)回归损失L^reg

为获得准确的匹配结果，本发明对CRB模块预测的偏移量δ进行监督。具体而言，本发明将待查询RGB图像I_A中预测的粗匹配点

投影到检索图像I_B中，获得真值像素坐标

据此计算真值偏移量

已知预测的偏移量δ与真值偏移量δ^gt，回归损失L^reg定义为：

其中，N^f表示粗匹配点数量。为防止外点对于梯度反向传播的影响，使得网络向错误的方向优化，只有当δ^gt(i)小于预先设定好的阈值θ时，才计算相应的回归损失。

(3)分类损失L^cla

本发明预测了每对粗匹配点的置信度c，并计算了分类损失。具体而言，已知真值偏移量δ^gt，本发明定义当真值偏移量δ^gt小于θ时为正样本，置信度置为1，否则为负样本，置信度置为0：

已知真值置信度c^gt与预测置信度c，本发明使用二分类交叉熵损失计算分类损失：

针对软分配矩阵M的粗匹配损失L^c，用于评估粗匹配结果；针对预测偏移量的回归损失L^reg，用于评估匹配优化结果；针对置信度的分类损失L^cla，用于评估匹配置信度。最终，总损失L^a定义为：

L^a＝L^c+ψL^reg+βL^cla

其中，ψ表示回归损失权重；β表示分类损失权重。

实施例

由于针对室内场景的Scannet数据集多达1.2T的RGB图像，所以本发明使用Scannet数据集训练出的稠密特征匹配网络RAFMP模型具有良好的泛化性，可泛化到未知室内场景。为验证这一观点，本发明在室内场景中采集了四组图像，分别包括复杂纹理场景(杂物)，稀疏纹理场景(地面)以及剧烈的光照、视角变化，并使用训练好的稠密特征匹配网络RAFMP进行了匹配预测，匹配结果如图8所示，可见即使针对未见过的室内场景，RAFMP依旧体现出了优越的匹配效果，虽然存在一些错误的匹配点，但在计算相对位姿时，能够通过RANSAC等鲁棒算法进行剔除。因此可使用RAFMP替代传统的SIFT特征点匹配方式，实现更加精确的机器人开机重启自定位。

针对基于锚点的稠密特征匹配网络RAFMP，在训练过程中，低阶特征图与高阶特征图的特征维度分别设为

锚点数T取2000。针对稠密特征匹配网络的匹配生成模块的全局信息整合模块，自注意力层与交叉注意力层的循环次数设为L＝5。在预测粗匹配结果时，置信度阈值设为λ＝0.2。考虑到原始图像被划分为8×8的网格，稠密特征匹配网络的匹配细化模块CRB中截取的局部窗口尺寸设为w＝8(8×8)，损失函数中几何距离阈值设为θ＝8。为平衡粗匹配损失L^c、回归损失L^reg与分类损失L^cla，权重系数设为ψ＝0.25，β＝0.25。在Focal Loss中，权重系数设为α＝0.25，聚焦系数设为γ＝2。

本发明在一个实际场景中采集了数据集以验证所提出的基于稠密特征匹配的室内服务机器人重定位算法的有效性。在采集过程中，使用NoMachine软件远程连接工控机，并使用遥控器操作室内服务机器人，在ROS melodic环境下，通过Intel RealSense D435深度相机提供的/camera/color/image_raw与/camera/aligned_depth_to_color/image_raw话题以10帧/s的速度获取RGB图像和深度图像，获取的RGB图像和深度图像会自动对齐，再分别保存到机械硬盘的两个文件夹内，随后使用时间戳对RGB图与深度图像对齐。实际室内实验场景的面积为12m×9m，包含大量的重复纹理、大视角变化等恶劣情况下的图像。根据采集到的RGB图像与深度图像，采用ORB-SLAM2算法对机器人位姿进行估计，最终构建了场景的关键帧数据库，关键帧数据库中包括5410张RGB图像、深度图像与相应的机器人位姿p＝[x,q]，x表示平移向量，q表示旋转矩阵对应的四元数。此外，采集了1830张待查询RGB图像用于测试。实际场景示意图与机器人采集轨迹如图9所示。其中，蓝线表示关键帧数据库中图像的采集轨迹，红线表示测试图像的采集轨迹。

根据构建的关键帧数据库，使用ResNet-101提取关键帧数据库的全局描述子，建立全局描述子数据库。获取室内场景的一张待查询RGB图像I_A，提取待查询RGB图像I_A的全局描述子，根据MegLoc计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，关键帧数据库中选出相似度最高的一张图像为检索图像I_B。使用基于锚点的稠密特征匹配网络(RAFMP)建立查询图像I_A与检索图像I_B间的2D特征点的匹配关系

根据检索图像I_B在关键帧数据库中对应的深度图像与机器人位姿，计算检索图像I_B的2D匹配点

所对应的3D路标点的世界坐标P_w，从而建立查询图像I_A的2D特征点

与3D路标点P_w间的对应关系。将检索图像I_B的机器人位姿作为优化过程的初始位姿，随后依次使用RANSAC和PNP算法通过优化重投影误差求解室内服务机器人位姿。

为更好地体现本发明所提出的基于稠密特征匹配的室内服务机器人重定位算法的有效性与优越性，选择了两种常用的视觉重定位算法与本发明所提算法进行了对比：

(1)基于图像检索的室内服务机器人重定位算法：将检索图像I_B所对应的机器人位姿作为查询图像I_A的位姿，不使用SIFT或RAFMP预测的匹配点进行优化。

(2)基于图像检索与SIFT特征匹配的室内服务机器人重定位算法：将检索图像I_B所对应的机器人位姿作为查询图像的初始位姿。随后通过SIFT特征点匹配建立查询图像I_A的2D像素点

与3D路标点P_w间的对应关系。据此通过优化重投影误差获得查询图像的位姿。

(3)基于稠密特征匹配的室内服务机器人重定位算法(本发明提出)：通过图像检索与RAFMP稠密匹配实现。将检索图像I_B所对应的机器人位姿作为查询图像的初始位姿。随后通过RAFMP稠密特征匹配建立查询图像I_A的2D像素点

与3D路标点P_w间的对应关系。据此通过优化重投影误差获得查询图像的位姿。

为缓解错误预测匹配的影响，本发明只保存RAFMP预测的置信度最高的200组匹配结果。其中，置信度指的是粗匹配模块(CCL)中获得的软分配矩阵M中的值。本发明所对比的三种室内服务机器人重定位算法在不同评价指标下的性能如下表所示。

表1不同视觉重定位算法的对比实验数据

由表可见，与常用的重定位算法相比，本发明提出的基于稠密特征匹配的室内服务机器人重定位算法显著降低了平移误差中值Δt、旋转误差中值ΔR、误差的RMSE。这表明，本发明所提出的重定位算法在降低定位误差的同时，误差的浮动(RMSE)也变小。此外，本发明所提出的重定位算法显著提升了室内服务机器人开机定位准确率与召回率。

为更好地可视化本发明所提出的室内服务机器人重定位算法效果，本发明展示了真实轨迹、重定位算法(1)、重定位算法(3)(本发明提出)的预测轨迹。考虑到算法(2)的召回率较低，在此不进行可视化。考虑到Z方向的预测误差对机器人的定位效果没有显著的影响，为了更好的可视化，本发明绘制了XY平面上的机器人轨迹。可见，本发明所提出的重定位算法的预测轨迹(直线)相比于直接将检索图像所对应的位姿作为机器人位姿(点线)要更加接近机器人真值轨迹(虚线)。在标注的方框1)、2)、3)中，这一优越性更加明显。

Claims (10)

1.一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：它包括以下步骤：

步骤一、采集室内场景的RGB图像和深度图像；

步骤二、利用ORB-SLAM2算法得到RGB图像和深度图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿；

步骤三、建立ResNet-101网络，利用数据集ImageNet对建立的ResNet-101网络进行训练，得到训练好的ResNet-101网络；

利用训练好的ResNet-101网络提取关键帧数据库中每张RGB图像的特征向量，将所述特征向量作为全局描述子，重复上述操作，得到关键帧数据库中所有RGB图像的全局描述子，将所有全局描述子合并，得到全局描述子数据库；

步骤四、建立稠密特征匹配网络，利用Scannet数据集对稠密特征匹配网络进行训练，得到训练好的稠密特征匹配网络；

步骤五、将室内场景中的室内服务机器人开机，获得室内场景的待查询RGB图像，利用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的全局描述子，根据MegLoc计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，将相似度最高的RGB图像作为检索图像；

步骤六、利用步骤四训练好的稠密特征匹配网络建立待查询RGB图像和检索图像之间2D特征点的对应关系；

步骤七、根据检索图像对应的关键帧数据库中的室内服务机器人的位姿计算检索图像的2D特征点对应的3D路标点，再根据待查询RGB图像与检索图像之间2D特征点的对应关系和所述3D路标点建立待查询RGB图像的2D特征点与所述3D路标点的对应关系；

步骤八、根据待查询RGB图像的2D特征点与3D路标点间的对应关系，依次利用RANSAC和PNP算法计算待查询RGB图像对应的室内服务机器人的位姿。

2.根据权利要求1中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤一中采集室内场景的RGB图像和深度图像，具体过程为：

利用数据采集设备采集室内场景的RGB图像和深度图像，采集的RGB图像和深度图像根据采集时间自动对齐。

3.根据权利要求2中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤二中利用ORB-SLAM2算法得到RGB图像和深度图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿，具体过程为：

根据步骤一中对齐后的图像利用ORB-SLAM2算法得到每张图像对应的室内服务机器人的位姿，并建立关键帧数据库，关键帧数据库包括RGB图像、深度图像和对应的室内服务机器人的位姿，将对齐后的第一张图像对应的室内服务机器人位置作为世界坐标系原点。

4.根据权利要求3中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤四中建立稠密特征匹配网络，利用Scannet数据集对稠密特征匹配网络进行训练，得到训练好的稠密特征匹配网络，具体过程为：

利用Scannet数据集对稠密特征匹配网络进行训练，设置训练次数为40，每结束一次训练均用当前稠密特征匹配网络进行一次测试，若当前测试结果优于上一轮训练得到的测试结果，则将当前稠密特征匹配网络的参数保存为最优参数，继续训练，直至训练结束，得到最优的稠密特征匹配网络，将最优的稠密特征匹配网络作为训练好的稠密特征匹配网络。

5.根据权利要求4中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述稠密特征匹配网络依次包括特征提取器、锚点选择器、匹配生成模块、匹配细化模块。

6.根据权利要求5中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤五中将室内场景中的室内服务机器人开机，获得室内场景的待查询RGB图像，利用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的全局描述子，根据MegLoc计算所述全局描述子与全局描述子数据库中所有的全局描述子的余弦相似度，将相似度最高的待查询RGB图像作为检索图像，具体过程为：

将室内服务机器人在室内场景的任意位置开机后，获得室内场景的一张待查询RGB图像I_A，使用步骤三中训练好的ResNet-101网络提取所述待查询RGB图像的512维特征向量，即得到了对应的全局描述子v_A，计算全局描述子v_A与全局描述子数据库V_train中所有全局描述子的余弦相似度c_i：

其中，v_i表示全局描述子数据库中第i个全局描述子，i＝1,2,…,m；<·,·>表示向量点乘；|·|表示绝对值；

选取全局描述子数据库V_train中与全局描述子v_A余弦相似度c_i最高的全局描述子对应的RGB图像作为检索图像I_B。

7.根据权利要求6中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤六中利用步骤四训练好的稠密特征匹配网络建立待查询RGB图像和检索图像之间2D特征点的对应关系，具体过程为：

a、利用稠密特征匹配网络的特征提取器分别提取待查询RGB图像和检索图像的低阶特征图和高阶特征图，具体过程为：

利用特征金字塔网络提取待查询RGB图像的低阶特征图

和高阶特征图

以及检索图像的低阶特征图

和高阶特征图

定义N＝H/8×W/8，其中，H表示待查询RGB图像或检索图像的高；W表示待查询RGB图像或检索图像的宽；

表示低阶特征图的特征维度；

表示高阶特征图的特征维度；

b、利用稠密特征匹配网络的锚点选择器选取每张高阶特征图的锚点，根据选取的所有高阶特征图的锚点构建稀疏图神经网络，锚点选择器依次包括多尺度融合模块、位置编码模块、锚点选择模块，具体过程为：

1)、将每张高阶特征图输入多尺度融合模块内，多尺度融合模块使用三个不同大小的卷积核提取每张高阶特征图中以每个像素为中心的不同子窗口的特征，并在通道维度对特征进行拼接，获得上下文信息，得到包含多尺度上下文信息的高阶特征图

以及每张高阶特征图的一维特征X＝[0,1,…,W-1]和Y＝[0,1,…,H-1]；

其中，

表示待查询RGB图像I_A或检索图像I_B的高阶特征图，I∈{A,B}；

表示使用1×1的卷积核将特征维度压缩为原来的1/2；

表示使用3×3的卷积核将特征维度压缩为原来的1/4；

表示使用5×5的卷积核将特征维度压缩为原来的1/4；

2)、位置编码模块利用两个多层感知器分别将1)中得到的每张高阶特征图的一维特征X＝[0,1,…,W-1]和Y＝[0,1,…,H-1]投影到两个高维向量中，得到位置编码图，将位置编码图嵌入到包含多尺度上下文信息的高阶特征图

中，得到包含多尺度上下文信息和位置信息的高阶特征图

将包含多尺度上下文信息和位置信息的高阶特征图

中的每个像素点作为特征点，每个特征点对应的

维特征作为所述特征点的视觉描述子，则特征点为：

K_I＝(P_I,F_I)

其中，P_I表示特征点在步骤五得到的待查询RGB图像或检索图像中的像素坐标，维度为R^N×2；F_I表示特征点的视觉描述子，维度为

3)、将包含多尺度上下文信息和位置信息的高阶特征图

输入锚点选择模块，在锚点选择模块内以高阶特征图

中的每个像素点为中心，分别裁剪出尺寸为1×1和3×3的局部窗口，获得两组特征序列

I∈{A,B}，

根据特征序列

定义特征点对应的显著性张量为：

其中，

表示显著性张量，

表示

中的每个张量

与

相减；

根据显著性张量

依次利用一个全局平均池化层和多个卷积层提取每个特征点的显著性得分

选取显著性得分最大的T个特征点作为锚点，T≥1，则锚点为：

其中，

表示锚点在步骤五得到的待查询RGB图像或检索图像中的像素坐标，维度为R^{T ×2}；

表示锚点的视觉描述子，维度为

根据上述方法得到所有锚点，利用所有的锚点构建稀疏图神经网络，并计算得到软分配矩阵；

c、利用稠密特征匹配网络的匹配生成模块对构建的稀疏图神经网络进行特征增强，获得粗匹配结果，所述匹配生成模块依次包括全局信息整合模块和粗匹配模块，具体过程为：

1)、全局信息整合模块利用线性Transformer模型中的自注意力与交叉注意力机制构建编码器Enc，将全局信息整合至稀疏图神经网络的每个锚点的视觉描述子中，得到新的视觉描述子；

线性Transformer模型：

LAtt(Q,K,V)＝φ(Q)(φ(K)^TV)

φ(·)＝elu(·)+1

其中，Q表示查询张量；K表示指键张量；V表示值张量；elu(·)表示指数线性激活函数；φ(Q)、φ(K)表示函数形式；

编码器Enc：

U′＝Enc(U,R)＝U+MLP([UMLP(Λ)])

Λ＝LAtt(Q,K,V)

其中，U表示稀疏图神经网络中被更新的锚点；R表示检索锚点；[·||·]表示沿通道维度拼接操作；

在将全局信息整合至稀疏图神经网络的每个锚点的视觉描述子的过程中，在线性Transformer模型中的自注意力机制层，锚点U和锚点R是相同的；在线性Transformer模型中的交叉注意力层，锚点U和锚点R是不同的；

2)、在粗匹配模块内根据新的视觉描述子利用向量点乘计算得分矩阵，得分矩阵中的每个元素表示特征点间的相似度得分，分别使用softmax对得分矩阵的行和列进行归一化，得到软分配矩阵，根据软分配矩阵选择相似度得分大于阈值，且满足相互最近原则的匹配为预测匹配点，再获得预测匹配点在锚点中相应的索引，得到N^f对粗匹配结果

d、利用稠密特征匹配网络的匹配细化模块优化粗匹配结果，获得细匹配结果，具体过程为：

1)、将N^f对粗匹配结果输入匹配细化模块内，确定每一对粗匹配结果在低阶特征图中的位置，并以所述位置为中心对低阶特征图进行裁剪，得到一组尺寸为w×w的局部窗口，即得到每一对粗匹配结果的局部特征；

2)、将每一对粗匹配结果的局部特征在通道维度进行拼接，得到拼接后的局部特征；

3)、将拼接后的局部特征利用全局平均池化层和卷积层转换为一维张量，再利用两个单独的1×1卷积层得到偏移量δ与置信度c，

4)、利用sigmoid激活函数将置信度c归一化，得到N^f对细匹配结果

其中，

表示待查询RGB图像的细匹配结果，

表示检索图像的细匹配结果，即得到待查询RGB图像和检索图像之间2D特征点的对应关系。

8.根据权利要求7中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤七中根据检索图像对应的关键帧数据库中的室内服务机器人的位姿计算检索图像的2D特征点对应的3D路标点，再根据待查询RGB图像与检索图像之间2D特征点的对应关系和所述3D路标点建立待查询RGB图像的2D特征点与所述3D路标点的对应关系，具体过程为：

针对检索图像，根据检索图像的细匹配结果

和已知的检索图像在关键帧数据库中相应的深度图像，以及检索图像在世界坐标系中的机器人位姿TR_wc ^B，利用针孔模型得到检索图像的细匹配结果

对应的3D路标点的世界坐标P_w：

其中，K表示相机内参；z_c表示检索图像中匹配点所对应的3D路标点的世界坐标的深度值；

针对待查询RGB图像，根据待查询RGB图像与检索图像之间2D特征点的对应关系和3D路标点的世界坐标P_w得到待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系，按照上述方法，得到N^f组待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系。

9.根据权利要求8中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：所述步骤八中根据待查询RGB图像的2D特征点与3D路标点间的对应关系，依次利用RANSAC和PNP算法计算室内服务机器人的位姿，具体过程为：

随机选取N^o组待查询RGB图像的2D特征点

与3D路标点的世界坐标P_w间的对应关系优化重投影误差，优化公式为：

其中，d_i表示第i个3D路标点在相机坐标系中的深度值；TR表示优化后的机器人位姿，机器人位姿初值为

根据优化后的相机位姿TR，将N^o个3D路标点的世界坐标P_w投影至待查询RGB图像中，得到投影点，根据投影点与相应的2D特征点

计算重投影误差，若所有3D路标点的重投影误差均小于10像素，则优化结束，获得室内服务机器人位姿TR；若存在外点，则重复以上步骤，当循环设定次数后仍未满足以上条件，则求解机器人位姿失败。

10.根据权利要求9中所述的一种基于稠密特征匹配的室内服务机器人重定位方法，其特征在于：稠密特征匹配网络在训练过程中的损失函数为：

(1)利用Focal Loss计算粗匹配损失L^c：

其中，N^cp表示匹配的锚点数量；N^cn表示未匹配的锚点数量；α表示Focal Loss中的权重系数；γ表示Focal Loss中的聚焦系数；

(2)利用欧式距离计算回归损失L^reg：

其中，N^f表示粗匹配点数量；将待查询RGB图像中预测的粗匹配点投影到检索图像中，得到真值像素坐标

计算真值偏移量

当真值偏移量小于设定的阈值时计算相应的回归损失；

(3)利用二分类交叉熵损失分类损失L^cla：

(4)总损失L^a：

L^a＝L^c+ψL^reg+βL^cla

其中，ψ表示回归损失权重；β表示分类损失权重。

Images