A camera pose determination method based on visual SLAM

Abstract

The present invention provides a camera pose obtaining method based on visual SLAM, including: step 1: adjusting the image point feature extraction threshold of the current frame according to the image point feature utilization rate of the previous frame; step 2: predicting the common view range of two adjacent frames; step 3: judging whether the center coordinates of each small image block in the current frame are within the common view range; if within the common view range, proceeding to step 4; if not within the common view range, proceeding to step 5; step 4: optimizing the point feature extraction threshold within the common view range of the current frame according to the common view ratio; step 5: performing point feature extraction on the current frame image to obtain a descriptor of the point feature of the current frame image; step 6: using the descriptor to perform feature matching on valid points to be matched to obtain matching pairs; step 7: integrating the screened matching pairs into the ORB‑SLAM system for positioning and mapping. The present invention effectively improves the positioning accuracy, robustness and real-time performance of the ORB‑SLAM system under camera motion.

Description

Technical Field

[0001] The present invention relates to the technical field of image processing, and in particular to a camera posture obtaining method based on visual SLAM. Background Art

[0002] In recent years, thanks to the rapid development of science and technology in my country, autonomous mobile robots, drones, smart cars, and smart products and devices such as AR\VR are gradually being integrated into people's production and life. The Simultaneous Localization and Mapping (SLAM) technology behind it has gradually become a research hotspot. Among them, the visual SLAM algorithm is a research hotspot of SLAM technology due to its advantages such as low power consumption, low cost, and outstanding theoretical and application value. Among them, ORB-SLAM is one of the representative systems in modern visual SLAM systems, and subsequent scholars have proposed many excellent improvement methods based on this.

[0003] For example, the Chinese invention patent with publication number CN108305278A discloses "a method for improving the correlation of image matching in an ORB-SLAM algorithm". This invention optimizes the polar line search matching range through the inverse deep Gaussian method, and introduces the normalized cross correlation (NCC) to measure the correlation between the points to be matched, which effectively improves the positioning accuracy of semi-dense ORB-SLAM. The Chinese invention patent with publication number CN110838145A discloses "a visual positioning and mapping of indoor dynamic scenes", which takes into account the impact of dynamic targets in real environments on the SLAM system, improves the calculation efficiency of the basic matrix by improving the random sample consensus (RANSAC) algorithm, and filters out erroneous matching points through polar line constraints, effectively improving the positioning accuracy of the ORB-SLAM algorithm in complex dynamic environments. The Chinese invention patent with publication number CN111044054A discloses "a method for posture optimization based on monocular SLAM algorithm using Kalman filtering". The invention solves the rotation vector and translation vector by establishing a Kalman filtering model, and uses the Kalman filtering method to replace the reprojection error optimization algorithm in the ORB-SLAM system, effectively improving the real-time performance of the system.

[0004] However, when improving the above methods, the impact of the camera's own motion on the visual SLAM system is not taken into account, resulting in insufficient positioning accuracy and robustness of the ORB-SLAM system when the camera is in motion. Summary of the invention

[0005] In view of the accuracy and robustness problems caused by the ORB-SLAM system using a fixed point feature extraction and matching strategy under different motion states, the present invention provides a camera pose acquisition method based on visual SLAM, which aims to improve the accuracy of camera pose information and the speed of acquiring camera pose information, thereby ensuring the real-time performance and drift of the system and solving the system accuracy and robustness under long-term motion states.

[0006] A camera pose obtaining method based on visual SLAM comprises the following steps:

[0007] Step 1: Adjust the image point feature extraction threshold of the current frame according to the image point feature utilization of the previous frame to ensure the real-time, accuracy and stability of the system;

[0008] Step 2: Predict the common view range between two adjacent frames through the uniform acceleration motion model;

[0009] Step 3: Determine whether the center coordinates of each small image block in the current frame are within the common viewing range; if they are within the common viewing range, proceed to step 4; if they are not within the common viewing range, proceed to step 5;

[0010] Step 4: Determine the common view range according to the common view range, and optimize the point feature extraction threshold within the common view range of the current frame according to the common view ratio, so as to ensure the real-time, accuracy and stability of the system;

[0011] Step 5: extract point features of the current frame image according to the optimized point feature extraction threshold, and use the BRIEF algorithm to obtain a descriptor of the point features of the current frame image, which is used for feature matching;

[0012] Step 6: Determine the valid matching points within the common view range through the uniform acceleration motion model, and narrow the matching search range in combination with the image pyramid, and then use the descriptor to perform feature matching on the valid matching points after narrowing the matching search range to obtain matching pairs;

[0013] Step 7: Filter the matching pairs, and adjust the camera pose by integrating the filtered matching pairs into the ORB-SLAM system for positioning and mapping.

[0014] Furthermore, in the above-mentioned camera pose obtaining method based on visual SLAM, the adjustment method in step 1 is:

[0015] When the feature utilization rate of the image point of the previous frame is lower than a certain threshold, the feature extraction threshold of the image point of the current frame is increased to ensure the real-time performance and accuracy of the system;

[0016] When the utilization rate of the image point features of the previous frame is higher than a certain threshold, the image point feature extraction threshold of the current frame is reduced to ensure the accuracy and stability of the system.

[0017] Furthermore, in the above-mentioned camera pose obtaining method based on visual SLAM, the point feature utilization rate R e The expression is:

[0018] R e =N effect / N all (1)

[0019] Where: N all is the sum of all feature points in the frame image; N effect is the sum of the number of effective point features of the frame image; the conditions that the effective point features should satisfy are:

[0020] 1) Positive depth value: When a depth camera is used, the depth value corresponding to the feature point on the depth map is positive. When a binocular camera is used, the depth value calculated after binocular stereo matching is positive.

[0021] 2) The point feature has one or more matching relationships;

[0022] The matching relationship includes: a matching relationship established with the previous frame, a matching relationship established with the previous reference key frame, and a matching relationship established with a local map point during local map optimization;

[0023] 3) The point feature is not an outlier: After the matching relationship is established for each frame image, a pose estimation optimization will be performed. During the optimization process, the reprojection error greater than the error threshold will be eliminated, and the point feature will be marked as an outlier.

