Stereo SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints

Abstract

In view of the problems that when there are dynamic objects in the scene, a visual SLAM system has a large gap between the positioning result and the true value, and the system robustness is low. Based on the traditional visual odometer, a binocular SLAM front-end optimization algorithm that is robust in a dynamic environment is proposed. The algorithm first dedistorts the binocular image, then extracts and triangulates the image, and pre-screens the dynamic feature points according to the visual geometric constraints combined with the reprojection error chi-square distribution model. On this basis, a multidimensional space directed graph model is constructed by using the information of each feature point itself and the motion consistency of the feature points on the same dynamic object, and an AP clustering algorithm is introduced to accurately screen the feature points, thereby accurately filtering out the point groups that do not meet the static assumption. The overall positioning trajectory of the algorithm is closer to the theoretical value, and can effectively improve the operation accuracy and robustness of the SLAM system in dynamic scenes.

Description

Technical Field

[0001] The invention belongs to the technical field of computer vision, and in particular relates to a binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints. Background Art

[0002] Smart cars are conducive to improving traffic safety, alleviating road congestion, and improving social efficiency. SLAM (Simultaneous Localization and Mapping) technology, as a supporting technology for smart car environmental perception, navigation positioning, path planning and other functions, is of great significance to the research of smart cars. Visual SLAM is different from laser SLAM with lidar as the core sensor. It mainly uses the image sequence information collected by the camera for positioning and mapping. With the advantages of camera price advantage, easy disassembly, and extremely rich environmental information, it has become a hot spot in the SLAM field. Traditional visual SLAM systems achieve positioning and mapping by extracting image feature points. Most mobile devices using visual SLAM applications will work in environments with dynamic objects. Current vision-based SLAM solutions and data sets for evaluating these solutions are mostly based on static environment assumptions. Therefore, how to make the visual SLAM system maintain robustness in dynamic scenes is a current research hotspot. Summary of the invention

[0003] The invention belongs to the field of visual SLAM (simultaneous localization and mapping) of intelligent driving. When there are dynamic objects in the scene, the visual SLAM system has a large gap between the positioning result and the true value, the system robustness is low, and the like. Based on the traditional visual odometer, a robust binocular SLAM front-end optimization algorithm in a dynamic environment is proposed; the algorithm first performs dedistortion processing on the binocular image, then extracts and triangulates the image, and pre-screens the dynamic feature points according to the visual geometric constraint combined with the reprojection error chi-square distribution model. On this basis, the information of each feature point itself and the motion consistency of the feature points on the same dynamic object are used to construct a multidimensional space directed graph model, and an AP (Affinity Propagation) clustering algorithm is introduced to accurately screen the feature points, so as to accurately filter out the point group that does not meet the static assumption; finally, the KITTI data set is used to experimentally verify the effectiveness of the method of the embodiment, and the results show that the algorithm can accurately identify the dynamic feature points, the overall positioning trajectory of the algorithm is closer to the theoretical value, and the operation accuracy and robustness of the SLAM system in the dynamic scene can be effectively improved.

[0004] The present invention proposes a binocular vision SLAM method based on geometrically constrained dynamic feature point clustering for dynamic scenes. The method is based on geometric constraints and combines the idea of ​​feature point clustering to effectively solve the binocular vision SLAM problem in scenes containing dynamic objects.

[0005] The technical solution specifically adopted by the present invention to solve the technical problem is:

[0006] A binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints is characterized by: firstly, the binocular image is dedistorted, then the feature points are extracted and triangulated, and the dynamic feature points are pre-screened according to the visual geometric constraints combined with the reprojection error chi-square distribution model; on this basis, the information of each feature point itself and the motion consistency of the feature points on the same dynamic object are used to construct a multidimensional space directed graph model, and the AP clustering algorithm is introduced to accurately screen the feature points, thereby accurately filtering out the point groups that do not meet the static assumption.

[0007] Furthermore, the method specifically includes the following steps:

[0008] Step 1: Data collection and preprocessing: Use a binocular camera to collect image sequences containing dynamic objects and perform image distortion correction;

[0009] Step 2: Feature extraction and matching: Extract and triangulate feature points for each frame of image;

[0010] Step 3: Inter-frame pose calculation and feature point reprojection: Calculate the inter-frame pose based on the matching point pairs, project the three-dimensional feature points in the camera coordinate system at the previous moment to the camera pixel coordinate system at the current moment, and calculate the reprojection error.

[0011] Step 4: Reprojection error model analysis: Analyze the chi-square distribution model of the reprojection error and make corrections based on the depth of the feature points;

[0012] Step 5: Pre-screening of dynamic feature points: Use the maximum inter-class variance method combined with the reprojection error chi-square distribution model to pre-screen dynamic feature points;

[0013] Step 6: Construct a multi-dimensional directed graph model: Use the information of each feature point to construct the nodes of the directed graph, and use the information between feature points to establish directed edges between nodes;

