Monocular vision inertial odometry method based on feature point depth

Abstract

The application relates to a monocular vision inertial odometer method based on feature point depth, which comprises the following steps: acquiring input images of two adjacent frames and extracting feature points in the images respectively; matching the extracted feature points; predicting first camera pose changes by using acceleration measurement data and angular rate measurement data output by an inertial measurement sensor; obtaining the depth of each pair of matched feature points based on the first camera pose changes; if the number of feature points meets a preset condition, constructing a weight for each pair of feature points and obtaining a weight matrix; constructing a weighted observation equation set based on an epipolar geometry constraint equation and the weight matrix; constructing a quadratic form based on the weighted observation equation set and solving the quadratic form to obtain an essential matrix of the matched feature points in the least square sense; obtaining a second rotation matrix and a second translation vector by singular value decomposition based on the essential matrix and the second rotation matrix and the second translation vector, and obtaining the representation of actual camera pose changes.

Description

Technical Field

[0001] This invention relates to the field of visual odometry, and more particularly to a monocular visual inertial odometry method based on feature point depth. Background Technology

[0002] Visual odometry (VO) technology utilizes the similarity between adjacent frames to continuously estimate the camera's motion trajectory, thus achieving a positioning function similar to vehicle wheel odometry. Visual-Inertial Odometry (VIO) further integrates camera and inertial measurement unit (IMU) data. IMU measurement data can provide predictions of vehicle position and attitude changes between two frames, eliminating the spatial scale blurring problem of monocular cameras and improving the update output frequency and robustness of the entire visual-inertial odometry system.

[0003] Currently, visual odometry and visual inertial odometry can be mainly divided into two types: feature-point based methods and direct methods that do not use feature points. Feature-point based methods are the mainstream approach in current visual odometry and visual inertial odometry, and have a long research history. Feature-point based methods assume that for two adjacent image frames, some representative points, called feature points, should be selected first. Then, by analyzing the changes of these feature points in the pixel coordinate system, the camera's motion in the world coordinate system is estimated based on the camera model, and the spatial positions of the feature points are also estimated.

[0004] However, the relative motion speeds of these numerous feature points in the image pixel coordinate system differ. Feature points with shallower depths, being closer to the camera and lens, exhibit more significant changes in the pixel coordinate system; while feature points with greater depths, being farther from the camera and lens, show relatively less noticeable changes. This phenomenon is even more pronounced in outdoor scenes. Taking outdoor in-vehicle visual navigation applications as an example, when a vehicle is moving, nearby feature points move rapidly, while distant feature points move slowly. Therefore, feature points of different depths contribute differently to the estimation accuracy of visual odometry and visual-inertial odometry, thus introducing significant errors in camera trajectory estimation. Summary of the Invention

[0005] The purpose of this invention is to provide a monocular vision inertial odometry method based on feature point depth.

[0006] To achieve the above-mentioned objectives, this invention provides a monocular vision inertial odometry method based on feature point depth, comprising:

[0007] S1. Acquire two adjacent frames of input images based on a monocular camera, and extract feature points from the two frames of input images respectively;

[0008] S2. Match the feature points extracted from the two frames of the input images;

[0009] S3. Using the acceleration measurement data and angular rate measurement data output by the inertial measurement sensor, predict the first camera pose change corresponding to the two frames of the input image; wherein, the first camera pose change is represented by the corresponding first rotation matrix and first translation vector;

[0010] S4. Based on the pose change of the first camera, obtain the depth of each pair of matched feature points in the two frames of the input image;

[0011] S5. If the number of matching feature points in the two frames of the input image meets the preset condition, then construct weights for each pair of matching feature points and obtain the corresponding weighting matrix.

[0012] S6. Based on the epipolar geometric constraint equations and the weighting matrix, construct a set of weighted observation equations;

[0013] S7. Construct a quadratic form based on the weighted observation equations and solve the quadratic form to obtain the essential matrix of the matched feature points in the least squares sense;

[0014] S8. Using the essential matrix and through singular value decomposition, the second rotation matrix and the second translation vector of the camera between two adjacent frames are calculated, and the representation of the actual camera pose change is obtained based on the second rotation matrix and the second translation vector.

[0015] According to one aspect of the present invention, step S4, which involves obtaining the depth of each pair of matched feature points in the two frames of the input images based on the camera pose change, includes:

[0016] Assuming that a pair of matching feature points in two adjacent frames of the input image are divided into , ;

[0017] Obtain the feature points , Normalized coordinates on the normalized plane of the monocular camera , ;

[0018] Using the first rotation matrix and the first translation vector representing the pose change of the first camera, and the normalized coordinates of the feature points... , Obtain feature points , The depth is represented as:

[0019]

[0020]

[0021] in, Representing feature points depth, Representing feature points depth, Denotes the first rotation matrix. Let R represent the first translation vector, and let R represent the second rotation matrix used to represent the actual camera pose change, with the first rotation matrix as the reference. Instead of solving, t represents the second translation vector used to represent the actual camera pose change, with the first translation vector... Instead of solving.

[0022] According to one aspect of the present invention, in step S5, the preset condition is that the number of matched feature points in the two frames of the input images is greater than 8 pairs.

[0023] According to one aspect of the present invention, in step S5, in the step of constructing weights for each pair of matched feature points and obtaining the corresponding weighting matrix, the weights are as follows:

[0024]

[0025] in, Indicates the i-th pair of matching feature points , The depth in the two adjacent frames, Indicates the i-th pair of matching feature points , The weights in the two adjacent image frames;

[0026] The weighting matrix is ​​represented as follows:

[0027]

[0028] Where W represents the weighting matrix.

[0029] According to one aspect of the invention, in step S6, the step of constructing a weighted observation equation set based on the epipolar geometric constraint equations and the weighting matrix, wherein the weighted observation equation set is expressed as:

[0030]

[0031]

[0032]

[0033] Where e is the vector form of the essential matrix of the matched feature points, For the i-th pair of matching feature points , The pixel coordinates in the input image between two adjacent frames.

[0034] According to one aspect of the invention, in step S7, the quadratic form is constructed based on the weighted observation equations, and the quadratic form is solved to obtain the essential matrix of the matched feature points in the least squares sense, wherein the quadratic form is expressed as:

[0035]

[0036] According to one aspect of the present invention, step S8, which involves using the essential matrix and calculating the second rotation matrix and second translation vector of the camera between two adjacent frames using singular value decomposition, and obtaining a representation of the actual camera pose change based on the second rotation matrix and second translation vector, includes:

[0037] Using the essential matrix, and through singular value decomposition, multiple sets of the second rotation matrix and the second translation vector of the camera between two adjacent frames are calculated;

[0038] Based on the depth determination of at least one pair of matched feature points, it is determined whether each group of the second rotation matrix and the second translation vector is reasonable. If reasonable, they are retained as a representation of the actual camera pose change.

[0039] According to one aspect of the present invention, the step of calculating multiple sets of the second rotation matrix and the second translation vector of the camera between two adjacent frames using the essential matrix and through singular value decomposition includes:

[0040] The essential matrix is ​​transformed using the singular value decomposition algorithm, and the essential matrix is ​​represented as follows:

[0041]

[0042] Where E represents the essential matrix, U and V are orthogonal matrices, and ∑ is the singular value matrix;

[0043] Singular value decomposition of the essential matrix yields multiple sets of the second rotation matrix and the second translation vector, which are expressed as follows:

[0044]

[0045] in, This indicates that the rotation matrix is ​​obtained by rotating 90° along the Z-axis of the camera coordinate system.

[0046] According to one aspect of the present invention, in step S2, the feature points in the two extracted input images are matched using a brute-force matching algorithm or a fast approximate nearest neighbor algorithm.

[0047] According to one aspect of the present invention, in step S1, in the step of acquiring two adjacent frames of input images based on a monocular camera and extracting feature points from the two frames of input images respectively, the feature points are extracted using a corner point extraction algorithm or a local image feature extraction algorithm.

[0048] According to one aspect of the present invention, by utilizing the depth information of matched feature points to construct measurement weights, the role of each feature point in the image is allocated more rationally, thereby obtaining higher monocular visual inertial odometry estimation accuracy.

[0049] According to one aspect of the present invention, by using equal weights for visual updates on each feature point, the contribution of feature points at different depths in the estimation process is effectively and accurately reflected, and errors in camera motion trajectory estimation are more effectively suppressed.

[0050] According to one aspect of the present invention, the present invention can be conveniently implemented and effectively improve performance, without relying on binocular camera configuration or external sensors such as lidar and millimeter-wave radar, and has a low application cost.

