CN115451996A - Homography vision mileometer method for indoor environment - Google Patents
Homography vision mileometer method for indoor environment Download PDFInfo
- Publication number
- CN115451996A CN115451996A CN202211044239.XA CN202211044239A CN115451996A CN 115451996 A CN115451996 A CN 115451996A CN 202211044239 A CN202211044239 A CN 202211044239A CN 115451996 A CN115451996 A CN 115451996A
- Authority
- CN
- China
- Prior art keywords
- map
- points
- point
- map point
- current frame
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 58
- 239000011159 matrix material Substances 0.000 claims abstract description 48
- 238000013519 translation Methods 0.000 claims abstract description 21
- 230000000007 visual effect Effects 0.000 claims abstract description 21
- 230000009466 transformation Effects 0.000 claims abstract description 17
- 238000005070 sampling Methods 0.000 claims abstract description 7
- 230000008569 process Effects 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 5
- YREOLPGEVLLKMB-UHFFFAOYSA-N 3-methylpyridin-1-ium-2-amine bromide hydrate Chemical compound O.[Br-].Cc1ccc[nH+]c1N YREOLPGEVLLKMB-UHFFFAOYSA-N 0.000 claims description 3
- 241001394244 Planea Species 0.000 claims description 3
- 230000002093 peripheral effect Effects 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 238000006073 displacement reaction Methods 0.000 claims description 2
- 230000000717 retained effect Effects 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000007306 turnover Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
The invention discloses a homography visual odometer method facing indoor environment, which comprises the following steps: 1) Extracting ORB feature points of the current frame and performing feature matching with the reference frame; 2) Finding a coplanar map point combination by using a weighted random sampling strategy; 3) Calculating a homography transformation matrix of the positions of the current frame and the reference frame according to the plane with the most map points, and then obtaining a rotation position matrix and a translation position vector by resolving; 4) Optimizing a rotation pose matrix and a translation pose vector by minimizing a weighted re-projection error function; 5) And constructing a local map, and optimizing a rotation pose matrix, a translation pose vector and the local map by a local light beam adjustment method. The accuracy and robustness of the visual odometry method in the indoor environment are improved by utilizing the coplanar constraint relation of map points.
Description
Technical Field
The invention relates to the technical field of vision simultaneous positioning and drawing, in particular to a homography vision odometer method facing indoor environment.
Background
The vision simultaneous localization and mapping technology is widely applied to the fields of unmanned autonomous systems such as mobile robots, unmanned vehicles, intelligent unmanned planes and AR/VR systems. The visual odometer is a core technology for simultaneously positioning and mapping vision, and has the main function of finishing real-time self pose estimation in the working process of an unmanned autonomous system. The conventional visual odometry method mainly comprises a PnP algorithm based on 2D-3D matching and an ICP algorithm based on 3D-3D matching, wherein the PnP algorithm can be used in monocular, binocular and RGB-D cameras due to less required information, and the ICP algorithm needs to acquire a three-dimensional point cloud of a current frame and is usually used in the RGB-D cameras. The mainstream visual odometry methods take less account of the spatial constraint relationship between the raw data. The method can improve the precision of the existing visual odometry method by exploring the space geometric relation of the original data and using the space geometric relation as the prior constraint information of the system, thereby finally improving the robustness and efficiency of the visual simultaneous positioning and mapping technology in the work of the unmanned autonomous system.
In addition, the artificial indoor environment is a common working place of an unmanned autonomous system, and the indoor environment often contains a large amount of plane information, so that the accuracy and robustness of the indoor environment visual odometry method are improved by fully utilizing the coplanar constraint relation of the characteristic points.
In combination with the above discussion, the invention provides the homography vision odometry method for the indoor environment, and the method has a high practical application value.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a homography visual odometer method facing an indoor environment, and the accuracy of the visual odometer is improved by comprehensively considering the coplanarity constraint prior relation and the observed times of map points.
In order to realize the purpose, the technical scheme provided by the invention is as follows: a homography vision odometer method facing indoor environment comprises the following steps:
1) Reading data, including a reference frame, a current frame and a local map, extracting ORB feature points from the current frame, filtering the extracted feature points by a quadtree method, and finally completing feature matching of the current frame and the reference frame;
2) Finding a coplanar map point combination by using a weighted random sampling strategy;
3) Calculating the pose of the current frame and the reference frame according to the plane with the most map pointsInverse transformation matrix H m Then, obtaining a rotation pose matrix R and a translation pose vector t by solution;
4) Optimizing a rotation pose matrix R and a translation pose vector t by minimizing a weighted reprojection error function;
5) And constructing a local map, and optimizing the rotation pose matrix R, the translation pose vector t and the local map by a local beam adjustment method.
Further, in step 1), the current frame refers to a current image of a pose to be estimated read from an image sequence, and the reference frame refers to a previous frame of image read from the image sequence, and step 1) includes the following steps:
1.1 Adopting a scaling coefficient b to continuously down-sample an original image of a current frame to obtain a d-layer image pyramid, and then extracting a corresponding number of FAST angular points according to the resolution of each layer of image, wherein the total number of extracted feature points is N;
1.2 Filtering the feature points extracted in the step 1.1) by a quadtree method, and then calculating BRIEF descriptors of all the remaining feature points;
1.3 For each feature point in the current frame, searching the feature point with the minimum Hamming distance with its descriptor in the neighboring area of the same position in the reference frame as the best match, and searching the next best match if the Hamming distance is the second smallest, and if the Hamming distance of the best match is greater than T b Or the difference between the best matching distance and the next best matching distance is less than T d If not, the best match is retained as the matched pair of feature match, wherein T b 、T d Is a set threshold; all matching pairs are denoted as q cr ＝{(q c1 ,q r1 ),(q c2 ,q r2 ),...,(q ck ,q rk ),...,(q cn ,q rn ) Where k =1,2.., n, q ck 、q rk The matching pairs of the initialized map points represent the feature points of the current frame and the corresponding feature points of the reference frame as
Wherein i =1,2,. Cndot., m, q ci 、q ri The current frame feature points and the reference frame corresponding feature points of the initialized map points are represented, and the corresponding map points are
Wherein i =1,2,. Multidot.m, P i Representing a map point corresponding to a number of observations of the map point of
Wherein i =1,2,. Cndot., m, N i Representing the number of times the corresponding map point is observed, and the matching pair of uninitialized map points is
Wherein l =1,2, n-m, q c(m+l) 、q r(m+l) The current frame feature points and the reference frame corresponding feature points representing uninitialized map points.
Further, in step 2), searching a coplanar map point combination through a weighted random sampling strategy, comprising the following steps:
2.1 According to the observation times, giving the ith map point the following weight:
in the formula, W i Representing map points P i B is the scaling factor of the image pyramid, s u Is the u-th observed map point P i Number of pyramid layers where feature points are located, N i For the number of observations of the ith map point,
is an intermediate variable;
setting map point probability:
in the formula I i Is the probability of the ith map point, W is the intermediate variable;
2.2 ) randomly selecting 3 map points P according to the result of step 2.1) v1 、P v2 、P v3 If the three points are collinear, discarding and re-selecting, wherein the map point probability l i As map point P i Probability of being selected;
2.3 ) randomly selecting a map point P v4 If P is selected v1 、P v2 Or P v3 Then abandon and re-select, wherein the map point probability l i As map point P i Probability of being selected, then calculating P v4 To by P v1 、P v2 、P v3 Form a plane
Is a distance h 4 If h is 4 Less than T h Then calculate the map point P v1 To from P v2 、P v3 、P v4 Form a plane
Is a distance h 1 ，P 2 ' to from P v1 、P v3 、P v4 Form a plane
Is a distance h 2 And P v3 To by P v1 、P v2 、P v4 Form a plane
Is a distance h 3 If h is 1 、h 2 、h 3 Are all less than T h Then the map point combination is passed throughPre-screening the dough and putting it into the set S p In, T h Is a set threshold; repeating the step until a specified number of coplanar map point combinations are pre-screened out or all map points are traversed, and discarding and re-selecting if repeated map point combinations appear in the process;
2.4 Repeating the steps 2.2) to 2.3) until the pre-screened coplanar map point combinations meet the requirement of the maximum number, or traversing all map point combinations, and if the same map point combinations appear in the process, abandoning the cycle and re-selecting;
2.5 ) further examining the co-planar map point combinations pre-screened in step 2.4) using space vector relations for S p One coplanar map point combination P in v1 、P v2 、P v3 、P v4 Separately solving the linear equation P v1 ＝x 1 P v2 +y 1 P v3 +z 1 P v4 ，P v2 ＝x 2 P v1 +y 2 P v3 +z 2 P v4 ，P v3 ＝x 3 P v1 +y 3 P v2 +z 3 P v4 ，P v4 ＝x 4 P v1 +y 4 P v2 +z 4 P v3 Then calculating the error e of each item 1 ＝|1-x 1 -y 1 -z 1 |，e 2 ＝|1-x 2 -y 2 -z 2 |，e 3 ＝|1-x 3 -y 3 -z 3 |，e 4 ＝|1-x 4 -y 4 -z 4 If e 1 、e 2 、e 3 、e 4 Are all less than T e Put the coplanar combination into the final coplanar map point set S a Wherein x is g 、y g 、z g G =1,2,3,4 is the variable for solving the system of equations, T e Is the set threshold.
Further, in step 3), the calculation of the homographic transformation matrix including the most map point plane correspondences includes the following steps:
3.1 ) set a threshold value T a =0, homography transformation matrix H m And inner circumference value matchPair set q c ′ r ；
3.2 Read S) a To obtain a set of coplanar points P v1 、P v2 、P v3 、P v4 And corresponding matching pair (q) c1 ,q r1 )、(q c2 ,q r2 )、(q c3 ,q r3 )、(q c4 ,q r4 ) Calculating homography transformation matrix H by four-point method, and checking homography constraint deviation of all other matching pairs
If e hk ＜T h Then the matching pair (q) will be matched ck ,q rk ) Marked as an inlier value, otherwise marked as an outlier, wherein,
is the homogeneous coordinate, T, of the kth feature point of the reference frame and the current frame h Is a set threshold, the symbol ^ represents antisymmetric transformation; if the number of inner peripheral values T c ＞T a Then let T a ＝T c 、H m = H, then emptying the set q c ′ r And putting the matching pair marked as the inner surrounding value into q c ′ r ；
3.3 ) repeat step 3.2) until S a The point combination of the middle and common plane maps is completely read to obtain a homography matrix H m And a set of inlier value matching pairs q c ′ r (ii) a Then decomposing the homographic transformation matrix H by utilizing a decomplexer homographic Mat function in an Opencv library m And obtaining a rotation pose matrix R and a horizontal displacement pose vector t.
Further, in step 4), the rotation pose matrix R and the translation pose vector t are optimized by minimizing a weighted reprojection error function, and the specific expression is as follows:
the above formula is solved iteratively by a Gauss-Newton method; in the formula, argmin (. Cndot.) represents the optimization of the functionSmall-valued parameter, K is the camera internal parameter matrix, function pi (·) represents the de-homogeneous operation, W i Is a map point P i Weighted value of (1), P i Represents the ith map point, q ci 、q ri Feature points of the current frame representing initialized map points and corresponding feature points of the reference frame, N m Representing the total number of map points.
Further, in step 5), local map construction and local beam adjustment optimization are performed, including the following steps:
5.1 According to the rotation pose matrix R and the translation pose vector t optimized in the step 4), matching pairs of uninitialized map points
Map points initialized by triangularization method
Wherein q is c(m+1) ,q c(m+2) ,...,q cn Representing the feature points of the current frame, q r(m+1) ,q r(m+2) ,...,q rn Representing reference frame feature points, P m+1 ,P m+2 ,...,P n Representing map points;
5.2 Set the weight of the newly initialized map point:
where m + l = m, m +1 m+l Representing map points P m+l B is the scaling factor of the image pyramid, s c(m+l) 、S r(m+l) Respectively representing the observation map points P of the current frame and the reference frame (m+l) The number of layers of the pyramid where the feature points are located,
is an intermediate variable;
5.3 By minimizing the weighted reprojection error function, the rotation pose matrix R, the translation pose vector t, and all map points are optimized
The specific expression is as follows:
the above formula is solved iteratively by a Gauss-Newton method; wherein argmin (r) represents a parameter when taking the minimum value of the function, K is a camera internal reference matrix, the function pi (r) represents a homogeneous operation, W k Is a map point P k Weighted value of (1), P k Represents the kth map point, k =1,2 ck 、q rk Representing the feature points of the current frame and the corresponding feature points of the reference frame, and n represents the total number of map points.
Compared with the prior art, the invention has the following advantages and beneficial effects:
1. the invention fully utilizes abundant plane information in the indoor environment, provides effective prior constraint information for the visual odometer, and improves the precision and robustness of the visual odometer method in the indoor environment.
2. The method comprehensively considers the observed times and variance of the map points, qualitatively estimates the accuracy of the map points, and improves the reliability of the coplanar constraint relation of the searched map points; and by endowing different weight coefficients to the nonlinear optimization error function, the reliability of the camera pose and local map optimization result is improved.
3. The invention has good working performance in indoor environment and is suitable for monocular, binocular and RGB-D cameras.
Drawings
FIG. 1 is a block diagram of a homography visual odometer process in the method of the present invention.
Detailed Description
The present invention will be described in further detail with reference to examples and drawings, but the present invention is not limited thereto.
As shown in fig. 1, the embodiment discloses a homography visual odometer method facing indoor environment, which comprises the following steps:
1) Firstly, reading data including a reference frame, a current frame and a local map, then extracting ORB feature points of the current frame, filtering the extracted feature points by a quadtree method, and finally completing feature matching of the current frame and the reference frame, wherein the specific steps are as follows:
1.1 Continuously down-sampling 7 times by adopting a 1.2 scaling factor on an original image of a current frame to construct an 8-layer image pyramid, then extracting a corresponding number of FAST angular points according to the resolution of each layer of image, and totally extracting 1000 feature points;
1.2 Filtering the feature points extracted in the step 1.1) by a quadtree method, and then calculating BRIEF descriptors of all the remaining feature points;
1.3 For each feature point in the current frame, taking the same position in the reference frame as a center O, searching a feature point with the minimum Hamming distance from a descriptor in a region with the radius r =20 pixels as the best match, taking the Hamming distance with the second smallest Hamming distance as the next best match, if the Hamming distance corresponding to the best match is greater than 30 or the distance difference between the distance of the next best match and the distance of the best match is less than 20, discarding the best match, otherwise, keeping the best match as a matching pair of feature matches; all matching pairs are denoted as q cr ＝{(q c1 ,q r1 ),(q c2 ,q r2 ),...,(q ck ,q rk ),...,(q cn ,q rn ) Where k =1,2.., n, q ck 、q rk The matching pair of the initialized map point represents the current frame feature point and the corresponding feature point of the reference frame is
Wherein i =1,2., m, q ci 、q ri The feature points of the current frame representing the initialized map points and the corresponding feature points of the reference frame are
Wherein i =1,2,. Multidot.m, P i Representing a map point corresponding to a number of observations of the map point of
Wherein i =1,2,. Cndot., m, N i Representing the observed times of the corresponding map points, the matching pairs of uninitialized map points are
Wherein l =1,2.,. N-m, q c(m+l) 、q r(m+l) The current frame feature points and the reference frame corresponding feature points representing uninitialized map points.
2) Searching a coplanar map point combination by using a weighted random sampling strategy, which comprises the following specific steps:
2.1 According to the observation times, endowing the ith map point with the following weight:
in the formula, W i Representing map points P i B is the scaling factor of the image pyramid, s u Is the u-th observation map point P i Number of pyramid layers where feature points are located, N i For the number of observations of the ith map point,
is an intermediate variable;
setting map point probability:
in the formula I i Is the probability of the ith map point, W is the intermediate variable;
2.2 ) randomly selecting 3 map points P according to the result of step 2.1) v1 、P v2 、P v3 If the three points are collinear, discarding and re-selecting, wherein the map point probability l i As map point P i Probability of being selected;
2.3 ) randomly selecting a map point P v4 If P is selected v1 、P v2 Or P v3 Then abandon and re-select, wherein the map point probability l i As map point P i Probability of being selected, then calculating P v4 To from P v1 、P v2 、P v3 Form a plane
A distance h 4 If h is 4 Less than T h (preferably 0.02 m), the map point P is calculated v1 To by P v2 、P v3 、P v4 Form a plane
Is a distance h 1 ，P v2 To by P v1 、P v3 、P v4 Form a plane
Is a distance h 2 And P v3 To from P v1 、P v2 、P v4 Form a plane
Is a distance h 3 If h is 1 、h 2 、h 3 Are all less than T h Then the map point combination is put into the set S through coplanar pre-screening p In, T h Is a set threshold; repeating the steps until a specified number of coplanar map point combinations are pre-screened out or all map points are traversed, wherein the process is carried outIf repeated map point combinations appear, discarding and reselecting;
2.4 Repeating the step 2.3) until 10 groups of coplanar map points are pre-screened out or all map points are traversed, and if repeated map points appear in the process, re-selecting;
2.5 Repeating the steps 2.2) to 2.4) until 50 groups of coplanar map points are pre-screened out or all map point combinations are traversed, if S appears in the process p If the map points exist, reselecting;
2.6 In turn over S p Middle coplanar map point combination P v1 、P v2 、P v3 、P v4 Separately solving the linear equation P v1 ＝x 1 P v2 +y 1 P v3 +z 1 P v4 ，P v2 ＝x 2 P v1 +y 2 P v3 +z 2 P v4 ，P v3 ＝x 3 P v1 +y 3 P v2 +z 3 P v4 ，P v4 ＝x 4 P v1 +y 4 P v2 +z 4 P v3 If the error of each result satisfies e 1 ＝|1-x 1 -y 1 -z 1 |＜0.05，e 2 ＝|1-x 2 -y 2 -z 2 |＜0.05，e 3 ＝|1-x 3 -y 3 -z 3 |＜0.05，e 4 ＝|1-x 4 -y 4 -z 4 If the absolute value is less than 0.05, putting the coplanar combination into a final coplanar map point set S a Wherein x is g 、y g 、z g G =1,2,3,4 is a variable for solving the system of equations.
3) Finding the plane with the most map points, and calculating the homography transformation matrix H of the poses of the current frame and the reference frame m And finally, calculating a rotation pose matrix R and a translation pose vector t, and specifically comprising the following steps:
3.1 ) set a threshold value T a =0, homography transformation matrix H m ＝I 3 And a set of inlier value matching pairs q c ′ r = phi, wherein I 3 Representing a 3-order identity matrix, phi represents a null set;
3.2 Read)Get set S a To obtain a set of coplanar map points P v1 、P v2 、P v3 、P v4 And corresponding matching pair (q) c1 ,q r1 )、(q c2 ,q r2 )、(q c3 ,q r3 )、(q c4 ,q r4 ) Calculating homography transformation matrix H by four-point method, and calculating homography constraint deviation of all other matching pairs
If e hk If < 4, then match pair (q) ck ,q rk ) Marked as an inlier value, otherwise marked as an outlier, wherein,
is the homogeneous coordinate of the kth characteristic point of the reference frame and the current frame; if the number of inner peripheral values T c ＞T a Then let T a ＝T c 、H m = H, then emptying the set q c ′ r Putting the matching pair marked as the inner value at present;
3.3 ) repeating step 3.2) until S a The point combination of the middle and common plane maps is completely processed to obtain a homography matrix H m And inner periphery value set q c ′ r (ii) a Decomposing a homography transformation matrix H by utilizing a decomplexeHomographyMat function in an Opencv library m And obtaining a rotation pose matrix R and a translation pose vector t.
4) By minimizing the weighted re-projection error function, the rotation pose matrix R and the translation pose vector t are optimized as follows:
the above formula is solved iteratively by a Gauss-Newton method; where K is the camera internal reference matrix and the function π (-) represents the de-homogeneous operation.
5) Constructing a local map, and optimizing a rotation pose matrix R, a translation pose vector t and the local map by a local beam adjustment method, wherein the method specifically comprises the following steps:
5.1 ) matching pairs of uninitialized map points according to the rotation pose matrix R and the translation pose vector t optimized in the step 4)
Map points initialized by triangularization method
Wherein q is c(m+1) ,q c(m+2) ,…,q cn Representing the feature points of the current frame, q r(m+1) ,q r(m+2) ,…,q rn Representing reference frame feature points, P m+1 ,P m+2 ,...,P n Representing map points;
5.2 Set new initialized map point weight:
where m + l = m, m +1 m+l Representing map points P m+l B is the scaling factor of the image pyramid, s c(m+l) 、S r(m+l) Respectively representing the observation map points P of the current frame and the reference frame (m+l) The number of layers of the pyramid where the feature points are located,
is an intermediate variable;
5.3 By minimizing the weighted reprojection error function, the rotation pose matrix R, the translation pose vector t, and all map points are optimized
The concrete expression is as follows:
the above formula is solved iteratively by a Gauss-Newton method; in the formula, argmin (·) represents the parameter when taking the minimum value to the function, K is the camera internal parameter matrix, function π (·) represents the homogeneous operation, W k Is a map point P k Weighted value of (2), P k Denotes the kth map point, k =1,2, \ 8230;, n, q ck 、q rk Representing the feature points of the current frame and the corresponding feature points of the reference frame, and n represents the total number of map points.
In conclusion, the invention makes full use of the plane constraint relationship in the indoor environment, and improves the precision and robustness of the visual odometry method in the indoor environment. According to the method, the re-projection error function is weighted and optimized according to the observed times and the estimated covariance of the map points, and the reliability of the camera pose and the local map optimization result is improved. The invention is suitable for common visible light cameras and obtains good working performance in indoor environment.
The above embodiments are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents thereof, and all such changes, modifications, substitutions, combinations, and simplifications are intended to be included in the scope of the present invention.
Claims (6)
1. A homography visual odometer method facing indoor environment is characterized by comprising the following steps:
1) Reading data including a reference frame, a current frame and a local map, then extracting ORB feature points from the current frame, filtering the extracted feature points by a quadtree method, and finally completing feature matching of the current frame and the reference frame;
2) Finding a coplanar map point combination by using a weighted random sampling strategy;
3) Calculating a homographic transformation matrix H of the poses of the current frame and the reference frame according to the plane with the most map points m Then, obtaining a rotation pose matrix R and a translation pose vector t by solution;
4) Optimizing a rotation pose matrix R and a translation pose vector t by minimizing a weighted reprojection error function;
5) And constructing a local map, and optimizing the rotation pose matrix R, the translation pose vector t and the local map by a local beam adjustment method.
2. The method for homography visual odometry towards an indoor environment according to claim 1, wherein in step 1), the current frame refers to a current image of a pose to be estimated read from an image sequence, the reference frame refers to a previous frame image read from the image sequence, and the step 1) comprises the following steps:
1.1 Adopting a scaling coefficient b to continuously perform downsampling on an original image of a current frame to obtain a d-layer image pyramid, and then extracting a corresponding number of FAST angular points according to the resolution of each layer of image, wherein the total number of extracted feature points is N;
1.2 Filtering the feature points extracted in the step 1.1) by a quadtree method, and then calculating BRIEF descriptors of all the remaining feature points;
1.3 For each feature point in the current frame, searching the feature point with the minimum Hamming distance with its descriptor in the neighboring area of the same position in the reference frame as the best match, and searching the next best match if the Hamming distance is the second smallest, and if the Hamming distance of the best match is greater than T b Or the difference between the best matching distance and the next best matching distance is less than T d If not, the best match is retained as the matched pair of feature match, wherein T b 、T d Is a set threshold; all matching pairs are denoted as q cr ＝{(q c1 ,q r1 ),(q c2 ,q r2 ),...,(q ck ,q rk ),...,(q cn ,q rn ) Where k =1,2.., n, q ck 、q rk The matching pair of the initialized map point represents the current frame feature point and the corresponding feature point of the reference frame is
Wherein i =1,2., m, q ci 、q ri Indicates that it is already at firstThe feature points of the current frame and the reference frame of the initialization map point correspond to the feature points, and the corresponding map points are
Wherein i =1,2,. Cndot., m, P i Representing map points corresponding to the number of observations of the map points as
Wherein i =1,2., m, N i Representing the number of times the corresponding map point is observed, and the matching pair of uninitialized map points is
Wherein l =1,2., n-m, q c(m+l) 、q r(m+l) And representing the current frame characteristic points of the uninitialized map points and the corresponding characteristic points of the reference frame.
3. The homography visual odometry method facing indoor environment of claim 2, wherein in step 2), the coplanar map point combination is searched for by a weighted random sampling strategy, comprising the following steps:
2.1 According to the observation times, giving the ith map point the following weight:
in the formula, W i Representing map points P i B is the scaling factor of the image pyramid, s u Is the u-th observed map point P i Number of pyramid layers where feature points are located, N i For the number of observations of the ith map point,
is an intermediate variable;
setting map point probability:
in the formula I i Is the probability of the ith map point, W is the intermediate variable;
2.2 ) randomly selecting 3 map points P according to the result of step 2.1) v1 、P v2 、P v3 If the three points are collinear, then the point probability l is discarded and reselected i As map point P i Probability of being selected;
2.3 ) randomly selecting a map point P v4 If P is selected v1 、P v2 Or P v3 Then abandon and re-select, wherein the map point probability l i As map point P i Probability of being selected, then calculating P v4 To by P v1 、P v2 、P v3 Form a plane
Is a distance h 4 If h is 4 Less than T h Then calculate map point P v1 To from P v2 、P v3 、P v4 Form a plane
A distance h 1 ，P′ 2 To from P v1 、P v3 、P v4 Form a plane
Is a distance h 2 And P v3 To by P v1 、P v2 、P v4 Form a plane
A distance h 3 If h is 1 、h 2 、h 3 Are all less than T h Then the map point combination is put into the set S through coplanar pre-screening p In, T h Is a set threshold; repeating the step until a specified number of coplanar map point combinations are pre-screened out or all map points are traversed, and discarding and re-selecting if repeated map point combinations appear in the process;
2.4 Repeating the steps 2.2) to 2.3) until the pre-screened coplanar map point combination meets the requirement of the maximum number, or traversing all map point combinations, and if the same map point combination appears in the process, abandoning the cycle and re-selecting;
2.5 ) further examining the co-planar map point combinations pre-screened in step 2.4) using space vector relations for S p One coplanar map point combination P in v1 、P v2 、P v3 、P v4 Separately solving the linear equation P v1 ＝x 1 P v2 +y 1 P v3 +z 1 P v4 ，P v2 ＝x 2 P v1 +y 2 P v3 +z 2 P v4 ，P v3 ＝x 3 P v1 +y 3 P v2 +z 3 P v4 ，P v4 ＝x 4 P v1 +y 4 P v2 +z 4 P v3 Then calculating the error e of each term 1 ＝|1-x 1 -y 1 -z 1 |，e 2 ＝|1-x 2 -y 2 -z 2 |，e 3 ＝|1-x 3 -y 3 -z 3 |，e 4 ＝|1-x 4 -y 4 -z 4 If e 1 、e 2 、e 3 、e 4 Are all less than T e Put the coplanar combination into the final coplanar map point set S a Wherein x is g 、y g 、z g G =1,2,3,4 is the variable for solving the system of equations, T e Is the set threshold.
4. A homographic visual odometry method facing an indoor environment according to claim 3, wherein in step 3), a homographic transformation matrix containing at most map point plane correspondences is calculated, comprising the steps of:
3.1 ) set a threshold value T a =0, homography transformation matrix H m And inner value matching pair set q' cr ；
3.2 Read S) a To obtain a set of coplanar points P v1 、P v2 、P v3 、P v4 And corresponding matching pair (q) c1 ,q r1 )、(q c2 ,q r2 )、(q c3 ,q r3 )、(q c4 ,q r4 ) Calculating homography transformation matrix H by four-point method, and checking homography constraint deviation of all other matching pairs
If e hk ＜T h Then the matching pair (q) will be matched ck ,q rk ) Marked as an inlier value, otherwise marked as an outlier, wherein,
is the homogeneous coordinate, T, of the kth feature point of the reference frame and the current frame h Is a set threshold, the symbol ^ represents antisymmetric transformation; if the number of inner peripheral values T c ＞T a Then let T a ＝T c 、H m = H, then emptying the set q' cr And putting the matching pair currently marked as the inner surrounding value into q' cr ；
3.3 ) repeat step 3.2) until S a The point combination of the middle coplanar map is completely read to obtain a homography matrix H m And inner value matching pair set q' cr (ii) a And decomposing the homography transformation matrix H by utilizing a decomplexeHomographyMat function in an Opencv library m And obtaining a rotation pose matrix R and a horizontal displacement pose vector t.
5. The homography visual odometry method facing indoor environment of claim 4, wherein in step 4), the rotation pose matrix R and the translation pose vector t are optimized by minimizing a weighted reprojection error function, and the specific expressions are as follows:
the above formula is solved iteratively by a Gauss-Newton method; in the formula, argmin (-) represents the parameter when taking the minimum value to the function, K is the camera internal parameter matrix, pi (-) represents the homogeneous operation, W i Is a map point P i Weighted value of (2), P i Represents the ith map point, q ci 、q ri Feature points of the current frame representing initialized map points and corresponding feature points of the reference frame, N m Representing the total number of map points.
6. The homography visual odometry method facing indoor environment of claim 5, wherein in step 5), local map construction and local beam adjustment optimization are performed, comprising the following steps:
5.1 According to the rotation pose matrix R and the translation pose vector t optimized in the step 4), matching pairs of uninitialized map points
Map points initialized by triangularization method
Wherein q is c(m+1) ,q c(m+2) ,...,q cn Representing the feature point of the current frame, q r(m+1) ,q r(m+2) ,...,q rn Representing reference frame feature points, P m+1 ,P m+2 ,...,P n Representing map points;
5.2 Set the weight of the newly initialized map point:
where m + l = m, m +1, \ 8230, n is the index number of the newly initialized map point, W m+l Representing map points P m+l B is the scaling factor of the image pyramid, s c(m+l) 、S r(m+l) Respectively representing the observation map points P of the current frame and the reference frame (m+l) The number of layers of the pyramid where the feature points are located,
is an intermediate variable;
5.3 Optimization of the rotation pose matrix R, translation pose vector t and all map points by minimizing the weighted reprojection error function
The specific expression is as follows:
the above formula is solved iteratively by a Gauss-Newton method; wherein argmin (r) represents a parameter when taking the minimum value of the function, K is a camera internal reference matrix, the function pi (r) represents a homogeneous operation, W k Is a map point P k Weighted value of (2), P k Represents the kth map point, k =1,2 ck 、q rk Representing the feature points of the current frame and the corresponding feature points of the reference frame, and n represents the total number of map points.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211044239.XA CN115451996B (en) | 2022-08-30 | 2022-08-30 | Homography visual odometer method facing indoor environment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211044239.XA CN115451996B (en) | 2022-08-30 | 2022-08-30 | Homography visual odometer method facing indoor environment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115451996A true CN115451996A (en) | 2022-12-09 |
| CN115451996B CN115451996B (en) | 2024-03-29 |
Family
ID=84301286
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211044239.XA Active CN115451996B (en) | 2022-08-30 | 2022-08-30 | Homography visual odometer method facing indoor environment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115451996B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108519102A (en) * | 2018-03-26 | 2018-09-11 | 东南大学 | A kind of binocular vision speedometer calculation method based on reprojection |
| CN110108258A (en) * | 2019-04-09 | 2019-08-09 | 南京航空航天大学 | A kind of monocular vision odometer localization method |
| CN113108771A (en) * | 2021-03-05 | 2021-07-13 | 华南理工大学 | Movement pose estimation method based on closed-loop direct sparse visual odometer |
| CN113345018A (en) * | 2021-05-31 | 2021-09-03 | 湖南大学 | Laser monocular vision fusion positioning mapping method in dynamic scene |
| WO2022002039A1 (en) * | 2020-06-30 | 2022-01-06 | 杭州海康机器人技术有限公司 | Visual positioning method and device based on visual map |
-
2022
- 2022-08-30 CN CN202211044239.XA patent/CN115451996B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108519102A (en) * | 2018-03-26 | 2018-09-11 | 东南大学 | A kind of binocular vision speedometer calculation method based on reprojection |
| CN110108258A (en) * | 2019-04-09 | 2019-08-09 | 南京航空航天大学 | A kind of monocular vision odometer localization method |
| WO2022002039A1 (en) * | 2020-06-30 | 2022-01-06 | 杭州海康机器人技术有限公司 | Visual positioning method and device based on visual map |
| CN113108771A (en) * | 2021-03-05 | 2021-07-13 | 华南理工大学 | Movement pose estimation method based on closed-loop direct sparse visual odometer |
| CN113345018A (en) * | 2021-05-31 | 2021-09-03 | 湖南大学 | Laser monocular vision fusion positioning mapping method in dynamic scene |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115451996B (en) | 2024-03-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110569704B (en) | Multi-strategy self-adaptive lane line detection method based on stereoscopic vision | |
| Ishikawa et al. | Lidar and camera calibration using motions estimated by sensor fusion odometry | |
| CN110223348B (en) | Robot scene self-adaptive pose estimation method based on RGB-D camera | |
| US9989969B2 (en) | Visual localization within LIDAR maps | |
| CN108537848B (en) | Two-stage pose optimization estimation method for indoor scene reconstruction | |
| CN110807809B (en) | Light-weight monocular vision positioning method based on point-line characteristics and depth filter | |
| CN103106688B (en) | Based on the indoor method for reconstructing three-dimensional scene of double-deck method for registering | |
| CN108921895B (en) | Sensor relative pose estimation method | |
| CN107590827A (en) | A kind of indoor mobile robot vision SLAM methods based on Kinect | |
| CN104835158B (en) | Based on the three-dimensional point cloud acquisition methods of Gray code structured light and epipolar-line constraint | |
| CN111080709B (en) | Multispectral stereo camera self-calibration algorithm based on track feature registration | |
| WO2021098083A1 (en) | Multispectral camera dynamic stereo calibration algorithm based on salient feature | |
| CN107917710B (en) | Indoor real-time positioning and three-dimensional map construction method based on single line laser | |
| CN113658337B (en) | Multi-mode odometer method based on rut lines | |
| CN112929626B (en) | Three-dimensional information extraction method based on smartphone image | |
| CN115421158B (en) | Self-supervision learning solid-state laser radar three-dimensional semantic mapping method and device | |
| CN111536970B (en) | Infrared inertial integrated navigation method for low-visibility large-scale scene | |
| Nagy et al. | Online targetless end-to-end camera-LiDAR self-calibration | |
| CN112183247A (en) | Laser point cloud data classification method based on multispectral image | |
| Stucker et al. | ResDepth: Learned residual stereo reconstruction | |
| CN115471748A (en) | Monocular vision SLAM method oriented to dynamic environment | |
| CN113887624A (en) | Improved feature stereo matching method based on binocular vision | |
| CN111415305A (en) | Method for recovering three-dimensional scene, computer-readable storage medium and unmanned aerial vehicle | |
| CN110807799B (en) | Line feature visual odometer method combined with depth map inference | |
| CN112017259B (en) | Indoor positioning and image building method based on depth camera and thermal imager |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |