Target pose estimation method based on consumer-level depth camera in real environment

Abstract

The target pose estimation method based on the consumer-level depth camera in the real environment is different from other pose estimation methods depending on a high-precision data set, and can be applied to a low-cost RGB-D sensor. The method is composed of two stages: in the first stage, a target point cloud is extracted from a scene by using a minimum bounding box obtained by a YOLO network, then a pure target point cloud is obtained by using an Euclidean clustering segmentation method, and an optimized surface point cloud is obtained by using MLS smoothing; in the second stage, ISS feature points are extracted from surface point cloud uniform sampling and model point cloud, 3DSC descriptors are selected as feature descriptors, then a heterogeneous spatial vector RANSAC algorithm is introduced for coarse alignment, and an ICP algorithm is used for optimizing fine alignment to achieve accurate and rapid pose estimation. According to the method, the problems of feature point loss, difficulty in data set production in a deep learning method, high computing power requirement and the like of a traditional method are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of target pose estimation and relates to a target pose estimation method based on a consumer-grade depth camera in a real environment. Background Art

[0002] In recent years, the acquisition and processing of 3D information has become a research hotspot, especially the in-depth understanding of the spatial position and posture of objects through 3D information, that is, the pose of objects, which plays an important role in many fields, such as augmented reality, autonomous driving, and robot visual servoing. In the past, obtaining accurate 3D information usually required the use of expensive professional equipment and complex sensors. However, with the emergence of more and more consumer-grade RGB-D cameras, new opportunities have been brought about for 6D pose estimation. RGB-D cameras can not only provide color image information, but also obtain depth information at the same time, which greatly enriches the input data. This depth perception capability allows the camera to better understand the surrounding environment, thereby improving the accuracy and reliability of pose estimation.

[0003] Traditionally, the task of estimating the 6D pose of an object is usually completed based on the relationship matching between the 3D model and the 3D scene. However, this method requires professional equipment to capture the obvious features of the target in the scene. In the research based on two-dimensional features, such as HOG, SIFT and some common edge detection operators, by using 2D features, object pose estimation can be performed on objects with rich textures. However, this method requires multi-view projection of the object. By finding matching points in multiple views, the correspondence between 2D pixels and 3D points is established. Then, calculating the position of the object in the two-dimensional image becomes a classic N-point perspective PnP (Perspective-n-Point) problem. However, these methods have poor performance under weak textures and cluttered backgrounds, so they are rarely used in practical applications. In order to solve the above problems, scholars have proposed descriptor-based methods, such as SI, 3DSC, FPFH, SHOT, RoPS, etc. These descriptors can accurately describe the features of point clouds, improve the accuracy of point cloud registration, have the advantages of noise resistance and rotation invariance, and are often used for the initial registration of point clouds. Another method based on point pair features (PPF) has been widely used in industry and can achieve high precision requirements. However, PPF-based methods have very high requirements on point cloud quality and are not suitable for consumer-grade RGB-D cameras.

[0004] With the powerful ability of deep learning in RGB image processing, some scholars have proposed an end-to-end framework Deep-6DPose, which jointly detects, segments and recovers the 6D pose of object instances from a single RGB image. However, this may limit the acquisition of depth information, resulting in the accuracy of pose estimation being inferior to that of RGB-D methods in some cases. Subsequently, methods based on RGB-D images and point clouds have evolved, such as DenseFusion and PointNet, but these methods require a large amount of labeled data for training to learn effective feature representations and patterns. However, it is often difficult and expensive to obtain large-scale high-quality 3D data. They require laser scanners or professional depth cameras, which limits the training and performance of the model, and due to the influence of factors such as lighting and position in the real environment, the accuracy of the model may be greatly affected, so it is not suitable for real environments. The main purpose of the present invention is to design and verify a method that can accurately and efficiently use consumer-grade depth cameras to estimate the pose of a target in a real environment. Summary of the invention

[0005] In order to solve the above technical problems, the purpose of the present invention is to provide a target pose estimation method based on a consumer-grade depth camera in a real environment.

[0006] A method for estimating target pose based on a consumer-grade depth camera in a real environment of the present invention is characterized by comprising:

[0007] Step 1: Train the YOLO detection network model of the target object;

[0008] Step 2: Use the RGB-D depth camera combined with the YOLO network to extract the point cloud within the target bounding box;

[0009] Step 3: According to the point cloud in the bounding box, set the distance threshold and use the Euclidean clustering method to segment the target complete surface point cloud;

[0010] Step 4: Use the MLS least squares method to optimize the quality of the surface point cloud and reduce fluctuations caused by factors such as lighting and distance;

[0011] Step 5: Perform uniform sampling feature point extraction on the optimized surface point cloud and perform ISS feature point extraction on the model point cloud;

[0012] Step 6: After selecting the feature points, calculate the 3DSC feature descriptors of the surface and model feature points respectively;

