Camera pose estimation method and system based on fusion uncertainty and bias elimination

Abstract

The present invention discloses a camera pose estimation method and system based on fusion uncertainty and deviation elimination, which relates to the technical field of computer vision processing, and comprises the following steps: acquiring a picture, calibrating a camera using the picture, obtaining a camera intrinsic parameter matrix, and calculating a covariance matrix of fusion uncertainty based on the camera intrinsic parameter matrix by using an uncertainty propagation formula; acquiring the Jacobian matrix and perturbation of the camera extrinsic parameters, converting a pre-established objective function of fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of fusion uncertainty is constructed based on the covariance matrix of fusion uncertainty; iteratively optimizing the linear least squares objective function, and solving it to obtain a corrected least squares solution as a camera pose estimation result.

Description

Technical Field

[0001] The present invention relates to the field of computer vision processing technology, and in particular to a camera pose estimation method and system based on fusion uncertainty and deviation elimination. Background Art

[0002] Camera pose estimation is a fundamental problem in the field of computer vision and is also a core component of technologies such as industrial robots, simultaneous localization and mapping (SLAM), augmented reality (AR), and autonomous driving. Accurate camera pose estimation is of great significance to the implementation and optimization of the above applications. In practical applications, the extraction of reference points is relatively simple, so camera pose estimation usually refers to the PnP (Perspective-n-Point) problem based on the reference point information in the image.

[0003] The PnP problem is to substitute the camera calibration intrinsic parameters as the true value for the solution when the camera intrinsic parameters have been calibrated. However, in practical applications, the camera calibration results are often not the true values, but an optimal calibration result is selected. This choice will lead to the uncertainty of the camera intrinsic parameters, thereby reducing the accuracy of the solution to the PnP problem. Since the covariance matrix describing the uncertainty of the camera intrinsic parameters is high in dimension, it is difficult to effectively integrate it into the solution of the PnP problem. In addition, the extraction accuracy of image points is closely related to the grayscale pattern of their neighborhood images, and the grayscale pattern often shows significant differences between different regions. Changes in these grayscale patterns may cause the coordinates of feature points to shift in different directions. Therefore, the uncertainty of different image points varies, and their contributions to the solution are also different. Current research usually regards image points as true values, or only considers the uncertainty in the image point extraction process, resulting in the inability to effectively improve the estimation accuracy. Summary of the invention

[0004] In order to solve the deficiencies mentioned in the above background technology, the purpose of the present invention is to provide a camera pose estimation method and system based on fusion uncertainty and bias elimination.

[0005] In a first aspect, the purpose of the present invention can be achieved by the following technical solution: a camera pose estimation method based on fusion uncertainty and deviation elimination, the method comprising the following steps:

[0006] Get the picture, use the picture to calibrate the camera, get the camera intrinsic parameter matrix, and use the uncertainty propagation formula based on the camera intrinsic parameter matrix to calculate the covariance matrix of the fusion uncertainty;

[0007] Obtaining the Jacobian matrix and perturbation of the camera extrinsic parameters, and converting the pre-established objective function of the fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of the fusion uncertainty is constructed based on the covariance matrix of the fusion uncertainty;

[0008] The linear least squares objective function is iteratively optimized and solved to obtain the corrected least squares solution as the camera pose estimation result.

[0009] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the process of calibrating the camera using the image to obtain the camera intrinsic parameter matrix:

[0010] Use the target camera to collect m pictures, extract and match feature point information, which includes image points and 3D points Where i represents the i-th feature point, j represents the j-th picture, and i = 1, 2, ..., n, j = 1, 2, ..., m, u ij and v ij Respectively represent the image point q in the pixel plane coordinate system ij The u-axis coordinate and v-axis coordinate, X ij , Y ij and Z ij Respectively represent the three-dimensional point p in the world coordinate system ij X W , Y W and Z W The coordinates of the axis. Use m pictures to calibrate the target camera to get the camera intrinsic parameter matrix K:

[0011]

