CN114429527A - 基于slam的远程操作场景三维重建方法 - Google Patents

Abstract

针对现有的航天员远场操作场景的虚拟仿真系统存在实效性差和实时诱导反馈能力不足的问题，本发明公开了一种基于SLAM的远程操作场景三维重建方法，其具体步骤包括：采用RGB‑D相机对航天员操作场景进行图像采集，获取航天员操作场景的RGB图和Depth图，根据当前帧图像与上一帧图像的匹配信息来估算当前相机的位姿，采用迭代最近点法实现两帧图像的配准，将迭代最近点法和RGB直接匹配法得到的结果进行加权融合，得到相机位姿的估计结果，闭环检测和建图，网格地图生成。本发明以航天员视觉关注焦点为中心，实现了多模态融合交互，利用该重建的远程操作场景，航天员可独立进行操作训练，减少航天员空间操作或训练过程对地面支持系统或教员的依赖性。

Description

基于SLAM的远程操作场景三维重建方法

技术领域

本发明涉及载人航天领域，具体涉及到一种基于SLAM的远程操作场景三维重建方法。

背景技术

目前,用于航天员在轨空间操作的地面训练的虚拟仿真系统中,航天员要获得设备的操作结果主要有两种途径,一是需地面任务指挥人员或远程教员通过语音通报获得；二是航天员转移到仪表机柜位置通过调取仪表信息页面,根据参数值人工判断操作结果是否正确。目前采取的两种方法均需中断航天员空间操作流程，其中语音通报获取方式航天员需中断操作训练，等待远程支持人员判读语音通报，导致航天员空间操作对地面支持系统的依赖性强，同时由于天地传输延迟导致航天员空间操作及训练持续时间长，效率低。而采用仪表判读方法，航天员需要在空间操作位置与仪表位置之间频繁来回切换，加重航天员的负担，易受环境影响，出错率高，而且过于依赖航天员的个人技能和经验。同时，目前基于增强现实/混合现实的新型航天员操作辅助系统都是“以计算机为中心”的程序化引导系统，普适能力差，仅实现了虚拟信息和真实场景的增强现实叠加，尚未做到对操作过程的实时状态自主判断与实时诱导反馈。综上所述的，现有的航天员远场操作场景的虚拟仿真系统存在实效性差和实时诱导反馈能力不足的问题，如何对航天员的远程操作场景进行有效的实时重建，是当前急需解决的问题。

发明内容

针对现有的航天员远场操作场景的虚拟仿真系统存在实效性差和实时诱导反馈能力不足的问题，本发明公开了一种基于SLAM的远程操作场景三维重建方法，其具体步骤包括：图像获取，利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，闭环检测和建图，网格地图生成。

所述的图像获取，采用RGB-D相机对航天员操作场景进行图像采集，获取航天员操作场景的RGB图和Depth图。Depth图用于记录相应的RGB图上每个像素点对应的空间点的深度值。

具体地，在相机运动过程中，利用RGB-D相机逐帧获得图像，并对获得的每帧图像都打上时间戳。

所述的利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，是根据当前帧图像与上一帧图像(或者之前所有帧的优化模型)的匹配信息来估算当前相机的位姿，采用迭代最近点法实现两帧图像的配准，利用RGB直接匹配法通过最小化空间点对应的图像RGB像素光度误差来优化相机位姿态，将迭代最近点法和RGB直接匹配法得到的结果进行加权融合，得到相机位姿的估计结果。

所述的采用迭代最近点法实现两帧图像的配准，其具体包括，将每帧图像的2D点数据转换为3D点云数据，利用两帧图像的3D点云数据的距离最小为约束条件，计算得到两帧图像之间的旋转矩阵和平移向量，从而实现两帧图像的配准；

所述的闭环检测和建图，利用相机位姿的估计结果，将各个时刻的3D点云数据配准并融合成一个完整的航天员操作场景的三维地图。

具体的，闭环检测的实现包含局部优化与全局优化两个步骤，局部优化的实现步骤为：利用位姿跟踪算法求解航天员所操作设备的位姿，根据位姿将不同图像的3D点云数据进行融合；按照时间将融合后的3D点云数据划分成ACTIVE和INACTIVE两个集合，ACTIVE为随时间发生变化的3D点云数据的集合，INACTIVE为不随时间发生变化的3D点云数据的集合；根据求解得到的航天员所操作设备的位姿，将ACTIVE集合和INACTIVE集合分别在图像上进行投影，得到两幅点云图像，并对其进行配准。

具体的，全局优化包括：按照邻接关系，将RGB-D相机获取的整个序列图像划分成等大小的图像块；对每个图像块进行块内优化，块内优化后，用块内的第一帧图像代表该块，对块内所有帧提取的特征做融合，该块的特征用融合后的特征表示。在图像块间建立匹配关系，进行全局优化。在进行块内或者块间的位姿匹配时，先通过稀疏的SIFT特征点进行估计，再用块内图像的稠密像素进行优化。

