A method and device for fast 6D pose estimation of a target object

Abstract

The application provides a 6D pose fast estimation method and device of a target object, relates to the technical field of machine vision and perception, and the field of digital data processing, and comprises the following steps: acquiring an RGB image and a depth image of the target object, inputting the RGB image into a segmentation model to obtain a binary mask of the target object; acquiring a camera intrinsic matrix, constructing a curved surface point cloud matrix of the target object according to the depth image, the binary mask and the camera intrinsic matrix; constructing a cubic package of the target object, constructing a fitting penalty function according to the curved surface point cloud matrix and the cubic package; calculating a gradient vector of a center point and Euler angles of the target object according to the curved surface point cloud matrix, the cubic package and the fitting penalty function; iteratively solving the gradient vector of the center point and the Euler angles by using a gradient descent algorithm to obtain an optimal solution of the center point and the Euler angles, and taking the optimal solution of the center point and the Euler angles as the 6D pose of the target object.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of machine vision and perception, and particularly relates to a method and device for quickly estimating the 6D pose of a target object. BACKGROUND

[0002] Existing pose estimation methods for target objects mainly include two categories: 1) feature matching-based methods; and 2) template matching-based methods.

[0003] The feature matching-based method extracts local features (such as point features, line features or edge features) in the image, and matches them with the 3D model of the target object, and finally solves the 6D pose parameters through the PnP (Perspective-n-Point) algorithm. The specific process includes feature extraction: detecting key points or geometric structures from the input image to generate high-dimensional feature representation. Feature matching: associating the extracted 2D features with the corresponding features in the 3D model point cloud or CAD model to establish 2D-3D correspondence. Pose solving: based on the matching correspondence, the PnP algorithm is used to minimize the re-projection error to optimize the rotation and translation parameters. The advantage of this method is that it has certain robustness to partial occlusion and background noise, but it relies on accurate feature extraction and matching algorithms, and the calculation efficiency is low, especially in complex scenes, which is prone to pose deviation due to matching errors.

[0004] The template matching-based method first generates synthetic rendered images (templates) of the target 3D model at different viewing angles, and then calculates the similarity between the input image and the template through a deep neural network, and takes the template pose with the highest similarity as the estimation result. The core steps include template generation: offline rendering of multi-view images of the 3D model to cover the complete viewing angle space. Similarity calculation: using CNN or Transformer network to extract deep features of the template and input image, and quantifying the similarity score. Pose regression: directly outputting the 6D pose by retrieving the pose parameters of the highest scoring template, or combining iterative optimization to improve accuracy. This method has significant effect on textured objects, but requires pre-storing a large amount of template data, resulting in large storage and calculation overhead, and the generalization ability is limited, making it difficult to adapt to objects that have not been seen or have variable shapes.

[0005] From the above, the current technology generally faces the following challenges:

[0006] High implementation complexity and strong dependence: both methods require detailed 3D models or multi-view image libraries of target objects, increasing the deployment threshold; deep learning-based template matching requires training of a special network, further increasing the complexity.

[0007] Generalization and real-time problem: the feature matching method performs poorly on low-texture objects, and the computational complexity of the matching process grows quadratically with the number of feature points, making it difficult to meet real-time requirements. The template matching method consumes a lot of resources in rendering and similarity calculation, and the delay on edge devices is significant. It is sensitive to changes in lighting and viewing angle, and has weak generalization ability.

[0008] Large amount of calculation: the deep learning model has a large number of parameters, and high computing power is required for inference. The energy efficiency ratio is low in edge deployment, affecting the feasibility of actual application. SUMMARY

[0009] In order to solve the above problems, the present application provides a 6D pose fast estimation method of a target object, comprising the steps of:

[0010] obtaining the RGB image and the depth image of the target object, inputting the RGB image into the segmentation model to obtain the binary mask of the target object;

[0011] obtaining the camera intrinsic matrix, and constructing the curved point cloud matrix of the target object according to the depth image, the binary mask and the camera intrinsic matrix;

[0012] constructing the cuboid package of the target object, and constructing the fitting penalty function according to the curved point cloud matrix and the cuboid package;

[0013] calculating the gradient vector of the center point and the Euler angle of the target object according to the curved point cloud matrix, the cuboid package and the fitting penalty function;

[0014] obtaining the optimal solution of the center point and the Euler angle by iterative solving the gradient vector of the center point and the Euler angle through the gradient descent algorithm, and taking the optimal solution of the center point and the Euler angle as the 6D pose of the target object.

