CN112233221B - 基于即时定位与地图构建的三维地图重建系统及方法 - Google Patents

Abstract

本发明实施例提供的基于即时定位与地图构建的三维地图重建系统及方法，其中系统包括：处于同一局域网内的RGBD相机、开发板及服务器；RGBD相机用于实时采集周围环境的数据；开发板用于获取相机的定位结果；获取相机位姿；剔除目标图像中深度图的目标物特征点，得到剔除后图像；按照定位结果，从目标图像的RGB彩色图中提取三维地图点；对目标图像的各图像帧，根据IMU数据、剔除后图像、相机位姿及三维地图点进行三维地图重建，得到各图像帧的三维地图，将各图像帧三维地图传输至服务器；服务器用于接收最新的各图像帧的三维地图，实时更新已有的各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示最终三维地图。

Description

基于即时定位与地图构建的三维地图重建系统及方法

技术领域

本发明涉及机器视觉技术领域，特别是涉及基于即时定位与地图构建的三维地图重建系统及方法。

背景技术

目前，即时定位与地图创建(Simultaneous Localization and Mapping，简称SLAM)技术，依靠传感器可实现机器的自主定位、建图、路径规划等功能。

随着计算机视觉的迅速发展，视觉SLAM因为信息量大，适用范围广等优点受到广泛关注，是未来研究的一个主流方向。当下大部分SLAM系统是一体化部署方式。比如，在无人机上部署一体化的SLAM系统，可以实现自主定位。再比如，在车辆上部署一体化的SLAM系统，可以实现建图及路径规划。

但是，一体化的SLAM系统，在特定场景下无法实现便携性和移动化的要求。

发明内容

本发明实施例的目的在于提供基于即时定位与地图构建的三维地图重建系统及方法，用以实现三维重建地图系统在特定场景下的便携性和移动化。具体技术方案如下：

第一方面，本发明实施例提供了基于即时定位与地图构建的三维地图重建系统，包括：

处于同一局域网内的相机、开发板及服务器；其中，所述相机包括：可穿戴的红绿蓝深度RGBD相机，所述RGBD相机为多传感器融合相机，所述RGBD相机包含惯性测量单元；

所述RGBD相机，用于实时采集周围环境的数据，所述数据包括：目标图像及惯性测量单元IMU数据，将所述数据通过数据线传输至所述开发板上，所述数据线连接在所述RGBD相机与所述开发板的端口之间；

所述开发板，用于获取相机的定位结果；获取相机位姿；剔除所述目标图像中深度图的目标物特征点，得到剔除后图像；按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，通过机器人软件平台ROS通信，将所述各图像帧三维地图传输至服务器；

所述服务器，用于接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图。

进一步的，所述各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和原有颜色的云点；

所述服务器，用于接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图，包括：

所述服务器，用于对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域；所述不同安全等级的区域包括：安全等级最高对应的安全区域及安全等级最低对应的危险区域；变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；

所述服务器，用于实时显示所述最终三维地图，包括：

通过所述服务器上的人机交互界面，实时显示所有图像帧的最终三维地图。

进一步的，所述服务器，用于对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域，包括：

所述服务器，用于利用所述数据得到的彩色缩略图，调用安全等级识别模型；将所述数据得到的彩色缩略图作为所述安全等级识别模型的输入，输出所述周围环境中不同区域的安全等级；其中，所述安全等级识别模型是基于危险场景的训练样本集训练得到的，所述训练样本集包括：已标注不同区域的安全等级的图像；

所述服务器，用于变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

使用用于标注危险区域的预设颜色，覆盖所述危险区域中的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图。

进一步的，所述服务器，用于通过如下步骤，得到所述安全等级识别模型：

获取已标注不同区域的安全等级的图像，所述图像中包含危险区域的特征；

将所述已标注不同区域的安全等级的图像，作为安全等级识别模型的训练样本集；

将所述安全等级识别模型的训练样本集作为待训练安全等级识别模型的输入，利用所述待训练安全等级识别模型进行训练，得到所述安全等级识别模型。

进一步的，所述开发板，用于剔除所述目标图像中深度图的目标物特征点，得到剔除后图像，包括：

所述开发板，用于对所述RGB彩色图，通过目标检测算法，检测所述周围环境中的目标物；在所述深度图中标注所述目标物的像素位置；在所述深度图中，利用所述目标物和背景深度之间的差异，剔除所述深度图中目标物的深度信息，得到剔除后图像；其中，剔除所述背景深度为除所述目标物所在的像素位置以外的其他区域；

所述开发板，用于按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，包括：

所述开发板，用于按照所述定位结果，从所述RGB彩色图中提取三维地图点颜色信息；针对所述目标图像的各图像帧，将所述三维地图点颜色信息及从所述剔除后图像提取深度信息，将所述剔除后图像的像素点依据相机内参矩阵，映射到以相机光心为原点的坐标系中，得到该图像帧的点云；通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标；通过各图像帧的像素点坐标及像素点颜色，得到各图像帧的三维地图。

第二方面，本发明实施例提供了基于即时定位与地图构建的三维地图重建方法，包括：

可穿戴的红绿蓝深度RGBD相机实时采集周围环境的数据，所述数据包括：目标图像及惯性测量单元IMU数据，所述RGBD相机为多传感器融合相机，所述RGBD相机包含惯性测量单元；

获取相机的定位结果；获取相机位姿；剔除所述目标图像中深度图的目标物特征点，得到剔除后图像；按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图；