具体的，所述的进行块内或者块间的位姿匹配，其具体包括：

E(T)＝w_sparseE_sparse(T)+w_denseE_dense(T)，

其中，T是待优化的位姿转化矩阵，E(T)是位姿误差函数，通过最小化E(T)来计算最优的位姿T，E(T)的最小化包括通过稀疏特征点匹配产生的误差E_sparse(T)和通过稠密像素匹配产生的误差E_dense(T)之和的最小化，w_sparse和w_dense分别为通过稀疏特征点匹配产生的误差和通过稠密像素匹配产生的误差的权重。

具体的，建图过程包括，获得图像中每个特征点的深度信息，根据投影算法计算出每个特征点对应的3D点云数据，采用surfel模型进行点云信息的存储与融合，在相机运动的过程中不断扩展、拼接3D点云数据，并通过全局优化和局部优化生成点云地图。

所述的采用surfel模型进行点云信息的存储与融合，其具体包括，对新获取的点云信息建立一个新的形变图deformation graph，该形变图包括若干个节点，该节点是从重建的3D点云数据均匀抽样得到。

每个节点包含了其节点信息

利用SLAM中的局部回环检测和全局回环检测建立优化约束参数

和

建立surfel模型的每个模型点和形变图节点之间的连接，并根据模型点和节点之间的距离关系，按照加权平均的方式，用优化后的

和

更新新获取的点云位置和法向量。

对于RGB-D相机采集的当前帧图像，根据当前帧图像位姿，将当前帧图像的点云融合到待重建的三维场景中之后，再用光线投影算法计算在当前视角下航天员所看到的场景的表面，并用该表面来对下一帧的输入图像进行配准，计算下一帧图像的位姿。采用光线投影算法计算得到点云，再计算其法向量，用带法向量的点云和下一帧的输入图像进行配准，获取下一帧输入图像的位姿。

所述的网格地图生成，将点云数据转化为Mesh三角网格模型，即将空间点(Vertex)转化成相互连接的三角网格Mesh；并根据点云深度值动态地调整三角网格Mesh的Mesh顶点的高度以及三角网格Mesh的分辨率，最后再将该三角网格Mesh融合到点云地图中，得到网格地图。

具体的，先预定义基础Mesh网格，并且以固定的拓扑结构对其进行三角分割，得到Mesh三角网格。在距离RGB-D相机指定远的位置预设一个平行于水平面的Mesh网格，对预定义的基础Mesh网格进行递归地分割，同时引入若干个的Mesh网格顶点。

具体的，采用动态分层的方式选择Mesh网格的分辨率等级，分辨率等级的计算公式为：

l＝round(log₂([ΔB]/a))，

其中，l为当前Mesh网格采用的分辨率级别，ΔB为一个基础Mesh网格在图像投影的区域大小，a为期望的投影区域大小，round表示取整数。

每个Mesh三角网格的高程值通过z＝f(x；y)函数来描述，z∈R^m，m＝n×n，n为Mesh三角网格在水平面上一个方向的节点数量，m为高程的维度。将点云投影到三角分割后的预定义Mesh网格平面中，每个Mesh三角网格中落入若干点，选择最接近每个Mesh三角网格质心的空间点，用其代表该Mesh三角网格。设第i个Mesh三角网格的关联空间点为(x_i,y_i,z_i)，其与该Mesh三角网格3个顶点的高度值

和

的关系表示为：

上式中，z_i为已知量，

和

为需要求解的值，k为Mesh三角网格的数目，将上述关系表达为矩阵形式：

Jh＝z，

其中，J∈R^k×m，h∈R^m，z∈R^k，J为Mesh三角网格的关联空间点坐标构成的矩阵，h为Mesh三角网格3个顶点的高度值构成的向量，z为Mesh三角网格的关联空间点的高程构成的向量，对该矩阵形式左乘J^T后，得到：

J^TJh＝J^Tz，

其中，J^TJ为对称稀疏结构，采用Gauss-Seidel算法来解算，得到上述三个顶点的高度值。

具体的，采用SLAM中的图优化方法来实现网格地图重建误差的最小化，将优化变量作为是SLAM中图的顶点(Vertex)，观测方程作为SLAM中图的边(Edge)。对于SLAM中顶点和边的选择，其具体包括：

具体的，从全局优化角度，用特征点匹配误差最小化来估计得到初步的相机位姿，再利用最优相机位姿估计的方法，得到全局最优的位姿误差代价函数：

E(T)＝E_s(T)+E_d(T)，

其中，E_s(T)为平面方向上的位姿误差函数,E_d(T)为深度方向上的位姿误差函数,T是待优化的位姿转化矩阵，E(T)是位姿误差函数；以该位姿误差代价函数最小为目标，对相机位姿进行估计。

