CN107798705B - 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

Attitude angle estimation method based on feature point set grouping

Technical Field

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

Background

With the development of signal processing theory and computer technology, it is the goal of researchers to make computers, robots or other intelligent machines acquire and process visual information like humans, and they attempt to acquire images of a three-dimensional scene using a camera, and through the processing of one or more images, make the computer recognize the surrounding scene information. For two images of a known set of matching points, the epipolar geometry constraint is the only information about the camera that can be obtained from the set of matching points. The epipolar geometry can be represented by a 3-order matrix with rank 2, i.e. a fundamental matrix, which includes information about internal and external parameters of the two image cameras. Thus, the epipolar geometry problem translates into an estimation problem for the fundamental matrix F.

The existing methods for estimating the fundamental matrix mainly include: linear methods, iterative methods and robust methods. The linear method has the advantages of small calculation amount and easy realization, but the matched corresponding points have mismatching or the points are not well positioned due to noise, the calculation of the linear method is unstable, and the obtained result precision is not ideal. Although the iterative method is more accurate than the linear method, it is really very time consuming and cannot handle abnormal data. Although the robust method can correctly simulate the epipolar set and can obtain a basic matrix with a better result, when the data contains error matching, the result is not ideal enough, and the algorithm is too dependent on the initial value obtained by the least square method.

Disclosure of Invention

Aiming at the defect that the estimation of a basic matrix by a mismatching point pair in a linear method is unstable, the invention provides an attitude angle estimation method based on characteristic point set grouping on the basis of the existing improved eight-point algorithm to overcome the defects of the existing method.

The technical scheme adopted by the invention for solving the technical problem is as follows: an attitude angle estimation method based on feature point set grouping comprises the following steps:

step (1), detecting and describing two image feature points by using SURF algorithm to obtain a feature point matching set S to be input_allTo S_allSorting the ratio of the variation of the horizontal axis and the vertical axis to obtain a preprocessed matching point set

Step (2), the preprocessed matching point set S_r(length N, usually greater than 150) are sorted on the horizontal axis into groups, each group being divisible into K (8. ltoreq. K.ltoreq.15) elements and thus into num [ N/K ]]Group (d);

step (3) translating and scaling the num group of point sets, and estimating a basic moment F by using a normalization eight-point algorithm_i(i＝1,2,3,…,num)；

Step (4), according to the corresponding basic matrix F of each group of samples_i(i＝1,2,3,…,num)Calculating the average value of the sum of the distances from all the matching points to the epipolar lines, finding the data position dex with the minimum average value, and taking the data at the position as a matching point data group for calculating a basic matrix

Obtaining the optimal basic matrix F_z；

Step (5) for the optimal basic matrix F_zSVD singular value decomposition is carried out to obtain two possible rotation matrixes R₁Or R₂(ii) a Deriving a corresponding rotation matrix R by rotating the three-dimensional space according to the sequence of the roll angle, the yaw angle and the pitch angle_zyx(ii) a By R_zyxThe first component is positive and the diagonal component is close to 1 to determine the matrix R₁Or R₂Obtaining a final matrix R; and obtaining the attitude angle of the motion estimation through the component of R.

Further, in the step (4), the sum of distances from all matching points to epipolar lines 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 of the num group dataset_i(i ═ 1,2,3, …, num), then the epipolar lines for each set of two images are:

where M1 and M2 are each datasets of matching points in the num group,

is F_iThe transposed matrix of (2);

using elementary matrices F for each group_i(i ═ 1,2,3, …, num) the sum of all matching point to epipolar distances is found:

where a (j,1), b (j,1), and c (j,1) are data of each row K of the data epipolar line I1 of each group, respectively, where j is 1,2,3 … K; equation (2) is calculationThe sum of the distances from the matching points to the epipolar lines in the picture 1 is calculated to be Davg2_iThe final distance average is Davg_i＝mean(Davg1_i+Davg2_i) Wherein i ═ 1,2,3, …, num; selecting the smallest mean value in the num group of candidate data as the optimal basic matrix F_z：Davg＝min(Davg₁,Davg₂,Davg₃,…,Davg_num) Wherein z is the position with the minimum mean value, and z is more than or equal to 1 and less than or equal to num.

