CN111402336A

CN111402336A - 基于语义slam的动态环境相机位姿估计及语义地图构建方法

Info

Publication number: CN111402336A
Application number: CN202010209728.0A
Authority: CN
Inventors: 蒋永实; 张琪; 朱晓阳; 于海涛
Original assignee: Zhongke Qichuang Tianjin Technology Co ltd; Institute of Automation of Chinese Academy of Science
Current assignee: Zhongke Qichuang Tianjin Technology Co ltd; Institute of Automation of Chinese Academy of Science
Priority date: 2020-03-23
Filing date: 2020-03-23
Publication date: 2020-07-10
Anticipated expiration: 2040-03-23
Also published as: CN111402336B

Abstract

本发明属于计算机视觉技术领域，具体涉及了一种基于语义SLAM的动态环境相机位姿估计及物体级语义地图构建方法，旨在解决现有技术在动态环境下相机位姿估计精度低以及所建地图对环境表达不充分的问题。本发明包括：采集RGB‑D图像序列，使用神经网络模型进行实例分割，自适应生成运动判定阈值，结合语义类别和极线约束关系检测处于运动状态的目标物体，滤除当前帧各运动目标的特征点，估计相机位姿。对目标前景和非目标背景分别建模，跟踪动态目标前景，并进行2D‑3D语义关联，建立前景‑背景独立的物体级语义地图。本发明动态场景下相机位姿估计精度高，所建地图能够对环境进行物体级的语义表达，有较高的鲁棒性和稳定性。

Description

基于语义SLAM的动态环境相机位姿估计及语义地图构建方法

技术领域

本发明属于计算机视觉技术领域，具体涉及了一种基于语义SLAM的动态环境相机位姿估计及语义地图构建方法。

背景技术

即时定位与地图构建(SLAM,Simultaneous localization and mapping)技术能够在未知环境下，通过传感器不断捕捉环境信息，实时进行相机自身位姿估计，并同时构建三维地图。在过去的三十年中，SLAM技术飞速发展，在机器人、自动驾驶、增强现实等领域都扮演着重要的角色。

然而，传统SLAM系统所建的三维地图仅包含点、线、面等描述三维空间几何结构的低层信息，难以满足更复杂的应用需求。语义SLAM技术结合了传统SLAM技术与基于深度学习的语义分割技术，能够构建同时包含几何信息和语义信息的三维地图，提供高层信息表达，进一步加深对未知环境的理解。但是，语义SLAM技术仍处于发展阶段，还存在以下问题：

第一，在动态场景下，传统SLAM系统难以精确估计的相机位姿轨迹，现有语义SLAM系统一定程度上利用语义信息解决了这个问题。如Berta Bescos等人提出的DynaSLAM、Chao Yu等人提出的DS-SLAM，这些语义SLAM系统使用不同方法检测动态物体，剔除动态物体范围内的全部特征点。但一方面，这些方法仅针对了某一类语义类别，没有考虑场景中多类物体的不同影响。另一方面，这些方法会误剔除属于静态目标的特征点，造成相机位姿的估计误差，甚至跟踪丢失。

第二，现有语义SLAM技术未能对场景中的动态物体跟踪建图。如DynaSLAM仅对静态背景进行了建图，DS-SLAM并未对动态目标进行跟踪，其他如McCormac J等人提出的SemanticFusion系统则仅进行了静态场景下的语义关联。这些语义SLAM系统所建的三维地图都仅针对静态场景，不能适应场景中物体的动态变化，地图的表达能力和稳定性不足。

总的来说，现有SLAM系统难以在动态场景下精确估计的相机位姿轨迹，以及无法构建语义地图动态环境下的语义地图，实现相机的准确定位和环境信息的高层次表达。

发明内容

为了解决现有技术中的上述问题，即现有技术在动态环境下相机位姿估计精度低以及所建地图对环境表达不充分的问题，本发明提供了一种基于语义SLAM的动态环境相机位姿估计和物体级建图方法，该方法包括：

