A semantic localization method based on environmental objects

Abstract

The present invention discloses a semantic positioning method based on environmental objects, which relates to the field of robotics. It includes semantic mapping, closed-loop detection, and positioning correction. The semantic information of the target object in the image is extracted by a convolutional neural network, and the camera's position information is estimated using a visual odometry. The label information of the target object and the position and size of its 2D bounding box in the image are extracted by a convolutional neural network; a two-way comparison and matching method is used to detect whether the camera has passed through a historical location. If so, geometric verification is performed to confirm whether the camera has reached the closed-loop position; finally, the relative position relationship of the objects is calculated and the position of the camera is corrected through pose graph optimization. The present invention can achieve centimeter-level positioning and map construction in complex environments such as those with changes in lighting and seasons and changes in camera observation angles.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a semantic positioning method based on environmental objects. Background Art

[0002] Mobile robots have brought great convenience to our daily lives, automatically executing tasks assigned by humans according to their programming. Accurate positioning is one of the fundamental requirements for mobile robots to complete their tasks safely and efficiently. Visual SLAM is currently the most popular high-precision, low-cost positioning method among researchers. Current visual SLAM methods extract manually designed pixel feature points from images, encode and cluster the brightness gradients of these feature points, and finally estimate the position and posture of the mobile robot using multi-view geometry techniques to achieve positioning. This method is susceptible to changes in ambient lighting and the angle at which the robot moves. This results in the mobile robot being unable to accurately determine its position and, consequently, unable to complete its assigned tasks. Furthermore, this method, based on manually designed pixel feature points, does not align with how humans understand the environment at the object and semantic levels.

[0003] Therefore, researchers in this field are committed to developing a semantic localization method based on environmental objects. This method simulates human perception of the environment and constructs a three-dimensional map of semantic objects to correct drift in the robot's positioning estimation during movement, thereby improving the robot's positioning accuracy during operation. Summary of the Invention

[0004] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is the low accuracy of the traditional camera positioning method based on artificially designed features in scenes where the ambient light intensity and camera observation angle change.

[0005] To achieve the above object, the present invention provides a semantic positioning method based on environmental objects, comprising the following steps:

[0006] Step 1: Semantic Mapping: A convolutional neural network extracts the semantic and positional information of target objects in camera images and constructs a 3D map of semantic objects for loop closure detection.

[0007] Step 2: Closed-loop detection: Combine the objects in the camera image with the objects in the semantic object 3D map for bidirectional comparison and matching to detect whether the camera has passed through the historical location. If so, a matching object is generated for camera positioning correction;

[0008] Step 3: Positioning correction: Calculate the relative position relationship between objects with matching relationships, and establish a pose graph of the camera position and the object position. Use the pose graph to optimize and correct the camera positioning.

[0009] Furthermore, the step 1 includes the following steps:

[0010] Step 1.1: Input the RGB color image captured by the left eye of the binocular camera, extract the target object in the image through the convolutional neural network, and obtain the target object's label information, the 2D bounding box position in the image, and the object size;

[0011] Step 1.2: Using the RGB color images of the left and right binocular cameras, calculate the 3D depth values of all target objects in the environment using a binocular depth calculation algorithm based on visual geometry methods. This is the vertical distance from the target object to the camera imaging plane, which is the 3D position of the object in the environment relative to the camera.

[0012] Step 1.3: Use the visual odometry to extract feature points from the left and right images, and establish a reprojection error constraint for the feature points to estimate the relative position of the cameras.

[0013] Step 1.4: The object 3D pose optimization algorithm based on spatiotemporal constraints minimizes the error between the CNN extraction results and the depth triangulation algorithm by establishing a constraint relationship between the target object extraction results in the temporally continuous RGB color image and the 3D position of the target object in the spatial environment.

[0014] Step 1.5: Obtain the target object label information, 2D bounding box position, object size, and 3D position in the environment. Represent the target object in the environment as a 3D cube bounding box, and finally obtain a 3D stereo local semantic map with the camera optical center as the origin as the reference system.

[0015] Furthermore, the step 2 includes the following steps:

[0016] Step 2.1: For the RGB color image input by the camera, a local semantic map is obtained based on semantic mapping. In the local semantic map, a topological point is established at the center of each 3D bounding box. The connection between each topological point is set as a topological edge, and the spatial distance between topological points is the weight of the topological edge corresponding to the point. The local topological map is obtained from the local semantic map.

[0017] Step 2.2: Use visual SLAM technology to estimate the camera's motion information and transform the local semantic map with the camera's optical center as the origin into a global semantic map with the camera's starting point as the origin. Then, use the local topological map generation method to obtain the global topological map.

