CN104517291B - 基于目标同轴圆特征的位姿测量方法 - Google Patents

Abstract

基于目标同轴圆特征的位姿测量方法，属于计算机视觉测量技术领域。其特征是：目标具有同轴的圆形特征，结合曲线提取和椭圆拟合技术得到图片中两个同轴圆投影方程，利用圆形特征的投影几何中绝对二次曲线、消失线、圆环点等概念的联系，获得目标平面的圆环点投影坐标以及消失线方程，利用极线‑极点定理，通过平面的消失线方程获得两个同轴圆的圆心投影坐标，最后以同轴圆的实际距离为先验条件并结合圆环点投影以及圆心投影坐标，求解出目标的姿态和位置信息。本发明的效果和益处是，利用单幅具有同轴圆特征的目标图片完成目标位姿的测量，操作简单并适用于实时测量。同时，不需要人工干预，实现较高精度的测量。

Description

基于目标同轴圆特征的位姿测量方法

技术领域

本发明属于计算机视觉测量技术领域，是一种适用于具有同轴圆特征的目标空间姿态和位置的测量方法。

背景技术

视觉测量是一门以图像处理为基础，综合运用电子学、光电探测、图像处理和计算机视觉等技术实现对目标的尺寸或位姿进行非接触测量的新兴测量技术。在视觉测量技术中，采用相机采集测量目标的图像，通过图像处理系统对采集的图像进行分析处理，并利用目标的已知特征或先验条件来完成对目标的几何尺寸或者目标的位置、姿态等信息的测量。

根据采用的摄像机数量将视觉测量系统分为单目、双目和多目视觉测量系统。其中，单目视觉测量具有结构简单、标定步骤少、成本低等优点，同时还避免了双目或多目视觉测量中的视场小和匹配难等问题，所以近年来这方面的研究比较活跃。单个摄像机无法直接测出目标的深度信息，因此单目视觉测量需要预先获取目标的部分特征信息。传统单目视觉测量方法多数需要人工添加特定的测量模板，利用模板上的几何特征或先验条件建立单幅或多幅图像中相同特征的对应关系，求解目标的位姿信息。传统方法主要集中在平面目标的位姿测量，在实际目标中很难找到需要的几何特征，而且多数测量方法需要目标不同视角的图片来完成测量。同时，传统的单一平面的几何特征能够提供的目标位姿信息较少，因此利用目标的三维几何特征或利用多帧图像叠加处理已经成为趋势。最后，目标的圆形特征相对于其他特征具有更多的投影几何性质，能够提供目标更多的位姿信息，因此被广泛应用。

发明内容

本发明提供了一种基于同轴圆形特征的目标位姿测量方法，解决了传统的单目视觉测量方法需要人工设计模板辅助测量以及模板上特征多为单一平面的几何特征的不足。

本发明的技术方案是：

对于存在同轴圆特征的较大口径物体，例如，圆柱体、舱体、隧道等，在其轴向方位拍摄一幅图像来实现对该目标的位姿测量。位姿参数包括θ_x，θ_y，t_x，t_y，t_z。本发明以两个同轴圆的实际距离作为先验条件，并预先对相机进行标定。本发明结合多种圆形特征的投影几何概念，例如无穷远量、消失线、绝对二次曲线、圆环点等，实现最终的求解过程。

本发明的技术方案分为如下五个步骤。步骤(1)介绍本发明的投影几何模型。步骤(2)介绍无穷远量以及消失点、线的概念。步骤(3)介绍极线-极点定理以及如何利用消失线求解圆心投影坐标。步骤(4)介绍绝对二次曲线、圆环点的概念以及如何利用圆环点求解目标平面的消失线方程。步骤(5)介绍如何利用已知的同轴圆距离并结合同轴圆的圆心投影和圆环点投影坐标，求解目标的姿态和位置信息。

(1)本发明的投影几何模型

