CN110706285A - 基于cad模型的物***姿预测方法 - Google Patents

Abstract

本发明公开了一种基于CAD模型的物***姿预测方法，涉及图像处理方法技术领域。所述方法包括如下步骤：通过标定获取单目摄像头的相关参数，并利用CAD模型生成粗匹配所需数据；检测识别图像中的物体并输出图像的遮罩，通过物体的遮罩得到物体的相关轮廓信息；通过物体的相关轮廓信息结合粗匹配数据得到物体的粗匹配位姿，然后通过迭代算法，得到物体的精确位姿。所述方法可以作为实时性要求不高时对物***姿检测的算法，其检测精度较高且有较强的抗干扰性能。

Description

基于CAD模型的物***姿预测方法

技术领域

本发明涉及图像处理方法技术领域，尤其涉及一种基于CAD模型的物***姿预测方法。

背景技术

增强现实(Augmented Reality，AR)以计算机图形技术和可视化技术为基础，在三维空间中增添定位虚拟物体，能够将真实场景与虚拟场景的信息集成，具有实时交互性。自基于增强现实的诱导维修的概念提出后，AR在维修领域的研究逐渐深入。如以增强现实技术的机器人，在执行抓取、焊接等任务时，需要通过摄像头采集的视觉信息来预先获取准确的物体三维位姿信息，此外在无人驾驶、航空航天、深海作业、武器制导等方面都需要利用视觉传感器信息来预先判断物体的三维位姿。目前增强现实的传感器主要依赖于摄像头、激光雷达、超声波雷达等，其中摄像头又分为单目摄像头和双目摄像头，其中双目摄像头存在体积大、重量重、价格高、易损坏的问题，而超声波雷达存在精度不高，实时性差，不能有遮挡，易受噪声影响的问题。

发明内容

本发明所要解决的技术问题是如何提供一种成本低且可准确的得到物***姿的识别方法。

为解决上述技术问题，本发明所采取的技术方案是：一种基于CAD模型的物***姿预测方法，其特征在于包括如下步骤：

通过标定获取单目摄像头的相关参数，并利用CAD模型生成粗匹配所需数据；

检测识别图像中的物体并输出图像的遮罩，通过物体的遮罩得到物体的相关轮廓信息；

通过物体的相关轮廓信息结合粗匹配数据得到物体的粗匹配位姿，然后通过迭代算法，得到物体的精确位姿。

进一步的技术方案在于，通过标定获取单目摄像头的相关参数的方法包括如下步骤：

构建相机成像模型：

M为三位空间点，m为M在图像平面投影的像点，按照相机涉及的坐标系之间的关系可得到世界坐标系到像素坐标的投影：

可将(1)写成(2)的形式

其中a_x，a_y分别是图像水平轴和垂直轴的尺度因子；K为相机内部参数矩阵；M₁中包含旋转矩阵和平移矢量，M₁中参数是由相机坐标系相对于世界坐标系的位置决定的，因此称M₁为相机外部参数矩阵；内部参数和外部参数矩阵的乘积M为投影矩阵；X_W为世界坐标系中物体中心W所在的x轴坐标，Y_W为世界坐标系中物体中心W所在的y轴坐标，Z_W为世界坐标系中物体中心W所在的z轴坐标；

相机焦距为f所在轴为z正方向，x与y轴处在光心O所在平面，以光心O为相机坐标系原点，在此相机坐标系下则物体中心所在位置用W来表示，其中：

W＝(W_x,W_y,W_z) (3)

规定物体中心就是物体CAD模型中心所在位置，若P＝(u,v)为物体对应像素在图像上的坐标，K为相机内参矩阵，则可以得到此等式：

此等式表示实际物体中心所在位置W在相机坐标系下经过相机内参K后投影到图像后的二维坐标位置P。

进一步的技术方案在于，利用CAD模型生成粗匹配数据的方法如下：

首先通过物体CAD模型在指定位姿下渲染出物体的遮罩，通过物体的遮罩得到物体的边界框，而后根据不同的需要在边界框上每隔一定距离对物体轮廓进行采样；

