A method for indoor mapping based on vector maps

Abstract

The present invention discloses an indoor mapping method based on a vector map, comprising the following steps: pre-processing laser radar scanning data to obtain a complete outer contour of an indoor environment; extracting straight line segments of each wall using a seed region growing method according to the complete outer contour of the indoor environment; calculating the intersection of straight line segments of different walls according to the slope intercept of the straight line segments, and if the number of scanning frames is 1 at this time, defining intersection_source as a reference straight line segment set source intersection set, otherwise, defining intersection_target as a current straight line segment set parameter intersection set; aligning the reference straight line segment set source intersection set intersection_source and the current straight line segment set parameter intersection set intersection_target, obtaining a rotation matrix and a translation vector and calculating a straight line set target after alignment, and fusing the aligned straight line segment set target under the current frame with the reference straight line segment set source of the previous frame to obtain a fused straight line. The present invention ensures the authenticity of the actual environment reflected by the map and avoids the occurrence of distortion.

Description

Technical Field

[0001] The present invention relates to the technical field of indoor mapping, and in particular to an indoor mapping method based on a vector map. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) can be used to solve the problem of navigation in unknown environments and use map information to locate and build environmental maps at the same time. The accuracy of environmental maps depends on the accuracy of positioning, and the realization of positioning is inseparable from environmental maps.

[0003] Generally speaking, SLAM systems usually include multiple sensors and multiple functional modules. According to the core functional modules, there are two forms: laser radar-based SLAM and vision-based SLAM. Laser SLAM uses 2D or 3D laser radar. 2D laser radar is generally used on indoor robots (such as sweeping robots), while 3D laser radar is generally used in the field of unmanned driving. The emergence and popularization of laser radar makes measurement faster and more accurate, and the information is richer. Usually, the laser SLAM system calculates the distance of the relative movement of the laser radar and the change in posture by matching and comparing two point clouds at different times, thus completing the positioning of the robot itself.

[0004] In 2D laser SLAM, the main research focuses on grid maps and feature maps. Although grid maps are highly accurate and can fully reflect the structure of the environment, they take up too much space. As the map becomes larger, the required memory will also increase. The path planning efficiency is not high and precise robot posture is required.

[0005] And vector Figure 1 It is generally composed of geometric features such as points and lines extracted from the environment, has low spatial complexity, and does not require accurate robot position information. Line extraction using 2D laser data has been studied in many existing works, including segmentation and merging, RANSAC, Hough, and region growing. Existing algorithms are not ideal in terms of line segment extraction efficiency, correctness, and accuracy. Summary of the invention

[0006] Purpose of the invention: In order to overcome the above-mentioned deficiencies of the prior art, the present invention provides an indoor mapping method based on vector maps.

[0007] Technical solution: The indoor mapping method based on vector map of the present invention comprises the following steps:

[0008] (1) Preprocess the laser radar scanning data to obtain the complete outer contour of the indoor environment;

[0009] (2) according to the complete outer contour of the indoor environment, a seed region growing method is used to extract the straight line segment set parameter of each wall surface, and if the number of scanning frames is 1 at this time, source is defined as a reference straight line segment set and parameter is stored;

[0010] (3) Calculate the intersection of the straight line segments of different walls according to the slope intercept of the straight line segment, and if the number of scanning frames is 1 at this time, define intersection_source as the intersection set of the reference straight line segment set source, otherwise, define intersection_target as the intersection set of the current straight line segment set parameter;

[0011] (4) Align the intersection set intersection_source of the reference straight line segment set source and the intersection set intersection_target of the current straight line segment set parameter, obtain the rotation matrix and translation vector, and calculate the aligned straight line set target, and fuse the aligned straight line segment set target in the current frame with the reference straight line segment set source in the previous frame to obtain the fused straight line;

[0012] (5) Find the intersection of the fused straight lines according to the slope intercept of the fused straight lines, and refresh and store it in the set intersection_source;

[0013] (6) Repeat steps (1)-(5) until the number of scanned frames > N 1 , N 1 The number of laser radar scan frames.

[0014] Further, including:

[0015] The step (1) specifically comprises the following steps:

[0016] (11) Use a handheld laser radar scanner to scan the indoor environment, record and store laser frame data lidar_data, and obtain the current frame laser radar scanning data according to the starting position of each frame of laser radar scanning data in lidar_data;

[0017] (12) The coordinates of the laser radar in the Cartesian coordinate system are obtained by the coordinate conversion formula [x_org, y_org], where x_org is the horizontal coordinate and y_org is the vertical coordinate. The coordinate conversion formula is:

