A method for object feature processing in robot vision SLAM

Abstract

A method for processing object features in robot vision SLAM relates to the field of robot image feature processing. The method solves the problem that the plane information of the existing object feature processing task is not fully constrained and the Mask RCNN is introduced into SLAM, and the real-time performance of the Mask RCNN is poor, making it difficult for the system to be truly used. The method includes: carrying out the representation of the dual quadratic surface of the object feature; calculating the center point and the average axis length of the ellipsoid, and constructing a spatial ellipsoid according to the center point and the average axis length of the ellipsoid; performing data association on the same object and feature; constructing a projection model of the object feature; calculating the reprojection error of the ellipsoid; establishing a plane map in the indoor space, and obtaining the support plane of the object in the plane map, adding support plane constraints to the object; and constructing an object factor graph according to the support plane constraints. It is applied to the field of three-dimensional semantic map generation.

Description

Technical Field

[0001] The present invention relates to the field of robot image feature processing, and in particular to an object feature processing method in robot vision SLAM. Background Art

[0002] Traditional robot SLAM maps usually only have the most basic geometric information and do not carry object-level semantic information. For indoor mobile robots, a SLAM system with rich semantic information is of great significance. It can greatly enrich the robot's understanding of the surrounding world and is an indispensable component for future robots to work in human work or home environments. Object-level semantic information in space is very common, such as common computer monitors, keyboards, mice, and water cups in office environments, and common televisions, refrigerators, and microwave ovens in home environments. Therefore, a method for processing object features in robot vision SLAM is studied, which has important theoretical research and engineering practice significance.

[0003] For the task of object feature processing in robot SLAM, scholars have adopted different methods to deal with it. For example, Yang et al. proposed "Monocular object and plane slam in structure denvironments" in 2019, which added planes to the robot SLAM system and proposed indoor monocular SLAM based on objects and planes, but the plane information was not fully constrained; for example, Ming et al. proposed "EAO-SLAM: Monocular semi-dense object slam based on ensemble data association" in 2020, which introduced Mask RCNN into SLAM to obtain accurate instance segmentation of objects, and carry out the tasks of establishing three-dimensional bounding boxes and estimating poses of objects, but due to the poor real-time performance of Mask RCNN, the entire system is difficult to be truly used. Summary of the invention

[0004] Aiming at the problem that the plane information of the object feature processing task in the existing robot SLAM is not fully constrained and the Mask RCNN is introduced into the SLAM, the real-time performance of the Mask RCNN is poor, making it difficult for the entire system to be truly used, the present invention proposes a method for object feature processing in robot vision SLAM, the method comprising:

[0005] S1: Develop the representation of the dual quadratic surface of object features;

[0006] S2: According to multiple target observation points generated by the same object under multiple viewing angles, the center point and average axis length of the ellipsoid are calculated, and a spatial ellipsoid is constructed according to the center point and average axis length of the ellipsoid;

[0007] S3: Data association of the same object and features based on the constructed spatial ellipsoid;

[0008] S4: Construct a projection model of object features based on the spatial ellipsoid projection;

[0009] S5: Calculate the reprojection error of the ellipsoid;

[0010] S6: establishing a plane map in the indoor space, obtaining a support plane of the object in the plane map, and adding a support plane constraint to the object;

[0011] S7: Construct object factor graph based on support plane constraints.

[0012] Furthermore, a preferred method is proposed, wherein step S1 comprises:

[0013] The space dual quadratic surface of the object characteristics is:

[0014] π Τ Qπ=0

[0015] Among them, Q is the dual quadratic surface, π Τ is the transpose of the tangent plane of Q, and π is the tangent plane of Q;

[0016] Ellipsoid Q in the Dual Quadric Surface * :

[0017]

[0018] Among them, s1, s2, and s3 correspond to the three semi-axis lengths of the ellipsoid on the x, y, and z axes respectively. The last element of the matrix -1 is used to define the global scale. The position and posture of the ellipsoid are expressed as Z:

[0019]

[0020] Among them, θ represents the three rotation angles of the ellipsoid, expressed as θ = [θ1 θ2 θ3] Τ Indicates, t represents the translation of the ellipsoid, and t = [t1 t2 t3] Τ , represented by a 9-dimensional vector q:

[0021] q=[θ1 θ2 θ3 t1 t2 t3 s1 s2 s3] Τ

[0022] Under the transformation of the ellipsoid posture Z, a new ellipsoid Q is obtained* ′:

[0023] Q * ′=ZQ * Z Τ .

[0024] Furthermore, a preferred method is proposed, wherein step S2 comprises:

[0025] The linear equations formed by all observation frames of the object are expressed in matrix form:

[0026]

[0027] The calculation of the three-dimensional center point of the object is converted into the problem of solving a linear equation system, and the singular value decomposition method is used to solve it. The calculated result is O′=[O′1 O′2 O′3 O′4] Τ , then O = O' / O'4, and the three-dimensional center point of the object is obtained;

[0028] Calculate the projection of the two-dimensional detection frame to the camera coordinate system z=z c The average width w and average height h of the plane:

[0029]

[0030] Among them, x′ i,2 is the horizontal coordinate of the lower right point of the detection box, x′ i，1 is the horizontal coordinate of the upper left point of the detection box, y′ i,2 is the ordinate of the lower right point of the detection box, y′ i，1 is the vertical coordinate of the upper left point of the detection box, w i is the detection box width, h i is the detection frame height;