以左边界框长度L为基准，把L分为n等份，每隔L/n为一个采样横坐标点，遍历每一个轮廓上的点在其横坐标等于采样横坐标点时计算其到左边框的距离，由于每个采样横坐标点可能对应多个轮廓采样距离，所以取多个距离中的最大和最小值作为此采样横坐标上对轮廓采样的采样值，把轮廓信息变为成一组采样值；

对采样值进行归一化，即把左边界框长度统一到一个单位下；

在指定距离上，以物体CAD模型中心为中心，在不同的旋转角度对物体的轮廓进行采样，把轮廓采样信息与相对应的位姿信息保存，得到物体的粗匹配的模板数据。

进一步的技术方案在于，所述检测识别图像中的物体并输出图像的遮罩的方法如下：

利用Mask-RCNN神经网络进行图像识别，输出物体的类别与物体的遮罩。

进一步的技术方案在于，在训练Mask-RCNN神经网络时，利用blender以及Opencv软件自动生成了数据集来训练。

进一步的技术方案在于，粗匹配位姿方法如下：

刚体的位姿包括旋转R和位移T两部分，其旋转部分的匹配过程如下：

首先把输出的轮廓信息做归一化处理，统一到相同尺度下进行比较；

若对物体实际遮罩采样数据为Sⁱⁿ，模板数据中第i组数据为Sⁱ，每组有n个采样值，则计算实际遮罩采样数据与模板中每组数据的L₁距离，第i组数据的L₁距离L_i为：

理想情况下，在位姿相同时采样值应该一致，即在模板数据中使此距离为0的旋转角度，即为此轮廓所对应旋转角度，所以取所有结果中满足阈值情况下的最小值所对应旋转角度为当前匹配得到的旋转角度，不满足阈值认为匹配失败；

在粗匹配时，误差控制在欧拉角每个自由度误差不大于12°；而后把此欧拉角信息转化为旋转矩阵R，即得到物体的旋转信息；

其平移部分算法如下：

在生成模板数据时，由于是在指定距离上对物体进行采样，且CAD模型大小已知，所以物体对应的包围框大小与其距离成反比，即包围框越小距离越远，这与人肉眼认知一致，则模型中心点与相机光心的距离D即可以通过(5)求出:

D＝(w_in/w_i)·D_i (6)

其中w_in为物体识别输出边界框宽，w_i为与其旋转相匹配的模板数据的边界框宽，D_i为模板数据采集时指定的距离，D即为模型中心点与相机光心的距离；

CAD模型大小先验信息已知，既可以计算出模板中每像素所代表的实际物理距离，进而可以计算出物体的位移向量t_z：

其中，t_x为物体在x轴的位移量，t_y为物体在y轴的位移量，在得到物体的旋转R和位移T之后，结合相机的内外参得到物体的世界坐标。

进一步的技术方案在于，通过迭代算法，得到物体的精确位姿的方法如下：

若粗匹配物体旋转为A＝(ψ，θ,φ)，则在此基础上，每个坐标轴都加减一个角度Δε，Δε设置为粗匹配间隔的一半，在粗匹配旋转空间求出其相邻空间的若干个角度，利用相邻空间的若干个角度结合CAD模型得到物体的轮廓，利用轮廓采样法并结合(5)式，求出使得(5)式L_i值最小的旋转A₁＝(ψ₁，θ₁,φ₁)，即得到迭代一次后的物体旋转角度；

而后通过不断把角度Δε减半得到更小范围的角度值进行迭代，最终可以得到使得(3)式为0的旋转角度；

结合粗匹配时得到的物体平移信息，得到物体的精确位姿。

采用上述技术方案所产生的有益效果在于：本申请所述方法首先通过标定获取摄像头的相关参数，并利用CAD模型生成粗匹配所需数据，而后利用深度神经网络或者其他算法检测识别图像中的物体并输出图像的遮罩，通过物体的遮罩可以得到相关轮廓信息，此轮廓信息结合粗匹配数据可以得到物体的粗匹配位姿，然后通过迭代算法，可以得到物体的精确位姿。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1是本发明实施例所述方法的流程图；

图2是本发明实施例中坐标系的关系图；

图3是本发明实施例中相机针孔模型示意图；

图4是本发明实施例中物体轮廓采样的结果图；

图5是本发明实施例中Mask-RCNN图像分割效果图；

