[发明专利]一种基于三维几何模型的单目无纹理三维物体姿态跟踪方法

权利要求书

1.一种基于三维几何模型的单目无纹理三维物体姿态跟踪方法，其特征在于，包括步骤如下：

1)初始化系统状态，置重定位计数器为0；

2)采集三维物体的运动图像序列或视频；

3)判断系统当前状态；

3.1)如果系统状态为初始化状态，则从参数文件中读取三维物体的初始姿态；

3.2)如果系统状态为跟踪状态，则应用运动模型为三维物体计算初始姿态；具体计算方法为：

系统根据上一帧的姿态和上一帧姿态的速度，应用如下运动模型计算当前帧姿态优化的初始姿态；

p′_k+l＝ln(exp(v_k)exp(p_k))

其中，p_k是第k帧三维物体的姿态，p′_k+1是第k+1帧姿态优化的初始姿态，v_k是第k帧三维物体姿态的速度；

v_k＝ln(exp(p_k)exp(p_k-1)^-1)

其中，p_k-1是第k-1帧三维物体的姿态；

3.3)如果系统为重定位状态，则应用快速模板检测算法为三维物体计算初始姿态；以关键帧集合中的图像为模板，采用LINE2D快速物体检测算法定位三维物体在图像中的位置，将物体的初始姿态设置为物体检测中最佳模板对应的姿态；

4)基于边缘距离场的姿态优化

4.1)根据三维物体的初始姿态，渲染三维物体的几何模型，获取初始姿态下三维物体的深度图像，在深度图像上提取三维物体的轮廓投影，将轮廓投影反投影到三维物体几何模型表面得到三维轮廓点集Φ；

4.2)使用Canny边缘检测算法提取当前帧图像的边缘图，在边缘图上使用快速距离场变换算法计算得到图像的边缘距离场D；

4.3)对于每一个三维轮廓点X_i∈Φ，其匹配误差表示为

其中，K是摄像机内参矩阵，为数映射函数，其将李代数表示的6维姿态参数转换为4x4的欧氏变换矩阵，是X_i的齐次坐标表示，函数将齐次坐标转化为非齐次坐标；因此，三维轮廓点集Φ和图像边缘距离场D间的匹配误差为所有三维轮廓点匹配误差之和，即目标能量函数：

其中，是鲁棒估计算法的权值函数，系统选择Tukey鲁棒估计算法，故

其中c是固定阈值，表示投影点在边缘距离场D中的最大值；

4.4)对于第k+1帧姿态，以步骤3)中得到的初始姿态为迭代初始点，通过L-M优化算法迭代求解E(p)，得到最优姿态参数值p_k+1；

5)迭代优化后，在最优姿态下将三维轮廓点投影到图像的边缘距离场，读取每个三维轮廓点的投影在图像边缘距离场中的值W，计算所有W值的平均值，即为每个三维轮廓点的平均残差；如果每个三维轮廓点的平均残差小于残差阈值，则判定当前姿态优化成功，更新关键帧集合、更新运动模型、置系统状态为跟踪状态，最后输出最优姿态参数值，然后转至步骤8)；否则判定当前姿态优化失败，则转至步骤6)；

6)判断系统当前状态；如果系统状态为初始化状态，则保持系统状态，并输出三维物体初始姿态，然后转至步骤8)；如果系统状态为跟踪状态，置系统状态为重定位状态，下一帧进行错误恢复，并输出三维物体初始姿态，然后转至步骤8)；如果系统状态为重定位状态，重定位计数器加1，并转至步骤7)；

7)判断重定位次数是否小于次数阈值；如果重定位次数小于次数阈值，则置系统状态为重定位状态，输出三维物体初始姿态；如果重定位次数大于等于次数阈值，判定重定位已不能完成错误恢复，置重定位计数器为0，置系统状态为初始化状态，输出三维物体初始姿态；

8)判断当前帧是否为最后一帧；如果当前帧为最后一帧，则工作结束；如果当前帧不是最后一帧，则将下一帧作为当前帧，重复步骤2)～7)，直至当前帧为最后一帧。

2.根据权利要求1所述的基于三维几何模型的单目无纹理三维物体姿态跟踪方法，其特征在于，所述步骤5)中，更新关键帧集合的具体过程为，判断当前帧姿态到关键帧集合所有姿态的最小距离，如果最小距离大于距离阈值，则当前帧加入关键帧集合；同时对关键帧计数，如果关键帧总数大于指定阈值，则移除关键帧集合中最早被加入的关键帧。

3.根据权利要求1所述的基于三维几何模型的单目无纹理三维物体姿态跟踪方法，其特征在于，所述步骤5)中，更新运动模型的具体过程为，依据步骤3.2)中的公式根据当前帧物体姿态和上一帧物体的姿态计算当前帧三维物体姿态的速度。