[0014] Step 7: AP clustering initialization: Initialize the values ​​of nodes and edges in the directed graph;

[0015] Step 8: AP clustering algorithm update: Update the node's attribution matrix R and attraction matrix A until convergence, and complete the AP clustering of feature points;

[0016] Step 9: Visualization and filtering of dynamic point groups: visualize the final dynamic feature point recognition results and filter out dynamic point groups based on the final clustering results.

[0017] Furthermore, in step 1, for each pixel point in the image, the image coordinate system is converted to a normalized coordinate system, and then the distortion correction model combining the radial distortion model and the tangential distortion model is used to correct the pixel point in the normalized coordinate system after correction; finally, the pixel point in the normalized coordinate system after correction is converted back to the image coordinate system to obtain a corrected image;

[0018] In step 2, the Harris corner detection algorithm is used to extract image feature points, and then the feature points are projected to two cameras respectively to obtain three-dimensional coordinates from similar triangles to triangulate the feature points.

[0019] Furthermore, in step 3, consider that there are n three-dimensional points P in space n , click P i The projection in the normalized coordinate system of the first camera pose is Z i1 , through feature matching, we can know that the actual observation value Z i1 and Z i2 The corresponding point is the same space point P i , P i Project the estimated inter-frame pose to obtain point Z i2' With the observed value Z i2 There is a certain distance D between i12 , which is point P i The reprojection error of

[0020] When calculating the pose of two frames, the estimated pose needs to be continuously adjusted so that the gap between the observed value and the estimated value is infinitely close to 0; considering multiple pairs of points, taking ξ^ as the optimization amount, the optimization function is solved using the least squares problem solution, and finally the reprojection error of the feature points is obtained.

[0021] Furthermore, in step 4: reprojection error model analysis: the chi-square distribution model of the reprojection error is analyzed and corrected in combination with the depth of the feature points;

[0022] When there is no influence of dynamic objects, the reprojection error distribution model conforms to the chi-square distribution with 2 degrees of freedom; when there are dynamic objects, due to the motion similarity of the feature points on the dynamic objects and the reprojection error will be greater than that of the static points, the depth of the dynamic point group is considered to be negatively correlated with its reprojection error. Therefore, the reprojection error of the points is corrected in combination with the distance of the dynamic target from the camera, and then the maximum inter-class variance method is used to pre-divide the feature points into two categories: S: static and D: dynamic and poor matching.

[0023] Furthermore, in step five:

[0024] The number of feature points between the statistical frames is recorded as N, and the points are divided into M intervals according to the size of the corrected reprojection. The corrected reprojection of the jth point in the i-th interval is rij ',n i is the number of feature points in the i-th interval, p i is the ratio of the number of points in the ith interval to the total number of points, then:

[0025]

[0026] When the threshold is k, the probability that a feature point is classified as class S is The average corrected reprojection error of all S-class midpoints is The probability of being classified as class D is The average corrected reprojection error of all D-class midpoints is The average corrected reprojection error of all points is R, then:

[0027] According to the concept of variance:

[0028]

[0029] Combining the above formulas, we get:

[0030]

[0031] in:

[0032] Traverse each classification situation and obtain the best threshold k to maximize the variance of the S and D categories and complete dynamic pre-screening.

[0033] Furthermore, in step six, the spatial position, velocity magnitude and direction, and reprojection error of the feature points are combined to construct a multidimensional variable, each variable is represented as a node, and the type and direction of the edge are determined based on the relationship between the variables. The edges used include node similarity edges, node attraction edges, and node attribution edges.

[0034] Furthermore, in step 7, considering the differences in dimensions of different variables, the Pearson correlation coefficient is used to describe the similarity between feature points. For the similarity edge between two nodes, the corresponding Pearson correlation coefficient is used as the initial value;

[0035] Assume that the point set V contains n points. 1 ,P 2 ,…,P n}, each feature point in V is regarded as a potential cluster center, and it may be part of multiple cluster centers; a directed edge connection is constructed for every two feature points in V to form a directed graph model, and a similarity edge set S is defined, where edge s(i,j) = s(j,i) is the node P i and node P jThe similarity of s(i,j)>s(i,k) if and only if P i With P j is more similar to P k The larger the similarity value of s(i,j), the greater the similarity value of node P i With node P j The closer the distance is; for each node, define its reference attribute, s(i,i) refers to the node P i As the reference degree of the cluster center, it represents the node P in AP clustering. i The degree of being a cluster center, therefore cannot be 0;

[0036] Before the iteration starts, it is assumed that all points have the same ability to become cluster centers, so the reference degree is set to the minimum value of all values ​​in the similarity matrix. The larger the reference degree, the stronger the ability of the data point to become a cluster center, which is related to the number of final cluster centers.