步骤S10，通过RGB-D相机获取设定帧数的室内环境彩色图像、深度图像，并基于时间戳匹配图像，获得彩色图像序列、深度图像序列；

步骤S20，分别提取彩色图像序列中每一帧的ORB特征点，获得第一特征点集；通过深度神经网络Mask R-CNN对所述彩色图像序列逐帧进行实例分割，获得各帧彩色图像的实例分割结果；所述实例分割结果包括彩色图像的语义mask以及各物体的物体类别、位置框；

步骤S30，基于各物体的先验运动权重、基础运动判定阈值，分别获取各类别物体的运动判定阈值，结合所述彩色图像的语义mask以及各物体的物体类别、位置框，获取处于运动状态的目标物体，并剔除对应的第一特征点集中目标物体的特征点，获得第二特征点集；

步骤S40，将所述第二特征点集中当前帧与前一帧的特征点进行特征匹配，结合非线性优化方法，获取全局最优相机位姿序列。

在一些优选的实施例中，步骤S30包括：

步骤S31，基于各物体的先验运动权重、基础运动判定阈值，分别获取各类别物体的运动判定阈值；通过L-K光流法对当前帧与前一帧进行光流跟踪，获得当前帧匹配点集、前一帧匹配点集；

步骤S32，计算所述当前帧匹配点集、前一帧匹配点集之间的基础矩阵，并分别结合各物体的运动判定阈值通过极线约束法判断当前帧中的每一个物体是否处于运动状态，获得运动目标；

步骤S33，剔除所述第一特征点集中处于所述运动目标对应的位置框内且对应的语义mask类别与物体类别一致的特征点，获得第二特征点集。

在一些优选的实施例中，步骤S32包括：

步骤S321，计算所述当前帧匹配点集、前一帧匹配点集之间的基础矩阵；对于当前帧中任一物体，若其先验运动权重为0，则该物体为非运动目标；否则跳转步骤S322；

步骤S322，判断当前帧匹配点集中位于所述物体对应的位置框内的每一个匹配点在语义mask的类别与物体类别是否一致，并保留类别一致的匹配点作为当前目标前景点；

步骤S323，以当前目标前景点中每一个点与其在前一帧图像中对应的点作为匹配点对，结合所述基础矩阵，计算当前目标前景点中每一个匹配点在当前帧中的极线，并计算该匹配点到极线的距离；

步骤S324，判断所述距离是否大于该点所属目标物体的运动判定阈值，若是，则该匹配点为当前目标物体动态匹配点；

步骤S325，若当前目标物体动态匹配点在当前目标前景点中的比例大于设定阈值，则该目标物体处于运动状态，获得运动目标。

在一些优选的实施例中，所述极线，其计算方法为：

其中，L代表

在第i帧图像中对应的极线；F代表第i帧匹配点集、第i-1帧图像匹配点集之间的基础矩阵；

代表第i帧图像中第k个前景点

在第i-1帧图像中对应的匹配点，

为

在第i-1帧图像上的齐次坐标，

代表以向量形式表示的极线的三个分量。

在一些优选的实施例中，步骤S323中“计算该匹配点到极线的距离”，其方法为：

其中，d代表

到其对应极线的距离，

代表第i帧图像中第k个前景点，

代表

在第i-1帧图像中对应的匹配点，F为当前帧匹配点集、前一帧匹配点集之间的基础矩阵，X代表极线向量的第一维参数，Y代表极线向量的第二维参数。

在一些优选的实施例中，步骤S40包括：

步骤S41，将所述各帧图像对应的第二特征点集中当前帧与前一帧的特征点进行特征匹配，通过ICP方法获取相机的初始位姿并更新当前相机位姿序列，将满足设定条件的图像帧作为关键帧图像；

步骤S42，基于所述关键帧图像、关键帧图像对应的深度图，构造重投影误差函数；

步骤S43，采用非线性优化框架g2o，通过Levenberg-Marquardt方法迭代直至所述重投影误差函数值小于设定值，得到全局最优相机位姿序列。

本发明的另一方面，提出了一种基于语义SLAM的动态环境语义地图构建方法，该方法包括：

