A cross-floor reverse car search method

Abstract

The present invention belongs to the field of visual SLAM technology, and specifically relates to a cross-floor reverse vehicle search method. In order to solve the problem of inaccurate cross-floor positioning, the method includes: first, the camera and IMU data are synchronously collected to lay the foundation for subsequent data fusion. Subsequently, 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, thereby improving the in-depth understanding of the environment. Using VIO technology, a real-time trajectory map is generated to mark the floor where the trajectory is located. Loop detection improves robustness by identifying position errors in the same area and correcting trajectory errors. Finally, the positioning accuracy is improved by minimizing the error between camera and IMU data.

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 rapid development of urbanization, the number of vehicles continues to increase, while parking resources are relatively limited. This is especially true in busy commercial and residential areas of cities, where land resources are relatively scarce. This has led to parking difficulties and a growing demand for multi-story parking lots. Many drivers find it difficult to quickly find their vehicles in large multi-story parking lots. The development of reverse car search technology caters to this social need.

[0003] In today's era of rapid technological advancement, intelligent driving technology has become a key development direction in the automotive industry, enabling vehicles to possess more intelligent features. One key application is smart parking and reverse vehicle tracking. Car owners expect intelligent technologies to more conveniently manage and locate their vehicles, improving parking efficiency and overall experience. With the continuous advancement of sensing technology, particularly the widespread use of sensors such as the Global Positioning System (GPS), cameras, and LiDAR, vehicle positioning and identification capabilities have been significantly enhanced, making reverse vehicle tracking more feasible. However, GPS signals have relatively poor penetration capabilities indoors. Structures such as buildings, walls, and ceilings weaken GPS signals, creating multipath effects. This makes it difficult to accurately determine the true path of the signal in underground parking lots, leading to positioning errors. Accurately determining the floor level is particularly challenging in multi-story parking lots. In contrast, visual positioning, independent of any base station infrastructure, can acquire rich environmental information, including not only location and orientation, but also the identification and understanding of surrounding objects, structures, and scenes. This holds great potential for more intelligent indoor navigation and positioning services. Summary of the Invention

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

[0005] In order to achieve the above-mentioned purpose, 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 a camera and IMU to integrate into a dashcam, and place the dashcam on the vehicle to collect camera and IMU data;

[0008] Step 2: Build a 3D dense semantic map: A real-time 3D dense semantic map is created using camera data. The image is semantically segmented to obtain semantic information about different areas in the environment, including uphill slopes, downhill slopes, the ground, speed bumps, fire extinguishers, road markings, and signs.

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

[0010] Step 4, loop closure 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: Select a suitable camera and IMU: The camera should have sufficient resolution and the ability to adapt to the vehicle environment, and the IMU should have a high-precision accelerometer and gyroscope;

[0015] Step 1.2, integrating a dashcam: The dashcam has the functions of collecting, transmitting, and storing camera and IMU data, and can display the data on a monitor;

[0016] Step 1.3, Install the dashcam: Place the dashcam in the center of the front of the vehicle. Its field of view covers the road and surrounding environment in front of the vehicle. 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 a corresponding semantic label 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. The descriptor represents the semantic ORB feature.

[0019] Step 2.2, identify key frames: If more than 20 frames of images have passed 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 simultaneously by at least three other key frames, then delete the key frame; if the feature points in the key frame are less than 15 for 5 seconds, then save the map to the database until the new key frame has more than 15 feature points, and then start building 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 is observed by more than 3 keyframes, then add the map point to the local map and visualize the semantic label based on 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, 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 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, the output is a feature map of size (H / 2, W / 2, 16). The processed feature map is input to Bneck1. After feature extraction by Bneck1, the output is a feature map of size (H / 2, W / 2, 16). This 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 the dense ASPP module, which includes a 1×1 convolution and four 3×3 depth-separable dilated convolutions with dilation 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 the Coordinate Attention, 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 through 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 splices it with the channel attention feature map. The spliced ​​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.