[0031] Taking the average of the width and height gives the radius of the sphere r = (w + h) / 2.

[0032] Furthermore, a preferred method is proposed, wherein step S3 comprises:

[0033] Construct a DQ cost matrix cost for each frame of the object, where D represents the detection box and Q represents the map ellipsoid. The cost matrix is:

[0034]

[0035] The cost matrix cost is solved according to the Hungarian algorithm to obtain the best matching relationship between each detection box and the map ellipsoid, so that the total cost is maximized, and the DQ is associated with the matching results.

[0036] Furthermore, a preferred method is proposed, wherein step S4 comprises:

[0037] Project the ellipsoid into the pixel coordinate system and solve the error e in the pixel coordinate system:

[0038] e=f(C * ,C * ′)=f(C * ,PQ * ′P Τ )

[0039] Among them, C * is the observation ellipse, C * ′ is the projected ellipse, P is the forward projection of the ellipsoid through the camera, and f(·) is the error function for calculating the distance between the two ellipses;

[0040] The constructed least squares error function F1(·) is:

[0041]

[0042] Furthermore, a preferred embodiment is proposed, wherein step S5 comprises:

[0043] Convert the distance error between two ellipses into the distance error between two two-dimensional normal distributions;

[0044] Use Wasserstein distance as the distance between two normal distributions.

[0045] Furthermore, a preferred method is proposed, wherein the Wasserstein distance is used as the distance between two normal distributions, including:

[0046] Two two-dimensional normal distributions N1(μ1,Σ1) and N2(μ2,Σ2), where μ1 and μ2 are the mean vectors of the normal distributions, and Σ1 and Σ2 are the covariance matrices;

[0047] Compute the Euclidean distance d between the mean vectors μ1 and μ2 of two distributions:

[0048] d=||μ1-μ2||

[0049] Compute the square of the Wasserstein distance:

[0050]

[0051] Among them, Tr(·) represents the inverse operation of the matrix;

[0052] Calculate the Wasserstein distance:

[0053]

[0054] Furthermore, a preferred embodiment is proposed, wherein step S6 comprises:

[0055]

[0056] Among them, Rot x (Q * ) represents the direction vector of the x-axis of the ellipsoid, Rot y (Q * ) represents the direction vector of the y-axis of the ellipsoid, and n is the normal vector of the supporting plane.

[0057] Based on the same inventive concept, the present invention also proposes a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor executes a method for processing object features in a robot visual SLAM as described in any one of the above items.

[0058] Based on the same inventive concept, the present invention also proposes a computer-readable storage medium, which is used to store a computer program, and the computer program executes any of the above-mentioned object feature processing methods in robot vision SLAM.

[0059] The present invention is beneficial in that:

[0060] The present invention solves the problem that the plane information of the object feature processing task in the existing robot SLAM is not fully constrained and the real-time performance of Mask RCNN is poor when Mask RCNN is introduced into SLAM.

[0061] The present invention proposes an object feature processing method in robot vision SLAM, which uses an ellipsoid to represent object-level semantic information in space. Since the ellipsoid is a dual quadratic surface in projective geometry, the representation method has better projective geometry properties, making it simpler to construct a reprojection model of object features, and constructing a more robust object feature processing algorithm and a new ellipse error calculation method, finally, a more accurate object-level semantic map can be built.

[0062] The present invention proposes an object feature processing method in robot vision SLAM, which provides the robot SLAM system with object features different from traditional point features, generates a three-dimensional semantic map with object features, and supports the robot SLAM to perform tasks related to object features.

[0063] The present invention proposes a method for processing object features in robot visual SLAM, which finally constructs an object feature map with rich semantic information through the detection frame generated by the target detection network, combined with the multi-frame observation and camera pose of the visual SLAM system. Compared with the traditional robot SLAM map, the object feature map directly has object-level information, which can improve the robot's mapping and positioning accuracy on the one hand, and the object feature information can directly provide the robot with environmental semantic information on the other hand, thereby improving the robot's environmental understanding ability.

[0064] The present invention is applied to the field of three-dimensional semantic map generation. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 A flow chart of an object feature processing method in a robot vision SLAM according to the first embodiment;

[0066] Figure 2 This is a schematic diagram of the object feature projection model described in Implementation Mode 5;

[0067] Figure 3 The corresponding diagram of the ellipse and the two-dimensional normal distribution described in the sixth embodiment;

[0068] Figure 4 This is a structural diagram of the object feature graph model described in Implementation Example 8;

[0069] Figure 5 This is a comparison diagram of different viewing angles of the TUM data set described in Implementation Example 11, wherein: Figure 5 (a) is a picture of the TUM dataset. Figure 5 (b) is the object plane point cloud, Figure 5 (c) is a picture from the TUM dataset. Figure 5 (d) Object plane point cloud;

[0070] Figure 6 This is a plane constraint effect diagram described in Implementation 11;

[0071] Figure 7 This is a schematic diagram of a dense point cloud of an object according to the eleventh embodiment;