步骤D10，依次基于上述的基于语义SLAM的动态环境相机位姿估计方法步骤S20-步骤S40获取的关键帧图像，获取实例分割结果中关键帧图像对应的实例分割结果；所述实例分割结果包括图像帧的语义mask以及各物体的物体类别、位置框；

步骤D20，依次将关键帧插入语义建图的待处理帧队列，并基于该关键帧对应的实例分割结果，通过模型管理器更新模型列表，获得跟踪成功模型列表；

步骤D30，基于所述待处理帧队列的队头帧图像、实例分割结果、跟踪成功模型列表，更新静态背景模型和前景物体模型；

步骤D40，将所述跟踪成功模型列表中每一个当前跟踪成功的前景模型，根据对应的相机位姿，通过转移矩阵变换到世界坐标系后叠加到静态背景模型上，完成动态环境语义地图构建。

在一些优选的实施例中，步骤D20包括：

步骤D21，传入当前所处理关键帧对应的实例分割结果，对语义地图中的模型进行跟踪；

步骤D22，对于检测到的一个目标，判断是否存在与其同类别的跟踪中的模型，若不存在，则为此目标新建一个模型，并插入所述跟踪模型列表；否则，对于所有同类别且未被跟踪到的模型，计算当前目标的语义mask与该模型在最近更新一帧上的语义mask的IoU，得到IoU最大值和对应模型，并跳转步骤D23；

步骤D23，基于该目标类别对应的先验权重，获取跟踪阈值，并判断所述IoU最大值是否大于所述跟踪阈值，若不大于，则为此目标新建一个模型，插入跟踪模型列表；否则，使用此目标更新对应模型，并标记此模型在当前帧被成功跟踪到，跳转步骤D24；

步骤D24：遍历当前跟踪模型列表，更新每个模型连续未跟踪到的帧数，并基于该目标类别对应的先验权重，动态确定跟踪失败的连续帧数阈值；

步骤D25，遍历当前跟踪模型列表，若一模型连续未跟踪到的帧数大于所述跟踪失败的连续帧数阈值，则判定此模型在当前视角下跟踪丢失，从模型列表中移除，获得跟踪成功模型列表。

本发明的第三方面，提出了一种存储装置，其中存储有多条程序，所述程序适于由处理器加载并执行以实现上述的基于语义SLAM的动态环境相机位姿估计方法以及基于语义SLAM的动态环境语义地图构建方法。

本发明的第四方面，提出了一种处理装置，包括处理器、存储装置；所述处理器，适于执行各条程序；所述存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的基于语义SLAM的动态环境相机位姿估计方法以及基于语义SLAM的动态环境语义地图构建方法。

本发明的有益效果：

(1)本发明基于语义SLAM的动态环境相机位姿估计方法，针对动态场景，提出了基于语义类别和极线约束的位姿估计改进方法，结合语义类别自适应生成运动判定阈值，通过极线约束关系检测处于运动状态的目标物体，以更精确的方式滤除运动目标的特征点，克服了场景内运动物体的对相机位姿估计产生的影响，提高了动态场景下相机位姿估计精度。

(2)本发明针对动态场景，提出了基于语义SLAM的动态环境语义地图构建方法，将二维帧与三维点云进行语义关联，使得三维点云地图含有语义标签。同时对目标前景和非目标背景分别建模，跟踪动态目标前景，建立前景-背景独立的物体级语义地图，对于不同类别目标的运动状态，自适应生成不同的跟踪阈值，减小了因动态前景物体非刚体形变和局部运动而在地图中产生的局部重影和边缘模糊，并提高了静态前景物体在局部遮挡、视野缺失情况下的跟踪精度，提高了动态建图的稳定性和鲁棒性，增强了地图对于环境的表达能力。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1是本发明基于语义SLAM的动态环境相机位姿估计方法的流程示意图；

图2是本发明基于语义SLAM的动态环境相机位姿估计方法的动态目标判别和动态点剔除部分流程图；

图3是本发明基于语义SLAM的动态环境语义地图构建方法的流程示意图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

