WO2013086678A1

WO2013086678A1 - 平面模型下点匹配与位姿同步确定方法及计算机程序产品

Info

Publication number: WO2013086678A1
Application number: PCT/CN2011/083850
Authority: WO
Inventors: 张广军; 魏振忠; 王巍
Original assignee: 北京航空航天大学
Priority date: 2011-12-12
Filing date: 2011-12-12
Publication date: 2013-06-20
Also published as: US20140321735A1; US9524555B2

Abstract

本发明提供一种平面模型下点匹配与位姿同步确定的方法及计算机程序产品，该方法包括：通过粗略位姿估计得到两组位姿粗略解；以每组所述位姿粗略解作为初始迭代值，通过扩展TsPose算法经迭代搜索得到两组位姿待选解；基于目标函数，选出最终输出的位姿精确解。因此，本发明解决了二维射影变换下进行点匹配与位姿同步解算时估计位姿解随迭代算法的进行呈指数增长的问题，位姿的计算是基于平面点集，且与平面物体的几何形状无关，减弱了对空间平面物体位姿计算的条件限制，该方法在闭塞、混乱的环境下，以及存在不同级别的噪声的情况下也能表现出较好的鲁棒性，具有很好的应用价值。

Description

平面模型下点匹配与位姿同步确定方法及计算机程序产品技术领域本发明涉及基于模型的单目视觉位姿估计，尤其涉及一种平面模型下点匹配与位姿同步确定的方法及计算机程序产品。背景技术基于模型的单目视觉位姿估计涉及摄像机定位的问题，主要是在摄像机的内参已知的条件下，给定一组特征（点、线、二次曲线等）在世界坐标系下的描述及其在图像上的投影，求得摄像机坐标系与物体坐标系之间的刚体变换关系。基于模型的单目视觉位姿估计是计算机视觉中的一个重要研究课题，在视觉导航、模式识别、摄像机标定等诸多方面都有重要应用。

目前，常用的位姿计算方法主要分为两大类：解析方法和数值方法。其中，比较有代表性的解析方法为 Gerald Schweighofer、 Axel Pinz提出的基于平面点特征的算法；数值方法包括： O.Oberkampf, D.DeMenthon、 L.S.Davis提出的基于平面点特征的迭代求解算法， Tsai的平面靶标标定算法，张正友的平面靶标标定算法等，但是该类方法必须要在模型特征与其像特征对应关系已知的条件下进行，例如，点特征的对应关系或者线特征的对应关系已知。在数值方法中，有一类位姿求解算法可以同时完成特征匹配与位姿的同步解算，具有代表性的算法如 SoftPosit算法。

在物点与像点的对应关系未知的条件下求解位姿，现有方法只有针对三维物体的，例如：数值算法中的 SoftPoist算法是基于三维模型的点匹配与位姿同步求解算法，但是，将这类方法用于平面模型下的点匹配与位姿同步解算时，在求解过程中会遇到 "位姿冗余" 问题，即：估计位姿解随着迭代求解的进行呈指数增长，使得求解无法进行下去。

由此可见，现有的一些空间平面物体的位姿计算必须在一些特定的条件下才能进行，因而位姿计算适用的范围受到了限制。发明内容针对二维射影变换下进行点匹配与位姿同步解算时估计位姿解随迭代算法的进行呈指数增长的问题，本发明有必要提供一种平面模型下点匹配与位姿同步确定的方法及计算机程序产品，以解决上述的技术问题。

为解决上述问题，本发明提供一种平面模型下点匹配与位姿同步的确定方法，该方法包括：

通过粗略位姿估计得到两组位姿粗略解；

以每组所述位姿粗略解作为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解；

基于所述位姿粗略解，从所述位姿待选解中选出最终输出的位姿精确解。

上述方法中，所述通过扩展 TsPose算法经迭代搜索得到位姿待选解进一步包括：

以每组所述位姿粗略解作为初始迭代值，通过计算得到的初始 "像切面" 像点获取匹配矩阵，并对该匹配矩阵进行更新，利用更新后的匹配矩阵获取目标函数值；

更新仿射变换关系，并根据更新后的仿射变换关系来得到旋转矩阵，根据位姿粗略解中的粗略偏航角选择一个新的像切面和旋转矩阵，并利用所选择的新的像切面确定平移向量和新的 "像切面" 像点；

判断是否满足迭代收敛条件，如果满足迭代收敛条件，则结束迭代搜索过程；如果不满足迭代收敛条件，则所确定的新的 "像切面" 像点进行新一轮的迭代搜索，直到满足迭代收敛条件；

所述位姿估计解包括所确定的旋转矩阵、平移向量和 /或匹配矩阵；满足迭代收敛条件时得到的位姿估计解为所述位姿待选解。

进一步地，上述方法还包括：

在结束迭代搜索过程之后，判断两组所述初始迭代值是否均已进行过迭代，若还有初始迭代值未进行迭代，则以未进行迭代的初始迭代值为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解。

上述方法中，所述迭代收敛条件为：不确定度大于结束不确定度；其中，不确定度为初始不确定度 A与不确定度的更新频率 A之积；和 /或，所述迭代收敛条件为：所述目标函数值小于预先设置的表示找不到匹任一所述迭代收敛条件得到满足时，迭代搜索过程结束。

上述方法中，所述基于所述位姿粗略解，从所述位姿待选解中选出最终输出的位姿精确解进一步包括：