[0015] Optionally, the curved point cloud matrix is composed of the coordinates of each pixel point of the target object in the three-dimensional camera coordinate system, wherein the X axis of the three-dimensional camera coordinate system is the vertical direction, the Y axis is the horizontal direction, and the Z axis is the optical axis direction of the RGBD camera.

[0016] Optionally, the fitting penalty function is constructed according to the curved point cloud matrix and the cuboid package, specifically including:

[0017] S11: the cuboid package of the target object is the smallest cube wrapping the target object, and the height H, the width W and the depth D of the cuboid package are obtained by a measuring tool;

[0018] S12: the coordinates [X i ,Y i ,Z i ] of the i-th pixel point P i in the curved point cloud matrix are extracted;

[0019] S13: Based on the height H, width W, depth D, and pixel P of the cube bag i coordinates [X i ,Y i Z i ], calculate to obtain pixel point P i The sum of the distances M to the six faces of the cube bag i ;

[0020] S14: Repeat steps S12-S14 until the sum of the distances of N pixels is obtained. Loss is the fitting penalty function, where N represents the total number of pixels in the surface point cloud matrix.

[0021] Optionally, the independent variable for the center point of the target object includes the X-axis coordinate of the center point. c , center point Y-axis coordinate y c and the center point Z-axis coordinate z c The independent variables of the Euler angles of the target object include the first Euler angle ω0, the second Euler angle ω1, and the third Euler angle ω2.

[0022] Optionally, the calculation process for the gradient vector of the center point of the target object specifically includes:

[0023] The height H, width W, and depth D of the cube bag are obtained through measurement tools. The coordinates of each pixel in the surface point cloud matrix are obtained. The unit vector n1 of the target object in the 3D camera coordinate system is obtained, which is oriented towards the X-axis, the unit vector n2 towards the Y-axis, and the unit vector n3 towards the Z-axis.

[0024] Based on the height H, width W, depth D of the cube, the coordinates of each pixel, and the unit vector n1, the fitting penalty function Loss is calculated for the center point's X-axis coordinate x. c partial derivatives ;

[0025] Based on the height H, width W, depth D of the cube, the coordinates of each pixel, and the unit vector n2, the fitting penalty function Loss is calculated for the center point's Y-axis coordinate y. c partial derivatives ;

[0026] Based on the height H, width W, depth D of the cube, the coordinates of each pixel, and the unit vector n3, the fitting penalty function Loss is calculated for the center point's Z-axis coordinate z. c partial derivatives ;

[0027] partial derivatives Partial derivatives and partial derivatives The combined row vectors ( , , ) the gradient vector of the Euler angles of the target object.

[0028] Optionally, the process of calculating the gradient vector of the Euler angles of the target object specifically comprises:

[0029] obtaining the height H, the width W and the depth D of the cubic package by the measuring tool, obtaining the coordinates of each pixel point in the curved surface point cloud matrix, and obtaining the unit vector n1 towards the X axis, the unit vector n2 towards the Y axis and the unit vector n3 towards the Z axis of the target object in the three-dimensional camera coordinate system;

[0030] calculating the partial derivative of the fitting penalty function Loss with respect to the first Euler angle ω0 according to the height H, the width W, the depth D of the cubic package, the coordinates of each pixel point and the unit vector n1; ;

[0031] calculating the partial derivative of the fitting penalty function Loss with respect to the second Euler angle ω1 according to the height H, the width W, the depth D of the cubic package, the coordinates of each pixel point and the unit vector n2; ;

[0032] calculating the partial derivative of the fitting penalty function Loss with respect to the third Euler angle ω2 according to the height H, the width W, the depth D of the cubic package, the coordinates of each pixel point and the unit vector n3; ;

[0033] combining the partial derivative , the partial derivative and the partial derivative into a row vector , , as the gradient vector of the Euler angles.

[0034] Optionally, the gradient descent algorithm is used to iteratively solve the gradient vector of the center point and the Euler angles to obtain the optimal solution of the center point and the Euler angles, and the optimal solution of the center point and the Euler angles is taken as the 6D pose of the target object, specifically comprising:

[0035] setting the step size, the step size attenuation factor and the iteration stop parameter of the gradient descent algorithm, and calculating the optimal solution of the center point and the Euler angles that minimizes the gradient vector of the center point and the Euler angles by the set gradient descent algorithm;

[0036] the optimal solution of the center point comprises: the optimal center point X axis coordinate, the optimal center point Y axis coordinate and the optimal center point Z axis coordinate;

[0037] the optimal solution of the Euler angles comprises: the optimal first Euler angle, the optimal second Euler angle and the optimal third Euler angle;

