一种多RGB-D传感器的空间三维扫描方法及装置与流程

本发明涉及计算机视觉领域，尤其涉及的是一种多rgb-d传感器的空间三维扫描方法及装置。

背景技术：

目前，在视觉领域中，单一传感器的视觉slam扩展到多传感器协同slam是提升传感器追踪鲁棒性的重要技术手段，在多rgb-d传感器联合slam方面，提出采用双kinect2联合的方式来实现室内三维测图，研究结果表明，双rgb-d传感器联合测图可有效提升传感器追踪精度，并可获取更精细的室内三维信息，但是目前的多rgb-d传感器系统存在问题，首先忽略了slam累积误差，其次该双rgb-d传感器测图系统未集成闭环检测方案，且忽略了传感器同步不准确造成的误差累积问题。

因此，现有技术还有待于改进和发展。

技术实现要素：

鉴于上述现有技术的不足，本发明提供了一种多rgb-d传感器的空间三维扫描方法及装置，旨在构建多rgb-d传感器联合测图系统，探究混合多视角下二三维视觉特征与三维几何特征的传感器姿态更新模型构建方法，突破多rgb-d传感器slam关键技术难点，力争破解传统单rgb-d传感器slam技术低鲁棒性困境。

本发明的技术方案如下：

一种多rgb-d传感器的空间三维扫描方法，所述方法包括以下步骤：

a、选取多个rgb-d传感器搭建多rgb-d传感器测图系统；所述多rgb-d传感器测图系统获取图像数据帧序列；

b、获取所述图像数据帧序列中含有的特征匹配点，根据所述特征匹配点构建多rgb-d传感器姿态更新方程，利用所述多rgb-d传感器姿态更新方程得到传感器轨迹；

c、根据所述传感器轨迹建立回环概率模型，基于所述回环概率模型对传感器轨迹进行全局优化，得到优化后的传感器轨迹；

d、联合图像数据帧序列与所述优化后的传感器轨迹构建室内三角网模型，所述三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图。

可选的，所述步骤a还包括：

a1、通过rgb-d传感器内外方位校正方法、无重叠视角的多rgb-d传感器测图系统外方位半自动校正方法和深度传感器测量误差校正方法联合校正所述测图系统的参数。

可选的，所述无重叠视角多rgb-d传感器测图系统外方位半自动校正方法包括：

预先通过全站仪获取的棋盘角点三维点集、全站仪所在坐标系和各rgb-d传感器坐标系；

根据所述多rgb-d传感器测图系统中的传感器布局，在多个rgb-d传感器视角及测量范围内的三维结构上附着棋盘图案；

所述多个rgb-d传感器同步采集单帧图像，自动提取对应的棋盘角点序列，并映射到深度图像中获取对应的三维点集；

根据最小二乘原理，依次进行全站仪坐标系与各rgb-d传感器坐标系之间的坐标变换，得到全站仪坐标系与各rgb-d传感器坐标系之间的坐标变换关系，根据所述坐标变换关系将所述三维控制点集转换到各rgb-d传感器坐标系下实现校正。

可选的，所述特征匹配点包括二三维视觉特征匹配点以及三维几何特征匹配点。

可选的，所述步骤b包括：

b1、提取所述图像数据帧序列的关键帧；

b2、通过所述关键帧的点特征匹配得到二三维视觉特征匹配点；

b3、通过局部三维空间块的描述子建立局部3d数据的对应关系，获取三维几何特征匹配点；

b4、通过最小化二三维视觉特征匹配点的重投影误差与三维几何特征匹配点的距离误差构建多rgb-d传感器姿态更新方程。

可选的，所述步骤c中根据所述传感器轨迹建立回环概率模型的步骤包括：

c11、采用运动度量的方法，比较关键帧之间的位置和朝向筛选出阈值内的候选关键帧；

c12、采用词袋模型对所述候选关键帧与当前关键帧进行闭环检测，获取闭环概率；

c13、对多次检测的闭环概率归一化处理，构建多次检测结果的回环概率模型。

可选的，所述步骤c中所述基于所述回环概率模型对传感器轨迹进行全局优化的步骤包括：

c21、构建用于降低传感器追踪误差累积的全局实时平差网得到全局优化的传感器轨迹；

所述全局实时平差网，包括：回环运动矩阵优化、全局平差网构建、运动模型自由度定权和实时平差。

一种多rgb-d传感器的空间三维扫描装置，包括：由多个rgb-d传感器搭建的多rgb-d传感器测图系统和与所述rgb-d测图系统相连接的数据处理系统；

