Attitude self-constrained RTK image attitude optimization method, storage medium and device

Abstract

The invention discloses an attitude self-constrained RTK image attitude optimization method, a storage medium and a device, and relates to the technical field of real-time dynamic differential localization, and the technical scheme is characterized in that the method comprises the following steps: S1, feature point extraction; s2, feature point matching; s3, triangularizing the feature points; s4, constructing visual constraint information; s5, constructing attitude self-constraint information; and S6, performing nonlinear optimization. According to the method, the camera attitude is used as constraint information, so that attitude excessive optimization caused by visual information is inhibited.

Description

Technical Field

[0001] The present invention relates to the technical field of real-time dynamic differential positioning, and more specifically, to a method, storage medium and device for optimizing the attitude of an RTK image with self-constrained attitude. Background Art

[0002] With the rapid development of intelligent surveying and mapping and the urgent need to improve the efficiency of surveying and mapping operations, automated, intelligent, high-precision and high-efficiency measurement methods have become a work worthy of attention. As a technical device that combines RTK, GNSS / INS combined navigation and close-range photogrammetry technology, image RTK takes images with accurate POS data. The coordinates of points of interest can be obtained through image data, which effectively solves the problem of obtaining high-precision point information in complex scenes such as building textures under the eaves of buildings and areas where GNSS signals are interfered. In image RTK, the accuracy of camera attitude largely determines the measurement accuracy, and the commonly used attitude optimization method is to use visual information as a constraint, optimize the attitude through nonlinear optimization methods, and ensure global attitude consistency.

[0003] The accuracy of image RTK measurement depends largely on the accuracy of camera attitude, and the camera attitude comes from the transmission of IMU attitude. The attitude of IMU is not absolutely accurate, so visual information is usually used as a constraint to optimize the camera attitude through nonlinear optimization methods. However, when the common view area between image sequences is small or there are few features, it is difficult to continuously match enough common view points. In this case, there is less visual information available. Although nonlinear optimization can ensure the global consistency of attitude, the attitude may be over-optimized and there is an overall deviation, which ultimately results in poor accuracy of the solved point.

[0004] Therefore, a new solution needs to be proposed to solve this problem. Summary of the invention

[0005] In view of the shortcomings of the prior art, the purpose of the present invention is to provide a self-constrained RTK image attitude optimization method, storage medium and device, which use the camera attitude itself as constraint information, thereby suppressing the over-optimization of attitude caused by visual information.

[0006] The above technical purpose of the present invention is achieved through the following technical solution: a method for optimizing the attitude of an RTK image with attitude self-constraint, comprising the following steps:

[0007] S1, feature point extraction, extract SIFT feature points and their descriptors of all images;

[0008] S2, feature point matching, using the KNN algorithm to match the feature points between any two images;

[0009] S3, triangulate feature points, triangulate all matched feature points, and calculate the inverse depth of the feature points in the camera coordinate system;

[0010] S4, constructing visual constraint information, including calculating visual reprojection residuals and their derivatives with respect to each camera pose, position, and inverse depth of feature points;

[0011] S5. Construct attitude self-constraint information, define the constraints of the camera attitude through the attitude self-constraint residual formula, and derive the differential of the attitude self-constraint residual to the attitude error;

[0012] S6. Perform nonlinear optimization, incorporate visual constraint information and attitude self-constraint information into the objective function at the same time, and optimize the camera position, attitude and inverse depth of feature points by minimizing the objective function, wherein the objective function includes visual reprojection residual and attitude self-constraint residual.

[0013] In one embodiment, in step S4, the visual reprojection residual is calculated by a visual reprojection residual formula, and the visual reprojection residual formula is as follows:

[0014]

[0015] in, represents the visual reprojection residual of the kth feature point, and Respectively represent the posture of the camera in the i-th frame and the j-th frame in the world coordinate system, and Respectively represent the position of the camera in the i-th frame and the j-th frame in the world coordinate system, p i and pj represent the normalized plane coordinates of one of the feature points on the i-th frame and the j-th frame image respectively, and λ represents the inverse depth of the feature point in the normalized plane coordinate system on the i-th frame image.

[0016] In one embodiment, the derivative formula of the visual reprojection residual with respect to the coordinates of the feature point in the camera coordinate system is as follows:

[0017]