在两组初始迭代值均完成迭代搜索之后，根据每组所述位姿粗略解作为初始迭代值时得到的目标函数值，将较小的目标函数值所对应的位姿待选解选出，作为最终输出的位姿精确解。

根据本发明的另一个方面，还提供一种用于平面模型下实现点匹配与位姿同步确定的计算机程序产品，该产品内包括具有计算机可读程序编码的计算机可用媒介，该计算机可读程序编码用于执行平面模型下确定位姿的方法；该方法包括：通过粗略位姿估计得到两组位姿粗略解；以每组所述位姿粗略解作为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解；基于所述位姿粗略解，从所述位姿待选解中选出最终输出的位姿精确解。

上述产品中，所述通过扩展 TsPose算法经迭代搜索得到位姿待选解进一步包括：

上述产品中，所述方法还包括：在结束迭代搜索过程之后，判断两组所述初始迭代值是否均已进行过迭代，若还有初始迭代值未进行迭代，则以未进行迭代的初始迭代值为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解。

上述产品中，所述迭代收敛条件为：不确定度大于结束不确定度；其中，不确定度为初始不确定度 A与不确定度的更新频率 A之积；和 /或，所述迭代收敛条件为：所述目标函数值小于预先设置的表示找不到匹任一所述迭代收敛条件得到满足时，迭代搜索过程结束。

上述产品中，所述基于所述位姿粗略解，从所述位姿待选解中选出最终输出的位姿精确解进一步包括：

与现有技术相比，本发明具有以下优势：

本发明提出了一种平面模型下点匹配与位姿同步确定方法，该方法包括：第一步利用一种聚类方法获得位姿的粗略解，限定了位姿搜索空间，解决了迭代搜索的 "位姿冗余" 问题；第二步，在二维射影变换下，利用扩展 TsPose算法，通过迭代搜索获得更加精确的点匹配和位姿结果。这里，所述 TsPose算法是一种用于精确确定位姿的算法，即像切面滑动算法；所述扩展 TsPose算法是一种结合用于点匹配的算法如 Softassign算法与用于精确位姿确定的算法如 TsPose算法。

进一步来讲，本发明不仅能够解决二维射影变换下进行点匹配与位姿同步解算时，估计位姿解随迭代算法的进行呈指数增长的问题；而且，由于本发明的位姿计算方法是基于平面点集，且与平面物体的几何形状无关，因此，能够减弱对空间平面物体位姿计算的条件限制，本发明的方法在遮挡、杂乱的环境下，以及存在不同级别的噪声的情况下也能表现出较好的鲁棒性，具有很好的应用价值。附图说明图 1是坐标系及姿态角示意图；

图 2是 "种子对" 中的点按极角的大小由小到大排列；

图 3是目标函数曲线图；

图 4a示出了物点簇与像点簇的匹配示意图；

图 4b示出了变换后的物点簇与像点簇的匹配示意图；

图 5a是本发明粗略估计的方法实施例示意图；

图 5b是本发明精确位姿的方法实施例示意图；

图 6是本发明迭代搜索过程的示意图。具体实施方式本发明的基本思想在于：采用平面模型下点匹配与位姿同步求解的两步法：第一步，得到位姿粗略解，以限定位姿搜索空间，解决迭代搜索的 "位姿冗余" 问题；第二步，结合点匹配算法与精确位姿获取算法，将位姿粗略解作为迭代搜索的初始解和在迭代搜索过程中选择位姿估计解的基准，进而通过迭代搜索获得更加精确的点匹配和位姿精确解。其中，所述位姿粗略解包括粗略姿态角和粗略平移向量 Z 坐标值；所述位姿精确解包括姿态角和平移向量。

下面结合附图及实施例，对本发明的平面模型下点匹配与位姿同步的方法进行说明。

在本实施例中，通过粗略位姿估计得到两组位姿粗略解；分别以每组所述位姿粗略解作为初始迭代值，通过扩展 TsPose算法经迭代搜索得到两组位姿待选解，基于目标函数值，选出一组位姿待选解作为最终输出的位姿精确解。

笫一步——粗略位姿估计的方法，该方法包括以下步驟：

5101 , 分别针对每个物点与像点，选择物点簇和像点簇；

本步驟中，设定待选出的邻域点的个数为 k; 计算物点与其它所有物点的欧氏距离，将计算得到的欧式距离由小到大排列，选取前 k个最近的邻域点与该物点组成一个物点簇；其中，该物点簇中共有 k+1个点。

类似地，计算像点与其它像点的欧氏距离，将计算得到的欧式距离由小到大排列，选取前 k个最近的像点与该像点组成一个像点簇；其中，该像点簇中共有 k+1个点。本实施例中，设物点的个数为 m个，像点的个数为 n个，则相应地，物点有 m个物点簇，像点有 n个像点簇；

5102, 分别对每一个物点簇数据和每一个像点簇数据进行变换；本步驟中，所述分别对每一个物点簇和每一个像点簇数据进行的变换主要是指：使每个物点簇数据分别减去各自的质心，并作如下述公式（ 1-3 ) 所示的白化变换；相应地，使像点簇数据分别减去各自的质心，并作如下述公式（1-3 )所示的白化变换。 S103 , 对变换后的物点簇和像点簇内的点进行排序；

本步驟中，计算变换后的物点簇的极坐标，并按照极角的大小由小到大排列物点簇内的点的顺序；同理，计算变换后的像点簇的极坐标，并按照极角的大小由小到大排列像点簇内的点的顺序；

S104, 计算子空间距离，建立子空间距离矩阵；