Further, the step (5) comprises the following specific steps:

(5.1) obtaining the optimal basic matrix F_zPerforming SVD singular value decomposition, then F_z＝Udiag(1,1,0)V^T；

(5.2) decomposition from the basic matrix F_zWhen SR is equal to

Udiag(1,1,0)V^T＝F_z＝SR＝(UZU^T)(UXV^T)＝U(ZX)V^T，

ZX ═ diag (1,1,0) can be deduced, since X is a rotation matrix, X ═ W or X ═ W can be deduced again^TWherein W is an orthogonal matrix and Z is an antisymmetric matrix;

(5.3) two possible solutions of the rotation matrix can be derived from the above calculations: r₁＝UWV^TOr R₂＝UW^TV^TDetermining a rotation matrix R, wherein

(5.4) for arbitrary rotation in three-dimensional space, in the order of roll-yaw-pitch, the corresponding rotation matrix can be expressed as:

wherein:

R_y(θ)，R_z(phi) represents a rotation matrix around the x-axis, y-axis, and z-axis, respectively,

(5.5) by mutual reasoning in steps (5.3) and (5.4), the attitude angle information can be solved:

wherein:

represents the pitch angle, theta represents the yaw angle, and phi represents the roll angle.

The invention has the following beneficial effects: the invention reduces the influence of mismatching data on the algorithm and realizes the calculation of the attitude angle information of motion estimation through the fundamental matrix by preprocessing the characteristic points and re-presenting the characteristic points in a grouping form based on an improved eight-point method. The whole method is simple in flow and easy to implement, the requirement of an experimental tool is low, and the use result shows that the method can improve the calculation efficiency on the premise of ensuring the calculation accuracy, and particularly can embody the value of the method when the number of the characteristic points is large.

Drawings

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

Detailed Description

Referring to fig. 1, the present invention provides an attitude angle estimation method based on feature point set grouping, which includes firstly using SURF algorithm to detect and describe image feature points, and obtaining a feature point set to be input; the preliminary set is preprocessed and grouped and selected to form an initial characteristic point set, so that the influence of mismatching characteristic points on an eight-point method is solved to a great extent, and a good initial data set is obtained. Then, an improved eight-point algorithm is used for calculating the initial value of each group of sample basic matrix between the two images, and the distance sum of all the matching points to the epipolar line is calculated by each initial valueSorting the average values of the polar line distance sums of all the obtained groups from small to large, and selecting the data subset group corresponding to the minimum average value as the initial value of the accurate basic matrix of the algorithm; by carrying out SVD singular value decomposition on the matrix, two possible rotation matrixes R are calculated₁Or R₂Selecting a final matrix R mainly by comparing the signs of the two matrices; for the sequence of rotation in three-dimensional space, the invention selects the sequence of rotation according to the roll angle, the yaw angle and the pitch angle to obtain a rotation matrix R_zyxPerforming position calculation on the rotation matrix and the matrix R to obtain attitude angle information of motion estimation; the method comprises the following specific steps:

step (1), matching point set S of two initial images_allSorting the ratio of the horizontal and vertical axis variation, removing the point with the difference value larger than 5 in the front and rear variation, and obtaining the preprocessed matching point set

The specific implementation steps are as follows:

(1.1) respectively extracting and matching characteristic points of the two images through a SURF algorithm to obtain a matching point set S_all；

(1.2) to S_allCalculating the ratio of the horizontal axis variation and the vertical axis variation, sorting based on the ratio data, removing the data points with the difference value larger than 5 in the sorted data column in order to remove the mismatching data, and obtaining the preprocessed matching point set

Step (2), point set S_r(length N, typically greater than 150) is ordered on the horizontal axis, typically with a lower threshold K, each group having at least K elements, and thus divided into about num ═ N/K]And a large amount of experimental data show that when K is more than or equal to 8 and less than or equal to 15, the data precision of the algorithm is stable, the overall time efficiency is improved, and K is 8 in the implementation.

