CN110686650A - 一种基于点特征的单目视觉位姿测量方法 - Google Patents

Abstract

发明涉及一种基于点特征的单目视觉位姿测量方法，属于单目视觉位姿测量领域，其特征在于：(1)根据共面4个特征点之间的位置关系，分别对平行和相交两种情况进行分析；(2)根据特征点的空间坐标、图像坐标和空间位置关系，推导出世界坐标系中3个坐标轴上的向量变换到摄像机坐标系中的单位向量，进而求解出物体相对于摄像机的初始位姿；(3)用LM算法对初始位姿进行优化，得到最终位姿。上述方法仅需给出任意4个特征点的空间相对位置，其中任意3个特征点不共线，不需要给定其他限制条件，就可求解出物体相对于摄像机的位姿。解决单目视觉位姿求解过程中存在的解不唯一、选解难和解的精度不高等问题。

Description

一种基于点特征的单目视觉位姿测量方法

技术领域

本发明属于单目视觉位姿测量领域，涉及一种基于点特征的单目视觉位姿测量方法，解决单目视觉位姿求解过程中存在的解不唯一、选解难和解的精度不高等问题。

技术背景

视觉测量技术是基于计算机视觉将三维空间中的物体通过相机投射到二维图像中，然后借助于数学模型从二维图像还原为三维信息的技术，能够实现物体的几何尺寸以及物体在空间中的位置、姿态等测量。位置和姿态简称为位姿，通常用6个自由度来表示，即两个坐标系之间的3个旋转量和3个平移量。位姿测量目前已经广泛地应用于航空航天、目标跟踪、增强现实、自动导航、视觉伺服控制和工业制造等领域。求解位姿时首先需要选取特征，常用的有点特征、直线特征和二次曲线特征等。基于直线和曲线特征的算法一般是由点特征算法演变过来的，因此研究点特征具有重要意义。

基于点特征的位姿估计问题又称为PnP(Perspective-n-Point)问题。国内外学者对此进行了大量研究，主要集中于点的个数n及其算法，初步结论为当n<3时，PnP问题有无穷多个解；当n＝3时，PnP问题最多有4个解，如果3个点组成的三角形为等腰三角形，并且摄像机在特定区域内时，PnP问题有唯一解；当n＝4时，如果4点共面，则PnP问题有唯一解，如果4点不共面，则PnP问题最多有4个解；当n＝5时，PnP问题最多有2个解；当n>5时，PnP问题有唯一解，但解的形式不封闭，因此求解的算法具有局限性。除此之外，依据呈四边形分布的共面4特征点，用相应的几何关系求解位姿等方法也有应用，但是存在特征点提取困难、解不唯一、鲁棒性差和迭代结果不收敛等缺陷，并且在实际的测量场景中，特征点往往是未知的，此时特征点提取精度对位姿解算的精度有极大的影响。

针对这些问题，本发明提出了一种基于点特征的单目视觉位姿测量方法，根据任意不共线的4个特征点的空间相对位置，不需要给定特征点与光心连线形成的夹角、不需要给定光心到特征点的距离以及限制4个特征点呈矩形或特殊形状的分布等条件，利用给定4个不共线特征点相对位置已知的特性，求解出物体相对于摄像机的位姿。

发明内容

本发明的目的在于：提供一种基于点特征的单目视觉位姿测量方法，针对单目视觉位姿求解过程中存在的解不唯一、选解难和解的精度不高等问题。

本发明属于单目视觉位姿测量领域，涉及一种基于点特征的单目视觉位姿测量方法，解决单目视觉位姿求解过程中存在的解不唯一、选解难和解的精度不高等问题。

本发明的技术解决方案：一种基于点特征的单目视觉位姿测量方法，由以下步骤实现：

步骤一：获取摄像机拍摄的图像，用张正友标定法对摄像机进行标定，获取摄像机内参数矩阵；

步骤二：提取图像中任意4个不共面的特征点P_i(i＝1,2,3,4)，计算特征点在成像平面坐标系中的坐标{q_i(x_i,y_i,f),i＝1,2,3,4}，根据两点间距离公式计算出任意两个特征点之间的距离：d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

步骤三：设P₁为世界坐标系的原点，O_c为光心点，分析给定的任意不共线的4个特征点之间的相对位置关系：当4个特征点所在的两条直线平行时，定义特征点相关距离因子{k_i,i＝1,2,3,4}；当4个特征点所在的两条直线相交时，分别计算两条直线交点在世界坐标系中的坐标及成像坐标系中的坐标；

步骤四：分别求解出世界坐标系中X_w、Y_w、Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)；

步骤五：根据世界坐标系的原点在摄像机中的坐标求解出物体相对于摄像机的初始位置，并通过LM算法对初始位姿进行优化，得到物体相对于摄像机的初始姿态。

其中，步骤三和步骤四所述的分析给定的4个特征点的相对位置关系，求解出世界坐标系中X_w、Y_w、Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)的具体实现过程如下：

根据任意3点不共线的共面4个特征点P_i(i＝1,2,3,4)的位置关系，对P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄之间平行和相交两种情况分别分析：