本发明的有益效果为：

针对现有的航天员远场操作场景的虚拟仿真系统存在实效性差和实时诱导反馈能力不足的问题，研究航天员空间操作场景重建与匹配技术，以提高航天员增强现实操作辅助系统智能性和可用性。本发明具有以下优点：

1)以航天员视觉关注焦点为中心，实现了多模态融合交互；

2)基于SLAM重建远程操作场景，能重建操作设备状态，并根据状态驱动操作信息同步诱导，以主动方式为航天员提供实时信息支持；

3)利用该重建的远程操作场景，航天员可独立进行操作训练，减少航天员空间操作或训练过程对地面支持系统或教员的依赖性。

附图说明

图1为本发明方法的实现流程图；

图2为本发明的RGBD-SLAM位姿估计方法的实现示意图；

图3为本发明的ICP算法的实现示意图；

图4为本发明的RGB直接匹配法的估计过程示意图；

图5为本发明预定义的Mesh三角网格和拓扑结构的示意图；

图6为本发明的多分辨率Mesh网格示意图。

具体实施方式

为了更好的了解本发明内容，这里给出二个实施例。

图1为本发明方法的实现流程图；图2为本发明的RGBD-SLAM位姿估计方法的实现示意图；图3为本发明的ICP算法的实现示意图；图4为本发明的RGB直接匹配法的估计过程示意图；图5为本发明预定义的Mesh三角网格和拓扑结构的示意图；图6为本发明的多分辨率Mesh网格示意图。

实施例1：基于SLAM的远程操作场景三维重建方法

一种基于SLAM的远程操作场景三维重建方法，其具体步骤包括：图像获取，利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，闭环检测和建图，网格地图生成。

所述的图像获取，采用RGB-D相机对航天员操作场景进行图像采集，获取航天员操作场景的RGB图和Depth图。Depth图用于记录相应的RGB图上每个像素点对应的空间点的深度值。

具体地，在相机运动过程中，利用RGB-D相机逐帧获得图像，并对获得的每帧图像都打上时间戳。

所述的利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，是根据当前帧图像与上一帧图像(或者之前所有帧的优化模型)的匹配信息来估算当前相机的位姿，采用迭代最近点法实现两帧图像的配准，利用RGB直接匹配法通过最小化空间点对应的图像RGB像素光度误差来优化相机位姿态，将迭代最近点法和RGB直接匹配法得到的结果进行加权融合，得到相机位姿的估计结果。

所述的采用迭代最近点法实现两帧图像的配准，其具体包括，将每帧图像的2D点数据转换为3D点云数据，利用两帧图像的3D点云数据的距离最小为约束条件，计算得到两帧图像之间的旋转矩阵和平移向量，从而实现两帧图像的配准；

所述的闭环检测和建图，利用相机位姿的估计结果，将各个时刻的3D点云数据配准并融合成一个完整的航天员操作场景的三维地图。

具体的，闭环检测的实现包含局部优化与全局优化两个步骤，局部优化的实现步骤为：利用位姿跟踪算法求解航天员所操作设备的位姿，根据位姿将不同图像的3D点云数据进行融合；按照时间将融合后的3D点云数据划分成ACTIVE和INACTIVE两个集合，ACTIVE为随时间发生变化的3D点云数据的集合，INACTIVE为不随时间发生变化的3D点云数据的集合；根据求解得到的航天员所操作设备的位姿，将ACTIVE集合和INACTIVE集合分别在图像上进行投影，得到两幅点云图像，并对其进行配准。

具体的，全局优化包括：按照邻接关系，将RGB-D相机获取的整个序列图像划分成等大小的图像块；对每个图像块进行块内优化，块内优化后，用块内的第一帧图像代表该块，对块内所有帧提取的特征做融合，该块的特征用融合后的特征表示。在图像块间建立匹配关系，进行全局优化。在进行块内或者块间的位姿匹配时，先通过稀疏的SIFT特征点进行估计，再用块内图像的稠密像素进行优化。

具体的，所述的进行块内或者块间的位姿匹配，其具体包括：

E(T)＝w_sparseE_sparse(T)+w_denseE_dense(T)，

其中，T是待优化的位姿转化矩阵，E(T)是位姿误差函数，通过最小化E(T)来计算最优的位姿T，E(T)的最小化包括通过稀疏特征点匹配产生的误差E_sparse(T)和通过稠密像素匹配产生的误差E_dense(T)之和的最小化，w_sparse和w_dense分别为通过稀疏特征点匹配产生的误差和通过稠密像素匹配产生的误差的权重。

具体的，建图过程包括，获得图像中每个特征点的深度信息，根据投影算法计算出每个特征点对应的3D点云数据，采用surfel模型进行点云信息的存储与融合，在相机运动的过程中不断扩展、拼接3D点云数据，并通过全局优化和局部优化生成点云地图。