[0018] in, Represents the coordinates of the feature point in the camera coordinate system of the jth frame, and They represent the x and y coordinates of the feature point in the camera coordinate system of the jth frame respectively.

[0019] In one embodiment, the derivative of the visual reprojection residual with respect to the position of the feature point in the world coordinate system of the i-th frame is as follows:

[0020]

[0021] The derivative of the visual reprojection residual to the pose of the feature point in the world coordinate system of the i-th frame is as follows:

[0022]

[0023] The derivative of the visual reprojection residual to the position of the feature point in the world coordinate system of the jth frame is as follows:

[0024]

[0025] The derivative of the visual reprojection residual to the pose of the feature point in the world coordinate system of the jth frame is as follows:

[0026]

[0027] The derivative of the visual reprojection residual to the inverse depth of the feature point is as follows:

[0028]

[0029] In one embodiment, in step S5, the camera posture is expressed by a rotation matrix, and the posture self-constraint residual formula is as follows:

[0030]

[0031] in, Represents the attitude self-constraint residual, which is a three-dimensional rotation vector, represents the observed value of the posture, represents the initial value of the posture, and Log{} represents the logarithmic mapping from Lie group to Lie algebra.

[0032] In one embodiment, in step S5, in order to estimate the posture using nonlinear optimization, it is necessary to derive the differential of the posture residual to the posture error, specifically by using the Lie algebra perturbation method to perform error perturbation on the camera posture in the visual reprojection residual formula, the formula is as follows:

[0033]

[0034] Among them, δφ represents the attitude error disturbance, Exp() represents the exponential mapping from Lie algebra to Lie group, Represents the calculation function of the right inverse Jacobian matrix;

[0035] Based on the derivative formula of the visual reprojection residual to the coordinates of the feature points in the camera coordinate system, the camera posture error is derived to obtain the right inverse Jacobian matrix, as shown below:

[0036]

[0037] The posture error is weighted, and the weighted residual and right inverse Jacobian matrix are:

[0038]

[0039] In one embodiment, the nonlinear optimization has a quantity to be optimized, and the quantity to be optimized is as follows:

[0040]

[0041] in, and Respectively represent the posture of the camera in the i-th frame and the j-th frame in the world coordinate system, and Respectively represent the positions of the camera in the i-th frame and the j-th frame in the world coordinate system, and λ represents the inverse depth of the feature point in the normalized plane coordinate system of the i-th frame image;

[0042] The objective function is as follows:

[0043]

[0044] Where F represents the objective function, represents the visual reprojection residual of the k-th feature point between the i-th and j-th frames, Represents the self-constrained residual of the camera pose in the i-th frame.

[0045] In one of the embodiments, the nonlinear optimization is iteratively solved using the Gauss-Newton method or the Levenberg-Marquardt method to minimize the objective function and optimize the camera position, posture and inverse depth of feature points.

[0046] A computer-readable storage medium includes a posture self-constrained RTK image posture optimization method program. When the posture self-constrained RTK image posture optimization method program is executed by a processor, any of the posture self-constrained RTK image posture optimization methods described above is implemented.

[0047] A computer device includes a processor, a memory, a communication interface and a communication bus. The processor, the memory and the communication interface communicate with each other through the communication bus. The memory is used to store a posture self-constrained RTK image posture optimization method program. The posture self-constrained RTK image posture optimization method program enables the processor to execute any operation corresponding to the posture self-constrained RTK image posture optimization method.

[0048] In summary, the present invention has the following beneficial effects: by introducing attitude self-constraint information, the method can effectively suppress the attitude over-optimization problem that may occur when visual information is insufficient (such as a small common viewing area between image sequences or few feature points), avoid overall deviation of the attitude, thereby achieving better camera attitude estimation, ensuring that the camera attitude solution can maintain a high accuracy even in complex environments, thereby improving the solution accuracy of point coordinates. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 This is a flowchart of the attitude self-constrained RTK image attitude optimization method of an embodiment of the present application. DETAILED DESCRIPTION

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

[0051] like Figure 1 As shown, the embodiment of the present application provides 1. The above technical purpose of the present invention is achieved through the following technical solutions: a method for optimizing the attitude of RTK images with attitude self-constraint, comprising the following steps:

[0052] S1, feature point extraction, extract SIFT feature points and their descriptors of all images;