[0018]

[0019] Among them, angles is the current frame scanning angle, ranges is the current frame scanning distance;

[0020] (13) Determine whether the current frame scanning point is a data contour point. The judgment formula for the data contour point is:

[0021]

[0022] Among them, r is the result of cross product of two pairs of vectors formed by three adjacent points, ang is the mean polar angle between two points, and ang_2 is the polar angle threshold;

[0023] (14) Remove the point cloud within a radius of 0.5 m with the handheld laser radar scanner as the center, so as to obtain the outer contour of a frame of point cloud data after filtering, which is represented by the matrix [x_filt, y_filt].

[0024] Further, including:

[0025] The step (2) specifically comprises:

[0026] (21) Fit the laser points in the existing seed segment into a straight line segment, calculate the standard deviation std_error of the distance from the laser point in the current seed segment to the straight line segment, and the distance d1 between two adjacent points in the seed segment. The formula that the valid seed segment meets is:

[0027]

[0028] Among them, D_ENDTOBEGIN is the distance threshold between two adjacent points in the seed segment, and D_THTESHOLD is the standard deviation threshold of the distance from the current seed segment to the straight line;

[0029] (22) The subsequent laser points are added to the straight line segment fitted by the seed segment, and the cumulative error d_total_error of the distance from the subsequent laser points to the straight line segment is calculated. The distance d_twopoint between x_filt(j), y_fil(j) and x_filt(j-1), y_fil(j-1) is calculated. The region is continuously grown according to the effective seed segment until the condition for stopping growth is met. The condition for stopping growth is:

[0030] |d_total_error|>D_THTESHOLD

[0031] or

[0032] d_twopoint>4*D_THTESHOLD;

[0033] (23) Calculate the slope of each fitted straight line segment and the distance between the adjacent endpoints of the two straight line segments, and compare the slopes of the two straight line segments. If the slopes are the same and the distances between the endpoints are close, the corresponding straight line segments are considered to belong to the same wall. Otherwise, the corresponding two straight line segments belong to different walls. Find the straight line segments belonging to the same wall again, and then find the straight line segment parameters of all walls. If the number of scan frames is 1 at this time, define source as the reference straight line set and store parameter in source.

[0034] Further, including:

[0035] The cumulative error d_total_error of the distance from the laser point to the straight line segment is then updated as:

[0036] d_total_error=d_total_error+d

[0037] The d_total_error before the equal sign represents the cumulative error after the update of the laser points added later, and d_total_error represents the cumulative error before the update, and

[0038]

[0039] Among them, A, B, C are the coefficients of the straight line segment, (x 0 ,y 0 ) are the coordinates of the laser point added later.

[0040] Further, including:

[0041] The step (4) specifically comprises:

[0042] (41) Calculate the centroid of the two point sets to be registered, intersection_target and intersection_source, and obtain target_mean and source_mean respectively;

[0043] (42) Calculate the displacement of the two point sets to be registered intersection_target and intersection_source relative to their respective centroids target_mean and source_mean, and define them as data_target and data_source respectively;

[0044] (43) Define an empty matrix W, and traverse data_target and data_source to obtain the covariance matrix W through the following formula:

[0045] W=W+data_source(:,j)*data_target(:,j) T

[0046] Among them, data_source(:,j) represents the j-th intersection in data_source, and data_target(:,j) represents the j-th intersection in data_target.

[0047] (44) By applying the SVD decomposition method to W, we get [U, S, V], and then we can get the rotation matrix Rf and the translation vector Tf according to the following formula:

[0048] Rf=U*V T

[0049] Tf=source_mean-Rf*target_mean

[0050] (45) According to the rotation matrix Rf and the translation vector Tf, the target after alignment is obtained using the following formula.

[0051] target=parameter*Rf+Tf

[0052] Further, including:

[0053] The step (5) comprises:

[0054] (51) Get the line set target and the reference line set source after registration, define idx1 as the line index of the reference line set source, and the initial value of idx1 is 1. Define idx2 as the line index of the line set target after registration, and the initial value of idx2 is 1;

[0055] (52) Find and determine whether the straight lines belong to the same wall. If so, store them in I. Otherwise, idx1 = idx1 + 1, and re-determine the straight line idx1 in source and the straight line idx2 in target until idx1 > size(source, 2);

