A shape-constrained joint vector screening rigid object 6D pose estimation method

Abstract

The application discloses a rigid object 6D pose estimation method of shape constraint combined vector screening, which can effectively improve 6D pose estimation precision. The application defines a kind of shape for describing object in image. The application uses the geometric relationship between different 2D key points to assist 2D key point positioning. At the same time, when calculating the assumed key point by selecting any two vectors, the prior art does not consider the rationality of the used vector, resulting in a large deviation of the generated assumed key point position. The application proposes a vector screening strategy to prevent unreasonable vectors from calculating the assumed key point, thereby reducing the position deviation of the assumed key point and ultimately improving the performance of pose estimation.

Description

Technical Field

[0001] This invention belongs to the field of computer vision, and its goal is to generate more accurate 6D poses of rigid objects for robot grasping tasks and augmented reality tasks. Background Technology

[0002] In recent years, with the widespread application of robotics, the task of 6D pose estimation for rigid objects based on RGB images has received increasing attention. The goal of this task is to calculate the rotation and translation transformation matrix from the world coordinate system of the rigid object to the camera coordinate system, i.e., the 6D pose of the rigid object.

[0003] Currently, deep learning-based 6D pose estimation methods for rigid objects have shown superior performance. These methods require the annotation of 3D key points of the model and 2D key points on the model's projected image. Figure 1 This illustrates a set of 3D key points labeled for a specific model. The labeling method involves first sampling eight 3D key points {M} from the surface of the object's 3D model using a sampling algorithm. j |j=1,…,8}( Figure 1 (Black dots in the middle); then, for each 3D keypoint, mark its accurate projection point on the image, i.e., the true 2D keypoint m. j Simultaneously label the center point O of the 3D model. Figure 1 The black triangle in the image and its accurate projection point on the image, i.e., the center point o of the 2D object.

[0004] A deep learning-based 6D pose estimation method learns a deep network to predict eight 3D keypoints M. j The projection points on the image, i.e., the prediction of 8 2D keypoints m′ j Based on the one-to-one correspondence between the model's 3D keypoints and the predicted 2D keypoints, the 6D pose (R,t) is calculated using the Perspective-n-Point (PnP) algorithm, where R is a 3×3 rotation matrix representing the rotation transformation from the world coordinate system to the camera coordinate system, and t is a 3×1 translation vector representing the translation transformation from the world coordinate system to the camera coordinate system. In this type of method, the model's 3D keypoints M... j With the predicted 2D keypoint m′ j The one-to-one correspondence between 2D keypoints is the intermediate representation for 6D pose estimation; therefore, the accuracy of the predicted 2D keypoints directly affects the performance of pose estimation. If the target object is occluded by other objects, causing some 2D keypoints to be invisible, the performance of this type of method will significantly decrease.

[0005] To improve the performance of 6D pose estimation for rigid objects based on RGB images in occluded scenes, researchers propose a voting-based 6D pose estimation method. This method performs semantic segmentation on the input image to obtain the target object region, and uses each ground truth 2D keypoint m from the input image as a basis for further analysis. j Predict the unit vector field; locate the target object pixels based on the semantic segmentation results, arbitrarily select the predicted vectors of two pixels, calculate their intersection, and use the intersection as the hypothetical keypoint; vote on the hypothetical keypoints to obtain the confidence score of each hypothetical keypoint, and use the hypothetical keypoint with the highest confidence score as the predicted 2D keypoint m′. j ; Based on the predicted 2D keypoint set {m′ j} and 3D keypoint set {M j The PnP algorithm is used to calculate the 6D pose of an object. Voting-based 6D pose estimation methods have the following two problems:

[0006] 1. Predicted 2D keypoint m′ j The accuracy is evaluated only by measuring the error between the predicted value and the true value. That is, the eight 2D key points are predicted independently. The accuracy of the 2D key points predicted based on the existing error constraint is not ideal, which affects the pose estimation performance.

[0007] 2. In calculating the assumed key points, any two vectors selected may be approximately parallel or coincident, such as... Figure 2 As shown, the hypothetical keypoint obtained by the intersection of two approximately parallel or coincident vectors deviates significantly from the actual keypoint, thus affecting the prediction of the 2D keypoint m′ based on the hypothetical keypoint. j Distance from the real key point m j The positional deviation is large, which ultimately affects the pose estimation performance. Summary of the Invention

[0008] This invention proposes a 6D pose estimation method for rigid objects using shape constraint joint vector filtering, which can effectively improve the accuracy of 6D pose estimation. This invention defines a descriptor SD = {om} for describing the shape of objects in an image. j |1≤j≤8}∪{m j m j+1 |1≤j<8}∪{m8m1}, where om j This indicates connecting the center point o of a 2D object to the 2D key point m. j The line segment, m j m j+1 Indicates the connection of 2D key points m j With 2D key points m j+1 The line segment, j is the index of the key point.

[0009] The present invention proposes a 6D pose estimation method for rigid objects using shape constraint joint vector filtering, the specific process of which is as follows:

[0010] S1, unit vector field prediction and pixel semantic prediction;

[0011] Based on deep networks, predict unit vector fields and pixel semantics. The input of the deep network is an RGB image I∈R. H×W×3 The output is the semantic segment S∈R H×W×2 Sum of real 2D key points {m j The predicted unit vector field {V(m |j=1,...,8} is... j )∈R H ×W×2 |j=1,...,8}. Where R represents the feature space, H is the image height, and W is the image width.

[0012] The ResNet residual network is chosen as the backbone network, consisting of Conv+BN+ReLU layers, max pooling layers, strided convolutional layers, dilated convolutional layers, bilinear upsampling layers, and skip connection residual blocks at different levels. First, convolution and pooling operations are performed on the input H×W×3 RGB image; then, skip connections and upsampling are performed on the feature map until the feature map size reaches H×W; finally, a 1×1 convolution kernel is applied to the H×W feature map to increase its dimensionality, outputting the predicted unit vector field {V(m j )} and the predicted semantic segmentation S, where S[p w ,p h [,1] indicates that the index in image I is (p w ,p h The semantic category of pixel p is the probability of specifying an object, denoted as a′. p (p w ,p h ), where p w Let p be the x-coordinate of pixel p in the image. h Let p be the ordinate of pixel p on the image.

[0013] Based on the predicted semantic segmentation S, the set of pixels E = {p|a′} within the target object region is obtained. p (p w ,p h (p > 0.5) w ,p h ) represents the pixel coordinates of pixel p in image I. Based on the predicted unit vector field V(m) j ), which gives the index (p) in image I. w ,p h The pixel p points to the 2D keypoint m. j The unit vector V(m) j)[p w ,p h ,:] is the prediction unit vector v of pixel p. j (p).

[0014] S2. Calculation of hypothetical key points based on vector filtering;

[0015] The specific steps for calculating the hypothetical keypoint set of 2D keypoint m1 are as follows:

[0016] S2.1 Let r be the number of pixels within the target object region, then r = |E|. Based on the unit vector field V(m1) predicted for the 2D keypoint m1, the angle between each pair of predicted vectors v1(p), p∈E and v1(q), q∈E within the target object region is calculated using the inverse cosine function:

[0017]

[0018] Where p and q are the pixels within the target object region, p r ,q r Let p and q be the indices of pixel p and q in set E, respectively, and 0 ≤ p r ≤(r-1),0≤q r ≤(r-1).

[0019] S2.2 Calculate the identifier matrix A′∈R based on the set threshold η. r×r element The value of . 0≤q r ≤(r-1), r=|E|, if So otherwise In the experiment, the threshold η was set to π / 60.

[0020] S2.3 Randomly select elements from the identifier matrix if If the value is 0, the next element is randomly selected from the identifier matrix; if If the value is 1, then calculate the intersection point of the lines containing the two vectors v1(p) and v1(q), and use the intersection point as the hypothetical key point. Set the value to 0, and repeat step 2.3 to obtain N hypothetical keypoints for 2D keypoint m1. in Let m1 be the i-th hypothetical keypoint of 2D keypoint m1, where i is the index of the hypothetical keypoint.

[0021] S2.4 for the remaining 2D keypoints m j (2≤j≤8), based on the corresponding unit vector field V(m) jRepeat steps 2.1-2.3 above to obtain the corresponding set of hypothetical key points.

[0022] S3. Calculate the confidence score for voting on the key hypotheses;

[0023] For the i-th hypothetical keypoint of 2D keypoint m1 The voting process is as follows: Steps 3.1-3.2

[0024] S3.1 Using equation (1), calculate the direction of each pixel p in the target object region to the i-th hypothetical key point. unit vector

[0025]

[0026] S3.2 Using equation (2) for each hypothetical key point The confidence score is obtained from the vote:

[0027]

[0028] Where Γ is the indicator function and the threshold is... In the experiment, it was set to 0.99. A higher value indicates that more prediction unit vectors point to the hypothetical key points.

[0029] S3.3 For the remaining 2D keypoints m j Key point of the assumption (2≤j≤8) Repeat steps 3.1-3.2 above to calculate the confidence score.

[0030] S3.4 for 2D keypoints m j In this context, the keypoint with the highest confidence score is the predicted 2D keypoint m′. j , 1≤j≤8, 0≤i≤N-1.

[0031] S4. Calculate the network loss;

[0032] The total network loss is L = L g +L v +λL s λ is an empirical parameter, set to 0.1 in the experiment. g The semantic segmentation loss defined by equation (3):

[0033] L g =-a p loga′ p -(1-a p log(1-a′) p(3)

[0034] Where, a′ p a represents the probability that pixel p is predicted by the network to be a pixel of the target object. p This is a semantic truth value. L v The unit vector field prediction loss defined in equation (4) is:

[0035]

[0036] Where j is the keypoint index, and p is a pixel in the set E of pixels in the target object region. and These are the prediction unit vectors v j (p) Components along the horizontal and vertical directions of the image and They are the real unit vectors u j (p) Components along the horizontal and vertical directions of the image for Function. L s The shape constraint loss defined by equation (5):

[0037]

[0038] Among them, w j =[d j ,g j ,g j+1 ] is a feature vector defined based on SD, d j It is line segment m j m j+1 The length of the line segment is m8m1 when j=8; g j It is a line segment om j The length of w' is given by j=8, where w8=[d8,g8,g1]; w' j =[d′ j ,g′ j ,g′ j+1 ] is based on SD′={om′ j |1≤j≤8}∪{m′ j m′ j+1 The eigenvector defined by |1≤j<8}∪{m′8m′1}, d′ j It is line segment m′ j m′ j+1 The length of the line segment is m′8m′1 when j=8; g′ j It is line segment om′ j The length of the length, when j=8, w′8=[d′8,g′8,g′1].

[0039] S5. Calculate the 6D pose;

[0040] For 2D key points m j (1≤j≤8), based on the corresponding set of hypothetical key points and confidence scores of key hypotheses remember in, Is it a key assumption? The pixel x-coordinate, Is it a key assumption? The pixel ordinate. The mean vector μ is calculated using equations (6) and (7) respectively. j The covariance matrix Σ j :

[0041]

[0042]

[0043] Based on the 3D key points M of the model j (1≤j≤8) and predicted 2D keypoint m′ j (1≤j≤8), first calculate the 3D keypoint M in the world coordinate system. j The problem is transformed into a 3D-3D pose determination problem by using coordinates in the camera coordinate system; then, the ICP algorithm is used to initialize (R,t); finally, nonlinear optimization is used to solve for the optimal (R,t). The specific calculation method is as follows:

[0044] S5-1 Calculate 3D Key Points M j coordinates in the camera coordinate system

[0045] For 3D key points M j Randomly select 4 non-coplanar control points c jn (1≤n≤4), where j is the keypoint index and n is the control point index. Control point c jn The coordinates in the world coordinate system are: Formula (8) yields a set of weighting coefficients α. j1 ,α j2 ,α j3 ,α j4 :

[0046]

[0047] Among them, (x j ,y j ,z j ) is the 3D key point M j Coordinates in the world coordinate system.

[0048] Control point c jn The coordinates in the camera coordinate system are: Then, in the camera coordinate system, control point c jn With 3D key points M j The corresponding predicted 2D keypoint m′ j Satisfying equation (9):

[0049]

[0050] Among them, f u ,f v ,u c ,v c For camera intrinsic parameters, m j ′(x) is the prediction of 2D keypoint m′ j The pixel x-coordinate, m j ′(y) is the prediction of 2D keypoint m′ j The pixel ordinate, α j1 ,α j2 ,α j3 ,α j4 The weight coefficients are calculated using equation (8). Solving the system of equations corresponding to equation (9) yields the control point c. jn coordinates in the camera coordinate system

[0051] 3D Key Points M j The coordinates in the camera coordinate system are (x′) j ,y′ j ,z′ j If the control points and 3D key points in the camera coordinate system satisfy equation (10):

[0052]

[0053] The weighting coefficient α obtained by using equation (8) j1 ,α j2 ,α j3 ,α j4 The control point c calculated using equation (9) jn coordinates in the camera coordinate system Substituting into equation (10), the 3D key points M can be obtained. j Coordinates (x′) in the camera coordinate system j ,y′ j ,z′ j ).

[0054] S5-2 initializes (R,t) using the ICP algorithm.

[0055] KP j =[x j ,y j ,z j ] T, where (x j ,y j ,z j ) is the 3D key point M j Coordinates in the world coordinate system; denoted as KP′ j =[x′ j ,y′ j ,z′ j ] T , where (x′ j ,y′ j ,z′ j ) is the 3D key point M j The coordinates in the camera coordinate system are used to calculate M using equations (11) and (12). o and M o ′:

[0056]

[0057]

[0058] Define matrix SV as shown in equation (13):

[0059]

[0060] in, and The calculations are shown in equations (14) and (15):

[0061]

[0062]

[0063] By using the SVD algorithm to decompose the SV matrix, we can obtain SV = U∑V T Where ∑ is a diagonal matrix, and U,V are orthogonal matrices, the initial pose (R,t) is calculated by equation (16):

[0064]

[0065] S5-3 calculates the optimal (R,t)

[0066] Based on 3D key points M j The initial pose (R,t) and the mean vector μ calculated by equation (6) j (1≤j≤8) and the covariance matrix Σ obtained by equation (7) j (1≤j≤8) Solve equation (17) using the nonlinear least squares algorithm (Levenberg-Marquardt) to obtain the optimized (R,t):

[0067]

[0068] Where gluP is the perspective projection function.

[0069] Existing technologies predict 2D keypoints for target objects in images by locating each keypoint individually. However, for rigid objects, stable and invariant geometric relationships exist between 2D keypoints given a fixed viewpoint. Therefore, these relationships can be used to assist in keypoint localization. Furthermore, when arbitrarily selecting two vectors to calculate hypothetical keypoints, existing technologies do not consider the rationality of the vectors used, leading to significant positional deviations in the generated hypothetical keypoints. This invention proposes a vector filtering strategy to prevent the calculation of hypothetical keypoints using unreasonable vectors, thereby reducing the positional deviations and ultimately improving the performance of pose estimation. Attached Figure Description

[0070] Figure 1 3D model of the object.

[0071] Figure 2 Calculate the hypothetical key point diagram.

[0072] Figure 3 Skeleton structure diagram. Detailed Implementation

[0073] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0074] Figure 1 This is a 3D model of the object.

