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

Abstract

The invention discloses a camera pose estimation method and system based on fusion uncertainty and deviation elimination, and relates to the technical field of computer visual processing, and the method comprises the following steps: obtaining a picture, calibrating a camera through the picture, obtaining a camera internal reference matrix, and obtaining a camera pose estimation matrix; based on the internal reference matrix of the camera, an uncertainty propagation formula is adopted to calculate and obtain a covariance matrix of fusion uncertainty; a Jacobian matrix and disturbance of external parameters of the camera are obtained, a pre-established target function with the fusion uncertainty is converted into a linear least square target function, and the pre-established target function with the fusion uncertainty is established based on a covariance matrix with the fusion uncertainty; and carrying out iterative optimization on the linear least square objective function, and solving to obtain a corrected least square 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 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: 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; 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; The linear least squares objective function is iteratively optimized and solved to obtain the corrected least squares solution as the camera pose estimation result.

[0006] 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: Using target camera to collect Pictures, extract and match feature point information, feature point information includes image points and 3D points ,in Indicates feature points, Indicates pictures, and , , and Represents the image points in the pixel plane coordinate system The u-axis coordinate and v-axis coordinate of and Represents three-dimensional points in the world coordinate system of and The coordinates of the axis. Use Calibrate the target camera using the image to get the camera intrinsic parameter matrix : (1) in, are the focal lengths of the camera on the x-axis and y-axis, respectively. The main point coordinates.

[0007] 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 established by establishing a reprojection error function of camera calibration, taking partial derivatives of camera parameters to establish a Jacobian matrix , get the covariance matrix of the camera internal parameters , through the uncertainty propagation formula, we can get the covariance of the uncertainty propagated from the camera intrinsic parameter uncertainty to the pixel point , construct the inverse covariance matrix of feature point extraction error , the covariance matrix of feature point extraction error is , the covariance matrix of the fusion uncertainty is modeled as .

[0008] In combination with the first aspect, in some implementations of the first aspect, the method further includes: a covariance matrix of the camera intrinsic parameters The calculation of is as follows: The projection error of the feature points is modeled as: (2) 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, is the intermediate matrix, in the form of ; and Represents the external parameters of the camera in the following form: (3) in The first column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; The second column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; The third column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; Respectively represent the translation and The direction component. , represents the camera's external parameter matrix, for the camera parameters of formula (2) and Find the partial derivative and get the Jacobian matrix : (4) in, and is the intermediate matrix, , Indicates The partial derivative matrix of the residual of each pixel in the image with respect to the camera intrinsic parameters; Indicates The partial derivative matrix of the residual of each pixel in the picture with respect to the camera external parameters is as follows: (5) in, Representation function For variables Find partial derivatives; express No. parameters, in the following order ; express No. parameters, in the following order ; Camera parameters and The covariance matrix of : (6) in, Representation Matrix The transposed matrix of make ,in, express The diagonal matrix of It is expressed as: (7) Extract the camera intrinsic parameters from equation (7): The covariance matrix of : (8).

[0009] 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 The calculation process: The point in the camera coordinate system Point projected to pixel coordinates The projection equation is: (9) in, and Represents the three-dimensional points in the camera coordinate system of and The coordinates of the axes; and Represents the image points in the pixel coordinate system 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: (10) Covariance of pixel uncertainty caused by camera intrinsic uncertainty propagation for: (11) in, is the partial derivative matrix of the camera intrinsic parameters in formula (9): (12) The inverse covariance matrix of the feature point extraction error Modeling: (13) in, is the covariance matrix of measurement error, For feature points The elliptical area centered on 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.

[0010] In combination with the first aspect, in some implementations of the first aspect, the method further includes: a covariance matrix of the fusion uncertainty for: (14) Then, the objective function based on fusion uncertainty is constructed, and the pinhole camera projection model is: (15) Dividing both sides by the depth information, the pixel residual can be obtained through equation (15): : (16) in, , to simplify formula (16), let , formula (16) can be abbreviated as: (17) Then there is an objective function based on fusion uncertainty: (18) in, yes The inverse matrix of .