[0056] (53) When all the straight line segments in the reference straight line segment set source are compared with the idx2th straight line in the aligned straight line segment set target, that is, after completing one cycle of step (52), the same wall straight line index I is stored in the cell array I1, and the straight line index idx2 of the aligned straight line set target is increased by 1, that is: idx2=idx2+1, idx1=1, and jump to step (52) again to obtain all the straight line sets and then fuse the straight lines.

[0057] Beneficial effect: Compared with the prior art, the significant advantage of the present invention is that it proposes a method for indoor mapping based on vector maps, uses regional growth to extract straight lines and uses the intersection of straight lines as the feature points for straight line registration, uses stable corner point features for registration, has good anti-interference ability, and makes the two straight line sets meet the optimal match under a certain metric criterion. The use of geometric information to describe the vector map environment ensures the authenticity of the actual environment reflected by the map and avoids the occurrence of distortion. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 This is an overall flow chart of the indoor mapping method based on vector maps according to an embodiment of the present invention;

[0059] Figure 2 A scatter plot of a frame of indoor scanning laser data according to an embodiment of the present invention;

[0060] Figure 3 It is the outer contour of a frame of indoor scanning laser data according to an embodiment of the present invention;

[0061] Figure 4 This is a flow chart of the method for extracting straight lines from seed region growth according to an embodiment of the present invention;

[0062] Figure 5 A region growth result of a frame of indoor scanning laser data points according to an embodiment of the present invention;

[0063] Figure 6 A frame of indoor scanning laser data point area growth merging straight line result according to an embodiment of the present invention;

[0064] Figure 7 This is a flow chart of a two-frame straight line registration method according to an embodiment of the present invention;

[0065] Figure 8 The two-frame straight line registration result described in the embodiment of the present invention;

[0066] Fig. 9 This is a flow chart of a two-frame straight line fusion method according to an embodiment of the present invention;

[0067] Fig.10 This is the result of two-frame straight line fusion described in the embodiment of the present invention. DETAILED DESCRIPTION

[0068] The technical solution of the present invention is described in detail below.

[0069] First, the present invention designs an indoor mapping method based on vector maps, such as Figure 1 As shown, including the following:

[0070] Step 1: Obtain the laser radar scanning data and preprocess the laser radar scanning data to obtain the complete outer contour of the indoor environment. The specific method is as follows:

[0071] Step 1.1: Use a handheld laser radar scanner to scan the indoor environment, record and store the laser frame data lidar_data, obtain the laser radar scan data of the current frame according to the starting position of each frame of laser radar scan data in lidar_data, and use formula (1) to convert the coordinates of the laser radar in the Cartesian coordinate system to obtain [x_org, y_org], where x_org is the horizontal coordinate and y_org is the vertical coordinate:

[0072]

[0073] Among them, angles is the current frame scanning angle, ranges is the current frame scanning distance;

[0074] Step 1.2: Extract the outer contour of the point cloud. Figure 2 It is a scatter plot of indoor scanning laser data. To extract the outer contour of the scattered points, it is necessary to consider the concave points while retaining the convex points. Taking the current frame [x_org, y_org] as an example, each three adjacent points in [x_org, y_org] form two pairs of vectors and perform vector cross multiplication. Formula (2) is used to determine whether the point is a convex point of the data contour. For concave points, the polar angle threshold of formula (2) is used to determine, remove some concave points, and obtain the outer contour of the scattered points. Figure 3 ,like Figure 3 This is the outer contour obtained by removing the concave points within the overall contour of the scatter plot:

[0075]

[0076] Among them, r is the result of vector cross product, ang is the mean polar angle of each point from [x_org, y_org] to [x_org, y_org], and ang_2 is the polar angle threshold;

[0077] Step 1.3: To avoid human interference, remove the point cloud within a 0.5m radius of the handheld laser radar scanner, obtain the outer contour of a frame of point cloud data after filtering, and represent it with the matrix [x_filt, y_filt];

[0078] Step 2: Based on the complete outer contour of the indoor environment, [x_filt, y_filt] is obtained, and the straight line segment set parameter of each wall is extracted using the seed region growing method. If the number of scan frames is 1 at this time, define source as the reference straight line segment set and store parameter in the process as follows Figure 4 As shown, the specific method is:

[0079] Step 2.1: Extract valid seed segments. The seed segments are composed of a small number of continuous laser points. The laser points in the seed segments are fitted into a straight line. In this embodiment, 10 laser points are used to fit a straight line segment. The distance d1 between two adjacent points in the seed segment and the standard deviation std_error of the distance from the laser point to the straight line in the current seed segment are calculated. The validity of the seed segment is determined by formula (3):

