CN112115980A - Binocular vision odometer design method based on optical flow tracking and point line feature matching - Google Patents

Binocular vision odometer design method based on optical flow tracking and point line feature matching Download PDF

Info

Publication number
CN112115980A
CN112115980A CN202010862610.8A CN202010862610A CN112115980A CN 112115980 A CN112115980 A CN 112115980A CN 202010862610 A CN202010862610 A CN 202010862610A CN 112115980 A CN112115980 A CN 112115980A
Authority
CN
China
Prior art keywords
point
frame
line
matching
feature
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.)
Pending
Application number
CN202010862610.8A
Other languages
Chinese (zh)
Inventor
李慧平
宋晓
严卫生
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202010862610.8A priority Critical patent/CN112115980A/en
Publication of CN112115980A publication Critical patent/CN112115980A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/50—Extraction of image or video features by performing operations within image blocks; by using histograms, e.g. histogram of oriented gradients [HoG]; by summing image-intensity values; Projection analysis

Abstract

The invention relates to a binocular vision odometer design method based on optical flow tracking and point line feature matching, and belongs to the technical field of robot positioning and navigation. The method sequentially comprises the following steps: receiving a visual image input by a binocular camera; tracking point features by using a KLT optical flow method, and correlating point feature data; extracting line features and calculating line feature descriptors, and performing line breaking and merging operation on the extracted line features; carrying out data association on point and line characteristics between the previous frame and the next frame in an optical flow tracking matching and descriptor matching mode respectively, solving a reprojection error, and estimating the pose of the current frame; judging whether the current frame is a key frame, if so, extracting point features and calculating descriptors of the point features; if not, setting a reference key frame of the current frame; performing data association on the point and line characteristics among the local key frames in a descriptor matching mode, solving a reprojection error, and optimizing the pose again; and after the local key frame pose is optimized, adjusting the pose of the non-key frame.

Description