本发明的一种基于语义SLAM的动态环境相机位姿估计及语义地图构建方法，相机位姿估计方法包括：

语义地图构建方法包括：

为了更清晰地对本发明基于语义SLAM的动态环境相机位姿估计及语义地图构建方法进行说明，下面结合图1对本发明方法实施例中各步骤展开详述。

本发明一种实施例的基于语义SLAM的动态环境相机位姿估计方法，包括步骤S10-步骤S40，各步骤详细描述如下：

步骤S10，通过RGB-D相机获取设定帧数的室内环境彩色图像、深度图像，并基于时间戳匹配图像，获得彩色图像序列、深度图像序列。

本发明一个实施例中，通过装备了RGB-D传感器的相机连续不断地拍摄室内环境，获得N帧彩色图像和N帧深度图像，并通过图像中的时间戳将同一时刻的彩色图像和深度图像进行匹配，得到彩色图像序列C₁,C₂,…,C_i,…,C_N和深度图像序列D₁,D₂,…,D_i,…,D_N。

步骤S20，分别提取彩色图像序列中每一帧的ORB特征点，获得第一特征点集；通过深度神经网络Mask R-CNN对所述彩色图像序列逐帧进行实例分割，获得各帧彩色图像的实例分割结果；所述实例分割结果包括彩色图像的语义mask以及各物体的物体类别、位置框。

实例分割结果还包括置信度，置信度用于进行实例分割部分的检测结果的筛选，将置信度低于设定阈值的物体检测结果弃用。

本发明一个实施例中，对深度神经网络Mask R-CNN采用数据集Microsoft COCO进行训练，训练标签共80种，涵盖室内场景下常见物体，获得可以同时对输入图像进行目标检测和语义分割的Mask R-CNN模型。

对于每一个标签所表示的物体类别，根据其在室内场景下处于运动状态的可能性，预先设定先验权重w_i∈[0,1]，若此权重为0，则表示此类别物体在当前场景下一定为静态，后续不对此类别物体进行运动状态判断。w_i越大表示此类别物体处于运动状态的可能性越大，在系统运行过程中容易被判断为动态物体。

将N帧彩色图像依次转换为灰度图，通过训练好的Mask R-CNN模型进行实例分割。对于当前帧彩色图像C_i，得到其中J个目标的检测结果集合和语义mask，目标检测结果集合表示为

r_j＝{c_j，b_j，s_j}，c_j、b_j、s_j分别表示目标obj_j的类别、位置框、置信度，语义mask表示为M_i。将s_j小于设定阈值的目标从

中移除，即弃用置信度低于设定阈值的物体检测结果。

对当前帧彩色图像C_i，通过提取其ORB特征，获得特征点集f_i。

步骤S30，基于各物体的先验运动权重、基础运动判定阈值，分别获取各类别物体的运动判定阈值，结合所述彩色图像的语义mask以及各物体的物体类别、位置框，获取处于运动状态的目标物体，并剔除对应的第一特征点集中目标物体的特征点，获得第二特征点集，如图2所示，为本发明基于语义SLAM的动态环境相机位姿估计方法的动态目标判别和动态点剔除部分流程图，具体包括：

步骤S31，基于各物体的先验运动权重、基础运动判定阈值，分别获取各类别物体的运动判定阈值；通过L-K光流法对当前帧与前一帧进行光流跟踪，获得当前帧匹配点集、前一帧匹配点集。

根据目标obj_j的类别c_j，得到其先验权重

若

为0，则此目标不会处于运动状态，不进行后续运动判定；若

大于0，则计算此类别物体的运动判定阈值，如式(1)所示：

其中，TMoving₀为基础运动判定阈值。

对于当前帧C_i和前一帧C_i-1，使用L-K光流法进行光流跟踪，得到匹配点集P_i和P_i-1。

步骤S32，计算所述当前帧匹配点集、前一帧匹配点集之间的基础矩阵，并分别结合各物体的运动判定阈值通过极线约束法判断当前帧中的每一个物体是否处于运动状态，获得运动目标。

