一种3D目标6DoF精确定位方法及系统与流程

本发明涉及空间定位技术，尤其涉及利用计算机视觉技术，实现3d目标6dof精确定位的方法及系统。

背景技术：

1、6dof六自由度目标识别（six degrees of freedom object recognition）是在三维空间中通过对物体的特征进行识别，确定物体的位置和姿态，从而实现对物体的追踪和定位。此外，rgbd相机也可以实现对物体表面的纹理、颜色等特征的提取和分析，从而实现对物体的精准分类和识别。具体来说，6dof目标识别能够识别并确定物体在三个方向上的平移和三个方向上的旋转，因此广泛应用于计算机视觉、机器人、增强现实、虚拟现实等领域。

2、其中在计算机视觉领域，6dof目标识别主要应用于自动驾驶、智能家居、安防监控等场景中，智能汽车或扫地机器人可根据识别到物体的三维位姿实现自动导航。

3、在机器人领域，6dof技术可以帮助机器人实现对环境中的物体进行快速准确的识别和定位，从而更好地完成拣选、包装、物流等任务。而在增强现实和虚拟现实领域，可以实现虚拟物体与实际物体的精确对位，从而实现更加真实的增强现实和虚拟现实体验。例如，在真实环境中显示虚拟物体，使用户能够更加真实地感受到虚拟物体的存在。

4、目前，基于rgbd相机的6dof目标识别是指利用rgbd相机（同时获取彩色图像和深度图像）进行目标检测和识别，从而实现对物体在三维空间中的位置和姿态的精确测量和追踪。相对于传统的rgb相机，rgbd相机能够提供更加丰富的信息，包括物体的三维形状和深度信息，使得目标识别更加准确和稳定。

5、由于rgbd相机的工作原理是，通过结合红外光与彩色图像来测量物体到相机的距离。因此深度图像提供了物体在三维空间中的坐标信息，使得6dof目标识别能够实现更加精确的定位和追踪。

6、然而，受技术约束，当前市场上的rgbd相机分为：精度高但成像效率低和低精度高效率两种情况。但各个应用领域对6dof目标识别精度和效率的需求，却集中在高精度、高效率。

7、因此在没有革命性的rgbd相机出现之前，如何在低精度高效率的rgbd相机中获取高精度目标物体位置与姿态，是当前视觉识别算法的主要研究方向。

技术实现思路

1、本发明的主要目的在于提供一种3d目标6dof精确定位方法及系统，以实现在低精度rgbd相机传感器的基础上，获取目标物体精确的6dof三维位置与姿态。

2、为了实现上述目的，根据本发明的第一个方面，提供了一种3d目标6dof精确定位方法，其步骤包括：

3、步骤s100基于rgbd相机采集扫描场景的图像m、深度图d及相机内参k，计算出扫描场景的点云数据p，以供确定目标模型在扫描场景中的初始位姿；

4、步骤s200统计图像m中目标边缘特征点集合到对应目标模型中尖锐边缘me集合的投影线的垂直距离的平方和，作为线特征的匹配误差；

5、步骤s300计算点云数据p中目标特征点到对应目标模型所在平面的距离，作为面特征的匹配误差；

6、步骤s400计算目标模型可观测区域内的纹理与图像m在相同像素点处的颜色差异，作为纹理特征的匹配误差；

7、步骤s500将、和做非线性迭代优化，设置优化变量为目标模型在相机坐标系下的6dof位姿，计算出累计误差最小的目标模型位姿。

8、在可能的优选实施方式中，其中步骤s200中所述目标边缘特征点集合的获取步骤包括：

9、根据相机内参k及初始位姿，将目标模型尖锐边缘me集合投影到像平面上，计算图像m在投影线段附近的梯度值和梯度方向，从而找到与投影线段匹配的目标边缘特征点集合。

10、在可能的优选实施方式中，其中将目标模型尖锐边缘me集合投影到像平面上的步骤包括：

11、设尖锐边缘其中一条线段，线段的起点为，终点为，变换到相机坐标系下的坐标值为，根据相机内参k可以得到相机坐标系下线段的起点、终点在图像上的投影点为：

12、；

13、其中，及分别为及的z 轴坐标值，以极坐标的形式建立像平面上过和的直线方程为：

14、；