图6是本发明实施例中Mask-RCNN图像识别结果图；

图7是本发明实施例中粗匹配与迭代后位姿对比图；

图8是本发明实施例中遮挡情况下物***姿精度图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

如图1所示，本发明实施例公开了一种基于CAD模型的物***姿预测方法，包括如下步骤：

首先通过标定获取摄像头的相关参数，并利用CAD模型生成粗匹配所需数据，而后利用深度神经网络或者其他算法检测识别图像中的物体并输出图像的遮罩，通过物体的遮罩可以得到相关轮廓信息，此轮廓信息结合粗匹配数据可以得到物体的粗匹配位姿，然后通过迭代算法，可以得到物体的精确位姿。

下面对上述方法进行详细说明：

相机成像模型：

M为三位空间点，m为M在图像平面投影的像点。按照相机涉及的坐标系之间的关系可得到世界坐标系到像素坐标的投影(坐标系关系如图2所示)：

可将(1)写成(2)的形式

其中a_x，a_y分别是图像水平轴和垂直轴的尺度因子；K包含焦距、主点坐标等相机内部参数，因此称K为内部参数矩阵；M₁中包含旋转矩阵和平移矢量，M₁中参数是由相机坐标系相对于世界坐标系的位置决定的，因此称为相机的外部参数矩阵；内部参数和外部参数矩阵的乘积M称为投影矩阵。通过比较式(1)和式(2)，可以很容易地确定这些矩阵代表的相机内、外参的具体表现形式，相机标定就是确定相机的内部参数和外部参数。

相机标定：

假设相机为针孔模型，如图3所示，一个物体的位姿是物***置和姿态的统称。

相机焦距为f所在轴为z正方向，x与y轴处在光心O所在平面，以光心O为相机坐标系原点，在此相机坐标系下则物体中心所在位置可以用W来表示，其中：

W＝(W_x,W_y,W_z) (3)

规定物体中心就是物体CAD模型中心所在位置，一般为体积中心。若P＝(u,v)为物体对应像素在图像上的坐标，K为相机内参矩阵，则可以得到此等式：

此等式表示实际物体中心所在位置W在相机坐标系下经过相机内参K后投影到图像后的二维坐标位置P。

所以想要求得物体三维坐标位置，必须标定相机内参K，其中标定方法很多，在本申请中使用Opencv软件提供的相机标定方法获取K。

利用CAD模型生成粗匹配数据：

在利用物体CAD模型生成模板数据中最核心的算法就是基于物体轮廓的采样算法。首先通过物体CAD模型可以在指定位姿下渲染出物体的Mask，通过物体的Mask可以得到物体的边界框，而后根据不同的需要在边界框上每隔一定距离对物体轮廓进行采样，如图4所示为在左边框每隔一定距离对物体轮廓进行采样。

以左边界(其它边界类似)框长度L为基准，把L分为n等份，每隔L/n为一个采样横坐标点，遍历每一个轮廓上的点在其横坐标等于采样横坐标点时计算其到左边框的距离，由于每个采样横坐标点可能对应多个轮廓采样距离，所以取多个距离中的最大和最小值作为此采样横坐标上对轮廓采样的采样值。这样就把轮廓信息变为成了一组采样值。

由于轮廓可能会大小变化，所以需要对采样值进行归一化，即把左边界框长度统一到一个单位下，在实验中，把左边界框统一长度为128px，这样既可以保证采样的精度，也可以保证采样速度。

上述采样方式的好处是得到了轮廓的一组特征，即采样值，这个值对于轮廓具有缩放不变性，但是对物体旋转很敏感，且数据维度一致，便于比较。

在指定距离上，以物体CAD模型中心为中心，在不同的旋转角度对物体的轮廓进行采样，把轮廓采样信息与相对应的位姿信息保存，便得到了物体的粗匹配的模板数据。

图像识别输出物体遮罩：

目前图像识别效果比较突出的方法是利用深度神经网络，而其中Mask-RCNN为目前利用深度神经网络进行图像识别中效果较好的模型，其效果如图5，此深度神经网络模型经训练后，可以做到实时高精度输出物体的类别与物体的mask，所以本方法采用此模型作为图像检测端处理模块。

