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

Abstract

本发明涉及计算机视觉技术领域，尤其涉及一种单相机环境中主动式刚体的位姿定位方法及相关设备。该方法包括：获取相邻两帧的二维空间点坐标、二维空间点编码和相机参数，根据二维空间点编码，将二维空间点坐标进行匹配，得到多组二维空间特征对，将多组二维空间特征对和相机参数构造线性方程组，求解出本质矩阵；通过奇异值分解算法分解本质矩阵，得到多组旋转矩阵和平移矩阵；估算出三维空间点坐标，检测深度值，确定出目标旋转矩阵和目标平移矩阵，根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿。本发明在单相机环境中，能以较低的成本就可实现主动光刚体的跟踪定位，相较于复杂的多相机环境具有明显优势。

Description

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

技术领域

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

背景技术

传统的光学动捕方法是通过动捕相机内的超大功率近红外光源发出红外光，照射在被动式标记点上；涂有高反光材料的标记点反射被照射到的红外光，而这部分红外光和带有背景信息的环境光会经过低畸变镜头，到达摄像机红外窄带通滤光单元。由于红外窄带通滤光单元的通光波段跟红外光源的波段一致，因此，带有冗余背景信息的环境光会被过滤掉，只剩下带有标记点信息的红外光通过，并被摄像机感光元件记录。感光元件再将光信号转化为图像信号输出到控制电路，而控制电路中的图像处理单元使用现场可编程门阵列(Field Programmable Gate Array，FPGA)，以硬件形式对图像信号进行预处理，最后向跟踪软件流出标记点的2D坐标信息。

传统的光学动作捕捉系统中，无论是主动式刚体跟踪还是被动式刚体跟踪，都需要系统服务器接收多相机系统中各个相机的2D数据，然后采用多目视觉原理，根据2D点云之间的匹配关系以及提前标定计算好的各相机间的位姿关系，计算出三维空间内的3D坐标，并以此为基础解算刚体在空间内的运动信息。这种方式依赖于多相机之间的协同工作，从而可以应用在比较大的空间范围内实现对刚体的识别跟踪，这就导致了动捕系统的高成本和难维护问题。

发明内容

本发明的主要目的在于提供一种单相机环境中主动式刚体的位姿定位方法及相关设备，旨在解决目前被动式或主动式动捕方法中利用多相机系统引起的高成本和难维护的技术问题。

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

获取单目相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和所述相机的相机参数，根据所述二维空间点编码，将相邻两帧的所述二维空间点坐标进行匹配，得到多组二维空间特征对，将多组所述二维空间特征对和所述相机参数构造线性方程组，求解出本质矩阵；

通过奇异值分解算法分解所述本质矩阵，得到多组旋转矩阵和平移矩阵；

通过所述二维空间特征对、多组所述旋转矩阵和所述平移矩阵，估算出三维空间点坐标，检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿。

可选地，所述根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿，包括：

将所述三维空间点坐标内的所有三维空间点之间的距离求和后取平均值，得到三维平均距离；

获取刚体坐标，将所述刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离；

通过优化公式将所述目标平移矩阵进行优化，得到优化后的目标平移矩阵，根据所述目标旋转矩阵和优化后的所述目标平移矩阵确定刚体位姿；

所述优化公式为：

其中，L1为所述三维平均距离，L2为所述刚体平均距离，T为优化前的所述目标平移矩阵，T′为优化后的所述目标平移矩阵。

可选地，所述获取刚体坐标，将所述刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离前，包括：

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

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

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

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

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

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

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

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

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

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

可选地，所述通过所述二维空间特征对、多组所述旋转矩阵和所述平移矩阵，估算出三维空间点坐标，包括：

设两个相机分别为相机1和相机2，在同帧中捕捉到的两个二维空间点坐标分别为A(a1，a 2)，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的矩阵，通过下述方法可得到三维空间点坐标：

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(c1，c2，c3)；

4)根据多个不同的旋转矩阵和平移矩阵R1、T1、R2、T2，得到多个不同的三维空间点坐标。

可选地，所述检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，包括：

根据估算出的所述三维空间点坐标，检测所述三维空间点坐标对应的深度值是否为正数，若是，则将对应的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵。

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

计算本质矩阵模块，用于获取单目相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和所述相机的相机参数，根据所述二维空间点编码，将相邻两帧的所述二维空间点坐标进行匹配，得到多组二维空间特征对，将多组所述二维空间特征对和所述相机参数构造线性方程组，求解出本质矩阵；