所述的采用surfel模型进行点云信息的存储与融合，其具体包括，对新获取的点云信息建立一个新的形变图deformation graph，该形变图包括若干个节点，该节点是从重建的3D点云数据均匀抽样得到。

每个节点包含了其节点信息

利用SLAM中的局部回环检测和全局回环检测建立优化约束参数

和

建立surfel模型的每个模型点和形变图节点之间的连接，并根据模型点和节点之间的距离关系，按照加权平均的方式，用优化后的

和

更新新获取的点云位置和法向量。

对于RGB-D相机采集的当前帧图像，根据当前帧图像位姿，将当前帧图像的点云融合到待重建的三维场景中之后，再用光线投影算法计算在当前视角下航天员所看到的场景的表面，并用该表面来对下一帧的输入图像进行配准，计算下一帧图像的位姿。采用光线投影算法计算得到点云，再计算其法向量，用带法向量的点云和下一帧的输入图像进行配准，获取下一帧输入图像的位姿。

所述的网格地图生成，将点云数据转化为Mesh三角网格模型，即将空间点(Vertex)转化成相互连接的三角网格Mesh；并根据点云深度值动态地调整三角网格Mesh的Mesh顶点的高度以及三角网格Mesh的分辨率，最后再将该三角网格Mesh融合到点云地图中，得到网格地图。

具体的，先预定义基础Mesh网格，并且以固定的拓扑结构对其进行三角分割，得到Mesh三角网格。在距离RGB-D相机指定远的位置预设一个平行于水平面的Mesh网格，对预定义的基础Mesh网格进行递归地分割，同时引入若干个的Mesh网格顶点。

具体的，采用动态分层的方式选择Mesh网格的分辨率等级，分辨率等级的计算公式为：

l＝round(log₂([ΔB]/a))，

其中，l为当前Mesh网格采用的分辨率级别，ΔB为一个基础Mesh网格在图像投影的区域大小，a为期望的投影区域大小，round表示取整数。

每个Mesh三角网格的高程值通过z＝f(x；y)函数来描述，z∈R^m，m＝n×n，n为Mesh三角网格在水平面上一个方向的节点数量，m为高程的维度。将点云投影到三角分割后的预定义Mesh网格平面中，每个Mesh三角网格中落入若干点，选择最接近每个Mesh三角网格质心的空间点，用其代表该Mesh三角网格。设第i个Mesh三角网格的关联空间点为(x_i,y_i,z_i)，其与该Mesh三角网格3个顶点的高度值

和

的关系表示为：

上式中，z_i为已知量，

和

为需要求解的值，k为Mesh三角网格的数目，将上述关系表达为矩阵形式：

Jh＝z，

其中，J∈R^k×m，h∈R^m，z∈R^k，J为Mesh三角网格的关联空间点坐标构成的矩阵，h为Mesh三角网格3个顶点的高度值构成的向量，z为Mesh三角网格的关联空间点的高程构成的向量，对该矩阵形式左乘J^T后，得到：

J^TJh＝J^Tz，

其中，J^TJ为对称稀疏结构，采用Gauss-Seidel算法来解算，得到上述三个顶点的高度值。

具体的，采用SLAM中的图优化方法来实现网格地图重建误差的最小化，将优化变量作为是SLAM中图的顶点(Vertex)，观测方程作为SLAM中图的边(Edge)。对于SLAM中顶点和边的选择，其具体包括：

具体的，从全局优化角度，用特征点匹配误差最小化来估计得到初步的相机位姿，再利用最优相机位姿估计的方法，得到全局最优的位姿误差代价函数：

E(T)＝E_s(T)+E_d(T)，

其中，E_s(T)为平面方向上的位姿误差函数,E_d(T)为深度方向上的位姿误差函数,T是待优化的位姿转化矩阵，E(T)是位姿误差函数；以该位姿误差代价函数最小为目标，对相机位姿进行估计。

实施例2：基于SLAM的远程操作场景三维重建方法

本发明公开了一种基于SLAM的远程操作场景三维重建方法，其具体步骤包括：图像获取，利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，闭环检测和建图，网格地图生成。

所述的图像获取，采用RGB-D相机对航天员操作场景进行图像采集，获取航天员操作场景的RGB图和Depth图。Depth图用于记录相应的RGB图上每个像素点对应的空间点的深度值。

RGB图主要用于特征提取和匹配，获得不同帧图像的匹配的特征点，然后再根据Depth图计算特征点对应的3D点云，进而使用ICP算法进行点云匹配，并通过最小距离匹配估计最优的相机位姿。为了获取全局一致的相机轨迹和地图，使用了局部优化与全局优化的方法实现回环检测，最终建立三维场景点云模型。

