A lightweight slam segment mapping method

Abstract

The application discloses a kind of lightweight SLAM segmentation mapping methods, comprising: initialization parameter setting before mapping;Current frame multi-camera image and external sensor data are obtained;Incremental build subgraph, and the pose of current frame is updated based on visual correction odometer;Based on the segmentation of geometry and texture consistency;Trajectory alignment and map optimization after subgraph segmentation;Subgraph segmentation map saving and subgraph merging update;The application is based on the segmentation method of geometry and texture consistent map, reduces the space and computing power occupied when constructing and storing visual map;Segmentation mapping method based on multi-memory interaction, when loop occurs during operation, only the subgraph set related to loop relationship exists in current operation memory, avoid wasting memory and time;Segmentation trajectory alignment and map optimization method, after trajectory alignment, only map point is optimized, reduce computing power consumption, while improving optimization effect.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of SLAM mapping, and particularly relates to a lightweight SLAM segmented mapping method. BACKGROUND

[0002] SLAM (Simultaneous Localization and Mapping) refers to a technology that a robot performs self-localization according to self-measurement data and external observation data and performs incremental mapping during movement in an unknown environment, and is widely applied in the fields of automatic driving and intelligent robots. A visual map has advantages of low cost, high precision, strong reusability and strong updateability, and is the most commonly used map form in the SLAM technology.

[0003] However, the visual map information is rich and contains many describable visual features, and the construction, storage and reuse of the visual map have relatively high requirements on computing power and memory, which hinders the technology landing. Therefore, it has important practical application value to design a SLAM mapping method and system capable of constructing a lightweight visual map suitable for repositioning under low computing power consumption.

[0004] One of the difficulties in constructing a lightweight SLAM map under low computing power consumption is that geometric information and texture information are used for actual judgment of mapping, and the two information are not uniformly distributed in each time period, but related to the robot travel path and the scene. The geometric information refers to a trajectory, and the texture information refers to gray distribution information in a photographed image; for example, the photographed image in the current specified mapping time may be a desert, and it is difficult to extract effective feature points; or the feature points in the current specified mapping time and the feature points in the previous time period may not have new changes. In order to make the map feature points change, or the vehicle needs to have sufficient displacement change Δs, or the offset angle of the vehicle loaded with a sensor needs to change, when the offset angle changes, the corresponding scene will also change. The existing technology adopts a traditional mapping method, and the traditional mapping method maps according to a fixed time interval, and saves the map feature information collected in each time interval to the map, when the effective feature points in the time interval are very few or do not have new feature point changes, some invalid information is saved, causing waste of memory space;

[0005] The second difficulty in implementing low-computing-power consumption and constructing a lightweight SLAM mapping is the storage location of the map during the mapping process. There are two storage methods, each with advantages and disadvantages. One is to store all sub-maps in the current memory. When the number of sub-maps is large, the current memory will be occupied with a large amount of space, and the computing power consumed will also increase. The other method is to store multiple sub-maps in multiple memories: there is a storage memory for storing historical sub-maps, and a calculation memory for calculating the current sub-map. The multi-memory mapping method effectively reduces the congestion of the current memory, greatly increasing the operation speed. However, the difficulty of using the multi-memory mapping method is that it sacrifices some accuracy. Although the feature points of the current sub-map exist in the current calculation memory in most cases, global feature points are occasionally used. At this time, the global feature points may have been saved in another sub-map containing the feature points. Since the other sub-map belongs to the historical sub-map, it has been transferred to the storage memory, and the current calculation memory is not the same memory. If the feature points are extracted from the other memory for the current use, the implementation method is very complex. Therefore, the multi-memory mapping method generally sacrifices accuracy. Figure 1 SUMMARY

[0006] To solve the problems in the prior art, the present application provides a lightweight SLAM segmented mapping method. The first purpose is to solve the problem of the prior art that maps according to a fixed time interval, which causes a lot of non-effective information in the time period to be saved in the memory, resulting in high memory occupation and high computing power consumption. The second purpose is to solve the problem of the prior art that the multi-memory mapping method generally sacrifices accuracy. Figure 1

[0007] To solve the technical problems, the present application provides the following technical solutions:

[0008] A lightweight SLAM segmented mapping method, comprising the following steps:

[0009] Step 1: Initialize the parameters before mapping.

[0010] Initialize the default mapping function: B mapping function = true.

[0011] Initialize the default sub-map initialization: B sub-map initialization = false.

[0012] Initialize the default number of frames for sub-map initialization: N initialization frame = 0.

[0013] Initialize the default map merging: B map merging = false.

[0014] ​​Initialization default does not need to connect subgraph: B needs to connect subgraph = false;

[0015] Step two, get the current frame multi-camera image and external sensor data;

[0016] Step three, incremental build subgraph, and the current frame pose based on visual correction odometry update;

[0017] The visual correction based odometry update is to fuse the pose of the current frame given by the external sensor and the pose of the current frame given by the visual sensor tracking method, and the fused data is used as the pose of the current frame; the visual sensor tracking is to obtain the pose of the current n+1 frame by tracking n frames of data; the external sensor includes IMU+wheel speed meter, and the visual sensor includes a camera;

[0018] Step four, segment based on geometric and texture consistency;

[0019] Step five, trajectory alignment and map optimization after subgraph segmentation;

[0020] The trajectory alignment and map optimization is to align the completed subgraph trajectory with the visual correction odometry, update the map points, and locally optimize the map points after completing the subgraph construction;

[0021] Step six, subgraph segmentation map saving and subgraph merging update;

[0022] The subgraph segmentation map saving and subgraph merging update is to establish a connection between new and old subgraphs and realize the storage and update of the completed subgraph after the subgraph optimization in the segment trajectory alignment and map optimization module and the segment again before the mapping based on geometric and texture consistency is opened after the mapping;

[0023] Its characteristics are:

[0024] The incremental build subgraph of step three further includes loop detection based on multi-memory interaction, and the loop detection based on multi-memory interaction includes: if the loop frame is not in the current subgraph, the map is merged, and the map merging is to judge whether the loop frame is in the historical subgraph, and then the subgraph is called out from the memory of the historical subgraph and merged with the subgraph in the current memory;

[0025] The segment based on geometric and texture consistency of step four is to analyze the geometric information and image texture information of the current trajectory to control when to close the currently established subgraph and when to reopen the currently closed subgraph, so as to filter out the non-effective information in the subgraph establishment process and retain the effective information, and realize the lightweight SLAM segment mapping.

[0026] Further, the step three is incrementally building subgraph, and the pose of the current frame is updated by visual correction based odometry, and the specific process is as follows:

[0027] 1) Set the frame number n of subgraph initialization to 5, and initialize it;

[0028] Modify the initialization default value to that the subgraph has been initialized: B subgraph initialization = true;

[0029] 2) Track and locate the current frame;

[0030] The current frame is the n+1 frame after the subgraph initialization to the frame before the current subgraph segmentation;

[0031] 3) Update the current frame by visual correction based odometry;

[0032] 4) Incrementally build the map for the current frame;

[0033] 5) Loop detection;

[0034] The specific process is as follows:

[0035] i. According to the bag-of-words feature, find the loop frame with the similarity satisfying the threshold in the global key frame database;

[0036] ii. If there is no loop frame, do not process;

[0037] iii. If the loop frame is in the current subgraph, perform map fusion;

[0038] iv. If the loop frame is not in the current subgraph, perform map merging;

[0039] Modify the initialization default value to that the map merging has occurred: B map merging occurs = true;

[0040] 6) Whether the current frame is the last frame, if not, continue to process 7), if yes, return to step six to save and update the current subgraph segmentation map and subgraph merging;

[0041] 7) The current frame = the next frame of the current frame;

[0042] 8) Return to process 2) to track and locate the current frame until all the frames of the current subgraph are updated by visual correction based odometry.

[0043] Further, the step three process 1) is subgraph initialization, and the specific process is as follows:

[0044] a. According to the external sensor pose and anchor point external parameter, calculate the anchor point pose of each initialization frame:

[0045]

[0046] wherein, denotes the anchor pose of the t-th frame, T fb denotes the external parameter of the anchor point relative to the external sensor, denotes the external sensor pose of the t-th frame;

[0047] b. Extract feature points and descriptors of the initialization frame set, calculate the descriptor distance of the co-located camera of the t-th frame and the t+1-th frame, and perform feature matching;

[0048] c. The matched 2D-2D feature point pair indicates that the two feature points correspond to the same spatial point. According to the projection model of the camera, the 2D-3D point pair relationship can be expressed as:

[0049]

