一种刚体追踪的方法、装置和系统与流程

本发明涉及计算机技术领域，具体涉及一种刚体追踪的方法、装置和系统。

背景技术：

现有的追踪技术可以在不同的物理硬件上以不同的方式进行实现。现有的追踪技术的实现方式主要有以下几种：

(一)基于计算机图形学和图像处理技术的单双目rgb相机，ir相机，深度相机等。

(二)基于激光，雷达，电磁波，超声波等定位手段。

(三)基于imu的惯性传感单元等。

上述追踪技术在追踪算法的实现上各具有优缺点，其中，基于计算机图形学的追踪算法是最广泛使用的算法，有适应范围广，硬件条件低的优点，但是由于图像分辨率不高导致定位的精度不高。基于imu的追踪有抗干扰能力强，输出稳定的优点，但是imu在定位方面存在时间漂移问题，导致定位不准确。

另外，特征匹配是计算机视觉中的一个基本问题，只要涉及到两幅或者多幅图像时，都会涉及到对应特征的匹配问题。现有的特征匹配主要有以下两种，一种是基于纹理的特征匹配算法，其计算量较大；另外一种是基于射影不变量的特征匹配算法，其对应用场景的限制较高。

技术实现要素：

本发明提供了一种刚体追踪的方法、装置和系统，以解决现有的基于计算机图形学的追踪算法定位精度不高，imu由于存在时间漂移导致定位不准确以及现有特征匹配算法计算量大、应用场景局限性大问题。

根据本发明的一个方面，提供了一种刚体追踪方法，所述刚体包含多个特征点，预存所述刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，在所述刚体上设置惯性测量传感器imu，所述方法包括：

获取摄像头采集的所述刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；

获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态；

根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；

根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对；

根据所述特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息。

优选地，所述方法还包括：

根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态。

优选地，所述获取摄像头采集的所述刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标包括：

根据所述计算得到的图像中特征点相对于摄像头的三维位置坐标计算所述图像中的特征点之间的第一相对距离；

根据预存的刚体上每个特征点相对于刚体重心的三维位置信息计算刚体所有特征点之间的第二相对位置距离；

将所述第一相对距离与所述第二相对距离进行匹配，去除所述图像特征点中的伪特征点。

优选地，所述根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息包括：

根据所述旋转和平移信息计算所述刚体上所有特征点相对于摄像头的三维位置坐标；

根据所有特征点相对于摄像头的三维位置坐标计算摄像头光学系统中刚体的位置信息。

根据本发明的另一个方面，提供了一种刚体追踪的装置，所述刚体包含多个特征点，在所述刚体上设置惯性测量传感器imu，其特征在于，所述装置包括：

存储单元，用于预存所述刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息；

图像特征点位置坐标确定单元，用于获取摄像头采集的所述刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；

刚体姿态获取单元，用于获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态；

图像特征点身份信息确定单元，用于根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；

图像特征点对匹配单元，用于根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对；

刚体位置信息确定单元，用于根据所述特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息。

优选地，所述装置还包括刚体姿态校正单元；

所述刚体姿态校正单元，用于根据所述旋转和平移信息校正所述imu获取到摄像头拍摄时刻的刚体的姿态。

优选地，所述装置还包括：伪特征点去除单元；

所述伪特征点去除单元，用于根据所述计算得到的图像中特征点相对于摄像头的三维位置坐标计算所述图像中的特征点之间的第一相对距离；

根据预存的刚体上每个特征点相对于刚体重心的三维位置信息计算刚体所有特征点之间的第二相对位置距离；

将所述第一相对距离与所述第二相对距离进行匹配，去除所述图像特征点中的伪特征点。

优选地，所述刚体位置信息确定单元，用于根据所述旋转和平移信息计算所述刚体上所有特征点相对于摄像头的三维位置坐标；

根据所有特征点相对于摄像头的三维位置坐标计算摄像头光学系统中刚体的位置信息。

根据本发明的再一个方面，提供了一种刚体追踪的系统，其特征在于，所述系统包括摄像头、刚体和控制端，其中，imu设置在所述刚体的重心位置；