[0018] Step 2.3: Based on the object label information and position, the loop detection algorithm uses the KM algorithm to match the local and global topological maps. The topological point in the global topological map that corresponds to the local topological map at the current moment is calculated. This is the object that matches the local topological map at the current moment, which is the closed-loop object matching pair.

[0019] Step 2.4: After obtaining the matching object, perform geometric verification based on the object's semantic information and calculate the difference in the size, label, and posture information of the matching object. When the difference is less than a certain threshold, it is confirmed that the camera has reached the closed-loop position and positioning drift correction is immediately performed.

[0020] Furthermore, the step 3 includes the following steps:

[0021] Step 3.1: Align each closed-loop object pair, calculate the position error between the objects, and calculate the drift distance of the camera positioning estimate at the current moment by averaging the position errors between each pair of objects.

[0022] Step 3.2: Use the positioning estimation drift correction algorithm based on pose graph optimization to reduce the error of camera positioning estimation and correct the drift of camera positioning estimation.

[0023] Furthermore, the binocular depth calculation algorithm based on the visual geometry method calculates the relative distance of the target object in the scene based on the depth estimation algorithm of the binocular camera. First, the intrinsic parameters of the binocular camera and the rotation matrix and translation matrix between the two cameras are obtained through stereo camera calibration. Then, the binocular camera image is stereo corrected to obtain a distortion-free and horizontally aligned binocular image. Then, a semi-global stereo matching algorithm is applied to the corrected binocular image to obtain a depth map of the current scene. Combined with the 2D bounding box of the target object, the relative depth of the target center is calculated as the vertical distance of the target object to the camera imaging plane.

[0024] Furthermore, the binocular depth calculation algorithm based on the visual geometry method includes the following steps:

[0025] Step 1.2.1, stereo camera calibration;

[0026] Step 1.2.2, stereo image correction;

[0027] Step 1.2.3: Semi-global stereo matching.

[0028] Furthermore, the semi-global stereo matching includes cost matching, cost aggregation, disparity calculation and disparity optimization.

[0029] Furthermore, the disparity optimization includes epipolar constraints, similarity constraints, uniqueness constraints and left-right consistency constraints.

[0030] Furthermore, the object 3D pose optimization algorithm based on spatiotemporal relationship constraints first matches the local features in the predicted 3D cube bounding boxes in different image frames through a data association algorithm based on the local features of the object, and associates the 3D cube bounding boxes of the same object in adjacent frames; then, based on a space-time joint optimization algorithm, minimizes the projection error of the target in the image frame and spatial coordinates.

[0031] Furthermore, the object 3D pose optimization algorithm based on spatiotemporal relationship constraints includes the following steps:

[0032] Step 1.4.1, SIFT feature extraction and descriptor matching;

[0033] Step 1.4.2: Object association based on local feature matching;

[0034] Step 1.4.3: Optimize the object's 3D pose.

[0035] In a preferred embodiment of the present invention, a more accurate and robust semantic localization method based on environmental objects is proposed to address the low accuracy and robustness of camera localization methods based on artificially designed features in scenarios with varying ambient lighting intensity and camera observation angles. This method utilizes a convolutional neural network to extract semantic information about target objects in an image and uses visual odometry to estimate the camera's position, thereby improving localization accuracy and robustness. The method comprises three modules: semantic mapping, loop closure detection, and position correction. In the semantic mapping module, the input image can be an RGB color image of the left or right eye of a binocular camera. A convolutional neural network is used to extract the label information of the target object and its 2D bounding box position and size within the image. The loop closure detection module uses bidirectional comparison matching to detect whether the camera has passed through a historical location. If so, geometric verification is performed to confirm whether the camera has reached the loop closure position. Finally, the position correction module calculates the relative position of objects and corrects the camera's position through pose graph optimization.

[0036] Compared with the prior art, the present invention has the following obvious substantial features and significant advantages:

[0037] 1. This invention simulates the way humans perceive the environment and constructs a three-dimensional map of semantic objects to correct the drift of the robot's positioning estimation during movement, thereby improving the robot's positioning accuracy during operation.

[0038] 2. The present invention can achieve centimeter-level positioning and map construction in complex environments such as changes in lighting and seasons, and changes in camera observation angles.

[0039] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 This is a flow chart of a semantic positioning method based on environmental objects according to a preferred embodiment of the present invention;

[0041] Figure 2This is a binocular camera system model before stereo correction in a preferred embodiment of the present invention;

[0042] Figure 3 is a stereo-rectified binocular camera system model of a preferred embodiment of the present invention;

[0043] Figure 4 1 is a schematic diagram of a binocular camera system according to a preferred embodiment of the present invention;

[0044] Figure 5 This is the data association result of objects in dynamic and occluded situations in a preferred embodiment of the present invention;