[0037] Define the attraction degree edge set R and the belonging degree edge set A between nodes, and the edge r(i,j) describes the node P j Suitable as node P i The degree of the cluster center, that is, node P i Attracted by other nodes, it becomes P j is the probability of a node being the cluster center; the edge a(i,j) describes the node P i Select node P j As the fitness of its cluster center, the attraction edge set R and the belonging edge set A are all initialized to 0.

[0038] Furthermore, in step eight, the information between nodes is transmitted through the edges in the directed graph model, and the cluster center corresponding to each feature point is iteratively calculated, as shown in the following formula:

[0039]

[0040]

[0041]

[0042] Where a t (i,j') is the number of nodes except node P at time t j Other point-to-point i The initial value of the belonging degree is 0; s(i,j') is the number of nodes except node P j Other points outside and node P i The similarity, r t+1 (i,j) is the node P at time t+1 j Become a node P iThe possibility of the cluster center; when t+1 time node P i Consider node P j When it is the cluster center, the similarity s(i,j) between the two nodes is subtracted from the node P at time t. j Among the other candidate cluster nodes, i The similarity s(i,j') and the attribution a t The node with the largest sum of (i, j') completes the update of the attraction edge at time t+1;

[0043] r(i',j) represents the node P j As except node P i The possibility of the cluster center of other nodes outside the node. If its value is greater than 0, it means that the node P j Except for node P i Other nodes outside the node P i The highest similarity is node P j Become a node P i The clustering center has strong ability; when t+1 time node P i Consider node P j When it is the cluster center, all r greater than or equal to 0 at time t t (i',j) value and node P j The possibility of itself t The sum of (j,j) is a t+1 (i,j), to measure the node P i Select node P at time t+1 j As the fitness of its cluster center, complete the update of the attribute edge at time t+1;

[0044] When updating information, the decay coefficient λ is used to avoid the algorithm from not converging, as shown in the following formula;

[0045] r t+1 (i,j)=(1-λ)*r t+1 (i,j)+λ*r t (i,j)

[0046] a t+1 (i,j)=(1-λ)*a t+1 (i,j)+λ*a t (i,j)

[0047] If the values ​​of each link remain unchanged or the number of algorithm updates exceeds the set number, the algorithm ends and the nodes are divided into different clusters according to the node's degree of belonging, generating N determined cluster centers.

[0048] Furthermore, in step nine, considering the motion consistency of feature points of the same object, the speed of the point group is calculated by accumulating and averaging, marking the point group with a speed greater than a threshold, and determining the final dynamic point group result so that it does not participate in the pose calculation.

[0049] Compared with the prior art, the innovation of the present invention and its preferred solution is that, first, a binocular camera is used to obtain an image sequence in a scene containing dynamic objects, and image preprocessing is performed, including distortion removal, feature point extraction, etc.; secondly, based on the information obtained by the binocular camera and the inter-frame pose information, a multidimensional directed graph model is constructed, with feature points as nodes and information between feature points as edges, and the relationship between feature points is measured by Pearson similarity to form a multidimensional feature space; then, the multidimensional feature space is clustered using the AP clustering algorithm to obtain feature point clusters; finally, the dynamic feature points are finally determined based on the information of the feature point clusters, and they are removed to optimize the camera pose, solving the binocular vision SLAM problem in scenes containing dynamic objects. Compared with the prior art, the advantage of the present invention is that the feature point clustering and multidimensional directed graph model are used, which can effectively solve the binocular vision SLAM problem in scenes containing dynamic objects while ensuring the real-time performance of the algorithm, thereby improving the accuracy and robustness of the algorithm.

[0050] Therefore, it has important practical value and broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0052] Figure 1 This is a schematic diagram of Harris corner point detection according to an embodiment of the present invention;

[0053] Among them, (a) smooth area points, (b) edge points, (c) corner points;

[0054] Figure 2 This is a binocular camera model diagram of an embodiment of the present invention;

[0055] Figure 3 This is a schematic diagram of reprojection error according to an embodiment of the present invention;

[0056] Figure 4 This is a schematic diagram of connecting edges according to an embodiment of the present invention;

[0057] Among them, (a) similarity edge, (b) attraction edge update, (c) belonging edge update;

[0058] Figure 5 This is a diagram showing the dynamic point recognition effect of the algorithm according to an embodiment of the present invention in the KITTI data set;

[0059] Among them, (a) feature point inter-frame tracking, (b) dynamic point pre-screening, and (c) final dynamic point result;

[0060] Figure 6 The present invention is implemented as a flowchart of the embodiment of the present invention. DETAILED DESCRIPTION

[0061] In order to make the features and advantages of this patent more obvious and easy to understand, the following embodiments are specifically described in detail as follows:

[0062] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0063] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0064] like Figure 6 As shown, the implementation process of the embodiment of the present invention is specifically introduced in steps below. It should be noted that the present invention mainly provides a front-end optimization algorithm for SLAM, so the main solution steps only involve Figure 6 The subsequent processing of the dynamic point group removal process does not involve improvement in the present invention and is therefore not described in detail:

[0065] Step 1: Data collection and preprocessing: Use a binocular camera to collect image sequences containing dynamic object scenes and perform image distortion correction;

[0066] A binocular camera can obtain depth information by shooting the same scene with two cameras. After acquiring the image data, preprocessing is required to facilitate subsequent visual SLAM algorithm processing. First, the color image needs to be converted into a grayscale image, because the grayscale image has only one channel, the calculation efficiency is higher, and it has better robustness than the color image. Secondly, distortion correction is required because the distortion of the lens will cause the objects in the image to deform, thus affecting the subsequent feature extraction and matching.

[0067] The principle of distortion correction is to model the distortion through the calibration parameters of the camera and dedistort the image. The calibration parameters of the camera include internal parameters and external parameters. The internal parameters mainly include the focal length, principal point coordinates and distortion parameters of the camera, and the external parameters include the position and orientation of the camera. The camera calibration parameters are obtained by using the known calibration plate image to calibrate the camera. Then, the camera calibration parameters and the feature points in the image are used to perform distortion correction to obtain the dedistorted image. When dedistorting an image, it is necessary to first determine the distortion model of the camera. Common camera distortion models include radial distortion models, tangential distortion models, and offset models of the distortion center. This algorithm combines the radial distortion model and the tangential distortion model to obtain the following formula:

[0068]

[0069] For each pixel in the image, it is converted from the image coordinate system to the normalized coordinate system, and then corrected according to the distortion correction model to obtain the pixel in the corrected normalized coordinate system. Finally, the pixel in the corrected normalized coordinate system is converted back to the image coordinate system to obtain the corrected image.

[0070] Step 2: Feature extraction and matching: Extract and triangulate feature points for each frame of image;

[0071] The algorithm in this embodiment uses the Harris corner detection algorithm to extract image feature points. For a point in an image, if a small change in any direction will cause a large change in grayscale, then it is a corner point. Figure 1 As shown in (a), the window does not change in all directions. Figure 1 As shown in (b), the window does not change in the direction of the edge. Figure 1 As shown in (c), the window changes in all directions. It is divided into the following three steps:

[0072] 1) Shift the image window by [u, v] to generate grayscale change E(u, v), and calculate the pixel value change E(u, v) inside the window;

[0073]

[0074] From: I(x+u,y+v)=I(x,y)+I x u+I y v+O(u 2 ,v 2 ) can be obtained:

[0075]

[0076] For a small local movement [u,v], it can be approximately expressed as:

[0077]

[0078] Where M is a 2×2 matrix, which can be obtained by the derivative of the image:

[0079]

[0080] 2) For each window, calculate the corresponding corner point response function: R = λ 1 λ 2 -k(λ 1 +λ 2 ) 2 ; Perform threshold processing on the corner point response function. If the corner point response value is greater than the threshold, the window corresponds to a corner point feature. Assume that the binocular camera imaging model is ideal, that is, the camera is not distorted, the left and right camera imaging planes are coplanar, and the camera optical axis passes through the center of the imaging plane. For a feature point, triangulate it, such as Figure 2 The binocular camera model shown in the figure assumes that a point P is projected to both cameras at the same time, and the three-dimensional coordinates of point P are calculated. l x r With ΔPO l O r Similar, so:

[0081]

[0082] Where T is the baseline, which represents the component of the distance between the optical centers of the left and right cameras in the x direction, f is the focal length, and x l ,x r is the projection position of point P on the left and right cameras, x l ,x r is the distance from the left edge of the image plane to the projection position of P. Similarly, the X and Y coordinates of point P can also be obtained from similar triangles. Taking the left camera as the reference, let the pixel coordinates of point P projected to the left camera be (u, v), then:

[0083] Substitution have to:

[0084]

[0085]

[0086] At this point, the extraction and triangulation of feature points are completed.

[0087] Step 3: Inter-frame pose calculation and feature point reprojection: Calculate the inter-frame pose based on the matching point pairs, project the 3D feature points of the image at a certain moment into the image at another moment, and calculate the reprojection error;

[0088] The reprojection error is used to describe the error between the pixel coordinates (actually observed imaging plane position) of the same 3D space point and the imaging plane position obtained by projecting the space point according to the currently estimated inter-frame pose. Figure 3 As shown, consider that there are n three-dimensional points P in space n , click P i The projection (observation value) in the normalized coordinate system of the first camera pose is Z i1 , through feature matching, we can know that the actual observation value Z i1 and Z i2 The corresponding point is the same space point P i , P i Project the estimated inter-frame pose to obtain point Z i2' With the observed value Z i2 There is a certain distance D between i12 , which is point P i Reprojection error. For example, the spatial point P = [X i ,Y i ,Z i ] T , the pixel coordinate of its projection is u i =[u i ,v i ] T .

[0089] s i u i =K exp(ξ^)P i

[0090]

[0091] When calculating the pose of two frames, the estimated pose needs to be adjusted continuously so that the difference between the observed value and the estimated value is infinitely close to 0. Consider multiple pairs of points, with ξ ^ is the optimization amount, and the function to be optimized is as follows:

[0092] e(ξ^)=(p uv ) estimation -(p uv ) bserve

[0093] Using the least squares problem solution method to solve the optimization function, we get:

[0094]

[0095] The final result (puv ) estimation -(p uv ) e It is the reprojection error of the feature point.

[0096] Step 4: Reprojection error model analysis: Analyze the chi-square distribution model of the reprojection error and make corrections based on the depth of the feature points;

[0097] Assume the reprojection error in the x direction of the pixel plane Similarly, the y direction assumption Assuming the noise variance (pixel is the pixel error caused by noise), let X~χ 2 (2), Z 2 =e y , Because Z 1 , Z 2 are independent random variables that conform to the standard normal distribution, then X~χ 2 ( 2), that is, the reprojection error distribution satisfies the chi-square distribution with 2 degrees of freedom, and the chi-square distribution function is as follows:

[0098]

[0099] The reprojection error of the mismatched feature points is usually significantly higher than that of the correct matching points. The probability distribution (cumulative distribution function) is α = F(x), where x is a random variable. α = F(x) represents the probability that the random variable x takes each possible value and its value is less than or equal to x. Given α, determine the interval [0, F -1 (α)], distinguishing internal and external points, as shown in the following formula:

[0100]

[0101] When there is no dynamic object, the reprojection error distribution model basically conforms to the chi-square distribution with 2 degrees of freedom. When there is a dynamic object, due to the similarity of the motion of the feature points on the dynamic object and the reprojection error will be greater than that of the static point, this embodiment considers that the depth of the dynamic point group is negatively correlated with its reprojection error, so the reprojection error of the point is corrected in combination with the distance of the dynamic target from the camera as follows: i '=r i +(d i -μ)×λ, where μ is the reference depth value and λ is the depth correction coefficient of the feature point. The maximum inter-class variance method is then used to pre-divide the feature points into two categories: S (static) and D (dynamic and poor matching).

[0102] Step 5: Pre-screening of dynamic feature points: Use the OSTU (maximum between-class variance) method combined with the reprojection error chi-square distribution model to pre-screen dynamic feature points;

[0103] The number of feature points between the statistical frames is recorded as N, and the points are divided into M intervals according to the size of the corrected reprojection. The corrected reprojection of the jth point in the i-th interval is r ij ',n i is the number of feature points in the i-th interval, p i is the ratio of the number of points in the ith interval to the total number of points, then:

[0104]

[0105] When the threshold is k, the probability that a feature point is classified as class S is The average corrected reprojection error of all S-class midpoints is The probability of being classified as class D is The average corrected reprojection error of all D-class midpoints is The average corrected reprojection error of all points is R, then:

[0106] According to the concept of variance:

[0107]

[0108] Combining the above formulas, we get:

[0109]

[0110] in:

[0111] Traverse each classification situation and obtain the best threshold k to maximize the variance of the S and D categories and complete dynamic pre-screening.

[0112] Step 6: Construct a multi-dimensional directed graph model: Use the information of each feature point to construct the nodes of the directed graph, and use the information between feature points to establish directed edges between nodes;

[0113] First, determine the multidimensional variables to be studied and the relationship between them. In this method, the spatial position, velocity magnitude and direction, and reprojection error of the feature points are combined to construct multidimensional variables, and each variable is represented as a node. According to the relationship between the variables, the type and direction of the edge are determined. The edges used in this method include node similarity edges (undirected edges), node attraction edges, and node attribution edges (directed edges).

[0114] Step 7: AP clustering initialization: Initialize the values ​​of nodes and edges in the directed graph;

[0115] Considering the differences in dimensions of different variables, this embodiment uses the Pearson correlation coefficient to describe the similarity between feature points. n Two points P in i =(p i1 ,,p i2 ,…,p in ) and P j =(p j1 ,p j2 ,…,p jn ), Pearson similarity is calculated as follows. Compared with Euclidean distance, Pearson correlation coefficient is used to measure the correlation between variables. Its calculation result takes into account the different ranges of variable values ​​and uses the trend between variables as a measurement indicator.

[0116]

[0117] in For the similarity edge between two nodes, use the corresponding Pearson correlation coefficient as the initial value. Assume that the point set V contains n points = {P 1 ,P 2 ,…,P n}, each feature point in V is regarded as a potential cluster center, and it may be part of multiple cluster centers; a directed edge connection is constructed for every two feature points in V to form a directed graph model, and a similarity edge set S is defined, where edge s(i,j) = s(j,i) is the node P i and node P j The similarity, such as Figure 4 (a). Such that s(i,j)>s(i,k) if and only if P i With P j is more similar to P k The similarity between points is generally negative. The larger the similarity value of s(i,j), the higher the similarity value of node P. i With node P j For each node, define its reference attribute, for example, s(i,i) refers to node P i As the reference degree of the cluster center. In AP clustering, it represents node P i The degree of being a cluster center cannot be 0. Before the iteration starts, this algorithm assumes that all points have the same ability to become cluster centers, so the reference degree is generally set to the minimum value of all values ​​in the similarity matrix. The larger the reference degree, the stronger the ability of the data point to become a cluster center, which is related to the number of final cluster centers.

[0118] Define the attraction degree edge set R and the belonging degree edge set A between nodes, and the edge r(i,j) describes the node Pj Suitable as node P i The degree of the cluster center, that is, node P i Attracted by other nodes, it becomes P j The possibility of a node being the cluster center. The edge a(i,j) describes the node P i Select node P j As the fitness of its cluster center, the algorithm of this embodiment initializes the attraction degree edge set R and the belonging degree edge set A to 0.

[0119] Step 8: AP clustering algorithm update: Update the node's attribution matrix R and attraction matrix A until convergence, and complete the AP clustering of feature points;

[0120] The information between nodes is transmitted through the edges in the directed graph model, and the cluster center corresponding to each feature point is iteratively calculated, as shown in the following formula:

[0121]

[0122]

[0123]

[0124] Figure 4 (b) is a schematic diagram of the attractiveness edge update process. t (i,j') is the number of nodes except node P at time t j Other point-to-point i The initial value of the belonging degree is 0; s(i,j') is the number of nodes except node P j Other points outside and node P i The similarity, r t+1 (i,j) is the node P at time t+1 j Become a node P i The possibility of the cluster center. When the node P i Consider node P j When it is the cluster center, the similarity s(i,j) between the two nodes is subtracted from the node P at time t. j Among the other candidate cluster nodes, i The similarity s(i,j') and the attribution a t The node with the largest added value of (i, j') completes the update of the attraction edge at time t+1. Figure 4 (c) shows the schematic diagram of the updating process of the attributed edge. In the above formula, r(i',j) represents the node P j As except node P i The possibility of the cluster center of other nodes outside the node. If its value is greater than 0, it means that the node P j Except for node Pi Other nodes outside the node P i The highest similarity is node P j Become a node P i The clustering center has strong ability. When the node P i Consider node P j When it is the cluster center, all r greater than or equal to 0 at time t t (i',j) value and node P j The possibility of itself t The sum of (j,j) is a t+1 (i,j), to measure the node P i Select node P at time t+1 j As the fitness of its cluster center, the update of the attribute edge at time t+1 is completed. When updating information, this algorithm uses the attenuation coefficient λ to avoid the algorithm from not converging, as shown in the following formula.

[0125] r t+1 (i,j)=(1-λ)*r t+1 (i,j)+λ*r t (i,j)

[0126] a t+1 (i,j)=(1-λ)*a t+1 (i,j)+λ*a t (i,j)

[0127] If the values ​​of each link remain unchanged or the number of algorithm updates exceeds the set number, the algorithm ends and the nodes are divided into different clusters according to the node's degree of belonging, generating N determined cluster centers.

[0128] Step 9: Visualization and filtering of dynamic point groups: visualize the final dynamic feature point recognition results and filter out dynamic point groups based on the final clustering results;

[0129] Considering the motion consistency of feature points of the same object, this embodiment calculates the speed of the point group as follows, marks the point group with a speed greater than a threshold, and determines the final dynamic point group result so that it does not participate in the pose calculation.

[0130]

[0131] Step 10: Algorithm effectiveness evaluation: Use the KITTI dataset to verify the effectiveness of the algorithm of this embodiment, and conduct a comprehensive quantitative analysis of the positioning and mapping effects of the SLAM system before and after the integration of the algorithm of this embodiment.

[0132] like Figure 5The figure shows the dynamic point recognition effect of the algorithm of this embodiment in the KITTI data set. In order to measure the accuracy of the algorithm, the dynamic object feature points are manually marked and used as the true value to measure the dynamic feature point detection precision P and recall R of the algorithm, which are calculated as follows:

[0133] P=TD / (TD+FD)

[0134] R=TD / (TD+MTD)

[0135] Among them, TD (true dynamic) is the correct dynamic feature point extracted by the algorithm, FD (false dynamic) is the point where the algorithm mistakenly treats the static feature point as a dynamic feature point, and MTD (missing true dynamic) is a dynamic feature point that exists but is not detected by the algorithm.