[0072] Figure 8 This is a comparison diagram of different viewing angles of the self-built data set according to the eleventh embodiment, wherein: Figure 8 (a) is a picture of a desk. Figure 8 (b) is the object plane point cloud corresponding to the desk. Figure 8 (c) is the picture of the second desk. Figure 8 (d) is the object plane point cloud corresponding to desk 2. Figure 8 (e) is the picture of the second desk. Figure 8 (f) is the object plane point cloud corresponding to desk 2. Figure 8 (g) is the picture of the second desk. Figure 8 (h) is the object plane point cloud corresponding to desk 2;

[0073] Fig. 9 is a top view of the object feature according to the eleventh embodiment, wherein: Fig. 9 (a) is the object plane point cloud, Fig. 9 (b) is the dense point cloud of the object. DETAILED DESCRIPTION

[0074] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.

[0075] Implementation method 1, see Figure 1 The present embodiment is described as follows. The present embodiment describes a method for processing object features in a robot vision SLAM, the method comprising:

[0076] S1: Develop the representation of the dual quadratic surface of object features;

[0077] S2: According to multiple target observation points generated by the same object under multiple viewing angles, the center point and average axis length of the ellipsoid are calculated, and a spatial ellipsoid is constructed according to the center point and average axis length of the ellipsoid;

[0078] S3: Data association of the same object and features based on the constructed spatial ellipsoid;

[0079] S4: Construct a projection model of object features based on the spatial ellipsoid projection;

[0080] S5: Calculate the reprojection error of the ellipsoid;

[0081] S6: establishing a plane map in the indoor space, obtaining a support plane of the object in the plane map, and adding a support plane constraint to the object;

[0082] S7: Construct object factor graph based on support plane constraints.

[0083] In this embodiment, an ellipsoid is used to represent object-level semantic information in space. Since the ellipsoid is a dual quadratic surface in projective geometry, this representation method has better projective geometry properties, making it simpler to construct a reprojection model of object features, and construct a more robust object feature processing algorithm and a new ellipse error calculation method. Finally, a more accurate object-level semantic map can be built.

[0084] In this embodiment, by representing the object features with dual quadratic surfaces, the shape and surface features of the object can be captured more accurately, thereby improving the accuracy of object recognition and tracking. By calculating multiple target observation points generated by the same object under multiple perspectives, a spatial ellipsoid is constructed, and data association is performed on the same object and its features based on the spatial ellipsoid, thereby improving the accuracy of modeling and tracking of the object in three-dimensional space. According to the constructed spatial ellipsoid, a projection model of the object features is established, and the reprojection error of the ellipsoid is calculated. Through this step, the position of the object features can be accurately estimated and corrected. A plane map is established in the indoor space, and the support plane of the object is obtained from it. The support plane constraints are added to the object. This step can help improve the stability and accuracy of the object's posture estimation and motion tracking. An object factor graph is constructed based on the support plane constraints. Through this step, the three-dimensional information of the object features can be fused with the plane information in the map, thereby achieving accurate positioning and tracking of the object in three-dimensional space.

[0085] Embodiment 2: This embodiment further limits the object feature processing method in a robot vision SLAM described in embodiment 1, and the step S1 includes:

[0086] The spatial quadratic surface of the object feature is:

[0087] π Τ Qπ=0

[0088] Among them, Q is the dual quadratic surface, π Τ is the transpose of the tangent plane of Q, and π is the tangent plane of Q;

[0089] Ellipsoid Q in the Dual Quadric Surface * :

[0090]

[0091] Among them, s1, s2, and s3 correspond to the three semi-axis lengths of the ellipsoid on the x, y, and z axes respectively. The last element of the matrix -1 is used to define the global scale. The position and posture of the ellipsoid are expressed as Z:

[0092]

[0093] Among them, θ represents the three rotation angles of the ellipsoid, expressed as θ = [θ1 θ2 θ3] Τ Indicates, t represents the translation of the ellipsoid, and t = [t1 t2 t3] Τ , represented by a 9-dimensional vector q:

[0094] q=[θ1 θ2 θ3 t1 t2 t3 s1 s2 s3] Τ

[0095] Under the transformation of the ellipsoid posture Z, a new ellipsoid Q is obtained * ′:

[0096] Q * ′=ZQ * Z Τ .

[0097] Specifically, in this embodiment, the spatial quadratic surface of the object feature can be expressed in the matrix form shown in formula (1):

[0098] X Τ QX=0 (1)

[0099] In formula (1), Q is a 4×4 symmetric matrix, and X is [xyz 1] Τ , the matrix of Q is shown in formula (2):

[0100]

[0101] The quadratic surface Q has 9 degrees of freedom, corresponding to 10 independent elements in the matrix, minus 1 global scale, that is, the J element in the matrix, and finally 9 degrees of freedom are obtained. Generally, a spatial quadratic surface can be determined by 9 points. * It is the dual form of the quadratic surface Q, and still belongs to the quadratic surface, with all the properties of the quadratic surface. Similarly, under the point transformation X′=HX, the dual quadratic surface Q * The transformation is shown in formula (3):

[0102] Q * ′=HQ * H Τ (3)

[0103] It can be seen that in space transformation, the amount of algebraic operations for the dual quadratic surface is much lower than that for the quadratic surface. Moreover, in projective geometry, given the camera pose matrix P = K[R t], then the dual quadratic surface Q in space is * Through camera projection, we get the dual quadratic surface C of the pixel plane. * , the forward projection of the quadratic surface can be calculated by dual form, as shown in formula (4):