所述摄像头，用于采集所述刚体的图像，并将所述刚体的图像发送至所述控制端；

所述imu，用于采集所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态，并将所述姿态发送至所述控制端；

所述控制端，用于获取摄像头采集的所述刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态；根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对；根据所述特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息。

优选地，所述控制端，还用于根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态。

本发明的有益效果是：本发明的技术方案，将基于计算机图形学的追踪算法与基于imu的追踪算法相融合，首先，获取摄像头采集的刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态，根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对，解决了现有特征匹配算法计算量大、应用场景局限性大等问题；

其次，根据匹配的特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息，解决了计算机图形学追踪算法由于摄像头获取的图像分辨率不高导致的空间定位精度不高的问题；

最后，根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态解决了imu在空间定位过程中存在的时间漂移的问题。本发明的技术方案，将计算得到的摄像头光学系统中的刚体位置和纠正后的姿态进行输出，实现刚体的追踪，达到了将基于计算机图形学的追踪算法与imu的追踪算法进行优势互补的目的，提升了刚体追踪的精确度。

附图说明

图1是本发明一个实施例的一种刚体追踪的方法的流程图；

图2是本发明一个实施例的一种刚体追踪的方法的流程图；

图3是本发明一个实施例的一种刚体追踪的装置的结构示意图；

图4是本发明一个实施例的另一种刚体追踪的装置的结构示意图；

图5是本发明一个实施例的一种刚体追踪的系统的示意图。

具体实施方式

本发明的设计构思是：为了避免基于计算机图形学的追踪算法与imu的追踪算法的弊端，解决现有特征匹配算法计算量大、应用场景局限性大等问题，实现刚体的精确追踪，通过将基于计算机图形学的追踪算法与基于imu的追踪算法相融合，得到匹配的特征点对，根据匹配的特征点对计算光学系统中刚体的位置信息和姿态，并利用光学系统中刚体的姿态去纠正imu获取到的刚体的姿态；最后，将摄像头光学系统中的刚体位置和纠正后的姿态进行输出。

实施例一

图1是本发明一个实施例的一种刚体追踪的方法的流程图，如图1所示，

在步骤s110之前，将惯性测量传感器imu设置在所述刚体上，并预存刚体上每个特征点的身份信息及相对于刚体重心的三维位置信息；

在本发明的一个实施例中，将惯性测量传感器imu设置在所述刚体的重心位置，预存的刚体上每个特征点相对于刚体重心的三维位置信息指的是每个特征点在刚体上的几何三维位置信息，每个特征点相对于刚体重心的三维位置信息包括每个特征点的身份信息(例如，具体编号1、2、3···)、每个特征点相对于刚体重心的相对距离以及每个特征点相对于刚体重心的相对方向。需要说明的是，一旦每个特征点设定在刚体上，每个特征点相对于刚体重心的三维位置信息是固定不变的。例如，假设刚体是虚拟现实头盔，特征点是红外发光球，将一个红外发光球设置在虚拟现实头盔的顶点上，那么无论虚拟现实头盔怎样运动，红外发光球一直位于虚拟现实头盔的顶点上。

在步骤s110中，获取摄像头采集的所述刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标。

在本发明的一个实施例中，采用双目摄像头采集刚体运动时的图像，利用双目成像原理计算刚体图像中的特征点相对于摄像头的三维位置坐标。

在步骤s120中，获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态。

在本发明的一个实施例中，通过imu获取刚体在三维空间中的角速度和加速度，并根据所述角速度和加速度计算刚体的姿态。例如，imu获取到的刚体初始姿态为q0；imu获取到的对应所述摄像头拍摄时刻的刚体的姿态为q1。

在步骤s130中，根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息。

在步骤s140中，根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对。

在步骤s150中，根据所述特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息。

在本发明的一个实施例中，根据所述特征点对，利用透视n点定位pnp算法计算所述图像中的特征点相对于初始姿态下的旋转和平移信息。

在本发明的一个实施中，根据所述旋转和平移信息计算所述刚体上所有特征点相对于摄像头的三维位置坐标；