本发明的目标是一个空间圆柱体，其投影几何模型如图1所示。

图1中，O_w-X_w-Y_w-Z_w表示世界坐标系，用于描述现实世界中点的位置，且本发明假设目标平面为Z_w＝0平面，O_c-Ｘ_c-Y_c-Z_c表示摄像机坐标系，用于描述相机所在的方位，R，t表示两个坐标系之间的投影变换关系，R为 3*3的旋转矩阵，t为3*1的位移向量。

在投影几何中，两个同轴圆在图像平面的投影为两个椭圆，且两个椭圆的位置关系与R，t有关。三维场景中一点M在世界坐标系下的齐次坐标为M＝[x_w，y_w，z_w，1]^T，其投影在图像平面下的齐次坐标为m＝[u v 1]^T。在针孔模型下，空间点M与图像平面上对应点m的投影关系描述为:

sm＝K[R，t]M (1)

其中，s为非零尺度因子，R，t是3*4的相机外参数矩阵。其中，R是一个3×3的旋转矩阵，由3个Euler角来描述，t为3*1的位移向量，K为相机内参数矩阵，表示为：

其中，f为摄像机的焦距；dx，dy分别表示图像水平、垂直方向相邻像素之间的距离，即像元尺寸。本发明采用相机的像素是正方形，因此有：dx＝dy，f/dx＝f/dy。记：f/dx＝f/dy＝f_d,(u₀，v₀)表示主点。

本发明通过单幅目标图像完成目标的位姿测量，在图像中，利用曲线提取与椭圆拟合相结合的技术获得两个同轴圆的投影方程。假设两个空间同轴圆分 别为Q₁，Q₂，两个投影椭圆分别为E₁，E₂。空间圆投影表示为椭圆的二次曲线方程：

au²+buv+cv²+du+ev+g＝0 (2)

将其整理成矩阵形式如下：

m^TEm＝0 (3)

其中，E为椭圆方程(2)的3*3矩阵表示形式，其方程如下：

m为空间圆上点的投影，且m＝[u v 1]^T。通过公式(2)(3)得到两个椭圆的二次曲线方程及其矩阵表示形式E₁，E₂。然后，利用圆形的投影几何性质求解目标的位姿信息。然而，圆心投影是求解目标位姿信息的关键，步骤(2)首先给出圆心投影的求解方法以及相应的投影几何概念。

(2)投影几何中无穷远量及消失点、线的概念

求解目标位姿信息的关键步骤在于两个同轴圆的圆心投影的求解过程，在求解圆心投影的过程中应用投影几何中无穷远量以及消失点、线等概念。上述概念的定义及其数学表示形式如下。

无穷远量分成无穷远平面、直线、点。无穷远平面是投影几何为齐次坐标表示的完备性而定义的。在三维投影几何中，三维点的齐次坐标为4*1向量。如图1所示模型，在世界坐标系下一个点M的坐标的表示形式为：

M＝[x_w，y_w，z_w，t]^T (4)

特别地，当t＝0时，M＝[x_w，y_w，z_w，0]^T，其中x_w，y_w，z_w不全为0，称该点为无穷远点，所有无穷远点构成的集合为：

π_∞＝{P＝[x_w,y_w,z_w,0]|^Tx_w≠0∪y_w≠0∪z_w≠0} (5)

该集合构成无穷远平面。无穷远平面上任意一点坐标对应空间一条直线的方向。

无穷远点定义为空间中任意两条平行直线在无穷远平面的交点，用于描述平行直线的方向。无穷远直线定义为空间中任意两个平行平面在无穷远平面上的交线，用于描述该平行平面的方向。

综上，在无穷远量概念的基础上引出消失点、线的概念。消失点定义为一个无穷远点在图像平面上的投影，其投影几何关系如图2所示。图2描述两个平行直线交点在图像平面上的投影，该投影为该平行直线的消失点。消失线定义为一个无穷远直线在图像平面上的投影，其投影几何关系如图3所示。图3描述两个平行平面交线在图像平面上的投影，该交线的投影为该平行平面的消失线。每个平面都唯一确定一条消失线，且对于平行平面而言共享同一条消失线。