所述多rgb-d传感器测图系统，用于获取图像数据帧序列；

所述数据处理系统包括：获取传感器轨迹模块、优化传感器轨迹模块、三维点云或octomap多尺度地图生成模块；

所述获取传感器轨迹模块，用于获取所述图像数据帧序列中含有的特征匹配点，根据所述特征匹配点构建多rgb-d传感器姿态更新方程，利用所述多rgb-d传感器姿态更新方程得到传感器轨迹；

所述优化传感器轨迹模块，用于根据所述传感器轨迹建立回环概率模型，基于所述回环概率模型对传感器轨迹进行全局优化，得到优化后的传感器轨迹；

所述三维点云或octomap多尺度地图生成模块，用于联合图像数据帧序列与所述优化后的传感器轨迹构建室内三角网模型，所述三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图。

可选的，所述获取传感器轨迹模块包括多rgb-d传感器姿态更新单元；

所述多rgb-d传感器姿态更新单元，用于提取所述图像数据帧序列的关键帧；通过所述关键帧的点特征匹配得到二三维视觉特征匹配点；通过局部三维空间块的描述子建立局部3d数据的对应关系，获取三维几何特征匹配点；通过最小化二三维视觉特征匹配点的重投影误差与三维几何特征匹配点的距离误差构建多rgb-d传感器姿态更新方程。

可选的，所述优化传感器轨迹模块包括回环概率模型构建单元；

所述回环概率模型构建单元，用于采用运动度量的方法，比较关键帧之间的位置和朝向筛选出阈值内的候选关键帧；采用词袋模型对所述候选关键帧与当前关键帧进行闭环检测，获取闭环概率；对多次检测的闭环概率归一化处理，构建多次检测结果的回环概率模型。

本发明提供的一种多rgb-d传感器的空间三维扫描方法及装置，所述方法包括：选取多个rgb-d传感器搭建多rgb-d传感器测图系统；所述多rgb-d传感器测图系统获取图像数据帧序列；获取所述图像数据帧序列中含有的特征匹配点，根据所述特征匹配点构建多rgb-d传感器姿态更新方程，利用所述多rgb-d传感器姿态更新方程得到传感器轨迹；根据所述传感器轨迹建立回环概率模型，基于所述回环概率模型对传感器轨迹进行全局优化，得到优化后的传感器轨迹；联合图像数据帧序列与所述优化后的传感器轨迹构建室内三角网模型，所述三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图，解决了多rgb-d传感器slam的关键难点，提高了鲁棒性。

附图说明

图1为本发明所述多rgb-d传感器的空间三维扫描方法的步骤流程图；

图2为本发明所述无重叠视角的多rgb-d传感器测图系统外方位半自动校正方法示意图；

图3为本发明所述多rgb-d传感器测图系统的姿态更新的示意图；

图4为本发明所述建立回环概率模型的方法步骤流程图；

图5是本发明一个实施例的步骤流程图；

图6是本发明所述多rgb-d传感器的空间三维扫描装置的结构框图。

具体实施方式

本发明提供一种多rgb-d传感器的空间三维扫描方法及装置，为使本发明的目的、技术方案及效果更加清楚、明确，以下对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

一种多rgb-d传感器的空间三维扫描方法，请参阅图1，所述方法包括以下步骤：

s1、选取多个rgb-d传感器搭建多rgb-d传感器测图系统；所述多rgb-d传感器测图系统获取图像数据帧序列；

综合分析多种rgb-d传感器测距机制(结构光、tof)、可视角度、量测范围及便携性等特性，选取适当的设备组合和配置方式搭建多rgb-d传感器测图系统，在一个多rgb-d传感器测图系统中可以有n个rgb-d传感器。

可选的，所述步骤s1还包括：

s11、通过rgb-d传感器内外方位校正方法、无重叠视角的多rgb-d传感器测图系统外方位半自动校正方法和深度传感器测量误差校正方法联合校正所述测图系统的参数。

针对所述多rgb-d传感器测图系统，分别从视觉与深度传感器内外方位校正、重叠视角的多rgb-d传感器测图系统外方位半自动校正方法以及深度传感器测量误差校正三个方面完成多rgb-d传感器测图系统参数获取及误差校正。

具体的，所述无重叠视角多rgb-d传感器测图系统外方位半自动校正方法包括：

预先通过全站仪获取的棋盘角点三维点集、全站仪所在坐标系和各rgb-d传感器坐标系；

根据所述多rgb-d传感器测图系统中的传感器布局，在多个rgb-d传感器视角及测量范围内的三维结构上附着棋盘图案；