[0075] Figure 2 This is for calculating the hypothetical key point diagram.

[0076] For rigid objects, stable and invariant geometric relationships exist between 2D keypoints under a defined viewpoint. Therefore, the geometric relationships between different keypoints can be used to assist in evaluating the accuracy of predicted 2D keypoints. Furthermore, when calculating hypothetical keypoints, the angle between the two vectors involved in the calculation can be pre-calculated. Based on this angle, two approximately parallel or coincident vectors can be eliminated from the calculation, thus avoiding the generation of hypothetical keypoints with large errors. Based on these two points, this invention proposes a 6D pose estimation method for rigid objects using shape constraint joint vector filtering, which can effectively improve the accuracy of 6D pose estimation.

[0077] A skeleton is a widely used object shape descriptor. Figure 3 The set of light-colored line segments in the image represents the skeleton of the cup, a simplified representation of the cup's 3D model shape. Similar to the skeleton description, this invention defines a descriptor SD = {om} for describing the shape of objects in an image. j |1≤j≤8}∪{m j m j+1|1≤j<8}∪{m8m1}, where om j This indicates connecting the center point o of a 2D object to the 2D key point m. j The line segment, m j m j+1 Indicates the connection of 2D key points m j With 2D key points m j+1 The line segment, where j is the index of the key point.