[0024] Furthermore, in the above-mentioned method for obtaining camera pose based on visual SLAM, the step 2 comprises:

[0025] The image vertices at time t-1 are converted to the image coordinate system at time t, so that the image vertices of two adjacent frames are located in the same coordinate system. The problem of obtaining the common viewing range is then converted into obtaining the intersection of two polygons. The vertex coordinates of the common viewing range are determined by obtaining the intersection of the two polygons, and the vertex coordinates of the common viewing range are sequentially connected to determine the common viewing range.

[0026] Furthermore, in the above-mentioned method for obtaining camera pose based on visual SLAM, the step 2 comprises the following steps:

[0027] Step 21: Convert the image vertices at time t-1 to the image coordinate system at time t; assuming that the rotation matrix R' and the translation t' from time t-2 to time t-1 are known, obtain the projection coordinates on the image coordinate system at time t according to the camera's projection model:

[0028] p i =K·R′·K -1 p i ′+K·t′ (2)

[0029] Where: K is the camera internal parameter matrix; p i ' is a vertex of an image at time t-1; p i is the corresponding pixel coordinate at time t;

[0030] Step 22: Put the following coordinate points into set Q: the projection vertex at time t that is within the image range at time t-1; the image vertex at time t-1 that is within the image range at time t; the intersection of two polygons. At this time, the coordinate points in set Q are unordered points.

[0031] Among them, finding the intersection of polygons is actually finding the intersection of each edge between two polygons, which can be further extended to finding the intersection between any two line segments; assuming that the coordinates of the endpoint corresponding to line segment l1 are expressed in the form of a vector as p = [x, y] T and p+r=[x',y']. Similarly, the endpoints of line segment l2 are q=[z,w] and q+s=[z',w']. Then any point on the line segment is expressed as:

[0032]

[0033] If two line segments intersect, then:

[0034]

[0035] If s^r≠0, and t∈[0,1], there is only one intersection point, which is represented by p+tr; if s^r=0, and (pq)^r=0, the two line segments overlap and there are countless intersection points, then the four collinear line segment vertices are arranged in ascending order according to the coordinate x value, and the second to last point is taken as the vertex coordinate of the common view range;

[0036] Step 23: sort the unordered points by constructing vector angles, determine the connection order of the intersection points of the two polygons according to the angle, connect the intersection points into a closed polygon according to the vertex coordinates of the common viewing range and the connection order, and the closed polygon is the common viewing range between the previous and next frames; the coordinates of the center of gravity o' in the set Q are:

[0037]

[0038] And make a vector o'x' parallel to the x-axis with the center of gravity, and the center of gravity of the points in the set Q forms a vector o'p i And calculate the angle with the vector o'x'.

[0039] Furthermore, in the above-mentioned method for obtaining camera pose based on visual SLAM, the step 4 comprises the following steps:

[0040] Step 41: The common viewing area between two adjacent frames / the total area of ​​the current frame is used as the common viewing ratio. The calculation formula of the common viewing area S is as follows;

[0041]

[0042] Where (x i ,y i ) and (x i+1 ,y i+1 ) are the coordinates of any two points in the set Q;

[0043] Step 42: Optimizing the point feature extraction threshold within the common viewing range according to the common viewing ratio; when the common viewing ratio is greater than the set common viewing ratio threshold s set When , the image point feature extraction threshold within the common viewing range is increased, otherwise it is reduced; the adjustment ratio is adjusted according to formula (8); the image point feature extraction threshold expression is:

[0044]

[0045] Where: R e 'With R e are respectively the point feature utilization rate of the previous frame image and the point feature utilization rate set by the present invention; T * is the point feature detection threshold of the current frame; ρ(r) function completes the conversion between the regulator output signal and the detection threshold; h(s) is the detection threshold optimization function within the common view range: T is the point feature extraction threshold of the previous frame; k p is the first tuning parameter, k d is the second tuning parameter set, e(k) is the difference between the expected point feature rate and the point feature utilization rate R' of the previous frame;

[0046]

[0047] Where: S mut and s are the common viewing area and the common viewing ratio of the current frame respectively; s set is the common view ratio threshold.

[0048] Furthermore, in the above-mentioned method for obtaining camera pose based on visual SLAM, step six comprises the following steps:

[0049] Step 61: Determine valid points to be matched within the common viewing range through a uniform acceleration motion model;

[0050] Knowing the pose transformation matrix T' from time t-2 to time t-1, the current pose matrix T predicted by the uniform acceleration motion model is cw for:

[0051]

[0052] Therefore, according to the pose matrix T at time t cw and the pose matrix T at time t-1 lw , project the point features at time t-1 to the current frame:

[0053]

[0054] Where: R lw and t lw They are the pose matrices T lw The rotation matrix and translation in; R and t are the pose matrices T cw The rotation matrix and translation in [u i ',v i '] T is the camera pixel coordinate of the point feature at time t-1; p wi is the three-dimensional space coordinate of the point feature in the world coordinate system; p ci is the three-dimensional space coordinate of the point feature observed by the camera at the current time t; p i is the pixel coordinate of the point feature at the current time t; f x 、f y 、c x 、c y They are the intrinsic parameters of the camera respectively;

[0055] By judging p i Whether the coordinates are within the image range at time t can quickly determine whether the point feature is a valid point to be matched within the common view range;

[0056] Step 62: using the image pyramid to narrow the matching search range of the valid points to be matched;

[0057] According to the pose matrix T lw and T cw , derive the camera translation t at time t relative to time t-1 lc for:

[0058]

[0059] By comparing tlc The camera motion state is determined by the size relationship between the third-dimensional data and the camera baseline. If the camera moves forward, a matching search is performed between the current layer and the highest layer of the image pyramid; if the camera moves backward, a matching search is performed between the lowest layer and the current layer of the image pyramid.

[0060] Step 63: using the feature descriptor to perform feature matching on the valid to-be-matched points after the matching search range is narrowed, so as to obtain a matching pair;

[0061] The motion model predicts the approximate position of the valid matching point at time t. When performing matching search, this can be used to reduce a large number of useless matching search areas. A circle with a radius of r is made with the predicted pixel coordinates as the center. The range covered by the circle is the matching search area. The expression of the radius r is:

