红外相机位姿确定方法、设备及系统与流程

本发明涉及定位追踪领域，尤其涉及红外相机位姿确定方法、红外相机位姿确定设备和系统。

背景技术：

追踪系统广泛用于增强现实(augmentedreality，ar)/虚拟现实(virtualreality，vr)人机交互、机器人导航，是最核心、最底层的技术之一。在ar/vr等人机交互领域里，定位和定姿是非常关键的一部分，它是人与机器交互的基础。人机交互对定位和定姿(即确定位姿)的精度要求高(例如到达毫米级、角分级)、实时性要求高(例如10毫秒)。

运动追踪系统可以分为outside-intracking(oit)和inside-outtracking(iot)两类技术。oit的观测基站(摄像头)固定在环境中，而iot的观测基站(摄像头)放置在待追踪物体上。

iot分为基于标记的和不基于标记的技术。不基于标记的iot又称为同时定位及构图(slam)技术，目前还没有商用正式发售的产品，代表产品有microsofthololens、googletango。基于标记的iot由于需要在环境中布满标记，代表产品有华盛顿大学的artoolkit标识系统，还有法国泰雷兹集团(thales)旗下intersenseis1200。

对于oit而言：其成本昂贵，部署困难；其仅仅是3自由度追踪，只能追踪平移位置，而旋转方向需要通过捕捉一个刚体上的多个位置，来计算整个刚体的旋转方向，因此标记体积大，姿态精度差，在多人多点的情况下容易跟踪错误；此外，因为不是接收端本地计算，需要计算完位置之后再通过无线传送给接收端，因此无线传输容易引入延时。

对于基于标记的iot而言：虽然成本低，部署简单，适合于ar/vr/机器人追踪，但需要在环境中布满标记，因此其缺点是标记大导致难部署，黑白标记肉眼可，无法和环境融合十分不美观，从而限制了应用场景；

对于不基于标记的iot而言：虽然不需要在环境中进行任何部署，可随时随地使用，但是其十分不稳定，功耗高，需要很强的计算资源。

技术实现要素：

为解决或缓解上述技术问题的至少一个方面，提出本发明。

根据本发明的实施例的一个方面，提出了一种红外相机位姿确定方法，包括步骤：

获取红外相机在第一位置时相对于空间环境中三维位置已知的三维定位点的位姿，其中，所述三维位置已知的三维定位点构成点云；

在第二位置使用红外相机获取至少四个三维定位点的红外二维图像，其中，三维定位点在红外二维图像中构成码点；和

以红外相机在第一位置时位姿为初值，基于在第二位置获取的红外二维图像，获取红外相机在第二位置的位姿。

可选的，所述红外相机的红外线帧频至少为60hz。

可选的，获取红外相机在第一位置时位姿包括如下步骤：

使用红外相机在第一位置获取至少四个三维定位点的第一帧二维图像；

获取第一帧二维图像中所述至少四个三维定位点所对应的码点的二维位置；

获取第一帧二维图像中码点对应的所述至少四个三维定位点的三维位置；以及

基于码点在第一帧二维图像中的二维位置以及所述至少四个三维定位点的三维位置，获得红外相机在第一位置时的位姿。

可选的，获取红外相机在第二位置的位姿的步骤包括：

使用红外相机在第二位置获取第二帧二维图像，所述第二帧二维图像中包括至少四个三维定位点对应的码点；

获取第二帧二维图像中所述至少四个三维定位点对应的码点的二维位置；

基于点云以及红外相机在第一位置时位姿，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置；

基于第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置及在第二帧二维图像中的二维位置，获得红外相机在第二位置的位姿。

进一步可选的，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的步骤包括：

由所述红外相机在第一位置时位置向第二帧二维图像中的至少四个码点引出直线；以及

将所述点云中与对应的直线分别匹配的三维定位点的三维位置分别作为第二帧二维图像中直线经过的至少四个码点对应的至少四个三维定位点的三维位置。

或者进一步可选的，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的步骤包括：

由所述点云中相关的三维定位点朝向红外相机的位置引出直线；以及

找出所述第二帧二维图像的码点中与引出的直线分别匹配的码点，将与码点匹配的直线所对应的三维定位点的三维位置分别作为第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置。

可选的，获取红外相机在第二位置的位姿的步骤包括：

使用红外相机在第二位置获取所述至少四个三维定位点的第二帧二维图像；

获取第二帧二维图像中码点的二维位置；

选定第二帧二维图像的码点中与获取红外相机在第一位置时位姿所使用的三维定位点在第一帧二维图像中的码点相对应的至少四个码点；以及

基于在第二帧二维图像中选定的所述至少四个码点在第二帧二维图像中的二维位置以及选定的所述至少四个码点所对应的三维定位点的三维位置，获得红外相机在第一位置时的位姿。

可选的，上述方法还包括步骤：

使用红外相机在第三位置获取至少四个三维定位点的第三帧二维图像；

获取第三帧二维图像中所述至少四个三维定位点所对应的码点的二维位置；

获取第三帧二维图像中至少四个码点对应的所述至少四个三维定位点的三维位置；以及

基于码点在第三帧二维图像中的位置以及所述至少四个三维定位点的三维位置，获得红外相机在第三位置时的位姿其将其作为位姿初值。

可选的，空间环境中布置有多个光学定位标记，每一个光学定位标记包括：多个色块构成的颜色编码区域，所述多个色块为可见光色块且依序排布以形成颜色编码；和定位部，用于反射红外光或者吸收红外光，所述定位部设置在颜色编码区域围成的区域内或者在颜色编码区域附近，其中：定位部包括三维定位点，颜色编码构成所述三维定位点在环境空间中的位置id，所有三维定位点构成点云，每一个三维定位点有着对应的位置id；获取三维定位点的三维位置的步骤包括获得颜色编码区域的图像以及读取该颜色编码以获取三维定位点的位置id从而获取三维定位点的三维位置。