本步驟中，计算每个像点簇的共 k+1个环形排列组合与在步驟 S103中已排序的物点簇的子空间距离，记录下子空间距离最小的值、相应的物点簇内的点排列、以及与物点配对该子空间距离最小的值对应的像点簇，对所有的物点簇重复以上的步驟，得到一个 mxn维的由子空间距离组成的子空间距离矩阵，其中，子空间距离矩阵的行表示物点簇、列表示像点簇；

S105 , 设置子空间距离的阈值 d, 选取子空间距离矩阵中小于该阈值 d 所对应的物点簇及像点簇对；

利用步驟 S102-S105对物点簇与像点簇内的数据进行变换，能够减少可能的匹配关系，利用变换后的数据点获取正确的匹配关系，进而减少计算量。

例如，某物点簇和像点簇内的数据都是 6个，如图 4a所示的虚线圆中的点集，图中左边圆中的点代表物点簇内的点，右边圆中的点代表像点簇内的点，此时，两个点集簇内正确匹配的点之间的变换关系为仿射变换。物点簇内的点与像点簇内的点有 ⁶！ = ⁷²0个匹配可能性，例如给定物点簇内的顺序为 "a-b-c-d-e-Γ ,那么像点簇内的点与物点簇内的点匹配的可能顺序为 "A-B-C-D-E-F"、 "A-C-B-D-E-F"、 "A-D-C-B-E-F" 等。根据给出的一种判定正确匹配关系的判定准则，那么需要对 720个匹配对按照判定准则进行计算，根据预先设定好的判定准则阈值，选择出符合一定阈值大小的匹配对，对每一个可能的匹配关系都要根据判定准则进行计算，所以计算量是相当大的。经过 S102-S105变换后，左右两个点集内（即图中虚线圆内）正确匹配的点集之间的变换关系为旋转变换关系，如图 4b所示，所以此时可能的匹配关系就从 6! = 720次减少到了 6次，由于可能的匹配关系大大减少，利用判定准则只需要对这 6次可能的匹配关系进行计算判定，计算量也相应大大减少了。

5106, 根据选取的物点簇及像点簇对，利用其未变换前的数据计算仿射变换，获得初步的位姿粗略解；

在得到正确的匹配关系后，还是需要利用未变换前的数据来计算仿射变换。本步驟中，利用选取的物点簇及像点簇对的未变换前的数据计算仿射变换，并根据公式（2-10 )所示计算得到两组旋转矩阵，由旋转矩阵的第三列计算得到两组姿态角，即俯仰角和偏航角，同时由仿射变换关系，由公式（2-11 )计算得到平移向量 z坐标的粗略值；

5107, 利用 k均值聚类方法，对初步得到的每一个位姿粗略解聚类，得到位姿粗略解。

本步驟中，利用 k均值聚类方法将姿态角、平移向量 z坐标进行聚类，得到两组粗略姿态角和平移向量 z 坐标的粗略值。其中，所述位姿粗略解包括两组粗略姿态角和平移向量 Z 坐标的粗略值。所述粗略姿态角包括粗略偏航角和粗略俯仰角。

粗略位姿估计方法的实施例

本实施例中，通过选取得到物点簇和像点簇之后，对每一个物点簇和每一个像点簇数据作变换，该变换的步驟分为三步：

1)种子对中的物点簇数据减去其质心，种子对中的像点簇数据减去其质心。这里，所述种子对是指所选择的物点簇及像点簇对。设物点为 _Pi, 像点为两者存在仿射变换关系 q; = A ^t (1-1)

其中， A是一个 2x2的矩阵， t是一个 2x1的向量。设物点的质心为 m_p, 像点的质心是 m_q, 贝' J

_¾ί=Α_Ρί ( 1-2)

其中，

i=_Pi-m_p。所以，经过了步驟 1), 物点簇与像点簇之间仅存在一个线性变换关系。

2)对物点和像点 £分别作 "白化变换"，即： p =^s _p ²p, , =s_q ^2q' ^{( 1}-3)

其中，是像点的方差 _t

3)设变换后的物点 p;与像点 q;存在的线性变换为 , 显然 = S^ q As p , 计算物点 p;的极坐标计算像点 q;的极坐标( ， ;)，如图 2所示，将物点 p;按照其极角的大小，由小到大排列，将像点 q;按照其极角 0_q; 的大小，由小到大排列。需要说明的是，能够容易证明得出 X是正交矩阵。

经过上述步驟 1) -3) 的变换后，物点与像点的变换关系为：

其中， q:. = ("i, 是像点坐标， P; =(X_ot, y_OT)^T是物点坐标， R是一个 2x2的旋转矩阵。如图 4b所示，其示出了经过步驟 1 ) -3 )变换后的物点簇与像点簇的匹配示意图，左边圆中的点代表物点簇内的点，右边圆中的点代表像点簇内的点，此时，两个点集簇内正确匹配的点之间的变换关系为旋转变换。其中， T表示向量或者矩阵的转置。

本实施例中，如图 4b所示，物点簇内有给定按照极角从小到大排列的

6 个点，那么构造物点簇张成的维子空间为

ν₂] , ^νι

y_wl, y_wkf ; 同理，像点簇的一个子空间相应的维子空间为 S₂=sp_im[w₁ w₂] , w_x = (u u₂, ···, u_k) , w₂ ={v v_2/ v_k) 。那么，子间巨离的定义如下：