[0045] Figure 6 is a graph showing the relationship between the reprojection errors of a target object in a camera and world coordinate systems according to a preferred embodiment of the present invention;

[0046] Figure 7 is a time-space relationship diagram of a target object in different frames according to a preferred embodiment of the present invention;

[0047] Figure 8 Schematic diagram of pose graph optimization according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0048] The following describes several preferred embodiments of the present invention with reference to the accompanying drawings to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0049] In the drawings, components with identical structures are denoted by the same reference numerals, and components with similar structures or functions are denoted by similar reference numerals. The size and thickness of each component shown in the drawings are arbitrary and are not limited by the present invention. For clarity, the thickness of components in some places in the drawings is appropriately exaggerated.

[0050] like Figure 1 As shown, the present invention provides a semantic positioning method based on environmental objects, including: semantic mapping, closed-loop detection, and positioning correction.

[0051] The method includes the following hardware: a visual camera device

[0052] The method comprises the following steps:

[0053] Semantic mapping step: Use convolutional neural networks to extract the semantic information and location information of target objects in camera images and construct a 3D map of semantic objects for loop closure detection. The basic process is:

[0054] 1. Input the RGB color image captured by the left eye of the binocular camera, extract the target object in the image through a convolutional neural network (CNN), and obtain the target object's label information, 2D bounding box position in the image, and object size.

[0055] 2. Using the RGB color images of the left and right eyes of the binocular camera, the binocular depth calculation algorithm based on visual geometry proposed in this invention is used to calculate the 3D depth value of all target objects in the environment. This is used as the vertical distance from the target object to the camera imaging plane, that is, the 3D position of the object in the environment relative to the camera.

[0056] 3. Use the visual odometry to extract the feature points of the left and right eye images, and estimate the relative position information of the cameras by establishing the reprojection error constraints of the feature points.

[0057] 4. To minimize the error between the CNN extraction results and the deep triangulation algorithm, a 3D pose optimization algorithm based on spatiotemporal constraints is proposed to improve the accuracy of the target object's position calculation in the environment. This algorithm minimizes the error between the CNN extraction results and the deep triangulation algorithm by establishing a constraint relationship between the target object extraction results from multiple consecutive RGB color images in time and the target object's 3D position in the environment in space, thereby reducing the error rate in the mapping step.

[0058] 5. Obtain the target object label information, 2D bounding box position, object size and 3D position in the environment, represent the object in the environment as a 3D cube bounding box, and finally obtain a 3D stereo local semantic map (hereinafter referred to as the local semantic map) with the camera optical center as the origin as the reference system.

[0059] Closed-loop detection steps: Combine the bidirectional comparison and matching of objects in the camera image with objects in the semantic object 3D map to detect whether the camera has passed through historical locations. If so, multiple pairs of matching objects are generated for camera positioning correction. The basic process is:

[0060] 1. For the RGB color image input by the camera, a local semantic map is generated through the semantic mapping step. In the local semantic map, a topological point is established at the center of each 3D bounding box. The lines connecting each topological point are set as topological edges, and the spatial distance between topological points is the weight of the topological edge corresponding to that point. Therefore, a local topological map is obtained from the local semantic map.

[0061] 2. Using visual SLAM technology to estimate the camera's motion, we transform the local semantic map, whose reference frame is the camera's optical center, into a global semantic map, whose reference frame is the camera's starting point in the environment. We then generate a global topological map using the local topological map generation method.

[0062] 3. Use the KM algorithm to match the local and global topological maps, and calculate the topological points in the global topological map that correspond to the local topological map at the current moment, that is, to obtain the matching objects between the two. The present invention calls this a closed-loop object matching pair. The above steps are the loop detection algorithm based on object label information and posture proposed by the present invention.

[0063] 4. After obtaining multiple pairs of matching objects, geometric verification is performed based on the semantic information of the objects. The difference in size, label, and posture information of the matching objects is calculated. When the difference is less than a certain threshold, it is confirmed that the camera has reached the closed-loop position and positioning drift correction is immediately performed.

[0064] Positioning correction steps: Calculate the relative position relationship between multiple pairs of matching objects and establish a pose graph of the camera position and object position. Through the pose graph, optimize the correction and reduce the camera drift during the positioning process, achieving higher-precision camera positioning compared to manually designed features. The basic process is:

[0065] 1. By aligning each closed-loop object matching pair, the position error between the objects is calculated. The drift distance of the camera positioning estimate at the current moment is calculated by averaging the position errors between each pair of objects.

[0066] 2. Use the positioning estimation drift correction algorithm based on pose graph optimization to reduce the error of camera positioning estimation and correct the drift of camera positioning estimation.

