CN113643378B - 多相机环境中主动式刚体的位姿定位方法及相关设备 - Google Patents

Abstract

本发明涉及计算机视觉技术领域，尤其涉及一种多相机环境中主动式刚体的位姿定位方法及相关设备。该方法包括：获取相邻两帧的二维空间点坐标、二维空间点编码，计算三维空间点编码和三维空间点坐标；将所有三维空间点编码和三维空间点坐标转化为刚体坐标；通过求解位姿估计，确定刚体的初始位姿；利用梯度下降法，使用重投影误差构造代价函数，并使代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对刚体跟踪定位。本发明采用上述定位方式后，不但精简了传统光学动捕相机复杂器件结构，降低了相机成本，而且刚体不易磨损和毁坏，使用可持续性大大提升。

Description

多相机环境中主动式刚体的位姿定位方法及相关设备

技术领域

本发明涉及计算机视觉技术领域，尤其涉及一种多相机环境中主动式刚体的位姿定位方法及相关设备。

背景技术

传统的光学动捕方法是通过动捕相机内的超大功率近红外光源发出红外光，照射在被动式标记点上；涂有高反光材料的标记点反射被照射到的红外光，而这部分红外光和带有背景信息的环境光会经过低畸变镜头，到达摄像机红外窄带通滤光单元。由于红外窄带通滤光单元的通光波段跟红外光源的波段一致，因此，带有冗余背景信息的环境光会被过滤掉，只剩下带有标记点信息的红外光通过，并被摄像机感光元件记录。感光元件再将光信号转化为图像信号输出到控制电路，而控制电路中的图像处理单元使用现场可编程门阵列(Field Programmable Gate Array，FPGA)，以硬件形式对图像信号进行预处理，最后向跟踪软件流出标记点的2D坐标信息。跟踪定位软件采用计算机多目视觉原理，根据图像二维点云间的匹配关系及相机的相对位置和朝向，计算点云在三维捕捉空间内的坐标及方向。以点云的三维坐标为基础，跟踪定位软件通过识别不同的刚体结构，解算出每个刚体在捕捉空间内的位置及朝向。

以上被动式动捕方法存在以下缺点：

第一，要求动捕相机具有比较复杂的图像处理器件，相机造价相对较高；

第二，要求标记点涂有高反光材料，在使用过程中易造成磨损，影响系统正常运行；

第三，跟踪定位依赖于刚体的结构，刚体的设计使得刚体数量很局限，而且刚体的识别和跟踪需要相机捕捉到刚体上全部标记点，使用环境非常严苛。

发明内容

本发明的主要目的在于提供一种多相机环境中主动式刚体的位姿定位方法及相关设备，旨在解决目前被动式动捕方法中对动捕相机要求较高、跟踪定位依赖于刚体结构等技术问题。

为实现上述目的，本发明提供一种多相机环境中主动式刚体的位姿定位方法，所述方法包括以下步骤：

获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；

将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；

将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；

通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿；

获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。

可选地，所述将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标，包括：

将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，利用多视几何中的三角测量原理，通过奇异值分解求解最小二乘法方法，解算得到一个三维空间点，遍历所有两两匹配的相机后，得到一组三维空间点，一组所述三维空间点即为所述标记点的三维空间点坐标；

判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；

计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；

将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。

可选地，所述将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标，包括：

计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；

分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标；

将所述标记点的三维空间点编码赋值给所述刚体坐标对应的编码，得到所述标记点的刚体编码。

可选地，所述通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿，包括：

求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计时，将所述三维空间点坐标和所述刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，所述等式如下公式：

P1＝RP1′+T

其中，P1为第一帧所述三维空间点坐标，P1′为第一帧所述刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；

根据所述欧式变换旋转矩阵和所述平移矩阵得到刚体的初始位姿。

进一步地，为实现上述目的，本发明还提供一种多相机环境中主动式刚体的位姿定位装置，包括：

获取数据模块，用于获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；

计算三维空间数据模块，用于将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；

计算刚体数据模块，用于将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；

确定刚体初始位姿模块，用于通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿；

刚体定位模块，用于获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。

可选地，所述计算三维空间数据模块，包括：

计算三维空间点坐标组单元，用于将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，利用多视几何中的三角测量原理，通过奇异值分解求解最小二乘法方法，解算得到一组三维空间点坐标；