可选的，所述第一位置为红外相机的标定位置；获取红外相机在第一位置时位姿的步骤包括利用rgb相机获得颜色编码区域的图像以及读取该颜色编码以获取三维定位点的位置id。

可选的，所述第三位置为红外相机的标定位置；获取红外相机在第三位置的位姿的步骤包括利用rgb相机获得颜色编码区域的图像以及读取该颜色编码以获取三维定位点的位置id。

根据本发明的实施例的另一方面，提出了一种红外相机位姿确定设备，包括：

获取红外相机在第一位置时相对于空间环境中的三维位置已知的三维定位点的位姿的位置标定装置，其中，所述三维位置已知的三维定位点构成点云；

用于在第二位置使用红外相机获取至少四个三维定位点的红外二维图像的红外二维图像获取装置，其中，三维定位点在红外二维图像中构成码点；和

以红外相机在第一位置时的位姿为初值、基于在第二位置获取的红外二维图像、获取红外相机在第二位置的位姿的位置跟踪装置。

可选的，所述红外相机的红外线帧频至少为60hz。

可选的，所述红外相机用于在第一位置获取至少四个三维定位点的第一帧二维图像；所述位置标定单元包括：

用于确定第一帧二维图像中所述至少四个三维定位点所对应的码点的二维位置的码点位置分析部；

用于获取第一帧二维图像中码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部；以及

基于码点在第一帧二维图像中的二维位置以及所述至少四个三维定位点的三维位置、获得红外相机在第一位置时位姿的位姿确定部。

可选的，所述红外相机用于在第二位置获取第二帧二维图像，所述第二帧二维图像中包括至少四个三维定位点对应的码点；

所述位置跟踪装置包括：

基于点云以及红外相机在第一位置时位姿，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部；

获取第二帧二维图像中所述至少四个三维定位点对应的码点的二维位置的码点位置分析部；

基于第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置及在第二帧二维图像中的二维位置、获得红外相机在第二位置时位姿的位姿确定部。

进一步可选的，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部包括：

由所述红外相机在第一位置时位置向第二帧二维图像中的至少四个码点引出直线的装置；以及

将所述点云中与对应的直线分别匹配的三维定位点的三维位置分别作为第二帧二维图像中直线经过的至少四个码点对应的至少四个三维定位点的三维位置的装置。

或者进一步可选的，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部包括：

由所述点云中相关的三维定位点朝向红外相机的位置引出直线的装置；以及

找出所述第二帧二维图像的码点中与引出的直线分别匹配的码点、且将与码点匹配的直线所对应的三维定位点的三维位置分别作为第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的装置。

可选的，所述红外相机用于在第二位置获取所述至少四个三维定位点的第二帧二维图像；

所述位置跟踪装置包括：

用于确定第二帧二维图像中码点的二维位置的码点位置分析部；

选定第二帧二维图像的码点中与获取红外相机在第一位置时位姿所使用的三维定位点在第一帧二维图像中的码点相对应的至少四个码点的装置；以及

基于在第二帧二维图像中选定的所述至少四个码点在第二帧二维图像中的二维位置以及选定的所述至少四个码点所对应的三维定位点的三维位置、获得红外相机在第一位置时位姿的位姿确定部。

可选的，所述红外相机用于在第三位置获取至少四个三维定位点的第三帧二维图像；

所述位置跟踪装置包括：

用于获取第三帧二维图像中所述至少四个三维定位点所对应的码点的二维位置的码点位置分析部；

用于获取第三帧二维图像中至少四个码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部；以及

基于码点在第三帧二维图像中的位置以及所述至少四个三维定位点的三维位置、获得红外相机在第三位置时位姿以及将该位姿作为初值的位姿确定部。

位置标定装置包括深度相机，所述深度拥挤用于获取三维定位点的三维位置。

根据本发明的实施例的再一方面，提出了一种红外相机位姿确定系统，包括：

上述的设备；

空间环境中布置的多个光学定位标记，每一个光学定位标记包括：

多个色块构成的颜色编码区域，所述多个色块为可见光色块且依序排布以形成颜色编码；和

定位部，用于反射红外光或者吸收红外光，所述定位部设置在颜色编码区域围成的区域内或者在颜色编码区域附近，

其中：

定位部包括三维定位点，颜色编码构成所述三维定位点在环境空间中的位置id，每一个三维定位点有着对应的位置id；

所述系统还包括rgb相机，所述rgb相机用于获得颜色编码区域的图像；

对应的定位点位置分析部读取该颜色编码以获取三维定位点的位置id从而获取三维定位点的三维位置。

附图说明

以下描述与附图可以更好地帮助理解本发明所公布的各种实施例中的这些和其他特点、优点，图中相同的附图标记始终表示相同的部件，其中：

图1为根据本发明的一个示例性实施例的红外相机位姿确定方法的流程图；

图2为根据本发明的另一个示例性实施例的红外相机位姿确定方法的流程图；

图3为第二帧二维图像中码点的示意图；

图4为根据本发明的一个实施例的获取第二帧二维图像中的三维定位点的三维位置的方式的示意图；

图5为根据本发明的另一个实施例的获取第二帧二维图像中的三维定位点的三维位置的方式的示意图；

图6为根据本发明的一个实施例的获取第二帧二维图像中的新三维定位点的方式的示意图；

图7为根据本发明的另一个实施例的获取第二帧二维图像中的新三维定位点的方式的示意图；

图8为根据本发明的一个示例性实施例的光学定位标记的示意图；

图9为根据本发明的另一个示例性实施例的光学定位标记的示意图；

图10为根据本发明的一个示例性实施例的红外相机位姿确定设备的示意图；

图11为根据本发明的一个示例性实施例的位置标定装置的示意图；

图12为根据本发明的一个示例性实施例的位置跟踪装置的示意图；