步骤S321，计算所述当前帧匹配点集、前一帧匹配点集之间的基础矩阵；对于当前帧中任一物体，若其先验运动权重为0，则该物体为非运动目标；否则跳转步骤S322。

根据匹配点集P_i和P_i-1，计算C_i和C_i-1之间的基础矩阵，用F表示。对于目标obj_j，判断其先验权重

是否为0，若

为0，则此目标不会处于运动状态，为非运动目标；若

大于0，则通过下面的步骤进一步判断。

步骤S322，判断当前帧匹配点集中位于所述物体对应的位置框内的每一个匹配点在语义mask的类别与物体类别是否一致，并保留类别一致的匹配点作为当前目标前景点。

目标位置框b_j为矩形，包含目标前景与非目标背景，对于匹配点集P_i处于位置框范围内的每个点，检查其在语义mask M_j中对应坐标的类别与此目标类别c_j是否一致，保留类别一致的点作为目标前景点，进行进一步的动态点判定。

步骤S323，以当前目标前景点中每一个点与其在前一帧图像中对应的点作为匹配点对，结合所述基础矩阵，计算当前目标前景点中每一个匹配点在当前帧中的极线，并计算该匹配点到极线的距离。

对于第k个目标前景点

与其在前一帧C_i-1的匹配点构成匹配点对，如式(2)所示：

其中，

分别代表第i帧图像中第k个前景点

和其在第i-1帧图像中对应的匹配点

在图像坐标系下的齐次坐标。

所对应的极线L，如式(3)所示：

其中，L代表

代表第i帧图像中第k个前景点

在第i-1帧图像中对应的匹配点，

为

在第i-1帧图像上的齐次坐标，

代表以向量形式表示的极线的三个分量。

若三维点是静态的，则其在连续两帧上的投影满足极线约束，如式(4)所示：

其中，

代表第i帧图像中第k个目标前景点，T代表矩阵转置；L代表极线。

当三维点处于运动状态时，极线约束不再满足，除去噪声造成的影响，

到极线的距离可一定程度上反映运动的剧烈程度。

到L的归一化极线距离d，其计算方法如式(5)所示：

其中，d代表

到其对应极线的距离，

代表第i帧图像中第k个前景点，

代表

步骤S324，判断所述距离是否大于该点所属目标物体的运动判定阈值，若是，则该匹配点为当前目标物体动态匹配点。

若d大于此目标的运动判定阈值

则认为此目标前景点对应的三维点处于运动状态，将此匹配点对标记为运动匹配点对。

统计目标位置框b_j内运动匹配点的总数N_moving和目标前景点总数N_obj，并计算运动点占比，如式(6)所示：

若η_moving大于设定阈值，则认为此目标处于运动状态，为运动目标。

对于当前帧提取的所有尺度下的ORB特征点集合f_i，判断其是否处于运动目标的位置框内以及在M_j中对应坐标的类别与此目标类别c_j是否一致，若均为是，则此特征点属于运动物体，若令其参与相机位姿估计，会引入误差，因此，将其从f_i中剔除。剔除所有属于运动物体的特征点，得到新的特征点集f′_i。

步骤S41，将所述各帧图像对应的第二特征点集中当前帧与前一帧的特征点进行特征匹配，通过ICP方法获取相机的初始位姿并更新当前相机位姿序列，将满足设定条件的图像帧作为关键帧图像。

将经过动态点滤除后的特征点集f′_i与f′_i-1进行特征匹配，通过ICP方法估计相机的初位姿ξ_i，并更新相机位姿序列ξ。

关键帧图像可以参照ORB-SLAM2中的方法进行提取，本发明在此不进行详述。

步骤S42，基于所述关键帧图像、关键帧图像对应的深度图，构造重投影误差函数。

根据相机位姿序列ξ和深度图D_i，构造重投影误差函数。

使用非线性优化框架g2o，通过Levenberg-Marquardt方法迭代最小化重投影误差，求得全局最优相机位姿序列。

本发明第二实施例的基于语义SLAM的动态环境语义地图构建方法，包括步骤D10-D40，如图3所示，各步骤详细描述如下：

