CN107798705B - An Attitude Angle Estimation Method Based on Feature Point Set Grouping - Google Patents

Abstract

The invention discloses an attitude angle estimation method based on feature point set grouping, which comprises the following steps: 1) detecting and describing image feature points by an SURF algorithm to obtain a feature point set to be input; 2) preprocessing an input feature set and grouping points; 3) respectively calculating a basic matrix for the grouped feature point sets, calculating the mean value of the distance sum from each group of matching points to the epipolar line, and selecting a data subset group corresponding to the minimum mean value as an optimal basic matrix; 4) and carrying out SVD singular value decomposition on the optimal matrix, calculating a rotation matrix, and carrying out position calculation on the rotation matrix and the rotation matrix in the sequence of rolling angle, yaw angle and pitch angle to obtain attitude angle information. The invention improves the calculation efficiency on the premise of ensuring the calculation precision.

Description

An Attitude Angle Estimation Method Based on Feature Point Set Grouping

technical field

The invention relates to the fields of robot navigation and positioning, in particular to an attitude angle estimation method based on feature point set grouping.

Background technique

With the development of signal processing theory and computer technology, it has become the goal of researchers to make computers, robots or other intelligent machines acquire and process visual information like human beings. They try to use cameras to collect images of three-dimensional scenes. Image processing enables computers to recognize surrounding scene information. For two images with a known matching point set, the epipolar geometric constraint is the only information about the camera that can be obtained from the matching point set. The epipolar geometric relationship can be represented by a third-order rank 2 matrix, that is, the fundamental matrix, which includes the relevant information of the internal and external parameters of the two images of the camera. Therefore, the problem of epipolar geometry is transformed into the problem of estimating the fundamental matrix F.

The existing methods for estimating the fundamental matrix mainly include: linear method, iterative method and robust method. The advantage of the linear method is that the amount of calculation is small and it is easy to implement, but there are mismatches in the matching corresponding points or the positioning of the points is not good due to noise. The calculation of the linear method is unstable, and the accuracy of the obtained results is not ideal. Iterative methods, although more accurate than linear methods, are indeed very time-consuming and cannot handle abnormal data. Although the robust method can correctly simulate the epipolar set and obtain the basic matrix with better results, when the data contains incorrect matching, the results are not ideal. The algorithm relies too much on the initial value obtained by the least squares method. .

SUMMARY OF THE INVENTION

Aiming at the defect of unstable estimation of the fundamental matrix by the mismatched points in the linear method, the present invention overcomes the above shortcomings of the existing method, and proposes an attitude angle based on the feature point set grouping on the basis of the existing improved eight-point algorithm. estimation method.

The technical solution adopted by the present invention to solve the technical problem is as follows: an attitude angle estimation method based on feature point set grouping, comprising the following steps:

Step (1), use the SURF algorithm to detect and describe the feature points of two images, obtain the feature point matching set S _all to be input, and sort S _all by the ratio of the horizontal and vertical axis changes to obtain a preprocessed matching point set

Step (2): Sort and group the preprocessed matching point set S _r (length is N, usually greater than 150) based on the horizontal axis, and each group can be divided into K (8≤K≤15) elements, Thus divided into num=[N/K] groups;

Step (3), translate and scale the num group of points, and use the normalized eight-point algorithm to estimate the fundamental moment F _i (i=1, 2, 3, . . . , num);

得到最优的基本矩阵F_z；Step (4): Calculate the mean value of the sum of the distances from all matching points to the epipolar line according to the basic matrix F _i (i=1, 2, 3, ..., num) corresponding to each group of samples, and find the data position dex with the smallest mean value, Use the data at this location as the matching point data set for computing the fundamental matrix

Obtain the optimal fundamental matrix F _z ;

Step (5), perform SVD singular value decomposition on the optimal fundamental matrix F _z to obtain two possible rotation matrices R ₁ or R ₂ ; by rotating the three-dimensional space in the order of roll angle-yaw angle-pitch angle, Derive the corresponding rotation matrix R _zyx ; judge the matrix R ₁ or R ₂ by the sign of the first component of R _zyx being positive and the component on the diagonal is close to 1, and obtain the final matrix R; obtain the motion estimation through the components of R attitude angle.

Further, in step (4), the sum of the distances from all matching points to the epipolar line is calculated according to the basic matrix corresponding to each group of samples, and the specific steps are as follows:

By calculating the fundamental matrix F _i (i=1,2,3,...,num) of num groups of data sets, the epipolar lines of each group of two images are:

为F_i的转置矩阵；where M1 and M2 are the datasets of matching points in the num group, respectively,