[0013] Step 7: Use the improved RANSAC algorithm that introduces the heterogeneous space vector to eliminate incorrect matches, and estimate the transformation matrix from the correct matches as the initial pose transformation matrix;

[0014] Step 8: Use the ICP algorithm to further optimize and refine the registration results.

[0015] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 1 is specifically as follows:

[0016] Step 1.1: Obtain RGB images of the four targets in the experiment and make labels;

[0017] Step 1.2: The data is divided into three parts according to a certain ratio: training set traindata, validation set validdata and test set testdata. The YOLO target detection network is trained. YOLO can recognize objects and obtain the minimum bounding box.

[0018] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 2 is specifically as follows:

[0019] Step 2.1: YOLO detection and RGB-D camera use TOF (Time-of-Flight) or structured light technology to obtain the depth information of each pixel, and use the minimum bounding box generated by YOLO to segment the pixel area of ​​interest to generate a point cloud of the target.

[0020] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 3 is specifically as follows:

[0021] Step 3.1: Create an empty cluster list C and a point cloud queue Q to be checked. Construct a KD-Tree representation for the input point cloud dataset P;

[0022] Step 3.2: Find a point p in space 10 , and use KD-Tree to find its nearest neighbor n points. Determine the distance to these points, and place the point p whose distance is less than the threshold r 11 ,p 12 ,p 13 ...add to queue Q;

[0023] Step 3.3: From the queue Q(p 10 ,p 11 ) to extract a point p 11 Repeat step 3.2 to find p 21 ,p 22 ,p 23 ...and put them all into queue Q;

[0024] Step 3.4: When there are no new points to add, the current search is completed, and the result Q is added to the cluster list C, and the queue Q is cleared;

[0025] Step 3.5: Select unprocessed points and repeat step 3.2 until all points are processed, and then output the clustering result C.

[0026] After classification by clustering, since target detection provides the smallest bounding box for the target, the point cloud with the largest number of points in the clustering result is identified as the target point cloud.

[0027] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 4 is specifically as follows:

[0028] Step 4.1: Calculate the local approximate hyperplane H. The local approximate hyperplane is expressed as:

[0029] H={x∈R 3 |n T x=n T q}

[0030] Where n is the normal vector of the hyperplane and q is the point p i Neighborhood points, H should minimize the following nonlinear energy function:

[0031] E MLS (q,n)=∑(n T -n T q) 2 θ(||p i -q||)

[0032] Where θ is a monotonically decreasing positive weight function, usually taken as:

[0033]

[0034] Where h is the global estimated sampling space distance, which can control the smoothness of the moving least squares surface and can be dynamically taken as Where r is the radius of the bounding sphere of the neighborhood of point K.

[0035] Step 4.2: Least squares fitting. Use the local quadratic surface g(u,v) to fit the local point cloud. Let q i Yes i Orthogonal projection onto H, (x i ,y i ) is its local two-dimensional coordinate, f i =||q i -p i || is its height on H, and g(u,v) minimizes the following error:

[0036] ∑(g(x i ,y i )-f i )2 θ(||p i -q||)

[0037] Step 4.3: Get the moving least squares surface corresponding to point p i The corresponding point in:

[0038] p * i =q i +g(0,0)·n

[0039] MLS is suitable for smoothing only the target surface point cloud. It should be noted that parameters need to be carefully selected for different objects. Improper parameter settings may over-smooth sharp features or introduce artifacts.

[0040] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 5 is specifically as follows:

[0041] Step 5.1: For each query point P in the point cloud data P i Establish a local coordinate system and set the search radius r frame ;

[0042] Step 5.2: Determine the neighboring points P of all query points i At radius r frame The adjacent points within are weighted and the weight w is set. ij :

[0043]

[0044] Step 5.3: Calculate all P i The covariance matrix of its neighboring points

[0045]

[0046] Step 5.4: Calculate the eigenvalues ​​of each covariance matrix The magnitude of the eigenvalues ​​is the length of the ellipsoid axis and is arranged in decreasing order;

[0047] Step 5.5: Set the thresholds ε1 and ε2. The feature with the smallest eigenvalue is the significant feature. Generally, the threshold is less than 1. If the following formula is met, it is extracted as the ISS feature point:

[0048]

[0049] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 6 is specifically as follows:

[0050] Step 6.1: First calculate the normals N of all key points j, and at a certain point p i is the center of the sphere, r 3DSC is the radius, with normal N j Establish a spherical support region for the North Pole direction, based on R 3DSC = {R0…R J} Divide the support area into J+1 concentric circles according to the logarithmic distance. Evenly divide L+1 and K+1 intervals vertically and horizontally to form J×K×L sub-areas. Let R0 be the minimum radius r min , R j is the maximum radius r max , the radius calculation formula is as follows:

[0051]

[0052] Step 6.2: Calculate the weighted value w(ρ) of all points in each sub-region by the following formula: j ):

[0053]

