A fiducial marker pose estimation method based on binocular vision

Abstract

The application provides a fiducial mark pose estimation method based on binocular vision, a re-projection error pose optimization objective function based on binocular vision is established based on the pose relationship constraint between binocular cameras, the measurement uncertainty of feature points on the fiducial mark is established, and then the objective function is refined, and the pose of the fiducial mark is obtained by optimizing the objective function. The method mainly comprises the following steps: 1) calibrating the binocular camera to obtain the internal and external parameters of the binocular camera. 2) establishing a re-projection error pose optimization objective function based on the pose relationship between binocular cameras. 3) calculating the measurement uncertainty of each feature point on the fiducial mark, and weightedly summing the re-projection error to refine the objective function. 4) optimizing the objective function to obtain the pose of the fiducial mark. The measurement uncertainty of each feature point on the fiducial mark is first proposed in the method, which is used for weighting the re-projection error, and the method has the advantages of high pose estimation precision and strong robustness.

Description

Technical Field

[0001] This invention belongs to the field of computer vision applications, specifically relating to a reference marker pose estimation method based on binocular vision. Background Technology

[0002] A surgical navigation system is a real-time system that uses computer technology to provide the pose information of surgical instruments during surgery, helping surgeons complete operations faster and more accurately. The pose estimation of surgical instruments is the core component of a surgical navigation system. To achieve this, a reference marker is rigidly connected to the surgical instrument, and the pose of the reference marker is estimated to obtain the pose information of the surgical instrument.

[0003] Currently, most methods for estimating the pose of reference markers calculate the 3D coordinates of feature points using binocular ranging and then use the iterative nearest-point algorithm to calculate the pose information of the reference markers. This method has many problems. For example, there are errors in calculating the spatial coordinates of feature points, and the subsequent iterative solution process does not reduce these errors, resulting in low accuracy and poor stability of the pose estimation results. In addition, each feature point contributes the same value to the cost function, resulting in poor robustness of the pose estimation. Summary of the Invention

[0004] The present invention aims to overcome the shortcomings of the prior art and provide a reference marker pose estimation method based on binocular vision.

[0005] The objective of this invention is achieved through the following technical solution: a reference marker pose estimation method based on binocular vision, comprising the following steps:

[0006] (1) Calibrate the binocular camera: Use Zhang's calibration method to calibrate the binocular camera and obtain the intrinsic and extrinsic parameters of the binocular camera.

[0007] (2) Establish the pose optimization objective function: calculate the reprojection error for each feature point on the reference mark, and establish the reprojection error pose optimization objective function based on the pose relationship between the binocular cameras.

[0008] (3) Weighted projection error: The binocular camera model is corrected to an ideal model, the measurement uncertainty of each feature point on the reference mark is calculated, and the reprojection error of each feature point in step (2) is weighted and summed through the uncertainty to refine the objective function of the reprojection error pose optimization based on binocular vision in step (2).

[0009] (4) Solve for the pose of the reference marker: The pose of the reference marker can be obtained by optimizing the objective function in step (3) using the Levenberg-Marquardt algorithm.

[0010] Further, step (1) includes the following sub-steps:

[0011] (1.1) Using Zhang's calibration method, single-target calibration is performed on the left and right cameras respectively to obtain the intrinsic parameter matrix, distortion parameters and pose of the calibration plate in the coordinate system of the left and right cameras.

[0012] (1.2) Calculate the positional relationship between the left and right cameras based on the pose of the calibration plate in the left and right camera coordinate systems obtained in step (1.1), i.e., the binocular camera extrinsic parameters, which are represented as rotation matrix Rs and translation vector ts.

[0013] Furthermore, step (2) includes the following sub-steps:

[0014] (2.1) Using the measured three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system and the two-dimensional pixel coordinates of the feature points in the left camera image as input, the pose data of the reference marker in the left camera coordinate system is calculated. First, consider a feature point p on the reference marker, and let its homogeneous coordinates be P = (X, Y, Z, 1). T When projected onto the image, its homogeneous pixel coordinates are x = (u, v, 1). T The pose of the reference marker in the left camera coordinate system is represented by T = [R]. l |t l The diagram shows the relationship between the three-dimensional coordinates of the feature points and the two-dimensional coordinates of the image when T is expanded.