is the _transposed matrix of Fi;

Use the fundamental matrix F _i (i=1,2,3,...,num) of each group to find the sum of the distances from all matching points to the epipolar line:

where a(j,1), b(j,1) and c(j,1) are the data of each row K of each group of data to the polar line I1, where j=1,2,3...K; formula ( 2) is to calculate the sum of the distances from the matching points in the picture 1 to the epipolar line. It is understood that the sum of the distances from all the matching points in the picture 2 to the epipolar line is Davg2 _i , then the final mean distance is Davg _i =mean( _{Davg1 i} +Davg2 _i ), where i= ₁ , ₂ , ₃ , . ..., Davg _num ), where z is the position where the mean is the smallest, 1≤z≤num.

Further, the concrete steps of described step (5) are as follows:

(5.1) Perform SVD singular value decomposition on the obtained optimal fundamental matrix F _z , then F _z =Udiag(1,1,0)V ^T ;

(5.2) From the decomposition of the fundamental matrix F _z =SR, then

Udiag(1,1,0)V ^T =F _z =SR=(UZU ^T )(UXV ^T )=U(ZX)V ^T ,

It can be deduced that ZX=diag(1,1,0), because X is a rotation matrix, so X=W or X=W ^T , where W is an orthogonal matrix, and Z is an antisymmetric matrix;

(5.3) Two possible solutions of the rotation matrix can be obtained from the above calculation: R ₁ =UWV ^T or R ₂ =UW ^T V ^T , determine a rotation matrix R, where

(5.4) For any rotation in three-dimensional space, rotate in the order of roll angle - yaw angle - pitch angle, then the corresponding rotation matrix can be expressed as:

R_y(θ)，R_z(φ)分别表示绕x轴、y轴、z轴的旋转矩阵，in:

R _y (θ), R _z (φ) represent the rotation matrices around the x-axis, y-axis, and z-axis, respectively,

(5.5) Through the mutual reasoning of steps (5.3) and (5.4), the attitude angle information can be calculated:

表示俯仰角，θ表示偏航角，φ表示翻滚角。in:

represents the pitch angle, θ represents the yaw angle, and φ represents the roll angle.

The beneficial effects of the present invention are as follows: the present invention re-presents the feature points in the form of feature point grouping based on the improved eight-point method through the preprocessing of the feature points, reduces the influence of the mismatched data on the algorithm, and realizes the motion estimation through the basic matrix solution. attitude angle information. The process of the whole method is simple and easy to implement, and the requirements for experimental tools are also very low. The use results show that the invention can improve the calculation efficiency on the premise of ensuring the calculation accuracy, especially when the number of feature points is large, it can better reflect the value of the method.

Description of drawings

FIG. 1 is a flowchart of an attitude angle estimation method based on feature point set grouping according to the present invention.

Detailed ways

Referring to FIG. 1, the present invention provides an attitude angle estimation method based on feature point set grouping. First, the SURF algorithm is used to detect and describe image feature points, and the feature point set to be input is obtained; the preprocessing and grouping selection are performed on the preliminary set The initial feature point set is formed, which largely solves the influence of mismatched feature points on the eight-point method, and obtains a good initial data set. Then use the improved eight-point algorithm to calculate the initial value of the basic matrix of each group of samples between the two images, and calculate the mean value of the sum of the distances from all matching points to the epipolar line from each initial value. The mean is sorted from small to large, and the data subset group corresponding to the smallest mean is selected as the initial value of the exact basic matrix of the algorithm; by SVD singular value decomposition of the matrix, two possible rotation matrices R ₁ or R ₂ are calculated. , the final matrix R is mainly selected by comparing the symbols of the two matrices; for the rotation order in the three-dimensional space, the present invention chooses to rotate in the order of roll angle-yaw angle-pitch angle to obtain the rotation matrix R _zyx , The rotation matrix and the matrix R are used for position calculation, so as to obtain the attitude angle information of motion estimation; the specific steps are as follows:

具体实施步骤如下所示：Step (1), sort the matching point set S _all of the initial two images by the ratio of the horizontal and vertical axis changes, remove the points whose difference is greater than 5 in the before and after changes, and obtain the matching point set after preprocessing.

The specific implementation steps are as follows:

(1.1) extraction and matching of feature points are respectively performed on two images by the SURF algorithm to obtain a matching point set S _all ;

(1.2) Calculate the ratio of the horizontal and vertical axis changes to S _all , and sort based on the ratio data. In order to remove the mismatched data, remove the data points with a difference greater than 5 in the sorted data column, and obtain the preprocessed matching point set