[0038] The optimal center point X-axis coordinate, the optimal center point Y-axis coordinate, the optimal center point Z-axis coordinate, the optimal first Euler angle, the optimal second Euler angle and the optimal third Euler angle are taken as the 6D pose of the target object.

[0039] The application further provides a device for quickly estimating the 6D pose of a target object, which is used to implement the method for quickly estimating the 6D pose of the target object.

[0040] The binary mask obtaining module is used to obtain the RGB image and the depth image of the target object, and the binary mask of the target object is obtained by inputting the RGB image into the segmentation model.

[0041] The curved surface point cloud matrix obtaining module is used to obtain the camera intrinsic matrix, and the curved surface point cloud matrix of the target object is constructed according to the depth image, the binary mask and the camera intrinsic matrix.

[0042] The fitting penalty function constructing module is used to construct the cubic package of the target object, and the fitting penalty function is constructed according to the curved surface point cloud matrix and the cubic package.

[0043] The gradient vector obtaining module is used to calculate the gradient vector of the center point and the Euler angle of the target object according to the curved surface point cloud matrix, the cubic package and the fitting penalty function.

[0044] The 6D pose obtaining module is used to obtain the optimal solution of the center point and the Euler angle by iteratively solving the gradient vector of the center point and the Euler angle through the gradient descent algorithm, and the optimal solution of the center point and the Euler angle is taken as the 6D pose of the target object.

[0045] The application further provides an electronic device, which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, and the processor implements the method for quickly estimating the 6D pose of the target object when executing the program.

[0046] The application further provides a non-transitory computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the method for quickly estimating the 6D pose of the target object.

[0047] The application has the following beneficial effects:

[0048] 1. The center point and the Euler angle of the cubic package of the target object are taken as the 6D pose of the target object, and the 6D pose of the target object can be obtained only by constructing the cubic package of the target object, without using a neural network or obtaining a 3D model of the target object, so that the calculation process of the 6D pose of the target object is greatly simplified.

[0049] 2、According to the curved surface point cloud matrix of the target object and the cubic package, a fitting penalty function is constructed, the fitting penalty function can count the sum of the distances from each pixel point in the curved surface point cloud matrix to the six faces of the cubic package, and the relative position and inclusion relationship of the cubic package and the target object in the three-dimensional camera coordinate system can be fully reflected through the fitting penalty function, so as to improve the calculation accuracy of the center point and the Euler angle of the target object;

[0050] 3、The gradient vector of the center point and the Euler angle is iteratively solved through the gradient descent algorithm, the optimal solution of the center point and the Euler angle is obtained, and the optimal solution of the center point and the Euler angle is taken as the 6D pose of the target object, only the gradient descent algorithm with small calculation amount is needed to iteratively calculate the 6D pose, without processing a large amount of data, the real-time performance of obtaining the 6D pose is greatly improved, and the delay of displaying the 6D pose on the edge device is significantly reduced. BRIEF DESCRIPTION OF DRAWINGS

[0051] Figure 1 The method flowchart of the embodiment of the application is shown in the figure.

[0052] Figure 2 The cubic package schematic diagram of the yogurt beverage box is shown in the figure.

[0053] The implementation, functional features and advantages of the application will be further described with reference to the embodiments and the accompanying drawings. DETAILED DESCRIPTION

[0054] The technical solutions in the embodiments of the application will be clearly and completely described below with reference to the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, not all the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the application.

[0055] Referring to Figure 1 The application provides a 6D pose fast estimation method of a target object, comprising the following steps:

[0056] An RGB image and a depth image of the target object are obtained, and the RGB image is input into a segmentation model to obtain a binary mask of the target object.

[0057] In some embodiments, for an RGB image taken by an RGBD camera, a trained instance segmentation model (such as a YOLOv8-seg model) is used to obtain a binary mask of the target object, the YOLOv8-seg model extends the pixel-level segmentation capability on the basis of target detection, supports simultaneous output of target bounding box, class label and mask (Mask), is suitable for fine identification tasks in complex scenes, and the network composition includes a Backbone module, a Neck module and a Head module. The Backbone module is constructed based on a CSPDarknet53 feature extraction network, a C2f module is introduced to replace a C3 module of the YOLOv5, and the gradient flow efficiency is enhanced. The Neck module adopts a PANet (Path Aggregation Network) or FPN (Feature Pyramid Network) structure for fusing multi-scale features. The Head module adopts a dual-branch design of a detection head plus a segmentation head, the detection head is used for detecting the bounding box and the class label of the target object, and the segmentation head is used for segmentation operation according to mask coefficients. The dual-branch design of (Box+Cls) + segmentation head (MaskCoefficients). The segmentation core mechanism module is a Proto layer, the Proto layer uses an 80x80 feature map to generate a 160x160 32-channel prototype mask (Prototype Mask) through convolution upsampling, each detection head outputs a 32-dimensional coefficient, and matrix multiplication is performed with the Proto layer to generate the final binary mask.