[0050] wherein, denotes the inverse projection process of the i-th camera model, denotes the external parameter of the i-th camera relative to the anchor coordinate system, denotes the anchor pose of the t-th frame, is the j-th spatial point corresponding to the i-th camera of the t-th frame, denotes the projection point of λ t,i,j denotes the depth on the inverse projection ray vector; therefore, according to the projection model, the camera pose and the 2D-2D feature point matching relationship, the depth of each point on the inverse projection ray vector can be determined by triangulation, and the position of the three-dimensional space can be calculated:

[0051]

[0052] wherein, and represent the pixel coordinates of the j-th matched feature point pair of the i-th camera located in the t-th frame and the t+1-th frame, respectively, denotes the anchor pose of the t-th frame, denotes the inverse of the anchor pose of the t+1-th frame, denotes the external parameter of the i-th camera relative to the anchor coordinate system, denotes the inverse of the external parameter of the i-th camera relative to the anchor coordinate system, and the remaining symbols are described above;

[0053] d. After obtaining the preliminary map points, the map points are fused: the map points of each camera are projected to the same-position camera of other initialization frames except the matched frame pair, feature matching is performed in the projection point area, if there are feature points in the area range, a brute force matching is performed by calculating the Hamming distance between the descriptor of the map point and the feature point, the map point corresponding to the feature point with the closest descriptor distance is fused with the current map point, and the observation relationship between the map point and the frame is updated; the area range is a circular area with a radius of 3 pixels.

[0054] Further, the tracking and positioning of the current frame in the process 2) and the process 3) of the step three, and the visual correction-based odometer update of the current frame, are specifically as follows:

[0055] a. According to the external sensor pose and the anchor point pose of the previous frame, the initial anchor point pose of the current frame is calculated

[0056]

[0057] wherein, represents the anchor point pose of the previous frame, and represent the external sensor poses of the current frame and the previous frame, respectively;

[0058] b. According to the initial pose of the current frame, the map points of each camera of the previous frame are projected to the same-position camera of the current frame, feature matching is performed in the projection point area, an optimization problem is constructed according to the minimization of the projection error, and the primary optimized pose of the current frame is calculated:

[0059]

[0060] wherein, represents the projection process of the i-th camera model, represents the external parameter of the i-th camera relative to the anchor point coordinate system, is the j-th map point corresponding to the i-th camera of the previous frame, represents the matching feature points in the current anchor point frame projection point, k represents the number of cameras, n i represents the number of 3D-2D point pairs matched successfully by the i-th camera;

[0061] c. According to the primary optimized pose of the current frame, the map points corresponding to each camera of the set S covis of co-visible key frames of the previous frame are projected to the same-position camera of the current frame, feature matching is performed in the projection point area, an optimization problem is constructed according to the minimization of the re-projection error, the pose of the current frame is optimized again in a local range, and the secondary optimized pose of the current frame is obtained as the tracking pose of the current frame:

[0062]

[0063] wherein, Pi represents the projection process of the ith camera model, Pij represents the jth map point corresponding to the ith camera in the tth frame in the set of co-visible keyframes, Pij represents Pij represents the projection point of the current frame, k represents the number of cameras, n t,i Pij represents the number of 3D-2D point pairs matched successfully by the ith camera in the tth frame in the set of co-visible keyframes, S covis Pij represents the set of co-visible keyframes of the previous frame;

[0064] d. Pose fusion is performed on the current frame tracking pose and the external sensor pose using a pose graph to obtain a vision-corrected odometry.

[0065] Further, the step three process 4) of incrementally mapping the current frame has the following specific process:

[0066] a. Determine whether the incrementally mapping condition is met. If the number of successfully matched feature points in the current frame tracking process is less than 20, or the relative displacement length from the previous key frame exceeds 0.5 meters, or the time interval from the previous key frame exceeds 2 seconds, the current frame is set as a key frame and incrementally mapped.

[0067] b. According to the tracking pose of the current frame, feature matching is performed between the current frame and the set of adjacent key frames, and new map points are calculated according to the triangulation and map point fusion method in the initialization subgraph step.

[0068] Further, the step three process 5) of the loop frame in the current subgraph, then map fusion is performed, and the specific process is as follows:

[0069] a. Feature matching is performed between the feature points of the current frame and the map points of the loop frame, the relative pose of the current frame and the loop frame, and the loop optimization pose of the current frame are calculated.

[0070] b. According to the relative pose of the two, the map points of the current frame are projected into the loop frame for intra-regional feature matching, and the number of successfully matched feature points pn1 is recorded.

[0071] c. The map points of the loop frame are projected into the current frame for intra-regional feature matching, and the number of successfully matched feature points pn2 is recorded.

[0072] d. If pn1 and pn2 both meet the threshold requirement, such as greater than 50, the loop is successful, otherwise, the loop fails.

[0073] e. If the loop is successful, first, according to the loop optimization pose of the current frame, the adjacent key frame set and the map points are updated.

[0074] f. Project the map points in the adjacent keyframe set of the loopback frame to the current frame and its adjacent keyframe set, perform feature matching in the projected point area, and update the observation relationship between the map points and the keyframes according to the map point fusion method in the initialized submap;

[0075] g. Perform pose graph optimization to optimize the keyframe pose sequence of the updated visual submap and propagate to the map points. Finally, the visual corrected odometry pose is updated according to the pose change amount of the keyframe pose sequence of the visual submap.

[0076] Further, if the loopback frame in the step three process 5) is not in the current submap, map merging is performed, and the specific process is as follows:

[0077] a. Import the submap in which the loopback frame is located into the history memory loop ;

[0078] b. Determine whether the loopback is successful;

[0079] c. If the loopback is successful, first, import the intermediate submap set {Submap u , loop<u<cur} between the submap in which the loopback frame is located and the current submap Submap cur into the current memory;

[0080] d. Propagate and update the adjacent keyframe set and the map points according to the loopback optimized pose of the current frame;

[0081] e. Project the map points corresponding to each camera pair in the adjacent keyframe set of the loopback frame to the same position camera of the current frame and its adjacent keyframe set;

[0082] f. Perform feature matching in the projected point area, and update the observation relationship between the map points and the keyframes according to the map point fusion method in the initialized submap;

[0083] g. Merge the keyframes and map points of Submap loop into Submap cur to obtain the updated Submap cur ;

[0084] h. Perform pose graph optimization to optimize the keyframe pose sequence of all updated visual submaps {Submap u , loop<u≤cur} and propagate to the map points;

[0085] i. Update the visual corrected odometry pose according to the pose change amount of the keyframe pose sequence of all visual submaps.

[0086] Further, the step four of the establishment of the subgraph based on the geometry and texture consistent segmentation, including the following process:

[0087] 1) Calculate the yaw angle change amplitude Δyaw, and the texture information richness h i ;

[0088] 2) When the map division factor meets multiple conditions at the same time, close the mapping;

[0089] Modify the initialization default value to the current subgraph mapping function closed: B mapping function = false;

[0090] Modify the initialization default value to the current subgraph not completed subgraph initialization: B subgraph initialization = false;

[0091] Modify the initialization default value to the current subgraph has been initialized frame number zero: n initialization frame = 0;

[0092] 3) If the current mapping function is in the closed state, when one of the following conditions is met, open the mapping;

[0093] Modify the initialization default value to the current subgraph mapping function open: B mapping function = true;

[0094] Modify the initialization default value to the current subgraph connected subgraph function open: B need to connect subgraph = true;

[0095] Further, the step four process 1) calculates the yaw angle change amplitude Δyaw, and the texture information richness h i , the specific process is as follows:

[0096] a. The attitude change amount of the external sensor data at two times is expressed by Euler angle, and the yaw angle change amplitude Δyaw is obtained;

[0097] b. Calculate the cumulative relative displacement length Δs in the xOy plane from the last mapping state change time node using the vision corrected odometer;

[0098] c. Statistics the number of key frames n kf ;

[0099] d. Calculate the normalized gray level co-occurrence matrix of each camera image of the current frame. The gray level co-occurrence matrix describes the texture by studying the spatial correlation characteristics of gray level. Here, the information entropy of the gray level co-occurrence matrix is used to represent the texture information richness:

[0100]

[0101] Where, h i is the gray level co-occurrence matrix information entropy of the i-th camera image of the current frame, G iH is the normalized gray level co-occurrence matrix of the i-th camera image of the current frame, and l is the gray level, usually set to 2 m , l, m e [0, 8], the larger l is, the larger the calculation amount is, and the more detailed the texture description is.

[0102] Further, when the local map division factor of the step four process 2) meets multiple conditions at the same time, the mapping is closed, and the specific process is as follows:

[0103] a. According to the map division factor, it is judged whether it meets the following three conditions at the same time:

[0104] i. Delta yaw is less than threshold T yaw ;

[0105] ii. Among the k cameras, the gray level co-occurrence matrix information entropy is greater than threshold T h , the camera set C = {C i |i e {i | h i > T h and 1≤i≤k}} is obtained, wherein i represents the camera serial number, k represents the number of cameras, h i is the gray level co-occurrence matrix information entropy of the i-th camera image of the current frame, and C i represents the camera meeting the condition that h i is greater than threshold T h . It is required that the number of cameras contained in the camera set C is less than threshold T cn ;

[0106] iii. The number of map key frames n kf is greater than threshold T kfn or the cumulative relative displacement length Delta s is greater than threshold T s1 ;

[0107] Wherein, threshold T yaw recommended value range is in Since the value range of information entropy h i is [0, 2logl], therefore the selection of threshold T h is related to the setting of gray level l, when the gray level l = 8, the threshold T h recommended value range is 0.6-1.2, when the gray level increases, the threshold can be appropriately increased; in the case that the number of multi-channel cameras is 4, T cn recommended value is 2, if the number of multi-channel cameras increases, the threshold can be appropriately increased; threshold T kfn recommended value range is 15-25, threshold T s1 recommended value range is 20-50m, if the available computing power and memory increase, the two thresholds can be appropriately increased;

[0108] b. If the map partitioning factors meet all three of the above conditions, map creation will be shut down; otherwise, no action will be taken.

[0109] Furthermore, in step four, process 3), if the current mapping function is off, mapping will be enabled under one of several conditions, as follows:

[0110] a. Based on the map partitioning factors, determine whether it falls into one of the following three categories:

[0111] i. Δyaw is greater than or equal to the threshold T yaw ;

[0112] ii.{C i |i∈{i|h i >T h The number of cameras in the range} and 1≤i≤k is greater than or equal to the threshold T. cn ;

[0113] iii. The cumulative relative displacement length Δs is greater than the threshold T s2 ;

[0114] Wherein, threshold T yaw T h T cn The settings have been described above, threshold T s2 The recommended value range is 10-30m. If the available computing power and memory increase, this threshold can be appropriately reduced.

[0115] b. If the map partitioning factor falls under one of the above three conditions, then map creation will be enabled; otherwise, no action will be taken.

[0116] Furthermore, the trajectory alignment and map optimization after subgraph segmentation in step five are performed as follows:

[0117] 1) Align the segmented trajectories of the existing subgraph;

[0118] 2) Optimize the map segmentation based on the updated map point coordinates.

[0119] Furthermore, in step five, process 1), segmented trajectory alignment is performed on the existing subgraph, specifically as follows:

[0120] a. Based on timestamps, align the keyframe pose sequences of the existing subgraphs to the visually corrected odometry:

[0121]

[0122] in, T represents the pose of the i-th keyframe in the existing subgraph. fb This represents the extrinsic parameter of the anchor point relative to the external sensor. The pose of the timestamp in the visually corrected odometry corresponds to the keyframe of the i-th anchor point in the existing subgraph.

[0123] b. Update the map point coordinates frame by frame based on the keyframe poses before and after alignment:

[0124]

[0125] in, This represents the keyframe pose before alignment. This represents the inverse of the aligned keyframe pose. This represents the coordinates of the map points before alignment. This indicates the coordinates of the aligned map points.

[0126] Furthermore, in step five, process 2), the updated map point coordinates are used for segmented map optimization, specifically as follows:

[0127] Within each subgraph, for the existing observation relationships, an optimization problem is constructed based on the reprojection error. The keyframe pose sequence of the subgraph is fixed, and the map points are optimized:

[0128]

[0129] Where, n kf This represents the number of keyframes in the current subgraph, k represents the number of cameras, and n represents the number of keyframes in the current subgraph. t,i This represents the number of 3D-2D point pairs successfully matched by the i-th camera in the t-th frame of the current subgraph. This represents the j-th map point corresponding to the i-th camera in the t-th keyframe. express Matching feature points, This represents the projection process of the i-th camera model. This represents the extrinsic parameters of the i-th camera relative to the anchor point coordinate system. This represents the pose of the t-th keyframe.

[0130] Furthermore, the process of saving the subgraph segments and merging and updating the subgraphs in step six is ​​as follows:

[0131] a. Initialize the new subgraph;

[0132] b. In the already constructed submap cur Record the starting pose of the new submap, establish the connection between the old and new submaps, and then save and close the submap. cur ;

[0133] c. If map merging occurs in step three, modify the submap containing the imported loopback frame. loop and intermediate subgraph set {Submapu , loop<u<cur} is updated: the key frames and map points of Submap cur in the merged subgraph are used to update the key frames and map points of Submap loop where the loop frame is located, while keeping the subgraph start key frame sequence number and subgraph end key frame sequence number recorded in Submap loop unchanged, and then using the subgraph start key frame of Submap loop+1 to update the next subgraph start key frame of Submap loop , then, Submap loop is overwritten and closed, and finally, the updated intermediate subgraph set {Submap u , loop<u<cur} is overwritten and closed.

[0134] d. After completing the segmented map saving and subgraph merging update, start building new subgraphs, and adjust the system mapping state in real time.

[0135] Advantages and effects of the present application

[0136] 1. The map segmentation method based on geometric and texture consistency in the present application mainly has the following technical advantages: only a single subgraph is constructed during mapping, reducing the space and computing power occupied by the visual map during construction and storage; single subgraph is supported for repositioning, reducing the space and computing power occupied by the visual map during reuse; key geometric nodes and information-rich scene areas in the running track are recorded, which can improve the success rate of repositioning.

[0137] 2. The segmented mapping method based on multi-memory interaction in the present application mainly has the following technical advantages: when no loop occurs during running, only a single subgraph exists in the system, and the computing power, memory and time required for mapping are significantly reduced; when a loop occurs during running, only a subgraph set related to the loop exists in the system, avoiding waste of computing power, memory and time.

[0138] 3. The segmented trajectory alignment and map optimization method based on external sensors and loop relationships in the present application mainly has the following technical advantages: trajectory alignment and map optimization are performed on existing subgraphs in the system, reducing the calculation scale and improving the calculation efficiency; the key frame pose sequence in the visual subgraph is aligned with the visual corrected odometry, suppressing the scale distortion in monocular vision and the map distortion in the mapping process; after trajectory alignment, the key frame sequence pose is fixed, and only the map points are optimized, which reduces the computing power consumption and improves the optimization effect. BRIEF DESCRIPTION OF DRAWINGS

[0139] Figure 1A lightweight SLAM segmented mapping method of the present application is shown in the flowchart;

[0140] Figure 2 The incremental mapping and visual-based odometer updating process flowchart of the present application is shown in the flowchart.

[0141] Figure 3 A lightweight SLAM segmented mapping method of the present application is shown in the flowchart. DETAILED DESCRIPTION

[0142] The present application is further explained in conjunction with the accompanying drawings

[0143] Design principle of the present application

[0144] 1. Design difficulty of the present application: The difficulty lies in how to solve the problem that after using segmented switch for lightweight mapping, each small segment cannot be globally optimized and is prone to deformation. The traditional mapping method does not need to be segmented and does not need to perform trajectory alignment operation, and directly performs subsequent optimization. Because there is global information in the non-segmented mapping, the built map is also globally consistent. However, after segmentation, each segment is only a small segment, which cannot be globally optimized. The reason why it cannot be globally optimized is that some global information used for constraint is not in the current memory, but is saved to the history memory. Each small segment will be deformed due to the lack of global constraint.

[0145] 2. Solution of the present application: The segmented switch, multi-memory interaction, and trajectory alignment optimization are organically combined, coexist, and support each other. The trajectory alignment optimization method solves the problem caused by segmentation, the multi-memory interaction method solves the storage of the current segment and the history segment after segmentation and the joint calculation of the current segment and the history segment, and the segmented switch is the basis for the multi-memory interaction and trajectory alignment optimization to work. Through the combination of the three, the problem that each small segment cannot be globally optimized and is prone to deformation after segmentation is solved.

[0146] The segmented switch is determined by calculating the yaw angle change amplitude Δyaw, the texture information richness h i , and the relative displacement length Δs to determine whether to open the current mapping or close the current mapping. When the yaw angle change amplitude Δyaw exceeds the threshold, or the texture information richness h i , or the relative displacement length exceeds the threshold, the mapping is opened. When the yaw angle change amplitude Δyaw and the texture information richness h i are less than the threshold, and the relative displacement length and the map size exceed the threshold, the mapping is closed.