计算旋转矩阵和平移矩阵模块，用于通过奇异值分解算法分解所述本质矩阵，得到多组旋转矩阵和平移矩阵；

确定刚体位姿模块，用于通过所述二维空间特征对、多组所述旋转矩阵和所述平移矩阵，估算出三维空间点坐标，检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿。

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

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

本发明提供的单相机环境中主动式刚体的位姿定位方法，在确定刚体位姿过程中，通过对相邻两帧坐标中的特征点进行匹配，求解本质矩阵；通过奇异值分解算法分级本质矩阵，得到多组旋转矩阵和平移矩阵；通过检测特征点的深度值，确定出最终的目标旋转矩阵和平移矩阵。整个过程不依赖于刚体结构，只需根据编码和坐标就能得到所需匹配数据以解算刚体位姿信息。本发明在单相机环境中，能以较低的成本就可实现主动光刚体的跟踪定位，相较于复杂的多相机环境具有明显优势。此外，本发明由于每次都对相邻两帧的特征点进行匹配，使得每次跟踪定位主动光刚体都可以计算当前帧相较于初始帧的运动姿态，从而避免了单目相机跟踪常见的累积误差问题，进一步提升了跟踪精度。

附图说明

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

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

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

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

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

图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灯。因此刚体通常包含八个空间点数据，单目相机每帧数据中都包含八个标记点的空间点数据，同一个标记点在不同帧时的编码是相同的，不同的标记点在同帧时的编码是不同的。基于此可以把单目相机捕获的相邻两帧内的所有二维空间点进行匹配，将二维空间点编码相同的两个二维空间点作为一组二维空间特征对，并认为同一组二维空间特征对是空间中同一个标记点在单目相机上相邻两帧的投影。当刚体包含八个标记点时，则具有八组二维空间特征对。

在单目相机捕获空间点数据前，需要对单目相机标定好相机参数，即相机光心、焦距和畸变参数等，这些相机参数作为一个矩阵，记做矩阵M，应用于本质矩阵计算中。本步骤在求解本质矩阵时，采用对极几何约束原理，通过如下方式，对多组二维空间特征对和相机参数构造线性方程组，求解出本质矩阵：

为了求解本质矩阵，首先计算基础矩阵F，由

根据多组二维空间特征对得到基础矩阵F，根据F＝M^-TEM，由于相机参数对应的矩阵M已知，则可以得到本质矩阵E。

步骤S2，分解本质矩阵：通过奇异值分解算法分解本质矩阵，得到多组旋转矩阵和平移矩阵。

得到本质矩阵后，根据本质矩阵恢复出刚体的运动信息：旋转矩阵R和平移矩阵T，该过程通过本步骤由奇异值分解(Singular Value Decomposition，SVD)得到的。对步骤S1得到的本质矩阵E通过奇异值分解后，一共可以得到四个可能的解(R、T)，即为四组旋转矩阵和平移矩阵，其中只有一个正确的解在单目相机中具有正深度(深度值为正数)。因此还需要进行下一步检测深度信息的步骤。

步骤S3，确定刚体位姿：通过二维空间特征对、多组旋转矩阵和平移矩阵，估算出三维空间点坐标，检测三维空间点坐标的深度值，将深度值为正数的那组旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据目标旋转矩阵和目标平移矩阵确定刚体位姿。

在步骤S2中采用奇异值分解对本质矩阵进行分解后，得到四个可能的解，因此本步骤需要在四个可能的解中最终确定正确的解。首先需要估算出三维空间点坐标，根据三维空间点坐标来检测特征点的深度值，只有深度值为正数的那组解(R、T)才是最终的目标(R、T)。

在一个实施例中，步骤S3中，通过二维空间特征对、多组旋转矩阵和平移矩阵，估算出三维空间点坐标，进一步包括：

设两个相机分别为相机1和相机2，在同帧中捕捉到的两个二维空间点坐标分别为A(a1，a 2)，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(c1，c2，c3)；

最后根据多个不同的旋转矩阵和平移矩阵，比如多组R1、T1和R2、T2等旋转矩阵和平移矩阵数据对，得到多个不同的三维空间点坐标。

例如，步骤S2中得到了四组旋转矩阵和平移矩阵，则通过本步骤可估算出4个不同的三维空间点坐标，但其中只有一个三维空间点坐标C的坐标值c3是大于0的，那么与该三维空间点坐标C对应的R、T就是最终的目标数据。