[0054] where V(j,k,l) ​​corresponds to the volume of the subregion, ρ j is the density of points in the sub-region.

[0055] Step 6.3: Finally, merge all w(ρ j ) values ​​form a feature vector This vector contains the shape context around the query point, i.e. Where k = {1, 2,…, n}, n = J×K×L.

[0056] This feature vector describes the shape feature information around the base point. It simplifies the relationship between the corresponding points between the source point cloud and the target point cloud to the greatest extent, and concentrates the context information of each point in the two point clouds to compare whether they are similar and get the most approximate arrangement.

[0057] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 7 is specifically as follows:

[0058] Step 7.1: Randomly select n (n>2) points from the source point cloud ISS feature points as sampling points {k p1 ,k p2 ,k p3 ,…,k pn};

[0059] Step 7.2: According to the 3DSC descriptor of the calculated point cloud, find the corresponding points {k q1 ,k q2 ,k q3 ,…,k qn};

[0060] Step 7.3: Construct two virtual polygons based on the sampling point pairs and calculate their relative heterogeneity vector δ. If δ is less than the threshold t poly , then proceed to the next step, otherwise return to step 7.1, where δ is:

[0061]

[0062] Step 7.4: Use n point pairs to estimate the assumed transformation matrix and apply it to the source point cloud, perform spatial nearest neighbor search in the source point cloud and the target point cloud, and then find internal points based on the Euclidean distance threshold. If the number of internal points is too small, return to step 7.1;

[0063] Step 7.5: When the number of internal points is sufficient, the transformation matrix is ​​recalculated using the internal points, and the coarse registration is completed to obtain the final rotation matrix R and translation matrix T.

[0064] In a target pose estimation method based on a consumer-grade depth camera in a real environment of the present invention, step 8 is specifically as follows:

[0065] Step 8.1: For the source point cloud X = {x1, x2, …, x Nx} and target point cloud P = {p i ,p2,…,p Np}, x i and p i Represents the point cloud coordinates, N x and N p represents the number of point clouds, R is the rotation matrix, T is the translation matrix, and the error function of the ICP algorithm is:

[0066]

[0067] Before registration, set the number of iterations, final error, and single iteration error thresholds. For each point in the source point cloud, find the corresponding point in the target point cloud with the smallest error function value and solve the corresponding spatial transformation matrix. The registration error of the current iteration is obtained based on the error function. The iteration terminates when the final registration error is less than the final error threshold, the single iteration error is less than the single iteration error threshold, or the number of iterations exceeds the threshold. Then calculate the final transformation matrix based on the relevant data.

[0068] The object pose estimation method based on a consumer-grade depth camera in a real environment of the present invention has at least the following beneficial effects:

[0069] (1) A preprocessing method combining YOLO and Euclidean clustering segmentation is proposed to effectively obtain the target surface point cloud, and the MLS algorithm is used to optimize it, providing a simple and effective method for obtaining the target surface point cloud.

[0070] (2) A new registration method is proposed to align the target surface point cloud with the object model. The method includes selecting ISS key points and 3DSC descriptors as feature descriptions, introducing the RANSAC algorithm of heterogeneous space vectors for coarse registration, and then using the ICP algorithm for fine registration to achieve accurate pose estimation.

[0071] (3) A new method for measuring the accuracy of posture estimation is proposed, and the effectiveness of the proposed method is verified through experiments on a high-precision translation and rotation platform.

[0072] (4) There is no need to use industrial-grade depth cameras or lasers. Instead, an ordinary consumer-grade Realsense D455 depth camera is needed to achieve efficient and accurate pose estimation of the target. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is a schematic diagram of a method for estimating target pose based on a consumer-grade depth camera in a real environment of the present invention;

[0074] Figure 2 It is a flow chart of the target pose estimation method based on consumer-grade depth cameras in real environment;

[0075] Figure 3 This is a schematic diagram of the minimum bounding box detected by YOLO;

[0076] Figure 4 It is a schematic diagram of RGB-D depth map;

[0077] Figure 5 It is a schematic diagram of the point cloud within the minimum bounding box;

[0078] Figure 6 This is a schematic diagram of the Euclidean clustering segmentation point cloud;

[0079] Figure 7 This is a schematic diagram of the MLS smoothing target surface point cloud effect (side view);

[0080] Figure 8 This is a schematic diagram of ISS feature point extraction;

[0081] Fig. 9 It is a schematic diagram of the pose transformation between the model point cloud and the surface point cloud;

[0082] Fig.10 is a schematic diagram of the real target, from left to right are models 1 to 4;

[0083] Fig.11 are the attributes of the four goals;

[0084] Fig.12It is the result of the number of model and surface feature points;

[0085] Fig.13 is the registration result of each method;

[0086] Fig.14 It is a schematic diagram of a device for measuring target posture change;

[0087] Fig.15 The target relative position diagram is obtained by using a chessboard calibration plate;