[0078] Validating the effectiveness of a 6D pose estimation method for rigid objects using shape-constrained joint vector filtering.

[0079] This invention conducts experiments on two commonly used 6D object pose estimation datasets: the LINEMOD dataset and the OCC-LINEMOD dataset, and compares them with other state-of-the-art 6D pose estimation methods. Experimental results show that the proposed shape constraint joint vector filtering method for rigid objects can improve the performance of 6D pose estimation for rigid objects.

[0080] This invention uses the ADD(-S) index to evaluate pose estimation performance.

[0081] ADD refers to applying the estimated pose (R,t) and the true pose to the vertex x of a 3D model of an object. Perform the transformation and calculate the average distance between the two point sets after the transformation. The calculation of ADD is as shown in equation (18):

[0082]

[0083] Where B is the set of 3D model points, and b = |B| is the number of 3D model points. For the true rotation matrix, Let R be the true translation vector, R be the rotation matrix estimated by this invention, and t be the translation vector estimated by this invention. When the ADD distance is less than 10% of the model diameter (the maximum ADD distance is called the model diameter), the estimated pose is considered correct.

[0084] For symmetrical objects, due to the ambiguity of pose, the ADD-S index is used for evaluation. ADD-S calculates the average distance between point sets based on the nearest point distance, as shown in equation (19):