本实施例将匹配的多组二维空间特征对与四个可能的解(R、T)进行结合，根据三角测量原理，通过上述方式估计出对应的三维空间坐标数据(x,y,z)，为后续检测深度值z提供精确数据。

在一个实施例中，步骤S3中，检测三维空间点坐标的深度值，将深度值为正数的那组旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，包括：

根据估算出的三维空间点坐标，检测三维空间点坐标对应的深度值是否为正数，若是，则将对应的那组旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵。

本实施例通过上述方式求解得到多个深度值z，剔除深度值z为零或负数时对应的解(R、T)，将深度值z为正数时对应的解(R、T)保留，并作为最终的目标数据，以此目标数据确定刚体位姿。

在一个实施例中，步骤S3中，将深度值为正数的那组旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵后，根据目标旋转矩阵和目标平移矩阵确定刚体位姿前，如图3所示，包括：

步骤S301，计算三维平均距离：将三维空间点坐标内的所有三维空间点之间的距离求和后取平均值，得到三维平均距离。

计算三维平均距离时，可以随机任一取一个三维空间点1，计算三维空间点1与其他任一另一个三维空间点2之间的距离，计算时，可以采用如下公式：

D＝sqrt((a₁-b₁)²+(a₂-b₂)²+(a₃-b₃)²)

其中，D为两个三维空间点之间的距离，(a₁，a₂，a₃)为三维空间点1的三维空间点坐标，(b₁，b₂，b₃)为三维空间点2的三维空间点坐标。

计算三维空间点2与还未参与计算的其他任一另一个三维空间点3之间的距离，直到所有的三维空间点都参与了计算后，将所有的距离进行求和，再取平均值。也可以在所有的三维空间点都参与了计算后，最后参与计算的三维空间点8再与第一个随机取的三维空间点1计算距离，将所有的距离进行求和，再取平均值。

例如，当刚体中的标记点为八个时，三维空间点坐标内的三维空间点为八个，通过上述方式，计算得到八个距离数值，将此八个距离数值进行相加，再除以八，得到三维平均距离。

步骤S302，计算刚体平均距离：获取刚体坐标，将刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离。

在计算刚体平均距离时，可以采用与步骤S301相似的距离计算公式，分别计算两个刚体标记点在刚体坐标系下两者坐标之间的距离后，进行求和，再取平均值。

本步骤中的刚体坐标，可以通过实际测量标记点的刚体坐标获得，如图4所示，也可以通过多相机系统获得，即采用如下方式，只需一次初始化即可获得准确的刚体坐标，无需多次计算：

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

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

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

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

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

本步骤进一步包括：

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

设两个相机分别是相机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，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。

本步骤最终将所有两两匹配的相机捕捉到的两个二维空间点坐标均进行解算，得到一组三维空间点坐标。

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

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

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

将剔除错误数据后的所有三维空间点坐标计算其平均值，计算时将三维空间点坐标的每个维度分别计算平均值，得到三维空间点坐标C＇(c1＇，c2＇，c3＇)，通过高斯牛顿法对得到的三维空间点坐标进行优化，最终得到某一标记点的三维空间点坐标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)。

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

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

本步骤进一步包括：

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

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

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

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

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

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

本实施例通过多个相机来捕获多个二维空间点坐标，通过具体的求解算法，解析出一组三维空间点数据，并对多个三维空间点数据进行整合、平均及优化等操作后，最终得到较为准确的三维空间点数据，根据准确的三维空间点数据转化为刚体坐标系下的刚体坐标数据，为后续计算刚体平均距离提供确定且精确的数据。

步骤S303，优化：通过优化公式将目标平移矩阵进行优化，得到优化后的目标平移矩阵，根据目标旋转矩阵和优化后的目标平移矩阵确定刚体位姿。优化公式为：

其中，L1为三维平均距离，L2为刚体平均距离，T为优化前的目标平移矩阵，T′为优化后的目标平移矩阵。

单目相机下，估计出刚体的目标旋转矩阵R和目标平移矩阵T后，在刚体同一旋转角度情况下，其平移量可能有多种情况，因此无法完全保证该平移矩阵T是准确真实的数据。为了获得更优化可靠的刚体位姿信息，进而确定刚体运动情况，在通过三角测量原理估计出刚体的三维空间点坐标后，根据估计出的三维空间点坐标和刚体坐标系下的刚体坐标，对目标平移矩阵进行优化。通过上述优化公式得到优化后的目标平移矩阵，使得最终得到的刚体位姿更准确、更具有真实性。