[0058] A segmentation large model SAM (Segment Anthing Model) based on a prompt word can also be used to estimate the binary mask (mask) of the target object. The SAM adopts a Transformer architecture, supports zero-shot transfer and adaptive segmentation capability, and can generate a pixel-level mask based on a point, a box, text and the like. The SAM includes three core modules that work together: an image encoder, a prompt encoder and a mask decoder. The image encoder is used to extract image features; the prompt encoder is used to encode interactive prompts (points / boxes / texts) into vectors. The positions of points / boxes are encoded, texts are encoded by CLIP, and mask prompts are processed by convolution. The mask decoder is used to fuse image and prompt features to generate a segmentation mask. The mask decoder is a lightweight Transformer decoder that outputs a mask through a cross-attention mechanism.

[0059] An intrinsic matrix of the camera is obtained, and a curved surface point cloud matrix of the target object is constructed according to the depth image, the binary mask and the intrinsic matrix of the camera.

[0060] In some embodiments, the curved surface point cloud matrix is composed of coordinates of each pixel point of the target object in a three-dimensional camera coordinate system, wherein the X-axis of the three-dimensional camera coordinate system is the vertical direction, the Y-axis is the horizontal direction, and the Z-axis is the optical axis direction of the RGBD camera.

[0061] In some embodiments, the known target object binary mask mask, the row coordinate vector U and the column coordinate vector V of the pixel points of the target object can be obtained, and U and V can be obtained by calling the numpy.where() function using the python program language. The length of the row coordinate vector U is N, the depth of the position of the vector [U[i], V[i]] of the i-th pixel point is the Z-axis coordinate Z[i] of the pixel point, wherein i represents the index i∈[0,N-1] of the row coordinate vector U, U[i] represents the i-th real number in the row coordinate vector U, and V[i] represents the i-th real number in the column coordinate vector. Assuming that the depth image collected by the RGBD camera is Depth, according to the above definition, there is Z[i]=Depth[U[i],V[i]].

[0062] According to the mapping relationship between the camera coordinate system and the image coordinate system, the X-axis coordinate and the Y-axis coordinate of the i-th pixel point in the surface point cloud matrix of the target object in the three-dimensional camera coordinate system are represented as follows:

[0063]

[0064]

[0065] wherein, represents the element-priority vector dot product, , , and represent each camera intrinsic parameter in the camera intrinsic parameter matrix K, and the camera intrinsic parameter matrix K is represented as: The surface point cloud matrix S=[X,Y,Z] of the target object with a dimension of N rows and 3 columns can be constructed from the coordinates of all the pixel points, X represents the X-axis coordinate column vector of the pixel point, Y represents the Y-axis coordinate column vector of the pixel point, and Z represents the Z-axis coordinate column vector of the pixel point.

[0066] Constructing a cuboid package of the target object, constructing a fitting penalty function according to the surface point cloud matrix and the cuboid package;

[0067] In some embodiments, the fitting penalty function is constructed according to the surface point cloud matrix and the cuboid package, specifically including:

[0068] S11: The cuboid package of the target object is the smallest cuboid wrapping the target object, and the height H, the width W and the depth D of the cuboid package are obtained by a measuring tool;

[0069] S12: Extracting the coordinates [X i ,Y i ,Z i ] of the i-th pixel point P i in the surface point cloud matrix;

[0070] S13: Based on the height H, width W, depth D, and pixel P of the cube bag i coordinates [X i ,Y i Z i ], calculate to obtain pixel point P i The sum of the distances M to the six faces of the cube bag i ;

[0071] In some embodiments, pixel P i The sum of the distances M to the six faces of the cube bag i The expression is as follows:

[0072]

[0073] Let P be the i-th pixel [X[i], Y[i], Z[i]] in the surface point cloud matrix S, and L be the pixel [X[i], Y[i], Z[i]]. j [i] can understand vectors In the normal vector n j The projection onto the target object is shown, with O' as the center point of the cube bag and j=1,2,3. The unit vectors of the target object in the 3D camera coordinate system are n1 towards the X-axis, n2 towards the Y-axis, and n3 towards the Z-axis. If point P is inside the cube bag, the sum of its distances to the six faces reaches the minimum value H+W+D; otherwise, if it is outside the cube bag, the sum of its distances to the six faces will exceed H+W+D. Therefore, it is easy to predict that when the loss reaches the minimum value H+W+D, the cube bag will completely enclose the surface of the target object, meaning there cannot be any points protruding from the cube bag. Furthermore, if all six faces of the cube bag contain points from the point cloud S, then the center point of the cube bag coincides with the center point of the target object, and the orientation and pose are geometrically coincident with the target object (assuming the cube bag of the target object is unique; irregular objects are usually unique, only symmetrical objects such as spheres and cylinders have non-unique cube bags).