Binocular vision odometer design method based on optical flow tracking and point line feature matching
Technical Field
The invention belongs to the technical field of robot positioning and navigation, and particularly relates to a binocular vision odometer design method based on optical flow tracking and point line feature matching.
Background
The Visual Odometer (VO) is an important component of a navigation system, and is widely applied to robotics, such as unmanned vehicles and unmanned aerial vehicles. In these applications, one alternative to VO is to use an Inertial Measurement Unit (IMU), but this solution has the disadvantage of: over time, a large number of errors accumulate because they cannot accurately counteract the effects of gravity. Conventional alternatives also include wheeled odometers, which cannot replace VO, and GPS-based navigation systems, among others, because: it has larger measurement error and poor long-time use precision; the disadvantages of the GPS-based navigation system solution are: these systems are limited to open, unobstructed outdoor environment use and do not allow estimation of the orientation of the devices to which they are connected. The VO technology can largely make up for the defects of the above navigation scheme, and another advantage of VO is that the required information (provided by the camera) can be used for SLAM and scene recognition and other navigation-related tasks.
In the existing VO algorithm, the method based on optical flow tracking can save a large amount of calculation because a descriptor and a matching descriptor are not required to be calculated, has high algorithm speed and better meets the requirement of real-time property. However, the optical flow tracking method has the disadvantages that the optical flow tracking method is easily influenced by illumination, weak texture scenes are not good in effect, and when a camera moves in a large scale, the optical flow tracking method cannot track the scene well; the method based on the point feature has higher robustness compared with the optical flow tracking method when the camera movement scale is too large, and the algorithm precision is higher because the feature points are matched in a descriptor mode, but the point feature method has the defects that: the extraction and matching calculation amount of the point features is large, more time is spent, and the point features cannot work normally in a scene with weak texture; the method based on the line features has higher robustness compared with the point feature method in the weak texture scene, but has the disadvantage that the extraction and matching of the line features are more computationally intensive compared with the point features.
Disclosure of Invention
Technical problem to be solved
In order to solve the problems that the positioning accuracy is not high by adopting an optical flow method and the real-time performance is not strong by adopting a point-line feature method in the existing VO technology, the invention provides a binocular vision odometer design method based on optical flow tracking and point-line feature matching.
Technical scheme
A binocular vision odometer design method based on optical flow tracking and point line feature matching is characterized by comprising the following steps:
step 1: acquiring an image by using a binocular camera, converting the image into a gray image, and then performing enhancement processing on the image by using a self-adaptive histogram equalization algorithm;
step 2: using a KLT optical flow method to track point features of a previous frame image, and establishing data association of the point features between the previous frame and the next frame by combining a bidirectional annular matching strategy:
step 2.1: tracking a left eye image of a previous frame by using a KLT optical flow method and a left eye image of a current frame, and performing tracking matching of point characteristics;
step 2.2: screening and supplementing point characteristics;
step 2.3: performing point feature tracking matching between the left eye image and the right eye image of the current frame by using a KLT optical flow method, and further calculating a three-dimensional coordinate corresponding to a matching point according to an example stereoscopic vision algorithm;
and step 3: extracting line features of the current frame:
step 3.1: extracting line features based on an LSD line feature extraction algorithm, and calculating a line feature descriptor based on an LBD line feature description algorithm;
step 3.2: performing disconnection merging operation on the extracted line characteristics;
step 3.3: performing line feature matching between the left eye image and the right eye image of the current frame;
and 4, step 4: matching line characteristics between the front frame and the rear frame;
and 5: based on the data correlation result of the point characteristics between the previous frame and the next frame, a PNP pose estimation method is adopted to obtain the initial pose estimation of the current frame;
step 6: optimizing and adjusting the pose of the current frame by using a cost function constructed by the reprojection errors of the point characteristics and the line characteristics between the previous frame and the next frame;
and 7: judging whether the current frame is a key frame, if so, executing the step 8; if not, setting the previous key frame as the reference key frame of the current frame, and then jumping to the step 12;
and 8: extracting the point characteristics of the current frame:
step 8.1: extracting and describing point features of the current frame by adopting an ORB algorithm;
step 8.2: performing sparse stereo matching on the point characteristics between the left and right eye images of the current frame in a descriptor matching mode;
and step 9: carrying out data association of point and line characteristics among local key frames;
step 10: performing secondary optimization adjustment on the pose of the local key frames by using a cost function constructed by the re-projection errors of the point features and the line features between the local key frames;
step 11: adjusting the pose of the non-key frame;
step 12: and outputting the poses of all the image frames.
The bidirectional ring matching strategy in the step 2 refers to: the last frame of the left eye image
Figure BDA0002648653030000031
Feature point set in (1)
Figure BDA0002648653030000032
Left eye image of current frame by means of bidirectional KLT optical flow method
Figure BDA0002648653030000033
In-tracking matching to a feature point set
Figure BDA0002648653030000034
The current frame left eye image
Figure BDA0002648653030000035
Feature point set in (1)
Figure BDA0002648653030000036
Right eye image of current frame by bidirectional KLT optical flow method
Figure BDA0002648653030000037
Matching the intermediate tracking to a temporary feature point set X1; the current frame of the right eye image
Figure BDA0002648653030000038
The temporary feature point set X1 in the previous frame is a right eye image by means of a bidirectional KLT optical flow method
Figure BDA0002648653030000039
Matching the intermediate tracking to a temporary feature point set X2; judging whether the characteristic points in the temporary characteristic point set X2 fall into the original characteristic point set or not
Figure BDA0002648653030000041
The original feature points in the image are deleted from X2 for the feature points falling outside the domain range, and those feature points corresponding to the deleted feature points in X2 are deleted from X1 according to the matching relationship between X1 and X2, thereby obtaining the right image of the current frame
Figure BDA0002648653030000042
Feature point set in (1)
Figure BDA0002648653030000043
In the step 4, two line segments l successfully matched1,l2The following conditions are satisfied:
(1) the line segments detected by the LSD have directivity, and the included angle of the direction vectors of the two matched line segments is less than phi;
(2) ratio of the lengths of two line segments
Figure BDA0002648653030000044
(3) Calculating the length l of the two-segment overlapping regionoverlap，
Figure BDA0002648653030000045
(4) The distance corresponding to the LBD eigenvector is less than rhoTAnd is where the distance is smallest.
In step 7, the key frame is determined according to the following principle: the following conditions are also satisfied:
(1) at least 20 image frames are contained between the previous key frame and the previous key frame;
(2) at least 50 point features and 15 line features are successfully tracked;
(3) the co-viewpoint, line feature information with the last key frame is less than 75%.
Advantageous effects
The invention provides a binocular vision odometer design method based on optical flow tracking and point line feature matching, which firstly introduces line features in a point feature-based vision odometer calculation method, namely: the characteristic matching mode of combining the dotted line characteristics is used, so that the robustness and the positioning accuracy of the algorithm are improved, and the algorithm still has good performance even in a weak texture scene; meanwhile, an optical flow method is introduced into a visual mileage calculation method based on point-line features to track the matching point features, so that the rapidity of the algorithm is improved. The method has the advantages of an optical flow tracking method, a point feature method and a line feature method, and overcomes the defects of a single-use optical flow tracking method, a single-use point feature method and a single-use line feature method.
Drawings
FIG. 1 is a flow chart of an embodiment of the present invention
FIG. 2 illustrates a two-way circular matching strategy
FIG. 3 is a line feature merge strategy
FIG. 4 is a schematic of a reprojection error of line features
FIG. 5 shows the experimental results on MH _04_ difficult image sequence
FIG. 6 shows a comparison of APE results
Detailed Description
The invention will now be further described with reference to the following examples and drawings:
experiments this algorithm was tested using a sequence of MH _04_ difficult images in the internationally recognized data set EuRoC, with a computer configured to: the CPU is A4-5000, the main frequency is 1.50GHz, the memory is 8GB, and the system is Ubuntu 16.04.
The EuRoC data set is used as a data acquisition platform by an unmanned aerial vehicle carrying sensors such as vision, inertial navigation and radar, wherein the positioning data tracked by the millimeter-scale laser radar is used as a motion true value of the unmanned aerial vehicle. The MH _04_ difficult image sequence was acquired by the drone moving 91.7m at 0.93m/s, 0.24rad/s angular velocity in a dark indoor building, with the camera acquiring data at a frequency of 20 Hz.
FIG. 1 is a flow chart of an embodiment of the present invention. As shown in fig. 1, the method for designing a binocular vision odometer based on optical flow tracking and point line feature matching, provided by the invention, comprises the following steps:
step 1: reading a binocular image, and preprocessing the image;
reading binocular images from an MH _04_ diffcult image sequence, wherein the image preprocessing method comprises the following steps: firstly, converting an image into a gray image, and then performing enhancement processing on the image by using an adaptive histogram equalization algorithm.
Step 2: the method comprises the following steps of tracking point features of a previous frame image by using a KLT optical flow method, and establishing data association of the point features between the previous frame and a next frame by combining a bidirectional annular matching strategy, wherein the method comprises the following steps:
step 2.1: tracking a left eye image of a previous frame by using a KLT optical flow method and a left eye image of a current frame, and performing tracking matching of point characteristics;
optical flow tracking describes a method by which pixels on an image move between images over time, namely: and determining the position of the pixel point in the previous frame to be about to appear in the next frame. The process of tracking the characteristics of the matching points by the optical flow is the process of tracking the pixel points.
The optical flow treats an image as a function of time, I (t), and then a pixel at time t, located at (x, y), whose gray scale can be written as I (x, y, t). For the same spatial point, in order to determine its upcoming position in the next frame, the optical flow method makes a basic assumption: the gray scale invariant assumption, namely: the pixel gray values of the same spatial point are fixed and invariant in each image.
At time t, the pixel at image coordinate (x, y), which moves to image coordinate (x + dx, y + dy) at time t + dt, according to the assumption of unchanged gray level, there are:
I(x+dx,y+dy,t+dt)＝I(x,y,t) (1)
taylor expansion is performed on the left side of the above equation, and a first order term is retained, which is:
Figure BDA0002648653030000061
according to the assumption of unchanged gray scale, the gray scale value of the same spatial point at the time t + dt is the same as the gray scale value at the time t, so that:
Figure BDA0002648653030000062
the left side and the right side of the above formula are divided by dt to obtain:
Figure BDA0002648653030000063
wherein dx/dt is the moving speed of the pixel point in the x-axis direction, and is recorded as: u; dy/dt is the movement speed of the pixel point in the y-axis direction, and is recorded as: v. At the same time, the user can select the desired position,
Figure BDA0002648653030000064
the gradient of the pixel point in the x-axis direction is recorded as: i isx；
Figure BDA0002648653030000065
The gradient of the pixel point in the y-axis direction is recorded as: i isy. In addition, note ItThe matrix form of the variation of the image gray scale to the time is as follows:
Figure BDA0002648653030000071
after the motion speed u, v of the pixel point between the images is calculated, the position of the pixel point in the next frame image at the next moment can be estimated.
Fig. 2 illustrates a bidirectional ring matching strategy. As shown in FIG. 2, first, the previous frame of the left eye image is divided into two frames
Figure BDA0002648653030000072
Feature point set in (1)
Figure BDA0002648653030000073
Left eye image of current frame by means of bidirectional KLT optical flow method
Figure BDA0002648653030000074
In-tracking matching to a feature point set
Figure BDA0002648653030000075
The current frame left eye image
Figure BDA0002648653030000076
Feature point set in (1)
Figure BDA0002648653030000077
Right eye image of current frame by bidirectional KLT optical flow method
Figure BDA0002648653030000078
Matching the intermediate tracking to a temporary feature point set X1; the current frame of the right eye image
Figure BDA0002648653030000079
The temporary feature point set X1 in the previous frame is a right eye image by means of a bidirectional KLT optical flow method
Figure BDA00026486530300000710
Matching the intermediate tracking to a temporary feature point set X2; judging whether the characteristic points in the temporary characteristic point set X2 fall into the original characteristic point set or not
Figure BDA00026486530300000711
The original feature points in (2) are deleted from X2 for the feature points falling outside the domain range, and those feature points corresponding to the deletion in X2 are deleted from X1 according to the matching relationship between X1 and X2, thereby obtaining the right-eye image of the current frame
Figure BDA00026486530300000712
Feature point set in (1)
The bi-directional KLT optical flow method refers to: assuming that point feature matching is performed on the image Picture1 and the image Picture2, point feature tracking is performed from the image Picture1 to the image Picture2 by using the KLT optical flow method, points that fail tracking and points that are tracked to the edge of the image are removed, then the remaining points are again tracked in reverse direction from the image Picture2 to the image Picture1 by using the KLT optical flow method, and the points that fail tracking and the points that are tracked to the edge of the image are removed again, and then the remaining points are used as points matched between the image Picture1 and the image Picture 2.
Step 2.2: point features were screened and supplemented.
In step 2.1, an optical flow tracking method is used, the left eye image of the current frame tracks the left eye image of the previous frame, point feature tracking is carried out, the number of the point features successfully tracked is judged, the point features in the left eye image of the current frame are extracted through a Shi-Tomasi corner point detection method, the point features falling into the field of the set range of the original point features are deleted, newly added point features are obtained, and the point features in the left eye image of the current frame are supplemented to 200.
Step 2.3: and (3) performing point feature tracking matching between the left eye image and the right eye image of the current frame by using a KLT optical flow method, and further calculating a three-dimensional coordinate corresponding to the matching point according to an example stereoscopic vision algorithm. And calculating the three-dimensional coordinates corresponding to the matching points according to a stereoscopic vision algorithm, wherein the calculation process is as follows:
d＝uL-uR (6)
Figure BDA0002648653030000081
wherein (X, Y, Z) is three-dimensional coordinates of a space point in a camera coordinate system, f represents the focal length of the camera, b represents a base line of the binocular camera, and cx,cyIs the internal reference of the camera, uLIs the pixel coordinate, u, of a spatial point in the left eye image in the x-axis directionRPixel coordinates, v, of spatial points in the direction of the x-axis in the right eye imageLThe y-axis direction pixel coordinate of the space point in the left eye image, d is the abscissa difference of the space point in the left eye image and the right eye image, namely: parallax error.
And step 3: extracting line characteristics of a current frame, comprising the following steps:
step 3.1: extracting line features based on an LSD line feature extraction algorithm, and calculating a line feature descriptor based on an LBD line feature description algorithm;
step 3.2: performing disconnection merging operation on the extracted line characteristics;
FIG. 3 is a line feature merging strategy. As shown in fig. 3, the line feature merging mainly includes the following sub-steps:
step 3.2.1: line feature set L ═ Li}i＝1,...,nSorting according to length and selecting the longest line segment Lmax；
Step 3.2.2: at LmaxThe principal direction difference dist (ang) between the L and the member of the L is obtainedi,angj) The distance d between the dotted line and the endpoint, the distance L between the endpoint and the descriptor distance t, and the distance L from the set L is found out according to the set threshold valuemaxThe approximate line segments are compared to form a candidate line segment group L'.
Step 3.2.3: members of L' with LmaxSegment merging is performed and the descriptors are recalculated to obtain a merged segment group L ".
Step 3.2.4: calculate L ″)iThe length s of the line, and L ″)iAnd LmaxThe difference in main direction between them, and whether it is less than the set threshold value. Find and LmaxTo-be-merged line producing minimum difference in principal directionSegment L'iAnd with merge line segment L ″)iTo replace L'iAnd Lmax。
Wherein des ═ { des ═ desi}i＝1,...,nIs line feature set L ═ Li}i＝1,...,nCorresponding descriptor subset, angTHIs a main direction difference threshold, dTHIs the dot-line distance threshold,/THAs end point distance threshold, desTHTo describe the sub-distance threshold, sTHIs a segment length threshold.
Step 3.3: and performing line feature matching between the left eye image and the right eye image of the current frame.
And 4, step 4: and matching line characteristics between the front image frame and the rear image frame.
Two line segments l successfully matched1,l2The following conditions are satisfied:
the segment for LSD detection has directivity, and the included angle of the direction vectors of two matched segments is smaller than phi;
the ratio of the lengths of the two line segments
Figure BDA0002648653030000091
Calculating the length l of the overlapping area of the two sectionsoverlap，
Figure BDA0002648653030000092
The distance between the fourth step and the LBD characteristic vector is smaller than rhoTAnd is where the distance is smallest.
And 5: based on the data correlation result of the point characteristics between the previous frame and the next frame, a PNP pose estimation method is adopted to obtain the initial pose estimation of the current frame;
step 6: optimizing and adjusting the pose of the current frame by using a cost function constructed by the reprojection errors of the point characteristics and the line characteristics between the previous frame and the next frame;
the constraint relation between the point characteristics and the pose is as follows:
Pc＝[Px Py Pz]T＝RcwPw+tcw (8)
Figure BDA0002648653030000093
where Φ represents the projection model of the pinhole camera.
The reprojection error of the spatial point features is defined as follows:
Figure BDA0002648653030000101
in the above formula, (x)i,j,yi,j) And the pixel coordinates of the j-th point feature on the i-th frame image are represented.
FIG. 4 is a schematic of the reprojection error of a line feature, where sw,k,
Figure BDA0002648653030000102
Is two 3D end points of the feature of the kth line observed by the ith frame image under a world coordinate system,
Figure BDA0002648653030000103
is to mix sw,k,ew,kTwo 2D endpoints, s, re-projected onto the ith frame imagei,k,
Figure BDA0002648653030000104
Is sw,k,ew,kAt the corresponding two endpoints in the ith frame image,hsi,k,
Figure BDA0002648653030000105
are respectively si,k,ei,kCorresponding homogeneous coordinate point, ds,deRespectively representing two end points
Figure BDA0002648653030000106
Each to the re-projected line segment
Figure BDA0002648653030000107
The distance of (c).
Pi is formed by two homogeneous coordinate pointshsi,k,hei,kAnd optical center c of cameraiDetermined plane byi,kUnit normal vector representing plane pi:
Figure BDA0002648653030000108
the reprojection error of the spatial line features is defined as follows:
Figure BDA0002648653030000109
in addition, the first and second substrates are,hΦ is a homogeneous coordinate form of Φ, i.e.:
Figure BDA00026486530300001010
under the condition that the observation errors are assumed to be in Gaussian distribution, a cost function F integrating the reprojection errors of the point-line characteristics can be constructed:
Figure BDA00026486530300001011
wherein the content of the first and second substances,
Figure BDA00026486530300001012
information matrices of the reprojection errors of the point features, line features, respectively, HP,HlHuber robust kernel function, rho, of point and line features, respectivelyl,ΙlRespectively representing a point feature set and a line feature set.
Since the minimum is the quadratic sum of two norms of error terms, the growing speed is the square of the error, and if mismatching occurs, the system is optimized to the wrong value, so that the Huber kernel function is introduced to reduce the influence of the mismatching.
The specific form of the Huber kernel function is as follows:
Figure BDA0002648653030000111
the error is the sum of the reprojection errors of the point feature and the line feature, and when the value of the error is larger than a threshold value, the increasing speed of the cost function is converted from a quadratic function to a linear function, so that the influence of mismatching is reduced.
The process of minimizing the cost function F is the process of solving the state quantities to be estimated. In order to optimize the cost function by using the nonlinear optimization method, a Jacobian matrix of the error function with respect to the state variable needs to be calculated first.
When the camera generates a small pose change xi belongs to SE (3) (xi is a transformation matrix TiwCorresponding lie algebra), remember gPIs Pw,jCorresponding coordinates in the camera coordinate system, gs,geAre respectively sw,k,ew,kCorresponding coordinates under the camera coordinate system:
gP＝errorξPw,j＝RiwPw,j+tiw (15)
gs＝errorξsw,k＝Riwsw,k+tiw (16)
ge＝errorξew,k＝Riwew,k+tiw (17)
error of point feature reprojection
Figure BDA0002648653030000112
The Jacobian matrix for the small pose change ξ ∈ SE (3) of the camera is as follows:
Figure BDA0002648653030000121
wherein the content of the first and second substances,
Figure BDA0002648653030000122
Figure BDA0002648653030000123
line feature reprojection error
Figure BDA0002648653030000124
The Jacobian matrix for the small pose change ξ ∈ SE (3) of the camera is as follows:
Figure BDA0002648653030000125
wherein the content of the first and second substances,
Figure BDA0002648653030000131
Figure BDA0002648653030000132
Figure BDA0002648653030000133
in the above process, the]∧Representing conversion of an antisymmetric matrix by the following formula
Figure BDA0002648653030000134
And after calculating the Jacobian matrix, solving the optimization problem by adopting a Gauss-Newton nonlinear optimization method.
And 7: judging whether the current frame is a key frame, if so, executing the step 8; if not, setting the previous key frame as the reference key frame of the current frame, and then jumping to the step 12;
the key frame selection refers to removing redundant image frames and reserving representative image frames. When the number of the selected key frames is too large, information among the key frames is redundant, so that a large amount of calculation is brought, and the requirement on real-time performance is difficult to meet; when the number of the selected key frames is too small, the information correlation between the key frames is poor, and the camera pose is difficult to estimate correctly, so that the positioning fails. The key frame selected by the invention simultaneously meets the following conditions:
the method comprises the steps of firstly, at least 20 image frames are contained between a key frame and a previous key frame;
at least 50 point features and 15 line features are successfully tracked;
and c, the common viewpoint and line characteristic information between the three key frames is less than 75%.
According to the judgment condition of the key frame, if the current frame is not the key frame, setting the previous key frame as the reference key frame of the current frame, calculating the Relative pose relationship between the two frames, and recording the Relative pose relationship.
The pose of the current frame CurrentImage is TCBelongs to SE (3), and the pose of the reference key frame RefermImage is TRE, SE (3), the Relative pose quantity Relative is: t isRel＝TC(TR)-1。
And 8: extracting the point characteristics of the current frame, comprising the following steps:
step 8.1: extracting and describing point features of the current frame by adopting an ORB algorithm;
and establishing a pyramid for the current frame image, respectively carrying out image blocking on the pyramid image to obtain an image area with a certain size, and extracting and describing point characteristics in each block by utilizing an ORB characteristic point extraction algorithm.
Step 8.2: and carrying out sparse stereo matching on the point characteristics between the left and right eye images of the current frame in a descriptor matching mode.
And matching point characteristics between the left eye image and the right eye image according to the distance between the descriptors, and solving the 3D coordinates of the corresponding space points according to the characteristic point pairs obtained by binocular matching.
And step 9: carrying out data association of point and line characteristics among local key frames;
the local key frame herein refers to 11 key frames composed of the current frame and the previous 10 key frames. The point features between local key frames are data-related by means of descriptor matching, and it should be particularly noted that the feature points between the previous and next frames in step 2 are data-related by tracking the point features by using the KLT optical flow method.
Step 10: performing secondary optimization adjustment on the pose of the local key frames by using a cost function constructed by the re-projection errors of the point features and the line features between the local key frames;
Figure BDA0002648653030000141
wherein the content of the first and second substances,
Figure BDA0002648653030000142
information matrices of the reprojection errors of the point features, line features, respectively, HP,HlHuber robust kernel function, rho, of point and line features, respectivelyl,Ιl,κlRespectively representing a point feature set, a line feature set and a local key frame.
In step 6, a cost function is constructed according to the data association relation of the point and line characteristics between the previous frame and the next frame, so that the pose of the current frame is optimized and solved; in step 10, a new cost function is constructed again according to the data association relationship of the point and line characteristics between the local key frames, and the pose of the local key frames is optimized and adjusted by adopting a Gauss-Newton nonlinear optimization method.
Step 11: adjusting the pose of the non-key frame;
for each non-key frame, a corresponding reference key frame is set for the non-key frame, and the relative pose quantity between the non-key frame and the reference key frame is solved. In step 10, the pose of the local key frame is adjusted, so that the pose T of the local key frame RefermImage is referred toRIs optimally adjusted to TRAt' time, the poses of those non-key frames are also adjusted according to the Relative pose quantity Relative, that is: t isCIs adjusted toIs TC', wherein, TC′＝TRelTR'. By setting the reference key frame, the pose of the non-key frame is optimized and adjusted again.
Step 12: and outputting the pose of the image frame.
And (5) processing the MH _04_ difficult image sequence in a circulating manner from the step 1 to the step 12, calculating the corresponding pose of each image frame, outputting a pose result and recording the pose result in a txt document form.
And then, evaluating and analyzing the performance of the algorithm according to the pose result of the image frame recorded in the txt document.
Fig. 5 shows a comparison effect graph of the experimental result of the present algorithm on the MH _04_ differential image sequence and the real track, wherein the experimental result shows that: the algorithm can complete the positioning task on the MH _04_ differential image sequence.
In the process of positioning accuracy analysis, Absolute Pose Error (APE) is adopted as an evaluation index to evaluate the positioning accuracy of the algorithm, the APE is obtained by calculating the distance between an estimated Pose and a real Pose, and the real Pose is T at the moment iiBelongs to SE (3), and estimates the pose as Ti'. epsilon SE (3), APE is: APEi＝Ti(T′i)-1。
The positioning accuracy of the algorithm and the ORB-SLAM2 algorithm without the loop detection link on the EuRoC MH-04 _ diffucult image sequence is compared.
FIG. 6-1 is a comparison graph of the APE of the algorithm and the APE of the ORB-SLAM2 algorithm without loop detection, and for better comparison of the positioning accuracy, the positioning accuracy information in FIG. 6-2 is summarized as follows:
Figure BDA0002648653030000161
as can be seen from the above table, when an MH _04_ differential image sequence is tested, the maximum positioning error of the algorithm is 0.268466m, which is reduced by 0.046196m compared with the ORB-SLAM2 algorithm, the minimum positioning error of the algorithm is 0.011937, which is reduced by 0.000562m compared with the ORB-SLAM2 algorithm, the root mean square error of the algorithm is 0.096488m, and the positioning accuracy is improved by 28.8% compared with the ORB-SLAM2 algorithm.
The average processing time of the algorithm for each image frame is 42.837ms, about 23.3 frames/second, and compared with the data acquisition frequency of the camera at 20Hz, the algorithm can meet the real-time requirement.