所述多个rgb-d传感器同步采集单帧图像，自动提取对应的棋盘角点序列，并映射到深度图像中获取对应的三维点集；

根据最小二乘原理，依次进行全站仪坐标系与各rgb-d传感器坐标系之间的坐标变换，得到全站仪坐标系与各rgb-d传感器坐标系之间的坐标变换关系，根据所述坐标变换关系将所述三维控制点集转换到各rgb-d传感器坐标系下实现校正。

在一个多rgb-d传感器测图系统中，rgb-d传感器之间的信号干扰是重要考虑因素，当不同的传感器采用相同或者相近波段进行深度测量时，相互之间容易造成信号干扰，降低传感器测图精度。因此本发明采用了多视角无重叠方式构建多rgb-d传感器测图系统，提出了无重叠视角的多rgb-d传感器测图系统外方位半自动校正方法，参照图2，具体描述如下：

a、根据多rgb-d传感器测图系统中的传感器布局，在传感器c1,c2,c3…cn视角及测量范围内的三维结构上附着棋盘图案，所有棋盘角点三维坐标采用全站仪提前获取，获取的棋盘角点三维点集为记全站仪所在坐标系为wk；

b、传感器c1,c2,c3…cn同步采集单帧图像，自动提取对应棋盘角点，并将所述棋盘角点映射到深度图像中获取对应的三维点集，记作

c、根据最小二乘原理，全站仪坐标系wk与传感器c1坐标系wc1之间的坐标变换关系t1可以通过最小化方程获取，见公式(1)

根据全站仪坐标系wk与传感器c1坐标系wc1之间的坐标变换关系t1，可以将所有全站仪获取的棋盘角点三维点集pk转换到wc1坐标系下，将转换后的棋盘角点三维点集记为公式(2)示出了全站仪获取的棋盘角点三维点集pk与wc1坐标系的变换关系t1^*；

其中a为1～n的集合。

e、根据最小二乘原理，获取传感器ci提取的三维点集与经过坐标变换后的棋盘角点三维点集之间的相对变换，由于pk′所在坐标系与wc1一致，如公式(3)所示经过适合的矩阵变换即可计算出传感器ci与传感器c1间的相对变换关系ti1，ti1^*表示是通过最小二乘法获取的ci与传感器c1间的相对变换关系。

为保证计算结果的准确性，一方面需保证单帧校正数据中包含足够的控制点，且具有丰富的几何特征，另一方面将采用不同传感器作为基准传感器，获取最优变换。

s2、获取所述图像数据帧序列中含有的特征匹配点，根据所述特征匹配点构建多rgb-d传感器姿态更新方程，利用所述多rgb-d传感器姿态更新方程得到传感器轨迹；请参阅图3。

与传统的单rgb-d传感器仅采用两帧数据帧用于姿态更新不同，多rgb-d传感器测图系统的姿态更新涉及到多视角观测值融合问题。

多视角观测值融合的传感器追踪包含关键帧检测方法和多维多视角观测值混合优化的姿态更新算法两部分内容。两者相辅相成，前者可为传感器追踪提供可靠、高质量的数据帧，关键帧检测将采用图像模糊度、数据帧基线以及特征点匹配率进行约束；多维多视角观测值混合优化的姿态更新算法将采用局部最小二乘平差的方式最小化相邻关键帧间多维视觉匹配点与几何匹配点混合重投影误差，实现传感器姿态更新。

具体的，请参阅图3(a)，多rgb-d传感器可从多个视角获取图像观测值，在不同位置下，潜在的图像观测值不仅仅存在于同一传感器相邻数据帧中，同时可能存在于不同传感器相邻数据帧中。

具体的，所述特征匹配点包括二三维视觉特征匹配点以及三维几何特征匹配点。

请参阅图3(b)，为了实现多rgb-d传感器测图系统的姿态更新，本发明拟采用多维多视角观测值混合优化的姿态更新算法；其核心思想是在传感器数据帧高精度同步的基础上，获取相邻数据帧集合内所有的视觉图像二三维特征匹配点以及三维集合特征匹配点，将所有的特征点统一到wc1坐标系统，构建多rgb-d传感器姿态更新方程。

具体的，所述步骤s2包括：

s21、提取所述图像数据帧序列的关键帧；

s22、通过所述关键帧的点特征匹配得到二三维视觉特征匹配点；进而根据观测值深度信息的有效性划分为2d观测值、2d-3d观测值与3d观测值；