消失线是求解圆心投影的关键，为通过消失线求得两个同轴圆的圆心投影坐标，步骤(3)给出极线-极点定理并证明消失线与圆心投影满足极线-极点关系。

(3)极线-极点定理及如何利用消失线求解圆心投影

在投影几何中，极线-极点定理描述相同平面中的点与线在某种映射关系下的一一对应关系。这种映射关系的数学表达形式为平面二次曲线的3*3矩阵，

假设，m＝[x_w，y_w，1]^T为目标平面(z_w＝0)中任意一点，

(x_w-x₀)²+(y_w-y₀)²＝r²为目标平面上任意一个圆方程，并整理成3*3矩阵方程形式Q,

l＝[A B D]^T描述目标平面中一条直线Ax_w+By_w+D＝0，当点m与直线l满足如下公式：

sl＝Qm (6)

称点m与直线l在Q的约束下满足极线-极点关系，直线l称作点m的极线，点m称作直线l的极点。公式(6)中Q为目标平面特定圆方程的3*3矩阵表示形式，s为非零尺度因子。特别地，当点m为该圆圆心时，通过公式(6)求得的直线l为该目标平面的无穷远直线。证明如下，设目标平面无穷远直线方程为Ax_w＝By_w+D*0＝0，且表示为l_∞＝[A B D]^T。因为，无穷远直线上点坐标x_w或y_w无穷大，所以为满足该直线方程，A＝0，B＝0，且D归一化为1，则l_∞＝[00 1]^T。相反地，m₀＝[x₀ y₀ 1]^T为圆Q的圆心坐标，利用公式(6)解得l=[0 0 1]^T＝l_∞，证毕。

极线-极点定理满足投影不变性，在图像平面中，一个圆的圆心投影与目标平面的消失线，在圆的投影方程约束下满足极线-极点关系。在已知目标平面的 消失线方程l_v以及两个同轴圆的投影E₁，E₂时，通过公式(6)求得两个同轴圆的圆心投影坐lv标，这里分别假设为m_c1，m_c2。步骤(4)详解目标平面消失线的求解过程并介绍相关的投影几何概念。

(4)绝对二次曲线、圆环点的概念及消失线的求解

绝对二次曲线具有投影不变性，定义在无穷远平面上，且其在图像平面的投影方程与相机和目标之间的位姿信息无关，其空间模型如图4所示。

图4中，Π_∞表示无穷远平面，Ω表示绝对二次曲线，ω表示绝对二次曲线在图像平面上的投影(IAC)。

绝对二次曲线的数学表达式为无穷远平面上满足如下公式的点集：

在本发明的模型中，假设同轴圆目标的前端所在平面为z_w＝0平面，其中M＝[x_w y_wz_w 0]^T,令并将公式(7)整理成矩阵形式：

其中，Ω是绝对二次曲线方程的3*3矩阵表示形式。

设绝对二次曲线Ω在图像平面的投影为ω，由公式(2)得到ω满足方程m^Tω_m＝0，其中m＝[u v 1]^T为ω上的点坐标T。对针孔成像模型作如下变形：

其中，r₁，r₂，r₃为旋转矩阵R的三个列向量。令公式(9)中的KR＝H，并结合公式(8)与公式(9)得到如下推导：

m^TH^-TH^-1m＝m^TK^-Tm＝m^Tωm＝0 (10)

根据公式(10)推导出ω＝K^-TK^-1，且ω只与相机的内参数有关。相机内参数作为一个已知量，通过预先标定相机获得。