Claims (4)

1. A binocular vision odometer design method based on optical flow tracking and point line feature matching is characterized by comprising the following steps:
step 1: acquiring an image by using a binocular camera, converting the image into a gray image, and then performing enhancement processing on the image by using a self-adaptive histogram equalization algorithm;
step 2: using a KLT optical flow method to track point features of a previous frame image, and establishing data association of the point features between the previous frame and the next frame by combining a bidirectional annular matching strategy:
step 2.1: tracking a left eye image of a previous frame by using a KLT optical flow method and a left eye image of a current frame, and performing tracking matching of point characteristics;
step 2.2: screening and supplementing point characteristics;
step 2.3: performing point feature tracking matching between the left eye image and the right eye image of the current frame by using a KLT optical flow method, and further calculating a three-dimensional coordinate corresponding to a matching point according to an example stereoscopic vision algorithm;
and step 3: extracting line features of the current frame:
step 3.1: extracting line features based on an LSD line feature extraction algorithm, and calculating a line feature descriptor based on an LBD line feature description algorithm;
step 3.2: performing disconnection merging operation on the extracted line characteristics;
step 3.3: performing line feature matching between the left eye image and the right eye image of the current frame;
and 4, step 4: matching line characteristics between the front frame and the rear frame;
and 5: based on the data correlation result of the point characteristics between the previous frame and the next frame, a PNP pose estimation method is adopted to obtain the initial pose estimation of the current frame;
step 6: optimizing and adjusting the pose of the current frame by using a cost function constructed by the reprojection errors of the point characteristics and the line characteristics between the previous frame and the next frame;
and 7: judging whether the current frame is a key frame, if so, executing the step 8; if not, setting the previous key frame as the reference key frame of the current frame, and then jumping to the step 12;
and 8: extracting the point characteristics of the current frame:
step 8.1: extracting and describing point features of the current frame by adopting an ORB algorithm;
step 8.2: performing sparse stereo matching on the point characteristics between the left and right eye images of the current frame in a descriptor matching mode;
and step 9: carrying out data association of point and line characteristics among local key frames;
step 10: performing secondary optimization adjustment on the pose of the local key frames by using a cost function constructed by the re-projection errors of the point features and the line features between the local key frames;
step 11: adjusting the pose of the non-key frame;
step 12: and outputting the poses of all the image frames.
2. The binocular vision odometer design method based on optical flow tracking and point line feature matching as claimed in claim 1, wherein: the bidirectional ring matching strategy in the step 2 refers to: the last frame of the left eye image
Figure FDA0002648653020000021
Feature point set in (1)
Figure FDA0002648653020000022
Left eye image of current frame by means of bidirectional KLT optical flow method
Figure FDA0002648653020000023
In-tracking matching to a feature point set
Figure FDA0002648653020000024
The current frame left eye image
Figure FDA0002648653020000025
Feature point set in (1)
Figure FDA0002648653020000026
Right eye image of current frame by bidirectional KLT optical flow method
Figure FDA0002648653020000027
Matching the intermediate tracking to a temporary feature point set X1; the current frame of the right eye image
Figure FDA0002648653020000028
The temporary feature point set X1 in the previous frame is a right eye image by means of a bidirectional KLT optical flow method
Figure FDA0002648653020000029
Matching the intermediate tracking to a temporary feature point set X2; judging whether the characteristic points in the temporary characteristic point set X2 fall into the original characteristic point set or not
Figure FDA00026486530200000210
The original feature points in the image are deleted from X2 for the feature points falling outside the domain range, and those feature points corresponding to the deleted feature points in X2 are deleted from X1 according to the matching relationship between X1 and X2, thereby obtaining the right image of the current frame
Figure FDA00026486530200000211
Feature point set in (1)
Figure FDA00026486530200000212
3. The binocular vision mileage based on optical flow tracking and point line feature matching as claimed in claim 1The design method is characterized by comprising the following steps: in the step 4, two line segments l successfully matched1,l2The following conditions are satisfied:
(1) the line segments detected by the LSD have directivity, and the included angle of the direction vectors of the two matched line segments is less than phi;
(2) ratio of the lengths of two line segments
Figure FDA0002648653020000031
(3) Calculating the length l of the two-segment overlapping regionoverlap，
Figure FDA0002648653020000032
(4) The distance corresponding to the LBD eigenvector is less than rhoTAnd is where the distance is smallest.
4. The binocular vision odometer design method based on optical flow tracking and point line feature matching as claimed in claim 1, wherein: in step 7, the key frame is determined according to the following principle: the following conditions are also satisfied:
(1) at least 20 image frames are contained between the previous key frame and the previous key frame;
(2) at least 50 point features and 15 line features are successfully tracked;
(3) the co-viewpoint, line feature information with the last key frame is less than 75%.
CN202010862610.8A 2020-08-25 2020-08-25 Binocular vision odometer design method based on optical flow tracking and point line feature matching Pending CN112115980A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010862610.8A CN112115980A (en) 2020-08-25 2020-08-25 Binocular vision odometer design method based on optical flow tracking and point line feature matching

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010862610.8A CN112115980A (en) 2020-08-25 2020-08-25 Binocular vision odometer design method based on optical flow tracking and point line feature matching