图13为根据本发明的另一个示例性实施例的位置跟踪装置的示意图；

图14为根据本发明的再另一个示例性实施例的位置跟踪装置的示意图；

图15为根据本发明的一个示例性实施例的红外相机位姿确定系统的示意图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。在说明书中，相同或相似的附图标号指示相同或相似的部件。下述参照附图对本发明实施方式的说明旨在对本发明的总体发明构思进行解释，而不应当理解为对本发明的一种限制。

实施例1

图1为根据本发明的一个示例性实施例的红外相机位姿确定方法的流程图。如图1所示，根据本发明的红外相机位姿确定方法，包括步骤：

获取红外相机在第一位置时相对于空间环境中的三维位置已知的三维定位点的位姿，其中，所述三维位置已知的三维定位点构成点云；

在第二位置使用红外相机获取至少四个三维定位点的红外二维图像，其中，三维定位点在红外二维图像中构成码点；和

以红外相机在第一位置时位姿为初值，基于在第二位置获取的红外二维图像，获取红外相机在第二位置的位姿，。

可选的，为了实现帧与帧之间的快速传播，所述红外相机的红外线帧频至少为60hz，更进一步的，至少为100hz。

不过需要指出的是，在动作较慢的情况下，红外帧频即使是10hz也是够用的。

需要说明的是，在本发明中，“获取红外相机在第二位置/第一位置的位姿”可以利用现有技术中已知的方法，例如使用solvepnp算法或者posit算法，只要是能够用二维到三维的映射关系，求出相机在当前位置的位姿的方法都在本发明的保护范围之内。为了便于说明，在本发明的实施例中，以使用solvepnp算法作为示例来获取相机的位姿。

需要说明的是，这里的“以红外相机在第一位置时位姿为初值”表示利用获取到的红外相机在第一位置时的位姿的数据，这里的数据包括了红外相机的三维位置(即相机的光学机构的光心所在的位置)、旋转姿态(即红外相机的成像平面的旋转姿态)、获得该三维位置以及旋转姿态时所用到的至少四个三维定位点的三维位置以及对应的第一位置时获得的二维图像或者说第一位置时每个码点的图像坐标，在实际使用中，可以是上述数据中的一部分，也可以是全部。例如，在第二位置获取的红外二维图像中的码点如果在第一位置获取的红外二维图像中也出现，则该码点对应的三维定位点的三维位置可以在计算第二位置时红外相机的位姿时直接利用；再如，因为红外线帧频较高，现实中第一位置与第二位置之间的距离很小，因此，即使红外相机在第二位置也可以使用其在第一位置时的三维位置来反算或推演第二位置时获得的红外二维图像中的码点所对应的三维定位点的三维位置。

还需要说明的是，这里的“基于在第二位置获取的红外二维图像”表示利用了红外二维图像所承载的数据，例如其中的码点在二维图像中的二维位置或二维坐标。

下面具体说明如何获取红外相机在第一位置时的位姿。获取红外相机在第一位置时的位姿包括如下步骤：

使用红外相机在第一位置获取至少四个三维定位点的第一帧二维图像；

获取第一帧二维图像中所述至少四个三维定位点所对应的码点的二维位置；

获取第一帧二维图像中码点对应的所述至少四个三维定位点的三维位置；以及

基于码点在第一帧二维图像中的二维位置以及所述至少四个三维定位点的三维位置，利用solvepnp算法获得红外相机在第一位置时的位姿。

需要说明的是，获取码点对应的三维定位点的三维位置，可以利用深度相机；也可以通过读取码点对应的id，该id与码点对应的三维定位点的三维位置对应；还可以采用直接获取三维坐标已知的至少四个三维定位点的二维图像的方式获取红外相机的初始位姿，这些都在本发明的保护范围之内。

还需要说明的是，在本发明中，确定相机的位姿实际上是确定其镜头光心的三维位置以及成像平面的姿态。

还需要说明的是，获取码点对应的三维定位点的三维位置，不仅可以是如下面的附图2中左半部所示在标定红外相机的初始位置时获取，也可以是图2中右半部所示在跟踪红外相机的三维位置的过程中获取。

可以利用红外相机在第一位置时的位置信息，基于码点在第二帧二维图像中的二维位置，获取红外相机在第二位置时的位姿。下面具体说明如何获取红外相机在第二位置时的位姿。

在一个具体的示例性实施例中，参见图4、5，获取红外相机在第二位置的位姿的步骤包括：

使用红外相机在第二位置获取第二帧二维图像，如图3所示，所述第二帧二维图像中包括第一帧二维图像中没有出现的新三维定位点对应的码点b1以及第一帧二维图像中已知三维位置的至少三个旧三维定位点对应的码点a1、a2、a3；

获取第二帧二维图像中所述至少三个旧三维定位点所对应的码点a1、a2、a3以及所述新三维定位点所对应的码点b1的二维位置；

基于点云以及红外相机在第一位置时的位姿，获取第二帧二维图像中码点b1对应的所述新三维定位点的三维位置；以及

基于新三维定位点和所述至少三个旧三维定位点的三维位置及在第二帧二维图像中对应的码点a1、a2、a3、b1的二维位置，利用solvepnp算法获得红外相机在第二位置的位姿。

需要说明的是，图3中的二维图像中仅仅是示意性的示出了四个码点，实际上，一帧二维图像中的码点数可以不止四个，例如三个旧码点以及两个或三个新码点。

在一个示例性实施例中，获取第二帧二维图像中码点对应的所述新三维定位点的三维位置可以采用将二维图像中的码点投影到点云所在平面的追踪方式，其步骤包括：获得由所述红外相机的第一位置以及第二帧二维图像中的所述新三维定位点对应的码点所确定的直线与所述至少三个旧三维定位点确定的平面的交点；以及将所述点云中与所述交点距离最近的三维定位点的三维位置作为所述新三维定位点的三维位置。如图4中所示，从红外相机在第一位置的三维位置o朝向二维图像p2中的新码点b1引出一条射线l，该射线l与旧码点a1、a2、a3所对应的三维定位点d1、d2、d3确定的平面相交，点云所在平面中与该交点o1最近或重合(或者两者之间的距离在阈值之内)的三维定位点d4可以认定为新码点所对应的三维定位点。