[0062] r=c·scale x (12)

[0063] Where: scale = 1.2 is the scaling factor of the image pyramid in this paper; c = 7 is the radius base value; x is the image pyramid level of the valid matching point;

[0064] According to the definition of the image pyramid, the higher the scale, the smaller the image ratio, and the sparser the point features contained in the layer. Therefore, it is necessary to expand the search radius to ensure the number of point features to be matched, thereby ensuring the stability of the system; conversely, the lower the scale, the denser the point features contained in the layer, and it is necessary to reduce the search radius to reduce the number of point features to be matched, thereby ensuring the real-time matching.

[0065] Furthermore, in the above-mentioned method for obtaining camera pose based on visual SLAM, the step seven includes: when performing matching screening, firstly adopting the nearest neighbor matching method to perform matching screening, and the screening discriminant is:

[0066] d / d′<L,L∈(0,1) (13)

[0067] Where: d, d' are the best and second best matching distances respectively; L = 0.75 is the setting threshold.

[0068] Beneficial effects;

[0069] 1) Effectively improve the positioning accuracy and robustness of the ORB-SLAM system under camera motion state: Since the present invention adopts steps 2 to 5 to optimize point feature extraction, the point feature extraction threshold can be adjusted in time according to the camera motion state. When the system has a large motion amplitude and is prone to tracking loss, the number of point features is increased by lowering the detection threshold, thereby ensuring the robustness of the system; conversely, the detection threshold is increased to improve the quality of the point features, thereby ensuring the real-time and accuracy of the system.

[0070] 2) The real-time performance of the ORB-SLAM system is ensured when the camera is in motion: Since the present invention adopts steps 6 and 7 to perform point feature matching optimization, the valid points to be matched and the corresponding matching search range can be quickly determined according to the motion state of the camera. By avoiding a large number of invalid points to be matched and matching search areas, the amount of system calculation is reduced and the processing time of the system is shortened.

[0071] 3) The present invention takes into account the impact of the camera's own motion on the visual SLAM system, so that the ORB-SLAM system can be better adapted to long-term operation in complex large-scale environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a flow chart of a camera pose obtaining method based on visual SLAM proposed by the present invention;

[0073] Figure 2 It is a schematic diagram of the common viewing range of images under different posture changes;

[0074] Where: O, O' are the camera optical centers of the previous frame and the current frame respectively; the solid line is the viewing angle range of the previous frame, the dotted line is the viewing angle range of the current frame, and the shaded part is the common viewing range between the two frames;

[0075] Figure 3 It is a comparison chart of the absolute trajectory error operation results of the improved method of the present invention and the ORB-SLAM system under the fr1 / room data set;

[0076] Wherein: Figure (a) is the estimation result of the present invention; Figure (b) is the estimation result of the ORB-SLAM system; Figure (c) is the displacement decomposition diagram of the estimated trajectory and the true trajectory; Figure (d) is the angle decomposition diagram of the estimated trajectory and the true trajectory; Figure (e) is the absolute trajectory error data box plot; Figure (f) is the absolute trajectory error data statistical comparison diagram;

[0077] Figure 4 It is a comparison chart of trajectory estimation between the improved method of the present invention and the ORB-SLAM system under the fr1 / desk data set;

[0078] Figure 5It is a comparison chart of the absolute trajectory error data curves of the improved method of the present invention and the ORB-SLAM system under the fr1 / desk data set. DETAILED DESCRIPTION

[0079] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0080] The embodiment of the present invention provides a method for obtaining a camera posture based on visual SLAM, comprising the following steps:

[0081] Step 1: According to the utilization rate of the image point features of the previous frame (time t-1), the image point feature extraction threshold of the current frame (time t) is adjusted to ensure the real-time, accuracy and stability of the system;

[0082] The specific adjustment method is: when the point feature utilization rate is low, it means that there are many invalid point features extracted. The detection threshold should be increased to improve the quality of point features, so as to ensure the real-time and accuracy of the system; when the point feature utilization rate is high, it means that most of the point features are used, and tracking loss is likely to occur. The detection threshold should be lowered to increase the number of point features, so as to ensure the stability and robustness of the system.

[0083] The point feature utilization rate R e The expression is:

[0084] R e =N effect / N all (1)

[0085] Where: N all is the sum of all feature points in the frame image; N effect is the sum of the number of effective point features of the frame image; the conditions that the effective point features should satisfy are:

[0086] 1) Positive depth value: When a depth camera is used, the depth value corresponding to the feature point on the depth map is positive. When a binocular camera is used, the depth value calculated after binocular stereo matching is positive.

[0087] 2) The point feature has one or more matching relationships;

[0088] The matching relationship includes: a matching relationship established with the previous frame, a matching relationship established with the previous reference key frame, and a matching relationship established with a local map point during local map optimization;

[0089] 3) The point feature is not an outlier: After the matching relationship is established for each frame image, a pose estimation optimization will be performed. During the optimization process, the reprojection error greater than the error threshold will be eliminated, and the point feature will be marked as an outlier.

[0090] Step 2: Predict the common viewing range between two adjacent frames (the two frames at time t-1 and time t) through the uniform acceleration motion model;

[0091] Specifically, the four vertices of the previous frame are first projected into the current frame, thereby converting the common view problem into finding the intersection (overlapping area) of two quadrilaterals. To find the overlapping area of ​​the two quadrilaterals, all the vertices of the overlapping area can be found first, and the vertices are connected in sequence. The polygon enclosed is the overlapping area. The vertices of the overlapping area are the points on the edges of both quadrilaterals. These vertices include: the vertices of the previous frame are within the range of the current frame (these vertices meet the condition of belonging to both quadrilaterals at the same time), the current frame is within the range of the previous frame after projection, and the intersection of the two quadrilaterals.

[0092] The vertex coordinates of the overlapping area are obtained, but they are disordered, so they need to be connected in a certain order (for example, you now have four vertices of a quadrilateral, but it is impossible to get a quadrilateral by connecting them randomly. Only by connecting them in a clockwise or counterclockwise order can you get a regular quadrilateral).

[0093] The specific method for obtaining the common viewing area includes the following steps:

[0094] The image vertices at time t-1 are converted to the image coordinate system at time t, so that the image vertices of two adjacent frames are located in the same coordinate system. The problem of obtaining the common viewing range is then converted into obtaining the intersection of two polygons. The vertex coordinates of the common viewing range are determined by obtaining the intersection of the two polygons, and the vertex coordinates of the common viewing range are sequentially connected to determine the common viewing range.

[0095] Specifically: The camera pose T at time t-2 is known t-2 And the camera pose T at time t-1 t-1 , the equation contained in it is T t-2 ×T a =T t-1 , T can be deduced from this formula a , where T a This is the so-called acceleration. Now we need to perform feature matching between the images at time t-1 and time t, and then use the algorithm to obtain the camera changes at time t-1 and time t, so as to obtain T t , that is, equation T t =T t-1×T a ', but currently only T is known t-1 The present invention assumes that the camera is in uniform accelerated motion in a very short period of time (the time interval between two adjacent frames is about 16ms), so that a To estimate the position of the current frame (this position is inaccurate, but has a certain predictive effect), that is, T t =T t-1× T a . Thus, the common view range at time t-1 and time t is calculated. However, before feature matching (matching is performed after feature points are extracted), it is assumed that the camera is moving with uniform acceleration in a very short time (the time interval between two adjacent frames is about 16ms), thus obtaining a hypothetical.

[0096] Step 21: Convert the image vertices at time t-1 to the image coordinate system at time t; assuming that the rotation matrix R' and the translation t' from time t-2 to time t-1 are known, obtain the projection coordinates on the image coordinate system at time t according to the camera's projection model:

[0097] p i =K·R′·K -1 p i ′+K·t′ (2)

[0098] Where: K is the camera internal parameter matrix; p i ' is a vertex of an image at time t-1; p i is the corresponding pixel coordinate at time t;

[0099] Step 22: After the two frames of images are placed at the same coordinates in step 21, the following coordinate points are put into the set Q: the projection vertex at time t that is within the image range at time t-1; the image vertex at time t-1 that is within the image range at time t-1; the intersection of the two polygons. At this time, the coordinate points in the set Q are unordered points.

[0100] Among them, finding the intersection of polygons is actually finding the intersection of each edge between two polygons, which can be further extended to finding the intersection between any two line segments; assuming that the coordinates of the endpoint corresponding to line segment l1 are expressed in the form of a vector as p = [x, y] T and p+r=[x',y']. Similarly, the endpoints of line segment l2 are q=[z,w] and q+s=[z',w']. Then any point on the line segment is expressed as:

[0101]

[0102] If two line segments intersect, then:

[0103]

[0104] If s^r≠0, and t∈[0,1], there is only one intersection point, which is represented by p+tr; if s^r=0, and (pq)^r=0, the two line segments overlap and there are countless intersection points. Then, after arranging the four collinear line segment vertices in ascending order according to the coordinate x value, the second to last point is taken as the final common view intersection point;

[0105] Step 23: sort the unordered points by constructing vector angles, determine the connection order of the intersection points of the two polygons according to the angle, connect the intersection points into a closed polygon according to the vertex coordinates of the common viewing range and the connection order, and the closed polygon is the common viewing range between the previous and next frames; the coordinates of the center of gravity o' in the set Q are:

[0106]

[0107] And make a vector o'x' parallel to the x-axis with the center of gravity, and the center of gravity of the points in the set Q forms a vector o'p i The angle between the points and the vector o'x' is calculated, and the connection order of the points is determined according to the size of the angle, so that the points are connected to form a closed polygon, which is the common viewing range between the previous and next frames.

[0108] Through step 21 and step 22, the vertex coordinates of the overlapping area are obtained, but they are disordered vertex coordinates, so they need to be connected in a certain order. Step 23 is to complete the sequential connection. The points are sorted in a counterclockwise direction, so that a closed polygon can be formed.

[0109] Step 3: Determine whether the center coordinates of each small image block (the small image blocks targeted by the images of the two frames at time t-2 and time t-1) in the current frame (time t) are within the common viewing range; if they are within the common viewing range, proceed to step 4; if not, proceed to step 5;

[0110] Specifically, because the point features at time t-1 have been extracted, what needs to be extracted now is the image of the current frame (time t). By extracting the point features of the current frame (time t), matching the features with the previous frame (t-1), and then using the algorithm to obtain an accurate posture conversion relationship between the current frame and the previous frame, an accurate current frame posture is obtained based on the known previous frame posture.

[0111] Step 4: Determine the common view range according to the common view range, and optimize the point feature extraction threshold within the common view range of the current frame (time t) according to the common view ratio, so as to ensure the real-time, accuracy and stability of the system;

[0112] Specifically, the point features at time t-1 have been extracted, and what needs to be extracted now is the image of the current frame (time t). By extracting the point features of the current frame (time t), matching the features with the previous frame (t-1), and then using the algorithm to obtain an accurate posture conversion relationship between the current frame and the previous frame, an accurate current frame posture is obtained based on the known previous frame posture. Therefore, what needs to be optimized is the point feature extraction threshold of the current frame.

[0113] Among them, optimizing the point feature extraction threshold within the common view range of the current frame includes: first, the area of ​​the closed polygon needs to be calculated, and then the area of ​​the polygon / total area is the common view ratio (the percentage of overlap). After the common view ratio is calculated, it is compared with the set common view ratio s set By comparison, when the co-viewing ratio (i.e., the co-viewing area between two adjacent frames / the total area of ​​the current frame) is greater than the set value (s in Table 1), set ) increases the image point feature extraction threshold within the common viewing range, otherwise it decreases. That is, when the common viewing range is large, the point feature extraction threshold within the common viewing range is increased on the basis of the point feature threshold in step 1, thereby improving the point feature quality and ensuring the real-time and accuracy of the system; when the common viewing range is small, the point feature extraction threshold within the common viewing range is reduced on the basis of the point feature threshold in step 1, increasing the number of point features and ensuring the tracking stability of the system. The adjustment ratio is adjusted according to formula (8). The specific calculation method is as follows:

[0114] Step 41: The common viewing area between two adjacent frames / the total area of ​​the current frame is used as the common viewing ratio. The calculation formula of the common viewing area S is as follows;

