Cross-floor reverse vehicle searching method

Abstract

The invention belongs to the technical field of visual SLAM (Simultaneous Localization and Mapping), and particularly relates to a cross-floor reverse vehicle searching method. In order to solve the problem of inaccurate cross-floor positioning, the method comprises the following steps: firstly, synchronously collecting camera and IMU data, and laying a foundation for subsequent data fusion; and then a real-time three-dimensional dense semantic map is constructed through camera data, and semantic segmentation is performed to obtain semantic information of different areas in the environment, so that deep understanding of the environment is improved. And generating a real-time trajectory diagram by using a VIO technology, and marking the floor where the trajectory is located. The loopback detection corrects the track error by identifying the position error of the same region, and improves the robustness. And finally, by minimizing an error between the camera and IMU data, the positioning accuracy is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of visual SLAM, and in particular relates to a cross-floor reverse vehicle search method. Background Art

[0002] With the accelerated development of urbanization, the number of vehicles continues to increase, while parking resources are relatively limited, especially in busy commercial and residential areas of cities, where land resources are relatively scarce, leading to parking problems and the demand for multi-storey parking lots is gradually increasing. Many car owners find it difficult to quickly find their vehicles in large multi-storey parking lots, and the development of reverse car search technology caters to this social demand.

[0003] In the current era of rapid technological development, intelligent driving technology has gradually become a key development direction of the automotive industry, enabling vehicles to have more intelligent functions. One of the important applications is intelligent parking and reverse car search. Car owners expect to manage and find their vehicles more conveniently through intelligent technology, improving parking efficiency and experience. With the continuous advancement of sensing technology, especially the widespread application of sensors such as global positioning system (GPS), cameras, and lidar, the vehicle positioning and recognition capabilities have been significantly improved, making reverse car search technology more feasible. However, the ability of GPS signals to penetrate objects indoors is relatively weak. Structures such as buildings, walls, and ceilings will weaken the strength of GPS signals, forming a multipath effect, resulting in the inability to accurately determine the true path of the signal in underground parking lots, thereby causing positioning errors. Especially in multi-story parking lots, accurately determining the floor has become a bottleneck problem that is difficult to break through. In contrast, visual positioning can obtain rich environmental information without relying on any base station facilities, including not only location and direction, but also identification and understanding of surrounding objects, structures, and scenes. This makes it have great potential for more intelligent indoor navigation and positioning services. Summary of the invention

[0004] Aiming at the problem of inaccurate cross-floor positioning, the present invention provides a cross-floor reverse vehicle search method, which utilizes camera and IMU data to construct a three-dimensional dense semantic map.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] The present invention provides a cross-floor reverse vehicle search method, comprising the following steps:

[0007] Step 1, data collection: select the camera and IMU to be integrated into the driving recorder, and place the driving recorder on the vehicle to collect camera and IMU data;

[0008] Step 2: Build a 3D dense semantic map: Use camera data to build a real-time 3D dense semantic map and perform semantic segmentation on the image to obtain semantic information of different areas in the environment, including uphill, downhill, ground, speed bumps, fire extinguishers, markings, and signs.

[0009] Step 3, generate a trajectory map: The trajectory map is displayed in real time in the 3D dense semantic map. The real-time trajectory map is obtained by fusing camera and IMU data using VIO technology. The odometer of visual and inertial information is used to estimate the vehicle's posture. The trajectory map takes into account the height difference between floors. The posture between different floors is smooth and uninterrupted, and the floor where the trajectory is located is marked.

[0010] Step 4, loop detection: identify the position error of vehicles passing through the same area at different time points;

[0011] Step 5, back-end optimization pose graph: Use back-end optimization techniques to refine and optimize the trajectory to minimize the error between the camera and IMU data;

[0012] Step 6, reverse vehicle search: Compare the real environment information with the 3D dense semantic map, and search for the vehicle in reverse based on the trajectory.

[0013] Preferably, the data collection in step 1 is specifically as follows:

[0014] Step 1.1, choose the right camera and IMU: the camera should have sufficient resolution and the ability to adapt to the vehicle environment, and the IMU should have high-precision accelerometers and gyroscopes;

[0015] Step 1.2, integrated driving recorder: The driving recorder has the functions of camera and IMU data collection, transmission and storage, and can display the data on the display;

[0016] Step 1.3, install the dashcam: The dashcam is placed at the front center of the vehicle, its field of view covers the road and surrounding environment in front of the vehicle, and the coordinate system of the IMU is aligned with the vehicle coordinate system.

[0017] Preferably, the three-dimensional dense semantic map is constructed in step 2, specifically:

[0018] Step 2.1, data preprocessing: perform real-time semantic segmentation on the original image captured by the camera, assign corresponding semantic labels to each pixel, run ORB feature extraction and semantic segmentation in parallel, use the ORB detector to detect key pixels in the image, and calculate the descriptors of these key pixels, extract the semantic information of the key pixels from the segmented image, and add the semantic information to the descriptor of each key pixel, which represents the semantic ORB feature;

[0019] Step 2.2, identify key frames: if there are more than 20 frames of images from the previous key frame to the current frame, or the number of map points matched by the current frame is less than 50, then insert the current frame into the map as a key frame; if 90% of the feature points in the key frame can be observed by at least three other key frames at the same time, then delete the key frame; if the feature points in the key frame are less than 15 for 5 seconds, then store the map in the database until the feature points in the new key frame exceed 15, and then start to build a new map;

[0020] Step 2.3, build a 3D dense semantic map: convert the semantic ORB feature points of the new keyframe into map points through triangulation. If the map point can be found in more than 25% of the images and observed by more than 3 keyframes, add the map point to the local map and visualize the semantic label according to the semantic information in the descriptor;

[0021] Step 2.4, local map optimization: Use the graph optimization algorithm BundleAdjustment to bundle adjust the key frames and map points in the local map, minimize the reprojection error, and optimize the pose and position of the key frames and map points.

[0022] Preferably, the model used for semantic segmentation in step 2.1 is a semantic segmentation model MDC-Seg, including a backbone network, an encoder and a decoder, wherein the backbone network uses the first 11 layers of Bneck of MobileNetV3-Large to extract image semantic features at different levels, the encoder further captures multi-scale information of deep semantic features, and the decoder fuses shallow and deep semantic features to restore image resolution;

[0023] The size of the original image captured by the camera is (H, W, 3). After downsampling and channel expansion processing, a feature map with a size of (H / 2, W / 2, 16) is output. The processed feature map is input to Bneck1. After feature extraction by Bneck1, a feature map with a size of (H / 2, W / 2, 16) is output. The feature map is used as the input of Bneck2. After that, the input of each layer is the output of the previous layer.

[0024] The encoder inputs the feature map output by Bneck11 into a dense ASPP module, which includes a 1×1 convolution, 4 3×3 depth-separable dilated convolutions, and the dilated rates are 3, 6, 12, and 18 respectively. The feature map output by each layer is spliced ​​with all subsequent layers. The number of channels of the spliced ​​feature map is modified to 112 through 1x1 convolution, and the resolution remains unchanged. The modified feature map is input into Coordinate Attention, and average pooling is performed in the X direction and Y direction to obtain two 1D vectors. The vectors are spliced ​​and encoded separately as direction-aware attention and position attention, and finally the spatial information is fused by channel weighting.

[0025] The decoder obtains channel attention from the feature map output by Bneck3 through the ECA-NET attention mechanism, upsamples the feature map output by the encoder by 4 times and concatenates it with the channel attention feature map, enriches the semantic features of the concatenated feature map through 3x3 convolution and then continues to upsample by 4 times, and finally outputs a semantic feature map of the same size as the original image, in which each pixel is given semantic information.

[0026] Preferably, the trajectory diagram is generated in step 3, specifically:

[0027] Step 3.1, camera and IMU initialization: run the camera and IMU in parallel, calibrate the internal and external parameters of the camera, including the focal length, distortion parameters and the position of the camera relative to the vehicle coordinate system, extract the semantic ORB features of the current keyframe image, project the feature points in the local map to the current keyframe, perform feature matching, and the position of the current keyframe in the map is the trajectory; denoise the IMU data, use the acceleration and angular velocity information provided by the IMU, and calculate the speed and displacement of the vehicle in the vehicle coordinate system through IMU pre-integration; jointly optimize the camera and IMU to obtain the initial pose, speed and position of the vehicle;

[0028] Step 3.2, local map trajectory generation: The trajectory generation is run in parallel with the construction of the 3D dense semantic map in step 2. The trajectory in the local map is formed by connecting the positions of all key frames when constructing the 3D dense semantic map;

[0029] Step 3.3, floor marking: By comparing the z-axis coordinates of adjacent key frames, the relative change trend of the height is determined. If the driving path is flat ground-uphill-flat ground, the floor marking is increased by 1; if the driving path is flat ground-downhill-flat ground, the floor marking is decreased by 1.

[0030] Preferably, the feature matching in step 3.1 uses the ORB feature matching method with semantic constraints, specifically:

[0031] Determine whether the semantic ORB feature in the current key frame is consistent with the semantic information of the map point. If so, determine the similarity of the two feature descriptors through the Hamming distance to complete the feature point matching;

[0032] Use similar feature points and triangulation methods to reconstruct 3D map points and pose information, and use pure vision BundleAdjustment to optimize the camera pose and the spatial position of feature points to minimize the reprojection error;