[0067] The following is a detailed description of the proposed method: "Binocular depth calculation algorithm based on visual geometry method", "3D pose optimization algorithm based on spatiotemporal relationship constraints", "loop detection algorithm based on object label information and pose", and "positioning estimation drift correction algorithm based on pose graph optimization".

[0068] 1. Binocular depth calculation algorithm based on visual geometry method

[0069] Depth estimation algorithm is one of the important methods for estimating the relative distance of a scene in an image, where the relative distance refers to the vertical distance from each pixel in the scene to the camera imaging plane. The depth estimation algorithm in the present invention is used to estimate the relative distance of the target object in the scene. Compared with the deep learning method, the binocular depth estimation algorithm based on the visual geometry method proposed in the present invention has higher efficiency and lower computational cost. The traditional pixel depth estimation algorithm cannot obtain the depth information of the pixel only through a single image. It is necessary to calculate the posture transformation relationship of adjacent image frames during the camera movement through the simultaneous positioning and mapping (SLAM) algorithm, and then use the triangulation method to observe the angle of the matching pixel points in different image frames to determine the distance of the point. The relative distance estimated in this way is not only affected by the image motion blur, but also suffers from the phenomenon of scale drift.

[0070] Therefore, the present invention proposes a binocular camera-based depth estimation algorithm to calculate the relative distance of the target object in the scene. First, the intrinsic parameters of the binocular camera and the rotation matrix R and translation matrix T between the two cameras are obtained through stereo camera calibration. Then, the binocular camera image is stereo corrected to obtain a distortion-free and horizontally aligned binocular image. Then, a semi-global stereo matching algorithm is applied to the corrected binocular image to obtain a depth map of the current scene. Combined with the 2D bounding box of the target object, the relative depth of the target center is calculated as the vertical distance of the target object to the camera imaging plane.

[0071] The algorithm consists of the following steps:

[0072] a. Stereo camera calibration

[0073] The parameters that need to be calibrated for a binocular camera are: the camera intrinsic parameter matrix, the distortion coefficient matrix, the rotation matrix, and the translation matrix (the camera intrinsic parameter matrix and the distortion coefficient matrix can be calibrated using a single-target calibration method). The main difference between binocular camera calibration and monocular camera calibration is that the binocular camera needs to calibrate the relative relationship between the left and right camera coordinate systems.

[0074] Use the rotation matrix R and translation matrix T to describe the relative relationship between the left and right camera coordinate systems. l and T l is the rotation matrix and translation vector of the left camera relative to the calibration object obtained by single-target calibration, R r and T r The rotation matrix and translation vector of the right camera relative to the calibration object are obtained through single-target calibration.

[0075] Assume there is a point P in space, whose coordinates in the world coordinate system are P W , its coordinates in the left and right camera coordinate systems can be expressed as:

[0076] P l =R l P W +T l , P r =R r P W +T r (1)

[0077] in:

[0078] P r =RP l +T (2)

[0079] Combining the above formula, we can get:

[0080]

[0081] b. Stereo image correction:

[0082] To calculate the disparity formed by the target point in the left and right views, we must first match the two corresponding image points of the point in the left and right views. However, matching corresponding points in two-dimensional space is very time-consuming. In order to reduce the matching search range, the epipolar constraint can be used to reduce the matching of corresponding points from a two-dimensional search to a one-dimensional search. The purpose of binocular correction is to strictly correspond the two images after eliminating the distortion, so that the epipolar lines of the two images are exactly on the same horizontal line. In this way, any point on one image and its corresponding point on the other image must have the same row number, and only one-dimensional search is required on the row to match the corresponding point. The binocular system model before and after stereo correction is as follows: Figure 2 , Figure 3 shown.

[0083] During the correction process, both image planes are rotated by half R, which can minimize the reprojection distortion. At this time, the image planes of the two cameras are coplanar (the optical axes are also parallel after distortion correction), but the left and right images are not aligned in rows. To achieve row alignment, the image planes of the two cameras must be parallel to the line connecting the origins of the two camera coordinate systems. For this purpose, R can be calculated. rect The matrix makes the poles at infinity, let:

[0084]

[0085] Since the image plane is ultimately parallel to the line connecting the origin of the camera coordinate system, e1 satisfies the following formula:

[0086]

[0087] Where, T=[T x , T y , Tz ] T , and e2 is orthogonal to e1, and e3 is orthogonal to e1 and e2, so:

[0088]

[0089] R rect The final stereo correction matrix can be obtained by left-multiplying the matrix of the left and right camera coordinate systems after R decomposition.

[0090] The principle diagram of the binocular stereo vision system after stereo correction is as follows Figure 4 shown.