绝对二次曲线及其投影的概念与圆环点的概念紧密联系。在投影几何中，圆环点定义为目标平面的无穷远直线l_∞与绝对二次曲线Ω的交点，一般为复数表示形式，且已证明目标平面上任意圆与绝对二次曲线相交于两个共轭圆环点。圆环点的几何模型如图5所示。

图5中，Π_w表示目标平面，Π_∞表示无穷远平面，Ω表示绝对二次曲线，Q表示目标平面上任意圆，l_∞表示目标平面的无穷远直线，I,J表示目标平面的圆环点，彼此共轭。

已知目标平面为z_w＝0平面，且步骤(3)中已经证明目标平面的无穷远

直线方程l∞＝[0 0 1]^T，联立Ω与l_∞的方程得到圆环点I，J的表达式如下：

I＝[1 i 0]^T，J=[1-i 0]^T (11)

根据圆环点的投影不变性，设其在图像平面上的投影分别为I_m，J_m，且l_v为目标平面的消失线方程。因为圆环点投影经过目标平面的消失线，即I_m，J_m在l_v 上，所以通过I_m，J_m获得l_v的方程。

已证，在投影几何中，目标平面上任意圆Q与绝对二次曲线Ω相交于圆环点I，J。根据该性质的投影不变性，在图像平面中，利用绝对二次曲线投影ω的方程与两个同轴圆的投影方程E₁，E₂联立，求得公共解I_m，J_m，且两者互相共轭。通过I_m，J_m获得消失线l_v的方程，并利用消失线l_v与两个同轴圆圆心投影

m_c1，m_c2之间的极线-极点关系求解m_c1，m_c2：

其中，s₁，s₂为非零尺度因子。综上，利用绝对二次曲线及圆环点的性质求解出目标平面的消失线方程l_v，从而利用l_v和极线-极点定理获得两个同轴圆圆心投影坐标m_c1，m_c2。步骤(5)以两个同轴圆的实际距离为先验条件，利用两个同轴圆圆心的投影m_c1，m_c2和圆环点投影I_m，J_m解出目标的位姿信息。

(5)目标的位姿信息的求解过程

利用上述四个步骤分别获得两个同轴圆的投影坐标m_c1，m_c2及目标平面的圆环点投影坐标I_m，J_m。根据2针孔模型，圆环点的投影公式如下：

λI_m＝PI＝K[r₁ r₂ r₃ t][1 i 0 0]^T＝K[r₁ r₂ r₃][1 i 0]^T＝KRI (14)

其中，λ为非零尺度因子，r₁，r₂，r₃为3*3的旋转矩阵R的三个列向量。根据公式(14)推导如下：

r₁＝s₁K^-1Re{Im} (15)

r₂＝s₂K^-1Im{I_m} (16)

其中，s₁，s₂为非零尺度因子，且R矩阵为单位正交阵，则通过公式(15)(16)得到r₃的表达式如下：

r₃＝λ[K^-1Re{I_m}]×[K^-1Im{I_m}] (17)

r₃表示旋转矩阵R的第三列，λ为非零尺度因子，通过|r₃|＝1获得λ，从而解得r₃。根据投影几何中Euler角的定义，r₃的表达式如下：

r₃＝|-sinθ_y，sinθ_xcosθ_y，cosθ_xcosθ_y]^T (18)

其中，θ_x和θ_y分别表示为图1中世界坐标系X_w和Y_w轴相对于摄像机坐标系X_c和Y_c轴的夹角。通过公式(17)(18)解得θ_x，θ_y，且本发明采用同轴圆模型，目标的轴向与Z_w轴重合，所以目标沿Z_w轴具有旋转不变性。因此，本发明假定θ_z＝0符合实际要求。

在解出θ_x，θ_y后，根据两个同轴圆的圆心投影m_c1，m_c2的成像模型推导如下：

s₁m_c1＝[r₁，r₂，r₃，t]M_c1＝K[r₁，r₂，r₃，t][0，0，0，1]^T＝Kt＝K[t_x，t_y，t_z]^T (19)