根据所有特征点相对于摄像头的三维位置坐标计算摄像头光学系统中刚体的位置信息。

需要说明的是，摄像头获取到的图像中只包含了刚体中的部分特征点，在获取到光学系统中刚体旋转和平移信息后，通过计算即可获取到所有特征点相对于摄像头的三维位置坐标，并利用所有特征点相对于摄像头的三维位置坐标计算光学系统中刚体的位置信息，使得刚体的位置信息更加精确。与现有的基于计算机图形学的追踪技术相比，本申请的优势在于对刚体的定位精确更高，原因如下：现有的基于计算机图形学的追踪技术只能依据摄像头获取刚体上的部分特征点，因此只能计算刚体上部分特征点相对于摄像头的三维位置坐标，但是本申请结合imu获取到的姿态计算出刚体上所有特征点的相对于摄像头的三维位置坐标。另外，本发明的技术方案避免了由于摄像头获取的图像分辨率不高导致的空间定位精度不够精确的问题。

在本发明的一个实施例中，图2是本发明一个实施例的一种刚体追踪的方法的流程图，如图2所示，

在步骤s160中，根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态；

需要说明的是，图像中的特征点相对于初始姿态下的旋转和平移信息指的就是摄像头的光学系统中刚体的姿态，imu在实际应用中存在时间漂移导致定位不精确，利用计算得到的旋转和平移信息纠正imu获取到的刚体姿态，使得最终输出的刚体的姿态更加精确，进而实现刚体的精确追踪。

最后，将所述摄像头光学系统中刚体的位置和所述校正后的刚体姿态进行输出，实现所述刚体的追踪。

通过图1和图2所示的方法可知，本发明的技术方案，将基于计算机图形学的追踪算法与基于imu的追踪算法相融合，首先，获取摄像头采集的刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态，根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对，解决了现有特征匹配算法计算量大、应用场景局限性大等问题；

其次，根据匹配的特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息，解决了计算机图形学追踪算法由于摄像头获取的图像分辨率不高导致的空间定位精度不高的问题；

最后，根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态解决了imu在空间定位过程中存在的时间漂移的问题。本发明的技术方案，将计算得到的摄像头光学系统中的刚体位置和纠正后的姿态进行输出，实现刚体的追踪，达到了将基于计算机图形学的追踪算法与imu的追踪算法进行优势互补的目的，提升了刚体追踪的精确度。

为了使得本方案的方案更加清楚，下面举一个具体的例子进行解释。假设刚体上有m个特征点，

s11、预存刚体上m个特征点的身份信息及m个特征点相对于刚体重心的三维位置信息，例如，第一特征点(a1,b1,c1)、第二特征点(a2,b2,c2)、第三特征点(a3,b3,c3)、第四特征点(a4,b4,c4)、第五特征点(a5,b5,c5)、第六特征点(a6,a6,a6)、第七特征点(a7,b7,c7)和第八特征点(a8,b8,c8)。

s12、刚体的重心位置设置有imu，可以实时获取刚体的姿态。利用摄像头拍摄刚体运动时的图像，识别图中的n(n<m)个特征点，利用双目成像原理计算n个特征点相对于双目摄像头的三维位置坐标，例如，(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)、(x4,y4,z4)和(x5,y5,z5)；

s13、根据n个特征点相对于双目摄像头的三维位置坐标计算n个特征点之间的第一相对位置距离，例如，第一相对距离包括a1、b2、a3、a4和a5；同时根据预存的刚体上每个特征点相对于刚体重心的三维位置信息计算刚体所有特征点之间的第二相对位置距离,例如，第二相对距离包括a1、a2、a3、a4、a5、a6、a7和a8；将所述第一相对距离与所述第二相对距离进行匹配，去除所述图像特征点中的伪特征点b2，达到了去除噪声点的目的，提升了刚体定位精度。

