一种基于双目视觉的基准标记位姿估计方法技术

本发明专利技术提供一种基于双目视觉的基准标记位姿估计方法，基于双目相机间的位姿关系约束，建立基于双目视觉的重投影误差位姿优化目标函数，建立基准标记上的特征点的测量不确定度，进而对目标函数进行细化，通过优化求解目标函数得到基准标记的位姿。该方法主要包括如下步骤：1)标定双目相机，得到双目相机的内参和外参。2)基于双目相机间的位姿关系建立重投影误差位姿优化目标函数。3)计算基准标记上每个特征点的测量不确定度，对重投影误差进行加权求和，以细化目标函数。4)优化求解目标函数得到基准标记的位姿。本方法首次提出基准标记上每个特征点的测量不确定度，用于对重投影误差进行加权，具有位姿估计精度高、鲁棒性强的优点。优点。优点。

【技术实现步骤摘要】
一种基于双目视觉的基准标记位姿估计方法


[0001]本专利技术属于计算机视觉的应用领域，具体涉及一种基于双目视觉的基准标记位姿估计方法。

技术介绍

[0002]手术导航系统是一种实时系统，在手术过程中使用计算机技术提供手术器械的位姿信息，帮助医生更快、更准确地完成手术。对手术器械的位姿估计是手术导航系统的核心部分，为完成对手术器械的位姿估计，将基准标记与手术器械进行刚性连接，对基准标记进行位姿估计，进而得到手术器械的位姿信息。
[0003]目前对基准标记的位姿估计方法多是通过双目测距原理计算特征点三维坐标，再利用迭代最近点算法计算出基准标记的位姿信息，这种方法存在诸多问题，比如在计算特征点空间坐标时存在误差，后续的迭代求解过程并没有减小此误差，导致位姿估计结果精度低且稳定性差，此外，各个特征点对代价函数的贡献值相同，导致位姿估计鲁棒性差。

技术实现思路

[0004]本专利技术要克服现有技术的不足，提供一种基于双目视觉的基准标记位姿估计方法。
[0005]本专利技术的目的是通过以下技术方案来实现的：一种基于双目视觉的基准标记位姿估计方法，包括如下步骤：
[0006](1)对双目相机进行标定：利用张氏标定法标定双目相机，得到双目相机的内参和外参。
[0007](2)建立位姿优化目标函数：对基准标记上的每个特征点计算重投影误差，并基于双目相机间的位姿关系建立基于双目视觉的重投影误差位姿优化目标函数。
[0008](3)加权重投影误差：将双目相机模型校正为理想模型，计算基准标记上每个特征点的测量不确定度，通过不确定度对步骤(2)中的每个特征点的重投影误差进行加权求和，以细化步骤(2)中的基于双目视觉的重投影误差位姿优化目标函数。
[0009](4)求解基准标记位姿：通过Levenberg
‑
Marquardt算法优化求解步骤(3)中的目标函数即可得到基准标记的位姿。
[0010]进一步地，所述步骤(1)包括如下子步骤：
[0011](1.1)利用张氏标定法分别对左右相机进行单目标定，得到左右相机的内参矩阵、畸变参数和标定板在左右相机坐标系下的位姿。
[0012](1.2)根据步骤(1.1)中得到的标定板在左右相机坐标系下的位姿计算出左右相机之间的位置关系，即双目相机外参，表示为旋转矩阵Rs和平移向量ts。
[0013]进一步地，所述步骤(2)包括如下子步骤：
[0014](2.1)以基准标记上的特征点在基准标记坐标系中的三维坐标和特征点在左相机图像中的二维像素坐标测量值作为输入，计算出基准标记在左相机坐标系中的位姿数据。
首先考虑基准标记上的某个特征点p，设它的齐次坐标为P＝(X,Y,Z,1)
T
，投影至图像中，它的像素齐次坐标为x＝(u,v,1)
T
，基准标记在左相机坐标系中的位姿用T＝[R
l
|t
l
]表示，把T展开得到特征点三维坐标和图像二维坐标的关系：
[0015][0016]消去最后一行的s，可以得到关于u和v的约束：
[0017][0018]设t
a
＝(t1,t2,t3,t4)
T
，t
b
＝(t5,t6,t7,t8)
T
，t
c
＝(t9,t
10
,t
11
,t
12
)
T
，则上式可表示为：
[0019][0020]当基准标记上一共有n个特征点时，则可以写出关于t的线性方程组：
[0021][0022]当匹配点大于6对时，使用奇异值分解对超定方程求解，得到基准标记相对于左相机的位姿初值为R
l
和t
l
。
[0023](2.2)基于步骤(2.1)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至左相机图像，得到特征点在左相机图像中的二维像素坐标估计值。设标志物上一特征点p在标志物坐标系中的3D坐标为P＝(X,Y,Z)
T
，在左相机坐标系下的坐标为P
′
＝R
l
P+t
l
＝(X
′
,Y
′
,Z
′
)
T
，将其投影至归一化平面得到点p
a
，其归一化坐标有如下形式：
[0024][0025]设透镜的径向畸变参数为k1，k2，k3，则点p
a
与坐标系原点之间距离为与坐标系原点之间距离为则畸变矫正后的坐标(u
distorted
,v
distorted
)为
[0026][0027]像素坐标的估计值x
L
＝(u
b
,v
b
)经相机模型投影变换后确定：
[0028][0029](2.3)计算基于步骤(2.2)中得到的特征点在左相机图像中的二维像素坐标估计值与二维像素坐标测量值的欧式距离，得到特征点在左相机图像中的重投影误差。设此特征点在左图中的像素坐标测量值为x
′
L
，则重投影误差表示为：
[0030]e
L
＝x
′
L
‑
x
L
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0031](2.4)引入双目相机之间的位姿关系就可以作为先验信息作为固定条件约束，基于步骤(2.1)中得到的位姿数据R
l
和t
l
与双目相机之间的位姿关系R
s
和t
s
，得到基准标记在右相机坐标系中的位姿数据R
r
和t
r
。
[0032][0033](2.5)基于步骤(2.4)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至右相机图像，得到特征点在右相机图像中的二维像素坐标估计值x
R
。
[0034](2.6)计算基于步骤(2.5)中得到的特征点在右相机图像中的二维像素坐标估计值x
R
与二维像素坐标测量值x
′
R
的欧式距离，得到特征点在右相机图像中的重投影误差e
R
＝x
′
R
‑
x
R
，将特征点在右相机图像中的重投影误差与基于步骤(2.3)得到的特征点在左相机图像中的重投影误差求和，得到特征点的双目重投影误差。
[0035](2.7)基于步骤(2.6)得到的特征点的双目重投影误差，对基准标记中所有特征点的双目重投影误差求和，设T＝[R
l
|t
l
]，则上述由特征点的世界坐标P经外参投影、畸变矫正和相机模型投影的变换得到像素坐标x...