[0053] S2, feature point matching, using the KNN algorithm to match the feature points between any two images;

[0054] S3, triangulate feature points, triangulate all matched feature points, and calculate the inverse depth of the feature points in the camera coordinate system;

[0055] S4, constructing visual constraint information, including calculating visual reprojection residuals and their derivatives with respect to each camera pose, position, and inverse depth of feature points;

[0056] S5. Construct attitude self-constraint information, define the constraints of the camera attitude through the attitude self-constraint residual formula, and derive the differential of the attitude self-constraint residual to the attitude error;

[0057] S6. Perform nonlinear optimization, incorporate visual constraint information and attitude self-constraint information into the objective function at the same time, and optimize the camera position, attitude and inverse depth of feature points by minimizing the objective function, wherein the objective function includes visual reprojection residual and attitude self-constraint residual.

[0058] In the above method, by introducing attitude self-constraint information, this method can effectively suppress the attitude over-optimization problem that may occur when visual information is insufficient (such as a small common view area between image sequences or few feature points), avoid overall deviation of the attitude, and thus achieve better camera attitude estimation. It ensures that the camera attitude solution can maintain a high accuracy even in complex environments, thereby improving the solution accuracy of point coordinates, and by combining visual reprojection residuals with attitude self-constraint residuals for nonlinear optimization, this method can effectively reduce the errors of camera attitude and position estimation.

[0059] In this embodiment, in step S4, the visual reprojection residual is calculated by a visual reprojection residual formula, and the visual reprojection residual formula is as follows:

[0060]

[0061] in, represents the visual reprojection residual of the kth feature point, and Respectively represent the posture of the camera in the i-th frame and the j-th frame in the world coordinate system, and Respectively represent the position of the camera in the i-th frame and the j-th frame in the world coordinate system, p i and pj represent the normalized plane coordinates of one of the feature points on the i-th frame and the j-th frame image respectively, and λ represents the inverse depth of the feature point in the normalized plane coordinate system on the i-th frame image.

[0062] In the above method, the visual reprojection residual is an important indicator to measure the accuracy of the camera pose, position and inverse depth estimation of feature points. By minimizing this residual, the projection position of feature points under different viewing angles can be ensured to be as consistent as possible, thereby improving the positioning accuracy of the entire system and the accuracy of map construction.

[0063] In this embodiment, the derivative formula of the visual reprojection residual with respect to the coordinates of the feature point in the camera coordinate system is as follows:

[0064]

[0065] in, Represents the coordinates of the feature point in the camera coordinate system of the jth frame, and They represent the x and y coordinates of the feature point in the camera coordinate system of the jth frame respectively.

[0066] In the above method, by calculating the derivative of the visual reprojection residual with respect to the coordinates of the feature point in the camera coordinate system, the optimization algorithm (such as the Gauss-Newton method, the Levenberg-Marquardt method, etc.) can use the gradient information to guide the search direction, thereby converging to the optimal solution or a state close to the optimal solution more quickly, effectively improving the efficiency of the optimization process.

[0067] In this embodiment, the derivative of the visual reprojection residual with respect to the position of the feature point in the world coordinate system of the i-th frame is as follows:

[0068]

[0069] The derivative of the visual reprojection residual to the pose of the feature point in the world coordinate system of the i-th frame is as follows:

[0070]

[0071] The derivative of the visual reprojection residual to the position of the feature point in the world coordinate system of the jth frame is as follows:

[0072]

[0073] The derivative of the visual reprojection residual to the pose of the feature point in the world coordinate system of the jth frame is as follows:

[0074]

[0075] The derivative of the visual reprojection residual to the inverse depth of the feature point is as follows:

[0076]

[0077] In this embodiment, in step S5, the camera posture is expressed by a rotation matrix, and the posture self-constraint residual formula is as follows:

[0078]

[0079] in, Represents the attitude self-constraint residual, which is a three-dimensional rotation vector, represents the observed value of the posture, represents the initial value of the posture, and Log{} represents the logarithmic mapping from Lie group to Lie algebra.

[0080] In this embodiment, in step S5, in order to estimate the posture using nonlinear optimization, it is necessary to derive the differential of the posture residual to the posture error. The specific method is: using the Lie algebra perturbation method to perform error perturbation on the camera posture in the visual reprojection residual formula. The formula is as follows:

[0081]