[0147] The multi-memory interaction is accompanied by the segment switch, and since the segment is saved, multiple memories are needed to save the subgraph, at least the memory for the current subgraph calculation and the memory for storing the historical subgraph, and since it is multi-memory, it involves how to interact with multiple memories.

[0148] The trajectory alignment optimization adopts a method of visual correction based odometer, and all key frames in the subgraph are corrected again before segment saving, also known as segment alignment, and the alignment method is to find the pose corresponding to the i th anchor key frame in the existing subgraph based on the timestamp of the visual correction based odometer, and correct the pose of the i th key frame in the existing subgraph one by one with the pose, until all key frames of the current subgraph to be saved are corrected again with the key frames of the visual correction based odometer corresponding to the timestamp one by one, and the alignment optimization before segment saving is realized by using the secondary correction method, solving the problems of each small segment of the segment that cannot be globally optimized and is easy to be deformed in scale. The alignment is to align the current key frame to the pose of the visual correction based odometer.

[0149] Based on the above principle, the application designs a lightweight SLAM segment mapping method.

[0150] A lightweight SLAM segment mapping method as shown in Figure 1 、 Figure 2 includes the following steps:

[0151] Step 1, initialization parameter setting before mapping;

[0152] Initialize the default mapping function: B mapping function = true;

[0153] Initialize the default subgraph initialization: B subgraph initialization = false;

[0154] Initialize the default frame number for subgraph initialization: N initialization frame = 0;

[0155] Initialize the default map merging: B map merging = false;

[0156] Initialize the default subgraph connection: B need to connect subgraph = false;

[0157] Step 2, get the current frame multi-camera image and external sensor data;

[0158] Step 3, incrementally build a subgraph, and update the pose of the current frame based on the visual correction odometer;

[0159] The visual correction-based odometer update is to fuse the pose of the current frame given by the external sensor and the pose of the current frame given by the visual sensor tracking method, and take the fused data as the pose of the current frame; the visual sensor tracking is to obtain the pose of the current n+1 frame by tracking the n frames of data through the visual sensor; the external sensor includes an IMU+wheel speed meter, and the visual sensor includes a camera;

[0160] Step four, segmentation based on geometric and texture consistency;

[0161] Step five, trajectory alignment and map optimization after subgraph segmentation;

[0162] The trajectory alignment and map optimization are to align the completed subgraph trajectory with the visual correction-based odometer after completing the subgraph construction, update the map points, and perform local optimization on the map points;

[0163] Step six, subgraph segmentation map saving and subgraph merging update;

[0164] The subgraph segmentation map saving and subgraph merging update are to establish a connection between new and old subgraphs and realize the storage and update of the completed subgraph after the subgraph optimization module completes the subgraph optimization and the segmentation before the map construction based on geometric and texture consistency is opened again after the map construction;

[0165] The characteristics are:

[0166] The incremental subgraph construction of step three further includes loop detection based on multi-memory interaction, and the loop detection based on multi-memory interaction includes: if the loop frame is not in the current subgraph, map merging is performed, and the map merging is to: if it is judged that the current loop frame is in the historical subgraph, the subgraph is called out from the memory of the historical subgraph and the map merging is performed with the subgraph in the current memory;

[0167] The segmentation based on geometric and texture consistency of step four is to analyze the geometric information and image texture information of the current trajectory to control when to close the currently established subgraph and when to reopen the currently closed subgraph, so as to filter out the non-effective information in the subgraph construction process and retain the effective information, thereby realizing the lightweight SLAM segmentation mapping;

[0168] Further, the incremental subgraph construction of step three and the visual correction-based odometer update of the pose of the current frame have the following specific process:

[0169] 1) Set the frame number n of subgraph initialization to 5, and initialize it;

[0170] Modify the initialization default value to that the subgraph has completed initialization: B subgraph initialization=true;

[0171] 2) Track and localize the current frame;

[0172] The current frame is the n+1 frame after the subgraph initialization to the frame before the current subgraph segmentation;

[0173] 3) Visual correction-based odometry update for the current frame;

[0174] 4) Incremental mapping for the current frame;

[0175] 5) Loop detection;

[0176] The specific process is as follows:

[0177] i. According to the bag-of-words feature, find the loop frame with a similarity that meets the threshold in the global key frame database;

[0178] ii. If there is no loop frame, do not process;

[0179] Supplementary note 1

[0180] If the area where the current frame is located is an area that has been passed through before, loop will occur. During the operation of the SLAM system, the error of the external sensor will accumulate over time. In order to eliminate the cumulative error, loop detection is needed to effectively determine whether the camera has passed through the same place. If it has passed through the same place, the cumulative error can be eliminated through optimization to improve the global consistency of SLAM positioning and mapping.

[0181] iii. If the loop frame is in the current subgraph, perform map fusion;

[0182] iv. If the loop frame is not in the current subgraph, perform map merging;

[0183] Modify the initialization default value to have occurred map merging: B has occurred map merging = true;

[0184] 6) Whether the current frame is the last frame, if it is not the last frame, continue to process 7), if it is the last frame, return to step six to perform current subgraph segmentation map saving and subgraph merging update;

[0185] 7) Current frame = next frame of the current frame;

[0186] 8) Return to process 2) to track and localize the current frame until all frames of the current subgraph have completed visual correction-based odometry update.

[0187] Further, the subgraph initialization of step three process 1) has the following specific process:

[0188] a. According to the external sensor pose and anchor point external parameters, calculate the anchor point pose of each initialization frame:

[0189]

[0190] wherein, denotes the anchor pose of the t-th frame, T fb denotes the external parameter of the anchor point relative to the external sensor, denotes the external sensor pose of the t-th frame;

[0191] b. Extract the feature points and descriptors of the initialization frame set, calculate the descriptor distance of the co-located cameras of the t-th frame and the t+1-th frame, and perform feature matching;

[0192] c. The 2D-2D feature point pairs that match successfully indicate that the two feature points correspond to the same spatial point. According to the projection model of the camera, the 2D-3D point pair relationship can be expressed as:

[0193]

[0194] wherein, denotes the inverse projection process of the i-th camera model, denotes the external parameter of the i-th camera relative to the anchor coordinate system, denotes the anchor pose of the t-th frame, is the j-th spatial point corresponding to the i-th camera of the t-th frame, denotes the projection point of λ t,i,j denotes the depth on the inverse projection ray vector; therefore, according to the projection model, the camera pose and the 2D-2D feature point matching relationship, the depth of each point on the inverse projection ray vector can be determined by triangulation, and the position of the three-dimensional space can be calculated:

[0195]

[0196] wherein, and represent the pixel coordinates of the j-th matched feature point pair of the i-th camera located in the t-th frame and the t+1-th frame, respectively, denotes the anchor pose of the t-th frame, denotes the inverse of the anchor pose of the t+1-th frame, denotes the external parameter of the i-th camera relative to the anchor coordinate system, denotes the inverse of the external parameter of the i-th camera relative to the anchor coordinate system, and the remaining symbols are as described above;

[0197] d. After obtaining a plurality of groups of preliminary map points, map point fusion is performed: the map points of each camera are projected to the same-position camera of other initialization frames except the matching frame pair, feature matching is performed in the projection point area, if there are feature points in the area range, a Hamming distance is calculated between the descriptors of the map points and the current map points for brute-force matching, the map points corresponding to the feature points with the closest descriptors are fused with the current map points, and the observation relationship between the map points and the frames is updated; the area range is a circular area with a radius of 3 pixels.

[0198] Supplementary note 2

[0199] The subgraph initialization takes the pose of the external sensor as the initial anchor point pose, and the three-dimensional coordinates of the map points corresponding to the feature points are calculated through stereo vision. For example, the same corner of a wall is photographed at five positions in the first five frames, and the three-dimensional coordinates of the corner of the wall can be determined through the method of stereo vision in the following steps: step a obtains the three-dimensional coordinates of the vehicle body through the two known conditions on the right side of formula (1) Step b performs 2D feature matching on the five photos to identify which pixels on each photo correspond to the corner of the wall, and the 2D coordinates of the corner of the wall in the photo coordinate system are obtained through 2D feature matching; step c has the 2D coordinates of the corner of the wall in the photo coordinate system of the five frames, and the coordinates of the anchor point (the center point of the vehicle body) in the world coordinate system Four three-dimensional coordinates of the corner of the wall in the world coordinate system are obtained (five frames of data, 1 and 2 can calculate the coordinates of one corner of the wall, 2 and 3 can calculate one... 4 and 5, which calculates four coordinates, and these coordinates should not differ much, but they cannot be exactly the same); step d fuses the four coordinates about the corner of the wall into one coordinate to eliminate the repeated map points. At this time, the three-dimensional coordinates of the corner of the wall are determined.