And (3) translating and scaling the num group of point sets, and estimating a basic matrix F by using a normalized eight-point algorithm_i(i＝1,2,3,…,num)；

Step (4), according to the corresponding basic matrix F of each group of samples_i(i ═ 1,2,3, …, num) calculating the average value of the sum of all matching points to epipolar line distances, finding the data position dex with the minimum average value, and using the data at this position as the matching point data set for calculating the basic matrix

Obtaining the optimal basic matrix F_z(ii) a The specific implementation steps are as follows:

(4.1) by calculating the fundamental matrix F for num groups of datasets_i(i ═ 1,2,3, …, num), then the epipolar lines for each set of two images are:

where M1 and M2 are each datasets of matching points in the num group,

is F_iThe transposed matrix of (2);

(4.2) Using the fundamental matrix F of each group_i(i ═ 1,2,3, …, num) the sum of all matching point to epipolar distances is found:

where a (j,1), b (j,1) and c (j,1) are respectively data of each row K of the data epipolar line I1 of each group, where j is 1,2,3 … 8; equation (2) is to calculate the sum of the distances from the matching points to the epipolar line in the picture 1, and to understand that the sum of the distances from all the matching points to the epipolar line in the picture 2 is Davg2_iThe final distance average is Davg_i＝mean(Davg1_i+Davg2_i) Wherein i ═ 1,2,3, …, num; selecting the smallest mean value in the num group of candidate data as the optimal basic matrix F_z：Davg＝min(Davg₁,Davg₂,Davg₃,…,Davg_num) Wherein z is the position with the minimum mean value, and z is more than or equal to 1 and less than or equal to num.

Step (5) for the optimal basic matrix F_zSVD singular value decomposition is carried out to obtain two possible rotation matrixes R₁Or R₂(ii) a Deriving a corresponding rotation matrix R by rotating the three-dimensional space according to the sequence of the roll angle, the yaw angle and the pitch angle_zyx(ii) a By R_zyxThe first component is positive and the diagonal component is close to 1 to determine the matrix R₁Or R₂Obtaining a final matrix R; and obtaining the attitude angle of the motion estimation through the component of R. The specific implementation steps are as follows:

(5.1) obtaining the optimal basic matrix F_zPerforming SVD singular value decomposition, then F_z＝Udiag(1,1,0)V^T；

(5.2) decomposition from the basic matrix F_zWhen SR is equal to

Udiag(1,1,0)V^T＝F_z＝SR＝(UZU^T)(UXV^T)＝U(ZX)V^T，

ZX ═ diag (1,1,0) can be deduced, since X is a rotation matrix, X ═ W or X ═ W can be deduced again^T(ii) a Where W is an orthogonal matrix and Z is an antisymmetric matrix, which are generally defined as the matrix form:

(5.3) two possible solutions of the rotation matrix can be derived from the above calculations: r₁＝UWV^TOr R₂＝UW^TV^TDetermining a rotation matrix R, wherein

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

R_y(θ)，R_z(φ) represents a rotation matrix around the x-axis, y-axis, and z-axis, respectively.

(5.4) for any rotation in three-dimensional space, the invention rotates according to the sequence of roll angle-yaw angle-pitch angle, and the corresponding rotation matrix can be expressed as:

(5.5) by mutual reasoning in the steps (5.3) and (5.4), the attitude angle information, namely the pitch angle respectively, can be calculated

Yaw angle θ, roll angle Φ:

the invention reduces the influence of mismatching data on the algorithm and realizes the calculation of the attitude angle information of motion estimation through the fundamental matrix by preprocessing the characteristic points and re-presenting the characteristic points in a grouping form based on an improved eight-point method. The whole algorithm flow is simple and easy to realize, the requirement of an experimental tool is low, and the use result shows that the method can improve the calculation efficiency on the premise of ensuring the calculation precision, and particularly can embody the value of the method when the number of the characteristic points is large.

The embodiments described in this specification are merely illustrative of implementations of the inventive concept and the scope of the present invention should not be considered limited to the specific forms set forth in the embodiments but rather by the equivalents thereof as may occur to those skilled in the art upon consideration of the present inventive concept.

Claims (3)