[0136] The experimental results are shown in Table 1, which shows that the algorithm can correctly identify most dynamic feature points. Table 2 shows the various indicators of the positioning trajectory compared with the true value under the loop detection when the Vins-Fusion algorithm and the algorithm of this embodiment run the KITTI data set. For the 07 and 09 sequences with more dynamic objects, the improvement rate of each indicator is 21.64%-32.88% when using loop detection. The results show that the absolute pose error obtained by the algorithm of this embodiment is significantly reduced compared with the Vins-Fusion algorithm, while for the 04 and 06 sequences with almost no dynamic objects, the results of the two algorithms are not significantly different, which verifies the effectiveness of the algorithm of this embodiment in a dynamic environment.

[0137] Table 1 Evaluation of dynamic feature point extraction results

[0138] Dataset Precision P / % Recall rate R / % ZED2i self-collection 73.18 68.22 KITTI dataset 75.95 78.30

[0139] Table 2 KITTI absolute pose error (using loop closure detection)

[0140]

[0141] The above is only a preferred embodiment of the present invention, and does not limit the present invention in other forms. Any technician familiar with the profession may use the above disclosed technical content to change or modify it into an equivalent embodiment with equivalent changes. However, any simple modification, equivalent change and modification made to the above embodiment according to the technical essence of the present invention without departing from the technical solution of the present invention still belongs to the protection scope of the technical solution of the present invention.

[0142] This patent is not limited to the above-mentioned optimal implementation mode. Anyone can derive other forms of binocular SLAM front-end optimization methods based on dynamic feature point clustering based on geometric constraints under the inspiration of this patent. All equal changes and modifications made according to the scope of the patent application of the present invention should be covered by this patent.

Claims

1. A binocular SLAM front-end optimization method based on dynamic feature point clustering with geometric constraints. It is characterized in that The following steps are involved: Step 1: Data collection and preprocessing: Use a binocular camera to collect image sequences containing dynamic object scenes and perform image distortion correction; Step 2: Feature extraction and matching: Extract and triangulate feature points for each frame of image; Step 3: Inter-frame pose calculation and feature point reprojection: Calculate the inter-frame pose based on the matching point pairs, project the 3D feature points in the camera coordinate system at the previous moment into the camera pixel coordinate system at the current moment, and calculate the reprojection error; Step 4: Reprojection error model analysis: Analyze the chi-square distribution model of the reprojection error and make corrections based on the depth of the feature points; Step 5: Pre-screening of dynamic feature points: Use the maximum inter-class variance method combined with the reprojection error chi-square distribution model to pre-screen dynamic feature points; Step 6: Construct a multi-dimensional directed graph model: Use the information of each feature point to construct the nodes of the directed graph, and use the information between feature points to establish directed edges between nodes; Step 7: AP clustering initialization: Initialize the values ​​of nodes and edges in the directed graph; Step 8: AP clustering algorithm update: Update the node's attribution matrix R and attraction matrix A until convergence, and complete the AP clustering of feature points; Step 9: Visualization and filtering of dynamic point groups: visualize the final dynamic feature point recognition results and filter out dynamic point groups based on the final clustering results.

2. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 1, Features: In step 1, for each pixel point in the image, the image coordinate system is converted to the normalized coordinate system, and then the distortion correction model combining the radial distortion model and the tangential distortion model is used to correct the pixel point in the normalized coordinate system after correction. Finally, the pixel point in the normalized coordinate system after correction is converted back to the image coordinate system to obtain the corrected image. In step 2, the Harris corner detection algorithm is used to extract image feature points, and then the feature points are projected to two cameras respectively to obtain three-dimensional coordinates from similar triangles to triangulate the feature points.

3. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 2, Features: In step 3, consider n three-dimensional points P in space n , click P i The projection in the normalized coordinate system of the first camera pose is Z i1 , through feature matching, we can know that the actual observation value Z i1 and Z i2 The corresponding point is the same space point P i , P i Project the estimated inter-frame pose to obtain point Z i2' With the observed value Z i2 There is a certain distance D between i12 , which is point P i The reprojection error of When calculating the pose of two frames, the estimated pose needs to be adjusted continuously so that the difference between the observed value and the estimated value is infinitely close to 0; Consider multiple pairs of points, with ξ ^ In order to optimize the amount, the optimization function is solved by the least squares problem method, and finally the reprojection error of the feature points is obtained.

4. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 3, Features: In step 4: Reprojection error model analysis: analyze the chi-square distribution model of the reprojection error and make corrections based on the depth of the feature points; When there is no influence of dynamic objects, the reprojection error distribution model conforms to the chi-square distribution with 2 degrees of freedom; when there are dynamic objects, due to the motion similarity of the feature points on the dynamic objects and the reprojection error will be greater than that of the static points, the depth of the dynamic point group is considered to be negatively correlated with its reprojection error. Therefore, the reprojection error of the points is corrected in combination with the distance of the dynamic target from the camera, and then the maximum inter-class variance method is used to pre-divide the feature points into two categories: S: static and D: dynamic and poor matching.

5. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 4, Features: In step five: The number of feature points between the statistical frames is recorded as N, and the points are divided into M intervals according to the size of the corrected reprojection. The corrected reprojection of the jth point in the i-th interval is r ij ',n i is the number of feature points in the i-th interval, p i is the ratio of the number of points in the ith interval to the total number of points, then: When the threshold is k, the probability that a feature point is classified as class S is The average corrected reprojection error of all S-class midpoints is The probability of being classified as class D is The average corrected reprojection error of all D-class midpoints is The average corrected reprojection error of all points is R, then: According to the concept of variance: Combining the above formulas, we get: in: Traverse each classification situation and obtain the best threshold k to maximize the variance of the S and D categories and complete dynamic pre-screening.

6. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 5, Features: In step six, the spatial position, velocity magnitude and direction, and reprojection error of the feature points are combined to construct a multidimensional variable. Each variable is represented as a node. The type and direction of the edge are determined based on the relationship between the variables. The edges used include node similarity edges, node attraction edges, and node attribution edges.

7. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 6, Features: In step 7, considering the differences in dimensions of different variables, the Pearson correlation coefficient is used to describe the similarity between feature points. For the similarity edge between two nodes, the corresponding Pearson correlation coefficient is used as the initial value; Assume that the point set V contains n points. 1 ,P 2 ,…,P n }, each feature point in V is regarded as a potential cluster center, and it may be part of multiple cluster centers; a directed edge connection is constructed for every two feature points in V to form a directed graph model, and a similarity edge set S is defined, where edge s(i,j) = s(j,i) is the node P i and node P j The similarity of s(i,j)>s(i,k) if and only if P i With P j is more similar to P k The larger the similarity value of s(i,j), the greater the similarity value of node P i With node P j The closer the distance is; for each node, define its reference attribute, s(i,i) refers to the node P i As the reference degree of the cluster center, it represents the node P in AP clustering. i The degree of being a cluster center, therefore cannot be 0; Before the iteration starts, it is assumed that all points have the same ability to become cluster centers, so the reference degree is set to the minimum value of all values ​​in the similarity matrix. The larger the reference degree, the stronger the ability of the data point to become a cluster center, which is related to the number of final cluster centers. Define the attraction degree edge set R and the belonging degree edge set A between nodes, and the edge r(i,j) describes the node P j Suitable as node P i The degree of the cluster center, that is, node P i Attracted by other nodes, it becomes P j is the probability of a node being the cluster center; the edge a(i,j) describes the node P i Select node P j As the fitness of its cluster center, the attraction edge set R and the belonging edge set A are all initialized to 0.

8. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 7, Features: In step eight, the information between nodes is transmitted through the edges in the directed graph model, and the cluster center corresponding to each feature point is iteratively calculated, as shown in the following formula: Where a t (i,j') is the number of nodes except node P at time t j Other point-to-point i The initial value of the belonging degree is 0; s(i,j') is the number of nodes except node P j Other points outside and node P i The similarity, r t+1 (i,j) is the node P at time t+1 j Become a node P i The possibility of the cluster center; when t+1 time node P i Consider node P j When it is the cluster center, the similarity s(i,j) between the two nodes is subtracted from the node P at time t. j Among the other candidate cluster nodes, i The similarity s(i,j') and the attribution a t The node with the largest sum of (i, j') completes the update of the attraction edge at time t+1; r(i,j) represents the node P j As except node P i The possibility of the cluster center of other nodes outside the node. If its value is greater than 0, it means that the node P j Except for node P i Other nodes outside the node P i The highest similarity is node P j Become a node P i The clustering center has strong ability; when t+1 time node P i Consider node P j When it is the cluster center, all r greater than or equal to 0 at time t t (i',j) value and node P j The possibility of itself t The sum of (j,j) is a t+1 (i,j), to measure the node P i Select node P at time t+1 j As the fitness of its cluster center, complete the update of the attribute edge at time t+1; When updating information, the decay coefficient λ is used to avoid the algorithm from not converging, as shown in the following formula; r t+1 (i,j)=(1-λ)*r t+1 (i,j)+λ*r t (i,j) a t+1 (i,j)=(1-λ)*a t+1 (i,j)+λ*a t (i,j) If the values ​​of each link remain unchanged or the number of algorithm updates exceeds the set number, the algorithm ends and the nodes are divided into different clusters according to the node's degree of belonging, generating N determined cluster centers.

9. The binocular SLAM front-end optimization method based on dynamic feature point clustering based on geometric constraints according to claim 8, Features: In step nine, considering the motion consistency of feature points of the same object, the speed of the point group is calculated by accumulating and averaging, marking the point group with a speed greater than the threshold, and determining the final dynamic point group result so that it does not participate in the pose calculation.

Images