接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图；

其中，所述各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和原有颜色的云点；

所述接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域；所述不同安全等级的区域包括：安全等级最高对应的安全区域及安全等级最低对应的危险区域；变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；

所述实时显示所述最终三维地图，包括：

通过人机交互界面，实时显示所有图像帧的最终三维地图；

其中，所述对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域，包括：

利用所述数据得到的彩色缩略图，调用安全等级识别模型；将所述数据得到的彩色缩略图作为所述安全等级识别模型的输入，输出所述周围环境中不同区域的安全等级；其中，所述安全等级识别模型是基于危险场景的训练样本集训练得到的，所述训练样本集包括：已标注不同区域的安全等级的图像；

所述变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

使用用于标注危险区域的预设颜色，覆盖所述危险区域中的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图。

进一步的，通过如下步骤，得到所述安全等级识别模型：

获取已标注不同区域的安全等级的图像，所述图像中包含危险区域的特征；

将所述已标注不同区域的安全等级的图像，作为安全等级识别模型的训练样本集；

将所述安全等级识别模型的训练样本集作为待训练安全等级识别模型的输入，利用所述待训练安全等级识别模型进行训练，得到所述安全等级识别模型。

进一步的，所述剔除所述目标图像中深度图的目标物特征点，得到剔除后图像，包括：

对所述RGB彩色图，通过目标检测算法，检测所述周围环境中的目标物；在所述深度图中标注所述目标物的像素位置；在所述深度图中，利用所述目标物和背景深度之间的差异，剔除所述深度图中目标物的深度信息，得到剔除后图像；其中，剔除所述背景深度为除所述目标物所在的像素位置以外的其他区域；

按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述惯性测量单元数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，包括：

按照所述定位结果，从所述RGB彩色图中提取三维地图点颜色信息；针对所述目标图像的各图像帧，将所述三维地图点颜色信息及从所述剔除后图像提取深度信息，将所述剔除后图像的像素点依据相机内参矩阵，映射到以相机光心为原点的坐标系中，得到该图像帧的点云；通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标；通过各图像帧的像素点坐标及像素点颜色，得到各图像帧的三维地图。

本发明实施例有益效果：

本发明实施例提供的基于即时定位与地图构建的三维地图重建系统及方法，相机、开发板与服务器分别属于不同的单独设备，各独立设备之间配合使用。虽然相机与服务器分离，开发板也与服务器分离，但相机、开发板及服务器处于同一局域网中，可以通过可穿戴相机的RGBD相机实时采集周围环境的目标图像，然后通过开发板对目标图像中深度图的目标物特征点，得到剔除后图像；按照定位结果，从目标图像的RGB彩色图中提取三维地图点，再经过服务器实时显示最终三维地图。对于服务器而言，实现了服务器的远程监控拍摄范围内周围环境，对于用户而言，也便于用户携带相机和开发板。

当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的基于即时定位与地图构建的三维地图重建系统的第一示意图；

图2为本发明实施例提供的基于即时定位与地图构建的三维地图重建系统的第二示意图；

图3为本发明实施例的RGB彩色图；

图4为本发明实施例的目标物检测后的RGB彩色图中目标物的矩形框表示示意图；

图5为本发明实施例的图像数据中深度图的目标物；

图6为本发明实施例的剔除后图像的第一示意图；

图7为本发明实施例的剔除后图像的第二示意图；

图8为本发明实施例的坐标系变换示意图；

图9为本发明实施例的剔除动态物体之后的所有图像帧的最终三维地图效果；

图10为本发明实施例的基于即时定位与地图构建的三维地图重建方法的第一流程示意图；

图11为本发明实施例的基于即时定位与地图构建的三维地图重建方法的第二流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为了方便理解本发明实施例，做如下介绍。

目前，随着计算机视觉的迅速发展，视觉SLAM因为信息量大，适用范围广等优点受到广泛关注，是未来研究的一个主流方向。当下大部分SLAM系统是一体化部署方式。比如，在无人机上部署一体化的SLAM系统，可以实现自主定位。再比如，在车辆上部署一体化的SLAM系统，可以实现建图及路径规划。

但是，随着移动化互联网的普及，SLAM系统也在特定场景下有便携性和移动化的要求。此处的便携性和移动化，可以采用便携手持设备，满足便携性的要求。而，用户使用便携手持设备可以在特定场景下移动，从而满足移动化的要求。

基于此，本发明实施例基于即时定位与地图构建的三维地图重建系统(以下简称系统)中，相机和开发板均可作为便携手持设备，而相机和开发板均与服务器处于同一局域网内，相机采集完目标图像，经开发板重建各图像帧的三维地图以后，在同一局域网内开发板完成和服务器的通信。这样本发明实施例的系统可以满足用户使用便携手持设备进行重建，并远程监控特定环境需求。

本发明实施例的系统可以应用的上述特定场景，比如设备巡检、犯人押解等，本发明实施例的系统可以应用的领域，参见图1所示，工业领域、医疗领域及建筑领域。具体可以应用于机器人、无人机、无人驾驶、增强现实(Augmented Reality，简称AR)、虚拟现实(Virtual Reality，简称VR)等方面。

