A structured feature screening method based on Manhattan world hypothesis
By constructing energy functions based on the Manhattan world assumption in the SLAM system and iteratively optimized, the problem of low feature screening efficiency in sparse texture scenarios is solved, the accuracy and robustness of the SLAM system are improved, and it is suitable for scenarios such as autonomous driving.
Patent Information
- Application Number
- CN202111564380.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-12-20
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2041-12-20
AI Technical Summary
The existing SLAM system based on structured features lacks an efficient linear and surface feature screening mechanism in sparse texture scenarios, resulting in waste of computing resources and low efficiency.
Based on the Manhattan world assumption, energy functions of plane and line features are constructed, including first-order data terms and second-order geometric structure constraint terms, and energy functions are solved through an iterative confidence region optimization algorithm to filter out plane and line features.
It improves the accuracy and robustness of the SLAM system, saves computing resources, and achieves efficient and stable operation. It is suitable for scenarios such as autonomous driving and indoor navigation.
Smart Images

Figure CN114359643B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of image graphics, relates to a simultaneous positioning and mapping technology, and specifically relates to a structured feature screening method based on the Manhattan world hypothesis. Background Art
[0002] In recent years, intelligent robots have become an important research direction in academia and industry. To make robots more intelligent and autonomous, they must improve their ability to recognize unknown environments. Simultaneously positioning and mapping (SLAM) is a key technology for building three-dimensional scene maps and positioning the machine itself. It has been successfully applied to products such as unmanned vehicles, drones, express delivery robots, and intelligent cleaning robots.
[0003] At present, SLAM systems based on structured features in sparse texture scenes can already run stably and extract structural features with high accuracy. However, these systems lack an efficient screening mechanism for the extracted line and surface features, which makes the features redundant. If all line and surface features are tracked, it will lead to a waste of computing resources. Since there are a large number of structured scenes in the real world, based on the Manhattan world hypothesis, there are parallel and vertical relationships in structured scenes. Therefore, based on the energy function, the extracted line and surface features are screened, and setting irregular line feature angles (except parallel and vertical) will increase their energy values, thereby retaining mutually perpendicular or parallel line and surface features, which can better constrain the SLAM system for positioning, and at the same time save computing resources, so that the SLAM system can run efficiently and stably. Summary of the invention
[0004] To solve the above problems, the present invention discloses a structured feature screening method based on the Manhattan world hypothesis to screen line and surface features so that a SLAM system based on structured features can operate efficiently and stably.
[0005] To achieve the above object, the technical solution of the present invention is as follows:
[0006] A structured feature screening method based on the Manhattan world hypothesis can improve the accuracy and robustness of the SLAM system in weak texture scenes, including the following steps:
[0007] (1) Constructing the first-order data term and the second-order geometric structure constraint term of the plane feature energy function according to the fitting degree of the plane feature point cloud in the plane set and the geometric constraint relationship between the plane features in the structured scene;
[0008] (2) constructing the first-order data term and the second-order geometric structure constraint term of the line feature energy function according to the fitting degree of the line feature point cloud in the line set and the geometric constraint relationship between the line features;
[0009] (3) The energy function is solved based on the iterative confidence region optimization algorithm.
[0010] In step (1), the first-order data term and the second-order geometric structure constraint term of the plane feature energy function are constructed according to the fitting degree of the plane feature point cloud in the plane set and the geometric constraint relationship between the plane features in the structured scene. The specific steps are as follows:
[0011] (1.1) After segmenting the point cloud, the plane candidate set P is obtained;
[0012] (1.2) Calculate the first-order data item E1 of the plane feature energy function. First, calculate the distance between the plane centroid and the point cloud of the plane to determine the degree of fit between the plane candidate set and the point cloud. The higher the degree of fit, the lower the energy:
[0013]
[0014] Among them, π n is the candidate plane, λ is the fitting threshold between the plane candidate set and the point cloud, which is set to 30-50, N p is the number of points in the point cloud set corresponding to the candidate plane in the scene;
[0015] (1.3) Calculate the second-order geometric structure constraint related term E2 of the plane feature energy function. Based on the Manhattan world assumption, there is vertical / orthogonal information in the environment. Irregular plane angles will increase their energy, thereby eliminating plane features with irregular angles:
[0016]
[0017]
[0018] in, is the plane π in the candidate plane set m and plane π n The angle between the planes, is the angle between the two planes and the parallel or perpendicular geometric constraints that should be matched, and σ2 is the set angle growth step;
[0019] (1.4) The energy function of the plane feature is obtained as E(P)=E1+E2.
[0020] In step (2), the first-order data term and the second-order geometric structure constraint term of the line feature energy function are constructed according to the fitting degree of the line feature point cloud in the line set and the geometric constraint relationship between the line features. The specific steps are as follows:
[0021] (2.1) Extract the plane edge to obtain the line candidate set Q;
[0022] (2.2) Calculate the first-order data item E1 of the line feature energy function. First, calculate the distance between the geometric center of the line and the structured scene point cloud classified on the line, and judge the degree of fit between the line feature and the point cloud. The higher the degree of fit, the lower the energy:
[0023]
[0024] Among them, l n is the candidate line, λ is the fitting threshold between the line candidate set and the point cloud, which is set to 20-30, N p is the number of points in the point cloud set corresponding to the candidate line in the scene;
[0025] (2.3) Calculate the second-order geometric structure data item E2 of the line feature energy function. Based on the Manhattan world hypothesis, irregular line feature angles will increase their energy, thereby eliminating line features with irregular angles:
[0026]
[0027]
[0028] in, is the center line l of the candidate line set m and line l n The vector angle between is the angle between the two candidate lines and the parallel or perpendicular geometric constraints that should be matched, and ε2 is the set angle growth step;
[0029] (2.4) The energy function of the line feature is E(Q)=E1+E2.
[0030] In step (3), the energy function is solved based on the iterative confidence region optimization algorithm. The specific steps are:
[0031] (3.1) According to step (1) and step (2), obtain the candidate set P of planes and lines and the total energy function E(P);
[0032] (3.2) The energy function expression is approximated in the confidence region and converted into an unconstrained Lagrangian optimization problem. B is solved by minimizing the unconstrained Lagrangian function * ;
[0033] (3.3) According to candidate solution B * and the current solution B tCalculate the predicted energy reduction value P and the actual energy reduction value R, and select the next iteration B according to the relationship between R / P and the threshold σ1 = 2 and the threshold σ2 = 0.25. t+1 and λ t+1 , iterate until R>0, and obtain the solution B that minimizes the energy function and the filtered line and surface set.
[0034] The beneficial effects of the present invention are:
[0035] Aiming at the problem that the line and surface features extracted by the current SLAM system based on structured features lack an efficient screening mechanism, the present invention proposes a structured feature screening method based on the Manhattan world hypothesis. First, the first-order data items are constructed according to the degree of fit between the point cloud set and the plane and line, and then the second-order geometric structure constraint items are constructed according to the geometric constraint relationship in the structured scene. Then, the iterative confidence region optimization algorithm is used to solve the energy function to screen the plane set and the line set.
[0036] Aiming at the problem of low efficiency and robustness of SLAM system in texture sparse scenes, the present invention proposes a structured feature screening method based on Manhattan world hypothesis, which improves the efficiency and robustness of SLAM system based on structured features and can be widely used in scenes such as autonomous driving and indoor navigation. BRIEF DESCRIPTION OF THE DRAWINGS
[0037] Figure 1 A flow chart of a structured sign screening method based on the Manhattan world hypothesis;
[0038] Figure 2 Flowchart for solving the energy function using an iterative confidence region optimization algorithm. DETAILED DESCRIPTION
[0039] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.
[0040] Figure 1 It is a schematic diagram of the method flow of the present invention.
[0041] The structured feature screening method based on the Manhattan world hypothesis described in the present invention,
[0042] Step S1: Construct the first-order data item and second-order geometric structure constraint item of the plane set energy function according to the fitting degree of the point cloud in the plane set and the geometric constraint relationship (vertical or parallel) between the planes in the structured scene. Specifically including:
[0043] S1.1. After segmenting the point cloud, we obtain the plane candidate set P{π1, π2, ...π N};
[0044] S1.2. Calculate the first-order data item E1 of the plane feature energy function. The point cloud set corresponding to the candidate plane can be expressed as:
[0045]
[0046] in, It is a point cloud i To a candidate plane centroid , σ1 is the distance threshold. Calculate the distance between the centroid of the candidate plane and the structured scene point cloud classified to the plane, and judge the degree of fit between the plane candidate set and the point cloud. The more point clouds that meet the judgment conditions, the higher the degree of fit and the smaller the value of the energy function. Therefore, the first-order data item E1 of the plane feature energy function is expressed as:
[0047]
[0048] Among them, λ is the fitting threshold between the plane candidate set and the point cloud, N p The number of points in the point cloud set corresponding to the candidate plane in the scene;
[0049] S1.3. Calculate the second-order geometric structure constraint related term E2 of the plane feature energy function. Since there are many mutually perpendicular or parallel planes such as walls, floors and desks in structured scenes, adding geometric constraints between planes based on the Manhattan world assumption will increase the energy of irregular plane angles (except parallel and perpendicular), which can be expressed as:
[0050]
[0051]
[0052] in, is the set of candidate planes m and π n The angle between the planes, The angle between the two planes and the parallel or perpendicular set constraints that should match when the two planes are parallel or perpendicular is 0, σ2 is the set angle growth step, which is π / 18;
[0053] S1.4. Obtain the energy function of the plane feature:
[0054]
[0055] Among them, E(P) represents the overall energy function, E1 represents the first-order data term, and E2 represents the second-order geometric structure constraint term.
[0056] Step S2: Construct the first-order data term and second-order geometric structure constraint term of the edge set energy function based on the structural angle error of the distance between the point cloud set and the geometric center of the edge and the direction vector between the edges. Specifically including:
[0057] S2.1. Extract the plane edge to obtain the line candidate set Q{l1, l2, ..., l N};
[0058] S2.2. Calculate the first-order data item E1 of the line feature energy function. The point cloud set corresponding to the candidate line can be expressed as:
[0059]
[0060] in, is a point p in the point cloud i To the candidate line The geometric center of the candidate line, ε1 is the distance threshold. First, the distance between the geometric center of the candidate line and the structured scene point cloud classified to the line is calculated to determine the degree of fit between the candidate line and the point cloud. The more point clouds that meet the judgment conditions, the higher the degree of fit. Therefore, the first-order data item of the line feature energy function is:
[0061]
[0062] Among them, λ is the fitting threshold between the line candidate set and the point cloud, N p is the number of points in the point cloud set corresponding to the candidate line in the scene;
[0063] S2.3. Calculate the second-order geometric structure data item of the line feature energy function. Add the geometric constraint item of the line based on the Manhattan world hypothesis. For irregular line angles (i.e., except parallel and vertical), its energy will be improved, which can be expressed as:
[0064]
[0065]
[0066] in, is the set of candidate lines m and l n The vector angle between It is the angle between the two candidate lines and the parallel or perpendicular lines that should be matched. is 0, ε2 is the angle growth step, which is π / 18;
[0067] S2.4, the energy function of the line feature is obtained as:
[0068]
[0069] Among them, E(Q) represents the overall energy function, E1 represents the first-order data term, and E2 represents the second-order geometric structure constraint term.
[0070] Step S3: Solve the energy function based on the iterative confidence region optimization algorithm. The specific steps are:
[0071] S3.1. Obtain a candidate set P {I1, I2, ..., I} of planes and lines according to step (1) and step (2). N} and the total energy function is:
[0072]
[0073] Where E1 and E2 are the first-order data items and second-order geometric structure constraints of the candidate plane or line, respectively. n , b n ∈{0, 1} indicates whether the plane or line is selected, which can be expressed by matrix multiplication as:
[0074] E(B)=B T U+B T MB
[0075] Where B = [b1, b2, ..., b N ], b1, b2, ..., b N ∈{0, 1}, U is the first-order data term of the energy equation, M={m ij ∈R} is the correlation coefficient between two candidate members;
[0076] S3.2. Express the overall energy function E as a positively correlated term E N and negatively correlated term E P :
[0077]
[0078] Among them, M + Indicates the part where the correlation coefficient of the M matrix is positive M - Indicates the negative part of the correlation coefficient of the M matrix In the confidence region, the energy equation can be approximated as:
[0079] E t (B)=E N (B)+B T U t +c,U t =2M + B t
[0080] Among them U t +c is E p The first-order Taylor expansion of (B) is the approximate solution Convert it into an unconstrained Lagrangian optimization problem:
[0081] L t (B)=E t (B)+λ t ·||BB t ||
[0082] where λ t is the control coefficient of the confidence region size, and B is solved by minimizing the above unconstrained Lagrangian function * ;
[0083] S3.3, according to candidate solution B * and the current solution B t Calculate the predicted energy reduction value P:
[0084] P=E t (B t )-E t (B * )
[0085] Then calculate the actual energy reduction value R:
[0086] R=E(B t )-E(B * )
[0087] According to the relationship between R / P and thresholds σ1, σ2, select B for the next iteration. t+1 and λ t+1 :
[0088]
[0089]
[0090] Repeat the above two steps until R>0, and finally obtain the solution B that minimizes the energy function. The flow chart is as follows Figure 2 shown.
[0091] It should be noted that the above content only illustrates the technical idea of the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.
Claims
1. A structured feature screening method based on the Manhattan world hypothesis to improve the accuracy and robustness of the SLAM system in weak texture scenes, characterized in that: The steps include: (1) Constructing the first-order data term and the second-order geometric structure constraint term of the plane feature energy function according to the fitting degree of the plane feature point cloud in the plane set and the geometric constraint relationship between the plane features in the structured scene; The specific steps are: (1.1) After segmenting the point cloud, the plane candidate set P is obtained; (1.2) Calculate the first-order data item E1 of the plane feature energy function. First, calculate the distance between the plane centroid and the point cloud of the plane to determine the degree of fit between the plane candidate set and the point cloud. The higher the degree of fit, the lower the energy: Among them, π n is the candidate plane, λ is the fitting threshold between the plane candidate set and the point cloud, which is set to 30-50, N p is the number of points in the point cloud set corresponding to the candidate plane in the scene; (1.3) Calculate the second-order geometric structure constraint related term E2 of the plane feature energy function. Based on the Manhattan world assumption, there is vertical / orthogonal information in the environment. Irregular plane angles will increase their energy, thereby eliminating plane features with irregular angles: in, is the plane π in the candidate plane set m and plane π n The angle between the planes, is the angle between the two planes and the parallel or perpendicular geometric constraints that should be matched, and σ2 is the set angle growth step; (1.4) The energy function of the plane feature is obtained as E(P) = E1 + E2; (2) constructing the first-order data term and the second-order geometric structure constraint term of the line feature energy function according to the fitting degree of the line feature point cloud in the line set and the geometric constraint relationship between the line features; The specific steps are: (2.1) Extract the plane edge to obtain the line candidate set Q; (2.2) Calculate the first-order data item E1 of the line feature energy function. First, calculate the distance between the geometric center of the line and the structured scene point cloud classified on the line, and judge the degree of fit between the line feature and the point cloud. The higher the degree of fit, the lower the energy: Among them, l n is the candidate line, λ is the fitting threshold between the line candidate set and the point cloud, which is set to 20-30, N p is the number of points in the point cloud set corresponding to the candidate line in the scene; (2.3) Calculate the second-order geometric structure data item E2 of the line feature energy function. Based on the Manhattan world hypothesis, irregular line feature angles will increase their energy, thereby eliminating line features with irregular angles: in, is the center line l of the candidate line set m and line l n The vector angle between is the angle between the two candidate lines and the parallel or perpendicular geometric constraints that should be matched, and ε2 is the set angle growth step; (2.4) The energy function of the line feature is obtained as E(Q)=E1+E2; (3) The energy function is solved based on the iterative confidence region optimization algorithm.
2. The structured feature screening method based on the Manhattan world hypothesis according to claim 1, characterized in that: In step (3), the energy function is solved based on an iterative confidence region optimization algorithm, and the specific steps are: (3.1) According to step (1) and step (2), obtain the candidate set P of planes and lines and the total energy function E(P); (3.2) The energy function expression is approximated in the confidence region and converted into an unconstrained Lagrangian optimization problem. B is solved by minimizing the unconstrained Lagrangian function * ; (3.3) According to candidate solution B * and the current solution B t Calculate the predicted energy reduction value P and the actual energy reduction value R, and select the next iteration B according to the relationship between R / P and the threshold σ1 = 2 and the threshold σ2 = 0.
25. t+1 and λ t+1 , iterate until R>0, and obtain the solution B that minimizes the energy function and the filtered line and surface set.
Citation Information
Patent Citations
Indoor object cube detection method for depth image scene
CN104899883A
Tree extraction method in outdoor point cloud scene based on energy function
CN111986223A
Three-dimensional laser point cloud rapid repositioning method
CN112767456A
