A method for visual positioning and mapping of dynamic objects in complex environments
By separating static and dynamic feature points using ORB feature points and Mask R-CNN segmentation network, and combining adaptive threshold epipolar constraints and improved keyframe strategies, the problem of dynamic objects affecting visual SLAM localization and mapping in complex environments is solved, achieving higher accuracy and more stable localization and mapping.
Patent Information
- Application Number
- CN202310528902.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-11
- Publication Date
- 2025-12-09
- Estimated Expiration
- 2043-05-11
AI Technical Summary
Existing visual SLAM algorithms struggle to achieve accurate localization and mapping due to the influence of dynamic objects in complex environments on visual localization and mapping.
We employ ORB feature points and the instance segmentation network Mask R-CNN to separate static and potential dynamic feature points. We use an adaptive threshold epipolar constraint algorithm to remove dynamic feature points and combine an improved keyframe strategy and a common region detection algorithm for loop closure optimization and map fusion.
It improves positioning accuracy and mapping accuracy in complex environments, suppresses the influence of dynamic objects, and enhances the stability and system accuracy of medium- and long-term data association.
Smart Images
Figure CN116563341B_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the field of visual processing and state estimation, and more particularly to a method for visual positioning and mapping of dynamic objects in a complex environment. BACKGROUND
[0002] SLAM (Simultaneous localization and mapping) refers to determining the pose of a robot or vehicle in an unknown environment by perceiving external environment information, and completing environment mapping based on the pose of the robot or vehicle.
[0003] In the process of practical applications such as intelligent driving, autonomous mobile robots and unmanned aerial vehicles, some key problems of existing visual SLAM solutions need to be solved. One of the key problems is that dynamic objects in a complex environment will affect the entire process of visual positioning and mapping algorithm. Since the classic SLAM algorithm assumes that the environment is static, the appearance of dynamic objects will be understood by the algorithm as a change in the pose of the object itself, causing a change in the coordinates of the observed landmarks in the pixel coordinate system. In addition, other factors in a complex environment, such as human moving some objects, will also affect the results of the visual positioning and mapping algorithm. If the above factors are processed specifically, more accurate positioning and mapping can be achieved.
[0004] In summary, how to provide a method for visual positioning and mapping of dynamic objects in a complex environment is a problem that needs to be solved by those skilled in the art. SUMMARY
[0005] Therefore, the present application provides a method for visual positioning and mapping of dynamic objects in a complex environment, which realizes visual positioning and mapping of dynamic objects in a complex environment.
[0006] To achieve the above purpose, the present application adopts the following technical solutions:
[0007] A method for visual positioning and mapping of dynamic objects in a complex environment, comprising:
[0008] Obtaining image data, extracting ORB feature points in the image data and performing instance segmentation on the image data based on a neural network;
[0009] Calculating semantic ORB feature points based on the ORB feature points and the instance segmentation results;
[0010] Dividing the semantic ORB feature points into static feature points and potential dynamic feature points, performing inter-frame pose estimation based on the static feature point matching results, and removing dynamic feature points from the potential dynamic feature points according to an adaptive threshold epipolar constraint algorithm of feature point matching to obtain a feature point queue K p ;
[0011] based on the feature point queue K p obtain a local map of the current frame, track the camera pose according to the feature point queue K p track the camera pose, obtain inter-frame pose information Pi, and further track and optimize the pose information according to the local map of the current frame;
[0012] select a key frame KF according to a key frame strategy, and generate a map point and optimize a local map of the current key frame based on the key frame KF;
[0013] select and generate a loop and map fusion candidate key frame KF from the key frame KF l based on an improved common region detection algorithm to detect the common region, and after successful common region detection, perform loop optimization and map fusion on the corresponding pose information of the loop and map fusion candidate key frame KF l to obtain an optimized key frame pose P Ki and a globally consistent map.
[0014] Preferably, the semantic ORB feature point is calculated based on the ORB feature point and the instance segmentation result, and specifically includes:
[0015] the target semantics and masks in the instance segmentation result are taken as parameters for extracting the semantic ORB feature point;
[0016] the pixel values corresponding to the target semantics and masks are read according to the positions of the ORB feature points, and the feature points corresponding to the target semantics are determined according to the instance segmentation result, to obtain the ORB feature points.
[0017] Preferably, the inter-frame pose estimation is performed based on the static feature point matching result, and the dynamic feature points are removed from the potential dynamic feature points according to an adaptive threshold epipolar constraint algorithm of feature point matching, and specifically includes:
[0018] the static feature points and the potential dynamic feature points are respectively matched to obtain a static feature point queue and a potential dynamic feature point queue;
[0019] the inter-frame pose estimation is performed based on the static feature point queue to obtain a fundamental matrix;
[0020] an epipolar constraint threshold is determined according to a minimum distance interval of the feature point matching;
[0021] the epipolar equation of the potential dynamic feature point queue of the previous frame in the current frame is calculated according to the fundamental matrix and the feature point pixel coordinates of the potential dynamic feature point queue of the previous frame;
[0022] The distance between the current frame potential dynamic feature point queue and the pixel coordinate system of the epipolar equation is calculated, and whether it is a dynamic feature point is judged according to the epipolar constraint threshold. If the distance is greater than or equal to the epipolar constraint threshold, it is a dynamic feature point, otherwise the potential dynamic feature point is deleted from the queue.
[0023] Preferably, the key frame strategy includes:
[0024] a. The current frame is more than 25 ordinary frames away from the last key frame;
[0025] b. The proportion of the current frame and the reference key frame tracking to the map point is less than 0.85;
[0026] c. The current frame is more than 1s away from the repositioning;
[0027] d. The current frame tracks less than 50 near points, and the near point refers to the feature point whose depth is less than 40 times the baseline length of the camera;
[0028] e. The number of successfully tracked feature matching points is greater than 15;
[0029] f. The number of key frames to be processed does not exceed 2 frames;
[0030] g. When the number of dynamic feature points is removed, the threshold of the proportion of the current frame and the reference key frame tracking to the map point is increased, so that the b strategy is easier to judge success;
[0031] According to the above strategy, the judgment method of whether to be a key frame is: one of strategies a, c and d is met, one of conditions b and d is met, and strategies e and f are met at the same time. It is a key frame, and the g strategy is to adjust the b strategy when the dynamic feature points are removed.
[0032] Preferably, the specific process of generating a map point is:
[0033] A frame of key frame to be processed is taken out from the key frame queue, and the word feature corresponding to the ORB feature point is calculated;
[0034] Remove the unqualified map points in the initial map in the tracking process;
[0035] Based on the matching relationship between a certain number of co-view key frames and the current key frame, triangulation is performed to generate a three-dimensional point;
[0036] After quality inspection, the three-dimensional point is determined as the map point of the current key frame, and the observation relationship between the key frame and the map point is added.
[0037] Preferably, the specific process of local map optimization of the current key frame is:
[0038] The local BA optimization is performed on all map point coordinates in the local map of the current key frame and the pose of the first-level key frame of the current key frame.
[0039] In the local BA optimization, it is assumed that there are n local map points of the current key frame in the variables participating in the optimization, and there are m key frames observing the local map of the current key frame. A BA cost function CostFunction is constructed by using the idea of minimizing the re-projection error:
[0040]
[0041]
[0042] wherein h(ξ j ,p j ) is the pixel coordinate of the ORB feature point in the image corresponding to the jth map point based on the camera observation model, the Lie algebra corresponding to the camera pose is ξ i , the map point coordinate corresponding to the landmark is p j , and i and j represent the key frame number and the local map point number of the current key frame, respectively. ij is the predicted data, the inter-frame pose information P i estimated in the tracking stage is used to obtain the spatial coordinates M i of the landmark, K is the camera intrinsic parameter, and s i is the scale factor.
[0043] Preferably, the common region detection based on the improved common region detection algorithm comprises:
[0044] According to whether the loop and map fusion candidate key frame and the current key frame are in the same global map, all loop and map fusion candidate key frames KF l are divided into a loop candidate key frame group and a map fusion candidate key frame group.
[0045] In the loop and map fusion candidate key frame, all loop and map fusion candidate key frames that have no connection with the current key frame and have common word features are found, denoted as KeyFramCand, the maximum value of the number of common words of each candidate key frame in KeyFramCand and the current key frame is calculated, the 0.8 times of the value is denoted as minCommonWords, and finally only the part with the number of common words greater than minCommonWords is retained in KeyFramCand.
[0046] Count the total number of common words of each group of key frames with common view relationship in the above KeyFrameCand, and select the single loop with the largest number of common words in the first three groups as the initial candidate key frame for map fusion;
[0047] Calculate the relative pose transformation of the initial candidate key frame and the current key frame using the bag-of-words model, and then optimize the relative pose Tam between the two using the projection matching method;
[0048] Based on the relative pose Tam, use the 5 loops with the highest common view degree of the current key frame and the map fusion candidate key frame to perform geometric consistency test on KeyFrameCand. If the geometric consistency test is successful, the common area detection is successful. According to whether the current loop and the map fusion candidate key frame are in the loop candidate key frame group or the map fusion candidate key frame group, determine whether to perform loop optimization and map fusion.
[0049] Preferably, after the common area detection is successful, the loop optimization and map fusion are performed according to the pose information corresponding to the generated loop and map fusion candidate key frame KFl, and the optimized key frame pose PKi and the globally consistent map are obtained, including:
[0050] The semi-static feature points and the map points corresponding to the semi-static feature points in the loop and map fusion candidate key frame are processed, and the first observation time stamp of the semi-static feature points and the map points corresponding to the semi-static feature points is marked. Before the common area detection of the current loop and map fusion candidate key frame, according to the comparison between the first observation time stamp and the current time, the semi-static feature points and the map points corresponding to the semi-static feature points that meet the conditions are deleted.
[0051] The current loop and map fusion candidate key frame KF l If not in the current global map, perform map fusion, if in the current global map, perform loop optimization, obtain a globally consistent map, and eliminate the cumulative error of positioning, and obtain an optimized key frame pose P Ki .
[0052] Preferably, the semi-static feature points and the map points corresponding to the semi-static feature points are processed, including:
[0053] Compare the time interval of the current loop and map fusion candidate key frame with the loop candidate key frame group and the map fusion candidate key frame group used for common area detection, and mark the map points corresponding to the deleted semi-static feature points;
[0054] Add the map points corresponding to the above semi-static feature points to the semi-static map point to-be-deleted queue;
[0055] Traverse the semi-static map point to be deleted queue, check the number of map points corresponding to each semi-static feature point, and when the number of marks is greater than or equal to 50% of the total observation number, it is determined that the semi-static feature point has exceeded the set effective time, and it is deleted in the global map.
[0056] Preferably, the geometric consistency checking process is:
[0057] The projection matching is used to determine whether there is a common view relationship, and the successful verification of 3 of the 5 loopback and map fusion candidate key frames proves that the common area detection is successful; if the number of successful verifications is greater than 0 and less than 3, the time sequence geometry checking is entered: the above-mentioned verification is repeated on the newly added key frames in time sequence, and when the number of successful verifications reaches 3, it is proved that the common area detection is successful, and if both of the two newly added key frames in time sequence fail in the geometry checking, the current loopback and map fusion candidate key frame verification fails.
[0058] The present application has the following advantages:
[0059] (1) For the AGV running problem in a complex environment, dynamic objects and semi-static objects will affect the positioning and mapping effect of the AGV, and the present application combines the instance segmentation network Mask R-CNN to propose a visual SLAM algorithm which can remove dynamic objects and optimize medium and long term data association.
[0060] (2) For dynamic objects in the environment, a dynamic feature point removal method based on semantic information is proposed, semantic feature points are obtained by using instance segmentation results when extracting feature points; potential dynamic features are suppressed by using feature point matching to estimate the inter-frame pose; an adaptive threshold epipolar constraint method is used to determine the state of the potential dynamic feature points by using the obtained fundamental matrix, and different motion degree feature points are accurately removed.
[0061] (3) In order to keep the algorithm in medium and long term data association to maintain better effect, the key frame selection strategy is improved according to the dynamic feature removal, and then the local mapping effect, tracking stability and overall positioning accuracy are improved; for static objects in a complex environment which are easily moved by human beings, semi-static information is processed in the loopback and map fusion candidate frame in combination with time consistency, so as to avoid the influence of the movement of semi-static objects on the system accuracy in long term data association, and improve the loopback optimization effect and map reusability. BRIEF DESCRIPTION OF DRAWINGS
[0062] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or prior art description will be briefly introduced below, and obviously, the drawings in the following description are only embodiments of the present application, and other drawings can be obtained by those skilled in the art without creating labor.
[0063] Figure 1 The attached figure is a flowchart of the visual localization and mapping method for handling dynamic objects in complex environments provided by the present invention. Detailed Implementation
[0064] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0065] This invention discloses a method for visual localization and mapping of dynamic objects in complex environments, such as... Figure 1 As shown, it includes:
[0066] Acquire image data, extract ORB feature points from the image data, and perform instance segmentation of the image data based on a neural network;
[0067] Calculate semantic ORB feature points based on ORB feature points and instance segmentation results;
[0068] Semantic ORB feature points are divided into static feature points and potential dynamic feature points. Inter-frame pose estimation is performed based on the static feature point matching results. Dynamic feature points are then removed from the potential dynamic feature points using an adaptive threshold epipolar constraint algorithm based on feature point matching, resulting in a feature point queue K for localization and mapping. p ;
[0069] Based on feature point queue K p Obtain the local map of the current frame, based on the feature point queue K. p Track camera pose and obtain inter-frame pose information P i Based on the local map of the current frame, further track and optimize pose information;
[0070] Keyframes (FK) are selected based on the keyframe strategy, and map points are generated and local map optimization is performed based on the keyframes (FK).
[0071] Select and generate candidate keyframes KF for loop closure and map fusion from keyframe FK. l Common region detection is performed based on an improved common region detection algorithm. After successful common region detection, the generated loop closure and map fusion candidate keyframes (KF) are used. l The corresponding pose information is used for loop closure optimization and map fusion to obtain the optimized keyframe pose P. Ki A map that is consistent with the global map.
[0072] In the embodiment, the semantic information of each target in the image is obtained based on a Mask R-CNN instance segmentation neural network model, and for a dynamic target in the environment, after the potential dynamic target is recognized by using the Mask R-CNN instance segmentation neural network model, an instance segmentation result of each target is obtained.
[0073] In the embodiment, the semantic ORB feature points are divided into potential dynamic, semi-static and pure static three kinds, wherein the semi-static semantic refers to an object that is easy to be moved artificially.
[0074] Frame pose estimation is performed based on the static feature point matching result, and a dynamic feature point is removed from the potential dynamic feature point based on an adaptive threshold epipolar constraint algorithm of feature point matching, comprising:
[0075] The static feature points and the potential dynamic feature points are respectively subjected to feature point matching to obtain a static feature point queue and a potential dynamic feature point queue;
[0076] Frame pose estimation is performed based on the static feature point queue to obtain a fundamental matrix;
[0077] An epipolar constraint threshold is determined according to a minimum distance interval of feature point matching;
[0078] According to the feature point pixel coordinates of the last frame potential dynamic feature point queue and the fundamental matrix, an epipolar equation of the last frame potential dynamic feature point queue in the current frame is calculated;
[0079] The distance of the current frame potential dynamic feature point queue and the epipolar equation in the pixel coordinate system is calculated, whether it is a dynamic feature point is judged according to the epipolar constraint threshold, if the distance is greater than or equal to the epipolar constraint threshold, it is, otherwise the potential dynamic feature point is deleted from the queue.
[0080] The method of the application utilizes the semantic information of the feature points and the geometric constraint in the camera motion process to judge the potential dynamic feature points as static or dynamic, more static feature points can be reserved for positioning, and the adaptive threshold epipolar constraint algorithm is used to remove the dynamic feature points, so that different motion degree feature points can be accurately removed, and therefore the accuracy and robustness of the positioning and mapping algorithm can be improved.
[0081] In the embodiment, after the dynamic feature points are removed, the system formally enters the tracking process, first estimates the pose by frame matching (Scan-Scan), as the first stage tracking. The initial pose tracking uses three models: constant speed motion model to estimate the pose, reference frame to estimate the pose, and reposition to estimate the pose. That is, whether the system initialization is completed is detected, the system initialization obtains a local map, after the initialization is completed, three methods of constant speed model, reference key frame tracking model and reposition tracking model are used to track the camera pose according to the feature point queue, and the interframe pose transformation is obtained.
[0082] After the first stage tracking is successful, the local map tracking of the current frame is entered, and the local map tracking of the current frame is based on the matching (Scan-Map) of the ORB feature points and the map points. Since the drift of the current frame pose and the local map point coordinates after the local optimization is corrected, the error is small, and therefore, the observation information of the local map by the key frame before the current frame is used to further optimize the pose in the local map tracking stage of the current frame, so that higher positioning accuracy of the current frame can be achieved.
[0083] After the local map tracking and optimization of the current frame pose, the total number of the successfully matched and tracked feature points is counted to determine whether the tracking is successful.
[0084] In the embodiment, the key frames are selected according to the key frame strategy, each key frame calculates and updates the local map of the current key frame, the local map of the current key frame is obtained, and the local maps of the current key frames constitute a global map, and the specific indexes are as follows:
[0085] The key frame strategy includes:
[0086] a. The current frame is more than 25 normal frames away from the last key frame;
[0087] b. The ratio of the tracked map points of the current frame to the reference key frame is less than 0.85, the reference key frame is selected from the key frames, and the relationship between the reference key frame and the current frame is that the reference key frame is the key frame with the strongest co-visibility relationship with the current frame, and the reference key frame is generally the last key frame of the current frame;
[0088] c. The current frame is more than 1s away from the repositioning;
[0089] d. The number of near points tracked by the current frame is less than 50, and the near point refers to the feature point with a depth less than 40 times the baseline length of the camera;
[0090] e. The number of successfully tracked feature matching points is greater than 15;
[0091] f. The number of key frames to be processed is not more than 2 frames;
[0092] g. When the number of removed dynamic feature points exceeds a certain number m, the threshold of the ratio of the tracked map points of the current frame to the reference key frame is increased, so that the strategy b is more likely to be judged to be successful;
[0093] According to the above strategy, the judgment method for determining whether the key frame is as follows: one of the strategies a, c and d is satisfied, one of the conditions b and d is satisfied, and the strategies e and f are simultaneously satisfied, that is, (a||c||d)&&(b||d)&&e&&f, and the strategy g is an adjustment of the strategy b when the dynamic feature points are removed.
[0094] In the embodiment, the map point generation process is specifically as follows:
[0095] A frame of key frame to be processed is taken out from the key frame queue, and the word features corresponding to the ORB feature points of the frame are calculated.
[0096] Map points in the initial map that are unqualified in the tracking process are removed.
[0097] Based on the matching relationship between a certain number of co-view key frames and the current key frame, triangulation is performed to generate three-dimensional points.
[0098] After quality inspection of the three-dimensional points, the three-dimensional points determined as the map points of the current key frame are added with the observation relationship between the key frame and the map points.
[0099] Further, in the above process, the same landmark point is observed by different key frames to generate multiple map points. Therefore, the map points of the current key frame and adjacent key frames are checked and fused, and the process is completed to merge the repeated map points and update the descriptors, depths, and average observation directions of the map points.
[0100] In the embodiment, the local map optimization process of the current key frame is specifically as follows:
[0101] The local BA optimization is performed on the coordinates of all map points in the local map of the current key frame and the pose of the primary and secondary key frames of the current key frame based on the observation relationship of the map points by the primary and secondary key frames of the current key frame, the current key frame, and the primary and secondary key frames of the current key frame.
[0102] In the local BA optimization, it is assumed that there are n map points in the local map of the current key frame in the variables participating in the optimization, and there are m key frames observing the local map of the current key frame, and a BA cost function is constructed using the idea of minimizing the re-projection error:
[0103]
[0104] The primary key frame refers to the key frame having a direct co-view relationship with the current key frame, and the secondary key frame refers to the key frame having an indirect co-view relationship, i.e., the key frame in which other map points observed by the primary key frame form a direct co-view relationship.
[0105] In this embodiment, when there are temporarily stationary potential dynamic targets in a complex environment and semi-static targets such as stationary AGV vehicles and goods in the warehouse which are easily moved by human beings, they will not be removed in the dynamic feature elimination stage, thus interfering with the medium and long-term data association of the visual SLAM algorithm, and the movement of the above two types of targets after a period of time may cause the accuracy of loop detection to decrease, the system accuracy to decrease due to the change of the position of the map points participating in optimization after loop detection and map fusion, etc. Considering the influence of semi-static targets on long-term data association, a long-term data association method based on semantic feature points is proposed, that is, the loop and map fusion candidate key frames FK l are selected and generated from the key frames KF l , common region detection is performed based on the improved common region detection algorithm, and after successful common region detection, loop optimization and map fusion are performed according to the corresponding pose information of the loop and map fusion candidate key frames FK
[0106] Based on semantic feature points, the semi-static feature points in the candidate key frames for loop and map fusion detection and the corresponding map points are processed, and the first observation time stamp of the semi-static feature points and the corresponding map points is marked respectively. Before the common region detection of the current loop and map fusion candidate key frames, an inhibition strategy is formulated according to the time relationship, that is, the map points corresponding to the semi-static feature points that meet the conditions are deleted according to the comparison of the first observation time stamp and the current time;
[0107] The current loop and map fusion candidate key frames KF l If not in the current global map, map fusion is performed, if in the current global map, loop optimization is performed, a global consistent map is obtained, and the cumulative error of positioning is eliminated, and an optimized key frame pose P Ki
[0108] In this embodiment, the semi-static information processing based on semantic feature points is as follows:
[0109] In a complex environment, the movement of semi-static objects has randomness, which may be temporarily stationary or may remain stationary for a long time, therefore, the current system time stamp is added to the key frame information, and the following improvements are made before loop and map fusion detection:
[0110] 1. The time interval comparison is performed between the current loop and map fusion candidate key frames and the loop candidate key frame group and the map fusion candidate key frame group used for common region detection, the time interval comparison is the comparison of the first observation time and the current time, and the map points corresponding to the deleted semi-static feature points are marked;
[0111] 2. The above semi-static map points are added to the semi-static map point deletion queue;
[0112] 3. Traverse the semi-static map point deletion queue, check the number of map points corresponding to each semi-static feature point, and when the number of map points is greater than or equal to 50% of the total number of observations, it is determined that the semi-static feature point has exceeded the set effective time, and it is deleted in the global map.
[0113] In the present embodiment, the improved common area detection algorithm includes:
[0114] Based on the processing of semi-static information in the long-term data, the current frame is subjected to common area detection. The common area detection is to simultaneously detect the possible closed loop in the robot movement process and the fusion with the historical map, and the specific task is to find the key frame having a loop or map fusion relationship with the current key frame from the candidate frames. The algorithm flow is as follows:
[0115] According to whether the loop and map fusion candidate key frame and the current key frame are in the same global map, all loop and map fusion candidate key frames are divided into a loop candidate key frame group and a map fusion candidate key frame group;
[0116] In the loop and map fusion candidate key frame, all loop and map fusion candidate key frames having no connection relationship with the current key frame and having common word features are found, denoted as KeyFramCand, the maximum value of the common word number of each candidate key frame in KeyFramCand and the current key frame is calculated, and the 0.8 times of the value is denoted as minCommonWords, and finally only the part of KeyFramCand having a common word number greater than minCommonWords is reserved;
[0117] The total number of common words of each key frame group having a common view relationship in the above KeyFrameCand is calculated, and the single loop and map fusion candidate key frame having the largest common word number in the first 3 groups is selected as the initial candidate key frame;
[0118] The relative pose transformation of the initial candidate key frame and the current key frame is calculated by using the bag-of-words model, and the relative pose Tam of the two is optimized by using the projection matching method.
[0119] Based on the relative pose obtained in the previous step, the geometric consistency of the current key frame with the highest degree of co-visibility is checked with the 5 loopbacks and the map fusion candidate key frame KeyFrameCand: that is, whether there is a co-visibility relationship is judged by projection matching, and if 3 of the 5 are successful, it is proved that the common area detection is successful, and according to the current loopback and the map fusion candidate key frame in the loopback candidate key frame group or the map fusion candidate key frame group, loopback optimization and map fusion are determined; if the number of successful verification is greater than 0 and less than 3, the time sequence geometric verification is entered: the above-mentioned verification is repeated for the newly added key frames in time sequence, and when the number of successful verification reaches 3, it is proved that the common area detection is successful, and if the time sequence geometric verification fails for two newly added key frames in succession, the current loopback and map fusion candidate key frame verification fails.
[0120] After the common area detection is successful, if the common area is a match established by the current key frame and the key frame in the current active map, loopback optimization is performed; if the common area is a match established by the current key frame and the key frame in the historical map, map fusion is performed; if both map fusion and loopback are detected, the loopback is ignored, and map fusion usually involves larger range of data association, and a more complete map consistent globally can be obtained after map fusion, so the priority of map fusion is higher.
[0121] The application optimizes the key frame selection strategy: considering the reduction of feature points caused by dynamic feature point deletion, which further leads to more sparse local map points, and accordingly adjusting makes the number of selected key frames increase within a reasonable range when the dynamic feature points are removed, ensuring the stability of tracking and the effect of local optimization; the loopback and map fusion detection algorithm is optimized: the timestamp of the semi-static feature point is marked in the loopback and map fusion candidate frame, and whether the semi-static feature point and the map point need to be suppressed is judged according to the timestamp during common area detection, so as to avoid the influence of the semi-static object moved by human on the positioning and mapping results.
[0122] The embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments, and the same and similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple, and the related parts can be referred to the method part.
[0123] The above description of the disclosed embodiments enables a person skilled in the art to implement or use the application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the application. Therefore, the application will not be limited to the embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A method for visual positioning and mapping of dynamic objects in complex environments, the method comprising: The method comprises the following steps: acquiring image data, extracting ORB feature points in the image data, and performing instance segmentation on the image data based on a neural network; calculating semantic ORB feature points based on the ORB feature points and the instance segmentation result; The semantic ORB feature points are divided into static feature points and potential dynamic feature points, inter-frame pose estimation is performed based on the matching results of the static feature points, and the dynamic feature points are removed from the potential dynamic feature points according to an adaptive threshold epipolar constraint algorithm of feature point matching, to obtain a feature point queue K for positioning and mapping p ; Based on the feature point queue K p Obtain the local map of the current frame, track the camera pose according to the feature point queue K p Track the camera pose to obtain inter-frame pose information P i Further track and optimize the pose information according to the local map of the current frame; selecting a key frame KF according to a key frame strategy, and generating a map point and optimizing a local map of the current key frame based on the key frame KF; selecting and generating loop and map fusion candidate key frames KF from key frames KF l , performing common region detection based on an improved common region detection algorithm, and after successful common region detection, performing loop optimization and map fusion on the corresponding pose information of the loop and map fusion candidate key frames KF l to obtain optimized key frame poses P Ki and a globally consistent map, specifically comprising: processing semi-static feature points in a loop and map fusion candidate key frame and map points corresponding to the semi-static feature points, marking a first observation time stamp of the semi-static feature points and the map points corresponding to the semi-static feature points, and deleting the semi-static feature points and the map points corresponding to the semi-static feature points that meet the conditions before common area detection of the current loop and map fusion candidate key frame; Current loop and map fusion candidate key frame KF l If not in the current global map, map fusion is performed, if in the current global map, loop optimization is performed, a globally consistent map is obtained, and the cumulative error of positioning is eliminated, and an optimized key frame pose P is obtained Ki .
2. The method of claim 1, wherein, calculating semantic ORB feature points based on the ORB feature points and the instance segmentation result, specifically comprising: taking each target semantic and mask in the instance segmentation result as a parameter for extracting semantic ORB feature points; reading pixel values corresponding to each target semantic and mask according to the positions of the ORB feature points, and determining feature points corresponding to each target semantic according to the instance segmentation result to obtain the ORB feature points.
3. The method of claim 1, wherein, performing inter-frame pose estimation based on the static feature point matching result, and removing dynamic feature points from potential dynamic feature points according to an adaptive threshold epipolar constraint algorithm of feature point matching, specifically comprising: performing feature point matching on the static feature points and the potential dynamic feature points respectively to obtain a static feature point queue and a potential dynamic feature point queue; performing inter-frame pose estimation based on the static feature point queue to obtain a fundamental matrix; determining an epipolar constraint threshold according to a minimum distance interval of feature point matching; calculating an epipolar equation of the potential dynamic feature point queue of the last frame in the current frame based on the fundamental matrix and the feature point pixel coordinates of the potential dynamic feature point queue of the last frame; calculating the distance of the potential dynamic feature point queue of the current frame to the epipolar equation in the pixel coordinate system, and judging whether it is a dynamic feature point according to the epipolar constraint threshold, if the distance is greater than or equal to the epipolar constraint threshold, it is a dynamic feature point, otherwise, the potential dynamic feature point is deleted from the potential dynamic feature point queue.
4. The method of claim 1, wherein, The key frame strategy comprises: a. more than 25 normal frames are generated between the current frame and the last key frame; b. the proportion of map points tracked by the current frame and the reference key frame is less than 0.85; c. the current frame is more than 1s away from the repositioning; d. the number of near points tracked by the current frame is less than 50, the near point refers to a feature point with a depth less than 40 times the length of the camera baseline; e. the number of successfully tracked feature matching points is greater than 15; f. the number of key frames to be processed is not more than 2 frames; g. when the number of removed dynamic feature points exceeds a certain number m, the threshold of the proportion of map points tracked by the current frame and the reference key frame is increased, so that the strategy b is easier to judge success; According to the above strategy, the judgment method of whether to be a key frame is: meeting one of strategies a, c and d, and meeting one of conditions b and d, and meeting strategies e and f at the same time, then it is a key frame, and when judging the key frame, the strategy g adjusts the strategy b when the dynamic feature points are removed.
5. The method of claim 1, wherein, The specific process of generating a map point is: Take out a frame of key frame to be processed from the key frame queue, and calculate the word features corresponding to the ORB feature points of the frame; Remove the unqualified map points in the initial map during the tracking process; Based on the matching relationship between a certain number of co-visible key frames and the current key frame, triangulation is performed to generate three-dimensional points; After quality inspection of the three-dimensional points, the map points of the current key frame are determined, and the observation relationship between the key frame and the map points is added.
6. The method of claim 5, wherein, The specific process of local map optimization of the current key frame is as follows: Through the first and second key frames of the current key frame and the observation relationship of each key frame to its map points, the coordinates of all map points in the local map of the current key frame and the pose of the first key frame of the current key frame are optimized by local BA. During local BA optimization, assuming that there are n local map points in the variable participating in optimization, and m key frames observing the local map of the current key frame, a BA cost function CostFunction is constructed using the idea of minimizing the re-projection error: where h(ξ i ,p j ) is the pixel coordinate of the ORB feature point in the image corresponding to the jth map point based on the camera observation model, the Lie algebra corresponding to the camera pose is ξ i , the map point coordinate corresponding to the road sign is p j , i and j represent the key frame number and the local map point number of the current key frame respectively, and z ij is the prediction data.
7. The method of claim 6, wherein the method further comprises: Common region detection based on the improved common region detection algorithm includes: According to whether the loop-back and map fusion candidate key frame and the current key frame are in the same global map, all loop-back and map fusion candidate key frames KF l are divided into a loop-back candidate key frame group and a map fusion candidate key frame group; Find the loop and map fusion candidate key frames that have no connection with the current key frame and have common word features, denoted as KeyFrameCand, calculate the maximum value of the common word number of each candidate key frame in KeyFrameCand and the current key frame, and the 0.8 times of the value is denoted as minCommonWords, finally only the part of KeyFrameCand with common word number greater than minCommonWords is retained; Calculate the total number of common words of each key frame group with co-visibility relationship in the above KeyFrameCand, and select the single loop and map fusion candidate key frame with the most common word number in the top 3 groups as the initial candidate key frame; Calculate the relative pose transformation between the initial candidate key frame and the current key frame using the bag-of-words model, and then optimize the relative pose Tam between the two using the projection matching method; Based on the relative pose Tam, use the 5 loop and map fusion candidate key frames with the highest co-visibility degree of the current key frame to perform geometric consistency test on KeyFrameCand, if the geometric consistency test is successful, the common region detection is successful, and according to whether the current loop and map fusion candidate key frame is in the loop candidate key frame group or the map fusion candidate key frame group, loop optimization and map fusion are determined.
8. The method of claim 7, wherein, Processing of semi-static feature points and map points corresponding to semi-static feature points includes: Compare the time intervals of the current loop and map fusion candidate key frame and the loop candidate key frame group and the map fusion candidate key frame group used for common region detection, and mark the map points corresponding to the deleted semi-static feature points; Add the map points corresponding to the above semi-static feature points to the semi-static map point deletion queue; Iterate through the semi-static map point deletion queue, check the number of marks of the map points corresponding to each semi-static feature point, and when the number of marks is greater than or equal to 50% of the total number of observations, it is determined that the semi-static feature point has exceeded the set effective time, and it is deleted in the global map.
9. The method of claim 7, wherein, The geometric consistency test process is as follows: The common area detection is successful if three of the five loops are successfully verified by the map fusion candidate key frame; if the number of successful verification is greater than 0 and less than 3, the time sequence geometry verification is performed: the above verification is repeated on the newly added key frames in time sequence, and the common area detection is successful if the number of successful verification reaches 3; if the time sequence geometry verification fails for two newly added key frames in time sequence, the current loop and the map fusion candidate key frame verification fails.