剔除单元，用于判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；

确定三维空间点坐标单元，用于计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；

确定三维空间点编码单元，用于将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。

可选地，所述计算刚体数据模块，还包括：

计算原点单元，用于计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；

确定刚体坐标单元，用于分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标；

确定刚体编码单元，用于将所述标记点的三维空间点编码赋值给所述刚体坐标对应的编码，得到所述标记点的刚体编码。

可选地，所述确定刚体初始位姿模块，包括：

求解矩阵单元，用于求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计时，将所述三维空间点坐标和所述刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，所述等式如下公式：

P1＝RP1′+T

其中，P1为第一帧所述三维空间点坐标，P1′为第一帧所述刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；

根据所述欧式变换旋转矩阵和所述平移矩阵得到刚体的初始位姿。

为实现上述目的，本发明还提供一种多相机环境中主动式刚体的位姿定位设备，所述设备包括：存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的多相机环境中主动式刚体的位姿定位程序，所述多相机环境中主动式刚体的位姿定位程序被所述处理器执行时实现如上所述的多相机环境中主动式刚体的位姿定位方法的步骤。

为实现上述目的，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有多相机环境中主动式刚体的位姿定位程序，所述多相机环境中主动式刚体的位姿定位程序被处理器执行时实现如上所述的多相机环境中主动式刚体的位姿定位方法的步骤。

本发明提供的多相机环境中主动式刚体的位姿定位方法，在对刚体位姿进行定位过程中，首先根据捕获的二维空间点数据，计算三维空间点数据，其中空间点数据包括空间点编码和坐标，根据多个三维空间点数据组成一个刚体，并将这些三维空间点数据的空间坐标转化为刚体坐标系下的刚体坐标，根据三维空间点坐标和刚体坐标计算刚体的初始位姿，结合相机参数求出刚体的运动位姿，根据初始位姿和运动位姿最后对刚体进行跟踪定位。本发明采用上述定位方式后，不但精简了传统光学动捕相机复杂器件结构，降低了相机成本，而且刚体不易磨损和毁坏，使用可持续性大大提升。最为重要的是，对刚体的跟踪定位依据空间点数据，而不再对刚体结构进行约束，这不仅使得刚体结构统一化，美观方面大大优化，而且编码状态的多样性促使可识别刚体数量成倍增加。

附图说明

通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。

图1为本发明实施例方案涉及的多相机环境中主动式刚体的位姿定位设备的运行环境的结构示意图；

图2为本发明一个实施例中多相机环境中主动式刚体的位姿定位方法的流程图；

图3为本发明一个实施例中步骤S2的细化流程图；

图4为本发明一个实施例中步骤S3的细化流程图；

图5为本发明一个实施例中多相机环境中主动式刚体的位姿定位装置的结构图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。

参照图1，为本发明实施例方案涉及的多相机环境中主动式刚体的位姿定位设备运行环境的结构示意图。

如图1所示，该多相机环境中主动式刚体的位姿定位设备包括：处理器1001，例如CPU，通信总线1002、用户接口1003，网络接口1004，存储器1005。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard)，网络接口1004可选的可以包括标准的有线接口、无线接口(如WI-FI接口)。存储器1005可以是高速RAM存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。

本领域技术人员可以理解，图1中示出的多相机环境中主动式刚体的位姿定位设备的硬件结构并不构成对多相机环境中主动式刚体的位姿定位设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。

如图1所示，作为一种计算机可读存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及多相机环境中主动式刚体的位姿定位程序。其中，操作系统是管理和控制多相机环境中主动式刚体的位姿定位设备和软件资源的程序，支持多相机环境中主动式刚体的位姿定位程序以及其它软件和/或程序的运行。

在图1所示的多相机环境中主动式刚体的位姿定位设备的硬件结构中，网络接口1004主要用于接入网络；用户接口1003主要用于侦测确认指令和编辑指令等，而处理器1001可以用于调用存储器1005中存储的多相机环境中主动式刚体的位姿定位程序，并执行以下多相机环境中主动式刚体的位姿定位方法的各实施例的操作。

参照图2，为本发明一个实施例中的多相机环境中主动式刚体的位姿定位方法的流程图，如图2所示，一种多相机环境中主动式刚体的位姿定位方法，包括以下步骤：