[0088] Fig.16 is the result of the alignment error of each axis and each rotation angle;

[0089] Fig.17 It is the result of the measurement error change when each axis is translated and rotated;

[0090] Fig.18 are the translation and rotation measurement error results of the four models along different axes;

[0091] Fig.19 is the time consumption of each step of our pose estimation method (Model 2);

[0092] Fig. 20 This is a comparison diagram of scene point cloud registration (model 1) without one-stage processing;

[0093] Fig.21 This is a comparison diagram of the scene point cloud registration (model 1) after one stage of processing;

[0094] Fig. 22 It is a schematic diagram of PPF registration failure (model 3);

[0095] Fig.23 This is a schematic diagram of successful registration of our method (Model 3);

[0096] Fig.24 It is the error comparison result of the registration method with translation and rotation on different axes. DETAILED DESCRIPTION

[0097] like Figure 1 The figure shows a schematic diagram of a method for estimating target pose based on a consumer-grade depth camera in a real environment of the present invention. The steps are as follows: Figure 2 The flow chart shows:

[0098] Step 1 is as follows:

[0099] Step 1.1: Obtain RGB images of the four targets in the experiment and make labels;

[0100] Step 1.2: The data is divided into three parts according to a certain ratio: training set traindata, validation set validdata and test set testdata. The YOLO target detection network is trained. YOLO can recognize objects and obtain the minimum bounding box, such as Figure 3 shown.

[0101] Step 2 is as follows:

[0102] Step 2.1: Figure 4 As shown in Figure 1, YOLO detection and RGB-D cameras use TOF (Time-of-Flight) or structured light technology to obtain the depth information of each pixel, such as Figure 5 As shown in Figure 1, the point cloud of the target is generated by segmenting the pixel area of ​​interest through the minimum bounding box generated by YOLO.

[0103] Step 3 is as follows:

[0104] Step 3.1: Create an empty cluster list C and a point cloud queue Q to be checked. Construct a KD-Tree representation for the input point cloud dataset P;

[0105] Step 3.2: Find a point p in space 10 , and use KD-Tree to find its nearest neighbor n points. Determine the distance to these points, and place the point p whose distance is less than the threshold r 11 ,p 12 ,p 13 ...add to queue Q;

[0106] Step 3.3: From the queue Q(p 10 ,p 11 ) to extract a point p 11 Repeat step 3.2 to find p 21 ,p 22 ,p 23 ...and put them all into queue Q;

[0107] Step 3.4: When there are no new points to add, the current search is completed, and the result Q is added to the cluster list C, and the queue Q is cleared;

[0108] Step 3.5: Select unprocessed points and repeat step 3.2 until all points are processed, and then output the clustering result C.

[0109] like Figure 6 As shown in Figure 1, after classification by clustering, since target detection provides the smallest bounding box for the target, the point cloud with the largest number of points in the clustering result is identified as the target point cloud.

[0110] Step 4 is as follows:

[0111] Step 4.1: Calculate the local approximate hyperplane H. The local approximate hyperplane is expressed as:

[0112] H = {x∈R 3 |n T x=n T q}

[0113] Where n is the normal vector of the hyperplane and q is the point p i Neighborhood points, H should minimize the following nonlinear energy function:

[0114] E MLS (q,n)=∑(n T -n T q) 2 θ(||p i -q||)

[0115] Where θ is a monotonically decreasing positive weight function, usually taken as:

[0116]

[0117] Where h is the global estimated sampling space distance, which can control the smoothness of the moving least squares surface and can be dynamically taken as Where r is the radius of the bounding sphere of the neighborhood of point K;

[0118] Step 4.2: Least squares fitting. Use the local quadratic surface g(u,v) to fit the local point cloud. Let q i Yes i Orthogonal projection onto H, (x i ,y i ) is its local two-dimensional coordinate, f i =||q i -p i || is its height on H, and g(u,v) minimizes the following error:

[0119] ∑(g(x i ,y i )-f i ) 2 θ(||p i -q||)

[0120] Step 4.3: Get the moving least squares surface corresponding to point p i The corresponding point in:

[0121] p * i =q i +g(0,0)·n

[0122] like Figure 7 As shown in the figure, MLS can reduce fluctuations. MLS is suitable for smoothing only the target surface point cloud. It should be noted that parameters need to be carefully selected for different objects. Improper parameter settings may over-smooth sharp features or introduce artifacts.

[0123] Step 5 is as follows:

[0124] Step 5.1: For each query point P in the point cloud data P i Establish a local coordinate system and set the search radius r frame .

[0125] Step 5.2: Determine the neighboring points P of all query points i At radius r frame The adjacent points within are weighted and the weight w is set. ij :

[0126]

[0127] Step 5.3: Calculate all P i The covariance matrix of its neighboring points

[0128]

[0129] Step 5.4: Calculate the eigenvalues ​​of each covariance matrix The magnitudes of the eigenvalues ​​are the lengths of the ellipsoid axes and are listed in decreasing order.