[0091] The distance between the projection centers of the two cameras is b, also called the baseline. The imaging point of any point P in the three-dimensional space on the left camera is P L , the imaging point of the right camera is P R According to the principle of rectilinear propagation of light, the three-dimensional space point P is the intersection of the line connecting the projection center point of the two cameras and the imaging point. L and X R are the distances from the imaging points of the left and right cameras to the left imaging plane, respectively. The parallax of point P in the left and right cameras can be defined as follows:

[0092] d=|x L -x R | (7)

[0093] Two imaging points P L and P R The distance between them is:

[0094]

[0095] According to the theory of similar triangles, we can conclude that:

[0096]

[0097] We can further get the distance Z from point P to the projection center plane:

[0098]

[0099] When point P moves in three-dimensional space, the image position of point P on the left and right cameras will also change, and thus the parallax will also change accordingly. From the above formula, we can see that the parallax is inversely proportional to the distance from the point in three-dimensional space to the projection center plane. Therefore, as long as the parallax of a certain point is known, the depth information of that point can be determined.

[0100] c. Semi-global stereo matching:

[0101] The purpose of stereo matching is to find the correspondence between points in three-dimensional space on the left and right images, and then calculate depth information through disparity. Through stereo matching technology, the correspondence between points in the left and right images can be determined, thereby obtaining disparity and restoring the three-dimensional information of the points.

[0102] The semi-global block matching algorithm is a practical matching algorithm for calculating disparity maps. It effectively balances the advantages and disadvantages of local and global matching. While maintaining similar disparity map effects, it can significantly improve algorithm efficiency and achieve a better trade-off between accuracy and efficiency. The semi-global block matching algorithm is divided into four steps: matching cost calculation, cost aggregation, disparity calculation, and disparity optimization.

[0103] 1) Cost matching. Cost matching measures the similarity between the point to be matched and the candidate matching point. Matching metrics can be divided into correlation and difference methods. The larger the correlation metric, the higher the similarity between the two points, and the greater the probability that the two pixels are corresponding matching points; otherwise, the less likely the two pixels are corresponding matching points. Similarly, the smaller the difference metric, the higher the similarity between the pixels, and the larger the difference metric, the lower the similarity between the pixels. Before searching for matching points, a disparity range D is assigned to each pixel, limiting the search range to D. A matrix of size W×H×D (W, H are the image dimensions) is used to record the matching cost value of each pixel with the pixels within the disparity range. This matrix stores all matching cost values, effectively reducing repeated calculations and improving computational efficiency.

[0104] 2) Cost Aggregation. In the initial cost matching, the calculation scope is limited to the local window, only local correlation is considered, and it is very sensitive to noise. To obtain a better match, cost aggregation is performed. In cost aggregation, the semi-global matching algorithm adopts a global energy optimization strategy. This strategy sets a global energy function and continuously optimizes it to gradually minimize it, thus ensuring that each pixel is the best match. The function is defined as follows:

[0105]

[0106] Where D is the disparity map. E(D) is the energy function corresponding to the disparity map. p,q represent a pixel N in the image. p Refers to the adjacent pixels of pixel p (generally considered to be 8-connected). C(p, D p ) refers to the current pixel disparity D pThe cost of the pixel when . P1 is a penalty coefficient that applies to pixels whose disparity values among the neighboring pixels of pixel p differ by 1 from the disparity value of pixel p. P2 is a penalty coefficient that applies to pixels whose disparity values among the neighboring pixels of pixel p differ by more than 1 from the disparity value of pixel p. If the formula in the T[·] function is true, it returns 1, otherwise it returns 0.

[0107] 3) Disparity Calculation. A winner-takes-all algorithm is used for disparity calculation, where the optimal disparity for each pixel is the minimum-cost aggregate value. In this step, disparity is calculated for each pixel, resulting in a disparity map of the same size as the original image.

[0108] 4) Disparity Optimization. In the disparity calculation step, inaccurate cost aggregation directly affects the matching results. Therefore, a disparity optimization step has been added to eliminate incorrect matches and improve disparity accuracy according to basic matching constraints. These include epipolar constraints, similarity constraints, uniqueness constraints, and left-right consistency constraints.

[0109] 2. Object 3D pose optimization algorithm based on spatiotemporal constraints

[0110] Due to the presence of uncertain factors in the scene, including errors in image semantic coding, sensor noise, etc., the target detection based on convolutional neural networks and the binocular depth calculation algorithm based on visual geometry methods have uncertainty in estimating the 3D pose (position and posture) of the target object. Therefore, based on the prediction results of the two algorithms, the present invention proposes a space-time joint optimization algorithm to achieve higher-precision object 3D pose estimation. First, through a data association algorithm based on the local features of the object, the local features in the predicted 3D cube bounding boxes in different image frames are matched, and the 3D cube bounding boxes of the same object in adjacent frames are associated. Then, based on the space-time joint optimization algorithm, the uncertainty of the target position estimation is reduced by minimizing the projection error of the target in the image frame and spatial coordinates, and the accuracy of the pose in space is improved.