Publications (1)

Publication Number Publication Date
CN112115980A true CN112115980A (en) 2020-12-22

Family

ID=73805591

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010862610.8A Pending CN112115980A (en) 2020-08-25 2020-08-25 Binocular vision odometer design method based on optical flow tracking and point line feature matching

Country Status (1)

Country Link
CN (1) CN112115980A (en)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112802196A (en) * 2021-02-01 2021-05-14 北京理工大学 Binocular inertia simultaneous positioning and map construction method based on dotted line feature fusion
CN112880687A (en) * 2021-01-21 2021-06-01 深圳市普渡科技有限公司 Indoor positioning method, device, equipment and computer readable storage medium
CN112991388A (en) * 2021-03-11 2021-06-18 中国科学院自动化研究所 Line segment feature tracking method based on optical flow tracking prediction and convex geometric distance
CN113012196A (en) * 2021-03-05 2021-06-22 华南理工大学 Positioning method based on information fusion of binocular camera and inertial navigation sensor
CN113376669A (en) * 2021-06-22 2021-09-10 东南大学 Monocular VIO-GNSS fusion positioning algorithm based on dotted line characteristics
CN113465617A (en) * 2021-07-08 2021-10-01 上海汽车集团股份有限公司 Map construction method and device and electronic equipment
WO2022174603A1 (en) * 2021-02-21 2022-08-25 深圳市优必选科技股份有限公司 Pose prediction method, pose prediction apparatus, and robot
WO2022179047A1 (en) * 2021-02-26 2022-09-01 魔门塔(苏州)科技有限公司 State information estimation method and apparatus
CN116610129A (en) * 2023-07-17 2023-08-18 山东优宝特智能机器人有限公司 Local path planning method and system for leg-foot robot

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106909877A (en) * 2016-12-13 2017-06-30 浙江大学 A kind of vision based on dotted line comprehensive characteristics builds figure and localization method simultaneously
CN107869989A (en) * 2017-11-06 2018-04-03 东北大学 A kind of localization method and system of the fusion of view-based access control model inertial navigation information
CN108447097A (en) * 2018-03-05 2018-08-24 清华-伯克利深圳学院筹备办公室 Depth camera scaling method, device, electronic equipment and storage medium
CN108519102A (en) * 2018-03-26 2018-09-11 东南大学 A kind of binocular vision speedometer calculation method based on reprojection
CN108537876A (en) * 2018-03-05 2018-09-14 清华-伯克利深圳学院筹备办公室 Three-dimensional rebuilding method, device, equipment based on depth camera and storage medium
US20180297207A1 (en) * 2017-04-14 2018-10-18 TwoAntz, Inc. Visual positioning and navigation device and method thereof
CN108986037A (en) * 2018-05-25 2018-12-11 重庆大学 Monocular vision odometer localization method and positioning system based on semi-direct method
CN109558879A (en) * 2017-09-22 2019-04-02 华为技术有限公司 A kind of vision SLAM method and apparatus based on dotted line feature
CN109579840A (en) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 A kind of close coupling binocular vision inertia SLAM method of dotted line Fusion Features
CN109934862A (en) * 2019-02-22 2019-06-25 上海大学 A kind of binocular vision SLAM method that dotted line feature combines
CN110132302A (en) * 2019-05-20 2019-08-16 中国科学院自动化研究所 Merge binocular vision speedometer localization method, the system of IMU information
CN110490085A (en) * 2019-07-24 2019-11-22 西北工业大学 The quick pose algorithm for estimating of dotted line characteristic visual SLAM system
CN110570453A (en) * 2019-07-10 2019-12-13 哈尔滨工程大学 Visual odometer method based on binocular vision and closed-loop tracking characteristics
CN111445526A (en) * 2020-04-22 2020-07-24 清华大学 Estimation method and estimation device for pose between image frames and storage medium

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106909877A (en) * 2016-12-13 2017-06-30 浙江大学 A kind of vision based on dotted line comprehensive characteristics builds figure and localization method simultaneously
US20180297207A1 (en) * 2017-04-14 2018-10-18 TwoAntz, Inc. Visual positioning and navigation device and method thereof
CN109558879A (en) * 2017-09-22 2019-04-02 华为技术有限公司 A kind of vision SLAM method and apparatus based on dotted line feature
CN107869989A (en) * 2017-11-06 2018-04-03 东北大学 A kind of localization method and system of the fusion of view-based access control model inertial navigation information
CN108447097A (en) * 2018-03-05 2018-08-24 清华-伯克利深圳学院筹备办公室 Depth camera scaling method, device, electronic equipment and storage medium
CN108537876A (en) * 2018-03-05 2018-09-14 清华-伯克利深圳学院筹备办公室 Three-dimensional rebuilding method, device, equipment based on depth camera and storage medium
CN108519102A (en) * 2018-03-26 2018-09-11 东南大学 A kind of binocular vision speedometer calculation method based on reprojection
CN108986037A (en) * 2018-05-25 2018-12-11 重庆大学 Monocular vision odometer localization method and positioning system based on semi-direct method
CN109579840A (en) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 A kind of close coupling binocular vision inertia SLAM method of dotted line Fusion Features
CN109934862A (en) * 2019-02-22 2019-06-25 上海大学 A kind of binocular vision SLAM method that dotted line feature combines
CN110132302A (en) * 2019-05-20 2019-08-16 中国科学院自动化研究所 Merge binocular vision speedometer localization method, the system of IMU information
CN110570453A (en) * 2019-07-10 2019-12-13 哈尔滨工程大学 Visual odometer method based on binocular vision and closed-loop tracking characteristics
CN110490085A (en) * 2019-07-24 2019-11-22 西北工业大学 The quick pose algorithm for estimating of dotted line characteristic visual SLAM system
CN111445526A (en) * 2020-04-22 2020-07-24 清华大学 Estimation method and estimation device for pose between image frames and storage medium

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
QI NAIXIN ET AL: "An ORB Corner Tracking Method Based on KLT", 《INTERNATIONAL CONFERENCE ON MECHATRONICS AND INTELLIGENT ROBOTICS》 *
SANG JUN LEE ET AL: "Elaborate Monocular Point and Line SLAM with Robust Initialization", 《2019 IEEE/CVF INTERNATIONAL CONFERENCE ON COMPUTER VISION》 *
ZHANHAI YU ET AL: "The inter-frame feature matching and tracking of binocular vision based on the ORB-PyrLK algorithm", 《2017 36TH CHINESE CONTROL CONFERENCE》 *
何汉武等: "《增强现实交互方法与实现》", 31 December 2018 *
吴荻: "基于立体视觉里程计的地下铲运机定位技术研究", 《中国博士学位论文全文数据库 工程科技Ⅰ辑》 *
杨冬冬等: "基于局部与全局优化的双目视觉里程计算法", 《计算机工程》 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112880687A (en) * 2021-01-21 2021-06-01 深圳市普渡科技有限公司 Indoor positioning method, device, equipment and computer readable storage medium
CN112802196A (en) * 2021-02-01 2021-05-14 北京理工大学 Binocular inertia simultaneous positioning and map construction method based on dotted line feature fusion
CN112802196B (en) * 2021-02-01 2022-10-21 北京理工大学 Binocular inertia simultaneous positioning and map construction method based on dotted line feature fusion
WO2022174603A1 (en) * 2021-02-21 2022-08-25 深圳市优必选科技股份有限公司 Pose prediction method, pose prediction apparatus, and robot
WO2022179047A1 (en) * 2021-02-26 2022-09-01 魔门塔(苏州)科技有限公司 State information estimation method and apparatus
CN113012196A (en) * 2021-03-05 2021-06-22 华南理工大学 Positioning method based on information fusion of binocular camera and inertial navigation sensor
CN112991388A (en) * 2021-03-11 2021-06-18 中国科学院自动化研究所 Line segment feature tracking method based on optical flow tracking prediction and convex geometric distance
CN113376669A (en) * 2021-06-22 2021-09-10 东南大学 Monocular VIO-GNSS fusion positioning algorithm based on dotted line characteristics
CN113465617A (en) * 2021-07-08 2021-10-01 上海汽车集团股份有限公司 Map construction method and device and electronic equipment
CN113465617B (en) * 2021-07-08 2024-03-19 上海汽车集团股份有限公司 Map construction method and device and electronic equipment
CN116610129A (en) * 2023-07-17 2023-08-18 山东优宝特智能机器人有限公司 Local path planning method and system for leg-foot robot
CN116610129B (en) * 2023-07-17 2023-09-29 山东优宝特智能机器人有限公司 Local path planning method and system for leg-foot robot