下面从应用层、服务层、网络层及终端层，结合本发明实施例基于即时定位与地图构建的三维地图重建系统及方法进行简单说明。参见图1所示，相机11、开发板12及服务器13部署在终端层，在服务层实现本发明实施例的基于即时定位与地图构建的三维地图重建方法的业务逻辑。相机11可以获得红绿蓝深度(Red Green Blue Depth，简称RGBD)图像及惯性测量单元(Inertial Measurement Unit，简称IMU)数据等内容；开发板12可以实现提取特征点、计算位姿、定位及采集云点等处理；服务器13可以实现点云拼接、场景分类、点云染色及地图展示。为了布局清楚，关于相机、服务器及开发板详细内容在后文中予以介绍。

下面继续对本发明实施例提供的基于即时定位与地图构建的三维地图重建系统进行详细介绍。

如图1和图2所示，本发明实施例所提供的一种基于即时定位与地图构建的三维地图重建系统，包括：

处于同一局域网内的相机11、开发板12及服务器13；其中，所述相机包括：可穿戴的红绿蓝深度RGBD相机，所述RGBD相机为多传感器融合相机，所述RGBD相机包含惯性测量单元。

所述RGBD相机，用于实时采集周围环境的数据，所述数据包括：目标图像及惯性测量单元数据，将所述数据通过数据线传输至所述开发板上，所述数据线连接在所述RGBD相机与所述开发板的端口之间。

其中，上述相机选用多传感器融合相机，能够获取RGB图、深度图和IMU数据。这样直接获取深度图避免了单目相机尺度漂移以及双目相机使用大量资源计算深度信息。并且，IMU数据相较于只有RGBD图的SLAM的系统定位更加准确，鲁棒性更强。

而，上述目标图像是由图像帧组成。目标图像是从所述RGBD相机采集的图像中选择的图像。并且，周围环境可以是待监控的RGBD相机拍摄范围内的环境。

所述开发板，用于获取相机的定位结果；从所述目标图像中，获取相机位姿；剔除所述目标图像中深度图的目标物特征点，得到剔除后图像；按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述目标图像中的惯性测量单元(Inertial Measurement Unit，简称IMU)数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，通过机器人软件平台ROS通信，将所述各图像帧三维地图传输至服务器。这样可以在服务器上展示三维地图。

需要说明的是，考虑到选用开发板的性能，将场景分类的功能部署在服务器端。为了实时更新地图场景，将部分点云重建的结果实时地发送到服务器端。三维地图的安全等级分类需要场景图片，为了节省带宽，对图像帧对应的图片筛选压缩后传输。为了布局清楚，关于所述三维地图的安全等级分类过程在后文中予以介绍。

另外，上述开发板，用于获取相机的定位结果，包括：

使用相机中的图像数据，惯性数据，深度数据，根据视觉惯性系统(visual-inertial system，简称VINS)进行计算得到定位结果。

需要说明的是，上述目标物为需要剔除的物体，其可以为动态物，也可以为静态的预设物，动态物比如人，动物，汽车等，静态物比如临时围挡物，在此不一一举例。

开发板，用于获取相机的定位结果，是基于VINS实现的，而VINS作为一种开源项目，在VINS视觉惯性系统的基础上进行开发，通过使用VINS得到相机的定位结果，加入三维地图重建。这样通过VINS-RGBD相机结合，得到一个基于开源系统VINS-Mono的一个融合RGBD相机的实时三维重建地图系统。VINS-Mono融合了单目相机和IMU，具有较好的鲁棒性，能够在无人机和较大场景下都能完成较为准确的定位导航。VINS-RGBD相机在VINS-Mono的基础上融合了RGBD相机，能够准确的获得深度数据。

同时，RGBD相机通过红外结构光来测量像素距离可以直接获取深度图，避免了大量的计算，节省了计算资源。在采集到数据中包括的IMU数据可以进行预积分，得到当前时刻的位姿、速度、旋转角，同时计算在服务器优化中将要用到的相邻帧间的预积分增量。并且，在采集到数据中包括的IMU数据，可以对定位的轨迹进行进一步优化。将RGBD相机和IMU数据，进一步结合既直接获取了准确的深度信息，又可以通过IMU数据进一步精准定位。

所述服务器，用于接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图。

其中，最新的所述各图像帧的三维地图是指距离当前时刻最近的图像帧的三维地图。

本发明实施例中，相机、开发板与服务器分别属于不同的单独设备，各独立设备之间配合使用。虽然相机与服务器分离，开发板也与服务器分离，但相机、开发板及服务器处于同一局域网中，可以通过可穿戴相机的RGBD相机实时采集周围环境的目标图像，然后通过开发板对目标图像中深度图的目标物特征点，得到剔除后图像；按照定位结果，从目标图像的RGB彩色图中提取三维地图点，再经过服务器实时显示最终三维地图。对于服务器而言，实现了服务器的远程监控拍摄范围内周围环境，对于用户而言，也便于用户携带相机和开发板。

进一步的，在一种可能的实现方式中，上述开发板，用于获取相机位姿，包括：

所述开发板，用于获取由不同时刻视觉里程计测量的估计相机位姿，作为相机位姿。

其中，视觉里程计(Visual Odometry，简称VO)：视觉里程计的任务是估算相邻图像帧间相机的运动，以及各图像帧的三维地图。最简单的视觉里程计的任务是两张图像帧之间的运动关系。VO被称为里程计是因为它只计算相邻时刻的运动，而和再往前的过去信息没有关联。相邻时刻运动串联起来，构成了用户的运动轨迹，从而解决了定位问题。另一方面，根据每一时刻的相机位置，计算出各像素对应的空间点的位置，从而得到地图。