[0104] C * =PQ * P Τ (4)

[0105] In this embodiment, an ellipsoid in the dual quadratic surface is used to define an ellipsoid Q without translation and rotation. * , as shown in formula (5):

[0106]

[0107] In the above formula, s1, s2, and s3 correspond to the lengths of the three semi-axes of the ellipsoid on the x, y, and z axes respectively. The last element of the matrix -1 is used to define the global scale. The position and posture of the ellipsoid are expressed as Z, as shown in formula (6):

[0108]

[0109] In the above formula, θ represents the three rotation angles of the ellipsoid, expressed as θ = [θ1 θ2 θ3] Τ Indicates, t represents the translation of the ellipsoid, and t = [t1 t2 t3] Τ , in this implementation, it is represented by a 9-dimensional vector q, as shown in formula (7):

[0110] q=[θ1 θ2 θ3 t1 t2 t3 s1 s2 s3] Τ (7)

[0111] Under the transformation of the ellipsoid posture Z, a new ellipsoid Q is obtained * ′, the transformation formula is shown in (8):

[0112] Q * ′=ZQ * Z Τ (8)

[0113] Comparing equation (3) and equation (8), in this embodiment, the ellipsoid posture Z corresponds to the point transformation matrix H.

[0114] Implementation method 3: This implementation method further limits the object feature processing method in a robot vision SLAM described in implementation method 1, and step S2 includes:

[0115] The linear equations formed by all observation frames of the object are expressed in matrix form:

[0116]

[0117] The calculation of the three-dimensional center point of the object is converted into the problem of solving a linear equation system, and the singular value decomposition method is used to solve it. The calculated result is O′=[O′1 O′2 O′3 O′4] Τ , then O = O' / O'4, and the three-dimensional center point of the object is obtained;

[0118] Calculate the average width w and average height h of the two-dimensional detection box projected into the camera coordinate system:

[0119]

[0120] Among them, x′ i,2 is the horizontal coordinate of the lower right point of the detection box, x′ i，1 is the horizontal coordinate of the upper left point of the detection box, y′ i,2 is the ordinate of the lower right point of the detection box, y′ i，1 is the vertical coordinate of the upper left point of the detection box, w i is the detection box width, h i is the detection frame height;

[0121] Taking the average of the width and height gives the radius of the sphere r = (w + h) / 2.

[0122] Specifically, this implementation uses the detection frame information provided by the YOLOv5 target detection thread to construct an initial ellipsoid for the object, and adopts the means of conducting multiple observations on the object. When the number of observation frames of the object exceeds 10, and the observation angle between the first frame and the last frame is greater than 10°, the ellipsoid is constructed for the object, which is specifically implemented by the following steps:

[0123] Step 1: The method for calculating the observation angle is as follows: define the camera's intrinsic parameter matrix as K, and the camera pose rotation matrix of the first frame and the last frame as R H and R L , the center coordinate of the detection box is u H and u L , then the observation angle θ between the two frames is as shown in formula (9):

[0124]

[0125] In the above formula, the center coordinates are in homogeneous form. First, the center coordinates are back-projected to the camera coordinate system, and then the point is converted to a unit vector, which represents the observation direction of the point in the camera coordinate system. Then, the position rotation matrix is ​​used to transform it back to the world coordinate system, and finally, the angle between the two vectors is calculated.

[0126] Step 2: When the observation angle meets the requirements, calculate the three-dimensional center point of the object and define the camera pose of the i-th frame as P i =K[R i t i ], the dimension of the matrix is ​​3×4, the three-dimensional center point of the object is O = [xyz 1] Τ , according to the camera projection model, we can get the equation (10):

[0127]

[0128] In the above formula, P i The i-th row of i,1 , continue to simplify the above formula to obtain the equation system shown in formula (11):

[0129]

[0130] The linear equations formed by all observation frames of the object are written in matrix form, as shown in formula (12):

[0131]

[0132] Through the above calculation, the calculation of the three-dimensional center point of the object is converted into the problem of solving a linear equation system. The singular value decomposition method is used to solve it. The calculated result is O′=[O′1 O′2 O′3 O′4] Τ , then O=O′ / O′4, and the three-dimensional center point of the object is obtained.

[0133] Step 3: Calculate other parameters of the ellipsoid. Since the initialization data is often relatively small, the ellipsoid is initialized as a sphere during the object's ellipsoid initialization. The sphere simplifies the calculation. The center O of the sphere has been calculated. Since the sphere does not rotate, only the radius r of the sphere has not been solved.

[0134] The object detection frame output by the target detection thread is located by the upper left point and the lower right point. First, the coordinates of the center point of the object detection frame in the i-th frame in the camera coordinate system are defined as O c =[x c y c z c ] Τ , the pixel coordinate of the upper left point of the detection box is b i,1 =[u i,1 v i,1 ] Τ , the pixel coordinate of the lower right point of the detection box is b i,2 =[u i,2 v i,2 ] Τ , these two points are z=z in the camera coordinate system c The projection on the plane is point B' i,1 =[x′ i,1 y′ i,1 z c ] Τ and point B′ i,2 =[x′ i,2 y′ i,2 z c ] Τ , we can get the formula (13):

[0135]