[0200] Further, the step three process 2) and process 3) track the current frame, and the current frame is updated based on the visual correction of the odometer, and the specific process is as follows:

[0201] a. According to the pose of the external sensor and the anchor point pose of the previous frame, the initial anchor point pose of the current frame is calculated

[0202]

[0203] wherein, the anchor point pose of the previous frame is represented by and the current frame and the previous frame are represented by the pose of the external sensor.

[0204] b. According to the initial pose of the current frame, the map points of each camera of the last frame are projected into the same position camera of the current frame respectively, feature matching is performed in the projection point area, an optimization problem is constructed according to the minimization of the projection error, and the first optimization pose of the current frame is calculated:

[0205]

[0206] wherein, represents the projection process of the i th camera model, represents the external parameter of the i th camera relative to the anchor point coordinate system, is the j th map point corresponding to the i th camera of the last frame, represents The matching feature points of the current anchor point frame projection point, k represents the number of cameras, n i represents the number of 3D-2D point pairs matched successfully by the i th camera;

[0207] c. According to the first optimization pose of the current frame, the map points corresponding to each camera of the common view key frame set S covis of the last frame are projected into the same position camera of the current frame, feature matching is performed in the projection point area, an optimization problem is constructed according to the minimization of the re-projection error, the pose of the current frame is optimized again in a local range, and the second optimization pose of the current frame is obtained as the tracking pose of the current frame:

[0208]

[0209] wherein, represents the projection process of the i th camera model, represents the j th map point corresponding to the i th camera of the t th frame in the common view key frame set, represents in the projection point of the current frame, k represents the number of cameras, n t,i represents the number of 3D-2D point pairs matched successfully by the i th camera in the t th frame in the common view key frame set, S covis represents the common view key frame set of the last frame;

[0210] d. The tracking pose of the current frame and the external sensor pose are fused by using the pose graph to obtain the vision-corrected odometry.

[0211] Supplementary note 3

[0212] 1) Further explanation of the tracking and positioning of the current frame of step three of process 2): the tracking and positioning of the current frame is to calculate the anchor point pose of the current frame according to the external sensor pose and the anchor point pose of the last frame. Assuming that the current frame is the 6th frame, step a) can obtain the initial anchor point pose of the 6th frame through the external sensor poses of the 5th and 6th frames and the anchor point pose of the 5th frame, but this initial anchor point pose is not very accurate, so the more accurate anchor point pose of the 6th frame is calculated based on the initial anchor point pose through the photos obtained by the visual sensor; step b) uses the map points corresponding to the 5th frame that have been calculated before, such as a corner of a wall, the 6th frame also takes a photo of the corner of the wall, and the pixel position of the corner of the wall in the photo of the 6th frame can be calculated according to the three-dimensional coordinates of the corner of the wall and the initial anchor point pose of the 6th frame, but due to errors, the calculated pixel position and the actual pixel position of the corner of the wall in the photo of the 6th frame should be deviated, so optimization is needed, but only one corner of the wall cannot be optimized, and other corners of the wall, corners of the door frame, corners of the flowerpot and the like are also used to construct an optimization problem, and the first optimized pose of the 6th frame is calculated according to formula (5); the first optimized pose is to use all the map points seen by the 5th frame, including the corners of the wall, the corners of the door frame, the corners of the flowerpot and the like, to construct an optimization problem; step c) in order to use as much information as possible to make the pose accurate, the range of the map points is expanded, that is, not only the map points of the 5th frame are used, but also the co-visible frame set of the 5th frame (the difference between formula (6) and formula (5) is that t∈S covis ), such as the co-visible frame set including all the map points corresponding to the 1st, 2nd, 3rd and 4th frames, according to the first optimized coordinates of the 6th frame, the map points are projected onto the photo of the 6th frame, the principle is the same as step b), and a second optimization problem is constructed again, and the second optimized pose of the 6th frame is calculated according to formula (6), and this pose is considered to be more reliable, so it is taken as the tracking pose of the 6th frame. The second optimized pose is to construct a second optimization by using all the map points seen by the 1st, 2nd, 3rd, 4th and 5th frames.

[0213] When the 2D image of the subsequent n+2, n+m frame no longer sees the "corner of the wall", a new map point (such as the corner of the flowerpot) that can be seen in the current pixel is taken as the 2D feature point, and then the 2D images of a plurality of continuous frames take the corner of the flowerpot as the co-visible point and use the map points other than the corner of the flowerpot to perform a first optimization and a second optimization, so as to realize the tracking and positioning of the current frame.

[0214] 2) Further explanation of step 3) of the process of step 3: As mentioned in the previous paragraph, the tracking pose of the 6th frame has been calculated using the visual method. Step d) fuses the tracking pose of the 6th frame calculated using the visual method with the external sensor of the 6th frame to obtain the visual corrected odometry. The reason for fusion is that the external sensor has its shortcomings, such as cumulative error, and the visual tracking pose also has its shortcomings, such as the possibility of false matching when the photo quality is not good. Therefore, fusion is equivalent to complementing each other to obtain more accurate pose information.

[0215] Further, the process of step 3) of step 3) is to perform incremental mapping on the current frame, and the specific process is as follows:

[0216] a. Determine whether the incremental mapping condition is met. If the number of successfully matched feature points in the current frame tracking process is less than 20, or the relative displacement length from the last key frame exceeds 0.5 meters, or the time interval from the last key frame exceeds 2 seconds, the current frame is set as a key frame and incremental mapping is performed.

[0217] b. According to the tracking pose of the current frame, perform feature matching between the current frame and the set of adjacent key frames, and calculate new map points according to the triangulation and map point fusion method in the initialization subgraph step.

[0218] Further, the process of step 5) of step 3) is to perform map fusion on the current subgraph, and the specific process is as follows:

[0219] a. Perform feature matching between the feature points of the current frame and the map points of the loop frame, and calculate the relative pose of the current frame and the loop frame and the loop optimization pose of the current frame.

[0220] b. According to the relative pose of the two, project the map points of the current frame to the loop frame for intra-regional feature matching, and record the number of successfully matched feature points pn1.

[0221] c. Then project the map points of the loop frame to the current frame for intra-regional feature matching, and record the number of successfully matched feature points pn2.

[0222] d. If pn1 and pn2 both meet the threshold requirement, such as greater than 50, the loop is successful, otherwise the loop fails.

[0223] e. If the loop is successful, first propagate and update its adjacent key frame set and map points according to the loop optimization pose of the current frame.

[0224] f. Project the map points in the adjacent key frame set of the loop frame onto the current frame and its adjacent key frame set, perform feature matching within the projected point area, and update the observation relationship between the map points and the key frames according to the map point fusion method in the initialized submap;

[0225] g. Perform pose graph optimization, optimize and update the key frame pose sequence of the visual submap and propagate it to the map points. Finally, update the pose of the visual corrected odometer according to the pose change amount of the key frame pose sequence of the visual submap.

[0226] Further, if the loop frame in process 5) of step three is not in the current submap, map merging is performed, and the specific process is as follows:

[0227] a. Import the submap Submap where the loop frame is located from the historical memory loop ;

[0228] b. Determine whether the loop is successful;

[0229] c. If the loop is successful, first, import the intermediate submap set {Submap cur between the submap where the loop frame is located and the current submap Submap u , loop < u < cur} into the current memory;

[0230] d. According to the loop optimization pose of the current frame, propagate and update its adjacent key frame set and map points;

[0231] e. Project the map points corresponding to each camera in the adjacent key frame set of the loop frame onto the cameras at the same position in the current frame and its adjacent key frame set;

[0232] f. Perform feature matching within the projected point area, and update the observation relationship between the map points and the key frames according to the map point fusion method in the initialized submap;

[0233] g. Merge the key frames and map points of Submap loop into Submap cur to obtain the updated Submap cur ; <00007​​​​​​​​​​

[0237] As mentioned above, if the area where the current frame is located is the area that has been passed through, loop happens. For example, submap 5 is being constructed, and according to the bag-of-words feature, a loop frame that is similar to the current frame is found in the global keyframe database, and if the area where the current frame is located is the area that has been passed through, the loop frame is located in submap Figure 1 .The subsequent loop calculation needs to use the information of submap Figure 1 , 2, 3, and 4, but at this time, the detailed information of submap Figure 1 , 2, 3, and 4 is not in the current memory, so the information of submap Figure 1 , 2, 3, and 4 needs to be imported from the memory of the historical submap to perform the subsequent loop calculation, i.e., map merging.The loop frame is located in submap loop , and the intermediate submap set {Submap u , loop < u < cur} is imported into the current memory, which is submap Figure 1 , 2 , 3, and 4. Figure 1 Figure 2 3