[0051] According to one aspect of the present invention, the present invention can be applied to both indoor and outdoor application environments, and its scope of application is wider and more convenient. Attached Figure Description

[0052] Figure 1 This is a schematic diagram illustrating the steps of a monocular vision inertial odometry method according to an embodiment of the present invention;

[0053] Figure 2 This is a schematic diagram illustrating the correspondence between adjacent frame input images and external spatial points in a monocular visual inertial odometry method according to an embodiment of the present invention.

[0054] Figure 3 This is a schematic diagram illustrating the correspondence between four sets of solutions obtained by decomposing the essential matrix E in a monocular visual inertial odometry method according to an embodiment of the present invention and external spatial points. Detailed Implementation

[0055] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. The embodiments cannot be described in detail here, but the embodiments of the present invention are not limited to the following embodiments.

[0056] like Figure 1 As shown, according to one embodiment of the present invention, a monocular vision inertial odometry method based on feature point depth includes:

[0057] S1. Acquire two adjacent frames of input images based on a monocular camera, and extract feature points from the two frames of input images respectively;

[0058] S2. Match the feature points extracted from the two input images;

[0059] S3. Using the acceleration measurement data and angular rate measurement data output by the inertial measurement sensor, predict the pose change of the first camera corresponding to the two input images; wherein, the pose change of the first camera is represented by the corresponding first rotation matrix and first translation vector;

[0060] S4. Based on the pose change of the first camera, obtain the depth of each pair of matched feature points in the two input images;

[0061] S5. If the number of matching feature points in the two input images meets the preset condition, then construct weights for each pair of matching feature points and obtain the corresponding weighting matrix.

[0062] S6. Based on the epipolar geometric constraint equations and the weighting matrix, construct a weighted observation equation set;

[0063] S7. Construct a quadratic form based on the weighted observation equations and solve the quadratic form to obtain the essential matrix of the matched feature points in the least squares sense;

[0064] S8. Using the essential matrix and singular value decomposition, the second rotation matrix and the second translation vector of the camera between two adjacent frames are calculated, and the representation of the actual camera pose change is obtained based on the second rotation matrix and the second translation vector.

[0065] According to one embodiment of the present invention, a visual inertial odometry system is divided into a front end and a back end. The front end estimates a rough camera motion based on information from adjacent images and inertial measurement data, providing a better initial value to the back end. Visual inertial odometry calculation methods are mainly divided into two categories: feature point methods and direct methods. Among them, the feature point method-based front end has long been considered the mainstream method of visual inertial odometry. It has the advantages of stability and insensitivity to lighting and moving objects, and is currently a relatively mature solution. One of the core problems of visual inertial odometry is how to estimate camera motion from images. However, an image itself is a matrix composed of brightness and color, and it would be very difficult to consider motion estimation directly at the matrix level. Therefore, a more convenient approach is to first select representative points from the input images, called feature points. These feature points remain unchanged under the premise of slight changes in camera viewpoint, thus allowing the extraction of the same feature points from various input images. Then, based on these feature points, the problem of camera pose estimation and the localization of these feature points are discussed.

[0066] Therefore, in step S1, feature point extraction can be achieved through an intuitive method, namely, identifying corner points across different images and determining their correspondence. In this approach, corner points are referred to as feature points. In this embodiment, the corner point extraction algorithm can be one of the Harris corner point extraction algorithm, the FAST corner point extraction algorithm, or the GFTT corner point extraction algorithm. Of course, in another embodiment, when a simple corner point extraction algorithm cannot meet the requirements (for example, a place that appears as a corner point from a distance may no longer appear as a corner point when the camera gets closer; or, when the camera is rotated, the appearance of the corner point changes, making it difficult to identify that it is the same corner point), a local image feature extraction algorithm can be used for feature point extraction. In this embodiment, the local image feature extraction algorithm can be one of the SIFT algorithm, the SURF algorithm, or the ORB algorithm. Compared with the aforementioned naive corner point extraction algorithms, local image feature extraction algorithms have better repeatability and distinguishability, as well as higher efficiency and locality.

[0067] According to one embodiment of the present invention, in step S2, after calculating and extracting feature points from adjacent input images, matching these feature points is also a crucial step in visual inertial odometry. Broadly speaking, feature matching solves the data association problem in visual inertial odometry, that is, determining the correspondence between currently seen feature points and previously seen feature points. Accurate matching of descriptors between images or between images and maps can significantly reduce the burden on subsequent operations such as pose estimation and optimization.