[0080]

[0081] Among them, D_ENDTOBEGIN is the distance threshold between two adjacent points in the seed segment, and D_THTESHOLD is the standard deviation threshold of the distance from the current seed segment to the straight line; if the current 10 laser points cannot be used as a valid seed segment, other laser points are reselected as seed points.

[0082] Step 2.2: After finding the valid seed segment, start region growing to obtain the straight line segment.

[0083] Add the subsequent laser points to the straight line segment fitted by the seed segment, define d_total_error as the cumulative error of the distance from the subsequent laser points to the straight line, calculate the distance between x_filt(j), y_fil(j) and x_filt(j-1), y_fil(j-1) and define it as d_twopoint. Usually, the laser points are evenly distributed on both sides of the straight line. Use formula (4) to determine whether the region growth stops, or use formula (5) to determine whether there is a long distance gap between the end point of the seed segment and the subsequent laser point to stop the region growth. The results are as follows: Figure 5 The data is scanned in circles from 45 degrees to 315 degrees. The number of frames is divided according to the characteristics of the data. 45 degrees to 315 degrees is one frame. The laser radar scans a circle, which is less than one circle around the room. This is how growth grows. After growing in one direction, it grows in the next direction.

[0084] |d_total_error|>D_THTESHOLD (4)

[0085] d_twopoint>4*D_THTESHOLD (5)

[0086] Then the cumulative error d_total_error of the distance from the laser point to the straight line segment is updated as:

[0087] d_total_error=d_total_error+d

[0088] The d_total_error before the equal sign represents the cumulative error after the update of the laser points added later, and d_total_error represents the cumulative error before the update, and

[0089]

[0090] Among them, A, B, C are the coefficients of the straight line segment, (x 0 ,y 0 ) are the coordinates of the laser point added later.

[0091] Step 2.3: The slopes of the lines belonging to the same wall are similar, and the distances between the endpoints of the line segments are close. By judging whether the lines with the same slope belong to the same wall, if so, they belong to the same wall. Otherwise, start searching again. The merged results are as follows: Figure 6 , connect different straight line segments with similar slopes end to end to obtain the straight lines of the same wall in this frame.

[0092] Step 3: Find the intersection of different wall lines according to the slope intercept of the line. The specific method is: calculate the intersection of the straight line segments of different walls according to the slope intercept of the straight line segment, and if the number of scan frames is 1 at this time, define intersection_source as the reference straight line segment set source intersection set, otherwise, define intersection_target as the current straight line segment set parameter intersection set. The reference straight line set source and the reference straight line set intersection set intersection_source are two different definitions, one is the obtained straight line, and the other is the intersection of these straight lines. The intersection set intersection_source is obtained through the straight line set source. The straight line is obtained first and then the intersection is found based on the straight line. The intersection of two straight lines can be found by the slope and intercept of the straight line.

[0093] Step 4: Feature point registration, the process is as follows Figure 7 As shown, the reference straight line segment set source intersection point set intersection_source and the current straight line segment set parameter intersection point set intersection_target are registered, the rotation matrix and translation vector are obtained, and the registered straight line set target is calculated, and the registered straight line segment set target in the current frame is fused with the reference straight line segment set source in the previous frame to obtain the fused straight line.

[0094] At this time, the number of scan frames is 1, source is the obtained line set parameter, the number of scan frames>1, parameter is the line set before registration, the rotation matrix and translation vector are obtained through the intersection of the lines obtained in the two frames, and the target after registration is obtained. The subsequent source is the fusion of source and target (after parameter registration), and the source is constantly refreshed. The specific method is:

[0095] Step 4.1: Calculate the centroids of the two point sets to be registered, intersection_target and intersection_source, and obtain target_mean and source_mean respectively;

[0096] Step 4.2: Calculate the displacement of the two points to be registered, intersection_target and intersection_source, relative to their respective centroids target_mean and source_mean, and define them as data_target and data_source respectively;

[0097] Step 4.3: Define an empty matrix W, and traverse data_target and data_source to get the covariance matrix W through the following formula:

[0098] W=W+data_source(:,j)*data_target(:,j) T

[0099] Among them, data_source(:,j) represents the j-th intersection in data_source, and data_target(:,j) represents the j-th intersection in data_target.

[0100] Step 4.4: By using the SVD decomposition method on W, we get [U, S, V], and then we can get the rotation matrix Rf and translation vector Tf according to the following formula:

[0101] Rf=U*V T

[0102] Tf=source_mean-Rf*target_mean

[0103] (45) According to the rotation matrix Rf and the translation vector Tf, the target after alignment is obtained.

[0104] target=parameter*Rf+Tf

[0105] Step 4.5: According to the rotation matrix Rf and the translation vector Tf, use the following formula to get the target after registration. The result after registration is as follows Figure 8 As shown, the thicker straight line is the straight line of the first frame, and the thinner straight line is the straight line of the second frame:

[0106] target=parameter*Rf+Tf。

[0107] Among them, parameter is the set of straight line segments before registration, and target is the set of straight line segments after registration.

[0108] Step 5: In order to obtain the overall wall straight line, the original straight line is merged with the straight line after registration. The process is as follows: Fig. 9 As shown, the specific method is:

[0109] Step 5.1: Get the aligned straight line set target and the reference straight line set source, define idx1 as the source straight line index, define idx2 as the target straight line index, find and determine whether the straight lines belong to the same wall, if so, store it in I, otherwise idx1=idx1+1, and re-determine until idx1>size(source,2), where the initial values ​​idx1=1, idx2=1, I=[], I1={}, w=1.

[0110] Step 5.2: After the loop, the index of the same wall line is stored in the cell array I1. At this time, I1(w) = I, w = w + 1. Check whether idx2 is greater than size(target, 2). If not, idx2 = idx2 + 1 and then perform step 5.1 again. If idx2 is greater than size(target, 2), find the corresponding line and merge it.

[0111] Step 5.3: Repeat steps 5.1 and 5.2 until all the straight lines are obtained and then merge the straight lines, such as Fig.10 ;

[0112] Step 6: Find the intersection of the fused lines according to the slope and intercept of the fused lines. The specific method is: find the intersection of the lines according to the slope and intercept of the lines, and refresh and store it in intersection_source;

[0113] Step 7: Loop through steps 1.1 to 6 until the number of scanned frames > N1, where N1 is the number of scanned frames of the LiDAR, to obtain a vector map.

[0114] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0115] Obviously, those skilled in the art can make various changes and modifications to the embodiments of the present invention without departing from the spirit and scope of the embodiments of the present invention. Thus, if these modifications and variations of the embodiments of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A method for indoor mapping based on vector maps, characterized in that: The following steps are involved: (1) Preprocess the laser radar scanning data to obtain the complete outer contour of the indoor environment; (2) According to the complete outer contour of the indoor environment, the seed region growing method is used to extract the straight line segments of each wall, the slope of each straight line segment and the distance between the adjacent endpoints of two straight line segments are calculated, and the slopes of two straight line segments are compared. If the slopes are the same and the distances between the endpoints are close, it is determined that the corresponding straight line segments belong to the same wall. Otherwise, the corresponding two straight line segments belong to different walls. The straight line segments belonging to the same wall are searched again, and then the straight line segment set parameter of all walls is found. If the number of scanning frames is 1 at this time, source is defined as the reference straight line segment set and parameter is stored; (3) Calculate the intersection of the straight line segments of different walls according to the slope intercept of the straight line segment, and if the number of scanning frames is 1 at this time, define intersection_source as the intersection set of the reference straight line segment set source, otherwise, define intersection_target as the intersection set of the current straight line segment set parameter; (4) Align the reference straight line segment set source intersection point set intersection_source with the current straight line segment set parameter intersection point set intersection_target, obtain the rotation matrix and translation vector, and calculate the aligned straight line set target; (5) Fuse the aligned straight line segment set target in the current frame with the reference straight line segment set source in the previous frame to obtain the fused straight line; (6) Find the intersection of the fused straight lines according to the slope intercept of the fused straight lines, and refresh and store it in the set intersection_source; (7) Repeat steps (1)-(6) until the number of scan frames > N1, where N1 is the number of laser radar scan frames.

2. The indoor mapping method based on vector map according to claim 1, characterized in that: The step (1) specifically comprises the following steps: (11) Use a handheld laser radar scanner to scan the indoor environment, record and store laser frame data lidar_data, and obtain the current frame laser radar scanning data according to the starting position of each frame of laser radar scanning data in lidar_data; (12) The coordinates of the laser radar in the Cartesian coordinate system are obtained by the coordinate conversion formula [x_org, y_org], where x_org is the horizontal coordinate and y_org is the vertical coordinate. The coordinate conversion formula is: Among them, angles is the current frame scanning angle, ranges is the current frame scanning distance; (13) Determine whether the current frame scanning point is a data contour point. The judgment formula for the data contour point is: Among them, r is the result of cross product of two pairs of vectors formed by three adjacent points, ang is the mean polar angle between two points, and ang_2 is the polar angle threshold; (14) Remove the point cloud within a radius of 0.5 m with the handheld laser radar scanner as the center, so as to obtain the outer contour of a frame of point cloud data after filtering, which is represented by the matrix [x_filt, y_filt].