[0085]

[0086] The LINEMOD dataset is a standard 6D pose estimation dataset. This dataset contains 13 weakly textured objects, with approximately 1200 images for each object, totaling 15783 images. Due to the cluttered scenes and significant lighting variations during dataset construction, 6D pose estimation of the objects in this dataset is very difficult. Following common 6D pose estimation partitioning rules, this invention splits the dataset into 15% for training and 85% for testing. The experimental results on the LINEMOD dataset are shown in Table 1. Compared to other methods, the proposed method shows a significant improvement in pose estimation performance.

[0087] Table 1. Pose estimation performance on the ADD(-S) metric of the LINEMOD dataset ("*" indicates that the object is symmetrical)

[0088]

[0089] The OCC-LINEMOD dataset is a subset of the LINEMOD dataset, focusing primarily on occluded objects. Each image in this dataset contains objects with multiple labeled keypoints, and these objects are severely occluded, making 6D object pose estimation more challenging. The experimental results of this invention on the OCC-LINEMOD dataset are shown in Table 2. Compared to other methods, the proposed method shows a significant improvement in the ADD(-S) metric.

[0090] Table 2. Pose estimation performance on the ADD(-S) metric of the OCC-LINEMOD dataset ("*" indicates that the object is symmetrical).

[0091]

