一种轻量级ToF传感器与RGB图像传感器的标定方法

一种轻量级tof传感器与rgb图像传感器的标定方法
技术领域
1.本发明属于计算机视觉技术领域，特别涉及一种轻量级tof传感器与rgb图像传感器的标定方法。


背景技术：

2.无人驾驶、智能机器人以及混合现实等领域需要实现对环境的精确感知。通常，在感知时会融合使用多种传感器，来提升感知的精度。然而，融合使用多传感器的首要前提就是各传感器的联合标定，以保证各传感器在空间上的坐标统一。
3.轻量级tof传感器是一种较为新型的深度传感器，如stmicroelectronics公司的flightsense系列下的vl53l0，vl53l1，vl53l3和vl53l5等，这类传感器是为了实现简单的手势识别，自动对焦，简单的障碍物检测等应用而设计，具有低功耗、低成本的特点。与一般意义的tof(time-of-flight)传感器相比，这类轻量级tof传感器的主要区别在于这类轻量级tof传感器由于轻量级的设计初衷，其分辨率特别低(一般不超过10x10)，而一般意义的tof传感器通常具有几万以上的分辨率。同时轻量级的tof传感器每个像素测量的是一个区域的场景深度分布，因此具有较大的不确定性，而一般意义的tof传感器每个像素测量的是一个精确的三维位置对应的深度值。
4.尽管这类轻量级的tof传感器不具备测量高分辨率、稠密的场景深度信息，但是相比于基于结构光的深度传感器或者一般意义的tof(time-of-flight)传感器，其价格和功耗至少低一到两个数量级，因此在一些对功耗、成本有较为严格要求的移动端场景上得到广泛的应用。另外如果将该传感器与分辨率较高的图像传感器结合起来，形成一个多传感器系统，就可以进一步提升感知的精度和分辨率，同时由于图像传感器拥有同样低水平的功耗和价格，这套多传感器系统的功耗与成本不会明显增加。当然，组成这套多传感器系统需要首先解决该轻量级tof传感器和图像传感器的联合标定问题。针对轻量级tof传感器的特点，本发明提出了一种轻量级tof传感器与rgb图像传感器的标定方法，解决了轻量级tof由于测量深度具有空间不确定性而不便与rgb图像传感器联合标定的问题。


技术实现要素：