[0082] Among them, δφ represents the attitude error disturbance, Exp() represents the exponential mapping from Lie algebra to Lie group, Represents the calculation function of the right inverse Jacobian matrix;

[0083] Based on the derivative formula of the visual reprojection residual to the coordinates of the feature points in the camera coordinate system, the camera posture error is derived to obtain the right inverse Jacobian matrix, as shown below:

[0084]

[0085] The posture error is weighted, and the weighted residual and right inverse Jacobian matrix are:

[0086]

[0087] In the above method, the Lie algebra perturbation method provides an accurate mathematical model for small changes in camera posture. By introducing the posture error perturbation quantity and using the exponential mapping from Lie algebra to Lie group, the influence of small changes in posture on the reprojection residual can be accurately represented. The right inverse Jacobian matrix derived based on the Lie algebra perturbation method enables the optimization algorithm to use gradient information to guide the update of posture parameters, which not only improves the convergence speed of the optimization process, but also enhances the accuracy of the optimization results.

[0088] In this embodiment, the nonlinear optimization has a quantity to be optimized, and the quantity to be optimized is as follows:

[0089]

[0090] in, and Respectively represent the posture of the camera in the i-th frame and the j-th frame in the world coordinate system, and Respectively represent the positions of the camera in the i-th frame and the j-th frame in the world coordinate system, and λ represents the inverse depth of the feature point in the normalized plane coordinate system of the i-th frame image;

[0091] The objective function is as follows:

[0092]

[0093] Where F represents the objective function, represents the visual reprojection residual of the k-th feature point between the i-th and j-th frames, Represents the self-constrained residual of the camera pose in the i-th frame.

[0094] In this embodiment, the nonlinear optimization adopts Gauss-Newton method or Levenberg-Marquardt method for iterative solution to minimize the objective function and optimize the camera position, posture and inverse depth of feature points. By minimizing the objective function, the position and posture of the camera and the inverse depth of the feature points can be optimized. The visual constraint information ensures the global consistency of the posture to a certain extent, and the posture self-constraint information suppresses the over-optimization of the posture and avoids the overall deviation of the posture.

[0095] The present invention also discloses a computer-readable storage medium, which includes but is not limited to a hard disk drive, a solid state drive (SSD), a flash drive (such as a USB flash drive, an SD card), an optical disk (such as a CD-ROM, a DVD), a magnetic tape, and a virtual storage device based on cloud storage, etc. The computer-readable storage medium includes a posture self-constrained RTK image posture optimization method program, and when the posture self-constrained RTK image posture optimization method program is executed by a processor, any of the posture self-constrained RTK image posture optimization methods described above is implemented.

[0096] The present invention also discloses a computer device, including a processor, a memory, a communication interface and a communication bus. The processor, the memory and the communication interface communicate with each other through the communication bus. The communication interface should support multiple communication protocols and standards so as to connect with different types of devices and networks, including but not limited to wired and wireless interfaces such as USB, Ethernet, Wi-Fi, Bluetooth, etc. The communication bus can be a traditional PCI, PCI-E bus, or a more modern serial bus (such as USB, Thunderbolt, etc.). The memory is used to store a posture self-constrained RTK image posture optimization method program, and the posture self-constrained RTK image posture optimization method program enables the processor to perform any operation corresponding to the posture self-constrained RTK image posture optimization method.

[0097] The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A method for optimizing the attitude of an RTK image with attitude self-constraint, characterized by: The steps include: S1, feature point extraction, extract SIFT feature points and their descriptors of all images; S2, feature point matching, using the KNN algorithm to match the feature points between any two images; S3, triangulate feature points, triangulate all matched feature points, and calculate the inverse depth of the feature points in the camera coordinate system; S4, constructing visual constraint information, including calculating visual reprojection residuals and their derivatives with respect to each camera pose, position, and inverse depth of feature points; S5. Construct attitude self-constraint information, define the constraints of the camera attitude through the attitude self-constraint residual formula, and derive the differential of the attitude self-constraint residual to the attitude error; S6. Perform nonlinear optimization, incorporate visual constraint information and attitude self-constraint information into the objective function at the same time, and optimize the camera position, attitude and inverse depth of feature points by minimizing the objective function, wherein the objective function includes visual reprojection residual and attitude self-constraint residual.