其中，是子空间 Si的投影矩阵， P₂是子空间8₂的投影矩阵，矩阵范数为 2-范数，即：矩阵的最大奇异值。投影矩阵的求解可通过现有的计算方法实现，此处则不对其进行详细描述。

笫二步——精确位姿估计的方法，该方法包括以下步驟：

5201, 归一化处理给定像点的数据；

本步驟中，将给定像点的齐次坐标除以其模，即作归一化处理，得到一组模为 1的向量，简称为 "视线束"。

5202, 设置迭代收敛条件

其中，所述迭代收敛条件的设置包括：确定初始不确定度 °, 结束不确定度^ , 不确定度的更新频率 A , 设置表征匹配关系的匹配矩阵 M的初始值。其中，不确定度 ^{+1= χ}Α, 当 ⁺¹>Α时，则确定迭代结束。正确时，目标函数 (2-7)的理论值为 0, 所以，可以预先设置一个值很小的常数 ε , 当目标函数（2-7 )的值小于所设置表示找不到匹配点的常数 f时，则退出迭代搜索过程。

需要说明的是，同时采用上述两种迭代收敛条件，无论哪个条件先达到，都可以结束本次迭代搜索。

5203 , 获取精确位姿获取算法的初始迭代值；

本步驟中，采用精确位姿获取算法如扩展 TsPose算法，分别将第一步中通过粗略位姿估计方法得到的两组位姿粗略解即粗略姿态角和平移向量 z坐标值的粗略估计值作为扩展 TsPose算法的初始迭代值。

需要说明的是，本实施例不对获取初始迭代值的方式进行限制，换句话说，本实施例可以采用多种获取方式，例如：每次迭代时输入一个初始迭代值即一组位姿粗略解；或者，一次性将两组位姿粗略解同时输入，每次迭代时分别进行调用；或者，在粗略位姿估计完成之后，由执行粗略位姿估计的装置将其所得到的位姿粗略解输送给执行精确位姿估计的装置 , 或者由执行精确位姿估计的装置向执行粗略位姿估计的装置主动调取位姿粗略解等。

5204 , 计算旋转矩阵，并将计算得到的旋转矩阵作为扩展 TsPose算法的迭代搜索中旋转矩阵初始估计值；

本步驟中，由一组粗略估计的粗略姿态角，利用罗德里格斯公式计算得到旋转矩阵，并将计算得到的旋转矩阵作为扩展 TsPose算法的迭代搜索中旋转矩阵初始估计值，由 0-1 分布的随机数产生平移向量的 X坐标和 y 坐标值， Z坐标值由粗略位姿估计算法给出。其中，所述粗略姿态角包括粗略俯仰角和粗略偏航角。

S205, 确定初始 "像切面" 像点；

在步驟 S203-S204中，得到旋转矩阵 R和平移向量 T, 即：

T= f , ί,〃 t ( 2-2' )

由（2-Γ)、（2-2')得表征仿射变换关系的两个列向量:

其中，上标 = 0表示第 0次迭代，意思是迭代初始，在精确位姿估计方法中，上标都表示迭代的次数。

物点在世界坐标系中的齐次坐标为^„. =( ^, y_wi> 1)^τ，由（1)得迭代初始的切平面为 l =(r₁₃, r₂₃, r₃₃f , k = 0 , 因为切平面的位置 1^¾和 "视线束"（由归一化给定像点的数据步驟得到）均得到，由公式（2-2)、 (2-3) k 卜 k

计算得到 "像切面" 像点 q!' = , y_{ , k = 0

S206, 利用 "像切面" 像点获取匹配矩阵，更新所获取的匹配矩阵: 利用更新后的匹配矩阵确定目标函数；

本步驟中，设： + h *p -y.

( 2-5' ) 从而，计算匹配矩阵:

0 [β^-α)

m-: = r ¹ '

l^{] 1} ( 2-6' ) 这里，的上标表示进行 Sinkhorn算法前的初值。利用 Sinkhorn算法更新匹配矩阵。设更新后的匹配矩阵元素为 , 然后计算目标函数（2-7), 即：

=

其中， h_x =λ(α , d\2 ' )

需要注意的是，在第一次迭代时， h 由（2-3')、（2-4，）式给出，在以后的迭代中， h h₂、都是由公式（2-8)、（2-9) 以及（2-11 )计算得到。

S207 , 更新仿射变换关系，并根据更新后的仿射变换关系确定旋转矩阵，根据获取的粗略偏航角判据选择一个新的像切面和旋转矩阵；

本步驟中，通过精确位姿确定算法如扩展 TsPose算法，更新像点与物点的仿射变换关系，并由公式（2-10)计算得到两组旋转矩阵，两组旋转矩阵的第三列分别作为与单位球相切的两个新的 "像切面" 的法向量，并由法向量计算得到两组估计的第二姿态角。利用 sinkhorn算法不断更新物点与像点的匹配矩阵。例如：根据公式（ 2-8 )、 ( 2-9 ) , 设 = (/iu, h₁₂, h

ΙΤ-2 - (^^21，， ) ，己

, 则

'11 =h ' n 11， a 1? h * 11_2?.， a ?1 h ' 2.Ί， * 2..

由公式（2-11 )得，因

则可以得矩阵 A, 由公式（2-10)可得两个旋转矩阵 ^和^，设^的第三列为 ^R2的第三列为 1₂, ^和1₂分别代表两个新的 "像切面" 的表示，如图 2所示，要从新的 "像切面" ^和 1₂选择一个继续迭代搜索，选择的方式如下：