随着神经网络不断发展，不同的算法和深度神经网络模型性能必将会超越Mask-RCNN，本算法可以适用于任何输出遮罩(mask)或轮廓的算法或深度神经网络模型，即可以作为通用解决方案。其中训练Mask-RCNN神经网络时，利用blender以及Opencv软件自动生成了数据集来训练，识别精度较高。

粗匹配算法：

刚体的位姿包括旋转R和位移T两部分，其旋转部分的匹配过程如下：

由于不同框架输出的物体遮罩分辨率不同，若是物体分辨率过低则会影响采样算法采集的数据质量，物体分辨率过高则在采样时会导致采样速度下降，所以和采样算法相同，首先把输出的轮廓信息做归一化处理，统一到相同尺度下进行比较。

若对物体实际遮罩采样数据为Sⁱⁿ，模板数据中第i组数据为Sⁱ，每组有n个采样值，则计算实际遮罩采样数据与模板中每组数据的L₁距离，第i组数据的L₁距离L_i为：

理想情况下，在位姿相同时采样值应该一致，即在模板数据中使此距离为0的旋转角度，即为此轮廓所对应旋转角度，在实际中若是对角度分割过细，会产生大量的数据，匹配过慢，所以取所有结果中满足阈值情况下的最小值所对应旋转角度为当前匹配得到的旋转角度，不满足阈值认为匹配失败。为了保证匹配速度，在粗匹配时，误差控制在欧拉角每个自由度误差不大于12°(即把360°等分为30份进行采样生成粗匹配模板)。而后可以把此欧拉角信息转化为旋转矩阵R，即得到物体的旋转信息。

其平移部分算法如下：

在生成模板数据时，由于是在指定距离上对物体进行采样，且CAD模型大小已知，所以物体对应的包围框大小与其距离成反比，即包围框越小距离越远，这与人肉眼认知一致，则模型中心点与相机光心的距离即可以通过(5)求出:

D＝(w_in/w_i)·D_i (6)

其中w_in为物体识别输出边界框宽(利用边框长计算亦可)，w_i为与其旋转相匹配的模板数据的边界框宽，D_i为模板数据采集时指定的距离，D即为模型中心点与相机光心的距离。

与此类似，因为CAD模型大小先验信息已知，既可以计算出模板中每像素所代表的实际物理距离，进而可以计算出物体的位移向量：

在实际实验中，由于无法获取亚像素级别的信息，仅利用像素来计算时，位移向量误差较大，可以利用提高相机图像的分辨率来解决此问题，也就是相机分辨率越高，得到物体的位置越准。在得到物体的旋转R和位移T之后，结合相机的内外参即可得到物体的世界坐标。

迭代算法：

在得到物体的粗匹配位姿之后，物体的旋转理论上还有小于12°的误差，为了消除此误差，引入迭代算法，此算法在粗匹配得到的物体旋转信息上进行计算，最终得到误差为零的旋转信息(在浮点数设置为8位小数时)

若粗匹配物体旋转为A＝(ψ，θ,φ)，则在此基础上，每个轴都加减一个小角度Δε，由于之前设置粗匹配间隔为12°，所以Δε设置为粗匹配间隔的一半即6°，这样在粗匹配旋转空间求出其相邻空间的26个角度，利用这26个角度结合CAD模型得到物体的轮廓，轮廓采样法并结合(5)式，求出使得(5)式L_i值最小的旋转A₁＝(ψ₁，θ₁,φ₁)，即得到迭代一次后的物体旋转角度。

而后通过不断把Δε减半得到更小范围的角度值进行迭代，最终可以得到使得(3)式为0的旋转角度(在浮点数设置为8位小数时)，可以通过设置计算机浮点位数得到更为精确的旋转信息。

结合粗匹配时得到的物体平移信息，即得到了物体的精确的6Dof位置。

实验数据：

本实验环境配置为：笔记本为联想Y7000，***为ubuntul6.04，编程语言使用python3.6。

物体识别精度：本方法输入的遮罩数据采用Mask-RCNN神经网络所输出的遮罩，由于Mask-RCNN神经网络本身性能较强，经自行开发的数据集训练，实现了比较理想的识别精度，可以满足本方法需求，如图6所示。