具体地，在相机运动过程中，利用RGB-D相机逐帧获得图像，并对获得的每帧图像都打上时间戳。与其它相机不同的是，RGB-D相机除了可以获取一张RGB图，还能获取一张Depth图，它记录了RGB图上每个像素点对应的空间点的深度值。

所述的利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，是根据当前帧图像与上一帧图像(或者之前所有帧的优化模型)的匹配信息来估算当前相机的位姿，也就是其相对于参考帧的旋转矩阵R和平移向量t，该环节是SLAM技术的核心步骤。不同于一般的SLAM技术，本方法估计的相机位姿能配准空间点云，还能配准空间点云对应的色彩信息，采用迭代最近点法实现两帧图像的配准，利用RGB直接匹配法通过最小化空间点对应的图像RGB像素光度误差来优化相机位姿态，将迭代最近点法和RGB直接匹配法得到的结果进行加权融合，得到相机位姿的估计结果。

所述的采用迭代最近点法实现两帧图像的配准，其具体包括，将每帧图像的2D点数据转换为3D点云数据，利用两帧图像的3D点云数据的距离最小为约束条件，计算得到两帧图像之间的旋转矩阵和平移向量，从而实现两帧图像的配准；

所述的采用迭代最近点法(ICP)实现两帧图像的配准，ICP算法主要用于两帧点云配准，通过算法ICP实现基于相机位姿估计，实现方法如图3所示。

根据上述的步骤，我们可以得到两组匹配好的特征点对，但这些点对还只是图像平面的2D点，所以首先根据投影算法将它们转化为3D点云数据。已知图像平面像素点坐标为(u，v)以及该像素点对应的归一化深度值为d，相机坐标系下对应的点云为(X，Y，Z)，根据相机投影算法有：

其中f_x,f_y为焦距，c_x,c_y为像平面偏离光心的距离，均为相机内参，s深度值缩放因子，如果给定深度数单位为mm，则s＝1000。假设根据相机投影算法获得的是两组3D点云数据分别为：

P＝{p₁,p₂,…},P’＝{p’₁,p’₂,…}，

得到的是3D-3D间的关系，ICP算法假设两帧之间各对应点位姿关系满足：

其中R和t分别为点云P到点云P’的姿态变换矩阵和平移向量，反映了相机位姿的变化，是ICP算法的计算目标。可以看出，3D-3D点云之间的位姿姿态变换与相机无关，因为无需相机内参。当匹配的空间点距离最小时即为最优的R，t，

对于RGB直接匹配法实现，基于本发明三维重建需求，采用了直接法来进行相机匹配。直接法以图像像素为处理对象，而不只是其中的角点和边缘。直接法有一个假设前提：同一个空间点的像素值，在各个图像中是一样的。

空间点P的世界坐标为[X，Y，Z]，其在两帧相机图像上像素点分别为p1，p2。设第一帧处相机旋转矩阵为I和平移为0，求其到第二帧相机位姿态变换矩阵R，t(李代数为ξ)，相机内参为K。RGB直接匹配法的计算公式包括：

与特征点法的最小化重投影误差(Reprojecton error)来优化相机位姿态不同，直接法通过最小化空间点对应的图像RGB像素误差(称为光度误差，Photometric error)来优化相机位姿态R，t。本发明将ICP与直接法结合起来，将ICP中的重投影误差和直接法中的RGB像素误差同时最小化来优化相机位姿，同时加入权重参数来控制两种对相机位姿的贡献，用公式表示为：

e＝eicp+w*ergbd。

所述的闭环检测和建图，利用相机位姿的估计结果，将各个时刻的3D点云数据配准并融合成一个完整的航天员操作场景的三维地图。

具体的，闭环检测的实现包含局部优化与全局优化两个步骤，局部优化的实现步骤为：利用位姿跟踪算法求解航天员所操作设备的位姿，根据位姿将不同图像的3D点云数据进行融合；按照时间将融合后的3D点云数据划分成ACTIVE和INACTIVE两个集合，ACTIVE为随时间发生变化的3D点云数据的集合，INACTIVE为不随时间发生变化的3D点云数据的集合；根据求解得到的航天员所操作设备的位姿，将ACTIVE集合和INACTIVE集合分别在图像上进行投影，得到两幅点云图像，并对其进行配准。如果可以配准上，则说明存在局部回环。

局部优化针对相邻帧以及区域的位姿配准，全局优化实现整个三维空间的位姿与航天员操作场景的三维地图的一致性。准确的三维重建不但要对齐空间位置，还要对齐深度与RGB像素值，所以位姿估计的约束条件更多。具体的，全局优化包括：按照邻接关系，将RGB-D相机获取的整个序列图像划分成等大小的图像块，{Ti，i＝1,2,…,n}，新的图像帧加入后，当其数量累计到一个图像块所包含的图像数量后才开始做块内优化；对每个图像块进行块内优化，块内优化后，用块内的第一帧图像代表该块，对块内所有帧提取的特征做融合，该块的特征用融合后的特征表示。在图像块间建立匹配关系，进行全局优化。在进行块内或者块间的位姿匹配时，先通过稀疏的SIFT特征点进行估计，再用块内图像的稠密像素进行优化。