[0130] Step 5.5: Set the thresholds ε1 and ε2. The feature with the smallest eigenvalue is the significant feature. Generally, the threshold is less than 1. If the following formula is met, it is extracted as the ISS feature point:

[0131]

[0132] like Figure 8 As shown, ISS feature points are extracted from the model.

[0133] Step 6 is as follows:

[0134] Step 6.1: First calculate the normals N of all key points j , and at a certain point p i is the center of the sphere, r 3DSC is the radius, with normal N j Establish a spherical support region for the North Pole direction, based on R 3DSC = {R0…R J} Divide the support area into J+1 concentric circles according to the logarithmic distance. Evenly divide L+1 and K+1 intervals vertically and horizontally to form J×K×L sub-areas. Let R0 be the minimum radius r min , R jis the maximum radius r max , the radius calculation formula is as follows:

[0135]

[0136] Step 6.2: Calculate the weighted value w(ρ) of all points in each sub-region by the following formula: j ):

[0137]

[0138] where V(j,k,l) ​​corresponds to the volume of the subregion, ρ j is the density of points in the sub-region;

[0139] Step 6.3: Finally, merge all w(ρ j ) values ​​form a feature vector This vector contains the shape context around the query point, i.e. Where k = {1, 2,…, n}, n = J×K×L.

[0140] This feature vector describes the shape feature information around the base point. It simplifies the relationship between the corresponding points between the source point cloud and the target point cloud to the greatest extent, and concentrates the context information of each point in the two point clouds to compare whether they are similar and get the most approximate arrangement.

[0141] Step 7 is as follows:

[0142] Step 7.1: Randomly select n (n>2) points from the source point cloud ISS feature points as sampling points {k p1 ,k p2 ,k p3 ,…,k pn};

[0143] Step 7.2: According to the 3DSC descriptor of the calculated point cloud, find the corresponding points {k q1 ,k q2 ,k q3 ,…,k qn};

[0144] Step 7.3: Construct two virtual polygons based on the sampling point pairs and calculate their relative heterogeneity vector δ. If δ is less than the threshold t poly , then proceed to the next step, otherwise return to step 7.1, where δ is:

[0145]

[0146] Step 7.4: Use n point pairs to estimate the assumed transformation matrix and apply it to the source point cloud, perform spatial nearest neighbor search in the source point cloud and the target point cloud, and then find internal points based on the Euclidean distance threshold. If the number of internal points is too small, return to step 7.1;

[0147] Step 7.5: When the number of internal points is sufficient, the transformation matrix is ​​recalculated using the internal points, and the coarse registration is completed to obtain the final rotation matrix R and translation matrix T.

[0148] Step 8 is as follows:

[0149] Step 8.1: For the source point cloud X = {x1, x2, …, x Nx} and target point cloud P = {p i ,p2,…,p Np}, x i and p i Represents the point cloud coordinates, N x and N p represents the number of point clouds, R is the rotation matrix, T is the translation matrix, and the error function of the ICP algorithm is:

[0150]

[0151] Before registration, set the number of iterations, final error, and single iteration error thresholds. For each point in the source point cloud, find the corresponding point in the target point cloud with the smallest error function value, and solve the corresponding spatial transformation matrix. The registration error of the current iteration is obtained based on the error function. When the final registration error is less than the final error threshold, the single iteration error is less than the single iteration error threshold, or the number of iterations exceeds the threshold, the iteration terminates. Then calculate the final transformation matrix based on the relevant data. Fig. 9 The following figure shows the effect of aligning the two through the transformation matrix.

[0152] The present invention is further described below with reference to examples.

[0153] Embodiment 1 (experimental example):

[0154] In order to demonstrate the effectiveness of the target pose estimation method based on consumer-grade depth cameras in real environments, we designed two different experiments: “point cloud registration experiment” and “real object pose measurement experiment”.

[0155] In the point cloud registration experiment, we will demonstrate the effectiveness of the new registration method we proposed. We focus on aligning the point cloud surface obtained after the first stage of processing with our model. The four high-precision models used in the experiment are generated from real objects by professional laser scanners. The actual sizes of these models are as follows: Fig.10 Its basic properties are as follows Fig.11 shown.

[0156] In the experiment, we captured the surface point cloud of the model at an appropriate distance from the camera and processed the point cloud using the first stage method. After downsampling, we performed uniform sampling to obtain an appropriate number of feature points to align with the ISS feature points of our model. The point cloud data used for registration is as follows: Fig.12 shown.

[0157] In point cloud registration, we mainly use the root mean square error RMSE as an indicator of accuracy. The RMSE calculation formula is as follows:

[0158]