[0092]

Claims

1. A shape-constrained joint vector screening rigid object 6D pose estimation method, characterized by: The method comprises the following steps, S1, unit vector field prediction and pixel semantic prediction; Based on the deep network prediction unit vector field and pixel semantics, the deep network input is an RGB image I ∈ R H×W×3 , the output is semantic segmentation S ∈ R H×W×2 , and the unit vector field {V(m j ) ∈ R j |j=1,...,8} predicted for the real 2D key points {m H×W×2 |j=1,...,8}; wherein R represents a feature space, H is the image height, and W is the image width; The ResNet residual network is selected as the backbone network, which is composed of Conv+BN+ReLU layers, max pooling layers, stride convolution layers, atrous convolution layers, bilinear upsampling layers, and skip connection residual blocks at different levels. Firstly, the input RGB image of size HxWx3 is subjected to convolution and pooling operations. Then, skip connection and upsampling are performed on the feature map until the feature map size reaches HxW. Finally, a 1x1 convolution kernel is applied to the HxW feature map to increase the dimension, outputting the predicted unit vector field {V(m j )} and the predicted semantic segmentation S, where S[p w ,p h ,1] represents the probability that the semantic class of pixel p with index (p w ,p h ) in the image I is the specified object, denoted as a′ p (p w ,p h ); wherein p w is the horizontal coordinate of pixel p on the image, and p h is the vertical coordinate of pixel p on the image. Based on the predicted semantic segmentation S, the set of pixels E = {p|a′} within the target object region is obtained. p (p w ,p h (p > 0.5) w ,p h ) represents the pixel coordinates of pixel p in image I; based on the predicted unit vector field V(m j ), which gives the index (p) in image I. w ,p h The pixel p points to the 2D keypoint m. j The unit vector V(m) j )[p w ,p h ,:] is the prediction unit vector v of pixel p. j (p); S2, hypothesis key point calculation based on vector screening; The specific steps of calculating the hypothesis key point set of the 2D key point m1 are as follows: S2.1 Let r be the number of pixels in the target object region, then r = |E|; based on the unit vector field V(m1) predicted for the 2D key point m1, the angle between each pair of predicted vectors v1(p), p E and v1(q), q E in the target object region is calculated by the inverse cosine function: Wherein, p, q are pixel points in the target object region, p r r are the indexes of pixel point p and pixel point q in set E respectively, and 0≤p r ≤(r-1), 0≤q r ≤(r-1).​ S2.2 Compute the identification matrix A' e R according to the set threshold η r×r element value; If Then Else where the threshold η is set to π / 60 in the experiment; S2.3 Randomly select an element from the identification matrix If the value of is 0, randomly select the next element from the identification matrix; if the value of is 1, calculate the intersection point of the straight lines on which the two vectors v1(p) and v1(q) lie, and take the intersection point of the straight lines as the assumed key point, while setting the value of to 0, and repeat step 2.3 to obtain N assumed key points of the 2D key point m1 wherein represents the i-th assumed key point of the 2D key point m1, and i is the assumed key point index ​​​ S2.4 For the rest of 2D key points m j (2≤j≤8), based on the corresponding unit vector field V(m j ) Repeat the above steps 2.1-2.3 to get the corresponding set of hypothesis key points S3, vote for the hypothesis key point to calculate the confidence score; i-th hypothesized keypoint for 2D keypoint m1 The voting process as in steps 3.1-3.2: S3.1 Calculate for each pixel p in the target object region a pointing direction to the i-th hypothesized keypoint using formula (1) unit vector S3.2 Use Equation (2) to compute a confidence score for each hypothesized keypoint Vote gets confidence score: where Γ is an indicator function, threshold In the experiment, it is set to 0.99; The higher indicates more predicted unit vectors pointing to the hypothesized keypoint S3.3 For the remaining 2D keypoint m j Hypothesized keypoint (2≤j≤8) Repeat steps 3.1-3.2 above to calculate the confidence score; S3.4 2D keypoint m j The hypothesis keypoint with the highest confidence score is the predicted 2D keypoint m' j , S4, calculate the network loss; The total loss of the network is L = L g + L v + λL s , λ is an empirical parameter, which is set to 0.1 in the experiment; L g is the semantic segmentation loss defined in equation (3): L g = -a p loga′ p -(1-a p )log(1-a′ p ) (3) wherein a′ p represents the probability that the pixel p is predicted by the network as a target object pixel, a p is the semantic ground truth; L v is the unit vector field prediction loss defined by equation (4): where j is a key point index, p is a pixel point in the target object region pixel point set E, and are predicted unit vectors v j (p) along horizontal and vertical directions of the image, respectively, and are real unit vectors u j (p) along horizontal and vertical directions of the image, respectively, is a function; L s is a shape constraint loss defined by equation (5): where w j = [d j , g j , g j+1 ] is a feature vector defined based on SD, d j is the length of the line segment m j m j+1 , and j = 8 when the line segment is m8m1; g j is the length of the line segment om j , and w8= [d8, g8, g1] when j = 8; w' j = [d' j , g' j , g' j+1 ] is a feature vector defined based on SD' = {om' j | 1≤j≤8}∪{m' j m' j+1 | 1≤j<8}∪{m'8m'1}, d' j is the length of the line segment m' j m' j+1 , and j = 8 when the line segment is m'8m'1; g' j is the length of the line segment om' j , and w'8= [d'8, g'8, g'1] when j = 8; S5, calculate the 6D pose; For 2D key points m j (1≤j≤8), based on the corresponding set of hypothetical key points and confidence scores of key hypotheses remember in, Is it a key assumption? The pixel x-coordinate, Is it a key assumption? The pixel ordinates; the mean vector μ is calculated using equations (6) and (7) respectively. j The covariance matrix Σ j : 3D key points M according to the model j (1≤j≤8) and predicted 2D key points m' j (1≤j≤8), first calculate 3D key points M in the world coordinate system j In the camera coordinate system, the problem is converted into a 3D-3D pose solving problem; then the ICP algorithm is used to initialize (R, t); finally, the optimal (R, t) is solved by using a nonlinear optimization method.

2. The shape constrained joint vector screening rigid object 6D pose estimation method according to claim 1, wherein: The specific calculation method of step 5 is as follows, S5-1 calculate 3D key points M j Coordinates in the camera coordinate system; For 3D key point M j , randomly take 4 control points c jn (1≤n≤4), j is the key point index, n is the control point index; the coordinates of the control point c jn in the world coordinate system are denoted as A set of weight coefficients α j1 , α j2 , α j3 , α j4 can be obtained by calculating formula (8): wherein (x j ,y j ,z j ) is the 3D key point M j coordinates in the world coordinate system; Control point c jn The coordinates in the camera coordinate system are denoted as The control point c in the camera coordinate system is then jn The 3D key point M j The corresponding predicted 2D key point m' j satisfies equation (9): where f u ,f v ,u c ,v c are camera intrinsic parameters, m j '(x) is the pixel horizontal coordinate of the predicted 2D key point m' j '(y) is the pixel vertical coordinate of the predicted 2D key point m' j '(y) is the pixel vertical coordinate of the predicted 2D key point m' j , a j1 , a j2 , a j3 , a j4 is a weight coefficient calculated by formula (8); solving the equation group corresponding to formula (9) can obtain the coordinates of the control point c jn in the camera coordinate system 3D key point M j The coordinates in the camera coordinate system are denoted as (x′ j ,y′ j ,z′ j ), and the control point and the 3D key point satisfy equation (10) in the camera coordinate system: The weighting coefficient α obtained by using equation (8) j1 ,α j2 ,α j3 ,α j4 The control point c calculated using equation (9) jn coordinates in the camera coordinate system Substituting into equation (10), the 3D key points M can be obtained. j Coordinates (x′) in the camera coordinate system j ,y′ j ,z′ j ); S5-2 initialize (R, t) using the ICP algorithm; KP j =[x j ,y j ,z j ] T , where (x j ,y j ,z j ) is the 3D key point M j Coordinates in the world coordinate system; denoted as KP′ j =[x′ j ,y′ j ,z′ j ] T , where (x′ j ,y′ j ,z′ j ) is the 3D key point M j The coordinates in the camera coordinate system are used to calculate M using equations (11) and (12). o and M′ o : Define the matrix SV as formula (13): wherein and the calculation of which is as in equations (14) and (15): The SV matrix can be decomposed by using an SVD algorithm to obtain SV = U∑V T where ∑ is a diagonal matrix, and U and V are orthogonal matrices. An initial pose (R, t) is calculated from equation (16): S5-3 calculate the optimal (R, t); Based on 3D key points M j , the initial pose (R, t), the mean vector μ calculated from equation (6) j (1≤j≤8) and the covariance matrix Σ calculated from equation (7) j (1≤j≤8) to solve equation (17) using a non-linear least squares algorithm to obtain the optimized (R, t): Where gluP is a perspective projection function.

Images