还可以进行校正。具体的，利用pnp计算相机的位姿以后，利用相机姿态重新计算第二帧图像中的码点投影到三维点云所在的平面得到新的三维位置，计算其与码点对应的三维定位点之间的距离，利用阈值过滤掉错误的匹配，不断迭代，直到投影误差最小。需要注意的是，“将所述点云中与所述交点距离最近的三维定位点的三维位置作为所述新三维定位点的三维位置”也可以包括上述的迭代步骤。

在一个示例性实施例中，获取第二帧二维图像中码点对应的所述新三维定位点的三维位置可以采用将旧码点所对应的三维定位点朝向红外相机投影到二维图像上的追踪方式，其步骤包括：将所述点云(由旧码点a1、a2、a3所对应的三维定位点d1、d2、d3确定的平面中的三维定位点)朝向红外相机投影到所述第二帧二维图像中；以及将所述点云投影到第二帧二维图像而形成的码点中与所述新三维定位点对应的码点距离最近的码点所对应的三维定位点的三维位置作为所述新三维定位点的三维位置。如图5所示，由旧码点a1、a2、a3所对应的三维定位点d1、d2、d3确定的平面中的三维定位点d4、d5等均朝向红外相机在第一位置时的三维位置o投影在二维图像p2上，三维定位点d4、d5在二维图像上的码点分别是a4、a5，在二维图像中，码点a4与b1位置最近或重合(或者两者之间的距离在阈值之内)，从而确定新码点b1对应的三维定位点为d4。还可以进行校正。具体的，利用pnp计算相机的位姿以后，利用相机姿态重新计算三维定位点投影到二维图像平面得到新的二维点位置，计算其与码点之间的距离，利用阈值过滤掉错误的匹配，不断迭代，直到重投影误差最小。“将所述点云投影到第二帧二维图像而形成的码点中与所述新三维定位点对应的码点距离最近的码点所对应的三维定位点的三维位置作为所述新三维定位点的三维位置”也可以包括上述的迭代步骤。

在实际应用中，图4和图5中描述的技术方案可以结合使用。

在根据本发明的另一个示例性实施例中，获取红外相机在第二位置的位姿的步骤可以包括：

使用红外相机在第二位置获取第二帧二维图像，如图3所示，所述第二帧二维图像中包括至少四个三维定位点对应的码点；

基于点云以及红外相机在第一位置时位置，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置；

获取第二帧二维图像中所述至少四个三维定位点对应的码点的二维位置；

基于第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置及在第二帧二维图像中的二维位置，利用solvepnp算法获得红外相机在第二位置的位姿。

在更具体的实施例中，参见图6，可以将二维图像中所有码点投影到点云中或点云所在平面中以与点云中的三维定位点分别匹配，从而得到二维图像中所有码点对应的三维定位点的三维坐标。又，此时因为所有码点在二维图像中的二维位置已知，从而可以利用solvepnp算法获得相机的位姿。获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的步骤包括：参照图6，获得由所述红外相机在第一位置时位置o以及第二帧二维图像中的所述至少四个三维定位点对应的码点a1、a2、a3、a4所确定的直线分别与点云确定的平面的交点o1、o2、o3、o4；以及将所述点云中与对应的交点距离分别最近或重合(或者两者之间的距离在阈值之内)的三维定位点d1、d2、d3、d4的三维位置分别作为第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置。

相应的，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的步骤包括：

由所述点云中相关的三维定位点朝向红外相机的位置引出直线；以及

将所述点云中与对应的直线分别匹配的三维定位点的三维位置分别作为第二帧二维图像中直线经过的至少四个码点对应的至少四个三维定位点的三维位置。

还可以进行校正。具体的，利用pnp计算相机的位姿以后，利用相机位姿重新计算第二帧图像中的码点投影到三维点云所在的平面得到新的三维位置，计算其与码点对应的三维定位点之间的距离，利用阈值过滤掉错误的匹配，不断迭代，直到投影误差最小或者在阈值以内。需要注意的是，“将所述点云中与对应的直线分别匹配的三维定位点的三维位置分别作为第二帧二维图像中直线经过的至少四个码点对应的至少四个三维定位点的三维位置”可以包括上述的迭代步骤。

在图6的示例中，利用引出的直线与点云所在平面的交点，通过交点与三维定位点之间的距离来确定最接近的三维定位点。不过，在有些情况下，点云可能由多个平面中的定位点构成，此时，也可以通过确定与引出的直线距离最近来确定三维定位点。此外，在有的情况下，不仅要考虑某个直线(或交点)与某个三维定位点的最佳匹配(如距离最近)，还需要考虑全局的最佳匹配，例如匹配的直线与三维定位点之间的距离之和最小。在不能获得匹配的情况下，可以重新标定红外相机的位姿。

或者可选的，在更具体的实施例中，参见图7，可以将点云投影到二维图像中，点云投影得到的码点与二维图像中原本的码点进行匹配，匹配到的码点所对应的三维定位点即为与点云投影得到的码点匹配的码点所对应的三维定位点。又，此时因为所有码点在二维图像中的二维位置已知，从而可以利用solvepnp算法获得相机的位姿。获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的步骤包括：参照图7，将所述点云朝向红外相机投影到所述第二帧二维图像中；以及将所述点云投影到第二帧二维图像而形成的码点c1、c2、c3、c4、c5等中与所述至少四个三维定位点(包括d1、d2、d3、d4、d5等)对应的码点a1、a2、a3、a4、a5分别距离最近或重合(或者两者之间的距离在阈值之内)的码点c1、c2、c3、c4、c5所对应的三维定位点的三维位置分别作为第二帧二维图像中码点a1、a2、a3、a4对应的所述至少四个三维定位点d1、d2、d3、d4的三维位置。