[0159] Among them, n represents the number of corresponding points, represents the true Euclidean distance between corresponding points, X i Represents the Euclidean distance between corresponding points after registration. The smaller the RMSE value, the more accurate the obtained pose value. In addition, we recorded the number of corresponding points and running time of each experiment as a supplementary reference. In the comparison, we used the 4PCS algorithm based on point correspondence and the SAC-IA-ICP algorithm based on the FHPF feature descriptor, and also combined other algorithms using different descriptors and coarse registration methods. Fig.13 The experimental results of various methods are summarized. Fig.13 It can be concluded that our method effectively captures the spatial features between point clouds by combining the ISS-3DSC feature descriptor with the improved RANSAC method, so that more effective correspondences can be identified. Specifically, our method has the highest average accuracy. For model 3, our method found 360 corresponding points with an RMSE value of 0.0109857m, which is much more accurate than the registration results of other methods, especially the RMSE indicator has been significantly improved. In addition, with the help of the fast initial registration achieved by the improved RANSAC algorithm, our method shows an advantage in computing time. The fastest time recorded on model 4 is only 0.217895 seconds, while the slowest time recorded on model 1 is still only 0.77 seconds.

[0160] In the real object posture measurement experiment, the experimental device is as follows Fig.14As shown in the figure, the device uses a high-precision translation-rotation platform to control the position and posture changes of the object. The device can move horizontally along the X-axis and Z-axis, and rotate around three axes, where the Z-axis is represented by blue, the X-axis is represented by red, and the Y-axis is represented by green. We define Alpha as the angle of rotation around the X-axis, Beta as the angle of rotation around the Y-axis, and Gamma as the angle of rotation around the Z-axis. The object is fixed on the translation-rotation platform, and by adjusting the position and posture transformation of the platform, we can accurately control the position and posture of the object. But before that, we must ensure that the coordinate system of the translation-rotation platform is aligned with the coordinate system of the camera. To do this, we first apply a checkerboard pattern on the surface of the object for calibration to confirm the initial posture of the object. Suppose we move the object along an axis by X (or rotate it around an axis by X), and then measure it again using the checkerboard to obtain the new posture, such as Fig.15 If the true values ​​of the two pose transformations are equal to the manually set changes, it can be confirmed that the coordinate systems of the translation-rotation platform and the camera are aligned.

[0161] According to the comparison between observed data and real data, we use mean absolute error MAE and mean square error MSE as indicators. The calculation formulas of the two are as follows:

[0162]

[0163]

[0164] Where m represents the total number of measurements, y i represents the true value, Indicates the calibration value, the final data result is as follows Fig.16 As shown. In terms of displacement, the largest average error is in the Z axis, which is 0.0524cm, while the average error in the X axis is only 0.0048cm. In terms of angle, the maximum average error of the Alpha angle is 0.0429 degrees, and the mean square error is around 0.01. As can be seen from the figure, the method of calibration using the checkerboard pattern effectively aligns our camera coordinate system with the checkerboard coordinate system, thereby verifying the effectiveness of our consumer-grade depth camera for measuring the pose of the target object.

[0165] Next, we use the platform to measure the accuracy of our method. Since the transformation matrix obtained by registration represents the transformation from the model to the target, and the initial pose of the model is obtained by laser scanning, its practical significance is difficult to determine, so it is challenging to directly quantify the actual pose. We analyze the results by evaluating the relative pose error between the two registration results. Therefore, we translate the object along the X and Z axes of the camera coordinate system or rotate it around any axis of the camera coordinate system, calculate the translation distance and Euler angle using the method in this paper, and then compare the calculated results with the actual translation distance and rotation angle of the translation and rotation platform. The absolute error between the two sets of values ​​is used as the evaluation criterion. The error change of the translation measurement experiment is shown in the following figure. Fig.17 (a) (b) shows that the error variation of the rotation measurement experiment is as follows Fig.17 (c)(d)(e) show the measurement results of our method on different axes and rotation angles. Fig.18 shown.

[0166] Translation measurement: The Z axis represents the distance between the camera and the target, and as this distance decreases, the sampled point cloud becomes more detailed. However, if the object is too close, larger objects may exceed the measurable range of the depth camera, resulting in incomplete point cloud data. Therefore, the first three models can only produce a complete point cloud at a distance of about 40cm from the camera, while the smaller model 4 can still capture valid point cloud data at 30cm. In contrast, the error change along the X axis is relatively stable because the X axis is in the projection plane of the camera, and when the object moves along this axis, the point cloud changes little, resulting in more consistent errors. The average errors of the four models along the X and Z axes are 0.2123cm and 0.35225cm, respectively. In general, objects with richer features are easier to align, so their errors are relatively low.

[0167] Rotation angle measurement: In rotation angle measurement, the rotation around the Alpha angle and the Beta angle will significantly change the point cloud obtained in the two measurements, resulting in increased errors. The greater the angle deviation, the greater the error. The average errors are 0.6835 degrees and 0.763925 degrees, respectively. Similar to translation measurement, the rotation around the Gamma angle has less effect on the point cloud on the surface of the object, and the error is more stable, with an average error of only 0.287725 degrees. It is worth noting that Model 2 and Model 3, which have more distinct features, have smaller errors during the Gamma angle rotation process. Therefore, the quality of the point cloud is a key factor affecting the accuracy of point cloud registration.