[0074] S14: Repeat steps S12-S14 until the sum of the distances of N pixels is obtained. Loss is the fitting penalty function, where N represents the total number of pixels in the surface point cloud matrix.

[0075] In some embodiments, the physical meaning of the fitting penalty function Loss is to calculate the sum of the distances from each pixel P in the surface point cloud matrix S to the six faces of the cube (including the two parallel faces to the left and right, the two parallel faces to the top and bottom, and the two parallel faces to the front and back, for a total of six planes).

[0076] Based on the surface point cloud matrix, cube bag, and fitting penalty function, the gradient vectors of the center point and Euler angles of the target object are calculated.

[0077] In some embodiments, the independent variables of the center point of the target object include a center point X-axis coordinate x c , a center point Y-axis coordinate y c , and a center point Z-axis coordinate z c , the independent variables of the Euler angles of the target object include a first Euler angle ω0, a second Euler angle ω1, and a third Euler angle ω2.

[0078] In some embodiments, a 3D rotation matrix can be obtained according to the Euler angles ω = [ω0, ω1, ω2], Figure 2 A schematic diagram of a cubic package of a yogurt beverage box is shown, where O' = [x c , y c , z c ] represents the center point of the cubic package (yogurt beverage), [H, W, D] respectively represent the height, width, and depth of the yogurt beverage, and unit vectors n1, n2, n3 represent the orientations of the X-axis, Y-axis, and Z-axis of the yogurt beverage in the three-dimensional camera coordinate system, and are mutually orthogonal, corresponding to the three row vectors of the rotation matrix, that is:

[0079]

[0080]

[0081]

[0082] In some embodiments, the fitting penalty function Loss is differentiable, and the key is to calculate the partial derivatives of the fitting penalty function Loss with respect to the six independent variables [x c , y c , z c , ω0, ω1, ω2].

[0083] The calculation process of the gradient vector of the center point of the target object specifically includes:

[0084] Obtain the height H, width W, and depth D of the cubic package by a measuring tool, obtain the coordinates of each pixel point in the surface point cloud matrix, and obtain the unit vectors n1, n2, and n3 representing the orientations of the X-axis, Y-axis, and Z-axis of the target object in the three-dimensional camera coordinate system;

[0085] According to the height H, width W, and depth D of the cubic package, the coordinates of each pixel point, and the unit vector n1, the partial derivative of the fitting penalty function Loss with respect to the center point X-axis coordinate x c is calculated ;

[0086] According to the height H, width W, and depth D of the cubic package, the coordinates of each pixel point, and the unit vector n2, the partial derivative of the fitting penalty function Loss with respect to the center point Y-axis coordinate y c is calculated ;

[0087] According to the height H, the width W, the depth D of the cuboid, the coordinates of each pixel point, and the unit vector n3, the partial derivative of the fitting penalty function Loss with respect to the Z-axis coordinate z of the center point is calculated c ;

[0088] In some embodiments, let d1=W, d2=H, d3=D, , , According to the chain rule, the gradient vector of the center point is calculated as follows:

[0089]

[0090]

[0091] Where, L j [i]=<S[i,:]-O’,n j >(j=1,2,3), <·> represents the vector inner product operator, and S[i,:] represents the i-th row vector in the surface point cloud matrix.

[0092] The partial derivative , the partial derivative , and the partial derivative are combined into a row vector , , as the gradient vector of the center point.

[0093] In some embodiments, the process of calculating the gradient vector of the Euler angle of the target object specifically includes:

[0094] The height H, the width W, and the depth D of the cuboid are obtained by measuring tools, the coordinates of each pixel point in the surface point cloud matrix are obtained, and the unit vector n1 towards the X-axis, the unit vector n2 towards the Y-axis, and the unit vector n3 towards the Z-axis of the target object in the three-dimensional camera coordinate system are obtained.

[0095] According to the height H, the width W, the depth D of the cuboid, the coordinates of each pixel point, and the unit vector n1, the partial derivative of the fitting penalty function Loss with respect to the first Euler angle ω0 is calculated .