[0238] Supplementary note 5

[0239] The constructed map includes keyframes and map points. In the system, four cameras are used, and a keyframe represents the poses of the four cameras at a certain time and the photos taken by the four cameras at this time. The photo captures a corner, which is a 2D feature point, and the real-world corner is the map point corresponding to the 2D feature point.

[0240] Further, the step four of segmenting before establishing a submap based on geometric and texture consistency includes the following processes:

[0241] 1) Calculate the yaw angle change amplitude Δyaw and the texture information richness h i ;

[0242] 2) When the map division factor meets multiple conditions at the same time, turn off the mapping;

[0243] Modify the initialization default value to turn off the mapping function of the current submap: Bmapping function = false;

[0244] Modify the initialization default value to the current submap that has not completed submap initialization: Bsubmap initialization = false;

[0245] Modify the initialization default value to the number of frames that have been initialized in the current submap: ninitialized frames = 0;

[0246] 3) If the current mapping function is in the off state, turn on the mapping when one of the following conditions is met:​

[0247] Modify the initialization default value to turn on the current subgraph mapping function: B mapping function = true;

[0248] Modify the initialization default value to turn on the current subgraph connection subgraph function: B needs to connect subgraph = true;

[0249] Supplementary note 6

[0250] The yaw angle is the angle of the horizontal plane offset clockwise from north. The yaw angle change is actually the magnitude of the left and right swing of the vehicle head during the vehicle's travel.

[0251] The texture information is the gray scale distribution characteristics of the pixels on the photo. When there are many buildings in a photo, such as houses, mailboxes, etc., the texture information is relatively strong, because there are many gray scale changes in the photo, such as the house corners.

[0252] Further, the step four process 1) calculates the yaw angle change Δyaw and the texture information richness h i , the specific process is as follows:

[0253] a. The attitude change of the external sensor data at two times is expressed by Euler angle, and the yaw angle change Δyaw is obtained;

[0254] b. The relative displacement length Δs in the xOy plane from the last mapping state change time node is calculated by using the vision-corrected odometer;

[0255] c. The number of key frames n kf in the current subgraph is counted;

[0256] d. The normalized gray level co-occurrence matrix of each camera image of the current frame is calculated, and the gray level co-occurrence matrix describes the texture by studying the spatial correlation characteristics of the gray scale. Here, the information entropy of the gray level co-occurrence matrix is used to represent the texture information richness:

[0257]

[0258] Where h i is the information entropy of the gray level co-occurrence matrix of the i-th camera image of the current frame, G i is the normalized gray level co-occurrence matrix of the i-th camera image of the current frame, l is the gray level, and is usually set to 2 m , m ∈ [0, 8], the larger l is, the larger the calculation amount is, and the more detailed the texture description is.

[0259] Further, when the map division factor satisfies multiple conditions at the same time, the mapping is closed in the step four process 2), and the specific process is as follows:

[0260] a. Based on the map partitioning factors, determine whether it simultaneously meets the following three conditions:

[0261] i and Δyaw are less than the threshold T yaw ;

[0262] ii. Among k cameras, the information entropy of the gray-level co-occurrence matrix is ​​greater than the threshold T. h The set of cameras C = {C i |i∈{i|h i >T h and 1≤i≤k}}, where i represents the camera index, k represents the number of cameras, h i Let C be the gray-level co-occurrence matrix information entropy of the i-th camera image in the current frame. i Indicates that h i Greater than threshold T h The set of cameras, C, must contain a number of cameras less than a threshold T. cn ;

[0263] iii. Number of map keyframes n kf Greater than threshold T kfn Or the cumulative relative displacement length Δs is greater than the threshold T s1 ;

[0264] Wherein, threshold T yaw Recommended value range is Due to information entropy h i The range of values ​​is

[0265] [0, 2logl], therefore the threshold T h The selection of threshold T is related to the setting of gray level l. When gray level l = 8, the threshold T is... h The recommended value range is 0.6-1.2. When the grayscale level increases, the threshold can be appropriately increased; when the number of cameras is 4, T... cn The recommended value is 2. If the number of cameras increases, this threshold can be increased appropriately; Threshold T kfn The recommended value range is 15-25, and the threshold T is... s1 The recommended value range is 20-50m. If the available computing power and memory increase, these two thresholds can be increased appropriately.

[0266] b. If the map partitioning factors meet all three of the above conditions, map creation will be shut down; otherwise, no action will be taken.

[0267] Furthermore, in step four, process 3), if the current mapping function is off, mapping will be enabled under one of several conditions, as follows:

[0268] a. According to the map division factor, determine whether it is in one of the following three situations:

[0269] i. The yaw is greater than or equal to the threshold T yaw ;

[0270] ii. The number of cameras in {C i |i∈{i|h i >T h and 1≤i≤k}} is greater than or equal to the threshold T cn ;

[0271] iii. The cumulative relative displacement length As is greater than the threshold T s2 ;

[0272] Wherein, the threshold T yaw , T h , T cn , the setting, has been described above, the threshold T s2 Recommended value range is 10-30m, if the available computing power and memory increase, can appropriately reduce this threshold;

[0273] b. If the map division factor is in one of the above three situations, open the mapping, otherwise do not process.

[0274] Further, the trajectory alignment and map optimization of the segmented subgraph of step five are as follows:

[0275] 1) Segment trajectory alignment for existing subgraph;

[0276] 2) Segment map optimization for updated map point coordinates.

[0277] Further, the trajectory alignment of the existing subgraph in process 1) of step five is as follows:

[0278] a. Align the key frame pose sequence of the existing subgraph to the visual corrected odometry according to the timestamp:

[0279]

[0280] Wherein, represents the i-th key frame pose in the existing subgraph, T fb represents the external parameter of the anchor point relative to the external sensor, represents the timestamp corresponding to the i-th anchor key frame in the existing subgraph in the visual corrected odometry;

[0281] Supplementary note 7

[0282] 1) Normal unsegmented mapping does not need to do alignment operation, directly to the optimization behind, because the unsegmented mapping is global information, the map is consistent globally. But after segmentation each segment only a small segment, a small segment can not be global optimization, each segment due to no global constraints will produce deformation. The solution is to borrow the third step of the design idea of vision correction based on odometry update, since a vision correction based on odometry is obtained, the current key frame pose is corrected by using the odometry, that is, the current key frame is aligned to the vision correction based on odometry.

[0283] 2) The left side of formula (8) is is the key frame to be aligned, represents the timestamp in the vision correction odometry and the pose corresponding to the i th anchor key frame in the existing subgraph; T fb represents the external parameter of the anchor point relative to the external sensor, and the two terms on the right side of formula (8) are known conditions.

[0284] b. According to the key frame pose before and after alignment, update the map point coordinates frame by frame:

[0285]

[0286] wherein, represents the key frame pose before alignment, represents the inverse of the key frame pose after alignment, represents the map point coordinates before alignment, represents the map point coordinates after alignment.

[0287] Further, the step five process 2) updates the map point coordinates, and performs segmented map optimization, the specific method is as follows:

[0288] In each subgraph, for the observation relationship existing therein, an optimization problem is constructed according to the re-projection error, the key frame pose sequence of the subgraph is fixed, and the map point is optimized:

[0289]

[0290] wherein, n kf represents the number of key frames in the current subgraph, k represents the number of cameras, n t,i represents the number of 3D-2D point pairs matched successfully by the i th camera in the t th frame in the current subgraph, represents the j th map point corresponding to the i th camera in the t th key frame, represents the matching feature point, represents the projection process of the i th camera model, Pi represents the extrinsic parameter of the ith camera relative to the anchor coordinate system, Pt represents the tth key frame pose.

[0291] Further, the step six subgraph segmentation map saving and subgraph merging update, the specific process is as follows:

[0292] a. Initialize a new subgraph;

[0293] b. Record the starting point pose of the new subgraph in the subgraph Submap cur that has been constructed, establish the connection relationship between the new and old subgraphs, and then save and close Submap cur ;