[0068] In this embodiment, considering two adjacent input images at times t and t+1, if feature points are extracted from the input image It... Input image Extracting feature points The simplest feature matching method is brute-force matching. That is, for each feature point... With all The distance between descriptors is measured, then sorted, and the nearest feature point is taken as the matching point. Descriptor distance represents the similarity between two features, and different distance metric norms can be used in practice.

[0069] When the number of feature points is large, the brute-force matching method becomes computationally expensive, especially when matching a frame with an entire map. This can lead to computational latency, making it difficult to meet real-time requirements. In such cases, the Fast Library for Approximate Nearest Neighbors (FLANN) algorithm is more suitable for situations with a large number of matching points.

[0070] According to one embodiment of the present invention, in step S3, the acceleration measurement data and angular rate measurement data output by the inertial measurement sensor are used to predict the first camera pose change corresponding to the two frames of the input images. In this embodiment, the parameters output by the external inertial measurement sensor can be used to roughly obtain a camera pose change, i.e., the first camera pose change. Therefore, the rough first camera pose change can be used to calculate the depth of feature points in monocular vision, facilitating the smooth progress of subsequent calculations and effectively avoiding the drawback that monocular vision cannot directly calculate the depth of feature points. In this embodiment, the first camera pose change is represented by a corresponding first rotation matrix. and the first translation vector express.

[0071] According to one embodiment of the present invention, since a monocular camera is used to acquire the input image, only the pixel coordinates of 2D feature points can be obtained from the input image. Therefore, when estimating motion based on two sets of 2D feature points in adjacent frame images, the epipolar geometry method is used for solving.

[0072] Suppose a pair of well-matched feature points are obtained from two input images, as shown in the attached figure. Figure 2 As shown. The first frame of the input image on the left is denoted as I1, and the second frame of the image on the right is denoted as I2. If there are several pairs of such matching points, the camera motion between two frames can be recovered through the correspondence of these two-dimensional image points. (See appendix...) Figure 2The first frame image on the left side of the middle In the coordinate system, let the spatial position of the external point P be:

[0073]

[0074] Based on the pinhole camera model, it can be seen that two matching feature points on two adjacent input frames are... , The pixel position is:

[0075]

[0076] Where K is the camera intrinsics matrix; R is the rotation matrix between the two camera coordinate systems (i.e., the second rotation matrix to be obtained to represent the actual camera pose change); and t is the translation vector between the two camera coordinate systems (i.e., the second translation vector to be obtained to represent the actual camera pose change). , For two matching feature points , The depth. Specifically, what is being calculated here is... and Because they transform the coordinates from the first coordinate system to the second coordinate system.

[0077] For easier description, we further use homogeneous coordinates to represent pixels. When using homogeneous coordinates, a vector is equal to itself multiplied by any non-zero constant. This is typically used to represent a projection relationship. For example, and They are projected to each other, meaning they are equal in homogeneous coordinates. This equality is called equality to a scale, denoted as:

[0078]

[0079] Therefore, the two projection relationships mentioned above can be written as:

[0080]

[0081] Furthermore, take:

[0082] ,

[0083] In the above formula , Substituting the coordinates of the two pixels on the normalized plane into the aforementioned projection relationship, we get:

[0084]

[0085] Multiply both sides of the formula by the left side This is equivalent to taking the outer product of both sides with t simultaneously:

[0086]

[0087] Then, multiply by the left side on both sides simultaneously.

[0088]

[0089] It can be seen that the left side of the equation It is a combination of and t and Both are perpendicular vectors. It further relates to... When performing the inner product, we get 0. Since the left side of the equation is strictly zero, multiplying it by any non-zero constant also results in zero. Therefore, we can... Write it using the usual equals sign. Therefore, we get a concise formula:

[0090]

[0091] Furthermore, resubstitute , get:

[0092]

[0093] Both of the aforementioned formulas are called epipolar constraint equations, and their geometric meaning is... P All three are coplanar. Epipolar constraints simultaneously include translation and rotation. Let the middle part of the first epipolar constraint equation be denoted as the essential matrix E, and the middle part of the second epipolar constraint equation as the fundamental matrix F. Thus, the epipolar constraint equation can be further simplified as follows:

[0094] , ,

[0095] Therefore, the epipolar constraint concisely provides the spatial relationship between the two matching points. Thus, the monocular camera pose estimation problem is reduced to the following two steps:

[0096] 1) Calculate the essential matrix E or the fundamental matrix F based on the pixel positions of the paired feature points;

[0097] 2) Calculate the second rotation matrix R and the second translation vector t of the monocular camera based on the essential matrix E or the fundamental matrix F.

[0098] Since the essential matrix E and the fundamental matrix F differ only in the camera intrinsic parameters, and the camera intrinsic parameters are usually pre-calibrated and known, the essential matrix E, which has a simpler solution form, is chosen to obtain the actual pose changes of the monocular camera.

[0099] As mentioned earlier, before solving for the essential matrix E, it is necessary to estimate the depth information of the monocular visual inertial odometry feature points and assign weights to the feature points using this depth information. Therefore, in step S4, the step of obtaining the depth of each pair of matched feature points in the two input images based on camera pose changes includes:

[0100] Assume that a pair of matching feature points in two adjacent input frames are divided into , ;

[0101] Obtain feature points , Normalized coordinates on the normalized plane of a monocular camera , Specifically, according to the definition in polar geometry, let... , Let the normalized coordinates of two feature points be denoted as , then they satisfy:

[0102]

[0103] To calculate the depth of each pair of matched feature points and Multiply both sides of the above equation by one. have to:

[0104]

[0105] Feature points can be obtained from the above. , The relationship between the depth and the second rotation matrix R and the second translation vector t;

[0106] Furthermore, using the first rotation matrix and the first translation vector representing the pose change of the first camera, as well as the normalized coordinates of the feature points... , Obtain feature points , In this embodiment, as mentioned earlier, depth cannot be directly calculated in monocular vision. and However, the presence of an IMU sensor in a visual inertial odometry system allows for a coarse prediction of the camera's second rotation matrix R and second translation vector t between two input image frames. Therefore, based on this, the first rotation matrix can be coarsely predicted using inertial measurement data. and the first translation vector A pair of matching feature points is roughly obtained. , depth and .

[0107] Therefore, it can be expressed as:

[0108]

[0109]

[0110] in, Representing feature points depth, Representing feature points depth, Denotes the first rotation matrix. Let R represent the first translation vector, R represent the second rotation matrix used to represent the actual camera pose change, and t represent the second translation vector used to represent the actual camera pose change.

[0111] According to one embodiment of the present invention, the essential matrix E is typically solved using the classic "eight-point algorithm". The eight-point algorithm utilizes only the linear property of the essential matrix E, and therefore can be solved within a linear algebra framework. When there are exactly 8 pairs of matched feature points, the essential matrix E can be solved, and there is no issue of weighted optimization. Therefore, in step S5, if the number of matched feature points in the two input frames satisfies 8 pairs, weights are constructed for each pair of matched feature points, and the corresponding weighting matrix is ​​obtained.

[0112] In this embodiment, feature points with different depth information should play different weights in the visual pose estimation process. Feature points with smaller depth (i.e., closer) should have larger weights, while feature points with larger depth (i.e., farther) should have smaller weights. Therefore, a reciprocal model is used to weight the feature points. Thus, for matched feature points, their weights are expressed as follows:

[0113]

[0114] in, Indicates the i-th pair of matching feature points , The depth in the two adjacent frames, Indicates the i-th pair of matching feature points , The weights in the two adjacent image frames;

[0115] Furthermore, the weighted matrix is ​​represented as:

[0116]

[0117] Where W represents the weighting matrix.

[0118] According to one embodiment of the present invention, in step S6, a weighted observation equation set is constructed based on the aforementioned epipolar geometric constraint equation and weighting matrix; in this embodiment, the steps of constructing the weighted observation equation set are described in detail, taking the case of 8 pairs of matching feature points as an example.

[0119] Consider a pair of matched feature points , Their normalized coordinates are , According to the epipolar constraint, we have:

[0120]

[0121] Expand the matrix E in the middle of the left side of equation (1) and write it in vector form:

[0122]

[0123] Therefore, the epipolar constraint can be written in a linear form related to e:

[0124]