[0096] According to the height H, the width W, the depth D of the cuboid, the coordinates of each pixel point, and the unit vector n2, the partial derivative of the fitting penalty function Loss with respect to the second Euler angle ω1 is calculated .

[0097] ​According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n3, the partial derivative of the fitting penalty function Loss with respect to the third Euler angle ω2 is calculated ;

[0098] In some embodiments, let d1=W, d2=H, d3=D, for the gradient vector of the Euler angle, according to the chain rule, we have:

[0099]

[0100] Where:

[0101]

[0102] Where, L j [i]=<S[i,:]-O’,n j >(j=1,2,3),<·> indicates the vector inner product operator, S[i,:] indicates the i-th row vector in the surface point cloud matrix;

[0103]

[0104]

[0105]

[0106]

[0107]

[0108]

[0109]

[0110]

[0111]

[0112] The partial derivative , the partial derivative and the partial derivative are combined into a row vector , , as the gradient vector of the Euler angle.

[0113] The gradient descent algorithm is used to iteratively solve the gradient vector of the center point and the Euler angle, and the optimal solution of the center point and the Euler angle is obtained. The optimal solution of the center point and the Euler angle is taken as the 6D pose of the target object.

[0114] In some embodiments, the optimal solution of the center point and the Euler angle is obtained by iteratively solving the gradient vectors of the center point and the Euler angle through a gradient descent algorithm, and the optimal solution of the center point and the Euler angle is taken as the 6D pose of the target object, specifically including:

[0115] The step size, step size attenuation factor and iteration stop parameter of the gradient descent algorithm are set, and the optimal solution of the center point and the Euler angle that minimizes the gradient vector of the center point and the Euler angle is obtained through the set gradient descent algorithm;

[0116] In some embodiments, the optimal solution is obtained by using the Backtracking Line Search gradient descent algorithm. Backtracking Line Search is an adaptive step size selection strategy used in gradient descent methods to optimize convergence speed and stability. It dynamically adjusts the step size at each iteration to avoid divergence or slow convergence caused by fixed step size. The core of Backtracking Line Search is to introduce Armijo condition to ensure sufficient decrease of the objective function value at each iteration. The step size is 0.01, the step size attenuation factor is 0.8, and the iteration stop parameter is Armijo condition. The optimal solution of the center point and the Euler angle is obtained within 20 iterations;

[0117] The calculation process of the optimal solution of the center point and the Euler angle is as follows:

[0118]

[0119] where η is the learning rate, k is the iteration number, c (k) represents the center point of the kth iteration, c (k+1) represents the center point of the k+1th iteration, is the gradient vector of the center point, θ (k) is the Euler angle of the kth iteration, θ (k+1) is the Euler angle of the k+1th iteration, is the gradient vector of the Euler angle. The parameters are updated by gradient descent iteration, and the optimal solution of the center point and the Euler angle is output when the iteration converges or reaches the maximum number as the 6D pose.

[0120] The optimal solution of the center point includes: optimal center point X-axis coordinate, optimal center point Y-axis coordinate and optimal center point Z-axis coordinate;

[0121] The optimal solution of the Euler angle includes: optimal first Euler angle, optimal second Euler angle and optimal third Euler angle;

[0122] The optimal center point X-axis coordinate, the optimal center point Y-axis coordinate, the optimal center point Z-axis coordinate, the optimal first Euler angle, the optimal second Euler angle and the optimal third Euler angle are taken as the 6D pose of the target object.

[0123] The application further provides a 6D pose fast estimation device of a target object, which is used for realizing the 6D pose fast estimation method of the target object, and the device comprises:

[0124] The binary mask acquisition module is used for acquiring the RGB image and the depth image of the target object, and the binary mask of the target object is obtained by inputting the RGB image into the segmentation model.

[0125] The curved surface point cloud matrix acquisition module is used for acquiring the camera intrinsic matrix, and the curved surface point cloud matrix of the target object is constructed according to the depth image, the binary mask and the camera intrinsic matrix.

[0126] The fitting penalty function construction module is used for constructing the cubic package of the target object, and the fitting penalty function is constructed according to the curved surface point cloud matrix and the cubic package.

[0127] The gradient vector acquisition module is used for calculating and obtaining the gradient vector of the center point and the Euler angle of the target object according to the curved surface point cloud matrix, the cubic package and the fitting penalty function.

[0128] The 6D pose acquisition module is used for iteratively solving the gradient vector of the center point and the Euler angle by using the gradient descent algorithm, obtaining the optimal solution of the center point and the Euler angle, and taking the optimal solution of the center point and the Euler angle as the 6D pose of the target object.