其中，s₁，s₂为非零尺度因子，d为目标上两个同轴圆的实际距离，

M_c1＝[0，0，0，1]^T，M_c2＝[0，0，d，1]^T，r₃₁，r₃₂，r₃₃为r₃的三个分量。通过公式(19)(20)解出目标的位置信息t＝[t_x，t_y，t_z]^T。

综上，结合多种同轴圆的投影几何性质，实现了包括θ_x，θ_y，t_x，t_y，t_z的五个自由度的目标位姿测量。

本发明的效果和益处是，不需要人工设计测量模板，不局限于目标的尺寸，且结合曲线提取和椭圆拟合技术提高了椭圆拟合精度。同时，本发明测量步骤简单，利用单幅目标图像即可获取目标的位姿信息，适用于实时测量的场景。

附图说明

图1是本发明的空间模型图。1摄像机坐标系，2世界坐标系。图中O_c-X_c-Y_c-Z_c表示摄像机坐标系。O_w-X_w-Y_w-Z_w表示世界坐标系。

图2是消失点的投影关系图。3摄像机光心，4图像平面。图中l₁，l₂表示空间中两个平行直线。l_m1，l_m2表示l₁，l₂在图像平面的投影。v表示消失点。

图3是消失线的投影关系图。5摄像机光心，6图像平面。图中P₁，P₂表示 空间中两个平行平面。V_c1，V_c2表示该平行平面的两个消失点。l_v表示目标平面的消失线。

图4是绝对二次曲线及其投影的空间模型图。7摄像机光心，8图像平面，9无穷远平面。图中Π_∞为无穷远平面，Ω表示绝对二次曲线，ω表示绝对二次曲线在图像平面上的投影。O_c表示摄像机坐标系原点。

图5是圆环点、绝对二次曲线以及目标平面圆的几何关系模型图。10目标平面，11无穷远平面，12目标平面任意圆，13目标平面的无穷远直线，14绝对二次曲线。图中Π_w表示目标平面，Π_∞表示无穷远平面，Ω表示绝对二次曲线，Q表示目标平面上任意圆。l_∞表示目标平面的无穷远直线，I,J表示目标平面的圆环点。

图6是实施例的模型示意图。15摄像机坐标系，16图像平面，17同轴圆目标前端平面。图中两个圆形表示同轴圆目标。d为同轴圆目标的实际距离。

O_c-X_c-Y_c-Z_c表示摄像机坐标系。M_c1，M_c2表示两个同轴圆的圆心空间位置。O表示图像的主点位置。R，T描述目标与相机坐标系之间的位姿信息。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

S1.利用相机标定技术，获得相机的3*3内参数矩阵K,利用绝对二次曲线的投影ω＝K^-TK^-1,求得ω的3*3矩阵表示形式及其二次曲线方程。

S2.获取单幅同轴圆目标的图片，结合曲线提取和椭圆拟合技术，获得两个同轴圆投影的3*3矩阵方程E₁，E₂及其二次曲线方程。

S3.分别联立ω的二次曲线方程与两个投影椭圆E₁，E₂的二次曲线方程，对两个联立方程的一对近似解取均值，得到两个共轭圆环点的投影坐标I_m，J_m

S4.利用目标平面的圆环点投影I_m，J_m经过目标平面的消失线l_v的性质，获得目标平面的消失线方程l_v。

S5.利用两个目标平面的同轴圆圆心投影m_c1，m_c2与目标平面消失线l_v方程分别在投影椭圆E₁，E₂的约束下满足极线-极点关系，即 求得两个圆心投影m_c1，m_c2。

S6.利用已知的圆心投影m_c1，m_c2以及两个圆环点投影I_m，J_m，解出目标的角度信息θ_x，θ_y和位置信息t_x，t_y，t_z。

实施例：