具体的，所述的进行块内或者块间的位姿匹配，其具体包括：

E(T)＝w_sparseE_sparse(T)+w_denseE_dense(T)，

其中，T是待优化的位姿转化矩阵，E(T)是位姿误差函数，通过最小化E(T)来计算最优的位姿T，E(T)的最小化包括通过稀疏特征点匹配产生的误差E_sparse(T)和通过稠密像素匹配产生的误差E_dense(T)之和的最小化，w_sparse和w_dense分别为通过稀疏特征点匹配产生的误差和通过稠密像素匹配产生的误差的权重。稠密像素的误差包含了来自深度匹配的误差和RGB像素匹配的误差：

E_dense(T)＝w_depthE_depth(T)+w_colorE_color(T)，

具体地，该误差最小化可通过Randomized Ferns算法实现。

具体的，建图过程包括，获得图像中每个特征点的深度信息，根据投影算法计算出每个特征点对应的3D点云数据，采用surfel模型进行点云信息的存储与融合，在相机运动的过程中不断扩展、拼接3D点云数据，并通过全局优化和局部优化生成点云地图。

本发明中不但需要生成一个全局的三维点云，还需要每个点云的RGB信息，这个我们在SLAM前端的直接法阶段已经获取到了每个特征点的RGB值。采用surfel模型进行点云信息的存储与融合，采用该模型方便使用OpenGL进行处理，提高融合、更新的效率。Surfel模型对于每个点主要存储点的位置(x，y，z)，面片半径(r),法向量(n)，颜色信息(R，G，B)以及点的获取时间t等。

所述的采用surfel模型进行点云信息的存储与融合，其具体包括，对新获取的点云信息建立一个新的形变图deformation graph，该形变图包括若干个节点，该节点是从重建的3D点云数据均匀抽样得到，node的数量和重建好的点的数量成正相关。

每个节点包含了其节点信息

利用SLAM中的局部回环检测和全局回环检测建立优化约束参数

和

建立surfel模型的每个模型点和形变图节点之间的连接，并根据模型点和节点之间的距离关系，按照加权平均的方式，用优化后的

和

更新新获取的点云位置和法向量。

位置坐标更新公式如下：

法向量更新公式如下：

权值计算公式如下：

对于RGB-D相机采集的当前帧图像，根据当前帧图像位姿，将当前帧图像的点云融合到待重建的三维场景中之后，再用光线投影算法计算在当前视角下航天员所看到的场景的表面，并用该表面来对下一帧的输入图像进行配准，计算下一帧图像的位姿。光线投影算法在实际计算的时候，也是用GPU并行计算，GPU的单个线程处理单个的像素点。采用光线投影算法计算得到点云，再计算其法向量，用带法向量的点云和下一帧的输入图像进行配准，获取下一帧输入图像的位姿，如此是个循环的过程。

所述的网格地图生成，将点云数据转化为Mesh三角网格模型，即将空间点(Vertex)转化成相互连接的三角网格Mesh；并根据点云深度值动态地调整三角网格Mesh的Mesh顶点的高度以及三角网格Mesh的分辨率，最后再将该三角网格Mesh融合到点云地图中，得到网格地图。

具体的，为了高效地生成Mesh网格，先预定义基础Mesh网格，并且以固定的拓扑结构对其进行三角分割，得到Mesh三角网格，类似于Laplace金字塔分层模型，如图5所示。在距离RGB-D相机指定远的位置预设一个平行于水平面的Mesh网格，只允许Mesh网格顶点改变一个自由度：其法线方向上的距离，初始为垂直方向。根据点云数据以及Mesh分辨率要求，对预定义的基础Mesh网格进行递归地分割，同时引入若干个的Mesh网格顶点。预设Mesh网格数量和最大Mesh网格数量可根据要求设定。

假设预定义的Mesh网格数量为M0＝n×n(n为X和Y方向网格数)，进行一次过采样之后数量增加D1＝(2n-1)×(2n-1)，此时网格总数M1＝M0+D1，网格的分辨率也会提高一级。类似地，每提高一级分辨率，Mesh网格的数量就以该规律增长：

其中，i指当前的分辨率级别。理论上，最小可以达到像素级别的分辨率。

根据相机成像原理，随着与相机成像平面距离不同，物体的成像区域大小也不同：距离越近区域越大，反之越小。如果一直采用一个精细层次的Mesh网格结构，或者会使近距离的物理分辨率过低，降低重建精度；或者会使远距离的物体分辨率过高，增加系统运算负担。所以，具体的，采用动态分层的方式选择Mesh网格的分辨率等级，分辨率等级的计算公式为：