s14、假设在步骤s13之后，去除伪特征点后得到的特征点的数量为n1，此时n1个特征点相对于摄像头的三维位置坐标为(x1,y1,z1)、(x3,y3,z3)、(x4,y4,z4)和(x5,y5,z5)；利用imu获取此时刚体的姿态q1和刚体初始状态下的姿态q0，根据刚体的初始姿态q0和此时姿态q1可以得到刚体的旋转和平移信息，根据得到的刚体的旋转和平移信息即可将n1个特征点相对于摄像头的三维位置坐标恢复至n1个特征点在刚体初始姿态q0下的三维位置坐标。需要说明的是，假设第一特征点在刚体初始姿态下的三维位置坐标为(x0,y0,z0)，根据(x01,y01,z01)＝旋转矩阵*(x1,y1,z1)+平移，即可将第一特征点相对于双目摄像头的三维位置坐标恢复至第一特征点在刚体初始姿态下的三维位置坐标；同理(x3,y3,z3)恢复至刚体初始姿态q0下的三维位置坐标为(x03,y03,z03)、(x4,y4,z4)恢复至刚体初始姿态q0下的三维位置坐标为(x04,y04,z04)和(x5,y5,z5)恢复至刚体初始姿态q0下的三维位置坐标为(x05,y05,z05)，利用上下左右等空间位置信息确定图像中的第一特征点是刚体上的第5特征点，图像中的第三特征点是刚体上的第4特征点，图像中的第四特征点是刚体上的第3特征点，图像中的第五特征点是刚体上的第2特征点。

s15、将图像中的第一特征点与刚体上的第5特征点进行匹配，得到匹配的特征点对[(x1,y1,z1)，(a5,b5,c5)]；将图像中的第三特征点与刚体上的第4特征点进行匹配，得到匹配的特征点对[(x3,y3,z3)，(a4,b4,c4)]；将图像中的第四特征点与刚体上的第3特征点进行匹配，得到匹配的特征点对[(x4,y4,z4)，(a3,b3,c3)]；将图像中的第五特征点与刚体上的第2特征点进行匹配，得到匹配的特征点对[(x5,y5,z5)，(a2,b2,c2)]。

s16、将匹配的特征点对[(x1,y1,z1)，(a5,b5,c5)]、[(x3,y3,z3)，(a4,b4,c4)]、[(x4,y4,z4),(a3,b3,c3)]和[(x5,y5,z5)，(a2,b2,c2)]输入pnp算法，pnp计算出图像中的特征点相对于初始姿态q0下的旋转和平移信息；需要说明的是，根据根据所述特征点在刚体初始姿态q0下的三维位置坐标与预存的刚体上每个特征点相对于刚体重心的三维位置信息，就得到计算刚体上剩余特征点(m-n1个特征点)在刚体初始状态q0下的相对于摄像头的三维位置坐标；并根据所述旋转和平移信息，将刚体上剩余特征点(m-n1个特征点)在刚体初始状态q0下的相对于摄像头的三维位置坐标转换为刚体上剩余特征点(m-n1个特征点)在摄像头拍照时刻的相对于摄像头的三维位置坐标，从而根据所有特征点相对于摄像头的三维位置坐标计算摄像头光学系统中刚体的位置信息。

s17、根据所述旋转和平移信息校正所述imu获取到摄像头拍摄时刻的刚体的姿态；

s18、将所述摄像头光学系统中刚体的位置和所述校正后的刚体姿态进行输出，实现所述刚体的追踪。

实施例二

图3是本发明一个实施例的一种刚体追踪的装置的结构示意图，如图3所示，一种刚体追踪的装置，所述刚体包含多个特征点，将惯性测量传感器imu设置在所述刚体上，所述装置包括：

存储单元210，用于预存所述刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息；

图像特征点位置坐标确定单元220，用于获取摄像头采集的所述刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；

刚体姿态获取单元230，用于获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态；

图像特征点身份信息确定单元240，用于根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；

图像特征点对匹配单元250，用于根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对；

刚体位置信息确定单元260，用于根据所述特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息。

在本发明的一个实施例中，图4是本发明一个实施例的另一种刚体追踪的装置的结构示意图，如图4所示，刚体姿态校正单元270，用于根据所述旋转和平移信息校正所述imu获取到摄像头拍摄时刻的刚体的姿态；将所述摄像头光学系统中刚体的位置和所述校正后的刚体姿态进行输出，实现所述刚体的追踪。