由计算得到一组姿态角，设为 ^)( 代表俯仰角，代表偏航角 ), 由计算得到一组姿态角，设为 ( 代表俯仰角， ^代表偏航角），设步驟 S203获取的一组位姿粗略解中的姿态角为、 ( 代表俯仰角，代表偏航角），如果 - <1^- , 则保留 ι₁ 即新的 "像切面" 的表示为 l^k+1 = 1_: , 同时也保留旋转矩阵作为迭代第 fc+i步的旋转矩阵估计值，即 R^fc+1=R_1; 反之，则保留 1₂, 即新的 "像切面" 的表示为 1⁺¹=1₂, 保留旋转矩阵 ^R ₂作为迭代第 + 1步的旋转矩阵估计值，即 R^fc+1 = R₂。

本实施例中，所述位姿解包括：姿态角和平移向量。需要说明的是，根据旋转矩阵可以计算得到相应的姿态角。因此，也可以认为位姿解包括：旋转矩阵和平移向量。此外，作为本发明实施例的一个突出的优势在于，在进行位姿求解的同时能够实现点匹配，所以，最后的输出结果包括旋转矩阵、平移向量以及匹配矩阵。

S208, 利用新的像切面，确定平移向量和新的 "像切面" 像点；这里，将 l^fc+1代入公式（2-2)、 (2-3), 即： r⁺ⁱ. ^⁺ⁱopj = i

根据位姿粗略估计算法得到位姿粗略解中的偏航角，确定一组 "像切面，，。由 ^ν'· 和（第步迭代时的） "像切面" 像点 q '构造一组点集，记作：

、k -k fc+l、T

Qi中， Vi j (2-14) 由摄像机坐标系与世界坐标系的刚体变换关系得： (2-16)

公式（ 2-16 )中的、 R已在步驟 S207中得到 , 已在此步驟中得到物点坐标 ^ρ™'已知。

平移向量 τ可以通过下面最小化误差平方和的方式求解得到：

由于（2-17) 式是一个平移向量 T的二次函数，当旋转矩阵 R给定后平移向量 T可以给出解析解：

所以，由点集 {^Q^^¹''"'"}、点集 {^P 、旋转矩阵 R 可解得:

(2-19) 可以看到，迭代搜索过程中的旋转矩阵和平移向量的求解如图 6所示。每次迭代都会有两组位姿估计解，以所确定的新的 "像切面 "像点继续进行迭代搜索，直到达到迭代收敛条件。

本步驟中，求视线束与两个单位球的切平面的交点，得到两组交点。每组交点的前两维作为新的 "像切面"像点的坐标，第三维与步驟 S206中的 "旧"像点组成一个三维数据，利用该三维数据与物点坐标，由下述的公式 ( 2-18 )计算得到两组平移向量。本步驟中，选择新的像平面位置及新的 "像切面" 像点。

本实施例中，以每组所述位姿粗略解即粗略姿态角和平移向量 z 坐标的粗略值作为初始迭代值，确定两个新的 "像切面"，并根据所述两个新的

"像切面"得到两组位姿估计解即位姿估计解 1和位姿估计解 2。需要指出的是，根据粗略偏航角来选择两组位姿估计解中的一个，所述位姿估计解包括旋转矩阵和平移向量。

S209, 判断是否满足迭代收敛条件，如果满足迭代收敛条件，则结束迭代，执行步驟 S210; 如果不满足迭代收敛条件，则根据新的 "像切面" 像点返回步驟 S206;

本步驟中，可分别采用以下两种方式判断迭代收敛条件是否得到满足：

( 1 )判断迭代收敛条件是否得到满足，即判断不确定度^ ^ ⁼ ^ ^X^» 是否大于结束不确定度^ ; 若 ^{+1 >}A , 则退出迭代搜索过程，执行步驟 S210; 否则，回到步驟 S206。或者

( 2 )判断迭代收敛条件是否得到满足，即判断目标函数（ 2-7 )的值是否小于预先给定的参数当目标函数（2-7 )的值小于预先给定的参数 f时，退出迭代搜索过程，执行步驟 S210; 否则，回到步驟 S206。

在本步驟中，将两组估计的第二姿态角中的偏航角分别与步驟 S203所输入的所述粗略姿态角中的粗略偏航角相减，选择差绝对值 ' j、的所对应的切平面作为新的像平面位置、所对应的像点作为新的"像切面"像点，返回并执行步驟 S206, 直到达到预设的结束条件。

由公式（8 )得新的像切面像点为：

本步驟中，以新的像平面位置、新的"像切面 "像点作为新的迭代值，重新执行步驟 S206-S209, 如此反复，直到达到预设的结束条件。所述预设的结束条件是指迭代搜索过程的退出条件，即：当不确定度 > 时，则本次迭代搜索过程结束，或者当目标函数（2-7 )的值小于预先给定的参数 f时，退出迭代搜索过程。

那么，经过 n次迭代，满足迭代收敛条件时得到的位姿估计解，可以称之为位姿待选解 1 ; 同样，采用另一组位姿粗略解进行迭代搜索可以得到位姿待选解 2。

下面结合图 6, 对本实施例的迭代搜索过程作进一步说明：