[0015]

[0016] Eliminating the 's' in the last row yields the constraints on u and v:

[0017]

[0018] Let t a =(t1,t2,t3,t4) T , t b =(t5,t6,t7,t8) T , t c =(t9,t 10 ,t 11 ,t 12 ) T Then the above formula can be expressed as:

[0019]

[0020] When there are n feature points on the reference mark, a system of linear equations about t can be written:

[0021]

[0022] When there are more than 6 matching points, singular value decomposition is used to solve the overdetermined equations to obtain the initial pose value of the reference marker relative to the left camera as R. l and t l .

[0023] (2.2) Based on the pose data obtained in step (2.1), the three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system are projected onto the left camera image to obtain the estimated two-dimensional pixel coordinates of the feature points in the left camera image. Let the 3D coordinates of a feature point p on the marker in the marker coordinate system be P = (X, Y, Z). T The coordinates in the left camera coordinate system are P′=R l P+t l =(X′,Y′,Z′) T Projecting it onto the normalized plane yields point p. a Its normalized coordinates have the following form:

[0024]

[0025] Let the radial distortion parameters of the lens be k1, k2, k3, then point p a The distance between the origin and the coordinate system is Then the coordinates after distortion correction (u) distorted ,v distorted )for

[0026]

[0027] The estimated value of pixel coordinates x L =(u b ,v b Determined after projection transformation of the camera model:

[0028]

[0029] (2.3) Calculate the Euclidean distance between the estimated and measured two-dimensional pixel coordinates of the feature point in the left camera image obtained in step (2.2) to obtain the reprojection error of the feature point in the left camera image. Let the measured pixel coordinates of this feature point in the left image be x′. L The reprojection error is expressed as:

[0030] e L =x′ L -x L (8)

[0031] (2.4) Introducing the pose relationship between the binocular cameras can serve as prior information and a fixed constraint, based on the pose data R obtained in step (2.1). l and tl Pose relationship R with stereo camera s and t s The pose data R of the reference marker in the right camera coordinate system is obtained. r and t r .

[0032]

[0033] (2.5) Based on the pose data obtained in step (2.4), project the three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system onto the right camera image to obtain the estimated two-dimensional pixel coordinates x of the feature points in the right camera image. R .

[0034] (2.6) Calculate the estimated two-dimensional pixel coordinates x of the feature points in the right camera image obtained in step (2.5). R Compared with the two-dimensional pixel coordinate measurement value x′ R The Euclidean distance is used to obtain the reprojection error e of the feature points in the right camera image. R =x′ R -x R The reprojection error of the feature points in the right camera image is summed with the reprojection error of the feature points in the left camera image obtained based on step (2.3) to obtain the binocular reprojection error of the feature points.

[0035] (2.7) Based on the binocular reprojection error of the feature points obtained in step (2.6), sum the binocular reprojection errors of all feature points in the reference mark, and let T = [R l |t l The pixel coordinates x are obtained by transforming the world coordinates P of the feature point through extrinsic parameter projection, distortion correction, and camera model projection. L The process is x L = h(T,P). Let the number of feature points be n, then the objective function for optimizing the reprojection error pose based on binocular vision can be obtained as follows:

[0036]

[0037] Furthermore, step (3) includes the following sub-steps:

[0038] (3.1) Based on the binocular camera extrinsic parameters R obtained in step (1.2) s and t s The binocular camera model is calibrated to an ideal model so that the vertical coordinates of pixels in the left and right camera images are the same.

[0039] (3.2) Establish the measurement uncertainty of the feature point based on the absolute value of the difference between the measured pixel ordinate values ​​of the feature point on the reference mark in the left camera image and the right camera image:

[0040] σ=|v L -v R | (11)

[0041] (3.3) Based on the measurement uncertainty in step (3.2), obtain the reprojection error weights for all feature points on the reference mark:

[0042]

[0043] Where k is the threshold of uncertainty, σ avg This represents the average uncertainty of all feature points whose uncertainty is less than the threshold k.

[0044] (3.4) Based on the reprojection error weights in step (3.3), the binocular reprojection error in step (2.6) is weighted, and the binocular reprojection error of all feature points in the reference marker is summed to complete the refinement of the objective function for reprojection error pose optimization based on binocular vision:

[0045]

[0046] Where n is the number of feature points in the baseline marker, w i Weight the reprojection error for each feature point.

[0047] The technical solution of this invention can be summarized as follows:

[0048] 1. A reprojection error pose optimization objective function based on binocular vision principle is proposed. By using the pose relationship between the binocular cameras, the reprojection error of feature points on the reference marker in the left and right camera images is obtained. The summation of the errors yields the reprojection error pose optimization objective function based on binocular vision.

[0049] 2. Based on the alignment properties of binocular vision, the measurement uncertainty of feature points on the reference marker is established, thereby obtaining the reprojection error weights. The binocular reprojection errors of all feature points are weighted and summed to refine the objective function for optimizing the pose based on binocular vision reprojection error. The pose of the reference marker can then be obtained by optimizing and solving the refined objective function using the Levenberg-Marquardt algorithm.

[0050] The beneficial effects of this invention are that introducing the pose relationship between binocular cameras as a fixed constraint can reduce the uncertainty caused by noise, and improve the accuracy and stability of pose estimation results compared with existing methods. By establishing the measurement uncertainty of feature points, higher weights are assigned to feature points with more accurate measurement results, and lower weights are assigned to feature points with inaccurate measurement results, which improves the robustness to factors such as system noise, lighting conditions, and occlusion compared with existing methods. Attached Figure Description

[0051] Figure 1 This is the overall flowchart of the present invention. Detailed Implementation

[0052] The technical solutions of the embodiments of the present invention will be fully described below with reference to the accompanying drawings. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0053] Example:

[0054] like Figure 1 The figure shown is an embodiment of the present invention, which provides a reference marker pose estimation method based on binocular vision, including the following steps:

[0055] (1) Calibrate the stereo camera: Use Zhang's calibration method to calibrate the stereo camera and obtain its intrinsic and extrinsic parameters. This includes the following sub-steps:

[0056] (1.1) Using Zhang’s calibration method, single-target calibration is performed on the left and right cameras respectively to obtain the intrinsic parameter matrix K, radial distortion parameters k1, k2, k3 and the pose of the calibration plate in the coordinate system of the left and right cameras.

[0057] (1.2) Calculate the positional relationship between the left and right cameras, i.e., the binocular camera extrinsic parameters, based on the pose of the calibration plate in the left and right camera coordinate systems obtained in step (1.1), which is expressed as the rotation matrix R. s Translation vector t s Due to the presence of image noise, the calculated R for each view is... s and t s The values ​​may vary slightly, but a more accurate result can be obtained through nonlinear optimization algorithms.

[0058] (2) Calculate the reprojection error for each feature point on the reference mark, and establish a reprojection error pose optimization objective function based on the pose relationship between the binocular cameras, which includes the following sub-steps:

[0059] (2.1) In this embodiment, the reference marker contains 16 feature points. The pose data of the reference marker in the left camera coordinate system is calculated using the measured three-dimensional coordinates of the feature points in the reference marker coordinate system and the two-dimensional pixel coordinates of the feature points in the left camera image as input. First, consider a feature point p on the reference marker, assuming its homogeneous coordinates are P = (X, Y, Z, 1). T When projected onto the image, its homogeneous pixel coordinates are x = (u, v, 1). T The pose of the reference marker in the left camera coordinate system is represented by T = [R].l |t l The diagram shows the relationship between the three-dimensional coordinates of the feature points and the two-dimensional coordinates of the image when T is expanded.

[0060]

[0061] Eliminating the 's' in the last row yields the constraints on u and v:

[0062]

[0063] Let t a =(t1,t2,t3,t4) T , t b =(t5,t6,t7,t8) T , t c =(t9,t 10 ,t 11 ,t 12 ) T Then the above formula can be expressed as:

[0064]

[0065] Since there are 16 feature points on the reference mark, a system of linear equations about t can be written:

[0066]

[0067] Solving the overdetermined equations using singular value decomposition yields the initial pose value R of the reference marker relative to the left camera. l and t l .

[0068] (2.2) Based on the pose data R obtained in step (2.1) l and t l Projecting the 3D coordinates of the feature points on the reference marker in the reference marker coordinate system onto the left camera image yields an estimated 2D pixel coordinate of the feature points in the left camera image. Let the 3D coordinates of a feature point p on the marker in the marker coordinate system be P = (X, Y, Z). T The coordinates in the left camera coordinate system are P′=R l P+t l =(X′,Y′,Z′) T Projecting it onto the normalized plane yields point p. a Its normalized coordinates have the following form:

[0069]

[0070] Point p a The distance between the origin and the coordinate system is The radial distortion parameters obtained in step (1.1) are used to correct the distortion of the coordinates. The corrected coordinates (u) distorted ,v distorted )for