根据相机位姿估计结果和神经网络实例分割结果，将二维语义mask与三维点云进行语义关联，使得三维点云地图含有语义标签。同时对目标前景和非目标背景分别建模，跟踪动态目标前景，建立前景-背景独立的物体级语义地图。

语义地图构建过程中，所建地图包括一个静态背景模型和一系列前景目标模型：静态背景模型为全局唯一的背景实例，表示为L₀＝{pc₀}，pc₀为背景模型的点云集合，由非物体目标部分的三维点组成；前景目标模型为各个物体目标的模型，表示为L_i＝{cls_i,tra_i,pc_i}，其中i为此模型在模型列表中的索引，cls_i为此模型的类别标签，tra_i为此模型是否被成功跟踪的标记，pc_i为此模型的点云集合，由此物体部分的三维点组成。在本发明方法中通过模型管理器进行追踪，并构造模型列表进行维护。

语义地图构建模块在单独线程上运行，仅使用关键帧进行建图。创建待处理关键帧队列Q_keyfframe，并将此帧彩色图C_i、深度图D_i、实例分割结果

以及M_i共同送入Q_keyfframe队尾，语义地图构建线程中，从Q_keyfframe队首依次取出，生成语义地图。

步骤D10，依次基于上述的基于语义SLAM的动态环境相机位姿估计方法步骤S20-步骤S40获取的关键帧图像，获取实例分割结果中关键帧图像对应的实例分割结果；所述实例分割结果包括图像帧的语义mask以及各物体的物体类别、位置框。

步骤D20，依次将关键帧插入语义建图的待处理帧队列，并基于该关键帧对应的实例分割结果，通过模型管理器更新模型列表，获得跟踪成功模型列表。

步骤D21，传入当前所处理关键帧对应的实例分割结果，对语义地图中的模型进行跟踪。

传入当前所处理帧对应的的各物体的物体类别、位置框，当前所处理帧的语义mask，对语义地图中的模型进行跟踪。

步骤D22，对于检测到的一个目标，判断是否存在与其同类别的跟踪中的模型，若不存在，则为此目标新建一个模型，并插入所述跟踪模型列表；否则，对于所有同类别且未被跟踪到的模型，计算当前目标的语义mask与该模型在最近更新一帧上的语义mask的IoU，得到IoU最大值和对应模型，并跳转步骤D23。

使用模型管理器，对于目标检测和实例分割结果

中的每一个目标检测结果r_j，检查模型列表是否存在一模型，满足c_j＝cls_i，其中c_j和cls_i分别表示第j个目标和模型L_i的类别。若不存在，则为此目标新建一模型实例，插入模型列表。

若存在相同类别的模型，则对其进行跟踪。对于动态前景目标，在发生如人挥手等非刚体形变的运动时，增量式更新前景目标模型会导致形变局部产生重影，因此需要更严格的跟踪判定条件，从而减少因长周期更新动态目标模型而导致整个地图动态前景目标模型的边缘模糊、局部重影等问题；而对于静态前景目标，如显示器等类别，往往不会发生运动和形变，因此可以设置较宽松的跟踪判定条件，以保证目标在局部遮挡、视野缺失等条件下依然能被跟踪到。

步骤D23，基于该目标类别对应的先验权重，获取跟踪阈值，并判断所述IoU最大值是否大于所述跟踪阈值，若不大于，则为此目标新建一个模型，插入跟踪模型列表；否则，使用此目标更新对应模型，并标记此模型在当前帧被成功跟踪到，跳转步骤D24。

按照目标预测类别，本发明方法根据前景目标的运动状态将前景目标分成了静态前景目标和动态前景目标，对于一个模型L_i，若

则认为L_i是静态前景目标，否则认为L_i是动态前景目标。

对动态前景目标和静态前景目标分别设置了不同的跟踪阈值TT_moving、TT_static和丢失阈值TL_moving、TL_static，分别表示跟踪成功的最小IoU和跟踪失败的连续丢失帧数，本发明一个实施例中，TT_moving、TT_static、TL_moving、TL_static分别设置为0.99、0.5、1、5。