相应的，获取红外相机在第二位置的位姿的步骤包括：

使用红外相机在第二位置获取所述至少四个三维定位点的第二帧二维图像；

获取第二帧二维图像中码点的二维位置；

选定第二帧二维图像的码点中与获取红外相机在第一位置时位姿所使用的三维定位点在第一帧二维图像中的码点相对应的至少四个码点；以及

基于在第二帧二维图像中选定的所述至少四个码点在第二帧二维图像中的二维位置以及选定的所述至少四个码点所对应的三维定位点的三维位置，利用solvepnp算法获得红外相机在第二位置时的位姿。

还可以进行校正。具体的，利用pnp计算相机的位姿以后，利用相机姿态重新计算三维定位点投影到二维图像平面得到新的二维点位置，计算其与码点之间的距离，利用阈值过滤掉错误的匹配，不断迭代，直到重投影误差最小。“基于在第二帧二维图像中选定的所述至少四个码点在第二帧二维图像中的二维位置以及选定的所述至少四个码点所对应的三维定位点的三维位置，利用solvepnp算法获得红外相机在第二位置时的位姿”也可以包括上述的迭代步骤。

在图7的示例中，利用引出的直线与成像平面或二维图像的交点，通过交点与码点之间的距离来确定最接近的码点。不过，在有些情况下，不仅要考虑某个直线(或交点)与某个码点的最佳匹配(如距离最近)，还需要考虑全局的最佳匹配，例如匹配的直线与码点之间的距离之和最小。在不能获得匹配的情况下，可以重新标定红外相机的位姿。

在实际应用中，图6和图7的方案可以结合使用。

以上描述了第二帧二维图像中出现了新的码点且需要借助于新的码点确认红外相机的当前位姿时如何获取新码点对应的三维定位点的三维位置的情形。当然，即使没有出现新的码点，也可以采用图6和7中的方案获取二维图像中码点所对应的三维定位点的三维位置。

实际情况中，也存在在第一位置和第二位置，红外相机获取的第二帧红外二维图像中旧的码点足够获得红外相机在第二位置时的位姿的情形，下面具体描述在此情况下获取红外相机在第二位置的位姿的另一种方式，在该情况下，红外相机在第二位置的位姿的获取不需要基于红外相机在第一位置时的三维位置，具体的，获取红外相机在第二位置的位姿的步骤包括：

使用红外相机在第二位置获取所述至少四个三维定位点的第二帧二维图像；

获取第二帧二维图像中码点的二维位置；

选定第二帧二维图像的码点中与获取红外相机在第一位置时位姿所使用的三维定位点在第一帧二维图像中的码点相对应的至少四个码点；以及

基于在第二帧二维图像中选定的所述至少四个码点在第二帧二维图像中的二维位置以及选定的所述至少四个码点所对应的三维定位点的三维位置，利用solvepnp算法获得红外相机在第二位置时的位姿。

在上述的选定步骤中，因为红外帧频较高，至少四个码点在第一帧二维图像中的位置与在第二帧二维图像中的位置变化较小，可以认为第二帧二维图像中的码点距离第一帧二维图像中的码点在一个阈值范围内的情况下(可以认为是利用第一位置时二维图像或者以第一位置时每个码点的图像坐标作为初值)，认定这两个码点对应着同一个三维定位点。

需要注意的是，在上述步骤中，即使与计算红外相机在第二位置时的位姿不相关，但是为了后续使用，可以利用例如图4和图5中所示的方法获取第二帧红外二维图像中的新码点所对应的三维定位点的三维位置。

如图2所示，倘若追踪红外相机的位姿失败，或者换言之，不能以红外相机在第一位置时的位姿为初值获取红外相机在第二位置时的位置(例如，第一位置和第二位置距离相差太大，第一位置无法作为第二位置的初值，或者第一位置的码点在第二位置中出现的数目小于3)，则需要重新标定红外相机的初始位姿。此时，具体的，上述的红外相机位姿确定方法还包括步骤：

使用红外相机在第三位置获取至少四个三维定位点的第三帧二维图像；

获取第三帧二维图像中所述至少四个三维定位点所对应的码点的二维位置；

获取第三帧二维图像中至少四个码点对应的所述至少四个三维定位点的三维位置；以及

基于码点在第三帧二维图像中的位置以及所述至少四个三维定位点的三维位置，利用solvepnp算法获得红外相机在第三位置时的位姿且将其作为位姿初值。

需要专门指出的是，这里的第三位置可以是对应不能从第一位置时的位姿初值获得位姿的第二位置，也可以是不同于第二位置的其他位置。基于上述重新标定红外相机位姿初值的步骤，可以重新获取红外相机的位姿。如前所述，获取第三帧二维图像中码点对应的三维定位点的三维位置，可以利用深度相机，也可以通过读取码点对应的id，该id与码点对应的三维定位点的三维位置对应。

在图2中，主要是利用rgb图像进行编码点的解码获取相机的初始位置，再利用红外图像快速定位码点位置并进行追踪。利用帧间的连续性获得码点的可靠位置追踪以及三维位置传播，当获得匹配三维位置的码点数目较少时，利用全局三维地图(点云)和前一帧相机的位姿，将三维定位点投影到二维图像上，迭代优化求解出码点对应的三维位置。由于红外相机的高帧频，因此相邻帧之间的相机位置差异较小，基于这个假设，相机的姿态可以在帧间传播作为初值。红外工作模式下，如果码点追踪失败，则相机切到rgb模式，直到成功获得相机的姿态后切换回红外模式。