本实施例单相机环境中主动式刚体的位姿定位方法，主动光刚体带有编码信息使得动捕跟踪定位不再依赖于刚体结构，而是可以直接根据编码信息得到可匹配的二维空间特征对，以解算刚体位姿。单相机环境中，采用本发明可以以较低的成本实现对刚体的跟踪定位，相较于复杂的多相机环境具有明显优势。此外，由于是根据主动光刚体的编码信息，来对相邻两帧进行匹配，使得每次跟踪定位主动光刚体都可以计算当前帧相较于初始帧的运动姿态，从而避免了单目相机跟踪常见的累积误差问题，进一步提升了跟踪精度。

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

计算本质矩阵模块，用于获取单目相机捕捉的相邻两帧的二维空间点坐标、二维空间点坐标对应的二维空间点编码和相机的相机参数，根据二维空间点编码，将相邻两帧的二维空间点坐标进行匹配，得到多组二维空间特征对，将多组二维空间特征对和相机参数构造线性方程组，求解出本质矩阵；

计算旋转矩阵和平移矩阵模块，用于通过奇异值分解算法分解本质矩阵，得到多组旋转矩阵和平移矩阵；

确定刚体位姿模块，用于通过二维空间特征对、多组旋转矩阵和平移矩阵，估算出三维空间点坐标，检测三维空间点坐标的深度值，将深度值为正数的那组旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据目标旋转矩阵和目标平移矩阵确定刚体位姿。

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

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

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

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

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

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

Claims (10)

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

获取单目相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和所述相机的相机参数，根据所述二维空间点编码，将相邻两帧的所述二维空间点坐标进行匹配，得到多组二维空间特征对，将多组所述二维空间特征对和所述相机参数构造线性方程组，求解出本质矩阵；

通过奇异值分解算法分解所述本质矩阵，得到多组旋转矩阵和平移矩阵；

通过所述二维空间特征对、多组所述旋转矩阵和所述平移矩阵，估算出三维空间点坐标，检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿。

2.根据权利要求1所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿，包括：

将所述三维空间点坐标内的所有三维空间点之间的距离求和后取平均值，得到三维平均距离；

获取刚体坐标，将所述刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离；

通过优化公式将所述目标平移矩阵进行优化，得到优化后的目标平移矩阵，根据所述目标旋转矩阵和优化后的所述目标平移矩阵确定刚体位姿；

所述优化公式为：

其中，L1为所述三维平均距离，L2为所述刚体平均距离，T为优化前的所述目标平移矩阵，T′为优化后的所述目标平移矩阵。

3.根据权利要求2所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述获取刚体坐标，将所述刚体坐标内的所有刚体标记点之间的距离求和后取平均值，得到刚体平均距离前，包括：

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

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

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

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

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

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

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

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

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

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

6.根据权利要求1所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述通过所述二维空间特征对、多组所述旋转矩阵和所述平移矩阵，估算出三维空间点坐标，包括：

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

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

构造最小二乘法矩阵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；

根据等式X*C＝Y和所述矩阵X、矩阵Y，利用奇异值分解求得一个三维空间点坐标C(c1，c2，c3)；

根据多个不同的旋转矩阵和平移矩阵，得到多个不同的三维空间点坐标。

7.根据权利要求1所述的单相机环境中主动式刚体的位姿定位方法，其特征在于，所述检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，包括：

根据估算出的所述三维空间点坐标，检测所述三维空间点坐标对应的深度值是否为正数，若是，则将对应的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵。

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

计算本质矩阵模块，用于获取单目相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和所述相机的相机参数，根据所述二维空间点编码，将相邻两帧的所述二维空间点坐标进行匹配，得到多组二维空间特征对，将多组所述二维空间特征对和所述相机参数构造线性方程组，求解出本质矩阵；

计算旋转矩阵和平移矩阵模块，用于通过奇异值分解算法分解所述本质矩阵，得到多组旋转矩阵和平移矩阵；

确定刚体位姿模块，用于通过所述二维空间特征对、多组所述旋转矩阵和所述平移矩阵，估算出三维空间点坐标，检测三维空间点坐标的深度值，将深度值为正数的那组所述旋转矩阵和平移矩阵定义为目标旋转矩阵和目标平移矩阵，根据所述目标旋转矩阵和所述目标平移矩阵确定刚体位姿。

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

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

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

Images