对于L_model中每个与当前目标r_j同类别且未被跟踪到的模型L_i，计算r_j语义mask与M_i在最近更新一帧关键帧上的语义mask的IoU，得到所有同类型模型中IoU最大值IoU_max和对应模型L_max。

基于该目标类别对应的先验权重，动态确定L_max跟踪阈值，若IoU_max大于此类别对应跟踪阈值，则标记此模型被成功跟踪到，反之，为此目标新建一模型实例并插入模型列表。

步骤D24：遍历当前跟踪模型列表，更新每个模型连续未跟踪到的帧数，并基于该目标类别对应的先验权重，动态确定跟踪失败的连续帧数阈值。

遍历当前跟踪模型列表，对于每个模型L_i，通过步骤D30方法增量式更新前景模型，其中，目标点选取方法如式(7)所示：

其中，cls_i为所更新模型的类别，b_x、b_width、b_y、b_height为所更新模型在当前帧投影的目标框边界值。

为对应三维点按此模型类别赋予预先设定的色彩分量，在地图中使用不同颜色表示各物体模型的不同类别。

步骤D30，基于所述待处理帧队列的队头帧图像、实例分割结果、跟踪成功模型列表，更新静态背景模型和前景物体模型。

从Q_keyfframe队首取出当前待处理的彩色图C、深度图D、实例分割结果

以及M_i，更新地图背景模型部分pc₀，其具体过程为：

步骤D31，初始化地图背景模型增量

步骤D32，在深度图D中，以一定步长间隔step取点，本发明一个实施例中，设定step＝3，所取点p＝[u,v]^T需满足式(8)：

即所取点非前景目标，且深度在一定范围内。上式中，d_min、d_max为预设深度阈值，本发明一个实施例中设定为0.01和10，d＝D(u,v)，M(u,v)＝0表示语义mask中此点类别为背景。

步骤D33，设所取点对应三维点为P＝{x,y,z,b,g,r}，前三维分量表示P在相机坐标系下的坐标，后三维分量表示P的BGR颜色分量。根据相机内参，计算相机坐标系下三维点坐标，如式(9)所示：

其中，cx、cy、fx、fy为相机内参数，cx、cy表示主点坐标，fx、fy表示焦距。

步骤D34，将彩色图三通道数值赋值给P的b、g、r分量，设定对应三维点颜色即为彩色图对应点颜色。

步骤D35，根据当前所处理关键帧的相机位姿，得到世界坐标系到相机坐标系的转换矩阵T_cw，将P变换至世界坐标系下，得到P′，如式(10)所示：

其中，P_0:2、P′_0:2分别代表P、P′的前三维分量，即在世界坐标系下的坐标值，P_3:5、P_′3:5分别代表P、P′的后三维分量，即三维点的色彩BGR值。

步骤D36，将此待处理关键帧所得三维点P′依次添加至地图背景模型增量

步骤D37，用

与pc₀相加，逐帧(关键帧)迭代更新模型背景的点云集合。

本发明相机位姿估计与语义地图构建为并列进行的两个线程，相机位姿估计线程获取一张关键帧图像，即将其插入语义建图线程的待处理帧队列，同时基于待处理帧队列的队头帧图像、队头帧图像对应的语义mask以及各物体的物体类别、位置框进行动态环境的语义地图构建，大大提高了效率。

需要说明的是，上述实施例提供的基于语义SLAM的动态环境相机位姿估计及语义地图构建方法，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的步骤来完成，即将本发明实施例中的步骤再分解或者组合，例如，上述实施例的步骤可以合并为一个步骤，也可以进一步拆分成多个子步骤，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的步骤的名称，仅仅是为了区分各个步骤，不视为对本发明的不当限定。

本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适于由处理器加载并执行以实现上述的基于语义SLAM的动态环境相机位姿估计方法以及基于语义SLAM的动态环境语义地图构建方法。

本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的基于语义SLAM的动态环境相机位姿估计方法以及基于语义SLAM的动态环境语义地图构建方法。

所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。