[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 to calibrate the camera's internal and external parameters, including the camera's focal length, distortion parameters, and the camera's position 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, and perform feature matching. The position of the current keyframe in the map is the trajectory. Denoise the IMU data, and use the acceleration and angular velocity information provided by the IMU to calculate the vehicle's velocity and displacement in the vehicle coordinate system through IMU pre-integration. Combined optimization of the camera and IMU results in the vehicle's initial pose, velocity, and position.

[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 keyframes, 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 semantically constrained ORB feature matching method, specifically:

[0031] Determine whether the semantic ORB feature in the current keyframe is consistent with the semantic information of the map point. If so, use the Hamming distance to determine the similarity of the two feature descriptors, thereby completing feature point matching.

[0032] Utilize 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 reprojection errors.

[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 obtained. 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 keyframe i, R j represents the rotation matrix of key frame j, V j Indicates the motion speed of the camera at key frame j in the IMU coordinate system, V i represents the camera's motion speed 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 as follows:

[0037] After removing noise, the relative pose of IMU between camera key frame i and key frame j relative speed Relative Position Expressed as:

[0038]

[0039] in, represents the angular velocity measurement value at time k, Indicates 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 pose, 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 pose, relative speed, and relative position of the camera and the IMU. The pose, speed, and position with the smallest difference are the results of the joint optimization of the camera and the IMU.

[0044] The relative pose, relative velocity, 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 its minimum value, represents the inverse of the covariance matrix of the residual vector.

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

[0054] Loop detection starts when the current map has more than 10 key frames or when the map has been 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. This invention integrates sensors such as cameras and IMUs into the driving recorder, eliminating the need for additional equipment and the need for base stations in parking lots. It is suitable for any scenario.

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

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

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

[0064] 5. The cross-floor reverse vehicle search method of this invention provides users with a more convenient and intelligent vehicle-finding experience in scenarios such as indoor parking lots. It can help record the vehicle's parking location when the owner leaves the vehicle and use the track to reversely find the vehicle when needed. In scenarios such as large parking lots or multi-story parking garages, this improves the efficiency and accuracy of vehicle search, greatly facilitating the driver's daily driving experience. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 Build a flowchart for the map;

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

[0067] Figure 3 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 examples.

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

[0070] Step 1, data collection:

[0071] Select a camera and IMU to integrate into a dash cam, and place the dash cam on the vehicle to collect camera and IMU data.

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

[0073] The camera must have sufficient resolution and the ability to adapt to the vehicle's environment to meet the requirements of a dashcam. The IMU must include high-precision accelerometers and gyroscopes to meet the requirements of high-precision positioning. Select a device that is appropriately sized and lightweight so that installation in the vehicle does not negatively impact the dashcam's performance or the vehicle's operation.

[0074] Step 1.2, integrate the driving recorder:

[0075] The driving recorder has the functions of collecting, transmitting and storing camera and IMU data, 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 dashcam:

[0077] The driving recorder is placed at the center of the front 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 gain a deeper understanding of the environment, enabling drivers to better understand their surroundings and improve the robustness of positioning.

[0080] Step 2.1, data preprocessing:

[0081] Real-time semantic segmentation is performed on the original image captured by the camera, and corresponding semantic labels are 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. The descriptor 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 to 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 four 3×3 depth-wise separable dilated convolutions with dilation 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 without changing the resolution. 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 a factor of 4, 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 a factor of 4. Finally, a semantic feature map of the same size as the original image is output, in which each pixel is assigned semantic information.

[0086] Step 2.2, identify key frames:

[0087] If more than 20 frames of images have passed from the previous keyframe 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 keyframe; if 90% of the feature points in the keyframe can be observed simultaneously by at least three other keyframes, the keyframe is deleted; if the feature points in the keyframe are less than 15 for 5 seconds, the map is stored in the database until the new keyframe has more than 15 feature points, and a new map is constructed again.

[0088] Step 2.3, build a 3D 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 based on 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 on 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 position is estimated through the odometry of visual and inertial information. The trajectory map takes into account the height differences between floors, and the position 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 camera's internal and external parameters, including the camera's focal length, distortion parameters, and position 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 onto 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 vehicle's velocity and displacement in the vehicle coordinate system through IMU pre-integration. The camera and IMU are jointly optimized to obtain the vehicle's initial pose, velocity, and position.

[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 keyframe i, R j represents the rotation matrix of key frame j, V j Indicates the motion speed of the camera at key frame j in the IMU coordinate system, V i represents the camera's motion speed 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 calculates the relative change in IMU attitude, velocity, and position between two moments, allowing for joint optimization with the attitude, velocity, and position obtained from two keyframes of the camera. Raw IMU data contains accelerometer and gyroscope bias and white noise. Pure IMU optimization aims to reduce the impact of these two errors.

[0100] After removing noise, the relative pose of IMU between camera key frame i and key frame j relative speed Relative Position Expressed as:

[0101]

[0102] in, represents the angular velocity measurement value at time k, Indicates 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.

[0103] As pre-integration accumulates, the accelerometer and gyroscope biases also need to be continuously updated. The updated IMU relative pose, 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 velocity, 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 its minimum value, represents the inverse of the covariance matrix of the residual vector.

[0116] Step 3.2, local map trajectory generation:

[0117] 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 keyframes 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 in altitude is determined. The semantic segmentation algorithm can also determine whether the vehicle is traveling on an uphill, downhill, or relatively flat terrain. If the route is flat-uphill-flat, the floor level is incremented by 1; if the route is flat-downhill-flat, the floor level is decremented by 1.

[0120] Step 4: Loopback 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 of the semantic segmentation 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 has exceeded 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 back-end 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 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.

[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 accumulated error to each keyframe in the loop to eliminate the accumulated 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, achieve uninterrupted positioning across floors, and provide efficient reverse vehicle search function, making 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 car search: compare the real environment information with the 3D dense semantic map, and reversely search for the car based on the trajectory; Step 3 adopts feature matching using the ORB feature matching method with semantic constraints, 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.

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 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.

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 IMU 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.

7. A cross-floor reverse vehicle search method according to claim 6, 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.

8. A cross-floor reverse vehicle search method according to claim 7, 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 minimum threshold is exceeded 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.

9. A cross-floor reverse vehicle search method according to claim 8, 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