Step (2), sort the point set S _r (length is N, usually greater than 150) based on the horizontal axis, usually set a lower limit K, each group has at least K elements, so that it is divided into about num= Group [N/K], according to a large number of experimental data, when 8≤K≤15, the data accuracy of the algorithm is very stable, and the overall time efficiency is also improved. In this implementation, K is 8.

Step (3), translate and scale the num group of points, and use the normalized eight-point algorithm to estimate the fundamental matrix F _i (i=1, 2, 3, . . . , num);

得到最优的基本矩阵F_z；具体实施步骤如下所示：Step (4): Calculate the mean value of the sum of the distances from all matching points to the epipolar line according to the basic matrix F _i (i=1, 2, 3, ..., num) corresponding to each group of samples, and find the data position dex with the smallest mean value, Use the data at this location as the matching point data set for computing the fundamental matrix

Obtain the optimal fundamental matrix F _z ; the specific implementation steps are as follows:

(4.1) By calculating the fundamental matrix F _i (i=1, 2, 3, ..., num) of num groups of data sets, the epipolar lines of each group of two images are:

为F_i的转置矩阵；where M1 and M2 are the datasets of matching points in the num group, respectively,

is the _transposed matrix of Fi;

(4.2) Use the basic matrix F _i (i=1,2,3,...,num) of each group to find the sum of the distances from all matching points to the epipolar line:

where a(j,1), b(j,1) and c(j,1) are the data of each row K of each group of data to the polar line I1, where j=1,2,3...8; formula ( 2) is to calculate the sum of the distances from the matching points in the picture 1 to the epipolar line. It is understood that the sum of the distances from all the matching points in the picture 2 to the epipolar line is Davg2 _i , then the final mean distance is Davg _i =mean( _{Davg1 i} +Davg2 _i ), where i= ₁ , ₂ , ₃ , . ..., Davg _num ), where z is the position where the mean is the smallest, 1≤z≤num.

Step (5), perform SVD singular value decomposition on the optimal fundamental matrix F _z to obtain two possible rotation matrices R ₁ or R ₂ ; by rotating the three-dimensional space in the order of roll angle-yaw angle-pitch angle, Derive the corresponding rotation matrix R _zyx ; judge the matrix R ₁ or R ₂ by the sign of the first component of R _zyx being positive and the component on the diagonal is close to 1, and obtain the final matrix R; obtain the motion estimation through the components of R attitude angle. The specific implementation steps are as follows:

(5.1) Perform SVD singular value decomposition on the obtained optimal fundamental matrix F _z , then F _z =Udiag(1,1,0)V ^T ;

(5.2) From the decomposition of the fundamental matrix F _z =SR, then

Udiag(1,1,0)V ^T =F _z =SR=(UZU ^T )(UXV ^T )=U(ZX)V ^T ,

It can be deduced that ZX=diag(1,1,0), because X is a rotation matrix, X=W or X=W ^T is deduced again; wherein W is an orthogonal matrix, Z is an antisymmetric matrix, generally they are set as The following matrix form:

(5.3) Two possible solutions of the rotation matrix can be obtained from the above calculation: R ₁ =UWV ^T or R ₂ =UW ^T V ^T , determine a rotation matrix R, where

The attitude angle information of motion estimation is mainly obtained by mutual calculation between the rotation matrix decomposed by the basic matrix and the rotation matrix corresponding to the rotation in any direction in three-dimensional space. For the order of rotation in three-dimensional space, the present invention chooses to rotate in the order of roll angle-yaw angle-pitch angle, and the corresponding rotation matrix is the result of the order product of three rotation matrices, wherein

R_y(θ)，R_z(φ)分别表示绕x轴、y轴、z轴的旋转矩阵。

R _y (θ) and R _z (φ) represent the rotation matrices around the x-axis, y-axis, and z-axis, respectively.

(5.4) For any rotation in three-dimensional space, the present invention rotates in the order of roll angle-yaw angle-pitch angle, then the corresponding rotation matrix can be expressed as:

偏航角θ，翻滚角φ：(5.5) Through the mutual reasoning of steps (5.3) and (5.4), the attitude angle information can be calculated, which are the pitch angle respectively

Yaw angle θ, roll angle φ:

The present invention preprocesses the feature points and re-presents them in the form of feature point grouping based on the improved eight-point method, reduces the influence of mismatched data on the algorithm, and realizes the attitude angle information of motion estimation through the basic matrix solution. The process of the entire algorithm is simple and easy to implement, and the requirements for experimental tools are also very low. The use results show that the invention can improve the calculation efficiency under the premise of ensuring the calculation accuracy, especially when the number of feature points is large, it can better reflect the value of the method.