[0012] Among them, f x ,f y are the focal lengths of the camera on the x-axis and y-axis respectively, and (u0,v0) are the coordinates of the principal point.

[0013] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the covariance matrix of the fusion uncertainty is obtained by establishing a reprojection error function of camera calibration, taking partial derivatives of camera parameters to establish a Jacobian matrix J, and obtaining a covariance matrix Σ of the camera intrinsic parameters K , through the uncertainty propagation formula, we get the covariance Φ of the uncertainty propagated from the camera intrinsic parameter to the pixel point i , construct the inverse covariance matrix C of the feature point extraction error i -1 , the covariance matrix of feature point extraction error is C i , the covariance matrix of the fused uncertainty is modeled as Σ i =Φ i +C i .

[0014] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the covariance matrix Σ K The calculation of is as follows:

[0015] The projection error of the feature point is modeled as:

[0016]

[0017] in, and Respectively represent the difference between the u-axis coordinates and the v-axis coordinates of the image point and the projection point in the pixel coordinate system, e1, e2, e3 are intermediate matrices, in the form of e1 = [1, 0, 0], e2 = [0, 1, 0], e3 = [0, 0, 1]; R and t represent the external parameters of the camera, in the following form:

[0018]

[0019] Among them, r1, r2, r3 are the first column of the rotation matrix, representing the world coordinate system X W The projection of the axis in the camera coordinate system; r4, r5, r6 are the second column of the rotation matrix, representing the world coordinate system Y W The projection of the axis in the camera coordinate system; r7, r8, r9 are the third column of the rotation matrix, representing the world coordinate system Z W The projection of the axis in the camera coordinate system; t1, t2, t3 represent the translation in X W ,Y W and Z W Let T = [R, t], which represents the external parameter matrix of the camera, and take the partial derivative of the camera parameters K and T in equation (2) to obtain the Jacobian matrix J:

[0020]

[0021] Among them, A j and B j is the intermediate matrix, j = 1, 2, ..., m, A j represents the partial derivative matrix of the residual of each pixel in the jth image with respect to the camera intrinsic parameter; B j Represents the partial derivative matrix of the residual of each pixel in the jth image with respect to the camera extrinsic parameter, in the following form:

[0022]

[0023] in, It means the partial derivative of the function (*) with respect to the variable (·);

[0024] K i represents the i-th parameter of K, in order f x ,f y,u0,v0;

[0025] T i Represents the i-th parameter of T, in the order r1, r2, r3, r4, r5, r6, r7, r8, r9, t1, t2, t3;

[0026] The covariance matrix of camera parameters K and T is J·J:

[0027]

[0028] in, Represents the transposed matrix of the matrix [*];

[0029] make Among them, diag[*] represents the diagonal matrix of [*], It is expressed as:

[0030]

[0031] Extract the covariance matrix Σ about the camera intrinsic parameter K from equation (7): K :

[0032]

[0033] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the covariance Φ of the uncertainty propagated from the camera intrinsic parameter uncertainty to the pixel point i The calculation process:

[0034] The point in the camera coordinate system Point projected to pixel coordinates The projection equation is:

[0035]

[0036] Among them, x i ,y i and z i Represents the three-dimensional points in the camera coordinate system X C ,Y C and Z C Coordinate of the axis; u i and v i Respectively represent the image point q in the pixel coordinate system i The u-axis coordinates and v-axis coordinates of the pixel coordinate system are reversely projected into the camera coordinate system, and the reverse projection obtains the point of the camera coordinate system. for:

[0037]

[0038] The covariance of pixel uncertainty caused by the propagation of camera intrinsic parameter uncertainty is Φ i for:

[0039]

[0040] Among them, P i is the partial derivative matrix of the camera intrinsic parameters in formula (9):

[0041]

[0042] The inverse covariance matrix of the feature point extraction error Modeling:

[0043]

[0044] Among them, C i is the covariance matrix of measurement error, W i The characteristic point (u i ,v i ) is the elliptical area centered on i (u i ,v i ) is the sum of the pixel gray levels in the ellipse area, and are the gradients of the image in the u and v directions respectively.