(1)当P₁P₂与P₃P₄平行时

设P₁为世界坐标系的原点，O_c为光心点，点O_c、q₃、q₄确定的平面为π₁，点O_c、q₁、q₂确定的平面为π₂，点O_c到靶标特征点P_i(i＝1,2,3,4)的距离与到像点q_i(i＝1,2,3,4)的距离之比分别为k_i＝|O_cP_i|/|O_cq_i|(i＝1,2,3,4)。

根据向量

和相机内参数矩阵计算得到平面π₁的标准法向量

由于直线P₁P₂与直线P₃P₄平行，可得方程：

由|P₁P₂|＝d₁₂可得方程：

联立方程(1)和方程(2)解得：

同理，根据|P₃P₄|＝d₃₄解得：

所以，世界坐标系中X_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)为

Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₃,r₂₃,r₃₃)为Y_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₂,r₂₂,r₃₂)为(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

从而得到世界坐标系到摄像机坐标系的旋转矩阵R为：

由P₁为世界坐标系的原点可知，世界坐标系到摄像机坐标系的平移向量为

(2)当P₁P₂与P₃P₄相交时

设P₃P₄与P₁P₂相交于点P₀，根据给定的4个特征点P_i(i＝1,2,3,4)在世界坐标系中的坐标得到P₀在世界坐标系中的坐标，将点P₀设为世界坐标系的原点，P₀在图像坐标系中的投影点为q₀(x₀,y₀,f)，分别令|P₀P₁|＝d₀₁、|P₀P₂|＝d₀₂、|P₀P₃|＝d₀₃、|P₀P₄|＝d₀₄、|P₀P₅|＝d₀₅。根据投影点q_i(x_i,y_i,f)(i＝0,1,2,3,4)得到∠P₁O_cP₀、∠P₂O_cP₁、∠P₂O_cP₀、∠P₄O_cP₀、∠P₄O_cP₃和∠P₃O_cP₀。

在ΔO_cP₀P₂和ΔO_cP₀P₁中，由正弦定理可得：

解得点P₀在摄像机坐标系中的坐标

为：

同理，在ΔO_cP₀P₂和ΔO_cP₁P₂中，由正弦定理求解出点P₂在摄像机坐标系下的坐标为：

在ΔO_cP₀P₃和ΔO_cP₃P₄中，由正弦定理求解出点P₃在摄像机坐标系中的坐标

为：

则有：

根据式(8)和式(9)解得世界坐标系到摄像机坐标系的旋转矩阵R和平移向量t。则当cosβ≠0时，已知旋转矩阵R，求解得到相应的欧拉角为：

综上所述，从不同分布的4个特征点出发，由距离和角度的约束以及相应的几何关系均可得到唯一的位姿解。

附图说明

图1为本发明一种基于点特征的单目视觉位姿测量方法的流程图；

图2为本发明一种基于点特征的单目视觉位姿测量方法中当P₁P₂与P₃P₄平行时的位姿测量示意图；

图3为本发明一种基于点特征的单目视觉位姿测量方法中当P₁P₂与P₃P₄相交时的位姿测量示意图；

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

本发明的一种基于点特征的单目视觉位姿测量方法，针对位姿求解过程中存在的解不唯一、选解难和解的精度不高等问题，提出一种基于点特征的单目视觉位姿测量算法。

实施例1

如图1所示，本发明的一种基于点特征的单目视觉位姿测量方法，具体包括以下步骤：

步骤一：获取摄像机拍摄的图像，用张正友标定法对摄像机进行标定，获取摄像机内参数矩阵；

步骤二：如图2所示提取图像中任意4个不共面的特征点P_i(i＝1,2,3,4)，计算特征点在成像平面坐标系中的坐标{q_i(x_i,y_i,f),i＝1,2,3,4}，根据两点间距离公式计算出任意两个特征点之间的距离：d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

步骤三：由于P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄相互平行，设P₁为世界坐标系的原点，O_c为光心点，点O_c、q₃、q₄确定的平面为π₁，点O_c、q₁、q₂确定的平面为π₂，点O_c到靶标特征点P_i(i＝1,2,3,4)的距离与到像点q_i(i＝1,2,3,4)的距离之比分别为k_i＝|O_cP_i|/|O_cq_i|(i＝1,2,3,4)。

根据向量

和相机内参数矩阵计算得到平面π₁的标准法向量

由于直线P₁P₂与直线P₃P₄平行，即

可得方程：

由|P₁P₂|＝d₁₂可得方程：

联立方程(1)和方程(2)解得：

从而得到点P₁和P₂在摄像机坐标系中的坐标分别为：

则：

同理，根据|P₃P₄|＝d₃₄可求解出点P₃、P₄在摄像机坐标系下的坐标分别为：

则：

步骤四：分别求解出世界坐标系中X_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)为

Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₃,r₂₃,r₃₃)为