s23、通过局部三维空间块的描述子建立局部3d数据的对应关系，获取三维几何特征匹配点；

s24、通过最小化二三维视觉特征匹配点的重投影误差与三维几何特征匹配点的距离误差构建多rgb-d传感器姿态更新方程。

具体的，多维多视角观测值混合优化的姿态更新代价函数可由公式(4)表示：

其中，kl为相邻关键帧集合，为所有视觉图像关键帧中获取的二维视觉特征匹配点集，为视觉图像关键帧中获取的三维视觉特征匹配点集，后者可从深度图像中获取到有效的深度值；l代表传感器序号，可从1～n的数值。d为深度关键帧获取的三维几何特征匹配点集。obj为tukeybiweight目标函数，用以增强算法的鲁棒性。

分别为二维视觉特征匹配点和三维视觉特征匹配点的重投影误差，可由公式(5)和公式(6)得到：

为特征点量测噪声，σl2，l3为重投影误差标准差，为几何特征匹配点深度误差，和σd分别对应于几何匹配点深度量测噪声以及误差标准差，i,j也代表传感器序号，可从1～n的数值。最后通过最小化迭代优化关键帧姿态和三维视觉特征匹配点集中的三维坐标以及几何特征匹配点深度值，由公式(7)可得到传感器c1的姿态更新e^u1：

由于其它传感器与c1存在固定不变的相对变换关系，因此可获取同样的姿态更新e^u1，更新后的传感器姿态可表示为公式(8)：

eci′＝ti1·e^u1·ec1(8)

s3、根据所述传感器轨迹建立回环概率模型，基于所述回环概率模型对传感器轨迹进行全局优化，得到优化后的传感器轨迹；

基于投票策略的回环检测方案本质是将当前位置下获取的rgb-d传感器的数据帧集合，与其他关键数据帧进行比较，判断是否回到经过的位置。值得注意的是，本发明涉及的回环检测与单个rgb-d传感器回环检测方法相比存在以下特点：

多rgb-d传感器带来更大数据量，采用纯迭代搜索方式需要耗费大量的时间和空间，因此采用多级检索的方式降低回环搜索复杂度。

多rgb-d传感器测量同一时间产生多帧数据，多个数据帧同一时刻回环检测结果不一致，融合多次检测建立回环概率模型，确定是否闭环。

具体的，本发明采用基于投票策略的回环检测策略，采用基于词袋模型和运动度量联合搜索方法，请参阅图4，该方法分为一级检索，二级检索和回环概率确定三个部分。

具体的，所述步骤s3中根据所述传感器轨迹建立回环概率模型的步骤包括：

s311、采用运动度量的方法，比较关键帧之间的位置和朝向筛选出阈值内的候选关键帧；

一级检索是采用运动度量的方式，其核心在于比较帧与帧之间的位置和朝向是否在一定阈值内。位置上，先对传感器不同时间的位置建立kdtree索引，对当前关键帧位置进行最邻近查询，获取所在邻近范围内的关键数据帧；其次采用角度度量也就是数据帧的朝向进一步筛选出候选关键帧。

s312、采用词袋模型对所述候选关键帧与当前关键帧进行闭环检测，获取闭环概率；

二级检索是采用特征词袋模型，在一级检索的基础上很大程度的降低了回环的搜索范围，因此采用特征词袋模型进行二级检索，将候选关键帧与当前关键帧采用词袋模型进行闭环检测，获取闭环概率。

s313、对多次检测的闭环概率归一化处理，构建多次检测结果的回环概率模型。

考虑到同一时刻多个传感器数据帧在一级和二级检索中都可能检测到闭环，且检测回环得到的概率不一定相同，因此需要对多次检测结果进行归一化，构建多次检测结果的回环概率模型。

具体的，所述步骤s3中所述基于所述回环概率模型对传感器轨迹进行全局优化的步骤包括：

s321、构建用于降低传感器追踪误差累积的全局实时平差网得到全局优化的传感器轨迹；

所述全局实时平差网，包括：回环运动矩阵优化、全局平差网构建、运动模型自由度定权和实时平差。

全局实时平差网构建用于降低传感器追踪误差累积，包括回环运动矩阵优化、全局平差网构建、运动模型自由度定权、实时平差四个部分。

s4、联合图像数据帧序列与所述优化后的传感器轨迹构建室内三角网模型，所述三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图。

首先基于possion算法构建室内三角网模型，联合图像数据帧序列与传感器轨迹实现三角网纹理映射；其次基于octomap算法优化三维点云结构，构建octomap多尺度地图，供机器人或无人机等三维空间导航。最终进行多rgb-d传感器测图系统软硬件适配集成。