15、其中，为原点到直线的垂直距离，为过原点的垂线与x轴正方向的夹角。（x，y）为图像坐标系下位于直线上的坐标点。

16、在可能的优选实施方式中，其中计算图像m在投影线段附近的梯度值和梯度方向，从而找到与投影线段匹配的目标边缘特征点集合的步骤包括：

17、以和连线为轴线，与垂直于轴线方向±n个像素区域的矩形框为模板区域roi；将图像m对roi区域内的像素求梯度，并计算梯度方向与直线的夹角；提取夹角符合阈值的像素点为目标边缘特征点建立集合。

18、在可能的优选实施方式中，其中步骤s200中所述目标模型中尖锐边缘me集合的获取步骤包括：

19、由目标模型中任意一个三角面开始判断该三角面与相连接的数个三角面的法向夹角，如果相连的两个三角面的夹角呈预设锐角，即认为两个三角面相交的边为尖锐边缘me；持续判断，直至所有三角面全部判断完成后，获取所有尖锐边缘me的集合。

20、在可能的优选实施方式中，其中步骤s300中计算点云数据p中目标特征点到对应目标模型所在平面的距离步骤包括：

21、步骤s310根据初始位姿和点云数据p，将目标模型的三角面变换到相机坐标系下；

22、步骤s320计算三角面的法线与相机光轴方向的夹角，取大于120°的三角面为视野内可视三角面集合；

23、步骤s330遍历中三角面将其投影到像平面上，计算像平面上三角形的面积，统计深度图d在三角形内有效像素面积占三角形总面积的比例，取占比达标的三角面标记为可用，并将位于三角形内像素对应的目标特征点点云数据与三角面建立匹配关系；

24、步骤s340统计中每个点到三角面的垂直距离。

25、在可能的优选实施方式中，其中步骤s400中计算目标模型可观测区域内的纹理与图像m在相同像素点处的颜色差异的步骤包括：

26、步骤s410遍历所有三角面将其投影到像平面上，对像平面位于投影三角形内部的像素点，根据投射变换计算其与三角面的交点，以交点处的纹理信息作为该像素点的纹理值，获取渲染图为；

27、步骤s420计算归一化后的渲染图与图像m在相同像素点处的三通道像素值的颜色差异。

28、在可能的优选实施方式中，其中步骤s500中采用levenberg-marquarelt算法对、和做非线性迭代优化。

29、在可能的优选实施方式中，其中步骤s100中初始位姿的获取步骤包括：

30、对目标模型表面做点采样，得到点云格式的模型点云；分别在模型点云和点云数据p中，提取特征点及对应的特征描述子，根据特征描述子的相似性，寻找模型点云与点云数据p匹配的目标特征点；根据目标特征点计算模型点云到对应点云数据p的刚性变换，以获取目标模型在扫描场景中的初始位姿。

31、为了实现上述目的，对应上述方法，本发明的第二个方面，还提供了一种3d目标6dof精确定位系统，其包括：

32、存储单元，用于存储包括如上任一所述3d目标6dof精确定位方法步骤的程序，以供控制单元，处理单元，传输单元适时调取执行；

33、控制单元，用于控制rgbd相机采集扫描场景的图像m、深度图d；

34、处理单元，用于基于相机内参k、图像m、深度图d，计算出扫描场景的点云数据p，以供确定目标模型在扫描场景中的初始位姿；统计图像m中目标边缘特征点集合到对应目标模型中尖锐边缘me集合的投影线的垂直距离的平方和，作为线特征的匹配误差；计算点云数据p中目标特征点到对应目标模型所在平面的距离，作为面特征的匹配误差；计算目标模型可观测区域内的纹理与图像m在相同像素点处的颜色差异，作为纹理特征的匹配误差；之后将、和做非线性迭代优化，设置优化变量为目标模型在相机坐标系下的6dof位姿，计算出累计误差最小的目标模型位姿；

35、传输单元，用于传输目标模型位姿。

36、通过本发明提供的该3d目标6dof精确定位方法及系统，巧妙的利用了点云和纹理来快速区分物体定位目标模型位姿，同时利用图像高精度边界定位能力，弥补点云和纹理边界模糊的问题，从而创造出了一种融合图像和点云的6dof位姿识别方案，以支持在低精度rgbd相机传感器的基础上，也能实现三维物体高精度的位姿识别。