[0136] According to the results of the above formula, the average width w and average height h of the two-dimensional detection frame projected to the camera coordinate system are calculated, and formula (14) is obtained:

[0137]

[0138] Take the average value of width and height to get the radius r of the sphere = (w + h) / 2. Through the above calculation, the center point O and radius r of the initialized sphere have been obtained. In order to ensure the accuracy of the calculation, the calculation results will be verified. The verification method is to project the center point O of the sphere to the pixel coordinate system through the transformation matrix of each frame, and calculate the error distance from the true center of the detection frame. If it is greater than the error threshold, it is considered that the verification of the reconstruction center O in the current frame has failed. If the number of frames with failed verification is greater than 30% of the total number of frames, the initialization of the ellipsoid has failed, otherwise the initialization of the ellipsoid is successful.

[0139] Embodiment 4: This embodiment further limits the method for processing object features in a robot vision SLAM described in embodiment 1, and step S3 includes:

[0140] Construct a DQ cost matrix cost for each frame of the object, where D represents the detection box and Q represents the map ellipsoid. The cost matrix is:

[0141]

[0142] The cost matrix cost is solved according to the Hungarian algorithm to obtain the best matching relationship between each detection box and the map ellipsoid, so that the total cost is maximized, and the DQ is associated with the matching results.

[0143] Specifically, considering the object initialization or further optimization of the object posture process, it is necessary to observe the object in multiple frames, and it is necessary to solve the problem of how to determine whether the object observed in the current frame has been observed before.

[0144] To this end, the strategy adopted in this implementation is to construct a DQ cost matrix cost for each frame, where D represents the detection box and Q represents the map ellipsoid. The structure of the matrix is ​​shown in formula (15):

[0145]

[0146] The dimension of the cost matrix is ​​n×m, which means that there are n detection boxes in the current frame and there are m map ellipsoids that may match the current frame, where c i,jIndicates the matching degree between the i-th detection box and the j-th map ellipsoid (the higher the matching degree, the larger the value). This implementation uses the Hungarian algorithm to solve the cost matrix cost, obtain the best matching relationship between each detection box and the map ellipsoid, so that the total cost is maximized, and the DQ is associated with the matching results. The calculation method of the DQ cost matrix adopts the following steps:

[0147] Step 1: Match all DQ points with map points. This is because map points are relatively stable features in the system. The matches with the highest confidence are selected through map points, and the corresponding c in the DQ cost matrix is i,j When it is set to the maximum, a definite matching relationship is formed.

[0148] Step 2: Reproject all DQs and transform the map ellipsoid Q j Projected to the pixel coordinate system to obtain the projected ellipse C′ j , calculate the rectangular bounding box D′ of the projected ellipse j , the target detection box is defined as D i , then the corresponding c i,j The cost calculation formula is (16):

[0149]

[0150] In the above formula, Area(·) represents the pixel area of ​​the rectangular box. The cost interval calculated in this way is [0,1], and the intersection ratio is used to represent the degree of matching.

[0151] In addition, the system may initialize the same object multiple times under some inaccurate camera positions, resulting in multiple ellipsoid representations of the object in space. To solve this problem, we need to associate multiple ellipsoid representations of the same object. For example, for ellipsoid Q, the same ellipsoid Q as all the ellipsoids of the same category in the map i To perform matching, the matching method in this embodiment adopts the following steps:

[0152] Step 1: Calculate Q i Whether the center of is in Q;

[0153] Step 2: Calculate whether the center of Q is in Q i Inside;

[0154] Step 3: Calculate the intersection ratio of the enclosing cubes of the two ellipsoids using the analogous formula;

[0155] Step 4: Calculate the number of matching map points of the two ellipsoids.

[0156] Through the above four judgment methods, if one of them meets the requirements or reaches the threshold, the fusion reconstruction of the two ellipsoids is performed.

[0157] By constructing a cost matrix and using the Hungarian algorithm to solve the best matching relationship, accurate matching between each detection box and the map ellipsoid can be achieved. This precise data association can reduce false matches and missed matches, and improve the accuracy of tracking object features. The method proposed in this embodiment has strong applicability to various object feature processing scenarios, and can effectively track and locate object features in both complex indoor environments and dynamic outdoor scenes. Solving the best matching relationship through the Hungarian algorithm can improve the robustness of the algorithm to data noise and interference to a certain extent, making the entire system more reliable and stable.

[0158] Implementation method 5: See Figure 2 This embodiment further defines the method for processing object features in a robot vision SLAM described in Embodiment 1, and the step S4 includes:

[0159] Project the ellipsoid into the pixel coordinate system and solve the error e in the pixel coordinate system:

[0160] e=f(C * ,C * ′)=f(C * ,PQ * ′P Τ )

[0161] Among them, C * is the observation ellipse, C * ′ is the projected ellipse, P is the forward projection of the ellipsoid through the camera, and f(·) is the error function for calculating the distance between the two ellipses;

[0162] The constructed least squares error function F1(·) is:

[0163]

[0164] Specifically, the projection model of object features is different from that of plane features. The plane feature model solves errors in the camera coordinate system, but it is difficult for object features to use the plane feature method. Therefore, this implementation method projects the ellipsoid into the pixel coordinate system and solves the errors in the pixel coordinate system. The specific projection model is as follows: Figure 2 shown.