[0111] a. SIFT feature extraction and descriptor matching

[0112] This part extracts local SIFT feature points from the RGB image and encodes the pixel values around the feature points to obtain the descriptor of the feature points. Therefore, the feature descriptor quantifies the texture features around the feature points in some way and expresses them in the form of a vector.

[0113] The SIFT descriptor has excellent rotation invariance. After obtaining corner points at different scales, in order to ensure that the SIFT feature can produce the same descriptor under different image rotation transformations, SIFT calculates the main gradient direction of the pixels around the corner point and rotates it to unify the main direction to 0 degrees to achieve rotation invariance.

[0114] SIFT is an algorithm for detecting, describing, and matching local feature points in images. It detects extreme points in scale space, extracts position, scale, and rotation invariants, and abstracts them into feature vectors for description. These vectors are then used to match image feature points. SIFT features remain invariant to grayscale, contrast, rotation, and scaling, and are also robust to changes in perspective, affine shifts, and noise.

[0115] b. Object association based on local feature matching

[0116] Cross-frame data association is another important part of this technology. Compared to point matching, object association is easier because the surface of the object contains more texture, and many 2D object tracking or template matching methods can be used. Even in some simple cases, 2D bounding box overlap can work. However, these methods are not robust if there are severe object occlusions and repeated objects, such as Figure 5 Furthermore, dynamic objects need to be identified and removed from the current optimization, but standard object tracking methods cannot classify whether it is static or not unless specific motion segmentation methods are used.

[0117] Compared to many point-based data association methods, feature points in different views can be effectively matched by descriptor matching and epipolar geometry checking. Therefore, the matching of 2D bounding boxes is based on the principle that two 2D bounding boxes in different image frames are considered related if at least two points are observed within the object's 2D bounding box in two frames and their 3D distance to the cube center is less than 1 meter.

[0118] First, the feature points are associated with their corresponding objects. For example, Figure 5 In the algorithm, the color of the feature points is the same as the object they are associated with. Finally, if there are a maximum number of shared feature points between two objects and the number exceeds a certain threshold (10), they are matched in different frames. Through experiments, this method works well for wide baseline matching, repeated objects and occlusions. Dynamic feature points belonging to moving objects are discarded because they cannot satisfy the polarity constraint. Therefore, objects with a small number of related feature points are considered dynamic objects.

[0119] c. Object 3D pose optimization

[0120] In space, first, target detection based on convolutional neural networks can obtain the 3D cube bounding box of the object in the image frame after data association; according to the depth relationship between the local 3D cube bounding box in the image and the center of mass of the object in the real environment, the minimum 3D position error should be generated; in time, as the camera moves, the object is predicted as a different 3D cube bounding box in different image frames. However, if the target object is stationary in the world coordinate system, multiple 3D pose estimation results will be obtained for the same object in the world coordinate system based on the image mapping relationship. The reprojection error of the 3D object in the world coordinate system is constructed based on the Bundle Adjustment (BA) algorithm. Finally, the LM algorithm is used to minimize the projection error to obtain a more accurate 3D pose of the object, such as Figure 6 shown.

[0121] According to the spatiotemporal relationship of the target object, the reprojection error is established as:

[0122]

[0123] in Represents the camera’s position information at time t, [p o , R o ],[p ol , R ol ] represent the estimated pose of the target object and the landmark pose respectively; h(·) represents the projection function of the image, and log(·) maps the SO3 error to the vector space of 3DoF, so the error Belonging to the 6DoF vector space, such as Figure 7 Show.

[0124] The present invention is aimed at large-scale indoor and outdoor scenes. By incrementally adding optimized data based on the bundle adjustment algorithm, it can efficiently minimize the reprojection error based on the spatiotemporal relationship, and obtain the 3D pose information of the target object more quickly and accurately, that is, obtain more accurate 3D posture and position.

[0125] 3. Loop detection algorithm based on object label information and posture