由此可知，本发明的技术方案，将基于计算机图形学的追踪算法与基于imu的追踪算法相融合，首先，获取摄像头采集的刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态，根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对，解决了现有特征匹配算法计算量大、应用场景局限性大等问题；

其次，根据匹配的特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息，解决了计算机图形学追踪算法由于摄像头获取的图像分辨率不高导致的空间定位精度不高的问题；

最后，根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态解决了imu在空间定位过程中存在的时间漂移的问题。本发明的技术方案，将计算得到的摄像头光学系统中的刚体位置和纠正后的姿态进行输出，实现刚体的追踪，达到了将基于计算机图形学的追踪算法与imu的追踪算法进行优势互补的目的，提升了刚体追踪的精确度。

仍如图4所示，所述装置200还包括：伪特征点去除单元280；

所述伪特征点去除单元280，用于根据所述计算得到的图像中特征点相对于摄像头的三维位置坐标计算所述图像中的特征点之间的第一相对距离；根据预存的刚体上每个特征点相对于刚体重心的三维位置信息计算刚体所有特征点之间的第二相对位置距离；将所述第一相对距离与所述第二相对距离进行匹配，去除所述图像特征点中的伪特征点，避免了由于图像中的特征点存在杂质，导致刚体的定位精度降低的情况的发生。

在本发明的一个实施例中，所述图像特征点身份信息确定单元240，用于利用imu获取对应所述摄像头拍摄时刻的刚体的姿态，根据所述刚体姿态将所述计算得到的图像中特征点相对于摄像头的三维位置坐标恢复至所述刚体初始姿态下的三维位置坐标；根据所述特征点在刚体初始姿态下的三维位置坐标与预存的刚体上每个特征点相对于刚体重心的三维位置信息，利用空间位置信息(例如，上下左右等)确定所述图像中特征点的身份信息(例如，特征点的id)。获取图像中的特征点的身份信息有助于将图像中的特征点与预存的刚体上的特征点进行精确匹配。

在本发明的一个实施例中，所述刚体位置信息确定单元260，用于根据所述旋转和平移信息计算所述刚体上所有特征点相对于摄像头的三维位置坐标；根据所有特征点相对于摄像头的三维位置坐标计算摄像头光学系统中刚体的位置信息。利用刚体上所有特征点相对于摄像头的三维位置坐标计算刚体的位置信息，相比于现有的基于计算机图形学的追踪技术利用刚体上部分特征点相对于双目摄像头的三维位置坐标计算刚体的位置信息更加精确，解决了由于图像分辨率不高导致空间定位精度不高的问题，实现刚体的精确追踪。

需要说明的是，图3和图4所示的装置的工作过程与图1和图2所示的方法的各实施例的实现步骤对应相同，相同的部分不再赘述。

实施例三

图5是本发明一个实施例的一种刚体追踪的系统的示意图，如图5所示，所述系统30包括摄像头310、刚体320(在本实施例中，刚体指的是头戴显示设备)和控制端330，其中，imu340设置在所述刚体上(在本实施例中，将imu设置在刚体的重心位置)；在实际应用中，刚体可以是任意一种虚拟现实设备。

所述摄像头310，用于采集所述刚体320的图像，并将所述刚体320的图像发送至所述控制端330；

所述imu340，用于采集所述刚体320初始状态下的姿态和与所述刚体320对应的所述摄像头310拍摄时刻的姿态，并将所述姿态发送至所述控制端330；