The content described in the embodiments of the present specification is only an enumeration of the realization forms of the inventive concept, and the protection scope of the present invention should not be regarded as limited to the specific forms stated in the embodiments, and the protection scope of the present invention also extends to those skilled in the art. Equivalent technical means that can be conceived by a person based on the inventive concept.

Claims (3)

1. an attitude angle estimation method based on feature point set grouping, is characterized in that, comprises the following steps: Step (1), use the SURF algorithm to detect and describe the feature points of two images, obtain the feature point matching set S _all to be input, and sort S _all by the ratio of the horizontal and vertical axis changes to obtain a preprocessed matching point set

Step (2), sort and group the preprocessed matching point set S _r based on the horizontal axis, and each group can be divided into K elements, thus divided into num=[N/K] groups, where N is S _r length; Step (3), translate and scale the num group of points, and use the improved eight-point algorithm to calculate the fundamental moment F _i (i=1, 2, 3, . . . , num); Step (4): Calculate the mean value of the sum of the distances from all matching points to the epipolar line according to the basic matrix F _i (i=1, 2, 3, ..., num) corresponding to each group of samples, and find the data position dex with the smallest mean value, Use the data at this location as the matching point data set for computing the fundamental matrix

Obtain the optimal fundamental matrix F _z ; Step (5), perform SVD singular value decomposition on the optimal fundamental matrix F _z to obtain two possible rotation matrices R ₁ or R ₂ ; by rotating the three-dimensional space in the order of roll angle-yaw angle-pitch angle, Derive the corresponding rotation matrix R _zyx ; judge the matrix R ₁ or R ₂ by the sign of the first component of R _zyx being positive and the component on the diagonal is close to 1, and obtain the final matrix R; obtain the motion estimation through the components of R attitude angle. 2. a kind of attitude angle estimation method based on feature point set grouping according to claim 1, is characterized in that, in step (4), calculates all matching points to epipolar distance according to the corresponding basic matrix of each group of samples The specific steps are as follows: By calculating the fundamental matrix F _i (i=1,2,3,...,num) of num groups of data sets, the epipolar lines of each group of two images are:

Among them, M1 and M2 are the data sets of matching points in the num group respectively, and F _i ^T is the transpose matrix of F _i ; Use the fundamental matrix F _i (i=1,2,3,...,num) of each group to find the sum of the distances from all matching points to the epipolar line:

where a(j,1), b(j,1) and c(j,1) are the data of each group of the jth row of the epipolar line I1, where j=1,2,3...K; formula (2) ) is to calculate the sum of the distances from the matching points in the picture 1 to the epipolar line, and it is understood that the sum of the distances from all the matching points in the picture 2 to the epipolar line is Davg2 _i , then the final mean distance is Davg _i =mean(Davg1 _i +Davg2 _i ), where i=1,2,3,...,num; select the smallest mean value among the num groups of candidate data as the optimal fundamental matrix F _z : Davg=min(Davg ₁ , Davg ₂ , Davg ₃ ,... , Davg _num ), where z is the position where the mean is the smallest, 1≤z≤num. 3. a kind of attitude angle estimation method based on feature point set grouping according to claim 2, is characterized in that, described step (5) concrete steps are as follows: (5.1) Perform SVD singular value decomposition on the obtained optimal fundamental matrix F _z , then F _z =Udiag(1,1,0)V ^T ; (5.2) From the decomposition of the fundamental matrix F _z =SR, we can get Udiag(1,1,0)V ^T =F _z =SR=(UZU ^T )(UXV ^T )=U(ZX)V ^T , It can be deduced that ZX=diag(1,1,0), because X is a rotation matrix, so X=W or X=W ^T , where W is an orthogonal matrix, and Z is an antisymmetric matrix; (5.3) Two possible solutions of the rotation matrix can be obtained from the above calculation: R ₁ =UWV ^T or R ₂ =UW ^T V ^T , determine a rotation matrix R, where

(5.4) For any rotation in three-dimensional space, rotate in the order of roll angle - yaw angle - pitch angle, then the corresponding rotation matrix can be expressed as:

in:

R _y (θ), R _z (φ) represent the rotation matrices around the x-axis, y-axis, and z-axis, respectively,

(5.5) Through the mutual reasoning of steps (5.3) and (5.4), the attitude angle information can be calculated:

in:

represents the pitch angle, θ represents the yaw angle, and φ represents the roll angle.

Images