为了更清楚的理解本发明提供的一种多rgb-d传感器的空间三维扫描方法，参阅图5。

k1、搭建多rgb-d传感器测图系统，并对其参数进行校正；

校正具体有视觉与深度传感器内外方位校正、无重叠视角的多rgb-d外方位半自动校正和深度传感器测量误差校正；

k2、构建多rgb-d传感器姿态更新方程，实现传感器追踪；

首先提取关键帧，采用混合要素约束的关键帧检测方法，通过图像模糊检测，基线约束，特种点约束对关键帧进行约束；

其次采用多维多视角观测值联合姿态更新算法，通过数据帧实时高精度同步，二三维特征点分类匹配点提取，多视角观测值索引构建，局部重投影误差最小二乘优化构建多rgb-d传感器姿态更新方程，实现传感器追踪，得到传感器轨迹。

k3、建立回环概率模型，优化传感器轨迹；

通过基于词袋模型回环索引构建和运动度量下的回环索引构建，建立基于投票策略的回环概率模型，基于建立的回环概率模型对传感器轨迹进行全局实时平差与轨迹优化，包括，回环运动矩阵优化、全局平差网构建、回环约束自由度定权和实时平差，得到优化后的传感器轨迹。

k4、室内三角网重构与octomap生成；

根据优化后的传感器轨迹联合图像数据帧序列，重构室内三角网模型，三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图，可供机器人或无人机等三维空间导航。最终进行多rgb-d传感器测图系统软硬件适配集成。

基于上述方法实施例，本发明还提供一种多rgb-d传感器的空间三维扫描装置，

一种多rgb-d传感器的空间三维扫描装置，请参阅图6，包括：由多个rgb-d传感器搭建的多rgb-d传感器测图系统10和与所述rgb-d测图系统相连接的数据处理系统20；

所述多rgb-d传感器测图系统10，用于获取图像数据帧序列；

所述数据处理系统20包括：获取传感器轨迹模块201、优化传感器轨迹模块202、三维点云或octomap多尺度地图生成模块203；

所述获取传感器轨迹模块201，用于获取所述图像数据帧序列中含有的特征匹配点，根据所述特征匹配点构建多rgb-d传感器姿态更新方程，利用所述多rgb-d传感器姿态更新方程得到传感器轨迹；

所述优化传感器轨迹模块202，用于根据所述传感器轨迹建立回环概率模型，基于所述回环概率模型对传感器轨迹进行全局优化，得到优化后的传感器轨迹；

所述三维点云或octomap多尺度地图生成模块203，用于联合图像数据帧序列与所述优化后的传感器轨迹构建室内三角网模型，所述三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图。

可选的，所述获取传感器轨迹模块201包括多rgb-d传感器姿态更新单元；

所述多rgb-d传感器姿态更新单元，用于提取所述图像数据帧序列的关键帧；通过所述关键帧的点特征匹配得到二三维视觉特征匹配点；通过局部三维空间块的描述子建立局部3d数据的对应关系，获取三维几何特征匹配点；通过最小化二三维视觉特征匹配点的重投影误差与三维几何特征匹配点的距离误差构建多rgb-d传感器姿态更新方程。

可选的，所述优化传感器轨迹模块202包括回环概率模型构建单元；

所述回环概率模型构建单元，用于采用运动度量的方法，比较关键帧之间的位置和朝向筛选出阈值内的候选关键帧；采用词袋模型对所述候选关键帧与当前关键帧进行闭环检测，获取闭环概率；对多次检测的闭环概率归一化处理，构建多次检测结果的回环概率模型。

综上所述，本发明提供的一种多rgb-d传感器的空间三维扫描方法及装置，所述方法包括：选取多个rgb-d传感器搭建多rgb-d传感器测图系统；所述多rgb-d传感器测图系统获取图像数据帧序列；获取所述图像数据帧序列中含有的特征匹配点，根据所述特征匹配点构建多rgb-d传感器姿态更新方程，利用所述多rgb-d传感器姿态更新方程得到传感器轨迹；根据所述传感器轨迹建立回环概率模型，基于所述回环概率模型对传感器轨迹进行全局优化，得到优化后的传感器轨迹；联合图像数据帧序列与所述优化后的传感器轨迹构建室内三角网模型，所述三角网模型通过octomap算法优化得到三维点云或octomap多尺度地图，解决了多rgb-d传感器slam的关键难点，提高了鲁棒性。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。