在本发明中，在涉及到rgb识别的情况下，红外相机实际上包括了rgb识别功能和红外照相功能。rgb识别功能和红外照相功能可以集成到红外相机中，也可以红外相机实际上包括了两个相机，一个执行红外照相功能，一个执行rgb照相功能，系统会根据当前的状态选择使用哪一个相机。在rgb识别功能和红外照相功能集成到红外相机的情况下，可以通过控制器来控制输出不同类型的数据以实现rgb模式和红外模式的切换，也可以通过使用过滤片的选择达到切换工作模式的效果。

在根据本发明的一个示例性实施例中，三维定位点可以通过光学定位标记实现。具体的：

空间环境中布置有多个光学定位标记，如图8、图9所示，每一个光学定位标记100包括：

多个色块构成的颜色编码区域20，所述多个色块为可见光色块且依序排布以形成颜色编码；和

定位部10，用于反射红外光或者吸收红外光，所述定位部设置在颜色编码区域围成的区域内(参见图8)或者在颜色编码区域附近(参见图9)，

其中：

定位部包括三维定位点(例如图8中的十)，颜色编码构成所述三维定位点在环境空间中的位置id，所有三维定位点构成点云，每一个三维定位点有着对应的位置id；

获取三维定位点的三维位置的步骤包括获得颜色编码区域的图像以及读取该颜色编码以获取三维定位点的位置id从而获取三维定位点的三维位置。

需要指出的是，位置id与三维定位点之间不一定是一一对应的关系，有可能一个id对应多个三维位置(原因是编码资源不够，比如一共有200个不同的编码id，但是需要部署3000个点，就必须有重复的)。但是有办法可以在具体某一图像中，非歧义的通过id得到唯一的三维位置，比如同一图像中的码点对应的三维位置都很近。

需要专门指出的是，颜色编码区域可以与光反射部是一体的，也可以独立于定位部设置。例如，在定位部已经设置在墙体上的情况下，可以围绕该定位部再设置颜色编码区域。再如，定位部和颜色编码区域一体设置，从而可以一并粘贴到墙体上。

下面具体描述光学定位标记100的实施例。

如图8所示，中间包含十字的小圆为反光点或光反射部或定位部10的位置，该小圆为颜色编码区域的内边界，最外层的黑色圆环为颜色编码区域的外边界，中间的含有不同颜色扇环的圆环则为颜色编码区域20。不同的颜色表示不同的码。需要指出的是，该十字也可以并不存在；另外，定位部也可以设置为覆盖小圆的圆心即可；最外层的圆环可以是黑色，也可以是其他颜色，当然最好是与颜色编码区域中的颜色不同的颜色，有利的，最外层的圆环的颜色与背景环境的颜色不同。

最外圈的颜色信息可以快速、准确的确认光学定位标记的外形和轮廓，便于快速、准确的确认光学定位标记的圆心位置。

在解码时，首先可以找到基准色块，例如图8中的白色区域块，然后按照顺时针(当然也可以按照逆时针)的方向依次读取其它颜色块的颜色。以图8为例，中间的圆环被划分成了6份，每个码所占的区域为1/6个圆环(60度扇环)。图中有红(r)、蓝(b)、绿(g)、白(w)4种颜色，分别表示0，1，2，3四种码。从白色区域开始，顺时针进行编码，则上图中的颜色编码为320100。关于颜色编码的设计，如果确定了中间的颜色块的数目n，颜色的组合数目c，则总共可以产生c^n-1个编码，因为每个颜色块可以取c种颜色，而基准颜色块的颜色则为固定的一种特殊的颜色，主要是用来确定解码的顺序，在图4的实施例中，基准块的颜色不能与其它颜色块的颜色相同，否则会产生一个颜色编码产生多种解码方式，破坏了颜色编码解码时的唯一性。需要说明的是，之所以需要明确基准色块的颜色不同于其他色块，是因为色块构成了一个环形时基准色块并不适于用位置来确定，换言之，在色块构成环形的颜色编码区域的情况下，基准色块通过颜色来选定。

在图8的实施例中，中间的颜色块的设计是可扩展的，可以体现在以下方面：

1.颜色的选取，可以选取多种分辨度比较高的颜色组合进行编码。可以在多种颜色空间里进行选择，例如rgb，hsv颜色空间选取。例如选取纯红(r＝255，g＝0，b＝0)，纯蓝(r＝0，g＝0，b＝255)，纯绿(r＝0，g＝255，b＝0)三种颜色组合，基准颜色例如选取纯白(r＝255，g＝255，b＝255)。

2.中间颜色块的划分数目，数目越多，最终产生的编码数目也越多，主要取决于实际三维场景需要部署的编码点数目。例如，颜色块为4，颜色组合数为3，则产生27个编码，当颜色块数目增加到6，颜色组合数为3，则产生243个编码。

还需要说明的是，图8中的扇形色块也可以是不等分圆环的形式，此时，每一个色块或每一个编码点并非以角度确定，而是直接以色块的颜色确定。例如，在此情况下，图8中右上角的两个红色块就看做是一个色块。

下面参照图8示例性简单描述如何获得光学定位标记的反光点。首先，红外相机得到的是灰度图像，反光点位置处比较亮，灰度值高，利用阈值法可以把不是反光点的像素给过滤掉。然后，对反光点的像素进行聚类，得到反光点在图像中的连通区域。最后，根据连通区域，拟合圆形或椭圆，利用圆心近似求解反光点的质心。

图9为根据本发明的一个示例性实施例的光学定位标记的示意图。在图9中，整个光学定位标记大致为长条形，在颜色编码区域20，每一个色块为直的色条。相邻的色条之间可彼此邻接且颜色不同。还可以利用色条的宽度来判别不同的色条。定位部10可以设置在光学定位标记的任意位置，可选的，设置在长条形光学定位标记的一端，如图9所示。其中，颜色编码区域包括位于光学定位标记长度方向上的一端的单个基准色块，颜色编码自所述单个基准色块按预定顺序开始。图9中，其颜色编码可以为0212。