实验使用Basler pilot的高速面阵摄像机采集图像，相机型号为piA1600-35gc，CCD点阵数目为1606×1206，像元尺寸为7.4×7.4μm²，与PC接口为千兆网口。

实验采用相机镜头的标准焦距是25mm，因此f_d＝25/0.0074＝3378.38。实验使用Bevel Box测角仪来测量角度并作为参照标准，其分辨率为0.1°。实验以圆柱型桶为目标，桶顶部中空，底部密封，半径为285mm，高度为870mm。桶 前端到相机的距离在3.0m到3.5m之间。

对相机采用张正友棋盘格标定法进行标定，标定后相机的焦距f＝25.311mm,主点为(752.45521,559.10071)(像素)，并对图片进行径向畸变校正。

在测量前，将桶平放并固定在实验台上，将相机固定在云台上，该云台有四个方向的自由度，包括水平位移x，垂直位移y，沿水平轴旋转θ_x，沿垂直轴旋转θ_y。在实验过程中，首先测试本发明角度的测量精度，具体方法为,使桶前端沿着水平轴向上旋转，共旋转5次，间隔为1°，每次旋转后用测角仪记录桶底部平面的水平轴倾角，即桶平面相对于相机CCD平面的俯仰角G(θ_x)，然后将本发明的计算结果θ_x与测角仪的测量结果进行比较，测量结果如表1所示。

表1 角度测量结果

G(θx)/度	-0.7	0.3	1.3	2.3	3.3
						θx/度	-0.9011	0.1045	0.9736	2.4692	3.0157

然后测量位移的精度，首先使用本发明测量目标角度方法将桶两端平面与相机CCD平面的垂直轴倾角θ_y调至0，然后使云台沿着水平轴平移8次，间隔为10mm，最后将本发明提出算法的计算结果t_x与云台平移的真实结果G(t_x)进行比较。测量结果如表2所示。

表2 位移测量结果

G(tx)/mm	0	10	20	30	40	50	60	70	80
										tx/mm	0	10.20	19.77	30.01	40.15	50.20	59.83	69.71	79.79

表1，表2的实验结果显示，本发明的角度测量误差在0.3°以内，位移的测量误差在0.3mm以内。

Claims (1)

1.基于目标同轴圆特征的位姿测量方法，其特征在于，步骤如下：

S1.通过相机标定技术，获得相机的3*3内参数矩阵K，其中，f为摄像机的焦距，dx，dy分别表示图像水平、垂直方向相邻像素之间的距离，即像元尺寸；采用相机的像素是正方形，其中，dx＝dy，f/dx＝f/dy；记：f/dx＝f/dy＝f_d,(u₀，v₀)表示主点；利用绝对二次曲线投影ω＝K^-TK^-1,求得ω的3*3矩阵表示形式及其二次曲线方程；

S2.选择一幅同轴圆目标的图片，利用曲线提取技术获得两个同轴圆投影的边缘点，并利用椭圆拟合技术获得两个同轴圆投影的二次曲线方程及其3*3的矩阵方程E₁，E₂；

S3.根据圆形特征的投影几何性质中，目标平面中任意圆Q与绝对二次曲线Ω相交于目标平面的两个共轭圆环点I,J及其投影不变特性，在图像平面中，绝对二次曲线投影ω与两个投影椭圆E₁，E₂相交于圆环点I,J的投影I_m，J_m，联立ω的二次曲线方程与两个投影椭圆E₁，E₂的二次曲线方程，求得两个共轭圆环点投影的3*1齐次坐标I_m，J_m；

