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

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

The method and related equipment of orientation of active rigid body in multi camera environment

【技术实现步骤摘要】
多相机环境中主动式刚体的位姿定位方法及相关设备
本专利技术涉及计算机视觉
，尤其涉及一种多相机环境中主动式刚体的位姿定位方法及相关设备。
技术介绍
传统的光学动捕方法是通过动捕相机内的超大功率近红外光源发出红外光，照射在被动式标记点上；涂有高反光材料的标记点反射被照射到的红外光，而这部分红外光和带有背景信息的环境光会经过低畸变镜头，到达摄像机红外窄带通滤光单元。由于红外窄带通滤光单元的通光波段跟红外光源的波段一致，因此，带有冗余背景信息的环境光会被过滤掉，只剩下带有标记点信息的红外光通过，并被摄像机感光元件记录。感光元件再将光信号转化为图像信号输出到控制电路，而控制电路中的图像处理单元使用现场可编程门阵列(FieldProgrammableGateArray，FPGA)，以硬件形式对图像信号进行预处理，最后向跟踪软件流出标记点的2D坐标信息。跟踪定位软件采用计算机多目视觉原理，根据图像二维点云间的匹配关系及相机的相对位置和朝向，计算点云在三维捕捉空间内的坐标及方向。以点云的三维坐标为基础，跟踪定位软件通过识别不同的刚体结构，解算出每个刚体在捕捉空间内的位置及朝向。以上被动式动捕方法存在以下缺点：第一，要求动捕相机具有比较复杂的图像处理器件，相机造价相对较高；第二，要求标记点涂有高反光材料，在使用过程中易造成磨损，影响系统正常运行；第三，跟踪定位依赖于刚体的结构，刚体的设计使得刚体数量很局限，而且刚体的识别和跟踪需要相机捕捉到刚体上全部标记点，使用环境非常严苛。专利
技术实现思路
本专利技术的主要目的在于提供一种多相机环境中主动式刚体的位姿定位方法及相关设备，旨在解决目前被动式动捕方法中对动捕相机要求较高、跟踪定位依赖于刚体结构等技术问题。为实现上述目的，本专利技术提供一种多相机环境中主动式刚体的位姿定位方法，所述方法包括以下步骤：获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿；获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。可选地，所述将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标，包括：将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，利用多视几何中的三角测量原理，通过奇异值分解求解最小二乘法方法，解算得到一个三维空间点，遍历所有两两匹配的相机后，得到一组三维空间点，一组所述三维空间点即为所述标记点的三维空间点坐标；判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。可选地，所述将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标，包括：计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标；将所述标记点的三维空间点编码赋值给所述刚体坐标对应的编码，得到所述标记点的刚体编码。可选地，所述通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿，包括：求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计时，将所述三维空间点坐标和所述刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，所述等式如下公式：P1＝RP1＇+T其中，P1为第一帧所述三维空间点坐标，P1＇为第一帧所述刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；根据所述欧式变换旋转矩阵和所述平移矩阵得到刚体的初始位姿。进一步地，为实现上述目的，本专利技术还提供一种多相机环境中主动式刚体的位姿定位装置，包括：获取数据模块，用于获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；计算三维空间数据模块，用于将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；计算刚体数据模块，用于将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；确定刚体初始位姿模块，用于通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿；刚体定位模块，用于获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。可选地，所述计算三维空间数据模块，包括：计算三维空间点坐标组单元，用于将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，利用多视几何中的三角测量原理，通过奇异值分解求解最小二乘法方法，解算得到一组三维空间点坐标；剔除单元，用于判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；确定三维空间点坐标单元，用于计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；确定三维空间点编码单元，用于将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。可选地，所述计算刚体数本文档来自技高网...

【技术保护点】
1.一种多相机环境中主动式刚体的位姿定位方法，其特征在于，所述方法包括以下步骤：/n获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；/n将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；/n将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；/n通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿；/n获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。/n

【技术特征摘要】
1.一种多相机环境中主动式刚体的位姿定位方法，其特征在于，所述方法包括以下步骤：
获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述二维空间点坐标分为同类，且标记于同一个标记点下；
将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标；
将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标；
通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿；
获取多个所述相机的相机参数，根据所述相机参数计算所述标记点第二帧的重投影坐标，根据所述标记点第二帧的所述刚体坐标，确定重投影误差，利用梯度下降法，使用重投影误差构造代价函数，并使所述代价函数最小化，得到刚体的运动位姿，根据初始位姿和运动位姿对所述刚体跟踪定位。


2.根据权利要求1所述的多相机环境中主动式刚体的位姿定位方法，其特征在于，所述将多个所述相机两两进行匹配，根据两个所述相机的空间位置数据及同类同帧的多个所述二维空间点坐标，得到每个所述标记点每帧的三维空间点编码和三维空间点坐标，包括：
将捕捉到的同一个标记点的所有相机进行两两匹配，对匹配的两个相机在同帧中捕捉到的两个所述二维空间点坐标，通过奇异值分解求解最小二乘法方法，解算得到一个三维空间点，遍历所有两两匹配的相机后，得到一组三维空间点，一组所述三维空间点即为所述标记点的三维空间点坐标；
判断所述三维空间点坐标是否处于预设的阈值范围内，若超过所述阈值范围，则剔除所述三维空间点坐标，得到剔除后的一组所述三维空间点坐标；
计算一组所述三维空间点坐标的平均值，通过高斯牛顿法优化，得到所述标记点的三维空间点坐标；
将所述标记点的二维空间点编码赋值给所述三维空间点坐标对应的编码，得到所述标记点的三维空间点编码。


3.根据权利要求1所述的多相机环境中主动式刚体的位姿定位方法，其特征在于，所述将同帧的所有三维空间点编码和三维空间点坐标，转化为刚体坐标系下的刚体坐标，得到每个所述标记点每帧的刚体编码和刚体坐标，包括：
计算同帧的多个所述标记点对应的所述三维空间点坐标的坐标平均值，将所述坐标平均值记为刚体坐标系下的原点；
分别计算原点与同帧的每个所述标记点对应的所述三维空间点坐标之间的差值，得到每个所述标记点每帧的刚体坐标；
将所述标记点的三维空间点编码赋值给所述刚体坐标对应的编码，得到所述标记点的刚体编码。


4.根据权利要求1所述的多相机环境中主动式刚体的位姿定位方法，其特征在于，所述通过求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计，确定刚体的初始位姿，包括：
求解第一帧所述三维空间点坐标到所述刚体坐标的位姿估计时，将所述三维空间点坐标和所述刚体坐标代入等式中，通过迭代最近点求解欧式变换旋转矩阵和平移矩阵，所述等式如下公式：
P1＝RP1＇+T
其中，P1为第一帧所述三维空间点坐标，P1＇为第一帧所述刚体坐标，R为刚体的欧式变换旋转矩阵，T为平移矩阵；
根据所述欧式变换旋转矩阵和所述平移矩阵得到刚体的初始位姿。


5.一种多相机环境中主动式刚体的位姿定位装置，其特征在于，所述装置包括：
获取数据模块，用于获取多个相机捕捉的相邻两帧的二维空间点坐标、所述二维空间点坐标对应的二维空间点编码和多个所述相机的空间位置数据，将所述二维空间点编码相同的多个所述...

【专利技术属性】
技术研发人员：王越，许秋子，
申请(专利权)人：深圳市瑞立视多媒体科技有限公司，
类型：发明
国别省市：广东;44