旋转精度：粗匹配旋转精度由生成的粗匹配数据模板决定，本实验粗匹配时把欧拉角每个自由度分成了30等分，所以精度不大于12°(360°/30)，平均经过6轮迭代后达到8位浮点数最高精度。图7为遮罩图经粗匹配和迭代后效果对比，其中从左到右分别为物体的随机位姿遮罩图(即输入)、利用经过粗匹配后位姿得到的物体渲染图以及二者差别图。

经实验证明，迭代后旋转误差在8位浮点数时为0，图5部分绿色误差为位置误差所引起。

位置精度：由于本方法位置是由物体的包围框计算的，精度受限于像素精度，极端情况下例如较远的物体较小，包围框成比例缩小，这样包围框差一个像素，位置误差就增大很多，所以物体的位置精度取决于相机像素，相机像素越高，包围框误差越小，从而物***置精度越高。

经试验，在相机分辨率为512x512像素时本方法4x4x3(cm)物***置精度随物体距相机位置变化如表1所示：

表1位置误差随距离变化关系

物体与相机距离(mm)	误差(mm)
		500	0-5
1000	2-12
		2000	10-100
5000	＞100

与其他相关位姿法的对比：

本方法与目前神经网络中比较有代表性的SSD-6D，BB8等方法相比，当评价标准为目前通用标准2Dprojection、5cm5°或6Dpose，本方法在旋转准确度上都接近100％，远超其他各类算法，其主要误差来源是位置误差，在考虑位置误差成因取决于相机图片分辨率精度后，认为与其他方法无对比性。

在实时性上差距很大，本方法在上述个人笔记本环境下运行检测一张图片粗匹配用时约0.6s，迭代后平均用时约为40-60s。而一般基于神经网络的6dof位姿方法基本可达到实时(＞20fps)，基于算法的6Dof位姿方法比较有代表性的是Linemod也基本可以达到15-18fps。

本方法在抗干扰能力上比较突出，只要物体轮廓采样基本正确，物体遮罩中间缺失对本方法推算位姿影响不大，如图8所示。

综上所述方法可以作为实时性要求不高时对物***姿检测的一种通用算法，其检测精度较高且有较强的抗干扰性能，在实际应用中可以考虑通过使用C++代码以及并行计算提高其实时性，以满足使用要求。

Claims (7)

1.一种基于CAD模型的物***姿预测方法，其特征在于包括如下步骤：

通过标定获取单目摄像头的相关参数，并利用CAD模型生成粗匹配所需数据；

检测识别图像中的物体并输出图像的遮罩，通过物体的遮罩得到物体的相关轮廓信息；

通过物体的相关轮廓信息结合粗匹配数据得到物体的粗匹配位姿，然后通过迭代算法，得到物体的精确位姿。

2.如权利要求1所述的基于CAD模型的物***姿预测方法，其特征在于，通过标定获取单目摄像头的相关参数的方法包括如下步骤：

构建相机成像模型：

M为三位空间点，m为M在图像平面投影的像点，按照相机涉及的坐标系之间的关系可得到世界坐标系到像素坐标的投影：

可将(1)写成(2)的形式

其中a_x，a_y分别是图像水平轴和垂直轴的尺度因子；K为相机内部参数矩阵；M₁中包含旋转矩阵和平移矢量，M₁中参数是由相机坐标系相对于世界坐标系的位置决定的，因此称M₁为相机外部参数矩阵；内部参数和外部参数矩阵的乘积M为投影矩阵；X_W为世界坐标系中物体中心W所在的x轴坐标，Y_W为世界坐标系中物体中心W所在的y轴坐标，Z_W为世界坐标系中物体中心W所在的z轴坐标；

相机焦距为f所在轴为z正方向，x与y轴处在光心O所在平面，以光心O为相机坐标系原点，在此相机坐标系下则物体中心所在位置用W来表示，其中：

W＝(W_x,W_y,W_z) (3)

规定物体中心就是物体CAD模型中心所在位置，若P＝(u_,v)为物体对应像素在图像上的坐标，K为相机内参矩阵，则可以得到此等式：

此等式表示实际物体中心所在位置W在相机坐标系下经过相机内参K后投影到图像后的二维坐标位置P。