或者，在另一种可能的实现方式中，所述开发板，用于获取由不同时刻视觉里程计测量的估计相机位姿，对估计相机位姿进行回环检测以及非线性优化，得到优化相机位姿，作为相机位姿。其中，估计相机位姿中“估计”和优化相机位姿中“优化”是为了区分通过不同步骤得到不同的相机位姿。而，估计相机位姿是指由SLAM中不同时刻视觉里程计测量的相机位姿，优化相机位姿是指对估计相机位姿进行回环检测以及非线性优化，得到的相机位姿。这样可以通过回环检测以及非线性对估计相机位姿的处理，获取精准的相机位姿。

其中，上述回环检测(Loop Closure Detection，简称LCD)判断用户是否到达过先前的位置即回环检测的信息。如果检测到回环，它会把目标图像中相似的目标图像帧提供给开发板进行处理，可以显著地减小累积误差。回环检测实质上是一种检测目标图像相似性的算法。本发明实施例系统中回环检测可以为词袋模型(Bag-of-Words，简称BoW)。其中，词袋模型的执行过程包括：将目标图像中的视觉特征聚类，然后建立词典，进而寻找每个图中含有哪些关键词，即“单词”(word)。其中，所述目标图像中的视觉特征包括：SIFT及SURF。SIFT即为尺度不变特征转换，是用于图像处理领域的一种描述子。SURF是SIFT的一种优化加速。当然，本发明实施例中的回环检测，也可以构建一个分类问题，训练分类器进行分类。通过回环检测目标图像中各帧图像间的相似度，从而判断用户是否到达过先前的位置，以完成估计相机位姿。

同时，上述非线性优化(Optimization)是指，获取由SLAM中不同时刻视觉里程计测量的估计相机位姿，以及回环检测的信息；对相机位姿以及回环检测的信息进行优化，得到一致的轨迹和地图。而此处一致的轨迹和地图，可以从服务器实时显示，从而呈现出全局一致的轨迹和地图。

由于接在VO之后，又称为后端(Back End，简称BE)。后端优化主要是处理系统在三维重建地图中SLAM过程中噪声的问题。任何传感器都有噪声，所以除了要处理“如何从图像中估计出相机运动”即相机位姿，还要考虑相机位姿的估计，带来的噪声。VO发送目标图像及相机位姿，而后端负责整体的优化过程，它往往面对的只有数据，不必关系这些数据来自哪里。在视觉SLAM中，VO和计算视觉研究领域更为相关，比如图像的特征提取与匹配等，开发板则主要是滤波和非线性优化算法。

本发明实施例中的相机及开发板，实现系统中各设备之间的分离，满足用户便携性和远程监控的需要。通过相机进行周围环境的目标图像的采集，将三维重建和定位结果，在服务器进行实时更新实时显示，帮助用户实现对周围环境的实时监控。

为了能够区分出来周围环境的安全等级，可以通过服务器在实现场景安全等级分类后，得到变更后各图像帧三维地图，最后呈现最终三维地图。以下介绍三维地图的安全等级分类过程。在本发明实施例中，所述各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和变更的云点；

所述服务器，用于接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图，包括：

所述服务器，用于对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域；所述不同安全等级的区域包括：安全等级最高对应的安全区域及安全等级最低对应的危险区域；变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图。这样三维地图的分类通过图像帧分类实现。通过对图像帧分类，对三维点云地图添加了安全等级信息。

为了最终三维地图区分实时显示安全区域及危险区域，本发明实施例的一种可实现方式中，所述服务器，用于变更所述点云图中所述危险区域的点云变更，得到变更后各图像帧三维地图，包括：

将所述点云图中所述危险区域的点云原有颜色，变更为用于标注危险区域的预设颜色。

当然，预设颜色与所述点云图中危险区域的点云原有颜色不同，用于区分危险区域和安全区域。预设颜色可以是根据用户需求设置的，比如，预设也可以是默认预设颜色，比如红色，而点云原有颜色可以是与预设颜色不同的绿色。

所述服务器，用于实时显示所述最终三维地图，包括：

通过所述服务器上的人机交互界面，实时显示所有图像帧的最终三维地图。

本发明实施例中，相机、开发板与服务器分别属于不同的单独设备，各独立设备之间配合使用。虽然相机与服务器分离，开发板也与服务器分离，但相机、开发板及服务器处于同一局域网中，可以通过可穿戴相机的RGBD相机实时采集周围环境的目标图像，然后通过开发板对目标图像中深度图的目标物特征点，得到剔除后图像；按照定位结果，从目标图像的RGB彩色图中提取三维地图点，再经过服务器提供实时安全区域和危险区域的区别，服务器远程监控，查看最终三维地图，实时显示最终三维地图。

为了应用到更多的环境，各图像帧的三维地图的时候，开发板每次传输各图像帧的三维地图和缩略图，使用三维地图进行建图，使用缩略图进行场景安全识别，这样传输数据方式有效地减少了每次传输的数据量，相较于传输其他数据大大减少远程传输数据的时延，能够在各种网络环境下，实现远程实时监控。利用服务器计算资源对各图像帧的三维地图进一步挖掘。在服务器接收到的各图像帧的三维地图后，挖掘周围场景信息，判断周围场景安全等级，帮助服务器使用者更有效进行实时监控，危险提示信息会在地图中实时显示。对于服务器而言，不仅实现了服务器的远程监控拍摄范围内周围环境，而且，远程监控提供实时的预警信息；对于用户而言，也便于用户携带相机和开发板。