S4.在投影几何概念中，目标平面∏_w与无穷远平面∏_∞相交于目标平面的无穷远直线l_∞，且平行平面拥有相同的无穷远直线；假设m_∞＝[x_∞ y_∞ 1]^T为目标平面无穷远直线l_∞上任意点，且无穷远直线方程为Ax_∞+By_∞+D*0＝0，因为x_∞或y_∞无穷大，则A＝0，B＝0才能满足该直线方程且将D归一化为1，则l_∞＝[0 0 1]^T；利用投影几何性质中，目标平面的无穷远直线l_∞经过目标平面的两个共轭圆环点I,J且该性质投影不变,在图像平面中，无穷远直线l_∞投影成为目标平面的消失线l_v，且目标平面的圆环点投影I_m，J_m经过两个同轴圆目标平面的消失线l_v，通过I_m，J_m求得消失线方程l_v；

S5.利用投影几何中的极线-极点定理，并根据两个同轴圆目标的圆心M_c1，M_c2与目标平面的无穷远直线l_∞在两个同轴圆方程Q₁，Q₂的约束下，满足极线-极点定理且该性质投影不变，以两个投影椭圆E₁，E₂的3*3矩阵形式作为映射矩阵，通过目标平面的消失线方程l_v，利用如下公式，

$m_{c1}=s_1{E}_1^{-1}{l}_v$

$m_{c2}=s_2{E}_2^{-1}{l}_v$

求得两个同轴圆圆心投影坐标m_c1，m_c2，s₁，s₂为非零尺度因子；

S6.联立目标平面的无穷远直线方程l_∞与绝对二次曲线Ω的方程，求得目标平面的圆环点坐标为I＝[1 i 0]^T，J＝[1 -i 0]^T，其中i代表虚数单位，利用两个共轭圆环点的投影坐标I_m，J_m，根据针孔成像模型得到如下公式，

λI_m＝PI＝K[r₁ r₂ r₃ t][1 i 0 0]^T＝K[r₁ r₂ r₃][1 i 0]^T＝KRIλ为非零尺度因子，I为目标平面的圆环点坐标，I_m为I的投影坐标，P＝[R t]为3*4投影矩阵，K为相机3*3内参数矩阵，R＝[r1，r2，r3]为3*3旋转矩阵，r1，r2，r3为R中三个列向量，利用上述公式推导如下，

r₁＝s₁K^-1Re{I_m}

r₂＝s₂K^-1Im{I_m}

其中，s₁，s₂为非零尺度因子；利用R矩阵为单位正交阵，得到

r₃＝λ[K^-1Re(I_m)]×[K^-1Im(I_m)]

其中，λ为非零尺度因子，利用|r₃|＝1求解λ，求解r₃；根据Euler角公式，

r₃＝[-sinθ_y，sinθ_xcosθ_y，cosθ_xcosθ_y]^T

其中，θ_x，θ_y表示世界坐标系X，Y轴与摄像机坐标系X，Y轴间的夹角，利用r₃求解出目标的角度信息θ_x，θ_y；由于是同轴圆模型，其轴向与世界坐标系Z轴重合，目标沿世界坐标系Z轴具有旋转不变性，所以设定θ_z＝0；已求解的圆心投影m_c1，m_c2的针孔成像公式如下，

s₁m_c1＝K[r₁，r₂，r₃，t]M_c1＝K[r₁，r₂，r₃，t][0，0，0，1]^T＝Kt＝K[t_x，t_y，t_z]^T

s₂m_c2＝K[r₁，r₂，r₃，t]M_c2＝K[r₁，r₂，r₃，t][0，0，d，1]^T

＝K[t_x+r₃₁d，t_y+r₃₂d，t_z+r₃₃d]^T

其中，s₁，s₂为非零尺度因子，K为相机内参数矩阵，r₁，r₂，r₃为旋转矩阵R中三个列向量，t＝[t_x，t_y，t_z]^T,d为已知的同轴圆目标的距离，r₃₁，r₃₂，r₃₃为r₃的三个分量，利用上述公式获得目标的位置信息t_x，t_y，t_z；综上，利用一幅具有同轴圆特征的目标图像，完成5个自由度θ_x，θ_y，t_x，t_y，t_z的目标的位姿测量。