[0165] exist Figure 2 In the map ellipsoid Q * ′ represents the initialized ellipsoid or the optimized ellipsoid, which is forward projected by the camera P = K[R t] to generate a projection ellipse C in the camera's pixel coordinate system. *′, which is represented by a dotted ellipse in the figure. The target detection thread will generate a detection box for the object, which is represented by a yellow rectangular box in the figure. An observation ellipse C is generated through this rectangular detection box. * , represented by a solid ellipse in the figure, the error between the observed ellipse and the projected ellipse is represented by e. According to the above calculation process, the specific expression of the error e is shown in formula (17):

[0166] e=f(C * ,C * ′)=f(C * ,PQ * ′P Τ ) (17)

[0167] In the above formula, f(·) represents the error function for calculating the distance between two ellipses. All observations of the current object are projected to form the least squares error function F1(·) as shown in formula (18):

[0168]

[0169] In the above formula, the camera's projection matrix (same as camera pose) P is not optimized, so the projection matrix is ​​not considered as a variable. Only the map ellipsoid Q is optimized. * ′ has 9 parameters.

[0170] Implementation Method 6: See Figure 3 This embodiment further defines the method for processing object features in robot vision SLAM described in Embodiment 5, and the step S5 includes:

[0171] Convert the distance error between two ellipses into the distance error between two two-dimensional normal distributions;

[0172] Use Wasserstein distance as the distance between two normal distributions.

[0173] In order to achieve high-quality projection error calculation, this embodiment represents the ellipse as a two-dimensional normal distribution and defines the ellipse C * The major and minor axes are s1 and s2, the counterclockwise angle between the major axis and the x-axis is θ, and the center point is (c x ,c y ), define the corresponding two-dimensional normal distribution as N(μ,Σ), then through the ellipse C * The calculation formula for calculating the two-dimensional normal distribution N(μ,Σ) is shown in formula (19):

[0174]

[0175] Through the transformation of the above formula, the ellipse is converted into the corresponding two-dimensional normal distribution. Figure 3 This is an example of a scatter plot of an ellipse and its corresponding two-dimensional normal distribution. It can be seen that the two have a good correspondence.

[0176] This embodiment converts the distance error between ellipses into the distance error between two two-dimensional normal distributions, which can more accurately reflect the difference between ellipse projections. This conversion takes into account the shape and direction of the ellipse, making the error calculation more accurate. Using the Wasserstein distance as an indicator for calculating the distance between two two-dimensional normal distributions takes into account the geometric features between the ellipses, such as shape, size, and direction.

[0177] Embodiment 7: This embodiment further limits the method for processing object features in a robot visual SLAM described in Embodiment 6, wherein the method uses Wasserstein distance as a distance between two normal distributions, including:

[0178] Two two-dimensional normal distributions N1(μ1,Σ1) and N2(μ2,Σ2), where μ1 and μ2 are the mean vectors of the normal distributions, and Σ1 and Σ2 are the covariance matrices;

[0179] Compute the Euclidean distance d between the mean vectors μ1 and μ2 of two distributions:

[0180] d=||μ1-μ2||

[0181] Compute the square of the Wasserstein distance:

[0182]

[0183] Among them, Tr(·) represents the inverse operation of the matrix;

[0184] Calculate the Wasserstein distance:

[0185]

[0186] The distance error between two ellipses is converted into the distance error between two two-dimensional normal distributions. There are many methods for calculating the distance between two normal distributions. This implementation uses Wasserstein distance as a method for calculating the error distance between two ellipses. Assuming that there are two two-dimensional normal distributions N1(μ1,Σ1) and N2(μ2,Σ2), where μ1 and μ2 are their mean vectors, Σ1 and Σ2 are their covariance matrices, respectively. The specific implementation is implemented by the following steps:

[0187] Step 1: Calculate the Euclidean distance between the mean vectors of the two distributions:

[0188] d=||μ1-μ2|| (20)

[0189] Step 2: Calculate the square of the Wasserstein distance:

[0190]

[0191] In the above formula, Tr(·) represents the inverse operation of the matrix.

[0192] Step 3: Calculate the Wasserstein distance:

[0193]

[0194] Compared with simple Euclidean distance or Mahalanobis distance, Wasserstein distance can more accurately measure the difference between two ellipses. This method is applicable to ellipses of different shapes and sizes. Whether it is for plane projection or spatial projection, it can effectively calculate the reprojection error, thereby accurately evaluating the position and posture of object features. By considering the geometric features of the ellipse and using the Wasserstein distance, the algorithm can be more robust to data noise and uncertainty, allowing the system to accurately process object features in complex environments.

[0195] Implementation Method 8: See Figure 4 This embodiment further defines the method for processing object features in robot vision SLAM described in Embodiment 1, and the step S6 includes:

[0196]

[0197] Among them, Rot x (Q * ) represents the direction vector of the x-axis of the ellipsoid, Rot y (Q * ) represents the direction vector of the y-axis of the ellipsoid, and n is the normal vector of the supporting plane.

[0198] In this implementation, it is taken into consideration that most objects in the indoor environment will form a supporting relationship with the structural plane in the space. This supporting relationship can provide additional strong constraints on the posture of the object features, making the posture solution of the object features more accurate. To this end, the object and its supporting plane are bound, and constraints are added between the object and the supporting plane.