[0115]

[0116] Where (x i ,y i ) and (x i+1 ,y i+1 ) are the coordinates of any two points in the set Q obtained in step 22.

[0117] Step 42: When the common viewing ratio is greater than the set common viewing ratio threshold s set When , the image point feature extraction threshold within the common view range is increased, otherwise it is reduced; the image point feature extraction threshold expression is:

[0118]

[0119] Where: R e 'With R e are respectively the point feature utilization rate of the previous frame image and the point feature utilization rate set by the present invention; T *is the point feature detection threshold of the current frame; ρ(r) function completes the conversion between the regulator output signal and the detection threshold; h(s) is the detection threshold optimization function within the common view range: T is the point feature extraction threshold of the previous frame; k p The first tuning parameter to be set (k in Table 1 p ), k d The second setting parameter (k in Table 1) d ), e(k) is the expected point feature rate (R in Table 1 e ) and the difference between the feature utilization rate R' of the previous frame;

[0120]

[0121] Where: S mut and s are the common viewing area and the common viewing ratio of the current frame respectively; s set is the common view ratio threshold.

[0122] Step 5: Extract point features of the current frame image according to the optimized point feature extraction threshold, and use the BRIEF algorithm to obtain the descriptor of the point features for direct matching of feature points;

[0123] Specifically, after adjusting the point feature detection threshold of the current frame according to the point feature utilization of the previous frame, the common view range between two adjacent frames is predicted through the camera motion model, and the point feature extraction within the range is optimized. By dynamically adjusting the point feature extraction threshold under different motion states in real time, the relationship between the quality and quantity of point features is balanced to improve the accuracy and real-time performance of the system. Step six: Determine the valid points to be matched within the common view range through the uniform acceleration motion model, and narrow the matching search range in combination with the image pyramid, and then use the descriptor to perform feature matching on the valid points to be matched after the matching search range is narrowed to obtain a matching pair.

[0124] Step 61: Determine valid points to be matched within the common viewing range through a uniform acceleration motion model;

[0125] Specifically, the purpose of this step is to determine which feature points in the previous frame may have a matching relationship with the current frame; that is, through the uniform acceleration motion model, from the known T lw The matrix gets T cw Matrix, and use T lw With T cw The matrix gets the point features of the previous frame [u i ',v i '] T Project it into the current frame and determine whether the coordinates are within the range of the current frame. If they are within the range, they are valid points to be matched. If not, it is impossible to match the point features of the current frame. The specific calculation method is as follows:

[0126] Knowing the pose transformation matrix T' from time t-2 to time t-1, the current pose matrix T predicted by the uniform acceleration motion model is cw for:

[0127]

[0128] Therefore, according to T cw and T lw The pose matrix projects the point features at time t-1 to the current frame:

[0129]

[0130] Where: R lw and t lw They are the pose matrices T lw The rotation matrix and translation in; R and t are the pose matrices T cw The rotation matrix and translation in [u i ',v i '] T is the camera pixel coordinate of the point feature at time t-1; p wi is the three-dimensional space coordinate of the point feature in the world coordinate system; p ci is the three-dimensional space coordinate of the point observed by the camera at the current time t; p i is the pixel coordinate of the point at the current time t; f x 、f y 、c x 、c y They are the intrinsic parameters of the camera respectively;

[0131] By judging p i If the coordinates are within the image range at time t, it can be quickly determined whether the point feature is a valid point to be matched within the common view range;

[0132] Step 62: using the image pyramid to narrow the matching search range of the valid points to be matched;

[0133] Specifically, using the T derived in step 51 lw With T cw Matrix, derive the camera translation t at time t relative to time t-1 lc , thereby determining the motion state of the camera. The specific calculation method is as follows:

[0134] According to the pose matrix T obtained in step 51 lw and T cw , we can deduce the camera translation t at time t relative to time t-1 lc for:

[0135]

[0136] By comparing t lc The camera motion state is judged by the size relationship between the third-dimensional data and the camera baseline. If the camera moves forward, a matching search is performed between the current layer and the highest layer of the image pyramid; if the camera moves backward, a matching search is performed between the lowest layer and the current layer of the image pyramid.

[0137] Step 63: using the feature descriptor to perform feature matching on the valid to-be-matched points after the matching search range is narrowed, so as to obtain a matching pair;

[0138] Specifically, step 63 is used to complete the matching search in which pictures (pictures determined in step 62) of the point feature of the previous frame (the valid point feature determined in step 61), so as to achieve fast and accurate feature matching. The specific calculation method is as follows:

[0139] The motion model predicts the approximate position of the valid matching point at time t. When performing matching search, this can be used to reduce a large number of useless matching search areas. A circle with a radius of r is made with the predicted pixel coordinates as the center. The range covered by the circle is the matching search area. The expression of the radius r is:

[0140] r=c·scale x (12)

[0141] Where: scale = 1.2 is the scaling factor of the image pyramid in this paper; c = 7 is the radius base value; x is the image pyramid level of the valid matching point;

[0142] According to the definition of the image pyramid, the higher the scale, the smaller the image ratio, and the sparser the point features contained in the layer. Therefore, it is necessary to expand the search radius to ensure the number of point features to be matched, thereby ensuring the stability of the system; conversely, the lower the scale, the denser the point features contained in the layer, and it is necessary to reduce the search radius to reduce the number of point features to be matched, thereby ensuring the real-time matching.

[0143] Step 7: Filter the matching pairs obtained in step 6, and integrate the filtered matching pairs into the ORB-SLAM system for positioning and mapping.

[0144] Specifically, when performing matching screening, the nearest neighbor matching method is first used for matching screening, and its screening discriminant is:

[0145] d / d′<L,L∈(0,1) (13)

[0146] Where: d, d' are the best and second-best matching distances respectively; L = 0.75 is the setting threshold. Ideally, the best matching distance reflected by a good matching relationship is significantly smaller than the second-best matching distance; so when the threshold L is larger, the number of matches is more, but the matching accuracy is lower; conversely, when the acceptance threshold is smaller, the number of matches is smaller, but the matching accuracy is higher, and the matching process is more stable.

[0147] Example:

[0148] like Figure 1 The present invention is a flow chart of a camera pose obtaining method based on visual SLAM, which specifically includes the following steps:

[0149] (1) adjusting the image point feature extraction threshold of the current frame according to the image point feature utilization rate of the previous frame;

[0150] (2) First, the common view range between two adjacent frames is predicted through the uniform acceleration motion model, and the point feature extraction threshold within the common view range is further optimized. Then, point features are extracted from the image to obtain its feature descriptor;

[0151] Specifically, this step includes the following sub-steps:

[0152] (2-1) Convert the image vertices at time t-1 to the image coordinate system at time t, so that the image vertices of two adjacent frames are in the same coordinate system, and then convert the problem of obtaining the common view range into obtaining the intersection of two polygons. The overall idea is: 1) Put the vertices of the two polygons into set S; 2) Put all the intersection points of the two polygons into set S; 3) Traverse set S, retain the points that belong to both polygons, and organize them into an ordered point set; 4) According to the ordered point set, determine the common view range and common view ratio. The schematic diagram of the common view range of the image under different posture changes is shown in the figure below. Figure 2 shown.

[0153] Assuming that the rotation matrix R' and translation t' from time t-2 to time t-1 are known, the projection coordinates on the image coordinate system at time t are obtained according to the camera's projection model:

[0154]

[0155] Where: K is the camera internal parameter matrix; p i ' is a vertex of an image at time t-1; p i is the corresponding pixel coordinate at time t.

[0156] (2-2) The points in the filtered set S include: the projected vertices at time t-1 that are within the image range at time t; the image vertices at time t that are within the projected image range at time t-1; and the intersection of two polygons.

[0157] Among them, finding the intersection of polygons is actually finding the intersection of each edge between two polygons, which can be further extended to finding the intersection between any two line segments. Assume that the endpoint coordinates corresponding to line segment l1 are expressed in the form of vectors as p = [x, y] T and p+r=[x',y']. Similarly, the endpoints of line segment l2 are q=[z,w] and q+s=[z',w']. Then any point on the line segment is expressed as:

[0158]

[0159] If two line segments intersect, then:

[0160]

[0161] If s^r≠0, and t∈[0,1], there is only one intersection point, which is represented by p+tr; if s^r=0, and (pq)^r=0, the two line segments overlap and there are countless intersection points. Then arrange the four collinear line segment vertices in ascending order according to the coordinate x value, and take the second to last point as the final common view intersection point.

[0162] (2-3) Sort the unordered points by constructing vector angles. First, calculate the coordinates of the centroid o' of the point set:

[0163]

[0164] And make a vector o'x' parallel to the x-axis with the center of gravity, and the center of gravity of the points in the point set forms a vector o'p i And calculate the angle between it and the vector o'x', and determine the connection order of the points according to the size of the angle, so as to connect them into a closed polygon.

[0165] (2-4) After determining the common view range, it is necessary to determine the degree of optimization based on the common view ratio between two adjacent frames. The present invention characterizes the common view ratio by the common view area, and the area calculation formula is:

[0166]

[0167] (2-5) In summary, the threshold expression for image point feature extraction based on motion prediction is:

[0168]

[0169] Where: R e 'With R e are the point feature utilization rate of the previous frame image and the point feature utilization rate set in this paper; T *is the point feature detection threshold of the current frame; the ρ(r) function completes the conversion between the regulator output signal and the detection threshold; h(s) is the detection threshold optimization function within the common view range:

[0170]

[0171] Where: S mut and s are the common viewing area and the common viewing ratio of the current frame respectively; s set is the common view ratio threshold.

[0172] (3) First, the uniformly accelerated motion model is used to quickly determine the valid matching points within the common view range, and the image pyramid is used to narrow the matching search range, and then feature matching is performed;

[0173] Specifically, this step includes the following sub-steps:

[0174] (3-1) Given the pose transformation matrix T' from time t-2 to time t-1, the current pose matrix T predicted by the uniform acceleration motion model is cw for:

[0175]

[0176] Thus, the point features at time t-1 are projected into the current frame:

[0177]

[0178] Where: R lw and t lw are the rotation matrix and translation at time t-1 respectively; [u i ',v i '] T is the camera pixel coordinate of the point feature at time t-1; p wi is the three-dimensional space coordinate of the point feature in the world coordinate system; p ci is the three-dimensional space coordinate of the point observed by the camera at the current time t; p i is the pixel coordinate of the point at the current time t; f x 、f y 、c x 、c y are the intrinsic parameters of the camera. i Whether the point feature is within the image range at time t, it can be quickly determined whether the point feature is a valid matching point within the common view range.

[0179] (3-2) If a point to be matched is in the nth layer of the image pyramid and has a certain pixel area. When the camera moves forward, the area occupied by the point feature in the original layer increases, and it is necessary to keep the area unchanged in a higher level of the image pyramid to achieve detection and matching of the point feature; similarly, when the camera moves backward, it is necessary to achieve detection and matching of the point feature in a lower level of the image pyramid.

[0180] Combined with formula (9), the camera translation amount t at time t relative to time t-1 is lc for:

[0181]

[0182] By comparing t lc The camera motion state is determined by the size relationship between the third dimension data and the camera baseline. If the motion is forward, a matching search is performed between the current layer and the highest layer of the image pyramid; if the motion is backward, a matching search is performed between the lowest layer and the current layer of the image pyramid.

[0183] (3-3) The motion model predicts the approximate position of the valid matching point at time t, which can be used to reduce a large number of useless matching search areas during matching search. This paper takes the predicted pixel coordinates as the center and makes a circle with a radius of r. The range covered by the circle is the matching search area, and the expression of its radius r is:

[0184] r=c·scale x (12)

[0185] Among them: scale = 1.2 is the scaling factor of the image pyramid in this paper; c = 7 is the radius base value set in this paper; x is the image pyramid level of the valid points to be matched. According to the definition of the image pyramid, the higher the scale, the smaller the image ratio, and the sparser the point features contained in the layer, so it is necessary to expand the search radius to ensure the number of point features to be matched, thereby ensuring the stability of the system; conversely, the lower the scale, the denser the point features contained in the layer, so it is necessary to reduce the search radius to reduce the number of point features to be matched, thereby ensuring the real-time matching.