当然，上述安全等级分类可以根据用户需要分成多个安全等级，比如两级安全等级或三级安全等级。每个安全等级对应不同的区域，这些区域包括与安全等级对应的区域。安全区域用于表示开阔地带范围内没有障碍物的区域，危险区域为开阔的区域边缘的障碍物。以下以两级安全等级示例说明。

周围环境可以但不限于为广场，操场，安全等级最高对应的安全区域比如，安全区域为开阔地带范围内没有障碍物，比如，广场，操场。而，安全等级最低对应的危险区域是此开阔地带的边缘存在障碍物，比如，广场边上的树木，操场边缘的围挡。再比如，周围环境可以但不限于为室内环境，危险区域为室内的门窗区域或者大于走廊，而安全区域为室内除门窗以外的活动区域。

为了能够更加准确的区分出危险区域和安全区域，本发明实施例还提供一种可能的实现方式，所述服务器，用于对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域，包括：

所述服务器，用于利用所述数据得到的彩色缩略图，调用安全等级识别模型；将所述数据得到的彩色缩略图作为所述安全等级识别模型的输入，输出所述周围环境中不同区域的安全等级；其中，所述安全等级识别模型是基于危险场景的训练样本集训练得到的，所述训练样本集包括：已标注不同区域的安全等级的图像。

为了减少传输的数据量，RGBD相机拍摄可以直接得到目标图像中的彩色图，将此彩色图缩放尺寸，按照预设缩略图大小的规则，裁剪，得到所述彩色缩略图。

而上述所述安全等级识别模型，通过如下步骤训练得到：

获取已标注不同区域的安全等级的图像，所述图像中包含危险区域的特征；

此处危险区域的特征比如安全等级最低对应的危险区域是此开阔地带的边缘存在障碍物，比如，广场边上的树木特征，操场边缘的围挡特征。再比如，危险区域为室内的门窗区域特征或者大于走廊特征。

将所述已标注不同区域的安全等级的图像，作为安全等级识别模型的训练样本集；

将所述安全等级识别模型的训练样本集作为待训练安全等级识别模型的输入，利用所述待训练安全等级识别模型进行训练，得到所述安全等级识别模型。

待训练安全等级识别模型是还未训练的安全等级识别模型，而安全等级识别模型是已经训练得到的安全等级识别模型。

所述服务器，用于变更所述危险区域的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

使用用于标注危险区域的预设颜色，覆盖所述危险区域中的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，参见图9。

在发明实施例中，服务器在接收到点云和彩色缩略图，安全等级识别模型判断周围环境的不同区域的安全等级的类别，对点云进行重新染色，最后所有图像帧的最终三维地图。

基于上述介绍，相关技术中的SLAM方法，通过将动态内容分类为静态模型的异常值，处理一小部分动态内容。当机器处于拥挤的环境中，存在大量的动态内容，机器会将大量的动态内容，视为静态环境的一部分。这样将大量的动态都被分类为静态模型的异常值，那么机器将不能正常工作。因此，如何防止将动态内容收集在三维地图，建立三维地图，为了能够在本发明实施例的系统中，剔除动态内容，即目标物为动态物，能够完成建图，本发明实施例的一种可能的实现方式中，所述开发板，用于剔除所述目标图像中深度图的目标物特征点，得到剔除后图像，包括：

所述开发板，用于对所述RGB彩色图，通过目标检测算法，检测所述周围环境中的目标物；在所述深度图中标注所述目标物的像素位置；在所述深度图中，利用所述目标物和背景深度之间的差异，剔除所述深度图中目标物的深度信息，得到剔除后图像；其中，剔除所述背景深度为除所述目标物所在的像素位置以外的其他区域。这样动态剔除目标物，结合了目标检测和深度图，将目标检测的精度实现到了像素级。

上述目标物可以是人也可以是物，可以是任何用户需要剔除的动态物，在此不再一一举例。

由于上述得到剔除后图像主要分为两部分：(1)检测周围环境中的目标物；(2)剔除深度图中目标物的深度信息。这样可以目标物的识别可以采用目标检测算法实现，完成预测RGB图像上动态内容位置的工作。而，在确定的目标物和深度图所带的物体可见表面的几何形状信息结合后，剔除如图5所示的深度图上的目标物，得到剔除后图像如图6所示和图7所示。本发明实施例采用目标检测算法，来定位如图3所示的RGB彩色图中目标物的位置，一个目标物的位置是由包含了该目标物的矩形框表示，如图4所示。

通过目标检测算法，可以得到出现在当前RGB图像中的目标物的坐标值。但目标检测算法只能得到目标物所处的矩形区域的位置，并不能像像素级语义分割一样，显示出人体的具体形状。深度图可以直接反映物体可见表面的几何形状。在深度图中，每一个像素点的值代表的是在深度感应器的视野中，该点处物体到图像采集器的距离。因此，对于深度图来说，在目标检测算法检测出的区域中，动态物所在位置的像素值和其背景的像素值会有较大差值。本发明实施例将目标检测算法所得结果和深度图信息结合后，可以得到深度图中行人的位置及其轮廓，然后本发明实施例可以对深度图进行动态物去除的操作。但是，本发明实施例的方法偶尔会遗漏人身的一些像素点。为了降低遗漏像素点对于结果的影响，本发明实施例使用了统计滤波剔除离群点。在加入快速动态物去除后，系统在低动态场景和高动态场景下的鲁棒性都有了很大的提升。

