CN116091625A - 一种基于双目视觉的基准标记位姿估计方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于计算机视觉的应用领域，具体涉及一种基于双目视觉的基准标记位姿估计方法。

背景技术

手术导航系统是一种实时系统，在手术过程中使用计算机技术提供手术器械的位姿信息，帮助医生更快、更准确地完成手术。对手术器械的位姿估计是手术导航系统的核心部分，为完成对手术器械的位姿估计，将基准标记与手术器械进行刚性连接，对基准标记进行位姿估计，进而得到手术器械的位姿信息。

目前对基准标记的位姿估计方法多是通过双目测距原理计算特征点三维坐标，再利用迭代最近点算法计算出基准标记的位姿信息，这种方法存在诸多问题，比如在计算特征点空间坐标时存在误差，后续的迭代求解过程并没有减小此误差，导致位姿估计结果精度低且稳定性差，此外，各个特征点对代价函数的贡献值相同，导致位姿估计鲁棒性差。

发明内容

本发明要克服现有技术的不足，提供一种基于双目视觉的基准标记位姿估计方法。

本发明的目的是通过以下技术方案来实现的：一种基于双目视觉的基准标记位姿估计方法，包括如下步骤：

(1)对双目相机进行标定：利用张氏标定法标定双目相机，得到双目相机的内参和外参。

(2)建立位姿优化目标函数：对基准标记上的每个特征点计算重投影误差，并基于双目相机间的位姿关系建立基于双目视觉的重投影误差位姿优化目标函数。

(3)加权重投影误差：将双目相机模型校正为理想模型，计算基准标记上每个特征点的测量不确定度，通过不确定度对步骤(2)中的每个特征点的重投影误差进行加权求和，以细化步骤(2)中的基于双目视觉的重投影误差位姿优化目标函数。

(4)求解基准标记位姿：通过Levenberg-Marquardt算法优化求解步骤(3)中的目标函数即可得到基准标记的位姿。

进一步地，所述步骤(1)包括如下子步骤：

(1.1)利用张氏标定法分别对左右相机进行单目标定，得到左右相机的内参矩阵、畸变参数和标定板在左右相机坐标系下的位姿。

(1.2)根据步骤(1.1)中得到的标定板在左右相机坐标系下的位姿计算出左右相机之间的位置关系，即双目相机外参，表示为旋转矩阵Rs和平移向量ts。