[0129] The electronic device provided by the embodiment of the application comprises a processor and a memory; the memory stores a computer program, and the computer program is used for realizing the 6D pose fast estimation method of the target object of any one of the above schemes when being executed by the processor.

[0130] Specifically, the processor can comprise a general microprocessor, an instruction set processor and / or a related chipset and / or a special microprocessor (for example, an application specific integrated circuit (ASIC)), etc. The processor can further comprise an on-board memory for cache use. The processor can be a single processing unit or a plurality of processing units for executing different actions of the method flow according to the embodiment of the application.

[0131] Memory, for example, can be any media capable of storing, containing, communicating, propagating, or transporting instructions. For example, memory can include, without limitation, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, devices, or propagation mediums. Specific examples of memory include magnetic storage devices such as hard disks or hard disk drives (HDDs); optical storage devices such as compact disc (CD-ROM); and / or wired / wireless communication links.

[0132] The application also provides a computer readable medium having stored thereon a computer program which, when executed by a processor, implements the method for fast 6D pose estimation of a target object according to any of the above-mentioned solutions. The computer readable medium can be included in the device / apparatus / system described in the above-mentioned embodiments; or can exist separately and not be assembled into the device / apparatus / system. The computer readable medium carries one or more programs which, when executed, implement the method according to the embodiments of the application.

[0133] According to the embodiments of the application, the computer readable medium can be a computer readable signal medium or a computer readable storage medium or any combination of the two. The computer readable storage medium may, for example, be, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the above. More specific examples of the computer readable storage medium can include, but are not limited to, an electrical connection having one or more wires, a portable computer diskette, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this application, the computer readable storage medium can be any tangible medium that contains or stores a program used by or in connection with an instruction execution system, apparatus, or device. In this application, the computer readable signal medium can include a data signal carried in a baseband or as part of a carrier wave, in which the computer readable program code is carried. Such a propagated data signal can take any of a variety of forms, including, but not limited to, an electromagnetic signal, an optical signal, or any suitable combination of the above. The computer readable signal medium can also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer readable medium can be transmitted using any suitable medium, including, but not limited to, wireless, wired, optical fiber cable, radio frequency (RF), and / or the like, or any suitable combination of the above.

[0134] It will be appreciated by those skilled in the art that features of the various embodiments and / or claims of the present application can be combined and / or interchanged, even though such combinations and / or interchanges are not expressly disclosed in the present application. In particular, the features of the various embodiments and / or claims of the present application can be combined and / or interchanged, even though such combinations and / or interchanges are not expressly disclosed in the present application. All of these combinations and / or interchanges are within the scope of the present application. Accordingly, the scope of the present application should not be limited to the above-described embodiments, but should be determined by the appended claims and their equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the present application.

Claims