2. The RTK image attitude optimization method with attitude self-constraint according to claim 1, characterized in that: In step S4, the visual reprojection residual is calculated by a visual reprojection residual formula, and the visual reprojection residual formula is as follows: in, represents the visual reprojection residual of the kth feature point, and Respectively represent the posture of the camera in the i-th frame and the j-th frame in the world coordinate system, and Respectively represent the position of the camera in the i-th frame and the j-th frame in the world coordinate system, p i and p j They represent the normalized plane coordinates of one of the feature points on the i-th frame and the j-th frame image respectively, and λ represents the inverse depth of the feature point in the normalized plane coordinate system on the i-th frame image.

3. The RTK image attitude optimization method with attitude self-constraint according to claim 2, characterized in that: The derivative formula of the visual reprojection residual to the coordinates of the feature point in the camera coordinate system is as follows: in, Represents the coordinates of the feature point in the camera coordinate system of the jth frame, and They represent the x and y coordinates of the feature point in the camera coordinate system of the jth frame respectively.

4. The RTK image attitude optimization method with attitude self-constraint according to claim 3, characterized in that: The derivative of the visual reprojection residual to the position of the feature point in the world coordinate system of the i-th frame is as follows: The derivative of the visual reprojection residual to the pose of the feature point in the world coordinate system of the i-th frame is as follows: The derivative of the visual reprojection residual to the position of the feature point in the world coordinate system of the jth frame is as follows: The derivative of the visual reprojection residual to the pose of the feature point in the world coordinate system of the jth frame is as follows: The derivative of the visual reprojection residual to the inverse depth of the feature point is as follows:

5. The RTK image attitude optimization method with attitude self-constraint according to claim 1, characterized in that: In step S5, the camera posture is expressed by the rotation matrix, and the posture self-constraint residual formula is as follows: in, Represents the attitude self-constraint residual, which is a three-dimensional rotation vector, represents the observed value of the posture, represents the initial value of the posture, and Log{} represents the logarithmic mapping from Lie group to Lie algebra.

6. The RTK image attitude optimization method with attitude self-constraint according to claim 2, characterized in that: In step S5, in order to estimate the posture using nonlinear optimization, it is necessary to derive the differential of the posture residual to the posture error. The specific method is: using the Lie algebra perturbation method to perturb the camera posture in the visual reprojection residual formula. The formula is as follows: Among them, δφ represents the attitude error disturbance, Exp() represents the exponential mapping from Lie algebra to Lie group, Represents the calculation function of the right inverse Jacobian matrix; Based on the derivative formula of the visual reprojection residual to the coordinates of the feature points in the camera coordinate system, the camera posture error is derived to obtain the right inverse Jacobian matrix, as shown below: The posture error is weighted, and the weighted residual and right inverse Jacobian matrix are:

7. The RTK image attitude optimization method with attitude self-constraint according to claim 1, characterized in that: Nonlinear optimization has a quantity to be optimized, which is as follows: in, and Respectively represent the posture of the camera in the i-th frame and the j-th frame in the world coordinate system, and Respectively represent the positions of the camera in the i-th frame and the j-th frame in the world coordinate system, and λ represents the inverse depth of the feature point in the normalized plane coordinate system of the i-th frame image; The objective function is as follows: Where F represents the objective function, represents the visual reprojection residual of the k-th feature point between the i-th and j-th frames, Represents the self-constrained residual of the camera pose in the i-th frame.

8. The RTK image attitude optimization method with attitude self-constraint according to claim 7, characterized in that: The nonlinear optimization adopts Gauss-Newton method or Levenberg-Marquardt method to perform iterative solution to minimize the objective function and realize the optimization of the camera position, posture and inverse depth of feature points.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a posture self-constrained RTK image posture optimization method program. When the posture self-constrained RTK image posture optimization method program is executed by a processor, the posture self-constrained RTK image posture optimization method as described in any one of claims 1-8 is implemented.

10. A computer device, characterized in that: The method comprises a processor, a memory, a communication interface and a communication bus, wherein the processor, the memory and the communication interface communicate with each other via the communication bus, the memory is used to store a program of a method for optimizing the attitude of an RTK image with self-constraint, and the program of the method for optimizing the attitude of an RTK image with self-constraint enables the processor to perform operations corresponding to the method for optimizing the attitude of an RTK image with self-constraint as described in any one of claims 1 to 8.

Images