[0033] The optimized camera pose information is converted to the IMU coordinate system as the camera initial information, and the relative pose ΔP between the camera key frame i and key frame j in the IMU coordinate system is ij , relative speed ΔV ij , relative position ΔP ij It is expressed as follows:

[0034]

[0035] in, represents the transpose of the rotation matrix of key frame i, R j represents the rotation matrix of key frame j, V j represents the motion speed of the camera at key frame j in the IMU coordinate system, V i represents the speed of the camera at key frame i in the IMU coordinate system, g represents the gravity acceleration vector, Δt ij represents the time interval between key frame i and key frame j, P j represents the spatial position of the camera in the IMU coordinate system at key frame j, P i Represents the spatial position of the camera in the IMU coordinate system at key frame i.

[0036] Preferably, the MU pre-integration in step 3.1 is specifically:

[0037] Relative pose of IMU between camera key frame i and key frame j after noise removal Relative speed Relative Position It is expressed as:

[0038]

[0039] in, represents the angular velocity measurement value at time k, represents the gyroscope bias at time i, represents the acceleration measurement value at time k, represents the accelerometer bias at time i, and Δt represents the time interval between time i and time j;

[0040] As pre-integration accumulates, the accelerometer and gyroscope biases also need to be continuously updated. The updated IMU relative posture, relative velocity, and relative position are expressed as:

[0041]

[0042] in, represents the increment of gyroscope bias, Indicates the increment of accelerometer bias.

[0043] Preferably, the joint optimization of the camera and the IMU in step 3.1 is expressed as a problem of minimizing the difference between the relative posture, relative speed, and relative position of the camera and the IMU. The posture, speed, and position when the difference is the smallest are the results of the joint optimization of the camera and the IMU.

[0044] The relative pose, relative speed, and relative position difference between the camera and IMU are expressed as follows:

[0045] r(I ij )=[r(ΔR ij ),r(ΔV ij ),r(ΔP ij )]

[0046]

[0047] r(ΔV ij )=ΔV ij -ΔV' ij

[0048] r(ΔP ij )=ΔP ij -ΔP' ij

[0049] Among them, r(I ij ) represents the residual vector of relative pose, relative velocity and relative position, which is represented by combining the pose error, velocity error and position error;

[0050] The results of the joint optimization of the camera and IMU are shown as follows:

[0051]

[0052] in, express So that the entire expression reaches the minimum value, represents the inverse of the covariance matrix of the residual vector.

[0053] Preferably, the loop detection in step 4 is specifically:

[0054] Loop detection starts when the current map has more than 10 key frames or when the map is more than 10 key frames away from the last loop;

[0055] The semantic ORB feature descriptor extracted from each keyframe is represented as a discrete bag-of-words vector through the DBoW2 library. All bag-of-words vectors are added to the database to build a bag-of-words model. Each new keyframe is matched with all keyframes for semantic ORB features. If there are more than 15 matching feature points, the keyframe is marked as a common-view keyframe of the new keyframe. The five common-view keyframes with the highest similarity are taken through the bag-of-words model, and the lowest similarity is used as the minimum threshold. The bag-of-words model is used to detect the similarity between all keyframes in the database and the new keyframe. If the similarity exceeds the minimum threshold and the detected keyframe has a common-view relationship with at least three common-view keyframes of the new keyframes, it is considered that a loop is detected.

[0056] Preferably, the back-end optimization pose graph in step 5 is specifically as follows:

[0057] Step 5.1, update the map: If the current keyframe and the loop keyframe are in the same map, directly proceed to step 5.2 to optimize the cumulative error, otherwise first merge the two sub-maps where the current keyframe and the loop keyframe are located;

[0058] Step 5.2, optimize the cumulative error: After detecting that the current keyframe and the loop keyframe are in the same map, use the joint optimization of the camera and IMU in step 3.1 to optimize the relative transformation from the current frame to the loop keyframe to obtain the cumulative error, and disperse the cumulative error to each keyframe in the loop, thereby eliminating the cumulative error and adjusting the pose, velocity and position of the camera and IMU and the coordinates of the map points.

[0059] Compared with the prior art, the present invention has the following advantages:

[0060] 1. The present invention integrates sensors such as cameras and IMU into the driving recorder, without the need to add other equipment, and no base station needs to be deployed in the parking lot, and is not limited to any scene;

[0061] 2. The present invention combines semantic segmentation with SLAM to accurately determine vehicle driving trajectories and optimize map presentation.

[0062] 3. The present invention designs a novel lightweight semantic segmentation model MDC-Seg and a semantically constrained ORB feature matching method.

[0063] 4. The present invention can realize uninterrupted cross-floor vehicle positioning.

[0064] 5. The cross-floor reverse car search method of the present invention provides users with a more convenient and intelligent vehicle search experience in indoor parking lots and other scenarios. It can help record the parking location of the vehicle when the owner leaves the vehicle, and reversely search for the vehicle through the track when necessary. In scenarios such as large parking lots or multi-story parking buildings, it improves the efficiency and accuracy of users' car search and greatly facilitates the daily driving experience of car owners. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 Build a flow chart for the map;