3.如权利要求1所述的基于CAD模型的物***姿预测方法，其特征在于，利用CAD模型生成粗匹配数据的方法如下：

首先通过物体CAD模型在指定位姿下渲染出物体的遮罩，通过物体的遮罩得到物体的边界框，而后根据不同的需要在边界框上每隔一定距离对物体轮廓进行采样；

以左边界框长度L为基准，把L分为n等份，每隔L/n为一个采样横坐标点，遍历每一个轮廓上的点在其横坐标等于采样横坐标点时计算其到左边框的距离，由于每个采样横坐标点可能对应多个轮廓采样距离，所以取多个距离中的最大和最小值作为此采样横坐标上对轮廓采样的采样值，把轮廓信息变为成一组采样值；

对采样值进行归一化，即把左边界框长度统一到一个单位下；

在指定距离上，以物体CAD模型中心为中心，在不同的旋转角度对物体的轮廓进行采样，把轮廓采样信息与相对应的位姿信息保存，得到物体的粗匹配的模板数据。

4.如权利要求1所述的基于CAD模型的物***姿预测方法，其特征在于，所述检测识别图像中的物体并输出图像的遮罩的方法如下：

利用Mask-RCNN神经网络进行图像识别，输出物体的类别与物体的遮罩。

5.如权利要求4所述的基于CAD模型的物***姿预测方法，其特征在于，在训练Mask-RCNN神经网络时，利用blender以及Opencv软件自动生成了数据集来训练。

6.如权利要求1所述的基于CAD模型的物***姿预测方法，其特征在于，粗匹配位姿方法如下：

刚体的位姿包括旋转R和位移T两部分，其旋转部分的匹配过程如下：

首先把输出的轮廓信息做归一化处理，统一到相同尺度下进行比较；

若对物体实际遮罩采样数据为Sⁱⁿ，模板数据中第i组数据为Sⁱ，每组有n个采样值，则计算实际遮罩采样数据与模板中每组数据的L₁距离，第i组数据的L₁距离L_i为：

理想情况下，在位姿相同时采样值应该一致，即在模板数据中使此距离为0的旋转角度，即为此轮廓所对应旋转角度，所以取所有结果中满足阈值情况下的最小值所对应旋转角度为当前匹配得到的旋转角度，不满足阈值认为匹配失败；

在粗匹配时，误差控制在欧拉角每个自由度误差不大于12°；而后把此欧拉角信息转化为旋转矩阵R，即得到物体的旋转信息；

其平移部分算法如下：

在生成模板数据时，由于是在指定距离上对物体进行采样，且CAD模型大小已知，所以物体对应的包围框大小与其距离成反比，即包围框越小距离越远，这与人肉眼认知一致，则模型中心点与相机光心的距离D即可以通过(5)求出:

D＝(w_in/w_i)·D_i (6)

其中w_in为物体识别输出边界框宽，w_i为与其旋转相匹配的模板数据的边界框宽，D_i为模板数据采集时指定的距离，D即为模型中心点与相机光心的距离；

CAD模型大小先验信息已知，既可以计算出模板中每像素所代表的实际物理距离，进而可以计算出物体的位移向量t_z：

其中，t_x为物体在x轴的位移量，t_y为物体在y轴的位移量，在得到物体的旋转R和位移T之后，结合相机的内外参得到物体的世界坐标。

7.如权利要求6所述的基于CAD模型的物***姿预测方法，其特征在于，通过迭代算法，得到物体的精确位姿的方法如下：

若粗匹配物体旋转为A＝(ψ，θ,φ)，则在此基础上，每个坐标轴都加减一个角度Δε，Δε设置为粗匹配间隔的一半，在粗匹配旋转空间求出其相邻空间的若干个角度，利用相邻空间的若干个角度结合CAD模型得到物体的轮廓，利用轮廓采样法并结合(5)式，求出使得(5)式L_i值最小的旋转A₁＝(ψ₁，θ₁,φ₁)，即得到迭代一次后的物体旋转角度；

而后通过不断把角度Δε减半得到更小范围的角度值进行迭代，最终可以得到使得(3)式为0的旋转角度；

结合粗匹配时得到的物体平移信息，得到物体的精确位姿。

Images