分别通过步驟 S203中获取的两组位姿粗略解，完成图 6所示的迭代搜索。这里，设两组位姿粗略解分别进行的两个迭代搜索分别为左搜索分支和右搜索分支，如图 6所示，设左搜索分支输出的位姿解为^ ¹^ , 点集的匹配结果为 ^Mi , 目标函数值为右搜索分支输出的位姿解为 {^{R T2}}，点集的匹配结果为 M₂ , 目标函数值为 ^£ ₂ , 若^ ^ , 则该算法最后输出的位姿解为 , 点集的匹配结果为；反之，则该算法输出的位姿解为 {^R2 ' ^T ₂ ) , 点集的匹配结果为 M₂。

5210, 记录下该组位姿粗略解作为初始迭代值对应的目标函数值，并判断两组初始迭代值是否均已进行过计算，若还有初始值未计算过则返回步驟 S203; 若均已计算完毕，则执行步驟 S211 ;

若判断得知，还有迭代初始值未计算过则返回步驟 S203 , 将另一组位姿粗略解作为初始迭代值，开启新一次求解位姿精确解的计算。

5211 , 根据每组位姿粗略解作为初始迭代值对应的目标函数值，从迭代得到两组位姿待选解中选出最终输出的位姿精确解。

本步驟中，每次退出迭代搜索过程后，记录下每组位姿粗略解作为初始迭代值对应的目标函数值，选择目标函数值小的位姿粗略解所对应的位姿待选解作为位姿精确解，以最终输出。其中，所述目标函数是表征物点和像点匹配关系、物点和像点变化关系的二次函数，用于选出最终输出的位姿解。

例如：若位姿待选解 1对应的目标函数值小于位姿待选解 2对应的目标函数值，则输出位姿待选解 1 作为最终的位姿精确解，否则输出位姿待选解 2作为最终的位姿精确解。

精确位姿估计方法的实施例

本文的扩展 TsPose算法是在平面模型已知，摄像机已标定的条件下得出的，除非特别说明，算法所指的像点均是归一化像点，即焦距 / = 1。

如图 1所示，摄像机的光心为 O , 焦距为 1 , 像平面为 , 摄像机坐标系的原点建立在光心，摄像机坐标系的 X轴、 _ 轴分别与像平面的长与宽平行，摄像机坐标系的 z轴沿着光轴的方向并指向摄像机的前方，摄像机坐标系用表示。平面模型已知，世界坐标系的 z轴沿着平面的法线方向，世界坐标系的

X轴、 j轴与 z轴满足右手法则，世界坐标系用 o_wx_w_y_wz_w表示，由于物点是共面点，不失一般性，欧氏坐标用 =( _;., y_wi, ο)^τ表示，齐次坐标用

V_wi = (^i y_wi if 表示，物点在摄像机坐标系下的欧氏坐标用 p_{=(x_if y_ir f表示，其中 = 1,2,..., w , "表示物点的个数，像点的欧氏坐标表示为 q,. =( ,., Vi)^T , 像点的齐次坐标用 = v_ir if表示。

设。 q , v_ir 1)^Γ, i = l,2,...,n, 单位矢量 oq =oq /||oq」| , 像平面 τ的单位法向量为 1 = ， l_y, l_zf。由于该文中像平面 τ是单位球 S 的切平面且又是成像面，所以，像平面在本文中称为像切面，像切面可以表示为：

X

1 , 1 , 1 y (2-1 )

z 单位矢量 oq 在的射线与像切面 r的交点用 V,=(x,, _Vl, ζ,) 表示, 可由式 (2-2)、 (2-3)求得：

(2-3)

其中， i¾, = l,2,...,w代表放缩系数。为了与给定的像点 q区分，以表示新的像点，简称为 "像切面" 像点， "像切面，，像点的坐标是点 X的前两个分量，即 =(^, y_t , 相应的齐次坐标为

支设像切面 τ经过第步移动，即算法的第次迭代搜索过程，像切面的单位法向量为 l^k, 得到 "像切面"像点 Ϊ (上标表示像切面经过了第次移动，下标第个 "像切面" 像点。），由 "像切面" 像点和物点^ ^，建立近似仿射变换关系：

(2-4)可以简写为:

其中，是一个比例常数， H , f，矩阵 A , 向量

^21 ^ ₂₂ = (b b₂f。为了求解（2-5 ) 式中的仿射变换参数，建立如下所示的目标函数式：

^{£fc =} ∑| (^hi^,p- -^Ji ) (2-6)

其中, 1^= (flu, a₁₂, b_t) , h₂ = λ(α₂₁, a₂₂, b₂ ) 。

当点的匹配关系未知时，式（ 2-6 )的目标函数 E应该相应地作些修改。下面首先介绍一下表征匹配关系的矩阵匹配 M , 假设物点的个数是个，像点的个数是《₂个，由于点的对应关系不知道，那么，每一个像点 ( i = \,2,...,n₂ )可以和任意一个物点 p_wj ( 7 = 1,2,...,^ )进行匹配，以 ( i = \,2,...,n₂ , 二工 ,…, ）表示匹己关系， =1表示两点匹己， m_y=0表示不匹配，矩阵 M是一个置换矩阵（ permutation matrix ), 即：矩阵 M元素的值只能是 0或者 1, 且矩阵 M的每行和每列元素有且只有一个元素为 1, 这是一个对匹配矩阵进行 "双向约束" 的条件。由于遮挡或者图像提取算法等等的原因，某些特征点的匹配点可能丟失，即存在无法匹配的特征点的问题，为了表示这类情况，给矩阵 M增加一行和一列，即第 "2+1 "散行" 和第 +1行散列。这样， _{i i+1} =1表示第个像点找不到匹配的物点， m_¾+li. = 1表示第 j个物点找不到匹配的像点。

现在，为了同时求解匹配系数矩阵以及仿射变换参数，以匹配矩阵元素作为加权参数，式（2-6) 的目标函数式可以相应修改如下：

^£fc =∑∑( _")

i=l j=l

，〜 fc、² /，〜、² (2-7) i=l j=l V

其中, ！^二 ^^, ₁₂, b_x) , h₂ = λ α₂₁, a₂₂, b₂) , "是一个很小的常数。

利用 sinkhorn算法更新匹配矩阵 M , 具体的方法是： a) 初始化匹配矩阵 M ,令^ = 7^{X e}^^Xd" ， l≤i≤m, 1< j≤n , w,,_K+1,l≤i≤m + l分配一个很小的常数， <j< " + 1分配一个很小的常数；