[0294] c. If map merging occurs in step three, the memory of the subgraph Submap loop containing the loop frame and the intermediate subgraph set {Submap u , loop < u < cur} is updated: using the key frames and map points in the merged subgraph Submap cur to update the key frames and map points in the subgraph Submap loop containing the loop frame, while keeping the subgraph starting key frame sequence number and the subgraph ending key frame sequence number recorded in Submap loop unchanged, then using the subgraph starting key frame of Submap loop+1 to update the next subgraph starting key frame of Submap loop , then covering Submap loop in the memory and closing, and finally, covering the updated intermediate subgraph set {Submap u , loop < u < cur} in the memory and closing;

[0295] d. After completing the segmentation map saving and subgraph merging update, start constructing a new subgraph, and simultaneously adjust the system mapping state in real time.

[0296] It should be emphasized that the above specific embodiments are only an explanation of the present application, and are not a limitation of the present application, and those skilled in the art can make modifications to the above embodiments without creative contribution after reading the present specification, but as long as it is within the scope of the claims of the present application, it is protected by the patent law.

Claims

1. A lightweight SLAM segmented mapping method, comprising the following steps: Step 1: initialization parameter setting before mapping; Initialize the default open mapping function: B mapping function = true; Initialize the default subgraph initialization not completed: B subgraph initialization = false; Initialize the default number of frames for subgraph initialization to 0: N initialization frames = 0; Initialize the default map merging not occurred: B map merging occurred = false; Initialize the default subgraph connection not required: B subgraph connection required = false; Step 2: obtain the current frame multi-camera image and external sensor data; Step 3: incrementally build a subgraph, and perform visual correction-based odometry update on the pose of the current frame; The visual correction-based odometry update is to fuse the pose of the current frame given by the external sensor and the pose of the current frame given by the visual sensor tracking method, and take the fused data as the pose of the current frame; The visual sensor tracking is to obtain the pose of the current n+1 frame by tracking n frames of data with a visual sensor; the external sensor includes an IMU and a wheel speed meter, and the visual sensor includes a camera; Step 4: segmentation based on geometric and texture consistency; Step 5: trajectory alignment and map optimization after subgraph segmentation; The trajectory alignment and map optimization is to align the completed subgraph trajectory with the visual correction-based odometry, update the map points, and perform local optimization on the map points after completing the subgraph construction; Step 6: subgraph segmented map saving and subgraph merging update; The subgraph segmented map saving and subgraph merging update is to establish a connection between the new and old subgraphs after the subgraph optimization module completes the subgraph optimization and the segmentation before mapping based on geometric and texture consistency opens the mapping again, and to realize the storage and update of the completed subgraph; Characterized in that: The incremental subgraph building of step 3 further includes loop detection based on multi-memory interaction, which includes: if the loop frame is not in the current subgraph, perform map merging, which is to determine if the loop frame is in the historical subgraph, and then call the subgraph from the memory of the historical subgraph to perform map merging with the current memory subgraph; The segmentation based on geometric and texture consistency of step 4 is to analyze the current trajectory geometric information and image texture information to control when to close the currently established subgraph and when to reopen the currently closed subgraph, so as to filter out non-effective information and retain effective information during subgraph establishment, and realize lightweight SLAM segmented mapping, including the following processes: 1) Compute yaw angle change magnitude , and texture information richness ; 2) When the map division factor meets multiple conditions at the same time, close the mapping; Modify the initialization default value to close the current subgraph mapping function: B mapping function = false; Modify the initialization default value to the current subgraph not completing subgraph initialization: B subgraph initialization = false; Modify the initialization default value to the number of frames that the current subgraph has been initialized to zero: n initialization frames = 0; 3) If the current mapping function is in the closed state, open the mapping when one of the following conditions is met: Modify the initialization default value to turn on the current subgraph mapping function: B mapping function = true; Modify the initialization default value to turn on the current subgraph connection subgraph function: B needs to connect subgraph = true; When the local map division factor meets multiple conditions at the same time, the mapping is turned off, and the specific process is as follows: a. According to the map division factor, judge whether it meets the following three conditions at the same time: , less than threshold ; , the camera set whose gray level co-occurrence matrix information entropy is greater than a threshold value in k cameras , wherein i represents the camera serial number, and k represents the number of cameras, is the gray level co-occurrence matrix information entropy of the i-th camera image of the current frame, satisfies is greater than a threshold value , it is required that the number of cameras contained in the camera set C is less than a threshold value ; , number of map keyframes greater than a threshold or accumulated relative displacement length greater than a threshold ; Wherein, the threshold value The value range of ; since the information entropy The value range of Therefore, the selection of the threshold value Related to the setting of the gray level l, when the gray level l=8, the threshold value The value range is 0.6-1.2, when the gray level increases, the threshold value can be appropriately increased; in the case of the number of multi-path cameras being 4, The value is 2, if the number of multi-path cameras increases, the threshold value can be appropriately increased; the threshold value The value range is 15-25, the threshold value The value range is 20-50m, if the available computing power and memory increase, the two threshold values can be appropriately increased; b. If the map division factor meets the above three conditions at the same time, turn off the mapping, otherwise do not process.

2. The lightweight SLAM segment mapping method according to claim 1, wherein, The specific process of the step three is to build an incremental subgraph and update the pose of the current frame based on visual correction odometer update, and the specific process is as follows: 1) Set the subgraph initialization frame number n = 5, and initialize it; Modify the initialization default value to turn on the current subgraph initialization: B subgraph initialization = true; 2) Track and locate the current frame; The current frame is the n+1 frame after subgraph initialization to all frames before the current subgraph segmentation; 3) Update the current frame based on visual correction odometer; 4) Incremental mapping is performed on the current frame; 5) Loop detection; The specific process is as follows: , find loop frames in the global keyframe database that are similar to the current frame according to the bag-of-words features, and the similarity satisfies a threshold; , no loopback frame, not processed; , if the loop frame is in the current submap, then map fusion is performed; , if the loop frame is not in the current submap, then map merging is performed; Modify the initialization default value to turn on the current subgraph initialization: B subgraph initialization = true; 6) Whether the current frame is the last frame, if it is not the last frame, continue process 7), if it is the last frame, return to step six, segment the current subgraph and save the map and update the subgraph; 7) Current frame = next frame of current frame; 8) Return to process 2) to track and locate the current frame until all frames of the current subgraph complete the visual correction odometer update.

3. The lightweight SLAM segment mapping method according to claim 2, wherein, The specific process of the step three process 1) is as follows: a. According to the external sensor pose and anchor point external parameter, calculate the anchor point pose of each initialization frame: (1) wherein, denotes an anchor pose of the t-th frame, denotes an external parameter of the anchor point relative to the external sensor, denotes an external sensor pose of the t-th frame; b. Extract the feature points and descriptors of the initialization frame set, calculate the descriptor distance of the same position camera of the t frame and the t+1 frame, and perform feature matching; c. The matched 2D-2D feature point pair represents that the two feature points correspond to the same space point, and the 2D-3D point pair relationship can be represented according to the camera projection model: (2) wherein, represents the inverse projection process of the i-th camera model, represents the extrinsic parameter of the i-th camera relative to the anchor point coordinate system, represents the anchor point pose of the t-th frame, is the j-th spatial point corresponding to the i-th camera of the t-th frame, represents the projection point of represents the depth on the inverse projection ray vector; thus, according to the projection model, the camera pose and the 2D-2D feature point matching relationship, the depth of each point on the inverse projection ray vector is determined by triangulation, and the position of the three-dimensional space is calculated: (3) wherein, and represent the pixel coordinates of the i-th matched feature point pair of the j-th camera in the t-th frame and the t+1-th frame respectively, denotes the anchor pose in the t-th frame, denotes the inverse of the anchor pose in the t+1-th frame, denotes the extrinsic of the i-th camera with respect to the anchor coordinate system, denotes the inverse of the extrinsic of the i-th camera with respect to the anchor coordinate system, and the rest of the symbols are as described above. d. After obtaining multiple groups of preliminary map points, perform map point fusion: project the map points of each camera to the same position camera of other initialization frames except the matched frame pair, and perform feature matching in the projection point area, if there are feature points in the area, calculate the hamming distance between the descriptors to perform brute force matching, fuse the map point corresponding to the feature point with the closest descriptor distance with the current map point, and update the observation relationship between the map point and the frame; The area range is a circular area with a radius of 3 pixels.