3. The indoor mapping method based on vector map according to claim 2 is characterized in that: The step (2) specifically comprises: (21) Fit the laser points in the existing seed segment into a straight line segment, calculate the standard deviation std_error of the distance from the laser point in the current seed segment to the straight line segment, and the distance d1 between two adjacent points in the seed segment. The formula that the valid seed segment meets is: Among them, D_ENDTOBEGIN is the distance threshold between two adjacent points in the seed segment, and D_THTESHOLD is the standard deviation threshold of the distance from the current seed segment to the straight line; (22) The subsequent laser points are added to the straight line segment fitted by the seed segment, and the cumulative error d_total_error of the distance from the subsequent laser points to the straight line segment is calculated. The distance d_twopoint between x_filt(j), y_fil(j) and x_filt(j-1), y_fil(j-1) is calculated. The region is continuously grown according to the effective seed segment until the condition for stopping growth is met. The condition for stopping growth is: |d_total_error|>D_THTESHOLD or d_twopoint>4*D_THTESHOLD; (23) Calculate the slope of each fitted straight line segment and the distance between the adjacent endpoints of the two straight line segments, and compare the slopes of the two straight line segments. If the slopes are the same and the distances between the endpoints are close, then the corresponding straight line segments are considered to belong to the same wall. Otherwise, the corresponding two straight line segments belong to different walls. Find the straight line segments belonging to the same wall again, and then find the straight line segment set parameter of all walls. If the number of scan frames is 1 at this time, define source as the reference straight line set and store parameter in source.

4. The indoor mapping method based on vector map according to claim 3 is characterized in that: The cumulative error d_total_error of the distance from the laser point to the straight line segment is then updated as: d_total_error=d_total_error+d The d_total_error before the equal sign represents the cumulative error after the update of the laser points added later, and d_total_error represents the cumulative error before the update, and Among them, A, B, C are the coefficients of the straight line segment, and (x0, y0) are the coordinates of the laser point added later.

5. The indoor mapping method based on vector map according to claim 4 is characterized in that: The step (4) specifically comprises: (41) Calculate the centroid of the two point sets to be registered, intersection_target and intersection_source, and obtain target_mean and source_mean respectively; (42) Calculate the displacement of the two point sets to be registered intersection_target and intersection_source relative to their respective centroids target_mean and source_mean, and define them as data_target and data_source respectively; (43) Define an empty matrix W, and traverse data_target and data_source to obtain the covariance matrix W through the following formula: W=W+data_source(:,j)*data_target(:,j) T Among them, data_source(:,j) represents the jth intersection point in data_source, and data_target(:,j) represents the jth intersection point in data_target; (44) By applying the SVD decomposition method to W, we get [U, S, V], and then we can get the rotation matrix Rf and the translation vector Tf according to the following formula: Rf=U*V T Tf=source_mean-Rf*target_mean (45) According to the rotation matrix Rf and the translation vector Tf, the target after registration is obtained using the following formula: target=parameter*Rf+Tf Among them, parameter is the set of straight line segments before registration, and target is the set of straight line segments after registration.

6. The indoor mapping method based on vector map according to claim 5, characterized in that: The step (5) comprises: (51) Get the line set target and the reference line set source after registration, define idx1 as the line index of the reference line set source, the initial value of idx1 is 1, define idx2 as the line index of the line set target after registration, the initial value of idx2 is 1; (52) Find and determine whether the straight lines belong to the same wall. If so, store them in I. Otherwise, idx1 = idx1 + 1, and re-determine the straight line idx1 in source and the straight line idx2 in target until idx1 > size(source, 2); (53) When all the straight line segments in the reference straight line segment set source are compared with the idx2th straight line in the aligned straight line segment set target, that is, after completing one cycle of step (52), the same wall straight line index I is stored in the cell array I1, and the straight line index idx2 of the aligned straight line set target is increased by 1, that is: idx2=idx2+1, idx1=1, and jump to step (52) again to obtain all the straight line sets and then fuse the straight lines.

Images