【技术保护点】

【技术特征摘要】
1.一种基于双目视觉的基准标记位姿估计方法，其特征在于，包括以下步骤：(1)对双目相机进行标定：利用张氏标定法标定双目相机，得到双目相机的内参和外参；(2)建立位姿优化目标函数：对基准标记上的每个特征点计算重投影误差，并基于双目相机间的位姿关系建立基于双目视觉的重投影误差位姿优化目标函数；(3)加权重投影误差：将双目相机模型校正为理想模型，计算基准标记上每个特征点的测量不确定度，通过不确定度对步骤(2)中的每个特征点的重投影误差进行加权求和，以细化步骤(2)中的基于双目视觉的重投影误差位姿优化目标函数；(4)求解基准标记位姿：通过Levenberg
‑
Marquardt算法优化求解步骤(3)中的目标函数即可得到基准标记的位姿。2.根据权利要求1所述的基于双目视觉的基准标记位姿估计方法，其特征在于，所述步骤(1)包括如下子步骤：(1.1)利用张氏标定法分别对左右相机进行单目标定，得到左右相机的内参矩阵、畸变参数和标定板在左右相机坐标系下的位姿；(1.2)根据步骤(1.1)中得到的标定板在左右相机坐标系下的位姿计算出左右相机之间的位置关系，即双目相机外参，表示为旋转矩阵R
s
和平移向量t
s
。3.根据权利要求1所述的基于双目视觉的基准标记位姿估计方法，其特征在于，所述步骤(2)包括如下子步骤：(2.1)以基准标记上的特征点在基准标记坐标系中的三维坐标和特征点在左相机图像中的二维像素坐标测量值作为输入，计算出基准标记在左相机坐标系中的位姿数据；首先考虑基准标记上的某个特征点p，设它的齐次坐标为P＝(X,Y,Z,1)
T
，投影至图像中，它的像素齐次坐标为x＝(u,v,1)
T
，基准标记在左相机坐标系中的位姿用T＝[R
l
|t
l
]表示，把T展开得到特征点三维坐标和图像二维坐标的关系：消去最后一行的s，可以得到关于u和v的约束：设t
a
＝(t1,t2,t3,t4)
T
，t
b
＝(t5,t6,t7,t8)
T
，t
c
＝(t9,t
10
,t
11
,t
12
)
T
，则上式可表示为：当基准标记上一共有n个特征点时，则可以写出关于t的线性方程组：
当匹配点大于6对时，使用奇异值分解对超定方程求解，得到基准标记相对于左相机的位姿初值为R
l
和t
l
；(2.2)基于步骤(2.1)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至左相机图像，得到特征点在左相机图像中的二维像素坐标估计值；设标志物上一特征点p在标志物坐标系中的3D坐标为P＝(X,Y,Z)
T
，在左相机坐标系下的坐标为P
′
＝R
l
P+t
l
＝(X
′
,Y
′
,Z
′
)
T
，将其投影至归一化平面得到点p
a
，其归一化坐标有如下形式：设透镜的径向畸变参数为k1，k2，k3，则点p
a
与坐标系原点之间距离为r＝则畸变矫正后的坐标(u
distorted
,...

【专利技术属性】
技术研发人员：卜佳俊，付尧，顾静军，
申请(专利权)人：浙江大学，
类型：发明
国别省市：