b) 对矩阵 M 的每一行的元素进行归一化，

C 矩阵 M 的每一列的元素进行 m Π .. = : ~~ m—-- ^- , ⁷1 < z < m + 1, ⁷1 < J j≤n

m ..

y

目标函数^ ^是未知向量和 h₂的二次函数，设目标函数 _J ^对向量和11₂的偏导数分别为在每次迭代时，要寻求使目标函数

值最小化的向量 ^hl和 ^h2 , 这样的向量需要满足的条件是^" ^{= 0}且 dE^k

= 0 , 当匹配系数给定后，和11₂的最优解可以以解析的形式表达如下：

阵 A'可以计

算得到两个正交矩阵 R^PR₂ , 以及比例常数，其中正交矩阵 R pR₂左上角的 2x2的矩阵是矩阵 A, 计算公式如下：

¾1

"21

a31

其中， S = -sign (

), sign表示取符号操作，矩阵的第 1行、第 2行与第 3行满足右手定律， S_r =a_n ² + a_n ² +a_2l ² +a₂₂ ² ₀ 由于像平面 τ的单位法向量 1沿光轴的正向，表示的是垂直于物平面的矢量，所以，取大于 0的值，即

(2-12)

设和1₂分别为旋转矩阵！^和！^的第三列，即代表两个新的 "像切面" 的表示，由计算得到一组姿态角，设为（ ' 、 ( 代表俯仰角，代表偏航角），由计算得到一组姿态角，设为（ ' ( 代表俯仰角， ^代表偏航角），设步驟 S203获一组位姿粗略解中的姿态角为、 ( 代表俯仰角，代表偏航角），

, 反之， l^fc+1=l₂。

然后，将得到的 l^fc+1分别代入（2-2)、（2-3) 式，即：

l^fc+1. ⁺¹op!) = l

由和（第步迭代时的） "像切面" 像点 q '构造一组点集，记作:

k ^k+l

(2-14) 由摄像机坐标系与世界坐标系的刚体变换关系得：

Q卜 R_{P∞+T (2}-₁₆₎

平移向量 T可以通过下面最小化误差平方和的方式求解得到

i=l (2-17) 由于（2-17 ) 式是一个平移向量 τ的二次函数，当旋转矩阵 R给定后，平移向量 τ可以给出解析解：

T = -∑(^Q, -Rp_OT )

w '=i ( 2-18 ) 所以，由点集可解得：

需要说明的是，本实施例中，所述扩展 TsPose算法是一种结合用于点匹配的算法如 Softassign算法与用于精确位姿确定的算法如 TsPose算法，在该算法中，表征匹配关系的矩阵 M也是迭代算法中需要不断估计的值，当匹配矩阵 M始终为常矩阵一单位矩阵时，扩展 TsPose算法即为 TsPose 算法。

数字仿真实验实施例

数字仿真的摄像机的焦距为 769个像素，像面大小为 512x512,像面中心是（256, 256 ) , 像元尺寸为 0.0076毫米 /像素。实验选取的平面物点和虚假点均分布在边长为 200 毫米的正方形内，服从均匀分布，物点的个数为 50个，虚假点与物点的比为 1:2, 像点的个数为 75个，像点添加了标准差为 1个像素的高斯噪声。俯仰角设定为 65度，方位角设定为 60度，平移向量设定为 [0.5; 1;1600] (毫米)。扩展 TsPose算法的迭代最大值设置为 50 次。

步驟 1 : 设定领域点的个数为 7, 根据欧式距离准则，分别针对 50个物点生成 50个物点簇，针对 70个像点生成 70个像点簇。步驟 2: 对 50个物点簇进行坐标变换，即：分别减去其质心，然后进行 "白化变换" ，计算物点簇的极坐标，按照极角大小由小到大排列物点簇内的点的顺序，类似地，对像点簇的数据也作如此的处理。

步驟 3:计算每个像点簇的共 8个环形排列组合与已排序的物点簇的子空间距离，建立子空间距离矩阵。

步驟 4: 设置子空间距离的阈值为 2, 选取子空间距离矩阵中小于阈值 2所对应的物点簇与像点簇。

步驟 5: 根据选取的物点簇与像点簇，利用其未变换前的数据计算仿射变换，得到每一组匹配的物点簇与像点簇的位姿粗略解，利用 k均值聚类方法，设定类别为 2, 对所有的位姿解进行聚类，得到两组位姿粗略解。

步驟 6: 将两组位姿粗略解分别作为扩展 TsPose算法的迭代初始解和迭代搜索过程中选择位姿估计解的基准，通过迭代搜索得到两组位姿待选解和两组匹配矩阵待选解。