步骤S1，获取数据：获取多个相机捕捉的相邻两帧的二维空间点坐标、二维空间点坐标对应的二维空间点编码和多个相机的空间位置数据，将二维空间点编码相同的多个二维空间点坐标分为同类，且标记于同一个标记点下。

本步骤的标记点一般都设置在刚体的不同位置，通过多个相机捕捉标记点的二维空间坐标信息，通过预设的刚体编码技术，确定出空间点数据，空间点数据包括二维空间点坐标及对应的二维空间点编码。空间位置数据是由通过标定计算得到各相机的空间位置关系得到的。通常，在刚体上设有八个标记点，标记点可以是八个发光的LED灯。因此刚体通常包含八个空间点数据，在多个相机在捕捉到的信息中，单个相机每帧数据中都包含八个标记点的空间点数据，同一个标记点在不同帧时的编码是相同的，不同的标记点在同帧时的编码是不同的。基于此可以把所有相机中带有相同空间点编码的空间点数据划分在一起作为同类，并认为这些空间点数据是空间中同一个标记点在不同相机上的投影。

步骤S2，计算三维空间数据：将多个相机两两进行匹配，根据两个相机的空间位置数据及同类同帧的多个二维空间点坐标，得到每个标记点每帧的三维空间点编码和三维空间点坐标。

对每个标记点的每帧数据分别进行本步骤的处理，处理时将捕捉到此标记点的多个相机两两进行匹配，利用多视几何中的三角测量原理，通过奇异值分解(Singular ValueDecomposition，SVD)求解最小二乘法解算得到一组三维空间点数据。

例如，刚体包括八个标记点时，通过本步骤得到八个标记点的八个三维空间点编码和三维空间点坐标。

在一个实施例中，如图3所示，步骤S2进一步包括：

步骤S201，求解最小二乘法：将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个二维空间点坐标，利用多视几何中的三角测量原理，通过奇异值分解(SVD)求解最小二乘法方法，解算得到一个三维空间点，遍历所有两两匹配的相机后，得到一组三维空间点，一组三维空间点即为标记点的三维空间点坐标。

设两个相机分别是相机1和相机2，在同帧中捕捉到的两个二维空间点坐标分别为A(a1，a2)，B(b1，b2)，相机1的旋转矩阵为R1(R11，R12，R13)，R1是3*3的矩阵，平移矩阵为T1(T11，T12，T13)，T1是3*1的矩阵，相机2的旋转矩阵为R2(R21，R22，R23)，平移矩阵为T2(T21，T22，T23)，同样地，R2是3*3的矩阵，平移矩阵T2是3*1的矩阵，通过下述公式得到同帧中的一个三维空间点坐标C(c1，c2，c3)：

1)根据两个相机的内参和畸变参数，将像素坐标A(a1，a2)，B(b1，b2)转化为相机坐标A′(a1′，a2′)，B′(b1′，b2′)；

2)构造最小二乘法矩阵X和Y，其中X是4*3的矩阵，Y是4*1的矩阵；X矩阵第一行为a1′*R13-R11，X矩阵第二行为a2′*R13-R12，X矩阵第三行为b1′*R23-R21，X矩阵第四行为b2′*R23-R22；Y矩阵第一行为T11-a1′*T13，Y矩阵第二行为T12-a2′*T13，Y矩阵第三行为T21-b1′*T23，Y矩阵第四行为T22-b2′*T23。

3)根据等式X*C＝Y和已经构造好的矩阵X、Y，利用SVD分解即可以求得一个三维空间点坐标C。

本步骤最终将所有两两匹配的相机捕捉到的两个二维空间点坐标均通过上述方式进行解算，得到一组三维空间点，该组三维空间点即为标记点的三维空间点坐标。

步骤S202，剔除阈值外坐标：判断三维空间点坐标是否处于预设的阈值范围内，若超过阈值范围，则剔除三维空间点坐标，得到剔除后的一组三维空间点坐标。

在得到多个三维空间点坐标后，需要检查这些三维空间点坐标是否处于预设的阈值范围内，即较小的阈值距离，此阈值范围是提前预设的坐标参数。若发现三维空间点坐标偏离阈值范围，则认为此三维空间点坐标是错误数据，进行剔除。