l＝round(log₂([ΔB]/a))，

其中，l为当前Mesh网格采用的分辨率级别，ΔB为一个基础Mesh网格在图像投影的区域大小，a为期望的投影区域大小，如4个像素点，round表示取整数。距离越近，ΔB值随着变大，则分辨率级别提高；反之，分辨率级别减小。图6为多分辨率Mesh网格示意图。

每个Mesh三角网格的高程值通过z＝f(x；y)函数来描述，z∈R^m，m＝n×n，n为Mesh三角网格在水平面上一个方向的节点数量，m为高程的维度。将点云投影到三角分割后的预定义Mesh网格平面中，每个Mesh三角网格中落入若干点，选择最接近每个Mesh三角网格质心的空间点，用其代表该Mesh三角网格。设第i个Mesh三角网格的关联空间点为(x_i,y_i,z_i)，其与该Mesh三角网格3个顶点的高度值

和

的关系表示为：

上式中，z_i为已知量，

和

为需要求解的值，k为Mesh三角网格的数目，将上述关系表达为矩阵形式：

Jh＝z，

其中，J∈R^k×m，h∈R^m，z∈R^k，J为Mesh三角网格的关联空间点坐标构成的矩阵，h为Mesh三角网格3个顶点的高度值构成的向量，z为Mesh三角网格的关联空间点的高程构成的向量，对该矩阵形式左乘J^T后，得到：

J^TJh＝J^Tz，

其中J^TJ为对称稀疏结构，采用Gauss-Seidel算法来解算，得到上述三个顶点的高度值。当新的图像帧出现，更新J^TJ和J^T后，Gauss-Seidel算法迭代很少次就可以快速地收敛。且所有的计算值都保存于J^TJ和J^Tz结构中，所以其对计算量和内存要求也很有限。

在前端定位环节，通过特征匹配误差最小化来估算最优的位姿。由于特征提取与匹配存在噪声影响，所以无法总是保证位姿正确。为了提高位姿的估计精度、加强系统鲁棒性，本发明将在建图环节进一步优化位姿。

具体的，采用SLAM中的图优化方法来实现网格地图重建误差的最小化，图优化是用图论的概念进行优化，将优化变量作为是SLAM中图的顶点(Vertex)，观测方程作为SLAM中图的边(Edge)。对于SLAM中顶点和边的选择，其具体包括：

对于顶点的选择，以相机位姿T＝[R|t]＝:ξ∈se(3)(矩阵T对应的李代数)，以及网格地图中代表每个Mesh网格的空间点p∈R³，作为顶点；

对于边的选择，每个Mesh网格的空间点在相机或者全局DEM模型中的投影，以观测方程来描述：

z＝h(ξ,p)，

将该观测方程作为SLAM中图的边。假设位姿ξ和空间点p已知，当以位姿ξ去观测空间点p，可通过投影关系h(ξ,p)获得其在图像平面上的投影z。但实际上由于噪声的存在，z不可能精确地等于h(ξ,p)，于是就有了误差，其表达式为：

e＝z-h(ξ,p)，

设z_ij是在位姿ξ_i处观测Mesh网格空间点p_j产生的数据，那么整体优化的代价函数：

上式的求解属于最小二乘问题，其对相机和空间点空间位置同时做了优化调整。通过非线性优化算法求解，首先将其转化为线性的增量方程形式，然后可通过列文伯格-马夸尔特法(Liebenberg-Marquardt)求解。

具体的，从全局优化角度，用特征点匹配误差最小化来估计得到初步的相机位姿，再利用最优相机位姿估计的方法，得到全局最优的位姿误差代价函数：

E(T)＝E_s(T)+E_d(T)，

其中，E_s(T)为平面方向上的位姿误差函数,E_d(T)为深度方向上的位姿误差函数,T是待优化的位姿转化矩阵，E(T)是位姿误差函数；以该位姿误差代价函数最小为目标，对相机位姿进行估计。

最优相机位姿估计的方法也为闭环检测提供了判断依据，回顾在前述的定位步骤使用的位姿估计公式：

通过加权求和，得到全局最优的位姿误差代价函数：

E(T)＝E_s(T)+E_d(T)。

以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims (10)

1.一种基于SLAM的远程操作场景三维重建方法，其特征在于，其具体步骤包括：图像获取，利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，闭环检测和建图，网格地图生成。

2.如权利要求1所述的基于SLAM的远程操作场景三维重建方法，其特征在于，所述的图像获取，采用RGB-D相机对航天员操作场景进行图像采集，获取航天员操作场景的RGB图和Depth图；Depth图用于记录相应的RGB图上每个像素点对应的空间点的深度值。

3.如权利要求2所述的基于SLAM的远程操作场景三维重建方法，其特征在于，在相机运动过程中，利用RGB-D相机逐帧获得图像，并对获得的每帧图像都打上时间戳。