4. The lightweight SLAM segment mapping method according to claim 2, wherein, The specific process of the step three process 2) and process 3) is as follows: a. Compute the current frame initial anchor pose from the external sensor pose and the previous frame anchor pose : (4) wherein, denotes the upper frame anchor pose, and denotes the current frame and the previous frame external sensor pose, respectively; b. According to the initial pose of the current frame, project the map points of each camera of the last frame to the same position camera of the current frame, and perform feature matching in the projection point area, according to the minimization of the projection error, construct an optimization problem, and calculate the first optimization pose of the current frame: (5) wherein, Pi represents the projection process of the i-th camera model, Pi represents the extrinsic parameter of the i-th camera relative to the anchor point coordinate system, is the j-th map point corresponding to the i-th camera of the previous frame, Pi represents Pi represents the matching feature points of the current anchor point frame projection points, k represents the number of cameras, Pi represents the number of 3D-2D point pairs matched successfully by the i-th camera; c. according to the current frame once the optimization of the pose, the last frame of the common view key frame set Each camera corresponding to the map point of the road is projected to the current frame camera at the same position, the feature matching is carried out in the projection point area, the optimization problem is constructed according to the minimization of the re-projection error, the current frame pose is optimized again in a local range, and the current frame secondary optimization pose is obtained as the current frame tracking pose: (6) wherein, denotes the projection process of the i-th camera model, denotes the j-th map point corresponding to the i-th camera of the t-th frame in the set of co-visible keyframes, denotes at the projection point of the current frame, k denotes the number of cameras, denotes the number of 3D-2D point pairs matched successfully by the i-th camera of the t-th frame in the set of co-visible keyframes, denotes the set of co-visible keyframes of the previous frame; d. The current frame tracking pose is fused with the external sensor pose by using the pose graph to obtain the vision-corrected odometry.

5. The lightweight SLAM segment mapping method according to claim 2, wherein, The step three process 4) is incremental mapping for the current frame, and the specific process is as follows: a. Determine whether the incremental mapping condition is met, if the number of successfully matched feature points in the current frame tracking process is less than 20, or the relative displacement length from the last key frame is more than 0.5 meters, or the time interval from the last key frame is more than 2 seconds, the current frame is set as a key frame, and incremental mapping is performed; b. According to the current frame tracking pose, the current frame is matched with the adjacent key frame set, and the new map point is calculated according to the triangulation and map point fusion method in the initialization subgraph step.

6. The lightweight SLAM submapping method of claim 2, wherein, The step three process 5) is loop frame in the current subgraph, then map fusion is performed, and the specific process is as follows: a. Feature matching is performed between the feature points of the current frame and the map points of the loop frame, the relative pose of the current frame and the loop frame is calculated, and the loop optimization pose of the current frame is calculated; b. Project the map points of the current frame to the loop closure frame for local feature matching according to the relative pose of both, record the number of successfully matched feature points ; c. Project the map points of the loop-closing frame to the current frame for intra-region feature matching, and record the number of successfully matched feature points ; d.If and If the threshold requirement is met, greater than 50, then the loopback is successful, otherwise, the loopback fails. e. If the loop is successful, first, according to the loop optimization pose of the current frame, update its adjacent key frame set and map points; f. Project the map points in the adjacent key frame set of the loop frame to the current frame and its adjacent key frame set, and perform feature matching in the projection point area, update the observation relationship between the map points and the key frames according to the map point fusion method in the initialization subgraph; g. Perform pose graph optimization to optimize and update the key frame pose sequence of the visual subgraph and propagate to the map points, and finally, update the vision-corrected odometry pose according to the pose change of the key frame pose sequence of the visual subgraph.

7. The lightweight SLAM segment mapping method according to claim 2, wherein, The step three process 5) is loop frame not in the current subgraph, then map merging is performed, and the specific process is as follows: a. to the subgraph where the imported loop frame resides ; b. Determine whether the loop is successful; c. If the loopback succeeds, first, import the intermediate subgraph set between the subgraph where the loopback frame is located and the current subgraph into the current memory to the current memory;​ d. According to the loop optimization pose of the current frame, update its adjacent key frame set and map points; e. Project the map points corresponding to each camera in the adjacent key frame set of the loop frame to the same position camera of the current frame and its adjacent key frame set; f. Perform feature matching in the projection point area, update the observation relationship between the map points and the key frames according to the map point fusion method in the initialization subgraph; g. combine keyframes, map points to get updated ; h. Pose graph optimization is performed to optimize update all visual subgraphs of the keyframe pose sequence and propagate to map points; i. Update the corresponding vision-corrected odometry pose according to the pose change of the key frame pose sequence of all visual subgraphs.

8. The lightweight SLAM segment mapping method according to claim 1, wherein, The step four process 1) calculates the yaw angle change amplitude , and the texture information richness , the specific process is as follows: a. The attitude change amount of the external sensor data at two time points is expressed in Euler angle, and the change amplitude of the yaw angle is obtained ; b. Calculate the accumulated relative displacement length in the xOy plane from the last mapping state change time node using the vision corrected odometry ; c. Count the number of keyframes in the current subgraph ; d. Calculate the normalized gray level co-occurrence matrix of each camera image of the current frame, and the gray level co-occurrence matrix describes the texture by studying the spatial correlation characteristics of gray level, and the information entropy of the gray level co-occurrence matrix is used to represent the texture information richness: (7) wherein, is the information entropy of the gray level co-occurrence matrix of the i-th camera image of the current frame, is the normalized gray level co-occurrence matrix of the i-th camera image of the current frame, and l is the gray level, which is set as The greater l is, the greater the amount of calculation is, and the more detailed the texture description is.

9. The lightweight SLAM segment mapping method according to claim 1, wherein, The step four process 3) is when the current mapping function is in the off state, the mapping is opened under one of the following conditions, and the specific process is as follows: a. According to the map division factor, determine whether it is in one of the following three conditions: . greater than or equal to a threshold ; . the number of cameras in the camera array is greater than or equal to a threshold ; . cumulative relative displacement length greater than a threshold value ; Wherein, the threshold value , , The threshold value is 10-30m, and if the available computing power and memory increase, the threshold value can be appropriately reduced. b. If the map division factor is in one of the above three conditions, open the mapping, otherwise, do not process.

10. The lightweight SLAM segment mapping method of claim 1, wherein, The step five is subgraph segmentation trajectory alignment and map optimization, and the specific process is as follows: 1) Segment the trajectory alignment of the existing subgraph; 2) Update the map point coordinates and perform segmented map optimization.

11. The lightweight SLAM segment mapping method of claim 10, wherein, The step five process 1) is to align the segmented trajectory of the subgraph, and the specific method is as follows: a. According to the time stamp, align the key frame pose sequence of the existing subgraph to the visual corrected odometry: (8) wherein, denotes the i-th keyframe pose in the existing submap, denotes the extrinsic of the anchor point relative to the external sensor, denotes the pose corresponding to the i-th anchor keyframe in the existing submap with the timestamp in the vision-corrected odometry; b. According to the key frame pose before and after alignment, update the map point coordinates frame by frame: (9) wherein, denotes the keyframe poses before alignment, denotes the inverse of the keyframe poses after alignment, denotes the map point coordinates before alignment, denotes the map point coordinates after alignment.

12. The lightweight SLAM segment mapping method of claim 11, wherein, The step five process 2) is to optimize the updated map point coordinates, and the specific method is as follows: In each subgraph, according to the reprojection error, construct an optimization problem for the observation relationship existing therein, fix the key frame pose sequence of the subgraph, and optimize the map point: (10) wherein, denotes the number of keyframes in the current submap, k denotes the number of cameras, denotes the number of 3D-2D point pairs matched successfully for the i-th camera in the t-th frame in the current submap, denotes the j-th map point corresponding to the i-th camera in the t-th keyframe, denotes the matched feature points of the i-th camera in the t-th keyframe, denotes the projection process of the i-th camera model, denotes the extrinsic parameters of the i-th camera relative to the anchor coordinate system, denotes the t-th keyframe pose.

13. The lightweight SLAM segment mapping method of claim 1, wherein, The step six is to save the segmented map and update the subgraph merging, and the specific process is as follows: a. Initialize a new subgraph; b. In the constructed subgraph record the start pose of the new subgraph, establish the connection between the new and old subgraphs, and then save and close ; c. If map merging occurs in step three, the submap containing the imported loopback frame will be processed. and intermediate subgraph set Perform a memory update: use the merged subgraph Keyframes and map point pairs in the loopback subgraph Update keyframes and map points while retaining The subgraph start keyframe number and subgraph end keyframe number recorded in the file remain unchanged, and then... Subgraph start point keyframe update The next subgraph starting point keyframe, then... Overwrite and close the saved graph. Finally, update the intermediate subgraph set. Overwrite the saved image and close the window; d. After completing the segmented map saving and subgraph merging update, start to build a new subgraph, and simultaneously adjust the system mapping state in real time.

Images