[0125] Similarly, the same representation applies to other matched feature point pairs. Putting all the points into a single equation transforms it into a system of linear equations (where, , (This represents the i-th feature point, and so on):

[0126]

[0127] Therefore, the above equation can be written in matrix form:

[0128] Ae = 0

[0129] When there are 8 pairs of matching feature points, the above matrix form equation is positive definite, and the unique essential matrix E can be obtained by solving it.

[0130] When there are more than 8 pairs of matching feature points, the above matrix equation becomes an overdetermined equation, meaning that there may not be an e such that the above matrix equation holds. In this case, for each pair of matching feature points, the weights derived above can be used for weighting. That is, equation (1) can be rewritten as:

[0131]

[0132] Furthermore, let the number of matched feature points be n pairs. Similarly, by combining all the matched feature points, we can obtain the weighted observation equation system:

[0133] WAe=0

[0134] In the formula:

[0135]

[0136]

[0137] Where e is the vector form of the essential matrix of the matched feature points, For the i-th pair of matching feature points , The pixel coordinates in the input image between two adjacent frames.

[0138] According to one embodiment of the present invention, in step S7, the quadratic form is constructed based on the weighted observation equation system, and the quadratic form is solved to obtain the essential matrix of the matched feature points in the least squares sense. In this step, the quadratic form is expressed as:

[0139]

[0140] In this embodiment, the quadratic form can be solved by linear expansion or nonlinear direct optimization to obtain the essential matrix E of the matched feature points in the least squares sense, which will not be elaborated here.

[0141] According to one embodiment of the present invention, after obtaining the essential matrix E, the second rotation matrix R and the second translation vector t of the actual camera pose change of the monocular camera are then recovered based on the essential matrix E. Specifically, in step S8, the steps of using the essential matrix and calculating the second rotation matrix and the second translation vector of the camera between two adjacent frames using singular value decomposition, and obtaining the representation of the actual camera pose change based on the second rotation matrix and the second translation vector, include:

[0142] Using the essential matrix, and through Singular Value Decomposition (SVD), multiple sets of second rotation matrices and second translation vectors for the camera between two adjacent frames are calculated; where, it is assumed that the singular value decomposition of the essential matrix E is:

[0143]

[0144] Where E represents the essential matrix, U and V are orthogonal matrices, and ∑ is the singular value matrix;

[0145] In singular value decomposition, for any essential matrix E, there exist two possible second translation vectors t and second rotation matrices R corresponding to it:

[0146]

[0147] in, This indicates that the rotation matrix is ​​obtained by rotating 90° along the Z-axis of the camera coordinate system.

[0148] Furthermore, since the essential matrix -E and the essential matrix E are equivalent, taking the negative sign of any second translation vector t will yield the same result. Therefore, when decomposing the essential matrix E into the second translation vector t and the second rotation matrix R, there are a total of 4 possible solutions.

[0149] like Figure 3 As shown, this diagram illustrates the four solutions obtained by decomposing the essential matrix E. Given the projection (star-shaped point) of an external space point P onto the monocular camera (solid line), the goal is to solve for the camera's motion. Keeping the star-shaped projection point constant, four possible solutions can be drawn. It is evident that only the first solution shows the external space point P having a positive depth in both monocular camera positions (i.e., consistent with the real physical world). Therefore, by substituting any point into the four solutions and checking its depth in the two monocular camera positions, the correct solution (i.e., ...) can be determined. Figure 3 The first set of solutions in the equation is the second rotation matrix R and the second translation vector t, which are ultimately used to represent the actual camera pose changes.

[0150] It is important to note that Figure 2 and Figure 3 In and These represent the camera optical centers in the first and second input images, respectively. , This represents two image planes representing two adjacent input frames; , This represents a pair of matched feature points; , express Connection to the image plane , The intersection of these points is also known as the "epipole".

[0151] The above description is merely an example of a specific solution of the present invention. For any devices and structures not described in detail herein, it should be understood that they are implemented using common devices and methods already available in the art.

[0152] The above description is merely one embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the invention by those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A monocular vision-based inertial odometry method based on feature point depth, comprising: S1. Acquire two adjacent frames of input images based on a monocular camera, and extract feature points from the two frames of input images respectively; S2. Match the feature points extracted from the two frames of the input images; S3. Using the acceleration measurement data and angular rate measurement data output by the inertial measurement sensor, predict the first camera pose change corresponding to the two frames of the input image; wherein, the first camera pose change is represented by the corresponding first rotation matrix and first translation vector; S4. Based on the pose change of the first camera, obtain the depth of each pair of matched feature points in the two frames of the input image; wherein, including: Assume that a pair of matching feature points in two adjacent frames of the input image are respectively , ; Obtain the feature points , Normalized coordinates on the normalized plane of the monocular camera , ; Using the first rotation matrix and the first translation vector representing the pose change of the first camera, and the normalized coordinates of the feature points... , Obtain feature points , The depth is represented as: in, Representing feature points depth, Representing feature points depth, Denotes the first rotation matrix. Denotes the first translation vector. This represents the second rotation matrix used to represent the actual camera pose change, with the first rotation matrix as an example. Instead of solving, The second translation vector is used to represent the actual camera pose change, with the first translation vector as the basis. Alternative solution; S5. If the number of matching feature points in the two frames of the input image meets the preset condition, then construct weights for each pair of matching feature points and obtain the corresponding weighting matrix. S6. Based on the epipolar geometric constraint equations and the weighting matrix, construct a set of weighted observation equations; S7. Construct a quadratic form based on the weighted observation equations and solve the quadratic form to obtain the essential matrix of the matched feature points in the least squares sense; S8. Using the essential matrix and through singular value decomposition, the second rotation matrix and the second translation vector of the camera between two adjacent frames are calculated, and the representation of the actual camera pose change is obtained based on the second rotation matrix and the second translation vector.

2. The monocular vision inertial odometry method according to claim 1, characterized in that, In step S5, the preset condition is that the number of matching feature points in the two frames of the input image is greater than 8 pairs.

3. The monocular vision inertial odometry method according to claim 2, characterized in that, In step S5, where weights are constructed for each pair of matched feature points and the corresponding weighting matrix is ​​obtained, the weights are as follows: in, , Indicates the first For the matched feature points , The depth in the two adjacent frames, , Indicates the first For the matched feature points , The weights in the two adjacent image frames; The weighting matrix is ​​represented as follows: in, This represents a weighted matrix.

4. The monocular vision inertial odometry method according to claim 3, characterized in that, In step S6, the step of constructing a weighted observation equation set based on the epipolar geometric constraint equations and the weighting matrix, the weighted observation equation set is expressed as follows: Where e is the vector form of the essential matrix of the matched feature points, , For the first Matching feature points , The pixel coordinates in the input image between two adjacent frames.

5. The monocular vision inertial odometry method according to claim 4, characterized in that, In step S7, the quadratic form is constructed based on the weighted observation equations, and the quadratic form is solved to obtain the essential matrix of the matched feature points in the least squares sense. In this step, the quadratic form is expressed as: 。 6. The monocular vision inertial odometry method according to claim 5, characterized in that, Step S8, which involves using the essential matrix and calculating the second rotation matrix and second translation vector of the camera between two adjacent frames using singular value decomposition, and obtaining a representation of the actual camera pose change based on the second rotation matrix and second translation vector, includes: Using the essential matrix, and through singular value decomposition, multiple sets of the second rotation matrix and the second translation vector of the camera between two adjacent frames are calculated; Based on the depth determination of at least one pair of matched feature points, it is determined whether each group of the second rotation matrix and the second translation vector is reasonable. If reasonable, they are retained as a representation of the actual camera pose change.

7. The monocular vision inertial odometry method according to claim 6, characterized in that, The step of using the essential matrix and calculating multiple sets of the second rotation matrix and the second translation vector of the camera between two adjacent frames through singular value decomposition includes: The essential matrix is ​​transformed using the singular value decomposition algorithm, and the essential matrix is ​​represented as follows: Where E represents the essential matrix, and U and V are orthogonal matrices. It is a singular value matrix; Singular value decomposition of the essential matrix yields multiple sets of the second rotation matrix and the second translation vector, which are expressed as follows: in, This indicates that the rotation matrix is ​​obtained by rotating 90° along the Z-axis of the camera coordinate system.

8. The monocular vision inertial odometry method according to claim 7, characterized in that, In step S2, the feature points in the two extracted input images are matched using a brute-force matching algorithm or a fast approximate nearest neighbor algorithm.

9. The monocular vision inertial odometry method according to claim 8, characterized in that, In step S1, the feature points are extracted by using a corner point extraction algorithm or a local image feature extraction algorithm to obtain two adjacent frames of input images based on a monocular camera.

Images