需要说明的是，定位点的颜色编码的设计有多种方式，只要能够在rgb图像中可靠识别出来即可。此外，反光点的形状和大小设计不唯一，只需要能够被红外捕获且形状质心可以精确求解即可。

本发明的方案中，在红外模式下，码点或定位点的编码的设计比较灵活，可以设计得很小，且质心求解比较准确，定位的精度更高。此外，本发明中，定位点的部署比较简单，具体的，本发明中使用的定位点的编码特征明显，彼此之间的具有高可分辨性，部署的时候不需要规则部署。

在本发明的方案中，采用颜色对定位点编码，使得三维建模自动化成为可能。由于定位点附近具有丰富的高可分辨的色彩信息，因此可以利用数码相机对实际三维场景进行采样拍摄，部署的定位点能够利用其周边的颜色被轻易识别出来，而且还能够在不同图像之间找到对应关系。可以利用运动结构恢复(sfm，structurefrommotion)的方法自动求解出这些定位点的三维位置，而不需要去人工测量这些定位点之间的距离。

如图2所示，在一个示例性实施例中，所述第一位置为红外相机的标定位置；获取红外相机在第一位置时的位姿的步骤包括利用rgb相机获得颜色编码区域的图像以及读取该颜色编码以获取三维定位点的位置id。

如前面所述的，所述第三位置可以为红外相机的标定位置；此时，获取红外相机在第三位置时的位姿的步骤包括利用rgb相机获得颜色编码区域的图像以及读取该颜色编码以获取三维定位点的位置id。

本发明提出了一种基于红外rgb相机的光学定位方法，利用相机捕获的rgb颜色对定位点或码点进行快速提取与识别，并在数据库中检索出码点对应的三维点位置，利用多组对应关系求得相机初始姿态，再利用红外的高帧频，帧与帧之间的码点追踪，不断更新相机的姿态信息，从而能够在高精度的定位定姿下保证很小的时延。

在本发明的实施例中，可以利用rgb图像进行编码点的解码获取初始相机的位置，再利用红外图像快速定位码点位置并进行追踪。如此，本发明rgb图像只是用来提供相机初始姿态，相机绝大部分时间是在红外模式下工作，而红外的帧频要远高于rgb图像的帧频(同等分辨率下)，因此本发明的帧频主要取决于红外的帧频，高帧频的红外相机可以保证较低时延的定位。从而本发明的技术方案中定位定姿的时延小。

在本发明中，在红外工作模式下，关于码点的追踪，有以下三种追踪方式：1.相邻帧之间的码点追踪，由红外二维图像到红外二维图像中码点的追踪来获得三维定位点与码点的匹配。2.由红外二维图像中的码点到三维数据库中点(或点云中三维定位点)的匹配，获得码点与三维点的匹配。3.由三维数据库的点(或点云中的三维定位点)到红外二维图像的映射，来获得三维定位点与码点的匹配。当然上述三种方式可以自由组合，来获得更佳的码点追踪效果。具体的：

在追踪方式1中，利用帧间的连续性，直接得到当前帧与前一帧之间的码点的对应关系，由于前一帧图像已经记录了图像中的码点与数据库中三维点的对应关系，进而可以利用码点之间的匹配来得到当前帧码点与数据库中三维点的匹配关系，如果得到的匹配数目较少，则需要进行全局匹配，即利用方式2或方式3进行匹配，或者只针对新出现的码点(即未找到与三维定位点匹配关系的码点)采用方式2或方式3进行匹配。因为随着相机的行进，图像中各个码点之间的位置关系发生变化，基于该位置变化，利用solvepnp算法求出相机的实际位姿。

在追踪方式2中，从红外相机的位置到红外二维图像中码点的位置引一条射线，与空间的平面进行求交，得到空间三维点的位置，然后去数据库(或点云)的三维定位点中寻找与其距离最近的三维定位点，或者直接找到距离该射线距离最近的三维定位点，得到匹配的关系。

在追踪方式3中，将数据库中的三维定位点(或点云中的三维定位点)根据红外相机内参数(例如焦距)和外参数(旋转和平移)反投影回红外二维图像平面，然后在红外二维图像里找到与其距离最近的二维码点，得到匹配关系。

下面参照图2具体描述红外相机位姿确定方法。

首先，对于标定步骤，即获取红外相机在第一位置时相对于空间环境中的三维位置已知的三维定位点的位姿的步骤：

如图2所示，包括：相机在第一位置捕获rgb图像；然后进行rgb图像处理，识别或提取码点的颜色编码以获得颜色编码信息；利用颜色编码信息在数据库(对应于点云)中检索出码点对应的三维定位点的三维坐标；获取码点在rgb图像中的二维位置；最后，基于码点的二维位置以及码点对应的三维定位点的三维位置，利用solvepnp或pnp获得红外相机的初始位姿信息。

倘若标定不成功，则由相机继续捕获rgb图像接着进行分析。倘若标定成功，则由红外相机在第二位置捕获帧红外图像，帧红外图像中包括了三维定位点对应的码点，可以提取码点中的反光点，可以将反光点作为三维定位点在红外图像中的位置点并获得反光点在红外图像中的二维位置。

之后，以红外相机在第一位置时的位姿为初值，利用solvepnp算法迭代优化求解红外相机在第二位置时的位姿。

倘若获得了第二位置时的位姿，则可以获得第三位置时的红外图像，以红外相机在第二位置时的位姿为初值，利用solvepnp算法迭代优化求解红外相机在第三位置时的位姿。倘若不能获得第三位置或者第二位置时相机的位置，则相机再获取rgb图像，以对相机的位置进行标定(即获得相机的初始位姿)。

实施例2