[0126] Closed-loop detection in camera positioning estimation is the prerequisite for camera pose cumulative drift correction and is a key technology for achieving global consistency between the carrier trajectory and the semantic map. After completing the object semantic association in Algorithm 2, the geometric model and 3D position of the object constructed in the global map are obtained. Each object is set as a topological node V with semantic information such as label, 2D bounding box, position, and pose. A global map GM = {V g , E g}, similarly, the topological points of the local objects are obtained from the current frame image to establish the local map LM = {Vl , E l Therefore, the closed-loop detection of previously visited places is converted into matching the local topology map and the global topology map. Since these two topology map sets do not intersect with each other, the object-based closed-loop detection problem is a weighted bipartite graph matching problem.

[0127] First, the weight value of the edge E(O|I) in the topology graph is established through the object semantic information.

[0128] E(O|I)=φ′ dist (O, I) + φ′ scale (O, I) + φ′ angle (O, I) + φ′ hist (O, I) (13)

[0129] Where φ′ represents the normalized function of the cost function φ using the Z-score method. dist ,φ scale ,φ angel ,φ hist The cost functions for the semantic information of four objects, namely distance, size, rotation angle, and pixel value, are used. E(O|I) represents the weight of object O in image I.

[0130] a. Distance cost function φ dist is the object O in the global map GM G and the object O in the LM in the local map L distance difference.

[0131] φ dist =||t(O G )-t(O L )||2 (14)

[0132] b. Size cost function φ scale For object O G With object O L The difference between the length, width and height.

[0133] φ scale =|wid(O G )-wid(O L )|+|len(O G )-len(O L )|+|hei(O G )-hei(O L ) (27)

[0134] c. Angle cost function φ angle Represented as object O G With object O L The difference in yaw.

[0135] φ angel =|yaw(O G )-yaw(O L )| (15)

[0136] d. Pixel cost function φ hist Represented as object O G With object O L The cost function is used to calculate the color difference of the object.

[0137] φ hist =|hist(O G )-hist(O L )| (16)

[0138] Each optimized object is then set as a topological point V. The global and local bipartite graphs are matched using the weighted maximum matching (KM) algorithm. Closed-loop objects matching the currently observed object are calculated, known as closed-loop landmarks. For each matched closed-loop landmark, the camera image frame in which it is first observed is considered the closed-loop frame, thus completing closed-loop detection based on the object's semantic information.

[0139] 4. Positioning Estimation Drift Correction Algorithm Based on Pose Graph Optimization

[0140] When a closed-loop frame is found, the drift error can be eliminated by aligning the object positions, and the pose graph is built by minimizing the relative pose error to achieve the consistency of the global pose of the carrier.

[0141] exist Figure 8 The circle symbols represent repeatedly observed objects detected by the convolutional neural network (CNN) algorithm, and the triangle symbols represent key frames selected by the following strategy:

[0142] (1) The number of feature points tracked in the current frame is less than the threshold and is considered a key frame;

[0143] (2) If the average disparity of the features of the current frame relative to the nearest key frame is greater than a threshold, it is a key frame;

[0144] Therefore, a key frame is a frame selected from a series of local ordinary image frames to represent local information. Using a key frame strategy in the pose graph can improve the efficiency of achieving global pose consistency.

[0145] First, the objects in the current frame are matched with the objects in the global map by using the object semantics and position information in the above algorithm to obtain closed-loop landmarks and closed-loop frames. The matched landmarks in the current frame and closed-loop frame are represented as O c ={O c1 , Oc2 ,…,O cm}, O l ={O l1 , O l2 ,…,O lm}. And each landmark O has an observed starting frame I s and end frame I e The drift error from the camera pose aligned by matching objects is:

[0146]

[0147] Since the cuboid O obtained by landmark detection is symmetric, we optimize the local and global poses of the object in multiple consecutive frames where the landmark is detected to eliminate this ambiguity.

[0148]

[0149] where e se (O ci , O li ) is the 3D measurement error of the matched landmark between the start frame and the end frame.

[0150]

[0151] Where T I represents the pose of the carrier in image frame I, represents the local pose of the landmark at image frame I.

[0152] The corrected current frame pose can be further obtained Finally, the overall pose and trajectory of the carrier will be optimized based on the pose graph.

[0153]

[0154] The transformation relationship between adjacent poses is T ij =T i -1 T j .

[0155] The preferred embodiments of the present invention have been described in detail above. It should be understood that numerous modifications and variations based on the concepts of the present invention are possible without inventive effort by those skilled in the art. Therefore, any technical solution that can be derived by one skilled in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A semantic positioning method based on environmental objects, characterized in that: The following steps are involved: Step 1: Semantic Mapping: A convolutional neural network extracts the semantic and positional information of target objects in camera images and constructs a 3D map of semantic objects for loop closure detection. Step 2: Closed-loop detection: Combine the objects in the camera image with the objects in the semantic object 3D map for bidirectional comparison and matching to detect whether the camera has passed through the historical location. If so, a matching object is generated for camera positioning correction; Step 3: Positioning correction: Calculate the relative position relationship between objects with matching relationships, and establish a pose graph of the camera position and the object position. Use the pose graph to optimize and correct the camera positioning. The step 2 comprises the following steps: Step 2.1: For the RGB color image input by the camera, a local semantic map is obtained based on semantic mapping. In the local semantic map, a topological point is established at the center of each 3D cube bounding box. The connection between each topological point is set as a topological edge, and the spatial distance between topological points is the weight of the topological edge corresponding to the point. The local topological map is obtained from the local semantic map. Step 2.2: Use visual SLAM technology to estimate the camera's motion information and transform the local semantic map with the camera's optical center as the origin into a global semantic map with the camera's starting point as the origin. Then, use the local topological map generation method to obtain the global topological map. Step 2.3: Based on the object label information and position, the loop detection algorithm uses the KM algorithm to match the local and global topological maps. The topological point in the global topological map that corresponds to the local topological map at the current moment is calculated. This is the object that matches the local topological map at the current moment, which is the closed-loop object matching pair. Step 2.4: After obtaining the matching object, perform geometric verification based on the object's semantic information and calculate the difference in the size, label, and posture information of the matching object. When the difference is less than a certain threshold, it is confirmed that the camera has reached the closed-loop position and positioning drift correction is immediately performed; The step 3 comprises the following steps: Step 3.1: Align each closed-loop object pair, calculate the position error between the objects, and calculate the drift distance of the camera positioning estimate at the current moment by averaging the position errors between each pair of objects. Step 3.2: Use the positioning estimation drift correction algorithm based on pose graph optimization to reduce the error of camera positioning estimation and correct the drift of camera positioning estimation.

2. The semantic positioning method based on environmental objects according to claim 1, characterized in that: The step 1 comprises the following steps: Step 1.1: Input the RGB color image captured by the left eye of the binocular camera, extract the target object in the image through the convolutional neural network, and obtain the target object's label information, the 2D bounding box position in the image, and the object size; Step 1.2: Using the RGB color images of the left and right binocular cameras, calculate the 3D depth values of all target objects in the environment using a binocular depth calculation algorithm based on visual geometry methods. This is the vertical distance from the target object to the camera imaging plane, which is the 3D position of the object in the environment relative to the camera. Step 1.3: Use the visual odometry to extract feature points from the left and right images, and establish a reprojection error constraint for the feature points to estimate the relative position of the cameras. Step 1.4: The object 3D pose optimization algorithm based on spatiotemporal constraints minimizes the error between the CNN extraction results and the depth triangulation algorithm by establishing a constraint relationship between the target object extraction results in the temporally continuous RGB color image and the 3D position of the target object in the spatial environment. Step 1.5: Obtain the target object label information, 2D bounding box position, object size, and 3D position in the environment. Represent the target object in the environment as a 3D cube bounding box, and finally obtain a 3D stereo local semantic map with the camera optical center as the origin as the reference system.

3. The semantic positioning method based on environmental objects according to claim 2, characterized in that: The binocular depth calculation algorithm based on the visual geometry method and the depth estimation algorithm based on the binocular camera calculate the relative distance of the target object in the scene. First, the intrinsic parameters of the binocular camera and the rotation matrix and translation matrix between the two cameras are obtained through stereo camera calibration. The binocular camera image is then stereo corrected to obtain a distortion-free and horizontally aligned binocular image. The semi-global stereo matching algorithm is then applied to the corrected binocular image to obtain a depth map of the current scene. Combined with the 2D bounding box of the target object, the relative depth of the target center is calculated as the vertical distance of the target object to the camera imaging plane.

4. The semantic positioning method based on environmental objects according to claim 2, characterized in that: The binocular depth calculation algorithm based on the visual geometry method comprises the following steps: Step 1.2.1, stereo camera calibration; Step 1.2.2, stereo image correction; Step 1.2.3: Semi-global stereo matching.

5. The semantic positioning method based on environmental objects according to claim 4, characterized in that: The semi-global stereo matching includes cost matching, cost aggregation, disparity calculation and disparity optimization.

6. The semantic positioning method based on environmental objects according to claim 5, characterized in that: The disparity optimization includes epipolar constraints, similarity constraints, uniqueness constraints and left-right consistency constraints.

7. The semantic positioning method based on environmental objects according to claim 2, characterized in that: The object 3D pose optimization algorithm based on spatiotemporal relationship constraints first matches the local features in the predicted 3D cube bounding boxes in different image frames through a data association algorithm based on the local features of the object, and associates the 3D cube bounding boxes of the same object in adjacent frames; then, based on a space-time joint optimization algorithm, minimizes the projection error of the target in the image frame and spatial coordinates.

8. The semantic positioning method based on environmental objects according to claim 2, characterized in that: The object 3D pose optimization algorithm based on spatiotemporal relationship constraints includes the following steps: Step 1.4.1, SIFT feature extraction and descriptor matching; Step 1.4.2: Object association based on local feature matching; Step 1.4.3: Optimize the object's 3D pose.

Images