进一步地，所述步骤(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＝(t₁,t₂,t₃,t₄)^T，t_b＝(t₅,t₆,t₇,t₈)^T，t_c＝(t₉,t₁₀,t₁₁,t₁₂)^T，则上式可表示为：

当基准标记上一共有n个特征点时，则可以写出关于t的线性方程组：

当匹配点大于6对时，使用奇异值分解对超定方程求解，得到基准标记相对于左相机的位姿初值为R_l和t_l。

(2.2)基于步骤(2.1)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至左相机图像，得到特征点在左相机图像中的二维像素坐标估计值。设标志物上一特征点p在标志物坐标系中的3D坐标为P＝(X,Y,Z)^T，在左相机坐标系下的坐标为P′＝R_lP+t_l＝(X′,Y′,Z′)^T，将其投影至归一化平面得到点p_a，其归一化坐标有如下形式：

设透镜的径向畸变参数为k₁，k₂，k₃，则点p_a与坐标系原点之间距离为

则畸变矫正后的坐标(u_distorted,v_distorted)为

像素坐标的估计值x_L＝(u_b,v_b)经相机模型投影变换后确定：

(2.3)计算基于步骤(2.2)中得到的特征点在左相机图像中的二维像素坐标估计值与二维像素坐标测量值的欧式距离，得到特征点在左相机图像中的重投影误差。设此特征点在左图中的像素坐标测量值为x′_L，则重投影误差表示为：

e_L＝x′_L-x_L             (8)

(2.4)引入双目相机之间的位姿关系就可以作为先验信息作为固定条件约束，基于步骤(2.1)中得到的位姿数据R_l和t_l与双目相机之间的位姿关系R_s和t_s，得到基准标记在右相机坐标系中的位姿数据R_r和t_r。

(2.5)基于步骤(2.4)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至右相机图像，得到特征点在右相机图像中的二维像素坐标估计值x_R。

(2.6)计算基于步骤(2.5)中得到的特征点在右相机图像中的二维像素坐标估计值x_R与二维像素坐标测量值x′_R的欧式距离，得到特征点在右相机图像中的重投影误差e_R＝x′_R-x_R，将特征点在右相机图像中的重投影误差与基于步骤(2.3)得到的特征点在左相机图像中的重投影误差求和，得到特征点的双目重投影误差。

(2.7)基于步骤(2.6)得到的特征点的双目重投影误差，对基准标记中所有特征点的双目重投影误差求和，设T＝[R_l|t_l]，则上述由特征点的世界坐标P经外参投影、畸变矫正和相机模型投影的变换得到像素坐标x_L的过程为x_L＝h(T,P)。设特征点数为n，则可得到基于双目视觉的重投影误差位姿优化目标函数：

进一步地，所述步骤(3)包括如下子步骤：

(3.1)基于步骤(1.2)得到的双目相机外参R_s和t_s将双目相机模型校正为理想模型，使得左右相机图像中像素纵坐标相同。

(3.2)根据基准标记上的特征点在左相机图像和右相机图像中的像素纵坐标测量值的差值的绝对值建立该特征点的测量不确定度：

σ＝|v_L-v_R|            (11)

(3.3)基于步骤(3.2)中的测量不确定度得到基准标记上所有特征点的重投影误差权值:

其中k为不确定度的阈值，σ_avg表示所有不确定度小于阈值k的特征点的平均不确定度。

(3.4)根据步骤(3.3)中的重投影误差权值对步骤(2.6)中的双目重投影误差加权，对基准标记中所有特征点的双目重投影误差求和，完成对基于双目视觉的重投影误差位姿优化目标函数的细化：

其中n为基准标记中特征点数量，w_i为每个特征点重投影误差权重。

本发明的技术方案概括为：

1.提出了利用双目视觉原理构建的重投影误差位姿优化目标函数。通过双目相机间的位姿关系，得到基准标记上的特征点在左相机图像和右相机图像中的重投影误差，对误差求和得到基于双目视觉的重投影误差位姿优化目标函数。

2.根据双目视觉行对准的性质建立基准标记上特征点的测量不确定度，进而得到重投影误差权值，对所有特征点的双目重投影误差加权求和，完成对基于双目视觉的重投影误差位姿优化目标函数的细化。通过Levenberg-Marquardt算法优化求解细化后的目标函数即可得到基准标记的位姿。

本发明的有益效果是，引入双目相机之间的位姿关系作为固定条件约束可以降低噪声引起的不确定性，与现有方法相比，提高了位姿估计结果的精度和稳定性。通过建立特征点的测量不确定度，为测量结果较为准确的特征点赋予较高权值，为测量结果不准确的点赋予较低权值，与现有方法相比，提高了对于系统噪声、光照条件和遮挡等因素的鲁棒性。

附图说明

图1为本发明整体流程图。

具体实施方式

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

实施例：

如图1所示是本发明的一个实施例，提供了一种基于双目视觉的基准标记位姿估计方法，包括以下步骤：

(1)对双目相机进行标定：利用张氏标定法标定双目相机，得到双目相机的内参和外参，具体包括以下子步骤：

(1.1)利用张氏标定法分别对左右相机进行单目标定，得到左右相机的内参矩阵K、径向畸变参数k₁，k₂，k₃和标定板在左右相机坐标系下的位姿。

(1.2)根据步骤(1.1)中得到的标定板在左右相机坐标系下的位姿计算出左右相机之间的位置关系，即双目相机外参，表示为旋转矩阵R_s和平移向量t_s。由于图像噪声的存在，每个视图求出的R_s和t_s值会有微小差异，通过非线性优化算法得到更为准确的结果。

(2)对基准标记上的每个特征点计算重投影误差，并基于双目相机间的位姿关系建立基于双目视觉的重投影误差位姿优化目标函数，具体包括以下子步骤：

(2.1)实施例中的基准标记包含16个特征点，以基准标记上的特征点在基准标记坐标系中的三维坐标和特征点在左相机图像中的二维像素坐标测量值作为输入，计算出基准标记在左相机坐标系中的位姿数据。首先考虑基准标记上的一个特征点p，设它的齐次坐标为P＝(X,Y,Z,1)^T，投影至图像中，它的像素齐次坐标为x＝(u,v,1)^T，基准标记在左相机坐标系中的位姿用T＝[R_l|t_l]表示，把T展开得到特征点三维坐标和图像二维坐标的关系：

消去最后一行的s，可以得到关于u和v的约束：

设t_a＝(t₁,t₂,t₃,t₄)^T，t_b＝(t₅,t₆,t₇,t₈)^T，t_c＝(t₉,t₁₀,t₁₁,t₁₂)^T，则上式可表示为：

由于基准标记上一共有16个特征点，则可以写出关于t的线性方程组：

使用奇异值分解对超定方程求解，得到基准标记相对于左相机的位姿初值为R_l和t_l。

(2.2)基于步骤(2.1)中得到的位姿数据R_l和t_l将基准标记上的特征点在基准标记坐标系中的三维坐标投影至左相机图像，得到特征点在左相机图像中的二维像素坐标估计值。设标志物上一特征点p在标志物坐标系中的3D坐标为P＝(X,Y,Z)^T，在左相机坐标系下的坐标为P′＝R_lP+t_l＝(X′,Y′,Z′)^T，将其投影至归一化平面得到点p_a，其归一化坐标有如下形式：

点p_a与坐标系原点之间距离为

利用步骤(1.1)中得到的径向畸变参数对坐标进行畸变矫正，矫正后的坐标(u_distorted,v_distorted)为

特征点的像素坐标的估计值x_L＝(u_b,v_b)经相机模型投影变换后确定：

(2.3)设T＝[R_l|t_l]，则上述由特征点的世界坐标P经外参投影、畸变矫正和相机模型投影的变换得到像素坐标x_L的过程可表示为x_L＝h(T,P)。设此特征点在左图中的像素坐标测量值为x′_L，则重投影误差e_L表示为：

e_L＝x′_L-x_L         (8)

(2.4)引入双目相机的位姿关系约束R_s和t_s，基于步骤(2.1)中得到的位姿数据R_l、t_l和双目相机之间的位姿关系R_s、t_s，得到基准标记在右相机坐标系中的位姿数据R_r和t_r。

(2.5)基于步骤(2.4)中得到的位姿数据R_r和t_r将基准标记上的特征点在基准标记坐标系中的三维坐标投影至右相机图像，得到特征点在右相机图像中的二维像素坐标估计值x_R，该过程为x_R＝h([R_sR_l|t_s+R_st_l],P)。

(2.6)计算基于步骤(2.5)中得到的特征点在右相机图像中的二维像素坐标估计值x_R与二维像素坐标测量值x′_R的欧式距离，得到特征点在右相机图像中的重投影误差e_R＝x′_R-x_R，将特征点在右相机图像中的重投影误差与基于步骤(2.3)得到的特征点在左相机图像中的重投影误差求和，得到特征点的双目重投影误差e＝e_L+e_R。

(2.7)基于步骤(2.6)得到的特征点的双目重投影误差，对基准标记中所有特征点的双目重投影误差求和。由于此实施例中基准标记上特征点数为16，则可得到基于双目视觉的重投影误差位姿优化目标函数：

(3)基准标记的每一个特征点的重投影误差都参与了优化过程，在步骤(2.7)中的目标函数中每个特征点对代价函数的贡献值相同，然而在真实场景中，系统噪声、光照条件和遮挡等因素可能会给特征点像素坐标的测量带来较大误差，继续用步骤(2.7)中的目标函数优化容易陷入局部最优，进而降低对基准标记位姿估计的精度。因此，为不同特征点的重投影误差提供不同的权重，对步骤(2)中的每个特征点的重投影误差进行加权求和，以细化步骤(2)中的基于双目视觉的重投影误差位姿优化目标函数，具体包括以下子步骤：

(3.1)基于步骤(1.2)得到的双目相机外参R_s和t_s将双目相机模型校正为理想模型，使得左右相机图像中像素纵坐标相同，即像素的行对准。

设校正后相机坐标系为O_rec-X_recY_recZ_rec，坐标轴O_recX_rec平行于O_lO_r。将R_rec写成如下形式

由于旋转矩阵R_rec的行向量是其坐标轴在相机坐标系中的投影，因此

坐标轴O_recY_rec的方向向量可以由平移向量t和O_recZ_rec的方向向量叉乘得到，因此坐标轴O_recY_rec在相机坐标系中的投影为

计算出e₁和e₂后，O_recZ_rec的在相机坐标系中的投影e₃由它们正交，则

计算出e₁，e₂和e₃后，R_rec也就确定了，左右相机通过旋转矩阵R_l＝R_rec·r_l和R_r＝R_rec·r_r实现行对准，校正后的左右图像中特征点的纵坐标相同。

(3.2)根据基准标记上的特征点在左相机图像和右相机图像中的像素纵坐标测量值的差值的绝对值建立该特征点的测量不确定度：

σ＝|v_L-v_R|            (15)

(3.3)基于步骤(3.2)中的测量不确定度得到基准标记上所有特征点的重投影误差权值:

其中k为不确定度的阈值，当不确定度过大时，大于阈值k时，表明当前特征点的像素坐标值很不准确，因此将其重投影误差权值置为0，即不参与位姿优化过程。σ_avg表示所有不确定度小于阈值k的特征点的平均不确定度，如果特征点的不确定度小于σ_avg，则表示其像素坐标测量值较为准确，因此将其重投影误差权重设为1，如果不确定度大于σ_avg，则测量值误差较大，因此将其重投影误差权重设为

即不确定度越大，权值越小。

(3.4)根据步骤(3.3)中的重投影误差权值对步骤(2.6)中的双目重投影误差加权，对基准标记中所有特征点的双目重投影误差求和，完成对基于双目视觉的重投影误差位姿优化目标函数的细化：

其中w_i为每个特征点重投影误差权重。

(4)求解基准标记位姿：通过Levenberg-Marquardt算法优化求解式(17)中的目标函数得到基准标记的位姿。

以上是本发明的较佳实施例，凡依本发明技术方案所做的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。

Claims (4)

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＝(t₁,t₂,t₃,t₄)^T，t_b＝(t₅,t₆,t₇,t₈)^T，t_c＝(t₉,t₁₀,t₁₁,t₁₂)^T，则上式可表示为：

当基准标记上一共有n个特征点时，则可以写出关于t的线性方程组：

当匹配点大于6对时，使用奇异值分解对超定方程求解，得到基准标记相对于左相机的位姿初值为R_l和t_l；

(2.2)基于步骤(2.1)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至左相机图像，得到特征点在左相机图像中的二维像素坐标估计值；设标志物上一特征点p在标志物坐标系中的3D坐标为P＝(X,Y,Z)^T，在左相机坐标系下的坐标为P′＝R_lP+t_l＝(X′,Y′,Z′)^T，将其投影至归一化平面得到点p_a，其归一化坐标有如下形式：

设透镜的径向畸变参数为k₁，k₂，k₃，则点p_a与坐标系原点之间距离为r＝

则畸变矫正后的坐标(u_distorted,u_distorted)为

像素坐标的估计值x_L＝(u_b,v_b)经相机模型投影变换后确定：

(2.3)计算基于步骤(2.2)中得到的特征点在左相机图像中的二维像素坐标估计值与二维像素坐标测量值的欧式距离，得到特征点在左相机图像中的重投影误差；设此特征点在左图中的像素坐标测量值为x′_L，则重投影误差表示为：

e_L＝x′_L-x_L       (8)

(2.4)引入双目相机之间的位姿关系就可以作为先验信息作为固定条件约束，基于步骤(2.1)中得到的位姿数据R_l和t_l与双目相机之间的位姿关系R_s和t_s，得到基准标记在右相机坐标系中的位姿数据R_r和t_r；

(2.5)基于步骤(2.4)中得到的位姿数据将基准标记上的特征点在基准标记坐标系中的三维坐标投影至右相机图像，得到特征点在右相机图像中的二维像素坐标估计值x_R；

(2.6)计算基于步骤(2.5)中得到的特征点在右相机图像中的二维像素坐标估计值x_R与二维像素坐标测量值x′_R的欧式距离，得到特征点在右相机图像中的重投影误差e_R＝x′_R-x_R，将特征点在右相机图像中的重投影误差与基于步骤(2.3)得到的特征点在左相机图像中的重投影误差求和，得到特征点的双目重投影误差；

(2.7)基于步骤(2.6)得到的特征点的双目重投影误差，对基准标记中所有特征点的双目重投影误差求和，设T＝[R_l|t_l]，则上述由特征点的世界坐标P经外参投影、畸变矫正和相机模型投影的变换得到像素坐标x_L的过程为x_L＝h(T,P)；设特征点数为n，则可得到基于双目视觉的重投影误差位姿优化目标函数：

4.根据权利要求1所述的基于双目视觉的基准标记位姿估计方法，其特征在于，所述步骤(3)包括如下子步骤：

(3.1)基于步骤(1.2)得到的双目相机外参R_s和t_s将双目相机模型校正为理想模型，使得左右相机图像中像素纵坐标相同；

(3.2)根据基准标记上的特征点在左相机图像和右相机图像中的像素纵坐标测量值的差值的绝对值建立该特征点的测量不确定度：

σ＝|v_L-v_R|            (11)

(3.3)基于步骤(3.2)中的测量不确定度得到基准标记上所有特征点的重投影误差权值:

其中k为不确定度的阈值，σ_avg表示所有不确定度小于阈值k的特征点的平均不确定度；

(3.4)根据步骤(3.3)中的重投影误差权值对步骤(2.6)中的双目重投影误差加权，对基准标记中所有特征点的双目重投影误差求和，完成对基于双目视觉的重投影误差位姿优化目标函数的细化：

其中n为基准标记中特征点数量，w_i为每个特征点重投影误差权重。

Images