图10为根据本发明的一个示例性实施例的红外相机位姿确定设备的示意图。如图10所示，红外相机位姿确定设备1000包括：

获取红外相机在第一位置时相对于空间环境中的三维位置已知的三维定位点的位姿的位置标定装置200，其中，所述三维位置已知的三维定位点构成点云；

用于在第二位置使用红外相机获取至少四个三维定位点的红外二维图像的红外二维图像获取装置300，其中，三维定位点在红外二维图像中构成码点；和

以红外相机在第一位置时的位姿为初值、基于在第二位置获取的红外二维图像、利用solvepnp算法获取红外相机在第二位置的位姿的位置跟踪装置400。

可选的，为了实现帧与帧之间的快速传播，所述红外相机的红外线帧频至少为60hz，更进一步的，至少为100hz。

不过需要指出的是，在动作较慢的情况下，红外帧频即使是10hz也是够用的。

红外二维图像获取装置300可以对应于红外相机中获取红外图像的部分。

在一个可选的实施例中，所述红外相机用于在第一位置获取至少四个三维定位点的第一帧二维图像；如图11所示，所述位置标定装置200包括：

用于确定第一帧二维图像中所述至少四个三维定位点所对应的码点的二维位置的码点位置分析部210；

用于获取第一帧二维图像中码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部220；以及

基于码点在第一帧二维图像中的二维位置以及所述至少四个三维定位点的三维位置、利用solvepnp算法获得红外相机在第一位置时位姿的位姿确定部230。

可选的，所述红外相机用于在第二位置获取第二帧二维图像，所述第二帧二维图像中包括至少四个三维定位点对应的码点；如图12所示，位置跟踪装置400包括：

获取第二帧二维图像中所述至少四个三维定位点对应的码点的二维位置的码点位置分析部410；

基于点云以及红外相机在第一位置时位姿，获取第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部420；以及

基于第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置及在第二帧二维图像中的二维位置、利用solvepnp算法获得红外相机在第二位置时位姿的位姿确定部430。

可选的，如图12所示，所述定位点位置分析部包括定位点位置分析单元422。在图6的方式中，所述定位点位置分析单元用于：由所述红外相机在第一位置时位置向第二帧二维图像中的至少四个码点引出直线；以及将所述点云中与对应的直线分别匹配的三维定位点的三维位置分别作为第二帧二维图像中直线经过的至少四个码点对应的至少四个三维定位点的三维位置。或者，在图7的方式中，所述定位点位置分析单元用于：由所述点云中相关的三维定位点朝向红外相机的位置引出直线；以及找出所述第二帧二维图像的码点中与引出的直线分别匹配的码点、且将与码点匹配的直线所对应的三维定位点的三维位置分别作为第二帧二维图像中码点对应的所述至少四个三维定位点的三维位置。

在一个可选的实施例中，所述红外相机用于在第二位置获取所述至少四个三维定位点的第二帧二维图像；如图13所示，所述位置跟踪装置400包括：

用于确定第二帧二维图像中所述至少四个三维定位点所对应的码点的二维位置的码点位置分析部410’；

选定第二帧二维图像的码点中与获取红外相机在第一位置时位姿所使用的三维定位点在第一帧二维图像中的码点相对应的至少四个码点的选定装置440；以及

基于在第二帧二维图像中选定的所述至少四个码点在第二帧二维图像中的二维位置以及选定的所述至少四个码点所对应的三维定位点的三维位置、利用solvepnp算法获得红外相机在第一位置时位姿的位姿确定部430’。

在一个可选的实施例中，所述红外相机用于在第三位置获取至少四个三维定位点的第三帧二维图像，如图14所示，所述位置跟踪装置400包括：

用于获取第三帧二维图像中所述至少四个三维定位点所对应的码点的二维位置的码点位置分析部410”；

用于获取第三帧二维图像中至少四个码点对应的所述至少四个三维定位点的三维位置的定位点位置分析部420”；以及

用于基于码点在第三帧二维图像中的位置以及所述至少四个三维定位点的三维位置、利用solvepnp算法获得红外相机在第三位置时位姿以及将该位姿作为初值的位姿确定部430”。

可选的，虽然没有示出，所述位置标定装置包括深度相机，所述深度拥挤用于获取三维定位点的三维位置。

图15为根据本发明的一个示例性实施例的红外相机位姿确定系统的示意图。如图15所示，红外相机位姿确定系统包括：

上述的红外相机位姿确定设备1000；

空间环境中布置的多个光学定位标记100，如图8、9所示，每一个光学定位标记包括：

多个色块构成的颜色编码区域20，所述多个色块为可见光色块且依序排布以形成颜色编码；和

定位部10，用于反射红外光或者吸收红外光，所述定位部设置在颜色编码区域围成的区域内或者在颜色编码区域附近，

其中：

定位部10包括三维定位点，颜色编码构成所述三维定位点在环境空间中的位置id，每一个三维定位点有着对应的位置id；

所述系统还包括rgb相机500，所述rgb相机500用于获得颜色编码区域的图像；

对应的定位点位置分析部用于读取该颜色编码以获取三维定位点的位置id从而获取三维定位点的三维位置。

需要说明的是，实施例1中的描述也适用于实施例2。

还需要说明的是，虽然在实施例中将红外相机位姿确定设备1000描述为包括位置标定装置200、红外二维图像获取装置300、位置跟踪装置400，但是这三个装置可以共用相同的装置，换言之，这三个装置可以由相同的装置的不同功能部分实现。相应的，定位点位置分析部虽然附图标记不同，也可以是同一个定位点位置分析部；位姿确定部虽然附图标记不同，也可以是同一个位姿确定部；码点位置分析部虽然附图标记不同，也可以是同一个码点位置分析部。在涉及红外相机位姿确定设备及系统的权利要求中，相应的装置也应该如此理解。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行变化，本发明的范围由所附权利要求及其等同物限定。