5.本发明提出了一种轻量级tof传感器与rgb图像传感器的标定方法。轻量级tof传感器在测量时不能获得某一精确点的深度信息，而是会返回区域内深度的平均值与方差，这使得传统标定方法在rgb图像传感器与轻量级tof传感器联合标定的过程中不再适用。针对轻量级tof传感器的这一特点，本发明恰当选择了具有结构信息的标定环境，利用平面结构的连续性解决了这一标定问题。方法通过在三个以上不互相平行的面结构上粘贴标定纸，并使用rgb图像传感器和轻量级tof传感器分别采集二维图像信息和深度信息，使用slam系统将rgb图像传感器采集到的二维图像生成每张图像的位姿和对应地图点云，并通过em算法从轻量级tof传感器测量的深度分布数据中估计场景平面，这样将标定问题转化为点云地图和平面地图之间的对齐问题，再利用非线性优化的方法求解这一问题，就实现
了rgb图像传感器与轻量级tof传感器的联合标定。
6.为了实现上述目的，本发明采用如下技术方案：
7.本发明提供了一种轻量级tof传感器与rgb图像传感器的标定方法，其包括以下步骤：
8.步骤1：保持rgb图像传感器、轻量级tof传感器的相对固定状态，将固定后的整体称为待标定装置；
9.步骤2：选取一个合适的标定场景，要求场景中有至少三个不互相平行的面；通过待标定装置的rgb图像传感器和轻量级tof传感器分别采集场景每一个平面的rgb图像数据和深度分布数据；
10.步骤3：对于rgb图像传感器录制的数据，使用视觉slam系统处理rgb图像传感器拍摄的图片，生成每张图片在世界坐标系下的位姿，并对场景进行点云重建；由于标定环境为至少三个不互相平行的面，从重建的点云中将这些面提取出来，并将点云中所有有效点按其所归属的平面分类；
11.步骤4：对于轻量级tof传感器采集的深度分布信息，首先根据其视野范围和传感区域分布情况，按照测量的深度均值位于测量区域中心的假设对深度值进行反投，生成对应的点云并拟合出当前帧坐标系下的场景平面方程；然后利用em算法迭代优化测量的深度均值在测量区域的准确位置以及当前帧坐标系下的场景平面方程；最后将每一帧对应的场景平面信息保存；
12.步骤5：构建优化问题，求解一组最优的旋转平移矩阵r和t，使得在利用r和t将轻量级tof传感器生成的每一帧平面与rgb图像传感器生成的点云对齐后，点云中的点到对应平面的残差总和最小，求得的旋转平移矩阵r和t即为需要计算的标定结果。
13.现有的标定技术需要假设已知深度值的像素位置，因此不适用于轻量级tof传感器与rgb图像传感器的标定。本发明针对轻量级tof传感器测量的是区域内的平均深度与方差而不是某一精确点的深度信息这一特点恰当选择了具有结构信息的标定环境，利用平面结构的连续性通过em算法迭代优化深度点的像素位置与场景平面方程从而解决了这一标定问题。本发明的标定方法可用于同时搭载了轻量级tof传感器和rgb图像传感器的硬件设置，用来对齐轻量级tof传感器测量的深度分布数据与rgb图像传感器采集的rgb图像数据。该装置具备相当低的价格和功耗，在对齐两种传感器的测量数据后同时兼具rgb图像传感器的高分辨率与轻量级tof传感器的深度测量信息，可以应用于混合现实、机器人等领域。
附图说明
14.图1是本发明的实施例中使用的装置图；
15.图2是本发明的联合标定方法流程图；
16.图3是本发明中rgb图像传感器和轻量级tof传感器联合标定的准备状态示意图；
17.图4是本发明中使用em算法提取轻量级tof传感器观测区域对应主平面的流程图。
具体实施方式
18.本发明的一种rgb图像传感器与轻量级tof传感器的标定方法，首先使用rgb图像传感器和轻量级tof传感器分别采集二维图像信息和深度分布信息，然后使用视觉slam系
统将rgb图像传感器采集到的二维图像生成每张图像的位姿和对应地图点云，并通过em算法从轻量级tof传感器测量的深度分布数据中估计场景平面，最后通过rgb图像传感器生成的点云和轻量级tof传感器生成的场景平面之间对齐来计算得到轻量级tof传感器和rgb图像传感器之间的旋转平移矩阵，从而实现rgb图像传感器与轻量级tof的数据的联合标定。
19.本发明提出的标定方法具体包括以下步骤：
20.步骤1：保持rgb图像传感器、轻量级tof传感器的相对固定状态，将固定后的整体称为待标定装置；
21.步骤2：选取一个合适的标定场景，要求场景中有至少三个不互相平行的面；通过待标定装置的rgb图像传感器和轻量级tof传感器分别采集场景每一个平面的深度分布数据和rgb图像数据；
22.步骤3：对于rgb图像传感器录制的数据，使用视觉slam系统处理rgb图像传感器拍摄的图片，生成每张图片在世界坐标系下的位姿，并对场景进行点云重建；由于标定环境为至少三个不互相平行的面，从重建的点云中将这些面提取出来，并将点云中所有有效点按其所归属的平面分类；
23.步骤4：对于轻量级tof传感器采集的深度分布信息，首先根据其视野范围和传感区域分布情况，按照测量的深度均值位于测量区域中心的假设对深度值进行反投，生成对应的点云并拟合出当前帧坐标系下的场景平面方程；然后利用em算法迭代优化测量的深度均值在测量区域的准确位置以及当前帧坐标系下的场景平面方程；最后将每一帧对应的场景平面信息保存；
24.步骤5：构建优化问题，求解一组最优的旋转平移矩阵r和t，使得在利用r和t将轻量级tof传感器生成的每一帧平面与rgb图像传感器生成的点云对齐后，点云中的点到对应平面的残差总和最小，求得的旋转平移矩阵r和t即为需要计算的标定结果。
25.作为本发明的优选方案，步骤2中三个及以上不互相平行的面可以是自然场景，也可以是人工场景。
26.作为本发明的优选方案，步骤3中，首先使用ransac算法依次提取出点云中的平面，具体做法为：使用ransac拟合场景中的一个平面，编号为0，记录下该平面方程，然后在点云中去除该步拟合平面的所有内点，对剩下的点重复该拟合平面的步骤，逐步拟合得到场景中其它剩余平面，并得到各平面方程。假设场景中有3个不互相平行的面，则本发明先拟合场景中的一个平面0，再逐步拟合得到平面1、平面2。
27.作为本发明的优选方案，步骤4中首先按照测量的深度均值位于测量区域中心的假设，从轻量级tof传感器测量的深度分布中恢复平面；由于该假设并不总是成立，因此这一步恢复的平面并不十分准确；但是由平面的连续性，可知在所测区域内存在一点的深度等于轻量级tof传感器所测深度均值；因此，在求解得到平面后，利用em算法交替优化每个区域内深度均值的像素位置和当前帧下的场景平面方程，得到更加准确的平面方程。
28.作为本发明的优选方案，步骤4中，利用em算法交替优化每个区域内深度均值的像素位置和场景平面方程，具体为：
29.4.1)假设待优化的当前帧下的场景平面参数为{n,d}，第k个区域内深度等于tof测量的深度均值的像素点位置为(xk,yk)，则列写得到需要联合优化的损失项为：
[0030][0031][0032]
其中z表示轻量级tof传感器测量区域的集合，是第k个测量区域的边界范围，mk是第k个测量区域的深度均值，k则是轻量级tof传感器的内参属性，可以由传感器的视野范围和分辨率计算而来；
[0033]
将所有测量区域深度均值的像素点位置初始化为测量区域的中心点；
[0034]
4.2)根据深度均值对这些二维位置进行反投影得到若干个三维点，然后拟合出对应的平面；
[0035]
4.3)调整每个区域中深度均值的像素点位置，来最小化反投的三维点到前一步拟合的平面的距离；
[0036]
4.4)迭代运行步骤4.2)和4.3)，直到收敛；在迭代过程中，对距离平面太远的外点做丢弃处理。
[0037]
作为本发明的优选方案，步骤5中首先要确定每一帧tof所拍摄平面和rgb图像传感器重建出的平面的对应关系；随后，针对每一帧数据，首先将rgb图像传感器重建的点云通过步骤3中得到的逐帧位姿将其由世界坐标系转换到rgb图像传感器坐标系中；此时，tof观测数据所拟合得到的平面和rgb图像传感器数据重建的点云分别在各自坐标系中，两者只相差从rgb图像传感器坐标系到轻量级tof传感器坐标系的旋转平移矩阵，该矩阵即为所要求的标定结果；为求得该矩阵，使用点到平面的对齐的方式来构建一个优化问题，即求得一组r,t使得下式得到最小值
[0038][0039]
其中，r为rgb图像传感器坐标系到轻量级tof传感器坐标系的旋转矩阵，t为rgb图像传感器坐标系到轻量级tof传感器坐标系的平移向量：f表示拍摄的帧的集合，pi表示通过视觉slam系统重建出的点云中第i帧rgb图像传感器拍摄到的主平面内的所有点；p为pi内的点的三维空间坐标，ni和di为第i帧轻量级tof传感器测量区域对应主平面的平面方程系数，分别为平面法向方向和平面到原点的偏置距离；上式通过非线性优化方法，迭代求解。
[0040]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明的技术方案进行清楚、完整地描述。所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0041]
实施例
[0042]
如图1所示，本发明搭建了一套同时搭载了rgb图像传感器和轻量级tof传感器的装置来验证本发明的效果。该装置中rgb图像传感器使用的是intel realsense d435i中的彩色摄像头，轻量级tof传感器使用的是stmicroelectronics公司的vl53l5型号。如图2所示，本实施例实现rgb图像传感器与轻量级tof传感器的联合标定，主要包括如下步骤：
[0043]
步骤1：将rgb图像传感器、轻量级tof传感器保持相对固定的安装，轻量级tof传感
器采用多区域测量版本，分辨率为8*8。
[0044]
步骤2：选取一个合适的标定环境，要求环境中有三个不互相平行的面，本实施例中选取房间中的一无杂物放置的由两面墙壁和地面构成的角落，在墙壁和地面上粘贴标定纸以增加纹理信息，如图3所示。分别记这三个面为a、b、c面。首先手持待标定装置对准a面，依次上下左右缓慢移动待标定装置，在移动过程中同时伴有俯仰角的少量变化。在录制a面数据时使得视野范围内的主要拍摄物为a面。在a面录制完成后，镜头平稳地旋转切换到b面，并按照与a面相同的要求进行录制，录制完成后平稳地旋转切换到c面，并按相同要求录制。a、b、c面均录制完成后，数据采集工作完成。
[0045]
步骤3：对于rgb图像传感器录制的数据，使用视觉slam系统处理rgb图像传感器拍摄的图片，生成每张图片在世界坐标系下的位姿，并对场景进行点云重建。在本例中，使用开源视觉slam算法orb-slam2对rgb图像传感器拍摄的二维图像进行处理，处理后将每张照片的位姿和整体场景的点云地图保存下来。由于标定环境为三个不互相平行的面，可以从重建的点云中将这三个面提取出来，并将点云中所有有效点按其所归属的平面分为三类。这里从点云中提取平面主要使用ransac算法，依次从点云中提取三个平面。操作的具体步骤为：
[0046]
3.1平面提取使用开源算法库open3d中的segment_plane方法。平面提取时，ransac参数设置为：n＝3,distance_threshold＝0.02,迭代次数1000次。
[0047]
3.2使用算法依次提取出点云中的三个平面，具体做法为：使用ransac拟合场景中的第一个平面，编号为0，记录下该平面的方程，然后在点云地图中去除该步拟合平面的所有内点。随后，对剩下的点重复该步骤，继续得到平面1，平面2的方程。至此，3个平面的方程提取完成。
[0048]
3.3对点云地图中所有属于某个平面的内点进行标记，记录该点和所属平面的对应关系。
[0049]
步骤4：对于轻量级tof传感器采集的深度分布信息，首先根据其视野范围和传感区域分布情况，按照测量的深度均值位于测量区域中心的假设对深度值进行反投，生成对应的点云并拟合出当前帧坐标系下的场景平面方程；然后利用em算法迭代优化测量的深度均值在测量区域的准确位置以及当前帧坐标系下的场景平面方程，并将每一帧得到的场景平面方程保存。如图4所示，操作的具体步骤为：
[0050]
4.1按照轻量级tof传感器的视野角范围和区域分布数量生成投射方向，投射方向为轻量级tof传感器中心到其每个测量区域的中心连线的射线方向，与tof包含的接收区域数目相同。
[0051]
4.2读取解析轻量级tof传感器测得的深度分布数据，对于每个分布数据，在其对应的区域的射线方向上进行投射，即从轻量级tof传感器的光心出发，沿该方向向前运动测得的深度均值距离后到达的点记录为该区域对应的三维测量点。对所有的区域依次进行相同的处理，就将轻量级tof传感器测量得到的多个区域的深度分布数据转化为了对应的点云。
[0052]
4.3使用步骤三中所用的ransac平面拟合算法从轻量级tof传感器生成的点云数据中提取平面。根据步骤二中拍摄数据的要求，同一时刻轻量级tof传感器观察到的环境内容主体为环境中三个不互相平行的面中的一个，因此，运行平面拟合算法后会提取出对应
的平面方程。
[0053]
4.4轻量级tof传感器测得的深度并不是某一具体点的深度，而是区域范围内的深度均值和方差。但是由平面的连续性，可知在所测区域内存在一点的深度等于轻量级tof传感器所测深度。因此，在步骤4.3求解得到初始估计的场景平面后，继续使用em算法交替优化每个区域内深度等于轻量级tof传感器测量的深度均值的点的像素位置和场景平面方程。
[0054]
4.5对轻量级tof传感器记录的每一帧数据进行上述操作，并将对应平面方程存储下来。
[0055]
步骤5：构建优化问题，求解一组最优的旋转平移矩阵r和t，使得在利用r和t将轻量级tof传感器生成的每一帧平面与rgb图像传感器生成的地图对齐后，rgb图像传感器生成的地图点到对应轻量级tof传感器生成的平面的残差总和最小，求得的旋转平移矩阵r和t即为需要计算的标定结果。操作的具体步骤为：
[0056]
5.1观看步骤二中录制的标定数据，并记录rgb图像传感器分别拍摄三个平面的时间戳，以确定每一帧轻量级tof传感器所拍摄平面和rgb图像传感器重建出的平面的对应关系。这里将拍摄平面的时间戳连同平面的序号一同记录在一个文本文件中。
[0057]
5.2针对每一帧数据，首先将rgb图像传感器重建的点云通过步骤三中得到的逐帧位姿将其由世界坐标系转换到rgb图像传感器坐标系中，实施方法为分别对点云中每个点的齐次坐标左乘上从世界坐标系到rgb图像传感器坐标系的变换矩阵，如下式所示：pc＝t
cw
pw。其中pc表示变换后在rgb图像传感器坐标下的点，pw表示变换前世界坐标系下的点，t
cw
为在步骤三中所求得的从世界坐标系到rgb图像传感器坐标系的变换矩阵。
[0058]
5.3经过变换后，轻量级tof传感器观测数据所拟合得到的平面和rgb图像传感器数据重建的点云分别在各自坐标系中，两者只相差从rgb图像传感器坐标系到轻量级tof传感器坐标系的旋转平移矩阵，该矩阵即为所要求的标定结果。为求的该矩阵，使用点到平面的对齐的方式来构建一个优化问题，即求得一组r,t使得下式得到最小值，其中r为rgb图像传感器坐标系到轻量级tof传感器坐标系的旋转矩阵，t为rgb图像传感器坐标系到轻量级tof传感器坐标系的平移向量：
[0059][0060]
其中f表示拍摄的帧的集合，pi表示通过视觉slam系统重建出的点云中第i帧rgb图像传感器拍摄到的主平面内的所有点。ni和di为第i帧轻量级tof传感器测量区域对应主平面的平面方程系数，分别为平面法向方向和平面到原点的偏置距离。即要求：
[0061][0062]
上式可通过非线性优化方法，迭代求解，得到最终标定结果。如图1所示装置通过本发明的联合标定方法可以标定出两个传感器坐标系之间相差近90度的旋转矩阵以及接近5厘米的平移矩阵。标定前rgb图像传感器生成的地图点到对应轻量级tof传感器生成的平面的平均残差是7.5厘米，通过标定得到的旋转平移矩阵对齐后，该残差下降到1.5厘米。
[0063]
以上列举的仅是本发明的具体实施例。显然，本发明不限于以上实施例，还可以有
许多变形。本领域的普通技术人员能从本发明公开的内容直接导出或联想到的所有变形，均应认为是本发明的保护范围。