步骤S203，计算平均值：计算一组三维空间点坐标的平均值，通过高斯牛顿法优化，得到标记点的三维空间点坐标。

将剔除错误数据后的所有三维空间点坐标计算其平均值，计算时将三维空间点坐标的每个维度分别计算平均值，得到三维空间点坐标C′(c1′，c2′，c3′)，通过如下过程，即高斯牛顿法(Gauss-Newton)对得到的三维空间点坐标进行优化，最终得到某一标记点的三维空间点坐标C(c1，c2，c3)：

1)根据每台相机的R和T，为C′计算下列值并求总和g0、H0；

计算三维空间点坐标C′在每台相机的投影坐标，匹配实际图像坐标最近点并计算与最近点的图像坐标的残差；

根据每台相机的R和T计算C′在相机坐标系内的3D坐标q，定义：

返回D*R；

给定相机I坐标系里面的1个3D点p(x，y，z)及其在相机上的成像坐标(u，v)，则

相应的Jacobian矩阵

以世界坐标系中的3D点位变量，则有

根据Gauss-Newton算法，计算梯度

2)计算

3)最终得到优化后的三维空间点坐标C(c1，c2，c3)。

步骤S204，赋值：将标记点的二维空间点编码赋值给三维空间点坐标对应的编码，得到标记点的三维空间点编码。

由于任一标记点的编码，无论是二维空间点编码，还是三维空间点编码，都需要一致，因此本步骤直接将标记点对应的二维空间点编码赋值给三维空间点编码，即可得到包含三维空间点坐标和三维空间点编码的三维空间点数据。

本实施例根据已知的二维空间点数据通过具体的求解算法，解析出一组三维空间点数据，并对多个三维空间点数据进行整合、平均及优化等操作后，最终得到较为准确的三维空间点数据，为后续解析刚体数据提供精确数据。

步骤S3，计算刚体坐标：将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个标记点每帧的刚体编码和刚体坐标。

通过步骤S2可以得到每个标记点对应的三维空间点数据，将多个标记点对应得到的多个三维空间点数据组成一个刚体，若当前使用的刚体具有八个发光的LED灯，则此刚体包含八个三维空间点数据。通过多个三维空间点数据，如八个三维空间点数据中的三维空间点坐标，可以转化为刚体坐标系下的刚体坐标。

在一个实施例中，如图4所示，步骤S3进一步包括：

步骤S301，计算平均值：计算同帧的多个标记点对应的三维空间点坐标的坐标平均值，将坐标平均值记为刚体坐标系下的原点。

在确定刚体坐标时，首先确定刚体坐标系下的原点。本步骤通过对同一帧中的所有标记点对应的三维空间点坐标的每一维度分别计算平均值，得到坐标平均值，并将此坐标平均值记为刚体坐标系下的原点，作为所有标记点对应的三维空间点坐标的参考数据。

例如，刚体包含八个标记点时，则步骤S2得到八个三维空间点坐标数据，将这八个三维空间点坐标数据的每一维度计算平均值，得到坐标平均值。

步骤S302，计算差值：分别计算原点与同帧的每个标记点对应的三维空间点坐标之间的差值，得到每个标记点每帧的刚体坐标。

以坐标平均值作为刚体坐标系下的原点，将每个三维空间点坐标分别与原点进行差值计算，得到的差值即为每个标记点的刚体坐标。

例如，刚体包含八个标记点时，八个标记点对应的三维空间点坐标分别与原点进行差值计算，计算时，对每一维度的坐标分别与原点对应的维度坐标进行差值计算，最终得到八个刚体坐标。

步骤S303，赋值：将标记点的三维空间点编码赋值给刚体坐标对应的编码，得到标记点的刚体编码。

与步骤S204相似的，本步骤直接将标记点对应的三维空间点编码赋值给刚体编码，即可得到在刚体坐标系下包含刚体坐标和刚体编码的坐标数据。

本实施例根据三维空间点数据转化为刚体坐标系下的刚体坐标数据，为后续估计位姿提供确定且精确的数据。

步骤S4，确定初始位姿：通过求解第一帧三维空间点坐标到刚体坐标的位姿估计，确定刚体的初始位姿。