[0199] Based on the plane map of the indoor space, this implementation needs to find the supporting plane of the object in these planes, and then build constraints. Considering that the horizontal plane is consistent with most situations in reality, this implementation only considers the horizontal supporting plane. Based on the above processing, the specific implementation steps of binding objects and supporting planes and adding constraints between objects and supporting planes in this implementation are as follows:

[0200] Step 1: Find the horizontal plane among all map planes. The calculation method is shown in formula (23):

[0201]

[0202] In the above formula, n is the normal vector of the plane, [0 0 1] Τ is the vertical direction vector in the coordinate system of this article, and the angle between the two vectors is calculated. If the angle is less than 10°, the present invention considers that the plane is a horizontal plane.

[0203] Step 2: Find the support surface of the current object in the horizontal plane by calculating the distance from the center of the object to the plane. If the distance is positive and less than 1.5 times the average axis length of the object's ellipsoid, the present invention considers the horizontal plane to be the support surface of the object and binds it.

[0204] Step 3: Add the support surface constraint to the back-end optimization. The support constraint of the support surface to the object is as follows:

[0205]

[0206] In the above formula, Rot x (Q * ) represents the direction vector of the x-axis of the ellipsoid, Rot y (Q * ) represents the direction vector of the y-axis of the ellipsoid. If the object and the plane form a support relationship, then these two direction vectors are perpendicular to the normal vector of the plane. The third formula indicates that the plane and the object are in a tangent relationship, which also conforms to the support relationship.

[0207] Step 4: Construct the support plane constraint error F2(·) of the object, as shown in formula (25):

[0208]

[0209] This embodiment obtains a nonlinear least squares function F(·) of the overall error of the object, as shown in formula (26):

[0210]

[0211] Since there is a nonlinear part f(·) in the overall error, the Jacobian matrix of the error function cannot be directly solved. The present invention uses the automatic differentiation tool in the G2O optimization library to calculate the error function with respect to Q * The Jacobian matrix J of Q (·), and then use the LM algorithm to iteratively reduce the total error to obtain the optimized spatial ellipsoid pose matrix Q * ′.

[0212] The graphical model of the total error of object features in this embodiment is as follows: Figure 4 As shown. The graph model represents the variable relationship in the system in the form of vertices and edges. In the figure above, the circle represents the "vertices" in the graph model, and the black straight line represents the "edges" of the graph model. i Represents the coordinates of the map point in the map, T i represents the camera pose, π i Represents the map plane parameters in the map, Q i Represents the map ellipsoid parameters in the map. The edge formed between two vertices defines the error between the two vertices, z i represents the error between the camera pose and the map point, q i Represents the error between the camera pose and the map plane, m i Represents the error between the camera pose and the map ellipsoid, s i Represents the error between the map ellipsoid and its supporting surface.

[0213] Embodiment 9. A computer device described in this embodiment includes a memory and a processor, wherein a computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes a method for processing object features in a robot vision SLAM as described in any one of embodiments 1 to 8.

[0214] Embodiment 10: A computer-readable storage medium described in this embodiment, wherein the computer-readable storage medium is used to store a computer program, and the computer program executes a method for processing object features in a robot vision SLAM described in any one of embodiments 1 to 8.

[0215] Implementation method 11: See Figures 5 to 9 This embodiment provides a specific example of the method for processing object features in a robot vision SLAM described in the first embodiment, and is also used to explain the second to eighth embodiments, specifically:

[0216] In this embodiment, the object plane point cloud map is constructed on the TUM public dataset and the self-built dataset respectively.

[0217] (1) Experiments were conducted on fr2_desk in the TUM dataset. The dataset contains the following items: monitor, cup, book, mouse, keyboard, bowl. The point cloud result comparison is shown in the figure below. Figure 5 shown. Figure 5The result of comparing the original image of the dataset with the object plane point cloud map constructed by this embodiment is that the color correspondence in the point cloud map is as follows: monitor-green, keyboard-red, book-purple, mouse-yellow, water cup-dark blue, bowl-cyan. The two main horizontal planes in the dataset are extracted, the desktop is dark green, and the ground is dark purple. By comparing the left and right images, it can be seen that the position of the object in the plane is correctly reconstructed, and the three axis lengths of the ellipsoid can correctly represent the three-dimensional size of the real object. Figure 6 These are the front and side views of the object plane point cloud map constructed in this implementation mode, which are used to verify the support surface constraints. It can be seen from the above figure that the objects in the figure have correctly established support surface relationships with the desktop, and the z-axis of the object is perpendicular to the desktop and forms a tangent relationship with the plane. After the system adds the support surface, the reconstruction speed and accuracy of the object are greatly improved. Figure 7 It is the result of merging the object and the dense point cloud. The calculated object and the dense point cloud of the object have a high degree of overlap.

[0218] (2) The dataset was recorded in a real environment and the object plane point cloud map was constructed. The results are as follows Figure 8 As shown. Figure 8 In the paper, the office desks were observed for a week, and a plane point cloud map of the objects was established. The results showed that the plane information constructed by the system in this paper was accurate, the position of the objects was accurate, and the posture of most objects was accurate. Fig. 9 (a) The top view can more intuitively show the accuracy of object reconstruction. The integrity of the ellipsoid surrounding the object’s point cloud can verify the reconstruction effect. The ellipsoid of the object is added to the dense point cloud to form Fig. 9 (b) can be obtained by Fig. 9 (b) It can be seen that the object reconstruction effect is better.