Similar Documents

Publication Publication Date Title
CN112115980A (en) Binocular vision odometer design method based on optical flow tracking and point line feature matching
CN112435325B (en) VI-SLAM and depth estimation network-based unmanned aerial vehicle scene density reconstruction method
Strasdat et al. Double window optimisation for constant time visual SLAM
Kang et al. Detection and tracking of moving objects from a moving platform in presence of strong parallax
CN112785702A (en) SLAM method based on tight coupling of 2D laser radar and binocular camera
CN110570453B (en) Binocular vision-based visual odometer method based on closed-loop tracking characteristics
CN111829484B (en) Target distance measuring and calculating method based on vision
CN110599545B (en) Feature-based dense map construction system
CN111340922A (en) Positioning and mapping method and electronic equipment
CN112419497A (en) Monocular vision-based SLAM method combining feature method and direct method
CN113744315B (en) Semi-direct vision odometer based on binocular vision
CN110570474B (en) Pose estimation method and system of depth camera
CN114708293A (en) Robot motion estimation method based on deep learning point-line feature and IMU tight coupling
CN116449384A (en) Radar inertial tight coupling positioning mapping method based on solid-state laser radar
JP6922348B2 (en) Information processing equipment, methods, and programs
CN111860651A (en) Monocular vision-based semi-dense map construction method for mobile robot
Zhu et al. PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios
CN112945233A (en) Global drift-free autonomous robot simultaneous positioning and map building method
CN112731503A (en) Pose estimation method and system based on front-end tight coupling
WO2023130842A1 (en) Camera pose determining method and apparatus
Shao A Monocular SLAM System Based on the ORB Features
CN116147618A (en) Real-time state sensing method and system suitable for dynamic environment
CN114723811A (en) Stereo vision positioning and mapping method for quadruped robot in unstructured environment
CN113011212B (en) Image recognition method and device and vehicle
CN113888603A (en) Loop detection and visual SLAM method based on optical flow tracking and feature matching

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
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20201222

WD01 Invention patent application deemed withdrawn after publication