步驟 7:若位姿待选解 1对应的目标函数值小于位姿待选解 2对应的目标函数值，则输出位姿待选解 1作为最终的位姿精确解，匹配矩阵待选解 1 作为最终的匹配矩阵解，否则输出位姿待选解 2作为最终的位姿精确解，匹配矩阵待选解 2作为最终的匹配矩阵解。

如图 3所示，下面给出一个实例的目标函数曲线图。由图 3可以看到，在精确位姿估计算法进行迭代搜索的结束阶段，目标函数值基本不再发生变换，由于在每次迭代搜索时，位姿估计解是基于位姿粗略解进行选择的，所以精确位姿估计算法可以认为被限定在位姿粗略解的领域内进行搜索，所以，当位姿粗略解与真实位姿解相差很远时，在它的领域进行搜索不会得到好的位姿精确解，同时，它所对应的目标函数值也不会小于在与真实位姿解相差不多的位姿粗略解领域搜索时所对应的目标函数值。该程序的输出的位姿解为：俯仰角为 66.81 度，方位角 60 度，平移向量为 [-0.03;1.26;1611.40] (毫米)，俯仰角误差为 1.81度，方位角误差为 0度，距离相对误差为 0.72%, 匹配正确率为 100%。

本发明实施例提供一种基于平面模型的点匹配与位姿同步解算的方法，本实施例能够解决二维射影变换下进行点匹配与位姿同步解算时估计位姿解随迭代算法的进行呈指数增长的问题。而且，由于本实施例的位姿计算方法是基于平面点集，且与平面物体的几何形状无关，所以能够减弱对空间平面物体位姿计算的条件限制，本实施例所公开的方法在遮挡、杂乱的环境下，以及存在不同级别的噪声的情况下也能表现出较好的鲁棒性，具有很好的应用价值。

显然，本领域的技术人员应该明白，上述的本发明的各步驟可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上；可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步驟制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

为实现上述方法，本发明还提供一种用于平面模型下实现点匹配与位姿同步确定的计算机程序产品，该产品内包括具有计算机可读程序编码的计算机可用媒介，该计算机可读程序编码用于执行平面模型下确定位姿的方法；该方法包括：

通过粗略位姿估计得到两组位姿粗略解；

上述产品中所采用的所述方法还包括：

在结束迭代搜索过程之后，判断两组所述初始迭代值是否均已进行过迭代，若还有初始迭代值未进行迭代，则以未进行迭代的初始迭代值为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解。上述产品中，所述迭代收敛条件为：不确定度大于结束不确定度；其中，不确定度为初始不确定度 A与不确定度的更新频率 A之积；和 /或，所述迭代收敛条件为：所述目标函数值小于预先设置的表示找不到匹配点参数 £ ;

任一所述迭代收敛条件得到满足时，迭代搜索过程结束。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。

Claims

权利要求书

1、一种平面模型下点匹配与位姿同步的确定方法，其特征在于，该方法包括：

通过粗略位姿估计得到两组位姿粗略解；

2、根据权利要求 1 所述的方法，其特征在于，所述通过扩展 TsPose 算法经迭代搜索得到位姿待选解进一步包括：

3、根据权利要求 2所述的方法，其特征在于，该方法还包括：在结束迭代搜索过程之后，判断两组所述初始迭代值是否均已进行过迭代，若还有初始迭代值未进行迭代，则以未进行迭代的初始迭代值为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解。

4、根据权利要求 2所述的方法，其特征在于，

所述迭代收敛条件为：不确定度大于结束不确定度；其中，不确定度为初始不确定度 A与不确定度的更新频率之积；和 /或，

所述迭代收敛条件为：所述目标函数值小于预先设置的表示找不到匹配点参数 £ ;

任一所述迭代收敛条件得到满足时，迭代搜索过程结束。

5、根据权利要求 2、 3或 4所述的方法，其特征在于，所述基于所述位姿粗略解，从所述位姿待选解中选出最终输出的位姿精确解进一步包括：在两组初始迭代值均完成迭代搜索之后，根据每组所述位姿粗略解作为初始迭代值时得到的目标函数值，将较小的目标函数值所对应的位姿待选解选出，作为最终输出的位姿精确解。

6、一种用于平面模型下实现点匹配与位姿同步确定的计算机程序产品，其特征在于，该产品内包括具有计算机可读程序编码的计算机可用媒介，该计算机可读程序编码用于执行平面模型下确定位姿的方法；该方法包括：

通过粗略位姿估计得到两组位姿粗略解；

7、根据权利要求 6 所述的产品，其特征在于，所述通过扩展 TsPose 算法经迭代搜索得到位姿待选解进一步包括：

8、根据权利要求 7所述的产品，其特征在于，所述方法还包括：在结束迭代搜索过程之后，判断两组所述初始迭代值是否均已进行过迭代，若还有初始迭代值未进行迭代，则以未进行迭代的初始迭代值为初始迭代值，通过扩展 TsPose算法经迭代搜索得到位姿待选解。

9、根据权利要求 7所述的产品，其特征在于，

任一所述迭代收敛条件得到满足时，迭代搜索过程结束。

10、根据权利要求 7、 8或 9所述的产品，其特征在于，所述基于所述位姿粗略解，从所述位姿待选解中选出最终输出的位姿精确解进一步包括：在两组初始迭代值均完成迭代搜索之后，根据每组所述位姿粗略解作为初始迭代值时得到的目标函数值，将较小的目标函数值所对应的位姿待选解选出，作为最终输出的位姿精确解。