相关技术采用Mask-RCNN进行动态目标检测和分割。显然，这种基于像素的语义分割网络耗费了服务器大量的图形处理资源，难以在开发板上实时运行。而本发明实施例中的以RGBD相机得到的深度图像，已经提供了一些语义信息。为了节省图形处理资源，目标检测算法目标检测算法可以为R-CNN，快速Fast R-CNN，快速Faster R-CNN等，这些算法主要有两个阶段(two-stage)。算法需要先产生目标候选框，也就是目标位置，然后再对候选框做分类与回归。这类算法的准确性较高，但是速度慢，耗费计算资源多。优选的，目标检测算法也可以采用单射多盒探测器(Single Shot MultiBox Detector，简称SSD)算法或目标检测算法(Ou Only Look Once，简称YOLO)。SSD在准确性和速度方面相较于YOLO都有了一定的提升。此SSD算法说明如下：

先运行一个卷积神经网络作为基础模型，比如视觉几何组(Visual GeometryGroup，简称VGG)等，对输入目标图像计算特征映射，然后在得到的特征图上运行一个3×3大小的卷积核来预测边界框和分类概率。之后对这个特征图逐渐采用stride＝2的卷积操作或者池化操作来降低特征图大小，对降低大小之后的特征图再次预测边界框和分类概率。SSD的核心设计理念可以总结为以下三点：(1)采用多尺度特征图用于检测。卷积神经网络(Convolutional Neural Networks，简称CNN)一般前面的特征图比较大，后面会逐渐降低特征图大小。SSD算法会将不同尺度的特征图都用来做检测。这样做的好处是比较大的特征图来用来检测相对较小的目标，而小的特征图负责检测大目标。(2)采用卷积进行检测。与YOLO算法在模型最后采用全连接层不同，SSD直接采用卷积操作对不同的特征图来进行提取检测结果。这样运行速度足够快，可以嵌入跟踪线程，并在删除动态物上的特征点。

所述开发板，用于按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述惯性测量单元数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，包括：

所述开发板，用于按照所述定位结果，从所述RGB彩色图中提取三维地图点颜色信息；针对所述目标图像的各图像帧，将所述三维地图点颜色信息及从所述剔除后图像提取深度信息，将所述剔除后图像的像素点依据相机内参矩阵，映射到以相机光心为原点的坐标系中，得到该图像帧的点云；通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标；通过各图像帧的像素点坐标及像素点颜色，得到各图像帧的三维地图。

其中，所述开发板，用于通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标，包括：

参见图8，为坐标系变换示意图。图8中的世界坐标系的x轴、y轴和z轴，分别为x_w、y_w和z_w，P_w为点在世界坐标系下的坐标点。而，图8个点，T为世界坐标和相机坐标的变换矩阵。

采用如此公式，确定所述世界坐标系下的该图像帧的像素点坐标：

其中，Z为从深度图上获取深度，u为像素平面像素坐标的横坐标，v为像素平面像素坐标的纵坐标，P_uv为像素平面上的像素点，K为相机内参矩阵，R为相机位姿的旋转矩阵，P_w为点在世界坐标系下的坐标点，t为相机位姿的平移向量，T为相机位姿的变换矩阵，这样依据此方程可求得任意像素点在世界坐标系下的坐标。

在本发明实施例中，将各图像帧的三维地图在开发板上实现，采用目标检测算法，减少服务器计算资源的消耗，并且各图像帧的三维地图中的包括动态物的目标物被去除掉，也不会将包括动态物的目标物收集在三维地图中，建立三维地图。这样将各图像帧中的所有像素点放在世界坐标系下计算出该像素点的位置，并带有颜色，即确定出各图像帧的三维地图。

为了实现便携性的特点，相机及开发板可以选取轻便小巧且兼具一定计算能力的设备，完成图像采集及各图像帧的三维地图的重建即可。比如，相机的相机可以但不限于是D435i相机，开发板可以但不限于是Hikey970开发板，其中，上述D435i相机在使用前，接入Linux系统运行驱动。D435i摄像头和Hikey970开发板的端口，以通用串行总线USB有线连接。为防止网络传输的带宽过小造成的传输时延过大，为了避免时延过大，将估计相机位姿的过程和各图像帧的三维地图的重建过程，放入Hikey970开发板进行计算，得到各图像帧的三维地图，其中，各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和变更的云点，此处的与各图像帧各像素点对应的点云图中的点云，相对于服务器实时显示的最终三维地图而言，也可以称为局部地图的点云。

为了提高便携性，开发板选择使用Hikey970开发板，相较于树莓派，该开发板满足便携性的同时具有更高的性能。可穿戴D435i相机相较与自带相机的传感器种类更多样，能够根据需求结合使用更多传感器，创建精度更高的地图，能够同时获取RGB彩色图、深度图和IMU数据，且同时小巧便携。

而，服务器不仅需要查看地图，还需要能够在实时显示最终三维地图，提供更多地图信息。服务器与Hikey970开发板在局域网内基于ROS实现无线通信。

本发明实施例的移动化部署在实际使用时，可以实现在远端使用可穿戴的的D435i相机完成数据的采集，和开发板完成重构各图像帧的三维地图。在服务器对各图像帧的三维地图进行实时观察，实现远程监控。并且，本发明实施例能够将周围环境中的目标物较快进行移除，保证了其建图效果。同时，通过目标检测算和目标物移除的实时性良好，运行速度较快，能够嵌入跟踪线程。