[0071]

[0072] The estimated pixel coordinates x of the feature point L =(u b ,v b Determined after projection transformation of the camera model:

[0073]

[0074] (2.3) Let T = [R l |t l The pixel coordinates x are obtained by transforming the world coordinates P of the feature point through extrinsic parameter projection, distortion correction, and camera model projection. L The process can be represented as x L =h(T,P). Let the pixel coordinate measurement of this feature point in the left image be x′. L Then the reprojection error e L Represented as:

[0075] e L =x′ L -x L (8)

[0076] (2.4) Introduce pose relationship constraints R of the stereo camera s and t s Based on the pose data R obtained in step (2.1) l t l Pose relationship R between the stereo camera and the camera s t s The pose data R of the reference marker in the right camera coordinate system is obtained. r and t r .

[0077]

[0078] (2.5) Based on the pose data R obtained in step (2.4) r and t r Projecting the three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system onto the right camera image yields the estimated two-dimensional pixel coordinates x of the feature points in the right camera image. R The process is x R =h([R s R l |t s +R s tl ],P).

[0079] (2.6) Calculate the estimated two-dimensional pixel coordinates x of the feature points in the right camera image obtained in step (2.5). R Compared with the two-dimensional pixel coordinate measurement value x′ R The Euclidean distance is used to obtain the reprojection error e of the feature points in the right camera image. R =x′ R -x R The reprojection error of the feature points in the right camera image is summed with the reprojection error of the feature points in the left camera image obtained in step (2.3) to obtain the binocular reprojection error of the feature points e = e L +e R .

[0080] (2.7) Based on the binocular reprojection error of the feature points obtained in step (2.6), sum the binocular reprojection errors of all feature points in the reference marker. Since the number of feature points on the reference marker is 16 in this embodiment, the objective function for optimizing the reprojection error pose based on binocular vision can be obtained:

[0081]

[0082] (3) The reprojection error of each feature point of the reference marker participates in the optimization process. In the objective function in step (2.7), each feature point contributes the same value to the cost function. However, in real-world scenarios, factors such as system noise, lighting conditions, and occlusion may cause significant errors in the measurement of feature point pixel coordinates. Continuing to optimize using the objective function in step (2.7) is prone to getting trapped in local optima, thereby reducing the accuracy of the reference marker pose estimation. Therefore, different weights are provided for the reprojection errors of different feature points, and the reprojection errors of each feature point in step (2) are weighted and summed to refine the objective function for reprojection error pose optimization based on binocular vision in step (2). Specifically, the following sub-steps are included:

[0083] (3.1) Based on the binocular camera extrinsic parameters R obtained in step (1.2) s and t s The binocular camera model is calibrated to an ideal model so that the vertical coordinates of pixels in the left and right camera images are the same, that is, the pixels are aligned in rows.

[0084] Let the corrected camera coordinate system be O. rec -X rec Y rec Z rec coordinate axis O rec X rec Parallel to O l O r R recWrite it in the following form

[0085]

[0086] Due to the rotation matrix R rec The row vector is the projection of its coordinate axes onto the camera coordinate system, therefore

[0087]

[0088] coordinate axis O rec Y rec The direction vector can be determined by the translation vector t and O. rec Z rec The direction vector cross product is obtained, therefore the coordinate axis O is obtained. rec Y rec The projection in the camera coordinate system is

[0089]

[0090] After calculating e1 and e2, O rec Z rec If the projections e3 of the coordinate system in the camera coordinate system are orthogonal to each other, then...

[0091]

[0092] After calculating e1, e2, and e3, R rec This confirms that the left and right cameras are rotated via matrix R. l =R rec ·r l and R r =R rec ·r r To achieve row alignment, the vertical coordinates of feature points in the corrected left and right images are the same.

[0093] (3.2) Establish the measurement uncertainty of the feature point based on the absolute value of the difference between the measured pixel ordinate values ​​of the feature point on the reference mark in the left camera image and the right camera image:

[0094] σ=|v L -v R | (15)

[0095] (3.3) Based on the measurement uncertainty in step (3.2), obtain the reprojection error weights for all feature points on the reference mark:

[0096]

[0097] Where k is the uncertainty threshold, when the uncertainty is too large, exceeding the threshold k, it indicates that the pixel coordinates of the current feature point are very inaccurate. Therefore, its reprojection error weight is set to 0, meaning it does not participate in the pose optimization process. σ avg This represents the average uncertainty of all feature points whose uncertainty is less than the threshold k. If the uncertainty of a feature point is less than σ... avg If the pixel coordinate measurement is relatively accurate, then the reprojection error weight is set to 1. If the uncertainty is greater than σ, then... avg If the measurement error is large, then the reprojection error weight is set to 1. That is, the greater the uncertainty, the smaller the weight.

[0098] (3.4) Based on the reprojection error weights in step (3.3), the binocular reprojection error in step (2.6) is weighted, and the binocular reprojection error of all feature points in the reference marker is summed to complete the refinement of the objective function for reprojection error pose optimization based on binocular vision:

[0099]

[0100] Where w i Weight the reprojection error for each feature point.

[0101] (4) Solve the pose of the reference marker: The pose of the reference marker is obtained by optimizing the objective function in equation (17) using the Levenberg-Marquardt algorithm.

[0102] The above are preferred embodiments of the present invention. Any changes made to the technical solution of the present invention that do not exceed the scope of the technical solution of the present invention shall fall within the protection scope of the present invention.

Claims

1. A method for estimating the pose of a reference marker based on binocular vision, characterized in that, Includes the following steps: (1) Calibrate the binocular camera: Use Zhang's calibration method to calibrate the binocular camera and obtain the intrinsic and extrinsic parameters of the binocular camera; (2) Establish the pose optimization objective function: calculate the reprojection error for each feature point on the reference mark, and establish the reprojection error pose optimization objective function based on the pose relationship between the binocular cameras; (3) Weighted projection error: The binocular camera model is corrected to an ideal model, the measurement uncertainty of each feature point on the reference mark is calculated, and the reprojection error of each feature point in step (2) is weighted and summed through the uncertainty to refine the objective function of the reprojection error pose optimization based on binocular vision in step (2); (4) Solve for the pose of the reference marker: The pose of the reference marker can be obtained by optimizing the objective function in step (3) using the Levenberg-Marquardt algorithm; Step (1) includes the following sub-steps: (1.1) Using Zhang's calibration method, single-target calibration is performed on the left and right cameras respectively to obtain the intrinsic parameter matrix, distortion parameters and pose of the calibration plate in the coordinate system of the left and right cameras; (1.2) Calculate the positional relationship between the left and right cameras based on the pose of the calibration plate in the left and right camera coordinate systems obtained in step (1.1), i.e., the extrinsic parameters of the binocular camera, which is expressed as a rotation matrix. Translation vector ; Step (3) includes the following sub-steps: (3.1) Based on the binocular camera extrinsic parameters obtained in step (1.2) and The binocular camera model is corrected to an ideal model so that the pixel ordinates in the left and right camera images are the same; (3.2) Establish the measurement uncertainty of the feature point based on the absolute value of the difference between the measured pixel ordinate values ​​of the feature point on the reference mark in the left camera image and the right camera image: (11) (3.3) Based on the measurement uncertainty in step (3.2), obtain the reprojection error weights for all feature points on the reference mark: (12) Where k is the threshold of uncertainty, This represents the average uncertainty of all feature points whose uncertainty is less than the threshold k; (3.4) The binocular reprojection error is weighted according to the reprojection error weight in step (3.3), and the binocular reprojection error of all feature points in the reference mark is summed to complete the refinement of the objective function for reprojection error pose optimization based on binocular vision.

2. The reference marker pose estimation method based on binocular vision according to claim 1, characterized in that, Step (2) includes the following sub-steps: (2.1) Using the measured three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system and the two-dimensional pixel coordinates of the feature points in the left camera image as input, calculate the pose data of the reference marker in the left camera coordinate system; first consider a feature point p on the reference marker, let its homogeneous coordinates be... When projected onto the image, its pixel homogeneous coordinates are The pose of the reference marker in the left camera coordinate system is used This indicates the relationship between the three-dimensional coordinates of the feature points and the two-dimensional coordinates of the image obtained by expanding T: (1) Eliminating the 's' in the last row yields the constraints on u and v: (2) set up , , Then the above formula can be expressed as: (3) When there are n feature points on the reference mark, a system of linear equations about t can be written: (4) When there are more than 6 matching points, singular value decomposition is used to solve the overdetermined equations to obtain the initial pose of the reference marker relative to the left camera. and ; (2.2) Based on the pose data obtained in step (2.1), the three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system are projected onto the left camera image to obtain the estimated two-dimensional pixel coordinates of the feature points in the left camera image; let the 3D coordinates of a feature point p on the marker in the marker coordinate system be... The coordinates in the left camera coordinate system are Projecting it onto the normalized plane yields the point. Its normalized coordinates have the following form: (5) Let the radial distortion parameter of the lens be... , , Then point The distance between the origin and the coordinate system is Then the coordinates after distortion correction for (6) Estimated pixel coordinates Determined after projection transformation of the camera model: (7) (2.3) Calculate the Euclidean distance between the estimated two-dimensional pixel coordinates of the feature point in the left camera image obtained in step (2.2) and the measured two-dimensional pixel coordinates, to obtain the reprojection error of the feature point in the left camera image; let the measured pixel coordinates of this feature point in the left image be... The reprojection error is expressed as: (8) (2.4) Introducing the pose relationship between the binocular cameras can serve as prior information and fixed condition constraints, based on the pose data obtained in step (2.1). and Pose relationship with stereo cameras and The pose data of the reference marker in the right camera coordinate system is obtained. and ; (9) (2.5) Based on the pose data obtained in step (2.4), project the three-dimensional coordinates of the feature points on the reference marker in the reference marker coordinate system onto the right camera image to obtain the estimated two-dimensional pixel coordinates of the feature points in the right camera image. ; (2.6) Calculate the estimated two-dimensional pixel coordinates of the feature points in the right camera image based on the feature points obtained in step (2.5). With two-dimensional pixel coordinate measurement value The Euclidean distance is used to obtain the reprojection error of the feature points in the right camera image. The reprojection error of the feature points in the right camera image is summed with the reprojection error of the feature points in the left camera image obtained based on step (2.3) to obtain the binocular reprojection error of the feature points. (2.7) Based on the binocular reprojection error of the feature points obtained in step (2.6), sum the binocular reprojection errors of all feature points in the reference mark, and let... The pixel coordinates are obtained from the world coordinates P of the feature point through transformations including extrinsic projection, distortion correction, and camera model projection. The process is ; Let the number of feature points be n, then the objective function for optimizing the reprojection error pose based on binocular vision can be obtained as follows: (10)。

Images