4.如权利要求1所述的基于SLAM的远程操作场景三维重建方法，其特征在于，所述的利用获取的图像进行特征/像素匹配，再进行最优相机位姿估计，是根据当前帧图像与上一帧图像的匹配信息来估算当前相机的位姿，采用迭代最近点法实现两帧图像的配准，利用RGB直接匹配法通过最小化空间点对应的图像RGB像素光度误差来优化相机位姿态，将迭代最近点法和RGB直接匹配法得到的结果进行加权融合，得到相机位姿的估计结果。

5.如权利要求4所述的基于SLAM的远程操作场景三维重建方法，其特征在于，所述的采用迭代最近点法实现两帧图像的配准，其具体包括，将每帧图像的2D点数据转换为3D点云数据，利用两帧图像的3D点云数据的距离最小为约束条件，计算得到两帧图像之间的旋转矩阵和平移向量，从而实现两帧图像的配准。

6.如权利要求1所述的基于SLAM的远程操作场景三维重建方法，其特征在于，所述的闭环检测和建图，利用相机位姿的估计结果，将各个时刻的3D点云数据配准并融合成一个完整的航天员操作场景的三维地图。

7.如权利要求6所述的基于SLAM的远程操作场景三维重建方法，其特征在于，闭环检测的实现包含局部优化与全局优化两个步骤，局部优化的实现步骤为：利用位姿跟踪算法求解航天员所操作设备的位姿，根据位姿将不同图像的3D点云数据进行融合；按照时间将融合后的3D点云数据划分成ACTIVE和INACTIVE两个集合，ACTIVE为随时间发生变化的3D点云数据的集合，INACTIVE为不随时间发生变化的3D点云数据的集合；根据求解得到的航天员所操作设备的位姿，将ACTIVE集合和INACTIVE集合分别在图像上进行投影，得到两幅点云图像，并对其进行配准；

全局优化包括：按照邻接关系，将RGB-D相机获取的整个序列图像划分成等大小的图像块；对每个图像块进行块内优化，块内优化后，用块内的第一帧图像代表该块，对块内所有帧提取的特征做融合，该块的特征用融合后的特征表示；在图像块间建立匹配关系，进行全局优化；在进行块内或者块间的位姿匹配时，先通过稀疏的SIFT特征点进行估计，再用块内图像的稠密像素进行优化。

8.如权利要求7所述的基于SLAM的远程操作场景三维重建方法，其特征在于，所述的进行块内或者块间的位姿匹配，其具体包括：

E(T)＝w_sparseE_sparse(T)+w_denseE_dense(T)，

其中，T是待优化的位姿转化矩阵，E(T)是位姿误差函数，通过最小化E(T)来计算最优的位姿T，E(T)的最小化包括通过稀疏特征点匹配产生的误差E_sparse(T)和通过稠密像素匹配产生的误差E_dense(T)之和的最小化，w_sparse和w_dense分别为通过稀疏特征点匹配产生的误差和通过稠密像素匹配产生的误差的权重。

9.如权利要求6所述的基于SLAM的远程操作场景三维重建方法，其特征在于，建图过程包括，获得图像中每个特征点的深度信息，根据投影算法计算出每个特征点对应的3D点云数据，采用surfel模型进行点云信息的存储与融合，在相机运动的过程中不断扩展、拼接3D点云数据，并通过全局优化和局部优化生成点云地图；

所述的采用surfel模型进行点云信息的存储与融合，其具体包括，对新获取的点云信息建立一个新的形变图deformation graph，该形变图包括若干个节点，该节点是从重建的3D点云数据均匀抽样得到；

每个节点包含了其节点信息

利用SLAM中的局部回环检测和全局回环检测建立优化约束参数

和

建立surfel模型的每个模型点和形变图节点之间的连接，并根据模型点和节点之间的距离关系，按照加权平均的方式，用优化后的

和

更新新获取的点云位置和法向量；

对于RGB-D相机采集的当前帧图像，根据当前帧图像位姿，将当前帧图像的点云融合到待重建的三维场景中之后，再用光线投影算法计算在当前视角下航天员所看到的场景的表面，并用该表面来对下一帧的输入图像进行配准，计算下一帧图像的位姿；采用光线投影算法计算得到点云，再计算其法向量，用带法向量的点云和下一帧的输入图像进行配准，获取下一帧输入图像的位姿。

10.如权利要求1所述的基于SLAM的远程操作场景三维重建方法，其特征在于，

所述的网格地图生成，将点云数据转化为Mesh三角网格模型，即将空间点转化成相互连接的三角网格Mesh；并根据点云深度值动态地调整三角网格Mesh的Mesh顶点的高度以及三角网格Mesh的分辨率，最后再将该三角网格Mesh融合到点云地图中，得到网格地图。

Images