1. An attitude angle estimation method based on feature point set grouping is characterized by comprising the following steps:

step (1), detecting and describing two image feature points by using SURF algorithm to obtain a feature point matching set S to be input_allTo S_allSorting the ratio of the variation of the horizontal axis and the vertical axis to obtain a preprocessed matching point set

Step (2), the preprocessed matching point set S_rThe sorting and grouping are performed based on the horizontal axis, each group can be divided into K elements, so that the division is carried out into num ═ N/K]Group, in which N is S_rLength of (d);

step (3) translating and scaling the num group of point sets, and calculating a basic moment F by using an improved eight-point algorithm_i(i＝1,2,3,…,num)；

Step (4), according to the corresponding basic matrix F of each group of samples_i(i ═ 1,2,3, …, num) calculating the average value of the sum of all matching points to epipolar line distances, finding the data position dex with the minimum average value, and using the data at this position as the matching point data set for calculating the basic matrix

Obtaining the optimal basic matrix F_z；

Step (5) for the optimal basic matrix F_zSVD singular value decomposition is carried out to obtain two possible rotation matrixes R₁Or R₂(ii) a Deriving a corresponding rotation matrix R by rotating the three-dimensional space according to the sequence of the roll angle, the yaw angle and the pitch angle_zyx(ii) a By R_zyxThe first component is positive and the diagonal component is close to 1 to determine the matrix R₁Or R₂Obtaining a final matrix R; and obtaining the attitude angle of the motion estimation through the component of R.

2. The method for estimating the attitude angle based on the feature point set grouping according to claim 1, wherein in the step (4), the sum of the distances from all the matching points to the epipolar line is calculated according to the fundamental matrix corresponding to each group of samples, and the specific steps are as follows:

by calculating the fundamental matrix F of the num group dataset_i(i ═ 1,2,3, …, num), then the epipolar lines for each set of two images are:

where M1 and M2 are datasets of matching points in num groups, respectively, F_i ^TIs F_iThe transposed matrix of (2);

using elementary matrices F for each group_i(i ═ 1,2,3, …, num) the sum of all matching point to epipolar distances is found:

where a (j,1), b (j,1), and c (j,1) are data for each set of jth row of epipolar line I1, respectively, where j is 1,2,3 … K; equation (2) is to calculate the sum of the distances from the matching points to the epipolar line in the picture 1, and to understand that the sum of the distances from all the matching points to the epipolar line in the picture 2 is Davg2_iThe final distance average is Davg_i＝mean(Davg1_i+Davg2_i) Wherein i ═ 1,2,3, …, num; selecting the smallest mean value in the num group of candidate data as the optimal basic matrix F_z：Davg＝min(Davg₁,Davg₂,Davg₃,…,Davg_num) Wherein z is the position with the minimum mean value, and z is more than or equal to 1 and less than or equal to num.

3. The method for estimating the attitude angle based on the feature point set grouping according to claim 2, wherein the step (5) comprises the following specific steps:

(5.1) obtaining the optimal basic matrix F_zPerforming SVD singular value decomposition, then F_z＝Udiag(1,1,0)V^T；

(5.2) decomposition of the basic matrix F_zSR, available as

Udiag(1,1,0)V^T＝F_z＝SR＝(UZU^T)(UXV^T)＝U(ZX)V^T，

ZX ═ diag (1,1,0) can be deduced, since X is a rotation matrix, X ═ W or X ═ W can be deduced again^TWherein W is an orthogonal matrix and Z is an antisymmetric matrix;

(5.3) two possible solutions of the rotation matrix can be derived from the above calculations: r₁＝UWV^TOr R₂＝UW^TV^TDetermining a rotation matrix R, wherein

(5.4) for arbitrary rotation in three-dimensional space, in the order of roll-yaw-pitch, the corresponding rotation matrix can be expressed as:

wherein:

R_y(θ)，R_z(phi) represents a rotation matrix around the x-axis, y-axis, and z-axis, respectively,

(5.5) by mutual reasoning in steps (5.3) and (5.4), the attitude angle information can be solved:

wherein:

represents the pitch angle, theta represents the yaw angle, and phi represents the roll angle.

Images