所述控制端330，用于获取摄像头310采集的所述刚体320运动时的图像，确定所述图像中的所述刚体320的特征点相对于摄像头310的三维位置坐标；获取所述imu340采集的所述刚体320初始状态下的姿态和与所述刚体320对应的所述摄像头310拍摄时刻的姿态；根据所述刚体320初始状态下的姿态、所述摄像头310拍摄时刻的姿态和所述预存刚体320上每个特征点的身份信息及相对于所述刚体320重心的三维位置信息，确定所述图像中所述刚体320的特征点的身份信息；根据所述图像中所述刚体320的特征点的身份信息，将获取到的所述图像中所述刚体320的特征点相对于摄像头310的三维位置坐标与预存的刚体320上每个特征点相对于所述刚体320重心的三维位置信息进行匹配，得到匹配的特征点对；根据所述特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头310的光学系统中刚体320的位置信息。

在本发明的一个实施例中，所述控制端330，还用于根据所述旋转和平移信息校正所述imu320获取到的摄像头310拍摄时刻的所述刚体320的姿态。将所述摄像头310光学系统中刚体320的位置和所述校正后的刚体320姿态进行输出，实现所述刚体320的追踪。

在本发明的一个实施例中，摄像头310与控制端330通过有线或者无线的方式进行通信；imu340与控制端330通过有线或者无线的方式进行通信。在实际应用中，可以根据实际需要，选择合适的通信方式。

由此可知，本发明的技术方案，将基于计算机图形学的追踪算法与基于imu的追踪算法相融合，首先，获取摄像头采集的刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态，根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对，解决了现有特征匹配算法计算量大、应用场景局限性大等问题；

其次，根据匹配的特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息，解决了计算机图形学追踪算法由于摄像头获取的图像分辨率不高导致的空间定位精度不高的问题；

最后，根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态解决了imu在空间定位过程中存在的时间漂移的问题。本发明的技术方案，将计算得到的摄像头光学系统中的刚体位置和纠正后的姿态进行输出，实现刚体的追踪，达到了将基于计算机图形学的追踪算法与imu的追踪算法进行优势互补的目的，提升了刚体追踪的精确。

在本发明的一个实施例中，所述控制端330，还用于根据所述计算得到的图像中特征点相对于摄像头310的三维位置坐标计算所述图像中的特征点之间的第一相对距离；

根据预存的刚体上每个特征点相对于刚体重心的三维位置信息计算刚体所有特征点之间的第二相对位置距离；

将所述第一相对距离与所述第二相对距离进行匹配，去除所述图像特征点中的伪特征点，避免图像中的特征点存在杂质，影响刚体的定位精度。

需要说明的是，图5所示的系统的工作过程与图1和图2所示的方法的各实施例的实现步骤对应相同，相同的部分不再赘述。

综上所述，本发明的技术方案，将基于计算机图形学的追踪算法与基于imu的追踪算法相融合，首先，获取摄像头采集的刚体运动时的图像，确定所述图像中的所述刚体的特征点相对于摄像头的三维位置坐标；获取所述imu采集的所述刚体初始状态下的姿态和与所述刚体对应的所述摄像头拍摄时刻的姿态，根据所述刚体初始状态下的姿态、所述摄像头拍摄时刻的姿态和所述预存刚体上每个特征点的身份信息及相对于所述刚体重心的三维位置信息，确定所述图像中所述刚体的特征点的身份信息；根据所述图像中所述刚体的特征点的身份信息，将获取到的所述图像中所述刚体的特征点相对于摄像头的三维位置坐标与预存的刚体上每个特征点相对于所述刚体重心的三维位置信息进行匹配，得到匹配的特征点对，解决了现有特征匹配算法计算量大、应用场景局限性大等问题；

其次，根据匹配的特征点对，确定所述图像中的特征点相对于初始姿态下的旋转和平移信息；并根据所述旋转和平移信息计算摄像头光学系统中刚体的位置信息，解决了计算机图形学追踪算法由于摄像头获取的图像分辨率不高导致的空间定位精度不高的问题；

最后，根据所述旋转和平移信息校正所述imu获取到的摄像头拍摄时刻的所述刚体的姿态解决了imu在空间定位过程中存在的时间漂移的问题。本发明的技术方案，将计算得到的摄像头光学系统中的刚体位置和纠正后的姿态进行输出，实现刚体的追踪，达到了将基于计算机图形学的追踪算法与imu的追踪算法进行优势互补的目的，提升了刚体追踪的精确。

以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。