[0011] In combination with the first aspect, in some implementations of the first aspect, the method further includes: for each residual term Perform a first-order Taylor expansion: (19) in, is the residual to the camera extrinsic parameters The Jacobian matrix of is the deviation of the rotation matrix and the translation vector.

[0012] Construct a linear least squares objective function: (20) The normal equation is constructed from the linear least squares objective function (20): (twenty one) in, , , .

[0013] 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: Formula (20) is used as the optimization objective function to solve the pose parameters, and formula (20) is iteratively optimized until 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 The camera extrinsic parameters at the iteration are , according to equations (17) and (19), we can get the current pixel residual , the Jacobian matrix and deviation , the current normal equation is: (twenty two) Solve the normal equations to get : (twenty three) The current estimated correction value is then: (twenty four) And used as the camera external parameter for the next iteration. If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix is ​​output. .

[0014] 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: 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.

[0015] 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.

[0016] Beneficial effects of the present invention: 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

[0017] 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. Figure 1 It is a schematic flow chart of the method of the present invention; 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; Figure 3 It is a schematic diagram of the system structure of the present invention. DETAILED DESCRIPTION

[0018] 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.

[0019] Embodiment 1: The following is an introduction to the relevant terms involved in the embodiments of the present application: 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 coefficient. The purpose of camera calibration is to make the captured image 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.

[0020] like Figure 1 As shown, the camera pose estimation method based on fusion uncertainty and deviation elimination includes the following steps: 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; 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. Among them, and Represents the image points in the pixel plane coordinate system The u-axis coordinate and v-axis coordinate of and Represents three-dimensional points in the world coordinate system of and The coordinates of the axes, and Represents the three-dimensional points in the camera coordinate system of and Axis coordinates. The positional relationship between the world coordinate system and the camera coordinate system That is, the unknown parameter to be found.

[0021] The process of using pictures to calibrate the camera and obtain the camera intrinsic parameter matrix: Using target camera to collect Pictures, extract and match feature point information, feature point information includes image points and 3D points ,in Indicates feature points, Indicates pictures, and , ,,use Calibrate the target camera using the image to get the camera intrinsic parameter matrix : (1) in, are the focal lengths of the camera on the x-axis and y-axis, respectively. The main point coordinates.

[0022] The covariance matrix of the fused uncertainty is established by establishing the reprojection error function of the camera calibration and taking partial derivatives of the camera parameters to establish the Jacobian matrix , get the covariance matrix of the camera internal parameters , through the uncertainty propagation formula, we can get the covariance of the uncertainty propagated from the camera intrinsic parameter uncertainty to the pixel point , construct the inverse covariance matrix of feature point extraction error , the covariance matrix of feature point extraction error is , the covariance matrix of the fusion uncertainty is modeled as ; The covariance matrix of the camera intrinsic parameters The calculation of is as follows: The projection error of the feature point is modeled as: (2) in, and Represents the image point and projection point in the pixel coordinate system respectively Axis coordinates and The difference in axis coordinates, is the intermediate matrix, in the form of ; and Represents the external parameters of the camera in the following form: (3) in The first column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; The second column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; The third column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; Respectively represent the translation and The direction component. , represents the camera's external parameter matrix, for the camera parameters of formula (2) and Find the partial derivative and get the Jacobian matrix : (4) in, and is the intermediate matrix, , Indicates The partial derivative matrix of the residual of each pixel in the image with respect to the camera intrinsic parameters; Indicates The partial derivative matrix of the residual of each pixel in the picture with respect to the camera external parameters is as follows: (5) in, Representation function For variables Find partial derivatives; express No. parameters, in the following order ; express No. parameters, in the following order ; Camera parameters and The covariance matrix of : (6) in, Representation Matrix The transposed matrix of make ,in, express The diagonal matrix of It is expressed as: (7) Extract the camera intrinsic parameters from equation (7): The covariance matrix of : (8).