通过步骤S1到步骤S3可以得到相邻两帧中每一帧的三维空间点数据和刚体坐标数据，通过对第一帧的三维空间点坐标和刚体坐标进行位姿估计，确定出初始位姿，若相邻两帧中的第一帧是多个相机捕获的初始数据，则此初始位姿即为刚体的初始位姿。若相邻两帧中的第一帧并不是多个相机捕获的初始数据，例如是第三帧和第四帧，则第三帧的初始位姿是相对于第四帧是初始位姿，相对于刚体是运动过程中的运动位姿。

在一个实施例中，步骤S3进一步包括：

求解第一帧三维空间点坐标到刚体坐标的位姿估计时，将三维空间点坐标和刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，等式如下公式：

P1＝RP1′+T

其中，P1为第一帧三维空间点坐标，P1′为第一帧刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；

根据欧式变换旋转矩阵和平移矩阵得到刚体的初始位姿。

例如，刚体包含八个标记点时，八个标记点对应的三维空间点坐标P1＝{P11，P12…，P18}，八个刚体坐标P1′＝{P11′，P12′…，P18′}，其中P11′，P12′…，P18′保持不变，通过求解空间坐标系三维空间点坐标到刚体坐标系刚体坐标的位姿估计来解决刚体初始位姿问题。

本实施例在将数据代入等式中时，可以用迭代最近点(Iterative ClosestP_oint，ICP)求解R和T，采用SVD分解的方法进行ICP求解，可求得刚体的欧式变换旋转矩阵R和平移矩阵T。求出R和T便得到刚体的位姿数据，该位姿定义为刚体的初始位姿。

步骤S5，刚体跟踪定位：获取多个相机的相机参数，根据相机参数计算标记点第二帧的重投影坐标，根据标记点第二帧的刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对刚体跟踪定位。

在计算初始位姿时，仅考虑两组空间点之间的变化，和相机参数无关。但是在刚体运动跟踪时，不考虑相机参数误差会比较大，达不到动作捕捉的精度要求。因此为了提高精度，本步骤将相机模型数据，即标定后的相机参数也加入了计算。无论是刚体空间点还是相机图像点都带有各个标记点对应的编码，通过编码可以很容易的得到一组配对好的相机图像点和刚体空间点，由于每个刚体空间点匹配不同相机下的相机图像点，因此在计算位姿时需要在步骤S3的基础上加入相机模型数据，通过加入标定后的相机参数，利用Gauss-Newton梯度下降法，使用重投影误差来构造代价函数并使该代价函数最小化，由此就可以计算得到刚体的包括欧式变换旋转矩阵R和平移矩阵T的位姿信息，根据该位姿信息便可以实现主动光刚体的跟踪定位。

假设第二帧的二维图像点坐标为A(a1，a2)，三维空间点坐标为P2＝{P21，P22…，P28}，经过下述公式得到第二帧的重投影坐标：

假设三维空间点为C(c1，c2，c3)，相机a的旋转矩阵为Rcam，平移矩阵为Tcam，那么通过计算C′＝C*Rcam+Tcam，C′(c1′，c2′，c3′)是一个三维坐标，将C′归一化即可得到三维空间点C在相机a的重投影坐标A′(a1′，a2′)＝(c1′/c3′，c2′/c3′)。

将第二帧的相机图像坐标A(a1，a2)与重投影坐标A′(a1′，a2′)计算差值，得到重投影误差Error：

Error＝A-A′＝(a1-a1′，a2-a2′)

将上述误差函数扩展到相邻两帧之间，则最小化代价函数Error的计算方法为：

P′＝(P*R+T)；

P′′＝P′*Rcam+Tcam；

则A′(a1′，a2′)＝(p1′′/p3′′，p2′′/p3′′)；

根据Error＝A-A′＝(a1-a1′，a2-a2′)将上述参数代入其中，通过非线性优化算法可以得到误差最小化时相应的位姿变换(R，T)。

本步骤使代价函数最小化后，计算得到刚体的欧式变换旋转矩阵和平移矩阵后，还可将欧式变换旋转矩阵和平移矩阵分别与初始位姿的欧式变换旋转矩阵和平移矩阵分别进行比较，以此类推，相邻两帧可两两对比，从而有利于求出更精确的刚体位姿数据。