[0066] Figure 2 This is the structural diagram of the semantic segmentation model MDC-Seg;

[0067] Figure 3 To generate a trajectory flow chart. DETAILED DESCRIPTION

[0068] In order to further illustrate the technical solution of the present invention, the present invention is further described below through embodiments.

[0069] A cross-floor reverse vehicle search method of this embodiment includes the following steps:

[0070] Step 1, data collection:

[0071] Select the camera and IMU to integrate into the driving recorder, and place the driving recorder on the vehicle to collect camera and IMU data.

[0072] Step 1.1, choose the appropriate camera and IMU:

[0073] The camera has sufficient resolution and the ability to adapt to the vehicle environment to meet the requirements of the dashcam. The IMU has high-precision accelerometers and gyroscopes to meet the requirements of high-precision positioning. Choose a device that is appropriately sized and lightweight so that when installed on the vehicle, it will not negatively affect the performance of the dashcam and the operation of the vehicle.

[0074] Step 1.2, integrate the driving recorder:

[0075] The driving recorder has the functions of camera and IMU data collection, transmission and storage, and can display the data on the monitor. The intelligent perception cube serves as a driving recorder to ensure the synchronization of camera and IMU data for subsequent data fusion.

[0076] Step 1.3, install the driving recorder:

[0077] The driving recorder is placed at the front center of the vehicle. Its field of view covers the road and surrounding environment in front of the vehicle, and the coordinate system of the IMU is aligned with the vehicle coordinate system.

[0078] Step 2: Construct a 3D dense semantic map (such as Figure 1 shown):

[0079] A real-time three-dimensional dense semantic map is established through camera data, and the image is semantically segmented to obtain semantic information of different areas in the environment, including uphill, downhill, ground, speed bumps, fire extinguishers, markings, and signs. The dense semantic map can help the system have a deeper understanding of the environment, enable car owners to better understand the surrounding environment, and improve the robustness of positioning.

[0080] Step 2.1, data preprocessing:

[0081] The original image captured by the camera is semantically segmented in real time, and a corresponding semantic label is assigned to each pixel. ORB feature extraction and semantic segmentation are run in parallel. The ORB detector is used to detect key pixels in the image and the descriptors of these key pixels are calculated. The semantic information of the key pixels is extracted from the segmented image and added to the descriptor of each key pixel, which represents the semantic ORB feature.