[0045] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the covariance matrix Σ of the fusion uncertainty i for:

[0046] Σ i =Φ i +C i (14)

[0047] Then, the objective function based on fusion uncertainty is constructed, and the pinhole camera projection model is:

[0048]

[0049] Dividing both sides by the depth information, the pixel residual ε can be obtained through formula (15): i :

[0050]

[0051] in, To simplify formula (16), let Formula (16) can be abbreviated as:

[0052]

[0053] Then there is an objective function based on fusion uncertainty:

[0054]

[0055] in, is Σ i The inverse matrix of .

[0056] In combination with the first aspect, in some implementations of the first aspect, the method further includes: for each residual term ε i Perform a first-order Taylor expansion:

[0057] ε i ≈H i δ T (19)

[0058] Among them, H i is the Jacobian matrix of the residual to the camera extrinsic parameters T = [R, t], δ T is the deviation of the rotation matrix and the translation vector.

[0059] Construct a linear least squares objective function:

[0060]

[0061] The normal equation is constructed from the linear least squares objective function (20):

[0062]

[0063] in,

[0064] In combination with the first aspect, in some implementations of the first aspect, the method further includes: the calculation process of obtaining the corrected least squares solution:

[0065] Formula (20) is used as the optimization objective function to solve the pose parameters, and the formula (20) is iteratively optimized until δ T If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix is ​​output as follows:

[0066] Assume that the camera extrinsic parameters at the kth iteration are T k =[R k ,t k ], according to equations (17) and (19), we get the current pixel residual ε i k , the Jacobian matrix H i k and the deviation δ T k , the current normal equation is:

[0067]

[0068] Solve the normal equations to get δ T k :

[0069]

[0070] The current estimated correction value is then:

[0071] T k+1 =T k +δ T k (twenty four)

[0072] And used as the camera external parameter for the next iteration. Until δ T If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix T is output. k .

[0073] In a second aspect, in order to achieve the above-mentioned object, the present invention discloses a camera pose estimation system based on fusion uncertainty and deviation elimination, comprising:

[0074] The camera calibration module is used to obtain pictures, calibrate the camera using the pictures, obtain the camera intrinsic parameter matrix, and calculate the covariance matrix of the fused uncertainty using the uncertainty propagation formula based on the camera intrinsic parameter matrix;

[0075] An objective function establishment module is used to obtain the Jacobian matrix and perturbation of the camera extrinsic parameters, and convert the pre-established objective function of the fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of the fusion uncertainty is constructed based on the covariance matrix of the fusion uncertainty;

[0076] The pose estimation module is used to iteratively optimize and solve the linear least squares objective function to obtain the corrected least squares solution as the camera pose estimation result.

[0077] In another aspect of the present invention, in order to achieve the above-mentioned purpose, a terminal device is disclosed, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the memory stores a computer program capable of running on the processor, and when the processor loads and executes the computer program, the camera pose estimation method based on fusion uncertainty and bias elimination as described above is adopted.

[0078] Beneficial effects of the present invention:

[0079] The present invention considers the pixel uncertainty caused by the camera intrinsic parameter error and the feature point extraction error at the same time, and integrates them into the projection model to construct a new weighted objective function based on the fusion uncertainty. The new objective function is weighted by the corresponding uncertainty coefficient, so that it can adapt to different degrees of uncertainty. The propagation problem of pixel point uncertainty in the solution process is considered, and the pose solution obtained based on deviation elimination is the optimal solution when the feature points have uncertainty. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative work.

[0081] Figure 1 It is a schematic flow chart of the method of the present invention;

[0082] Figure 2 It is a schematic diagram of the corner points of the chessboard and their projections under arbitrary rotation and translation of the present invention;

[0083] Figure 3 It is a schematic diagram of the system structure of the present invention. DETAILED DESCRIPTION

[0084] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0085] Embodiment 1:

[0086] The following is an introduction to the relevant terms involved in the embodiments of the present application:

[0087] Camera calibration refers to the process of determining the internal and external parameters of a camera through a series of steps. These parameters include the camera's intrinsic parameter matrix, extrinsic parameter matrix, and distortion coefficients. The purpose of camera calibration is to enable the captured image to accurately reflect the size and position of the real world, ensuring the accuracy of measurement and 3D reconstruction. The importance of camera calibration lies in that it can eliminate image distortion caused by factors such as lens distortion and installation errors, thereby improving image quality and measurement accuracy.

[0088] like Figure 1 As shown, the camera pose estimation method based on fusion uncertainty and deviation elimination includes the following steps:

[0089] S101: Acquire an image, use the image to calibrate the camera, obtain the camera intrinsic parameter matrix, and calculate the covariance matrix of the fused uncertainty using the uncertainty propagation formula based on the camera intrinsic parameter matrix;

[0090] is the 2D image point in the pixel coordinate system, and the 3D points in the world coordinate system and camera coordinate system are respectively and Indicates. i and v i Respectively represent the image point q in the pixel plane coordinate system i The u-axis coordinate and v-axis coordinate, X i ,Y i and Z i Respectively represent the three-dimensional point p in the world coordinate system i X W ,Y W and Z W The coordinate of the axis, x i ,y i and z i They represent the three-dimensional point p in the camera coordinate system respectively. i C X C ,Y C and Z C The coordinates of the axis. The positional relationship between the world coordinate system and the camera coordinate system is R,t, which is the unknown parameter to be determined.

[0091] The process of using pictures to calibrate the camera and obtain the camera intrinsic parameter matrix:

[0092] Use the target camera to collect m pictures, extract and match feature point information, which includes image points and 3D points Where i represents the i-th feature point, j represents the j-th picture, and i=1,2,...,n,j=1,2,...,m,, and m pictures are used to calibrate the target camera to obtain the camera intrinsic parameter matrix K:

[0093]

[0094] Among them, f x ,f y are the focal lengths of the camera on the x-axis and y-axis respectively, and (u0,v0) are the coordinates of the principal point.

[0095] The covariance matrix of the fused uncertainty is obtained by establishing the reprojection error function of the camera calibration, taking partial derivatives of the camera parameters to establish the Jacobian matrix J, and obtaining the covariance matrix Σ of the camera intrinsic parameters K, through the uncertainty propagation formula, we get the covariance Φ of the uncertainty propagated from the camera intrinsic parameter to the pixel point i , construct the inverse covariance matrix of feature point extraction error The covariance matrix of feature point extraction error is C i , the covariance matrix of the fused uncertainty is modeled as Σ i =Φ i +C i ;

[0096] The covariance matrix Σ of the camera intrinsic parameters K The calculation of is as follows:

[0097] The projection error of the feature points is modeled as:

[0098]

[0099] in, and Respectively represent the difference between the u-axis coordinates and the v-axis coordinates of the image point and the projection point in the pixel coordinate system, e1, e2, e3 are intermediate matrices, in the form of e1 = [1, 0, 0], e2 = [0, 1, 0], e3 = [0, 0, 1]; R and t represent the external parameters of the camera, in the following form:

[0100]

[0101] Among them, r1, r2, r3 are the first column of the rotation matrix, representing the world coordinate system X W The projection of the axis in the camera coordinate system; r4, r5, r6 are the second column of the rotation matrix, representing the world coordinate system Y W The projection of the axis in the camera coordinate system; r7, r8, r9 are the third column of the rotation matrix, representing the world coordinate system Z W The projection of the axis in the camera coordinate system; t1, t2, t3 represent the translation in X W ,Y W and Z W Let T = [R, t], which represents the external parameter matrix of the camera, and take the partial derivative of the camera parameters K and T in equation (2) to obtain the Jacobian matrix J:

[0102]

[0103] Among them, A j and B j is the intermediate matrix, j = 1, 2, ..., m, A j represents the partial derivative matrix of the residual of each pixel in the jth image with respect to the camera intrinsic parameter; B j Represents the partial derivative matrix of the residual of each pixel in the jth image with respect to the camera extrinsic parameter, in the following form:

[0104]

[0105] in, It means the partial derivative of the function (*) with respect to the variable (·);

[0106] K i represents the i-th parameter of K, in order f x ,f y ,u0,v0;

[0107] T i Represents the i-th parameter of T, in the order r1, r2, r3, r4, r5, r6, r7, r8, r9, t1, t2, t3;

[0108] The covariance matrix of camera parameters K and T is J·J:

[0109]

[0110] in, Represents the transposed matrix of the matrix [*];

[0111] make Among them, diag[*] represents the diagonal matrix of [*], It is expressed as:

[0112]

[0113] Extract the covariance matrix Σ about the camera intrinsic parameter K from equation (7): K :

[0114]

[0115] The uncertainty of the camera intrinsic parameter is propagated to the covariance of the uncertainty of the pixel point Φ i The calculation process:

[0116] The point in the camera coordinate system Point projected to pixel coordinates

[0117] The projection equation is:

[0118]

[0119] Among them, x i ,y i and z i Represents the three-dimensional points in the camera coordinate system X C ,Y C and Z C Coordinate of the axis; u i and vi Respectively represent the image point q in the pixel coordinate system i The u-axis coordinates and v-axis coordinates of the pixel coordinate system are reversely projected into the camera coordinate system, and the reverse projection obtains the point of the camera coordinate system. for:

[0120]

[0121] The covariance of pixel uncertainty caused by the propagation of camera intrinsic parameter uncertainty is Φ i for:

[0122]

[0123] Among them, P i is the partial derivative matrix of the camera intrinsic parameters in formula (9):

[0124]

[0125] The inverse covariance matrix C of the feature point extraction error i -1 Modeling:

[0126]

[0127] Among them, C i is the covariance matrix of measurement error, W i The characteristic point (u i ,v i ) is the elliptical area centered on i (u i ,v i ) is the sum of the pixel gray levels in the ellipse area, and are the gradients of the image in the u and v directions respectively.

[0128] S102: Obtain the Jacobian matrix and perturbation of the camera extrinsic parameters, and convert the pre-established objective function of the fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of the fusion uncertainty is constructed based on the covariance matrix of the fusion uncertainty;

[0129] The covariance matrix of the fused uncertainty Σ i for:

[0130] Σ i =Φ i +C i (14)

[0131] Then, the objective function based on fusion uncertainty is constructed, and the pinhole camera projection model is:

[0132]

[0133] Dividing both sides by the depth information, the pixel residual can be obtained through formula (15):

[0134]

[0135] in, To simplify formula (16), let Formula (16) can be abbreviated as:

[0136]

[0137] Then there is an objective function based on fusion uncertainty:

[0138]

[0139] in, is Σ i The inverse matrix of .

[0140] For each residual term ε i Perform a first-order Taylor expansion:

[0141] ε i ≈H i δ T (19)

[0142] Among them, H i is the Jacobian matrix of the residual to the camera extrinsic parameters T = [R, t], δ T is the deviation of the rotation matrix and the translation vector.

[0143] Construct a linear least squares objective function:

[0144]

[0145] The normal equation is constructed from the linear least squares objective function (20):

[0146]

[0147] in,

[0148] S103: Iteratively optimize and solve the linear least squares objective function to obtain a corrected least squares solution as the camera pose estimation result.

[0149] The calculation process of obtaining the corrected least squares solution is:

[0150] Formula (20) is used as the optimization objective function to solve the pose parameters, and the formula (20) is iteratively optimized until δ TIf the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix is ​​output as follows:

[0151] Assume that the camera extrinsic parameters at the kth iteration are T k =[R k ,t k ], according to equations (17) and (19), we get the current pixel residual ε i k , the Jacobian matrix H i k and the deviation δ T k , the current normal equation is:

[0152]

[0153] Solve the normal equations to get δ T k :

[0154]

[0155] The current estimated correction value is then:

[0156] T k+1 =T k +δ T k (twenty four)

[0157] And used as the camera external parameter for the next iteration. Until δ T If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix T is output. k .

[0158] Specifically, the present invention is further described below by way of embodiments:

[0159] The present invention uses the reprojection error as a criterion for verifying the effectiveness of the algorithm, and compares the method of the present invention with other advanced PnP algorithms. Specifically, this experiment uses the pose relationship obtained by the PnP algorithm to reproject the world coordinates corresponding to all corner points on the calibration plate to the image plane, and calculates the absolute distance with the extracted corner points. The more accurate the pose information solved by the PnP algorithm, the smaller the distance between the corner points reprojected to the image plane and the extracted corner points.

[0160] The image acquisition device used in this experiment is Logitech C270. The checkerboard target used is custom-made from anodized aluminum panels and glass substrates with a processing accuracy of 0.01mm. The size of each checkerboard is 15mm, and the pattern array is 12×9. The camera calibration results are as follows:

[0161]

[0162] Experimental process:

[0163] 1. Use the corner detection algorithm to extract the corner points on the checkerboard target;

[0164] 2. Randomly select 20 points from the extracted corner points and use these points to solve the posture relationship between the camera and the calibration plate;

[0165] 3. Use the obtained pose relationship to reproject the world coordinates corresponding to all corner points on the calibration plate onto the image, and calculate the absolute distance with the extracted corner points. This experiment uses the mean reprojection error (RMSE) as an indicator to evaluate the solution accuracy of the PnP algorithm. The calculation method is as follows:

[0166]

[0167] In the formula, (u i ,v i ) is the coordinate of the corner point obtained by reprojection, is the coordinate of the extracted corner points, n is the number of extracted corner points, and N represents the number of experiments. Specifically, this experiment was repeated 100 times;

[0168] 4. Collect 5 images of the calibration plate at different postures and distances, and use the PnP algorithm to calculate the RMSE of each image. The visualization results of the reprojection error are as follows: Figure 2 As shown in the figure, the red "○" represents the extracted corner points, and the blue "□" represents the reprojected points. The RMSE results of each PnP algorithm are shown in Table 1, where EPnP and DLS are methods that directly solve the feature points as ideal situations; EPnPU and DLSU are improved methods based on EPnP and DLS that consider the uncertainty of two-dimensional image points; MLPnP is a maximum likelihood estimation method that considers the uncertainty of two-dimensional image points; and it also takes the uncertainty of two-dimensional image points and the uncertainty of camera intrinsic parameters into consideration. Analysis of the experimental data shows that the reprojection error of this method on all images is relatively small, and it has a good experimental effect.

[0169] Table 1: RMSE of each PnP algorithm

[0170]

[0171] Embodiment 2: In the second aspect, as Figure 3 As shown, in order to achieve the above-mentioned purpose, the present invention discloses a camera pose estimation system based on fusion uncertainty and deviation elimination, comprising:

[0172] The camera calibration module 11 is used to obtain pictures, calibrate the camera using the pictures, obtain the camera intrinsic parameter matrix, and calculate the covariance matrix of the fused uncertainty using the uncertainty propagation formula based on the camera intrinsic parameter matrix;

[0173] An objective function establishment module 12 is used to obtain the Jacobian matrix and perturbation of the camera extrinsic parameters, and convert the pre-established objective function of the fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of the fusion uncertainty is constructed based on the covariance matrix of the fusion uncertainty;

[0174] The pose estimation module 13 is used to iteratively optimize and solve the linear least squares objective function to obtain a corrected least squares solution as the camera pose estimation result.

[0175] Based on the same inventive concept, the present invention also provides a computer device, which includes: one or more processors, and a memory for storing one or more computer programs; the program includes program instructions, and the processor is used to execute the program instructions stored in the memory. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is used to implement one or more instructions, specifically for loading and executing one or more instructions in a computer storage medium to implement the above method.

[0176] It needs to be further explained that, based on the same inventive concept, the present invention also provides a computer storage medium, on which a computer program is stored, and the computer program is executed by a processor to execute the above method. The storage medium can adopt any combination of one or more computer-readable media. The computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electrical, magnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program, which can be used by an instruction execution system, device or device or used in combination with it.

[0177] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0178] The above shows and describes the basic principles, main features and advantages of the present disclosure. Those skilled in the art should understand that the present disclosure is not limited by the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present disclosure. Without departing from the spirit and scope of the present disclosure, the present disclosure may have various changes and improvements, and these changes and improvements fall within the scope of the present disclosure to be protected.

Claims

1. A camera pose estimation method based on fusion uncertainty and bias elimination, characterized in that: The method comprises the following steps: Get the picture, use the picture to calibrate the camera, get the camera intrinsic parameter matrix, and use the uncertainty propagation formula based on the camera intrinsic parameter matrix to calculate the covariance matrix of the fusion uncertainty; The process of using pictures to calibrate the camera and obtain the camera intrinsic parameter matrix: Use the target camera to collect m pictures, extract and match n feature point information, the feature point information includes image points and 3D points Where i represents the i-th feature point, j represents the j-th picture, and i = 1, 2, ..., n, j = 1, 2, ..., m, u ij and v ij Respectively represent the image point q in the pixel plane coordinate system ij The u-axis coordinate and v-axis coordinate, X ij ,Y ij and Z ij Respectively represent the three-dimensional point p in the world coordinate system ij X W ,Y W and Z W The coordinates of the axis are obtained by calibrating the target camera using m pictures to obtain the camera intrinsic parameter matrix K: Among them, f x ,f y are the focal lengths of the camera on the x-axis and y-axis respectively, and (u0, v0) are the coordinates of the principal point; The covariance matrix of the fused uncertainty is obtained by establishing the reprojection error function of the camera calibration, taking partial derivatives of the camera parameters to establish the Jacobian matrix J, and obtaining the covariance matrix Σ of the camera intrinsic parameters K , through the uncertainty propagation formula, we get the covariance Φ of the uncertainty propagated from the camera intrinsic parameter to the pixel point i , construct the inverse covariance matrix of feature point extraction error The covariance matrix of feature point extraction error is C i , the covariance matrix of the fused uncertainty is modeled as Σ i =Φ i +C i ; The covariance matrix Σ of the camera intrinsic parameters K The calculation of is as follows: The projection error of the feature point is modeled as: in, and Respectively represent the difference between the u-axis coordinates and the v-axis coordinates of the image point and the projection point in the pixel coordinate system, e1, e2, e3 are intermediate matrices, in the form of e1 = [1, 0, 0], e2 = [0, 1, 0], e3 = [0, 0, 1]; R and t represent the external parameters of the camera, in the following form: Among them, r1, r2, r3 are the first column of the rotation matrix, representing the world coordinate system X W The projection of the axis in the camera coordinate system; r4, r5, r6 are the second column of the rotation matrix, representing the world coordinate system Y W The projection of the axis in the camera coordinate system; r7, r8, r9 are the third column of the rotation matrix, representing the world coordinate system Z W The projection of the axis in the camera coordinate system; t1, t2, t3 represent the translation in X W ,Y W and Z W Components of the direction, let T = [R, t], which represents the external parameter matrix of the camera, and take partial derivatives of the camera parameters K and T in equation (2) to obtain the Jacobian matrix J: Among them, A j and B j is the intermediate matrix, j = 1, 2, ..., m, A j represents the partial derivative matrix of the residual of each pixel in the jth image with respect to the camera intrinsic parameter; B j Represents the partial derivative matrix of the residual of each pixel in the jth image with respect to the camera extrinsic parameter, in the following form: in, It means the partial derivative of the function (*) with respect to the variable (·); K i represents the i-th parameter of K, in order f x ,f y ,u0,v0; T i Represents the i-th parameter of T, in the order r1, r2, r3, r4, r5, r6, r7, r8, r9, t1, t2, t3; The covariance matrix of camera parameters K and T is J · J: in, Represents the transposed matrix of the matrix [*]; make Among them, diag[*] represents the diagonal matrix of [*], It is expressed as: Extract the covariance matrix Σ about the camera intrinsic parameter K from equation (7): K : The uncertainty of the camera intrinsic parameter is propagated to the covariance of the uncertainty of the pixel point Φ i The calculation process: The point in the camera coordinate system Point projected to pixel coordinates The projection equation is: Among them, x i ,y i and z i Represent the three-dimensional points in the camera coordinate system X C ,Y C and Z C Coordinate of the axis; u i and v i Respectively represent the image point q in the pixel coordinate system i The u-axis coordinates and v-axis coordinates of the pixel coordinate system are reversely projected into the camera coordinate system, and the reverse projection obtains the point of the camera coordinate system. for: The covariance of pixel uncertainty caused by the propagation of camera intrinsic parameter uncertainty is Φ i for: Among them, P i is the partial derivative matrix of the camera intrinsic parameters in formula (9): Inverse covariance matrix of feature point extraction error Modeling: Among them, C i is the covariance matrix of measurement error, W i The characteristic point (u i ,v i ) is the elliptical area centered on i (u i ,v i ) is the sum of the pixel gray levels in the ellipse area, and are the gradients of the image in the u and v directions respectively; The covariance matrix of the fused uncertainty Σ i for: S i =Φ i +C i (14) Then, the objective function based on fusion uncertainty is constructed, and the pinhole camera projection model is: Dividing both sides by the depth information, the pixel residual ε can be obtained through formula (15): i : in, To simplify formula (16), let Formula (16) can be abbreviated as: Then there is an objective function based on fusion uncertainty: in, is Σ i The inverse matrix of For each residual term ε i Perform a first-order Taylor expansion: e i ≈H i d T (19) Among them, H i is the Jacobian matrix of the residual to the camera extrinsic parameters T = [R, t], δ T is the deviation of the rotation matrix and the translation vector; Construct a linear least squares objective function: The normal equation is constructed from the linear least squares objective function (20): in, Obtaining the Jacobian matrix and perturbation of the camera extrinsic parameters, and converting the pre-established objective function of the fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of the fusion uncertainty is constructed based on the covariance matrix of the fusion uncertainty; Iteratively optimize and solve the linear least squares objective function to obtain the corrected least squares solution as the camera pose estimation result; The calculation process of obtaining the corrected least squares solution is: Formula (20) is used as the optimization objective function to solve the pose parameters, and the formula (20) is iteratively optimized until δ T If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix is ​​output as follows: Assume that the camera extrinsic parameters at the kth iteration are T k =[R k ,t k ], according to equations (17) and (19), the current pixel residual is obtained Jacobian matrix and the deviation δ T k , the current normal equation is: Solve the normal equations to get δ T k : The current estimated correction value is then: T k+1 =T k +δ T k (24) And used as the camera external parameter for the next iteration until δ T If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix T is output. k .

2. A camera pose estimation system based on fusion uncertainty and bias elimination, which adopts the camera pose estimation method based on fusion uncertainty and bias elimination described in claim 1, characterized in that: include: The camera calibration module is used to obtain pictures, calibrate the camera using the pictures, obtain the camera intrinsic parameter matrix, and calculate the covariance matrix of the fused uncertainty using the uncertainty propagation formula based on the camera intrinsic parameter matrix; An objective function establishment module is used to obtain the Jacobian matrix and perturbation of the camera extrinsic parameters, and convert the pre-established objective function of the fusion uncertainty into a linear least squares objective function, wherein the pre-established objective function of the fusion uncertainty is constructed based on the covariance matrix of the fusion uncertainty; The pose estimation module is used to iteratively optimize and solve the linear least squares objective function to obtain the corrected least squares solution as the camera pose estimation result.

3. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that: The memory stores a computer program that can be run on the processor. When the processor loads and executes the computer program, the camera pose estimation method based on fusion uncertainty and deviation elimination described in claim 1 is adopted.

Images