本实施例多相机环境中主动式刚体的位姿定位方法，主动光刚体带有编码信息使得动捕跟踪定位不再依赖于刚体结构，而是可以直接根据编码信息得到二维空间坐标与三维空间坐标的匹配关系，刚体的姿态运算更加快速的同时，也更加精确，上述主动式光学动捕相对于传统的光学动捕具有非常明显的优势。

在一个实施例中，提出了一种多相机环境中主动式刚体的位姿定位装置，如图5所示，该装置包括：

获取数据模块，用于获取多个相机捕捉的相邻两帧的二维空间点坐标、二维空间点坐标对应的二维空间点编码和多个相机的空间位置数据，将二维空间点编码相同的多个二维空间点坐标分为同类，且标记于同一个标记点下；

计算三维空间数据模块，用于将多个相机两两进行匹配，根据两个相机的空间位置数据及同类同帧的多个二维空间点坐标，得到每个标记点每帧的三维空间点编码和三维空间点坐标；

计算刚体数据模块，用于将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个标记点每帧的刚体编码和刚体坐标；

确定刚体初始位姿模块，用于通过求解第一帧三维空间点坐标到刚体坐标的位姿估计，确定刚体的初始位姿；

刚体定位模块，用于获取多个相机的相机参数，根据相机参数计算标记点第二帧的重投影坐标，根据标记点第二帧的刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对刚体跟踪定位。

基于与上述本发明实施例的多相机环境中主动式刚体的位姿定位方法相同的实施例说明内容，因此本实施例对多相机环境中主动式刚体的位姿定位装置的实施例内容不做过多赘述。

在一个实施例中，计算三维空间数据模块，包括：

计算三维空间点坐标组单元，用于将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个二维空间点坐标，利用多视几何中的三角测量原理，通过奇异值分解求解最小二乘法方法，解算得到一组三维空间点坐标；

剔除单元，用于判断三维空间点坐标是否处于预设的阈值范围内，若超过阈值范围，则剔除三维空间点坐标，得到剔除后的一组三维空间点坐标；

确定三维空间点坐标单元，用于计算一组三维空间点坐标的平均值，通过高斯牛顿法优化，得到标记点的三维空间点坐标；

确定三维空间点编码单元，用于将标记点的二维空间点编码赋值给三维空间点坐标对应的编码，得到标记点的三维空间点编码。

基于与上述本发明实施例的多相机环境中主动式刚体的位姿定位方法相同的实施例说明内容，因此本实施例对多相机环境中主动式刚体的位姿定位装置的实施例内容不做过多赘述。

在一个实施例中，计算刚体数据模块，还包括：

计算原点单元，用于计算同帧的多个标记点对应的三维空间点坐标的坐标平均值，将坐标平均值记为刚体坐标系下的原点；

确定刚体坐标单元，用于分别计算原点与同帧的每个标记点对应的三维空间点坐标之间的差值，得到每个标记点每帧的刚体坐标；

确定刚体编码单元，用于将标记点的三维空间点编码赋值给刚体坐标对应的编码，得到标记点的刚体编码。

基于与上述本发明实施例的多相机环境中主动式刚体的位姿定位方法相同的实施例说明内容，因此本实施例对多相机环境中主动式刚体的位姿定位装置的实施例内容不做过多赘述。

在一个实施例中，确定刚体初始位姿模块，包括：

求解矩阵单元，用于求解第一帧三维空间点坐标到刚体坐标的位姿估计时，将三维空间点坐标和刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，等式如下公式：

P1＝RP1′+T

其中，P1为第一帧三维空间点坐标，P1′为第一帧刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；根据欧式变换旋转矩阵和平移矩阵得到刚体的初始位姿。

基于与上述本发明实施例的多相机环境中主动式刚体的位姿定位方法相同的实施例说明内容，因此本实施例对多相机环境中主动式刚体的位姿定位装置的实施例内容不做过多赘述。

在一个实施例中，提出了一种多相机环境中主动式刚体的位姿定位设备，设备包括：存储器、处理器以及存储在存储器上并可在处理器上运行的多相机环境中主动式刚体的位姿定位程序，多相机环境中主动式刚体的位姿定位程序被处理器执行时实现上述各实施例的多相机环境中主动式刚体的位姿定位方法中的步骤。