[0082] The present invention designs a semantic segmentation model MDC-Seg (such as Figure 2 As shown in the figure, it includes a backbone network, an encoder and a decoder. The backbone network uses the first 11 layers of MobileNetV3-Large Bneck to extract image semantic features at different levels. The encoder further captures the multi-scale information of deep semantic features. The decoder fuses shallow and deep semantic features to restore image resolution.

[0083] The size of the original image captured by the camera is (H, W, 3). After downsampling and channel expansion processing, the output feature map has a size of (H / 2, W / 2, 16). The processed feature map is input into Bneck1. After feature extraction by Bneck1, the output feature map has a size of (H / 2, W / 2, 16), and the feature map is used as the input of Bneck2. After that, the input of each layer is the output of the previous layer.

[0084] The encoder inputs the feature map output by Bneck11 into the dense ASPP module, which includes a 1×1 convolution and 4 3×3 depth-separable dilated convolutions with dilated rates of 3, 6, 12, and 18 respectively. The feature map output by each layer is spliced ​​with all subsequent layers. The number of channels of the spliced ​​feature map is modified to 112 through 1x1 convolution, and the resolution remains unchanged. The modified feature map is input into CoordinateAttention, and average pooling is performed in the X and Y directions to obtain two 1D vectors. The vectors are spliced ​​and encoded separately as direction-aware attention and position attention. Finally, the spatial information is fused by channel weighting.

[0085] The decoder obtains channel attention from the feature map output by Bneck3 through the ECA-NET attention mechanism, upsamples the feature map output by the encoder by 4 times and concatenates it with the channel attention feature map. The concatenated feature map is enriched with semantic features through 3x3 convolution and then upsampled by 4 times. Finally, a semantic feature map of the same size as the original image is output, and each pixel in the map is given semantic information.

[0086] Step 2.2, determine the key frame:

[0087] If there are more than 20 frames of images from the previous key frame to the current frame, or the number of map points matched by the current frame is less than 50, the current frame is inserted into the map as a key frame; if 90% of the feature points in the key frame can be observed simultaneously by at least three other key frames, the key frame is deleted; if the feature points in the key frame are less than 15 for 5 seconds, the map is stored in the database until the feature points in the new key frame exceed 15, and a new map is constructed again.

[0088] Step 2.3, build a three-dimensional dense semantic map:

[0089] The semantic ORB feature points of the new keyframe are converted into map points through triangulation. If the map point can be found in more than 25% of the images and is observed by more than 3 keyframes, the map point is added to the local map and the semantic label is visualized according to the semantic information in the descriptor.

[0090] Step 2.4, local map optimization:

[0091] The graph optimization algorithm BundleAdjustment is used to bundle and adjust the key frames and map points in the local map to minimize the reprojection error and optimize the pose and position of the key frames and map points.

[0092] Step 3: Generate a trajectory map (such as Figure 3 shown):

[0093] The trajectory map is displayed in real time in the three-dimensional dense semantic map. The real-time trajectory map is obtained by fusing camera and IMU data using VIO technology. The vehicle's posture is estimated through the odometer of visual and inertial information. The trajectory map takes into account the height differences between floors, and the posture between different floors is smooth and uninterrupted, marking the floor where the trajectory is located.

[0094] Step 3.1, camera and IMU initialization:

[0095] The camera and IMU are run in parallel to calibrate the internal and external parameters of the camera, including the focal length, distortion parameters and the position of the camera relative to the vehicle coordinate system. The semantic ORB features of the current keyframe image are extracted, and the feature points in the local map are projected to the current keyframe for feature matching. The position of the current keyframe in the map is the trajectory. IMU data is denoised, and the acceleration and angular velocity information provided by the IMU is used to calculate the speed and displacement of the vehicle in the body coordinate system through IMU pre-integration. The camera and IMU are jointly optimized to obtain the initial pose, speed and position of the vehicle.

[0096] Within two seconds of the initial run, the camera and IMU are initialized separately to obtain the initial position, posture, velocity, bias and noise of the accelerometer and gyroscope. The camera inserts a keyframe at 4Hz, and the position of the keyframe in the map is the trajectory. The semantic ORB features of the current keyframe image are extracted, and then the feature points in the local map are projected to the current frame to find a matching relationship. The present invention proposes a semantically constrained ORB feature matching method. First, it is determined whether the semantic ORB features in the current keyframe are consistent with the semantic information of the map points. If they are consistent, the similarity of the two feature descriptors is determined by the Hamming distance to complete the feature point matching. 3D map points and posture information are reconstructed using similar feature points and triangulation methods, and the pure visual BundleAdjustment is used to optimize and adjust the spatial position of the camera posture and feature points to minimize the reprojection error, thereby improving the accuracy of the camera posture and map points. The optimized camera posture information is converted to the IMU coordinate system as the camera initial information, and the relative posture ΔR between the camera keyframe i and keyframe j in the IMU coordinate system ij , relative speed ΔV ij , relative position ΔP ij It is expressed as follows:

[0097]

[0098] in, represents the transpose of the rotation matrix of key frame i, R j represents the rotation matrix of key frame j, V j represents the motion speed of the camera at key frame j in the IMU coordinate system, V i represents the speed of the camera at key frame i in the IMU coordinate system, g represents the gravity acceleration vector, Δt ij represents the time interval between key frame i and key frame j, P j represents the spatial position of the camera in the IMU coordinate system at key frame j, P i Represents the spatial position of the camera in the IMU coordinate system at key frame i.

[0099] IMU pre-integration can calculate the relative change of IMU attitude, velocity, and position between two moments, so as to jointly optimize the attitude, velocity, and position obtained by the two key frames of the camera. The IMU raw data contains the bias and white noise of the accelerometer and gyroscope. Pure IMU optimization aims to reduce the impact of these two errors.

[0100] Relative pose of IMU between camera key frame i and key frame j after noise removal Relative speed Relative Position It is expressed as:

[0101]

[0102] in, represents the angular velocity measurement value at time k, represents the gyroscope bias at time i, represents the acceleration measurement value at time k, represents the accelerometer bias at time i, and Δt represents the time interval from time i to time j.

[0103] As pre-integration accumulates, the accelerometer and gyroscope biases also need to be continuously updated. The updated IMU relative posture, relative velocity, and relative position are expressed as:

[0104]

[0105] in, represents the increment of gyroscope bias, Indicates the increment of accelerometer bias.

[0106] The joint optimization of the camera and IMU can be expressed as a problem of minimizing the difference between the two. When the difference is minimum, the pose, velocity, and position are the results of the joint initialization of the camera and IMU.

[0107] The relative pose, relative speed, and relative position difference between the camera and IMU are expressed as follows:

[0108] r(I ij )=[r(ΔR ij ),r(ΔV ij ),r(ΔP ij )]

[0109]

[0110] r(ΔV ij )=ΔV ij -ΔV' ij

[0111] r(ΔP ij )=ΔP ij-ΔP' ij

[0112] Among them, r(I ij ) represents the residual vector of relative pose, relative velocity and relative position, which is represented by combining the pose error, velocity error and position error;

[0113] The results of the joint optimization of the camera and IMU are shown as follows:

[0114]

[0115] in, express So that the entire expression reaches the minimum value, represents the inverse of the covariance matrix of the residual vector.

[0116] Step 3.2, local map trajectory generation:

[0117] The trajectory generation runs in parallel with the construction of the 3D dense semantic map in step 2. The trajectory in the local map is formed by connecting the positions of all key frames when constructing the 3D dense semantic map.

[0118] Step 3.3, floor marking:

[0119] By comparing the z-axis coordinates of adjacent keyframes, the relative change trend of the height can be determined. At the same time, the semantic segmentation algorithm can also determine whether the current vehicle is traveling on an uphill, downhill, or relatively flat terrain. If the driving path is flat ground-uphill-flat ground, the floor is marked plus 1, and if the driving path is flat ground-downhill-flat ground, the floor is marked minus 1.

[0120] Step 4: Loop detection:

[0121] Identify the position error of vehicles passing through the same area at different time points. Usually, some features in the trajectory map or local map are used to correct the trajectory error, especially the key parts semantically segmented in step 2, to improve robustness.

[0122] Loop detection starts when the previous map has more than 10 key frames or when the time since the last loop is more than 10 key frames.

[0123] The semantic ORB feature descriptor extracted from each keyframe is represented as a discrete bag-of-words vector through the DBoW2 library. All bag-of-words vectors are added to the database to build a bag-of-words model. Each new keyframe is matched with all keyframes for semantic ORB features. If there are more than 15 matching feature points, the keyframe is marked as a common-view keyframe of the new keyframe. The five common-view keyframes with the highest similarity are taken through the bag-of-words model, and the lowest similarity is used as the minimum threshold. The bag-of-words model is used to detect the similarity between all keyframes in the database and the new keyframe. If the similarity exceeds the minimum threshold and the detected keyframe has a common-view relationship with at least three common-view keyframes of the new keyframes, it is considered that a loop is detected.

[0124] Step 5, backend optimization pose graph:

[0125] The trajectory is refined and optimized using backend optimization techniques to minimize the error between camera and IMU data, improve positioning accuracy and reduce drift.

[0126] Step 5.1, update the map:

[0127] If the current keyframe and the loop keyframe are in the same map, directly perform the optimization cumulative error in step 5.2. Otherwise, first merge the two sub-maps where the current keyframe and the loop keyframe are located.

[0128] The fused map first needs to align the coordinates of the current keyframe and the loop keyframe at the map point, and then migrate the map point where the current keyframe is located and all the keyframe information in the map to the map where the loop keyframe is located.

[0129] Step 5.2, optimize the cumulative error:

[0130] After detecting that the current keyframe and the loop keyframe are in the same map, use the joint optimization of the camera and IMU in step 3.1 to optimize the relative transformation from the current frame to the loop keyframe to obtain the cumulative error, and disperse the cumulative error to each keyframe in the loop to eliminate the cumulative error and adjust the pose, velocity and position of the camera and IMU as well as the coordinates of the map points.

[0131] Step 6, reverse vehicle search: Compare the real environment information with the 3D dense semantic map, and search for the vehicle in reverse based on the trajectory.

[0132] The above method can improve the accuracy of vehicle positioning, realize uninterrupted positioning across floors, provide efficient reverse vehicle search function, and make a beneficial contribution to the field of vehicle positioning and reverse vehicle search.

Claims

1. A cross-floor reverse vehicle search method, characterized in that: The following steps are involved: Step 1, data collection: select the camera and IMU to be integrated into the driving recorder, and place the driving recorder on the vehicle to collect camera and IMU data; Step 2: Build a 3D dense semantic map: Use camera data to build a real-time 3D dense semantic map and perform semantic segmentation on the image to obtain semantic information of different areas in the environment, including uphill, downhill, ground, speed bumps, fire extinguishers, markings, and signs. Step 3, generate a trajectory map: The trajectory map is displayed in real time in the 3D dense semantic map. The real-time trajectory map is obtained by fusing camera and IMU data using VIO technology. The odometer of visual and inertial information is used to estimate the vehicle's posture. The trajectory map takes into account the height difference between floors. The posture between different floors is smooth and uninterrupted, and the floor where the trajectory is located is marked. Step 4, loop detection: identify the position error of vehicles passing through the same area at different time points; Step 5, back-end optimization pose graph: Use back-end optimization techniques to refine and optimize the trajectory to minimize the error between the camera and IMU data; Step 6, reverse vehicle search: Compare the real environment information with the 3D dense semantic map, and search for the vehicle in reverse based on the trajectory.

2. A cross-floor reverse vehicle search method according to claim 1, characterized in that: The data collection in step 1 is specifically as follows: Step 1.1, choose the right camera and IMU: the camera should have sufficient resolution and the ability to adapt to the vehicle environment, and the IMU should have high-precision accelerometers and gyroscopes; Step 1.2, integrated driving recorder: The driving recorder has the functions of camera and IMU data collection, transmission and storage, and can display the data on the display; Step 1.3, install the dashcam: The dashcam is placed at the front center of the vehicle, its field of view covers the road and surrounding environment in front of the vehicle, and the coordinate system of the IMU is aligned with the vehicle coordinate system.

3. A cross-floor reverse vehicle search method according to claim 1, characterized in that: The three-dimensional dense semantic map is constructed in step 2, specifically: Step 2.1, data preprocessing: perform real-time semantic segmentation on the original image captured by the camera, assign corresponding semantic labels to each pixel, run ORB feature extraction and semantic segmentation in parallel, use the ORB detector to detect key pixels in the image, and calculate the descriptors of these key pixels, extract the semantic information of the key pixels from the segmented image, and add the semantic information to the descriptor of each key pixel, which represents the semantic ORB feature; Step 2.2, identify key frames: if there are more than 20 frames of images from the previous key frame to the current frame, or the number of map points matched by the current frame is less than 50, then insert the current frame into the map as a key frame; if 90% of the feature points in the key frame can be observed by at least three other key frames at the same time, then delete the key frame; if the feature points in the key frame are less than 15 for 5 seconds, then store the map in the database until the feature points in the new key frame exceed 15, and then start to build a new map; Step 2.3, build a 3D dense semantic map: convert the semantic ORB feature points of the new keyframe into map points through triangulation. If the map point can be found in more than 25% of the images and observed by more than 3 keyframes, add the map point to the local map and visualize the semantic label according to the semantic information in the descriptor; Step 2.4, local map optimization: Use the graph optimization algorithm BundleAdjustment to bundle adjust the key frames and map points in the local map, minimize the reprojection error, and optimize the pose and position of the key frames and map points.

4. A cross-floor reverse vehicle search method according to claim 3, characterized in that: The model used for semantic segmentation in step 2.1 is the semantic segmentation model MDC-Seg, which includes a backbone network, an encoder and a decoder, wherein the backbone network uses the first 11 layers of Bneck of MobileNetV3-Large to extract image semantic features at different levels, the encoder further captures the multi-scale information of deep semantic features, and the decoder fuses shallow and deep semantic features to restore image resolution; The size of the original image captured by the camera is (H, W, 3). After downsampling and channel expansion processing, a feature map with a size of (H / 2, W / 2, 16) is output. The processed feature map is input to Bneck1. After feature extraction by Bneck1, a feature map with a size of (H / 2, W / 2, 16) is output. The feature map is used as the input of Bneck2. After that, the input of each layer is the output of the previous layer. The encoder inputs the feature map output by Bneck11 into a dense ASPP module, which includes a 1×1 convolution, 4 3×3 depth-separable dilated convolutions, and the dilated rates are 3, 6, 12, and 18 respectively. The feature map output by each layer is spliced ​​with all subsequent layers. The number of channels of the spliced ​​feature map is modified to 112 through 1x1 convolution, and the resolution remains unchanged. The modified feature map is input into Coordinate Attention, and average pooling is performed in the X direction and Y direction to obtain two 1D vectors. The vectors are spliced ​​and encoded separately as direction-aware attention and position attention, and finally the spatial information is fused by channel weighting. The user decoder obtains channel attention from the feature map output by Bneck3 through the ECA-NET attention mechanism, upsamples the feature map output by the encoder by 4 times and splices it with the channel attention feature map, enriches the semantic features of the spliced ​​feature map through 3x3 convolution and then continues to upsample by 4 times, and finally outputs a semantic feature map of the same size as the original image, in which each pixel is given semantic information.

5. A cross-floor reverse vehicle search method according to claim 1, characterized in that: The trajectory map is generated in step 3, specifically: Step 3.1, camera and IMU initialization: run the camera and IMU in parallel, calibrate the internal and external parameters of the camera, including the focal length, distortion parameters and the position of the camera relative to the vehicle coordinate system, extract the semantic ORB features of the current keyframe image, project the feature points in the local map to the current keyframe, perform feature matching, and the position of the current keyframe in the map is the trajectory; denoise the IMU data, use the acceleration and angular velocity information provided by the IMU, and calculate the speed and displacement of the vehicle in the vehicle coordinate system through IMU pre-integration; jointly optimize the camera and IMU to obtain the initial pose, speed and position of the vehicle; Step 3.2, local map trajectory generation: The trajectory generation is run in parallel with the construction of the 3D dense semantic map in step 2. The trajectory in the local map is formed by connecting the positions of all key frames when constructing the 3D dense semantic map; Step 3.3, floor marking: By comparing the z-axis coordinates of adjacent key frames, the relative change trend of the height is determined. If the driving path is flat ground-uphill-flat ground, the floor marking is increased by 1; if the driving path is flat ground-downhill-flat ground, the floor marking is decreased by 1.

6. A cross-floor reverse vehicle search method according to claim 5, characterized in that: The feature matching in step 3.1 uses the semantically constrained ORB feature matching method, specifically: Determine whether the semantic ORB feature in the current key frame is consistent with the semantic information of the map point. If so, determine the similarity of the two feature descriptors through the Hamming distance to complete the feature point matching; Use similar feature points and triangulation methods to reconstruct 3D map points and pose information, and use pure vision BundleAdjustment to optimize the camera pose and the spatial position of feature points to minimize the reprojection error; The optimized camera pose information is converted to the IMU coordinate system as the camera initial information, and the relative pose ΔR between the camera key frame i and key frame j in the IMU coordinate system is ij , relative speed ΔV ij , relative position ΔP ij It is expressed as follows: in, represents the transpose of the rotation matrix of key frame i, R j represents the rotation matrix of key frame j, V j represents the motion speed of the camera at key frame j in the IMU coordinate system, V i represents the speed of the camera at key frame i in the IMU coordinate system, g represents the gravity acceleration vector, Δt ij represents the time interval between key frame i and key frame j, P j represents the spatial position of the camera in the IMU coordinate system at key frame j, P i Represents the spatial position of the camera in the IMU coordinate system at key frame i.

7. A cross-floor reverse vehicle search method according to claim 6, characterized in that: The MU pre-integration in step 3.1 is specifically as follows: Relative pose of IMU between camera key frame i and key frame j after noise removal Relative speed Relative Position It is expressed as: in, represents the angular velocity measurement value at time k, represents the gyroscope bias at time i, represents the acceleration measurement value at time k, represents the accelerometer bias at time i, and Δt represents the time interval between time i and time j; As pre-integration accumulates, the accelerometer and gyroscope biases also need to be continuously updated. The updated IMU relative posture, relative velocity, and relative position are expressed as: in, represents the increment of gyroscope bias, Indicates the increment of accelerometer bias.

8. A cross-floor reverse vehicle search method according to claim 7, characterized in that: The joint optimization of the camera and IMU in step 3.1 is expressed as the problem of minimizing the difference between the relative posture, relative speed, and relative position of the camera and the IMU. The posture, speed, and position with the smallest difference are the results of the joint optimization of the camera and the IMU. The relative pose, relative speed, and relative position difference between the camera and IMU are expressed as follows: r(I ij )=[r(ΔR ij ),r(ΔV ij ),r(ΔP ij )] r(ΔV ij )=ΔV ij -ΔV' ij r(ΔP ij )=ΔP ij -ΔP' ij Among them, r(I ij ) represents the residual vector of relative pose, relative velocity and relative position, which is represented by combining the pose error, velocity error and position error; The results of the joint optimization of the camera and IMU are shown as follows: in, express So that the entire expression reaches the minimum value, represents the inverse of the covariance matrix of the residual vector.

9. A cross-floor reverse vehicle search method according to claim 8, characterized in that: The loop detection in step 4 is specifically as follows: Loop detection starts when the current map has more than 10 key frames or when the map is more than 10 key frames away from the last loop; The semantic ORB feature descriptor extracted from each keyframe is represented as a discrete bag-of-words vector through the DBoW2 library. All bag-of-words vectors are added to the database to build a bag-of-words model. Each new keyframe is matched with all keyframes for semantic ORB features. If there are more than 15 matching feature points, the keyframe is marked as a common-view keyframe of the new keyframe. The five common-view keyframes with the highest similarity are taken through the bag-of-words model, and the lowest similarity is used as the minimum threshold. The bag-of-words model is used to detect the similarity between all keyframes in the database and the new keyframe. If the similarity exceeds the minimum threshold and the detected keyframe has a common-view relationship with at least three common-view keyframes of the new keyframes, it is considered that a loop is detected.

10. A cross-floor reverse vehicle search method according to claim 9, characterized in that: In step 5, the back-end optimizes the pose graph, specifically: Step 5.1, update the map: If the current keyframe and the loop keyframe are in the same map, directly proceed to step 5.2 to optimize the cumulative error, otherwise first merge the two sub-maps where the current keyframe and the loop keyframe are located; Step 5.2, optimize the cumulative error: After detecting that the current keyframe and the loop keyframe are in the same map, use the joint optimization of the camera and IMU in step 3.1 to optimize the relative transformation from the current frame to the loop keyframe to obtain the cumulative error, and disperse the cumulative error to each keyframe in the loop, thereby eliminating the cumulative error and adjusting the pose, velocity and position of the camera and IMU and the coordinates of the map points.

Images