[0219] In summary, the present invention is verified on public data sets and self-made data sets, which proves the effectiveness and superiority of the method of the present invention.

[0220] The above further describes in detail the technical solution provided by the present invention in conjunction with the accompanying drawings in order to highlight the advantages and benefits, and is not intended to be a limitation of the present invention. Any modification, combination of implementation modes, improvement and equivalent substitution of the present invention within the spirit and scope of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for processing object features in robot vision SLAM, characterized in that: The method comprises: S1: Develop the representation of the dual quadratic surface of object features; S2: According to multiple target observation points generated by the same object under multiple viewing angles, the center point and average axis length of the ellipsoid are calculated, and a spatial ellipsoid is constructed according to the center point and average axis length of the ellipsoid; S3: Data association of the same object and features based on the constructed spatial ellipsoid; S4: Construct a projection model of object features based on the spatial ellipsoid projection; S5: Calculate the reprojection error of the ellipsoid; S6: establishing a plane map in the indoor space, obtaining a support plane of the object in the plane map, and adding a support plane constraint to the object; S7: Construct object factor graph based on support plane constraints; The step S1 comprises: The space dual quadratic surface of the object characteristics is: π Τ Qπ＝0 Among them, Q is the dual quadratic surface, π Τ is the transpose of the tangent plane of Q, and π is the tangent plane of Q; Ellipsoid Q in the Dual Quadric Surface * : Among them, s1, s2, and s3 correspond to the three semi-axis lengths of the ellipsoid on the x, y, and z axes respectively. The last element of the matrix -1 is used to define the global scale. The position and posture of the ellipsoid are expressed as Z: Among them, θ represents the three rotation angles of the ellipsoid, expressed as θ = [θ1θ2θ3] Τ Indicates that t represents the translation of the ellipsoid, and t = [t1t2 t3] Τ , represented by a 9-dimensional vector q: q=[θ1θ2θ3t1 t2 t3 s1 s2 s3] Τ Under the transformation of the ellipsoid posture Z, a new ellipsoid Q is obtained * ′: Q * ′=ZQ * Z Τ ; The step S2 comprises: The linear equations formed by all observation frames of the object are expressed in matrix form: The calculation of the three-dimensional center point of the object is converted into the problem of solving a linear equation system, and the singular value decomposition method is used to solve it. The calculated result is O′=[O1′O2′O3′O4′] Τ , then O=O′ / O4′, and the three-dimensional center point of the object is obtained; Calculate the projection of the two-dimensional detection frame to the camera coordinate system z=z c The average width w and average height h of the plane: Among them, x i ' ,2 is the horizontal coordinate of the lower right point of the detection box, x i ' ，1 is the horizontal coordinate of the upper left point of the detection box, y i ' ,2 is the vertical coordinate of the lower right point of the detection box, y i ' ，1 is the vertical coordinate of the upper left point of the detection box, w i is the detection box width, h i is the detection frame height; Taking the average of the width and height gives the radius of the sphere r = (w + h) / 2; The step S3 comprises: Construct a DQ cost matrix cost for each frame of the object, where D represents the detection box and Q represents the map ellipsoid. The cost matrix is: The cost matrix cost is solved according to the Hungarian algorithm to obtain the best matching relationship between each detection box and the map ellipsoid, so that the total cost is maximized, and the DQ is associated with the matching results; The step S4 comprises: Project the ellipsoid into the pixel coordinate system and solve the error e in the pixel coordinate system: e=f(C * ,C * ′)=f(C * ,PQ * ′P Τ ) Among them, C * is the observation ellipse, C * ′ is the projected ellipse, P is the forward projection of the ellipsoid through the camera, and f(·) is the error function for calculating the distance between the two ellipses; The constructed least squares error function F1(·) is: The step S5 comprises: Convert the distance error between two ellipses into the distance error between two two-dimensional normal distributions; Use Wasserstein distance as the distance between two normal distributions; The method of using Wasserstein distance as a distance between two normal distributions includes: Two two-dimensional normal distributions N1(μ1,Σ1) and N2(μ2,Σ2), where μ1 and μ2 are the mean vectors of the normal distributions, and Σ1 and Σ2 are the covariance matrices; Compute the Euclidean distance d between the mean vectors μ1 and μ2 of two distributions: d=||μ1-μ2|| Compute the square of the Wasserstein distance: Among them, Tr(·) represents the inverse operation of the matrix; Calculate the Wasserstein distance: The step S6 comprises: Among them, Rot x (Q * ) represents the direction vector of the x-axis of the ellipsoid, Rot y (Q * ) represents the direction vector of the y-axis of the ellipsoid, is the support plane normal vector.

2. A computer device, characterized in that: The method comprises a memory and a processor, wherein a computer program is stored in the memory, and when the processor runs the computer program stored in the memory, the processor executes the method for processing object features in a robot vision SLAM as claimed in claim 1.

3. A computer-readable storage medium, characterized in that: The computer-readable storage medium is used to store a computer program, and the computer program executes the object feature processing method in robot vision SLAM according to claim 1.

Images