1. A method for fast 6D pose estimation of a target object, characterized in that, The method comprises the steps of: obtaining an RGB image and a depth image of a target object, inputting the RGB image into a segmentation model to obtain a binary mask of the target object; obtaining a camera intrinsic matrix, and constructing a curved surface point cloud matrix of the target object according to the depth image, the binary mask and the camera intrinsic matrix; constructing a cubic package of the target object, and constructing a fitting penalty function according to the curved surface point cloud matrix and the cubic package; calculating a gradient vector of a center point and Euler angles of the target object according to the curved surface point cloud matrix, the cubic package and the fitting penalty function; obtaining an optimal solution of the center point and the Euler angles by iterative solving of the gradient vector of the center point and the Euler angles through a gradient descent algorithm, and taking the optimal solution of the center point and the Euler angles as a 6D pose of the target object; constructing the fitting penalty function according to the curved surface point cloud matrix and the cubic package, specifically comprising: S11: the cubic package of the target object is a smallest cube wrapping the target object, and the height H, the width W and the depth D of the cubic package are obtained through a measuring tool; S12: Extract the coordinate [X, Y, Z] of the i-th pixel point P in the curved surface point cloud matrix i . i i i ​​​ S13: Based on the height H, width W, depth D, and pixel P of the cube bag i coordinates [X i ,Y i Z i ], calculate to obtain pixel point P i The sum of the distances M to the six faces of the cube bag i ; S14: repeat steps S12-S14 until the sum of distances of N pixel points is obtained, and the sum of distances of N pixel points is obtained as a fitting penalty function Loss. As the fitting penalty function Loss, N represents the total number of pixel points in the surface point cloud matrix. independent variables of the center point of the target object include a center point X-axis coordinate x c , a center point Y-axis coordinate y c , and a center point Z-axis coordinate z c , and independent variables of the Euler angle of the target object include a first Euler angle ω0, a second Euler angle ω1, and a third Euler angle ω2; the calculation process of the gradient vector of the center point of the target object specifically comprises: obtaining the height H, the width W and the depth D of the cubic package through a measuring tool, obtaining the coordinates of each pixel point in the curved surface point cloud matrix, and obtaining a unit vector n1 towards the X axis, a unit vector n2 towards the Y axis and a unit vector n3 towards the Z axis of the target object in a three-dimensional camera coordinate system; According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n1, the partial derivative of the fitting penalty function Loss to the x-axis coordinate x of the center point is calculated c ;​ According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n2, the partial derivative of the fitting penalty function Loss with respect to the Y-axis coordinate y of the center point is calculated c ;​ According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n3, the partial derivative of the fitting penalty function Loss with respect to the central point Z-axis coordinate z is calculated c ;​ partial derivatives , partial derivatives , and partial derivatives are combined into a row vector ( , , ) as a gradient vector of the center point; the calculation process of the gradient vector of the Euler angles of the target object specifically comprises: obtaining the height H, the width W and the depth D of the cubic package through a measuring tool, obtaining the coordinates of each pixel point in the curved surface point cloud matrix, and obtaining a unit vector n1 towards the X axis, a unit vector n2 towards the Y axis and a unit vector n3 towards the Z axis of the target object in a three-dimensional camera coordinate system; According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n1, the partial derivative of the fitting penalty function Loss with respect to the first Euler angle ω0 is calculated ; According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n2, a partial derivative of the fitting penalty function Loss with respect to the second Euler angle ω1 is calculated and obtained ; According to the high H, the wide W, the deep D of the cubic package, the coordinates of each pixel point and the unit vector n3, the partial derivative of the fitting penalty function Loss with respect to the third Euler angle ω2 is calculated ; partial derivatives , partial derivatives , partial derivatives are combined into a row vector ( , , ) as the gradient vector of the Euler angles.

2. The method of Claim 1, wherein, the curved surface point cloud matrix is composed of the coordinates of each pixel point of the target object in the three-dimensional camera coordinate system, wherein the X axis of the three-dimensional camera coordinate system is a vertical direction, the Y axis is a horizontal direction, and the Z axis is an optical axis direction of the RGBD camera.

3. The method of Claim 1, wherein, obtaining an optimal solution of the center point and the Euler angles by iterative solving of the gradient vector of the center point and the Euler angles through a gradient descent algorithm, and taking the optimal solution of the center point and the Euler angles as a 6D pose of the target object, specifically comprising: setting a step size, a step size attenuation factor and an iteration stop parameter of the gradient descent algorithm, and calculating the optimal solution of the center point and the Euler angles that minimize the gradient vector of the center point and the Euler angles through the set gradient descent algorithm; the optimal solution of the center point comprises: an optimal center point X axis coordinate, an optimal center point Y axis coordinate and an optimal center point Z axis coordinate; the optimal solution of the Euler angles comprises: an optimal first Euler angle, an optimal second Euler angle and an optimal third Euler angle; the optimal center point X axis coordinate, the optimal center point Y axis coordinate, the optimal center point Z axis coordinate, the optimal first Euler angle, the optimal second Euler angle and the optimal third Euler angle are taken as the 6D pose of the target object.

4. A device for fast 6D pose estimation of a target object, configured to implement the method for fast 6D pose estimation of a target object according to any one of claims 1 to 3, characterized in that, The device comprises: a binary mask acquisition module, which obtains an RGB image and a depth image of a target object, and inputs the RGB image into a segmentation model to obtain a binary mask of the target object; a curved surface point cloud matrix acquisition module, which obtains a camera intrinsic matrix, and constructs a curved surface point cloud matrix of the target object according to the depth image, the binary mask and the camera intrinsic matrix; The fitting penalty function construction module constructs a cubic bounding box of the target object, and constructs a fitting penalty function according to the curved surface point cloud matrix and the cubic bounding box; The gradient vector acquisition module calculates and obtains the gradient vector of the center point and the Euler angle of the target object according to the curved surface point cloud matrix, the cubic bounding box and the fitting penalty function; The 6D pose acquisition module iteratively solves the gradient vector of the center point and the Euler angle through a gradient descent algorithm, obtains the optimal solution of the center point and the Euler angle, and takes the optimal solution of the center point and the Euler angle as the 6D pose of the target object.

5. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor implements the 6D pose fast estimation method of the target object according to any one of claims 1 to 3 when executing the program.

6. A non-transitory computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program implements the 6D pose fast estimation method of the target object according to any one of claims 1 to 3 when executed by the processor.

Images