[0186] (4) Screening the matching pairs obtained in step (3) and integrating the accurate matching pairs into the ORB-SLAM system for positioning and mapping;

[0187] Specifically, when performing matching screening in this step, the nearest neighbor matching method is first used for matching screening, and its screening discriminant is:

[0188] d / d′<L,L∈(0,1) (13)

[0189] Where: d, d' are the best and second-best matching distances respectively; L = 0.75 is the setting threshold. Ideally, the best matching distance reflected by a good matching relationship is significantly smaller than the second-best matching distance. Therefore, when the threshold L is larger, the number of matches is more, but the matching accuracy is lower; conversely, when the acceptance threshold is smaller, the number of matches is less, but the matching accuracy is higher, and the matching process is more stable.

[0190] At the same time, when the image rotates as a whole, the point feature rotation angle difference in the matching pair will also change accordingly. Ideally, if the image rotates as a whole, the point feature angle difference in the matching pair will also produce a corresponding rotation angle change. In actual situations, there is a certain error in the change of the point feature rotation angle difference of the matching pair, but the overall angle difference will have a uniform and consistent change. Based on this, the main rotation direction of the matching pair can be determined to filter out the wrong matching pairs that do not belong to the main rotation direction.

[0191] Experimental Results

[0192] In order to verify the performance of the improved method provided by the present invention, it is compared with the standard ORB-SLAM2 algorithm. Six public data sets on the TUM data set are used, and the detailed information is shown in Table 1. It is run on an Intel Core i5-8300H@2.30GHz computer, and the positioning accuracy of each data set is recorded. The absolute trajectory error running comparison table is shown in Table 2.

[0193] Combining the data in Table 1 and Table 2, it can be seen that the optimization effect of the improved method provided by the present invention gradually improves as the movement intensifies. The average root mean square error of the present invention is about 0.018795m, and the average root mean square error of the ORB-SLAM2 algorithm is about 0.02975m, which greatly improves the accuracy of the system. Figure 3 It can be clearly seen that the improved method provided by the present invention has been significantly improved in terms of mean square error, error mean, median error and standard deviation under long-term motion conditions, among which the root mean square error is reduced by about 46%, and the minimum error with the true trajectory is 3.522 mm.

[0194] Combination Figure 4 and Figure 5 It can be seen that the original algorithm has a large error in pose estimation after being affected by severe camera shaking, and then tracking loss occurs. The improved method provided by the present invention can effectively reduce the error fluctuation range and ensure that the overall error of the system is within an acceptable range. Even after being affected by severe shaking, the camera motion pose can still be accurately estimated, making the estimated trajectory more consistent with the real trajectory. This proves that the improved method provided by the present invention has good robustness and stability.

[0195] Table 1: Details of the data sequence used in the present invention and main parameter settings

[0196]

[0197] [Note]: The above-listed parameters can reflect the advancement and performance of the present invention to a certain extent, but they are not the optimal parameters. The performance of the optimization algorithm can be further improved by adjusting the parameters multiple times.

[0198] Table 2: Absolute trajectory error operation comparison table

[0199]

[0200] [Note]: This paper experiments each data set sequence 50 times, and compares and analyzes the experimental results with the highest frequency.

[0201] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A camera pose determination method based on visual SLAM, characterized in that: The following steps are involved: Step 1: Adjust the image point feature extraction threshold of the current frame according to the image point feature utilization of the previous frame to ensure the real-time, accuracy and stability of the system; Step 2: Predict the common view range between two adjacent frames through the uniform acceleration motion model; Step 3: Determine whether the center coordinates of each small image block in the current frame are within the common viewing range; if they are within the common viewing range, proceed to step 4; if they are not within the common viewing range, proceed to step 5; Step 4: Determine the common view range according to the common view range, and optimize the point feature extraction threshold within the common view range of the current frame according to the common view ratio, so as to ensure the real-time, accuracy and stability of the system; Step 5: extract point features of the current frame image according to the optimized point feature extraction threshold, and use the BRIEF algorithm to obtain a descriptor of the point features of the current frame image, which is used for feature matching; Step 6: Determine the valid matching points within the common view range through the uniform acceleration motion model, and narrow the matching search range in combination with the image pyramid, and then use the descriptor to perform feature matching on the valid matching points after narrowing the matching search range to obtain matching pairs; Step 7: Filter the matching pairs, and adjust the camera pose by integrating the filtered matching pairs into the ORB-SLAM system for positioning and mapping.

2. The camera pose obtaining method based on visual SLAM according to claim 1, characterized in that: The adjustment method described in step 1 is: When the utilization rate of the image point feature of the previous frame is lower than the threshold, the image point feature extraction threshold of the current frame is increased to ensure the real-time and accuracy of the system; When the image point feature utilization rate of the previous frame is higher than the threshold, the image point feature extraction threshold of the current frame is reduced to ensure the accuracy and stability of the system.

3. The camera pose obtaining method based on visual SLAM according to claim 1, characterized in that: The point feature utilization rate R e The expression is: R e =N effect N all (1) Where: N all is the sum of all feature points in the frame image; N effect is the sum of the number of effective point features of the frame image; the conditions that the effective point features should satisfy are: 1) Positive depth value: When a depth camera is used, the depth value corresponding to the feature point on the depth map is positive. When a binocular camera is used, the depth value calculated after binocular stereo matching is positive. 2) The point has one or more matching relationships; The matching relationship includes: a matching relationship established with the previous frame, a matching relationship established with the previous reference key frame, and a matching relationship established with a local map point during local map optimization; 3) The point feature is not an outlier: After the matching relationship is established for each frame image, a pose estimation optimization will be performed. During the optimization process, the reprojection error greater than the error threshold will be eliminated, and the point feature will be marked as an outlier.

4. The camera pose obtaining method based on visual SLAM according to claim 1, characterized in that: The second step comprises: The image vertices at time t-1 are converted to the image coordinate system at time t, so that the image vertices of two adjacent frames are located in the same coordinate system. The problem of obtaining the common viewing range is then converted into obtaining the intersection of two polygons. The vertex coordinates of the common viewing range are determined by obtaining the intersection of the two polygons, and the vertex coordinates of the common viewing range are sequentially connected to determine the common viewing range.