[0023] The covariance of the uncertainty propagated from the camera intrinsic parameter to the pixel point The calculation process: The point in the camera coordinate system Point projected to pixel coordinates The projection equation is: (9) in, and Represents the three-dimensional points in the camera coordinate system of and The coordinates of the axes; and Represents the image points in the pixel coordinate system 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: (10) Covariance of pixel uncertainty caused by camera intrinsic uncertainty propagation for: (11) in, is the partial derivative matrix of the camera intrinsic parameters in formula (9): (12) Inverse covariance matrix of feature point extraction error Modeling: (13) in, is the covariance matrix of measurement error, For feature points The elliptical area centered on 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.

[0024] 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; The covariance matrix of the fused uncertainty for: (14) Then, the objective function based on fusion uncertainty is constructed, and the pinhole camera projection model is: (15) Dividing both sides by the depth information, the pixel residual can be obtained through equation (15): (16) in, , to simplify formula (16), let , formula (16) can be abbreviated as: (17) Then there is an objective function based on fusion uncertainty: (18) in, yes The inverse matrix of .

[0025] For each residual term Perform a first-order Taylor expansion: (19) in, is the residual to the camera extrinsic parameters The Jacobian matrix of is the deviation of the rotation matrix and the translation vector.

[0026] Construct a linear least squares objective function: (20) The normal equation is constructed from the linear least squares objective function (20): (twenty one) in, , , .

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

[0028] 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 formula (20) is iteratively optimized until 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 The camera extrinsic parameters at the iteration are , according to equations (17) and (19), we can get the current pixel residual , the Jacobian matrix and deviation , the current normal equation is: (twenty two) Solve the normal equations to get : (twenty three) The current estimated correction value is then: (twenty four) And used as the camera external parameter for the next iteration. If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix is ​​output. .

[0029] Specifically, the present invention is further described below by way of embodiments: 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.

[0030] 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.01 mm. The size of each checkerboard is 15 mm, and the pattern array is 12 × 9. The camera calibration results are as follows: (25) Experimental process: 1. Use the corner detection algorithm to extract the corner points on the checkerboard target; 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; 3. Use the obtained pose relationship to reproject the world coordinates of 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: (26) In the formula, are the corner coordinates obtained by reprojection, are the extracted corner point coordinates, is the number of corner points extracted, represents the number of experiments. Specifically, this experiment was repeated 100 times; 4. Collect 5 images of the calibration plate at different postures and distances, and use the PnP algorithm to calculate the , the visualization result of the reprojection error is as follows Figure 2 As shown, the red " " represents the extracted corner points, and the blue " " represents the reprojected point. 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 takes the uncertainty of two-dimensional image points and the uncertainty of camera intrinsic parameters into consideration at the same time. 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.

[0031] Table 1: RMSE of each PnP algorithm 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: 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; 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; 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.

[0032] 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.

[0033] It should 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 the 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.

[0034] 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.

[0035] 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; 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; The linear least squares objective function is iteratively optimized and solved to obtain the corrected least squares solution as the camera pose estimation result.

2. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 1, characterized in that: The process of using pictures to calibrate the camera and obtain the camera intrinsic parameter matrix: Using target camera to collect images, extract and match Feature point information includes image points and 3D points ,in Indicates feature points, Indicates pictures, and , , and Represents the image points in the pixel plane coordinate system The u-axis coordinate and v-axis coordinate of and Represents three-dimensional points in the world coordinate system of and The coordinates of the axes are given using Calibrate the target camera using the image to get the camera intrinsic parameter matrix : (1) in, are the focal lengths of the camera on the x-axis and y-axis, respectively. The main point coordinates.

3. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 1, characterized in that: The covariance matrix of the fused uncertainty is established by establishing the reprojection error function of the camera calibration and taking partial derivatives of the camera parameters to establish the Jacobian matrix , get the covariance matrix of the camera internal parameters , through the uncertainty propagation formula, we can get the covariance of the uncertainty propagated from the camera intrinsic parameter uncertainty to the pixel point , construct the inverse covariance matrix of feature point extraction error , the covariance matrix of feature point extraction error is , the covariance matrix of the fused uncertainty is modeled as .

4. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 3, characterized in that: The covariance matrix of the camera intrinsic parameters The calculation of is as follows: The projection error of the feature points is modeled as: (2) in, and Represents the image point and projection point in the pixel coordinate system respectively Axis coordinates and The difference in axis coordinates, is the intermediate matrix, in the form of ; and Represents the external parameters of the camera in the following form: (3) in The first column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; The second column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; The third column of the rotation matrix represents the world coordinate system The projection of the axis into the camera coordinate system; Respectively represent the translation and The direction component, let , represents the camera's external parameter matrix, for the camera parameters of formula (2) and Find the partial derivative and get the Jacobian matrix : (4) in, and is the intermediate matrix, , Indicates The partial derivative matrix of the residual of each pixel in the image with respect to the camera intrinsic parameters; Indicates The partial derivative matrix of the residual of each pixel in the picture with respect to the camera external parameters is as follows: (5) in, Representation function For variables Find partial derivatives; express No. parameters, in the following order ; express No. parameters, in the following order ; Camera parameters and The covariance matrix of : (6) in, Representation Matrix The transposed matrix of make ,in, express The diagonal matrix of It is expressed as: (7) Extract the camera intrinsic parameters from equation (7): The covariance matrix of : (8)。 5. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 4, characterized in that: The covariance of the uncertainty propagated from the camera intrinsic parameter to the pixel point The calculation process: The point in the camera coordinate system Point projected to pixel coordinates The projection equation is: (9) in, and Represent the three-dimensional points in the camera coordinate system of and The coordinates of the axes; and Represents the image points in the pixel coordinate system 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: (10) Covariance of pixel uncertainty caused by camera intrinsic uncertainty propagation for: (11) in, is the partial derivative matrix of the camera intrinsic parameters in formula (9): (12) Inverse covariance matrix of feature point extraction error Modeling: (13) in, is the covariance matrix of measurement error, For feature points The elliptical area centered on 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.

6. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 5, characterized in that: The covariance matrix of the fused uncertainty for: (14) Then, the objective function based on fusion uncertainty is constructed, and the pinhole camera projection model is: (15) Dividing both sides by the depth information, the pixel residual can be obtained through equation (15): : (16) in, , to simplify formula (16), let , formula (16) can be abbreviated as: (17) Then there is an objective function based on fusion uncertainty: (18) in, yes The inverse matrix of .

7. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 6, characterized in that: For each residual term Perform a first-order Taylor expansion: (19) in, is the residual to the camera extrinsic parameters The Jacobian matrix of is the deviation of the rotation matrix and the translation vector; Construct a linear least squares objective function: (20) The normal equation is constructed from the linear least squares objective function (20): (21) in, , , .

8. The camera pose estimation method based on fusion uncertainty and bias elimination according to claim 7, characterized in that: 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 formula (20) is iteratively optimized until 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 The camera extrinsic parameters at the iteration are , according to equations (17) and (19), we can get the current pixel residual , the Jacobian matrix and deviation , the current normal equation is: (22) Solve the normal equations to get : (23) The current estimated correction value is then: (24) And used as the camera external parameter for the next iteration until If the number of iterations is less than the threshold or exceeds the maximum number of iterations, the current rotation matrix is ​​output. .

9. 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 as claimed in any one of claims 1 to 8, 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.

10. 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 any one of claims 1 to 8 is adopted.

Images