在一个实施例中，一种计算机可读存储介质，计算机可读存储介质上存储有多相机环境中主动式刚体的位姿定位程序，多相机环境中主动式刚体的位姿定位程序被处理器执行时实现上述各实施例的多相机环境中主动式刚体的位姿定位方法中的步骤。其中，存储介质可以为非易失性存储介质。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明一些示例性实施例，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种多相机环境中主动式刚体的位姿定位方法，其特征在于，所述方法包括以下步骤：

获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；

将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；

将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；

通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿，具体为：若相邻两帧中的第一帧是多个相机捕获的初始数据，则所述初始位姿为刚体的初始位姿，若相邻两帧中的第一帧不是多个相机捕获的初始数据，则前一帧的初始位姿相对于后一帧是刚体的初始位姿，相对于刚体是运动过程中的运动位姿；

获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。

2.根据权利要求1所述的多相机环境中主动式刚体的位姿定位方法，其特征在于，所述将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标，包括：

将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，通过奇异值分解求解最小二乘法方法，解算得到一个三维空间点，遍历所有两两匹配的相机后，得到一组三维空间点，一组所述三维空间点即为所述标记点的三维空间点坐标；

判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；

计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；

将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。

3.根据权利要求1所述的多相机环境中主动式刚体的位姿定位方法，其特征在于，所述将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标，包括：

计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；

分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标；

将所述标记点的三维空间点编码赋值给所述刚体坐标对应的编码，得到所述标记点的刚体编码。

4.根据权利要求1所述的多相机环境中主动式刚体的位姿定位方法，其特征在于，所述通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿，包括：

求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计时，将所述三维空间点坐标和所述刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，所述等式如下公式：

P1＝RP1＇+T

其中，P1为第一帧所述三维空间点坐标，P1＇为第一帧所述刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；

根据所述欧式变换旋转矩阵和所述平移矩阵得到刚体的初始位姿。

5.一种多相机环境中主动式刚体的位姿定位装置，其特征在于，所述装置包括：

获取数据模块，用于获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；

计算三维空间数据模块，用于将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；

计算刚体数据模块，用于将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；

确定刚体初始位姿模块，用于通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿，具体为：若相邻两帧中的第一帧是多个相机捕获的初始数据，则所述初始位姿为刚体的初始位姿，若相邻两帧中的第一帧不是多个相机捕获的初始数据，则前一帧的初始位姿相对于后一帧是刚体的初始位姿，相对于刚体是运动过程中的运动位姿；

刚体定位模块，用于获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。

6.根据权利要求5所述的多相机环境中主动式刚体的位姿定位装置，其特征在于，所述计算三维空间数据模块，包括：

计算三维空间点坐标组单元，用于将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，通过奇异值分解求解最小二乘法方法，解算得到一组三维空间点坐标；

剔除单元，用于判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；

确定三维空间点坐标单元，用于计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；

确定三维空间点编码单元，用于将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。

7.根据权利要求5所述的多相机环境中主动式刚体的位姿定位装置，其特征在于，所述计算刚体数据模块，还包括：

计算原点单元，用于计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；

确定刚体坐标单元，用于分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标；

确定刚体编码单元，用于将所述标记点的三维空间点编码赋值给所述刚体坐标对应的编码，得到所述标记点的刚体编码。

8.根据权利要求5所述的多相机环境中主动式刚体的位姿定位装置，其特征在于，所述确定刚体初始位姿模块，包括：

求解矩阵单元，用于求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计时，将所述三维空间点坐标和所述刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，所述等式如下公式：

P1＝RP1＇+T

其中，P1为第一帧所述三维空间点坐标，P1＇为第一帧所述刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；

根据所述欧式变换旋转矩阵和所述平移矩阵得到刚体的初始位姿。

9.一种多相机环境中主动式刚体的位姿定位设备，其特征在于，所述设备包括：

存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的多相机环境中主动式刚体的位姿定位程序，所述多相机环境中主动式刚体的位姿定位程序被所述处理器执行时实现如权利要求1至4中任一项所述的多相机环境中主动式刚体的位姿定位方法的步骤。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有多相机环境中主动式刚体的位姿定位程序，所述多相机环境中主动式刚体的位姿定位程序被处理器执行时实现如权利要求1至4中任一项所述的多相机环境中主动式刚体的位姿定位方法的步骤。

Images