下面继续对本发明实施例提供的基于即时定位与地图构建的三维地图重建方法进行介绍。

参见图10，图10为本发明实施例提供的基于即时定位与地图构建的三维地图重建方法的第一流程示意图。本发明实施例所提供的基于即时定位与地图构建的三维地图重建方法，可以包括如下步骤：

步骤21，可穿戴的红绿蓝深度RGBD相机实时采集周围环境的数据，所述数据包括：目标图像及惯性测量单元IMU数据，所述RGBD相机为多传感器融合相机，所述RGBD相机包含惯性测量单元；

步骤22，获取相机的定位结果；获取相机位姿；剔除所述目标图像中深度图的目标物特征点，得到剔除后图像；按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图；

步骤23，接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图。

上述基于即时定位与地图构建的三维地图重建方法中的上述步骤22和上述步骤23的执行流程可以分别应用于具有显示核心(Graphics Processing Unit，简称GPU)处理功能的不同的电子设备。上述电子设备可以为计算机(Personal Computer，简称PC)、服务器，在此不做限定。

相机、不同的电子设备，各独立设备之间配合使用。虽然相机与不同的电子设备分离，但相机、不同的电子设备处于同一局域网中，可以通过可穿戴相机的RGBD相机实时采集周围环境的目标图像，然后通过一个电子设备对目标图像中深度图的目标物特征点，得到剔除后图像；按照定位结果，从目标图像的RGB彩色图中提取三维地图点，再经过服务器实时显示最终三维地图。对于服务器而言，实现了另一个电子设备的远程监控拍摄范围内周围环境，对于用户而言，也便于用户携带相机和上述一个电子设备。

参见图11，图11为本发明实施例提供的基于即时定位与地图构建的三维地图重建方法的第二流程示意图。

在一种可能的实现方式中，所述各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和原有颜色的云点；

所述步骤23进一步包括：

步骤231，对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域；所述不同安全等级的区域包括：安全等级最高对应的安全区域及安全等级最低对应的危险区域；变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；

步骤232，通过人机交互界面，实时显示所有图像帧的最终三维地图。

在一种可能的实现方式中，所述对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域，包括：

利用所述数据得到的彩色缩略图，调用安全等级识别模型；将所述数据得到的彩色缩略图作为所述安全等级识别模型的输入，输出所述周围环境中不同区域的安全等级；其中，所述安全等级识别模型是基于危险场景的训练样本集训练得到的，所述训练样本集包括：已标注不同区域的安全等级的图像；

所述变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

使用用于标注危险区域的预设颜色，覆盖所述危险区域中的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图。

在一种可能的实现方式中，通过如下步骤，得到所述安全等级识别模型：

获取已标注不同区域的安全等级的图像，所述图像中包含危险区域的特征；

将所述已标注不同区域的安全等级的图像，作为安全等级识别模型的训练样本集；

将所述安全等级识别模型的训练样本集作为待训练安全等级识别模型的输入，利用所述待训练安全等级识别模型进行训练，得到所述安全等级识别模型。

在一种可能的实现方式中，所述剔除所述目标图像中深度图的目标物特征点，得到剔除后图像，包括：

对所述RGB彩色图，通过目标检测算法，检测所述周围环境中的目标物；在所述深度图中标注所述目标物的像素位置；在所述深度图中，利用所述目标物和背景深度之间的差异，剔除所述深度图中目标物的深度信息，得到剔除后图像；其中，剔除所述背景深度为除所述目标物所在的像素位置以外的其他区域；

按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述惯性测量单元数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，包括：

按照所述定位结果，从所述RGB彩色图中提取三维地图点颜色信息；针对所述目标图像的各图像帧，将所述三维地图点颜色信息及从所述剔除后图像提取深度信息，将所述剔除后图像的像素点依据相机内参矩阵，映射到以相机光心为原点的坐标系中，得到该图像帧的点云；通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标；通过各图像帧的像素点坐标及像素点颜色，得到各图像帧的三维地图。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于方法实施例而言，由于其基本相似于系统实施例，所以描述的比较简单，相关之处参见系统实施例的部分说明即可。

以上所述仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (7)

1.一种基于即时定位与地图构建的三维地图重建系统，其特征在于，包括：

处于同一局域网内的相机、开发板及服务器；其中，所述相机包括：可穿戴的红绿蓝深度RGBD相机，所述RGBD相机为多传感器融合相机，所述RGBD相机包含惯性测量单元；

所述RGBD相机，用于实时采集周围环境的数据，所述数据包括：目标图像及惯性测量单元IMU数据，将所述数据通过数据线传输至所述开发板上，所述数据线连接在所述RGBD相机与所述开发板的端口之间；

所述开发板，用于获取相机的定位结果；获取相机位姿；剔除所述目标图像中深度图的目标物特征点，得到剔除后图像；按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，通过机器人软件平台ROS通信，将所述各图像帧三维地图传输至服务器；

所述服务器，用于接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图， 实时显示所述最终三维地图；

所述各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和原有颜色的云点；

所述服务器，用于接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图， 实时显示所述最终三维地图，包括：

所述服务器，用于对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域；所述不同安全等级的区域包括：安全等级最高对应的安全区域及安全等级最低对应的危险区域；变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；

所述服务器，用于实时显示所述最终三维地图，包括：

通过所述服务器上的人机交互界面，实时显示所有图像帧的最终三维地图。

2.如权利要求1所述的系统，其特征在于，所述服务器，用于对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域，包括：