[0168] In addition, we also recorded the time spent on each step of the registration process. Taking Model 2 as an example, the total time of all steps is as follows Fig.19 As shown, the accurate pose of the object can be obtained in only about 2.1 seconds.

[0169] Embodiment 2 (comparative example):

[0170] To demonstrate the necessity of the one-stage processing to obtain the target surface point cloud in our method, we directly used the scene point cloud and the model point cloud for registration.

[0171] in Fig. 20 It shows the registration effect in the whole scene. Fig.21 The registration effect of our one-stage method is shown. Practice has shown that it is impractical to directly use the entire scene point cloud for registration. First, the scene point cloud contains a large number of points, which poses a great challenge to the calculation. Second, the target point cloud features obtained in this way are not very obvious, and it is difficult to find effective matching point pairs. As shown in the figure, the registration fails using the unprocessed scene point cloud. However, after the first stage of processing, effective registration can be achieved.

[0172] In addition, we initially used the classic PPF algorithm for comparison, but we found that when the number of feature points is large, the PPF method requires too much computation time and performs poorly on our data. Fig. 22 As shown in , the point cloud exhibits obvious misalignment. This is because the PPF algorithm is designed for high-precision point clouds, while the point cloud quality obtained from our depth camera is relatively low, as shown in Fig.23 As shown, our method can successfully align.

[0173] In order to compare the effectiveness of our two-stage registration method, we conducted experiments on the four models with different registration methods and obtained the average errors of the four methods in the four models. The results are shown in Figure 2. Fig.24 As shown in the figure. From the results, it can be seen that the method with better registration performance also has smaller measurement errors in the actual environment. Among them, the 4PCS method is more suitable for planar registration, resulting in larger measurement errors of the Alpha and Beta angles, but still maintains good performance in the Gamma angle measurement. Based on the previous registration results and the data in the figure, it can be obtained that the method proposed by us has achieved the smallest average error among the feature-based registration methods, which proves that our method is better.

Claims

1. A method for estimating target pose based on a consumer-grade depth camera in a real environment, characterized in that: include: Step 1: Train the YOLO detection network model of the target object; Step 2: Use the RGB-D depth camera combined with the YOLO network to extract the point cloud within the target bounding box; Step 3: According to the point cloud within the bounding box, set the distance threshold and use the Euclidean clustering method to segment the target complete surface point cloud; Step 4: Use the MLS least squares method to optimize the quality of the surface point cloud and reduce fluctuations caused by factors such as lighting and distance; Step 5: Perform uniform sampling feature point extraction on the optimized surface point cloud and perform ISS feature point extraction on the model point cloud; Step 6: After selecting the feature points, calculate the 3DSC feature descriptors of the surface and model feature points respectively; Step 7: Use the improved RANSAC algorithm that introduces the heterogeneous space vector to eliminate incorrect matches, and estimate the transformation matrix from the correct matches as the initial pose transformation matrix; Step 8: Use the ICP algorithm to further optimize and refine the registration results.

2. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 1 is specifically as follows: Step 1.1: Obtain RGB images of the four targets in the experiment and make labels; Step 1.2: The data is divided into three parts according to a certain ratio: training set traindata, validation set validdata and test set testdata. The YOLO target detection network is trained. YOLO can recognize objects and obtain the minimum bounding box.

3. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 2 is specifically as follows: Step 2.1: YOLO detection and RGB-D camera use TOF (Time-of-Flight) or structured light technology to obtain the depth information of each pixel, and use the minimum bounding box generated by YOLO to segment the pixel area of ​​interest to generate a point cloud of the target.

4. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 3 is specifically as follows: Step 3.1: Create an empty cluster list C and a point cloud queue Q to be checked. Construct a KD-Tree representation for the input point cloud dataset P; Step 3.2: Find a point p in space 10 , and use KD-Tree to find its nearest neighbor n points. Determine the distance to these points, and place the point p whose distance is less than the threshold r 11 ,p 12 ,p 13 ...add to queue Q; Step 3.3: From the queue Q(p 10 ,p 11 ) to extract a point p 11 Repeat step 3.2 to find p 21 ,p 22 ,p 23 ...and put them all into queue Q; Step 3.4: When there are no new points to add, the current search is completed, and the result Q is added to the cluster list C, and the queue Q is cleared; Step 3.5: Select unprocessed points and repeat step 3.2 until all points are processed, and then output the clustering result C. After classification by clustering, since target detection provides the smallest bounding box for the target, the point cloud with the largest number of points in the clustering result is identified as the target point cloud.

5. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 4 is specifically as follows: Step 4.1: Calculate the local approximate hyperplane H. The local approximate hyperplane is expressed as: H={x∈R 3 |n T x=n T q} Where n is the normal vector of the hyperplane and q is the point p i Neighborhood points, H should minimize the following nonlinear energy function: E MLS (q,n)=∑(n T -n T q) 2 θ(||p i -q||) Where θ is a monotonically decreasing positive weight function, usually taken as: Where h is the global estimated sampling space distance, which can control the smoothness of the moving least squares surface and can be dynamically taken as Where r is the bounding sphere radius of the neighborhood of point K; Step 4.2: Least squares fitting. Use the local quadratic surface g(u,v) to fit the local point cloud. Let q i Yes i Orthogonal projection onto H, (x i ,y i ) is its local two-dimensional coordinate, f i =||q i -p i || is its height on H, and g(u,v) minimizes the following error: ∑(g(x i ,y i )-f i ) 2 θ(||p i -q||) Step 4.3: Get the moving least squares surface corresponding to point p i The corresponding point in: p * i =q i +g(0,0)·n MLS is suitable for smoothing only the target surface point cloud. It should be noted that parameters need to be carefully selected for different objects. Improper parameter settings may over-smooth sharp features or introduce artifacts.

6. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 5 is specifically as follows: Step 5.1: For each query point P in the point cloud data P i Establish a local coordinate system and set the search radius r frame ; Step 5.2: Determine the neighboring points P of all query points i At radius r frame The adjacent points within are weighted and the weight w is set. ij : Step 5.3: Calculate all P i The covariance matrix of its neighboring points Step 5.4: Calculate the eigenvalues ​​of each covariance matrix The magnitude of the eigenvalues ​​is the length of the ellipsoid axis and is arranged in decreasing order; Step 5.5: Set the thresholds ε1 and ε2. The feature with the smallest eigenvalue is the significant feature. Generally, the threshold is less than 1. If the following formula is met, it is extracted as the ISS feature point:

7. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 6 is specifically as follows: Step 6.1: First calculate the normals N of all key points j , and at a certain point p i is the center of the sphere, r 3DSC is the radius, with normal N j Establish a spherical support region for the North Pole direction, based on R 3DSC = {R0…R J } Divide the support area into J+1 concentric circles according to the logarithmic distance. Evenly divide L+1 and K+1 intervals vertically and horizontally to form J×K×L sub-areas. Let R0 be the minimum radius r min , R j is the maximum radius r max , the radius calculation formula is as follows: Step 6.2: Calculate the weighted value w(ρ) of all points in each sub-region by the following formula: j ): where V(j,k,l) ​​corresponds to the volume of the subregion, ρ j is the density of points in the sub-region; Step 6.3: Finally, merge all w(ρ j ) values ​​form a feature vector This vector contains the shape context around the query point, i.e. Where k = {1, 2,…, n}, n = J×K×L. This feature vector describes the shape feature information around the base point. It simplifies the relationship between the corresponding points between the source point cloud and the target point cloud to the greatest extent, and concentrates the context information of each point in the two point clouds to compare whether they are similar and get the most approximate arrangement.

8. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 7 is specifically as follows: Step 7.1: Randomly select n (n>2) points from the source point cloud ISS feature points as sampling points {k p1 ,k p2 ,k p3 ,…,k pn }; Step 7.2: According to the 3DSC descriptor of the calculated point cloud, find the corresponding points {k q1 ,k q2 ,k q3 ,…,k qn }; Step 7.3: Construct two virtual polygons based on the sampling point pairs and calculate their relative heterogeneity vector δ. If δ is less than the threshold t poly , then proceed to the next step, otherwise return to step 7.1, where δ is: Step 7.4: Use n point pairs to estimate the assumed transformation matrix and apply it to the source point cloud, perform spatial nearest neighbor search in the source point cloud and the target point cloud, and then find internal points based on the Euclidean distance threshold. If the number of internal points is too small, return to step 7.1; Step 7.5: When the number of internal points is sufficient, the transformation matrix is ​​recalculated using the internal points, and the coarse registration is completed to obtain the final rotation matrix R and translation matrix T.

9. The method for estimating target pose based on a consumer-grade depth camera in a real environment as claimed in claim 1, characterized in that: The step 8 is specifically as follows: Step 8.1: For the source point cloud X = {x1, x2, …, x Nx } and target point cloud P = {p i ,p2,…,p Np }, x i and p i Represents the point cloud coordinates, N x and N p represents the number of point clouds, R is the rotation matrix, T is the translation matrix, and the error function of the ICP algorithm is: Before registration, set the number of iterations, final error, and single iteration error thresholds. For each point in the source point cloud, find the corresponding point in the target point cloud with the smallest error function value and solve the corresponding spatial transformation matrix. The registration error of the current iteration is obtained based on the error function. The iteration terminates when the final registration error is less than the final error threshold, the single iteration error is less than the single iteration error threshold, or the number of iterations exceeds the threshold. Then calculate the final transformation matrix based on the relevant data.

Images