Y_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₂,r₂₂,r₃₂)为(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

步骤五：由P₁为世界坐标系的原点解得世界坐标系到摄像机坐标系的旋转矩阵R和平移向量t为：

并通过LM算法对初始位姿进行优化，得到物体相对于摄像机的初始位姿。

实施例2

如图1和图3所示，本发明的一种基于点特征的单目视觉位姿测量方法，具体包括以下步骤：

步骤一：获取摄像机拍摄的图像，用张正友标定法对摄像机进行标定，获取摄像机内参数矩阵；

步骤二：提取图像中任意4个不共面的特征点P_i(i＝1,2,3,4)，计算特征点在成像平面坐标系中的坐标{q_i(x_i,y_i,f),i＝1,2,3,4}，根据两点间距离公式计算出任意两个特征点之间的距离：d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

步骤三：设P₃P₄与P₁P₂相交于点P₀，根据给定的4个特征点P_i(i＝1,2,3,4)在世界坐标系中的坐标得到P₀在世界坐标系中的坐标，将点P₀设为世界坐标系的原点，P₀在图像坐标系中的投影点为q₀(x₀,y₀,f)，分别令|P₀P₁|＝d₀₁、|P₀P₂|＝d₀₂、|P₀P₃|＝d₀₃、|P₀P₄|＝d₀₄、|P₀P₅|＝d₀₅。根据投影点q_i(x_i,y_i,f)(i＝0,1,2,3,4)得到∠P₁O_cP₀、∠P₂O_cP₁、∠P₂O_cP₀、∠P₄O_cP₀、∠P₄O_cP₃和∠P₃O_cP₀。

在ΔO_cP₀P₂和ΔO_cP₀P₁中，由正弦定理可得：

解得：

则点P₀在摄像机坐标系中的坐标为：

同理，在ΔO_cP₀P₂和ΔO_cP₁P₂中，由正弦定理求解出点P₂在摄像机坐标系下的坐标

为：

在ΔO_cP₀P₃和ΔO_cP₃P₄中，由正弦定理求解出点P₃在摄像机坐标系中的坐标

为：

则有：

步骤四：根据式(8)和式(9)分别求解出世界坐标系中X_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)为

Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₃,r₂₃,r₃₃)为

Y_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₂,r₂₂,r₃₂)为(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

步骤五：当cos β≠0时，已知旋转矩阵R，根据世界坐标系的原点在摄像机中的坐标求解出物体相对于摄像机的初始位置，并通过LM算法对初始位姿进行优化，得到世界坐标系相对于摄像机的初始姿态的欧拉角为：

以上结合附图对本发明的具体实施方式作了说明，但这些说明不能被理解为限制了本发明的范围，本发明的保护范围由随附的权利要求书限定，任何在本发明权利要求基础上进行的改动都是本发明的保护范围。

Claims (6)

1.一种基于点特征的单目视觉位姿测量方法，其特征在于包括以下步骤：

步骤一：获取摄像机拍摄的图像，用张正友标定法对摄像机进行标定，获取摄像机内参数矩阵；

步骤二：提取图像中任意4个不共面的特征点P_i(i＝1,2,3,4)，计算特征点在成像平面坐标系中的坐标{q_i(x_i,y_i,f),i＝1,2,3,4}，根据两点间距离公式计算出任意两个特征点之间的距离：d₁₂＝|P₁P₂|、d₂₃＝|P₂P₃|、d₁₃＝|P₁P₃|、d₁₄＝|P₁P₄|；

步骤三：设P₁为世界坐标系的原点，O_c为光心点，分析给定的任意不共线的4个特征点之间的相对位置关系：当4个特征点所在的两条直线平行时，定义特征点相关距离因子{k_i,i＝1,2,3,4}；当4个特征点所在的两条直线相交时，分别计算两条直线交点在世界坐标系中的坐标及成像坐标系中的坐标；

步骤四：分别求解出世界坐标系中X_w、Y_w、Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)；

步骤五：根据世界坐标系的原点在摄像机中的坐标求解出物体相对于摄像机的初始位置，并通过LM算法对初始位姿进行优化，得到物体相对于摄像机的初始姿态。

2.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，分析给定的4个共面不共线特征点P_i(i＝1,2,3,4)的位置关系：P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄之间平行和相交。

3.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述步骤四中当P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄平行时：

世界坐标系中的X_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)为

世界坐标系中的Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₃,r₂₃,r₃₃)为

世界坐标系中的Y_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₂,r₂₂,r₃₂)为(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

4.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述步骤四中当P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄相交时：

世界坐标系中的X_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₁,r₂₁,r₃₁)为

世界坐标系中的Z_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₃,r₂₃,r₃₃)为

世界坐标系中的Y_w轴上的向量变换到摄像机坐标系中的单位向量(r₁₂,r₂₂,r₃₂)为(r₁₃,r₂₃,r₃₃)×(r₁₁,r₂₁,r₃₁)。

5.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述步骤五中当P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄平行时，根据世界坐标系的原点P₁解得的世界坐标系到摄像机坐标系的旋转矩阵R和平移向量t：

6.如权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述步骤五中当P₁、P₂两点所在的直线P₁P₂与P₃、P₄两点所在的直线P₃P₄相交时，根据世界坐标系的原点P₁解得的世界坐标系到摄像机坐标系的旋转矩阵R和平移向量t，得到的世界坐标系相对于摄像机的初始姿态的欧拉角为：

Images