所述服务器，用于利用所述数据得到的彩色缩略图，调用安全等级识别模型；将所述数据得到的彩色缩略图作为所述安全等级识别模型的输入，输出所述周围环境中不同区域的安全等级；其中，所述安全等级识别模型是基于危险场景的训练样本集训练得到的，所述训练样本集包括：已标注不同区域的安全等级的图像；

所述服务器，用于变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

使用用于标注危险区域的预设颜色，覆盖所述危险区域中的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图。

3.如权利要求2所述的系统，其特征在于，所述服务器，用于通过如下步骤，得到所述安全等级识别模型：

获取已标注不同区域的安全等级的图像，所述图像中包含危险区域的特征；

将所述已标注不同区域的安全等级的图像，作为安全等级识别模型的训练样本集；

将所述安全等级识别模型的训练样本集作为待训练安全等级识别模型的输入，利用所述待训练安全等级识别模型进行训练，得到所述安全等级识别模型。

4.如权利要求1所述的系统，其特征在于，所述开发板，用于剔除所述目标图像中深度图的目标物特征点，得到剔除后图像，包括：

所述开发板，用于对所述RGB彩色图，通过目标检测算法，检测所述周围环境中的目标物；在所述深度图中标注所述目标物的像素位置；在所述深度图中，利用所述目标物和背景深度之间的差异，剔除所述深度图中目标物的深度信息，得到剔除后图像；其中，剔除所述背景深度为除所述目标物所在的像素位置以外的其他区域；

所述开发板，用于按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，包括：

所述开发板，用于按照所述定位结果，从所述RGB彩色图中提取三维地图点颜色信息；针对所述目标图像的各图像帧，将所述三维地图点颜色信息及从所述剔除后图像提取深度信息，将所述剔除后图像的像素点依据相机内参矩阵，映射到以相机光心为原点的坐标系中，得到该图像帧的点云；通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标；通过各图像帧的像素点坐标及像素点颜色，得到各图像帧的三维地图。

5.一种基于即时定位与地图构建的三维地图重建方法，其特征在于，包括：

可穿戴的红绿蓝深度RGBD相机实时采集周围环境的数据，所述数据包括：目标图像及惯性测量单元IMU数据，所述RGBD相机为多传感器融合相机，所述RGBD相机包含惯性测量单元；

获取相机的定位结果；获取相机位姿；剔除所述目标图像中深度图的目标物特征点，得到剔除后图像；按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述IMU数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图；

接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；实时显示所述最终三维地图；

其中，所述各图像帧的三维地图包括：与各图像帧各像素点对应的点云图，所述点云图中带有位置和原有颜色的云点；

所述接收最新的所述各图像帧的三维地图，实时更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域；所述不同安全等级的区域包括：安全等级最高对应的安全区域及安全等级最低对应的危险区域；变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图；

所述实时显示所述最终三维地图，包括：

通过人机交互界面，实时显示所有图像帧的最终三维地图；

其中，所述对各图像帧三维地图中的周围环境进行安全等级分类，确定不同安全等级的区域，包括：

利用所述数据得到的彩色缩略图，调用安全等级识别模型；将所述数据得到的彩色缩略图作为所述安全等级识别模型的输入，输出所述周围环境中不同区域的安全等级；其中，所述安全等级识别模型是基于危险场景的训练样本集训练得到的，所述训练样本集包括：已标注不同区域的安全等级的图像；

所述变更所述点云图中所述危险区域的点云原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图，包括：

使用用于标注危险区域的预设颜色，覆盖所述危险区域中的点云的原有颜色，得到变更后各图像帧三维地图；更新已有的所述各图像帧的三维地图，得到所有图像帧的最终三维地图。

6.如权利要求5所述的方法，其特征在于，通过如下步骤，得到所述安全等级识别模型：

获取已标注不同区域的安全等级的图像，所述图像中包含危险区域的特征；

将所述已标注不同区域的安全等级的图像，作为安全等级识别模型的训练样本集；

将所述安全等级识别模型的训练样本集作为待训练安全等级识别模型的输入，利用所述待训练安全等级识别模型进行训练，得到所述安全等级识别模型。

7.如权利要求5所述的方法，其特征在于，所述剔除所述目标图像中深度图的目标物特征点，得到剔除后图像，包括：

对所述RGB彩色图，通过目标检测算法，检测所述周围环境中的目标物；在所述深度图中标注所述目标物的像素位置；在所述深度图中，利用所述目标物和背景深度之间的差异，剔除所述深度图中目标物的深度信息，得到剔除后图像；其中，剔除所述背景深度为除所述目标物所在的像素位置以外的其他区域；

按照所述定位结果，从所述目标图像的RGB彩色图中提取三维地图点；对所述目标图像的各图像帧，根据所述惯性测量单元数据、所述剔除后图像、所述相机位姿及所述三维地图点进行三维地图重建，得到各图像帧的三维地图，包括：

按照所述定位结果，从所述RGB彩色图中提取三维地图点颜色信息；针对所述目标图像的各图像帧，将所述三维地图点颜色信息及从所述剔除后图像提取深度信息，将所述剔除后图像的像素点依据相机内参矩阵，映射到以相机光心为原点的坐标系中，得到该图像帧的点云；通过欧式变换，使用所述相机位姿，将该图像帧的点云旋转和平移至世界坐标系下，确定所述世界坐标系下的该图像帧的像素点坐标；通过各图像帧的像素点坐标及像素点颜色，得到各图像帧的三维地图。

Images