5. The camera pose obtaining method based on visual SLAM according to claim 4, characterized in that: The step 2 comprises the following steps: Step 21: Convert the image vertices at time t-1 to the image coordinate system at time t; assuming that the rotation matrix R' and the translation t' from time t-2 to time t-1 are known, obtain the projection coordinates on the image coordinate system at time t according to the camera's projection model: p i =K·R′·K -1 p i ′+K·t′(2) Where: K is the camera internal parameter matrix; p i ' is the image vertex at time t-1; p i is the corresponding pixel coordinate at time t; Step 22: Put the following coordinate points into set Q: the projection vertex at time t that is within the image range at time t-1; the image vertex at time t-1 that is within the image range at time t; the intersection of two polygons. At this time, the coordinate points in set Q are unordered points. Among them, finding the intersection of polygons is actually finding the intersection of each edge between two polygons, which can be further extended to finding the intersection between any two line segments; assuming that the coordinates of the endpoint corresponding to line segment l1 are expressed in the form of a vector as p = [x, y] T and p+r=[x',y']. Similarly, the endpoints of line segment l2 are q=[z,w] and q+s=[z',w']. Then any point on the line segment is expressed as: If two line segments intersect, then: If s^r≠0, and t∈[0,1], there is only one intersection point, which is represented by p+tr; if s^r=0, and (pq)^r=0, the two line segments overlap and there are countless intersection points, then the four collinear line segment vertices are arranged in ascending order according to the coordinate x value, and the second to last point is taken as the vertex coordinate of the common view range; Step 23: sort the unordered points by constructing vector angles, determine the connection order of the intersection points of the two polygons according to the angle, connect the intersection points into a closed polygon according to the vertex coordinates of the common viewing range and the connection order, and the closed polygon is the common viewing range between the previous and next frames; the coordinates of the center of gravity o' in the set Q are: And make a vector o'x' parallel to the x-axis with the center of gravity, and the center of gravity of the points in the set Q forms a vector o'p i And calculate the angle with the vector o'x'.

6. The camera pose obtaining method based on visual SLAM according to claim 4, characterized in that: The step 4 comprises the following steps: Step 41: The common viewing area between two adjacent frames / the total area of ​​the current frame is used as the common viewing ratio. The calculation formula of the common viewing area S is as follows; Where (x i ,y i ) and (x i+1 ,y i+1 ) are the coordinates of any two points in the set Q; Step 42: Optimizing the point feature extraction threshold within the common viewing range according to the common viewing ratio; when the common viewing ratio is greater than the set common viewing ratio threshold s set When , the image point feature extraction threshold within the common viewing range is increased, otherwise it is reduced; the adjustment ratio is adjusted according to formula (8); the image point feature extraction threshold expression is: Where: R e 'With R e are the point feature utilization rate of the previous frame image and the set point feature utilization rate; T * is the point feature detection threshold of the current frame; ρ(r) function completes the conversion between the regulator output signal and the detection threshold; h(s) is the detection threshold optimization function within the common view range: T is the point feature extraction threshold of the previous frame; k p is the first tuning parameter, k d is the second tuning parameter set, e(k) is the difference between the expected point feature rate and the point feature utilization rate R' of the previous frame; Where: S mut and s are the common viewing area and the common viewing ratio of the current frame respectively; s set is the common view ratio threshold.

7. The camera pose obtaining method based on visual SLAM according to claim 5, characterized in that: The step six comprises the following steps: Step 61: Determine valid points to be matched within the common viewing range through a uniform acceleration motion model; Knowing the pose transformation matrix T' from time t-2 to time t-1, the current pose matrix T predicted by the uniform acceleration motion model is cw for: Therefore, according to the pose matrix T at time t cw and the pose matrix T at time t-1 lw , project the point features at time t-1 to the current frame: Where: R lw and t lw They are the pose matrices T lw The rotation matrix and translation in; R and t are the pose matrices T cw The rotation matrix and translation in [u i ',v i '] T is the camera pixel coordinate of the point feature at time t-1; p wi is the three-dimensional space coordinate of the point feature in the world coordinate system; p ci is the three-dimensional space coordinate of the point feature observed by the camera at the current time t; p i is the pixel coordinate of the point feature at the current time t; f x 、f y 、c x 、c y They are the intrinsic parameters of the camera respectively; By judging p i Whether the coordinates are within the image range at time t can quickly determine whether the point feature is a valid point to be matched within the common view range; Step 62: using the image pyramid to narrow the matching search range of the valid points to be matched; According to the pose matrix T lw and T cw , derive the camera translation t at time t relative to time t-1 lc for: By comparing t lc The camera motion state is determined by the size relationship between the third-dimensional data and the camera baseline. If the camera moves forward, a matching search is performed between the current layer and the highest layer of the image pyramid; if the camera moves backward, a matching search is performed between the lowest layer and the current layer of the image pyramid. Step 63: using the feature descriptor to perform feature matching on the valid to-be-matched points after the matching search range is narrowed, so as to obtain a matching pair; The motion model predicts the approximate position of the valid matching point at time t. A circle with a radius of r is made with the predicted pixel coordinates as the center. The range covered by the circle is the matching search area. The expression of the radius r is: r=c·scale x (12) Where: scale = 1.2 is the scaling factor of the image pyramid; c = 7 is the radius base value; x is the image pyramid level of the valid point to be matched; According to the definition of the image pyramid, the higher the scale, the smaller the image ratio, and the sparser the point features contained in the layer. Therefore, it is necessary to expand the search radius to ensure the number of point features to be matched, thereby ensuring the stability of the system; conversely, the lower the scale, the denser the point features contained in the layer, and it is necessary to reduce the search radius to reduce the number of point features to be matched, thereby ensuring the real-time matching.

8. The camera pose obtaining method based on visual SLAM according to claim 1, characterized in that: The step 7 includes: when performing matching screening, firstly adopting the nearest neighbor matching method to perform matching screening, and the screening discriminant formula is: d / d′<L,L∈(0,1) (13) Where: d, d' are the best and second best matching distances respectively; L = 0.75 is the setting threshold.

Images