CN111598946B - Object pose measuring method and device and storage medium - Google Patents

Abstract

The invention discloses an object pose measuring method, a device and a storage medium, wherein the method comprises an off-line modeling stage and an on-line matching stage, wherein the off-line modeling stage is used for carrying out feature modeling on an object three-dimensional model and storing the feature modeling for measuring the pose of an object in a subsequent scene; in the on-line matching stage, object pose measurement is carried out on a given scene RGB-D image; the invention provides an efficient model sampling strategy, which can reduce the subsequent operation amount; but also can keep enough object surface change information; the distance range of the point pair characteristic is limited, and the matching interference of excessive background point clouds is reduced; a quantization expansion method is provided, so that the influence of noise on the deviation of the point on the feature calculation is reduced; edge information is extracted from the color image, candidate object poses are screened and ICP registration is carried out, measurement accuracy is improved, and therefore scene recognition rate under the conditions of shielding, gathering, stacking and the like is more accurate. The invention is widely applied to the field of three-dimensional computer vision.

Description

Method, device and storage medium for measuring object pose

technical field

The invention relates to the field of three-dimensional computer vision, in particular to an object pose measurement method, device and storage medium.

Background technique

In recent years, with the development of industrial upgrading, manufacturing automation has become an important driver of economic development, and the automatic sorting of objects by robots in manufacturing is an important manifestation of manufacturing automation. The pose of an object in three-dimensional space is an important reference for robots to identify, locate, grasp, and manipulate objects. The process of obtaining the pose of an object is called object 6D pose measurement, which is an important problem in the field of 3D computer vision. An object is transformed from A in a certain reference coordinate system _to B through rotation and translation. This rotation and translation process is recorded as T _AB . There are 6 degrees of freedom in total, so T _AB is called the 6-dimensional attitude of the object, that is, the object pose.

A method based on Point Pair Feature (PPF) (Drost et al. Model Globally, Match Locally: Efficient and Robust 3D Object Recognition. In: Conference on Computer Vision and Pattern Recognition (2010)) is widely used in objects attitude measurement. This method constructs the global features of the whole 3D model, and then extracts the features in the scene for matching. In the modeling stage, all point clouds of the model are used, which is beneficial to characterize the surface information of the entire 3D model. This method uses a 4-dimensional feature to represent the information between two points on the surface of the model. Angle composition, referred to as point pair feature (Point Pair Feature, PPF). PPF needs to be quantized and stored in the hash table to facilitate subsequent quick search. Both the model and scene data construct these features for pairing, and vote to obtain some candidate 6-dimensional poses. Next, these candidate poses are clustered, and similar poses are aggregated and averaged to obtain a more accurate pose. Next, the Iterative Closet Point (ICP) algorithm is used to refine the ICP registration of the pose to improve the accuracy of the pose.

The existing PPF-based methods have (1) the disadvantage of oversimplification of the sampling method; when the model is sampled according to a grid of a certain size, the point cloud in the same grid is simply averaged, and when the points in the grid When there is a large change in the angle between the normal vectors of the cloud, the sampling method will lose more key information of surface changes, which reduces the ability to express the surface difference information of the model. (2) Disadvantage of large amount of calculation; after the model point cloud is sampled, it is necessary to perform feature calculation on all point pairs, but in the actual scene, the part of the object under any viewing angle is often smaller than the model diameter (the model diameter refers to the border surrounding the model Diagonal length of ), there is computational redundancy. (3) The lack of robustness to point cloud noise; the point cloud captured by the camera itself has noise, which will cause a certain deviation between the position and normal vector of the point cloud, and the calculated scene features will have a certain deviation. The method cannot compensate for noise bias. (4) The shortcomings of the color-depth (RGB-D) image cannot be comprehensively utilized; this method only operates on the depth image, only uses the three-dimensional space information of the scene, and cannot obtain some information from the color image to assist attitude measurement.

Contents of the invention

In view of at least one of the above technical problems, the object of the present invention is to provide a method, device and storage medium for measuring the pose and orientation of an object.

The technical solution adopted by the present invention is: On the one hand, the embodiment of the present invention includes a method for measuring the pose of an object, including an offline modeling stage and an online matching stage;

The offline modeling phase includes:

The three-dimensional model of input object, described three-dimensional model comprises model point cloud coordinates and model point cloud normal vector;

Sampling the model point cloud coordinates and the model point cloud normal vector;

Construct a feature set in the sampled model point cloud coordinates and model point cloud normal vectors, and calculate the model point pair features;

Store the extracted model point pair features in a hash table;

The online matching phase includes:

Input the depth image, calculate the scene point cloud coordinates corresponding to each pixel of the depth image according to the camera internal reference, and calculate the scene point cloud normal vector according to the point cloud coordinates;

Sampling the scene point cloud coordinates and the scene point cloud normal vector;

Construct a feature set from the sampled scene point cloud coordinates and scene point cloud normal vectors, and calculate scene point pair features;

Quantify the extracted scene point pair features and match them with the model point pair features stored in the hash table to obtain a plurality of candidate object poses;

Input the color image to extract the scene edge point cloud;

Filtering the candidate object poses according to the edge point cloud of the scene;

Cluster the screened candidate object poses to obtain multiple preliminary object poses;

The iterative closest point algorithm is used to register the preliminary object pose to obtain the final object pose.

Further, the step of sampling the model point cloud coordinates and the model point cloud normal vector specifically includes:

According to the model point cloud coordinates, calculate the bounding box surrounding the model point cloud, and obtain the model point cloud space;

Rasterize the model point cloud space to obtain multiple grids of equal size, each of which contains multiple point clouds, and each point cloud contains corresponding model point cloud coordinates and model point cloud normal vectors ;

Clustering the point clouds contained in each grid according to the size of the angle between the model point cloud normal vectors;

The model point cloud coordinates and model point cloud normal vectors in each cluster are averaged to obtain the model point cloud coordinates and model point cloud normal vectors after each raster sampling.

Further, the step of constructing a feature set in the sampled model point cloud coordinates and model point cloud normal vectors and calculating model point pair features specifically includes:

Construct a K-D tree for the sampled model point cloud coordinates;

Selecting a reference point, the reference point being any point in the model point cloud coordinates obtained by sampling;

Searching for a target point in the K-D tree, where the target point is a point whose distance from the reference point is less than a first threshold;

The model point pair features formed by the reference point and the target point are calculated sequentially.

Further, the step of storing the extracted model point pair features into a hash table specifically includes:

Carrying out quantitative processing on the extracted features of the model points;

Obtaining a key value from the quantization result through a hash function as an index of the point pair feature in the hash table;

Point-pair features with the same index are stored in the same bucket of the hash table, and point-pair features with different indexes are stored in different buckets of the hash table.

Further, the step of quantifying the extracted feature of the scene point pair and matching with the feature of the model point pair stored in the hash table, and obtaining a plurality of candidate object poses specifically includes:

Quantifying the features of the extracted scene points;

Extend the quantization results to compensate for feature shifts caused by noise;

Use the expanded multiple result values as key values, and look for model point pair features with the same key value in the hash table;

According to the point pair features of the model, multiple candidate object poses are obtained.

Further, the step of extracting scene edge point cloud from the input color image specifically includes:

Grayscale the color image;

Use an edge detector to perform edge detection on the grayscaled image;

Correspond the pixels at the edge of the image with the depth image one by one, and calculate the spatial coordinates of the pixels according to the internal parameters of the camera;

The spatial coordinates are extracted as a scene edge point cloud.

Further, the step of screening the candidate object poses according to the scene edge point cloud specifically includes:

Projecting the three-dimensional model of the object corresponding to the pose of the candidate object to the imaging plane according to the camera internal reference, and obtaining the edge point cloud of the three-dimensional model;

Select any point in the edge point cloud of the three-dimensional model as a reference point, find a corresponding matching point in the scene edge point cloud, and the matching point is the point closest to the reference point;

Calculate the first distance, the first distance is the distance from the matching point to the reference point, if the first distance is less than the second threshold, then keep the matching point, otherwise discard the matching point;

Calculate the ratio of the number of retained matching points to the total number of points in the edge point cloud of the 3D model, if the ratio is greater than a third threshold, keep the candidate object pose corresponding to the 3D model, otherwise discard it.

Further, the step of clustering the screened candidate object poses to obtain multiple preliminary object poses specifically includes:

Selecting any one of the selected candidate object poses as the first candidate object pose;

Calculate the distance between the first candidate object pose and other candidate object-free poses obtained through screening;

Initializing the candidate object poses obtained by the screening to a corresponding number of clusters;

According to the hierarchical clustering method, the screened candidate object poses are clustered;

In each cluster, the candidate object pose with the highest voting score is extracted to obtain multiple preliminary object poses.

On the other hand, the embodiment of the present invention also includes a device for measuring the pose of an object, including a memory and a processor, the memory is used to store at least one program, and the processor is used to load the at least one program to execute the A method for measuring the pose of an object.

On the other hand, the embodiment of the present invention also includes a storage medium, which stores processor-executable instructions, and the processor-executable instructions are used to execute the object pose when executed by the processor. Measurement methods.

The beneficial effects of the present invention are: (1) the present invention provides a more efficient model sampling strategy, which reduces the number of point clouds, thereby reducing the amount of follow-up calculations; and retaining sufficient surface change information; (2) limiting The distance range when calculating point pair features is reduced, the calculation amount of point pair features of model and scene data is reduced, and the matching interference of too many background point clouds is also reduced; (3) a quantitative expansion method is proposed to reduce noise on point pair Feature calculation produces offset effects; (4) Introduce color image information, increase input information, and extract edge information from color images, screen candidate object poses and perform ICP registration to improve measurement accuracy, thus for occlusion, The scene recognition rate in the case of aggregation, stacking, etc. is more accurate.

Description of drawings

Fig. 1 is a schematic flow chart of an object pose measurement method described in an embodiment;

Fig. 2 is the flow chart of the steps of the off-line modeling stage described in the embodiment;

Fig. 3 is a flow chart of steps in the online matching stage described in the embodiment.

Detailed ways

As shown in Figure 1, this embodiment includes a method for measuring the pose of an object, including an offline modeling stage and an online matching stage;

The offline modeling phase includes:

The three-dimensional model of input object, described three-dimensional model comprises model point cloud coordinates and model point cloud normal vector;

Sampling the model point cloud coordinates and the model point cloud normal vector;

Construct a feature set in the sampled model point cloud coordinates and model point cloud normal vectors, and calculate the model point pair features;

Store the extracted model point pair features in a hash table;

The online matching phase includes:

Input the depth image, calculate the scene point cloud coordinates corresponding to each pixel of the depth image according to the camera internal reference, and calculate the scene point cloud normal vector according to the point cloud coordinates;

Sampling the scene point cloud coordinates and the scene point cloud normal vector;

Construct a feature set from the sampled scene point cloud coordinates and scene point cloud normal vectors, and calculate scene point pair features;

Quantify the extracted scene point pair features and match them with the model point pair features stored in the hash table to obtain a plurality of candidate object poses;

Input the color image to extract the scene edge point cloud;

Filtering the candidate object poses according to the edge point cloud of the scene;

Cluster the screened candidate object poses to obtain multiple preliminary object poses;

The iterative closest point algorithm is used to register the preliminary object pose to obtain the final object pose.

In this embodiment, the offline modeling stage is mainly to perform feature modeling on the 3D model of the object and store it for subsequent scene attitude measurement; while the online matching stage is mainly to perform object pose measurement on a given scene RGB-D image. Measurement.

Further, referring to Figure 2, the offline modeling phase includes the following steps:

S1. Input the three-dimensional model of the object, the three-dimensional model includes the coordinates of the model point cloud and the normal vector of the model point cloud;

S2. Sampling the model point cloud coordinates and the model point cloud normal vector;

S3. Construct a feature set in the sampled model point cloud coordinates and model point cloud normal vectors, and calculate model point pair features;

S4. Store the extracted model point pair features in a hash table.

In this embodiment, for the offline modeling stage, since there is no need to input scene images, it is called the offline modeling stage; step S1 of this stage is to input the 3D model of the object, and the 3D model does not need to have textures, colors, etc. Information, only need to keep point cloud coordinates and point cloud normal vector, such as CAD model obtained by computer modeling or 3D model obtained by 3D reconstruction technology, this process can reduce the complexity of 3D model modeling.

Step S2, that is, the step of sampling the model point cloud coordinates and the model point cloud normal vector, specifically includes the following steps:

S201. According to the model point cloud coordinates, calculate the bounding box surrounding the model point cloud, and obtain the model point cloud space;

S202. Rasterize the model point cloud space to obtain multiple grids of equal size, each grid contains multiple point clouds, and each point cloud contains corresponding model point cloud coordinates and model point clouds normal vector;

S203. Clustering the point clouds contained in each grid according to the size of the angle between the model point cloud normal vectors;

S204. Average the model point cloud coordinates and model point cloud normal vectors in each cluster to obtain the model point cloud coordinates and model point cloud normal vectors of each grid sampled.

通过这种采样策略，既减少了后续运算量，又降低了采样策略丢失表面变化信息的影响，且点对之间的辨识度信息也能够被保留下来。In this embodiment, the bounding box surrounding the point cloud is calculated according to the maximum and minimum values of the model point cloud coordinates on the X, Y, and Z axes to obtain the model point cloud space, and the diagonal diameter of the bounding box is recorded as the model diameter d _m ; Rasterize the model point cloud space, each grid is a small cube, and each grid has the same size. The size of the grid is set to τ×d _m , and τ is the sampling coefficient, which is set to 0.05; each grid contains multiple point clouds, and each point cloud contains the corresponding model point cloud coordinates and model point cloud normal vector; for For each grid, we cluster the point clouds in it according to the angle between the normal vectors of the point cloud, and the angle does not exceed the threshold Δθ to belong to the same cluster, and then the model points in each cluster Cloud coordinates and model point cloud normal vectors are averaged to obtain the model point cloud coordinates and model point cloud normal vectors after each grid sampling. This sampling strategy is adopted for all grids in the model space, where Δθ is generally set to

Through this sampling strategy, it not only reduces the amount of follow-up calculations, but also reduces the influence of the sampling strategy on the loss of surface change information, and the identification information between point pairs can also be preserved.

Step S3, that is, the step of constructing a feature set from the sampled model point cloud coordinates and model point cloud normal vectors, and calculating the model point pair features, specifically includes the following steps:

S301. Construct a K-D tree for the sampled model point cloud coordinates;

S302. Select a reference point, where the reference point is any point in the sampled model point cloud coordinates;

S303. Search for a target point in the K-D tree, where the target point is a point whose distance from the reference point is less than a first threshold;

S304. Sequentially calculate the model point pair features formed by the reference point and the target point.

d_min和d_med分别是模型边界框的两条短边。在实际场景的大多数视角下，模型的可见部分长度往往小于d_m但又大于d_min，因此对场景中距离较远的点对构建特征时，会把较多的背景点云算进去，增加算法的误识别率，也增加了构建特征的计算量，因此，本实施例中选用d_range作为距离上限来减少上述影响，也就是说，该过程限定了计算点对特征时的距离范围，减少了模型和场景数据的点对特征计算量，也降低了过多背景点云的匹配干扰。In this embodiment, a KD tree is constructed for the sampled model point cloud coordinates to facilitate the subsequent fast distance search; for each point after sampling, record it as a reference point one by one, and then search for the target point in the KD tree, The target point is a point whose distance from the reference point is less than d _range , where d _range is the first threshold, and the model point pair features formed by the reference point and the target point are calculated in sequence. The specific calculation process is: record the coordinates of the reference point as m _r , the normal vector corresponding to the reference point is n _r , the target point is m _s , the normal vector corresponding to the target point is n _s , and the model point formed by the reference point and the target point For feature F _r,s ＝(||d _r,s ||,∠(n _r ,d _r,s ),∠(n _s ,d _r,s ),∠(n _r ,n _s )), where d _{r, s} represents the distance vector from point m _r to point m _s , ||d _{r, s} || represents the distance of d _{r, s} , ∠(n _r , d _{r, s} ) represents the normal vector between n _r and The angle between the distance vector d _{r, s} , and so on, ∠(n _s , d _{r, s} ) represents the angle between the normal vector n _s and the distance vector d _{r, s} , ∠(n _r , n _s ) represents the normal vector The angle between n _r and the normal vector n _s . Among them, the first threshold

d _min and d _med are the two short sides of the model bounding box, respectively. In most viewing angles of the actual scene, the length of the visible part of the model is often less than d _m but greater than d _min , so when constructing features for point pairs with far distances in the scene, more background point clouds will be included, increasing The misrecognition rate of the algorithm also increases the amount of calculation for constructing features. Therefore, in this embodiment, d _range is selected as the upper limit of the distance to reduce the above-mentioned impact. That is to say, this process limits the distance range when calculating the point pair feature, reducing It reduces the amount of point-to-feature calculations for model and scene data, and also reduces the matching interference of too many background point clouds.

Step S4, that is, the step of storing the extracted model point pair features into a hash table, specifically includes the following steps:

S401. Quantify the extracted model point pair features;

S402. Obtain a key value from the quantization result through a hash function, and use it as an index of the point pair feature in the hash table;

S403. Store point pair features with the same index in the same bucket of the hash table, and store point pair features with different indexes in different buckets of the hash table.

其中一般设置Δd＝0.05d_m，

将量化后的值Q_r,s通过哈希函数求出一个键值，所述键值为一个非负整数，作为该点对特征在哈希表中的索引，具有相同索引的点对特征存储在哈希表的同一个桶中，不同索引的点对6特征则存储于哈希表的不同桶中。In this embodiment, for the extracted model point pair feature F _r,s =(||d _r,s ||,∠(n _r ,d _r,s ),∠(n _s ,d _r,s ),∠(n _r ,n _s )) to quantify, and get

where Δd=0.05d _m is generally set,

Calculate the quantized value Q _{r, s} through a hash function to obtain a key value, the key value is a non-negative integer, as the index of the point pair feature in the hash table, and store the point pair feature with the same index In the same bucket of the hash table, point-to-6 features of different indexes are stored in different buckets of the hash table.

Regarding the online matching stage, the color image and the depth image of the scene need to be input respectively. Referring to Figure 3, this stage includes the following steps:

D1. Input the depth image, calculate the scene point cloud coordinates corresponding to each pixel of the depth image according to the camera internal parameters, and calculate the scene point cloud normal vector according to the point cloud coordinates;

D2. Sampling the scene point cloud coordinates and the scene point cloud normal vector;

D3. Construct a feature set from the sampled scene point cloud coordinates and scene point cloud normal vectors, and calculate scene point pair features;

D4. Quantify the extracted scene point pair features and match them with the model point pair features stored in the hash table to obtain a plurality of candidate object poses;

D5. Input the color image to extract the scene edge point cloud;

D6. Screening the candidate object poses according to the scene edge point cloud;

D7. Clustering the screened candidate object poses to obtain multiple preliminary object poses;

D8. Using the iterative closest point algorithm to register the preliminary object pose to obtain the final object pose.

其中，u、v是点在成像平面的坐标，X、Y、Z是点在相机坐标系下的3维坐标，f_x、f_y、c_x、c_y是相机内参。因此依照相机内参可以计算出输入的深度图像中每个像素点对应的3维空间坐标，也就是场景点云坐标，并根据所述场景点云坐标可估算出相应的场景点云法向量。In step D1, according to the camera imaging formula:

Among them, u, v are the coordinates of the point on the imaging plane, X, Y, Z are the 3-dimensional coordinates of the point in the camera coordinate system, and f _x , f _y , c _x , _cy are internal camera parameters. Therefore, the 3D space coordinates corresponding to each pixel in the input depth image can be calculated according to the internal camera parameters, that is, the scene point cloud coordinates, and the corresponding scene point cloud normal vector can be estimated according to the scene point cloud coordinates.

Regarding step D2, that is, the process of sampling the scene point cloud coordinates and scene point cloud normal vectors, in this embodiment, the same adoption strategy as step S2 of offline modeling is adopted, and the scene point cloud coordinates and scene point cloud normal vectors are sampled. The scene point cloud normal vector is sampled. This process can also reduce the number of point clouds, thereby reducing the amount of subsequent calculations; and can retain sufficient surface change information of the object.

个参考点；(3)对于每个参考点，在K-D树中查找与参考点距离小于d_range的点，并构造成场景点对特征，d_range的设置与场景点对特征的计算与离线建模阶段的步骤S3相同，在此不再重复赘述。Similarly, regarding step D3, this embodiment adopts the same method as step S3 in the offline modeling stage to calculate scene point pair features; it specifically includes: (1) constructing a KD tree for the scene point cloud coordinates sampled in step D2; (2) Assuming that the number of scene point cloud coordinates obtained by sampling in step D2 is N, one point is taken as a reference point for every n points, and there are a total of

(3) For each reference point, search for a point in the KD tree whose distance from the reference point is less than d _range , and construct a scene point pair feature, the setting of d _range and the calculation and offline construction of the scene point pair feature The step S3 of the modeling stage is the same, and will not be repeated here.

Step D4, that is, the step of quantifying the extracted scene point pair features and matching with the model point pair features stored in the hash table to obtain multiple candidate object poses, specifically includes the following steps:

D401. Quantify the features of the extracted scene points;

D402. Expand the quantization result to compensate for the feature shift caused by noise;

D403. Use the expanded multiple result values as key values, and look for model point pair features with the same key value in the hash table;

D404. Obtain multiple candidate object poses according to the model point pair features.

i＝1,2,3,4，其中e_i为第i维量化误差，F_i为F的每一维的值，Δ为量化间隔。以i＝1为例，当

时，Q_new＝(Q₁-1,Q₂,Q₃,Q₄)；当

时，Q_new＝(Q₁+1,Q₂,Q₃,Q₄)；当

时，不对Q进行扩充；按照此方式，一个场景点对特征F最多可以量化为16个量化特征，以此补偿噪声引起的特征偏移；(3)设第i个参考点为s_i，s_i构建了n_i个点对特征。对于s_i，创建一个投票矩阵，矩阵的每一行代表图像场景点云中的点，每一列代表量化的旋转角度，角度间隔一般取为

矩阵的每个坐标(m,α)表示图像场景中的点m的法向量旋转至与X轴平行，然后平移到原点之后，点m与其他点的连线还需绕着X轴旋转角度α，矩阵初始化为全0矩阵；(4)将扩充后的多个Q值作为键值，在哈希表中进行查找，寻找具有相同键值的模型点对特征。对于查找到的每个点对特征，计算与图像场景中的点m′和要旋转的角度α′，将投票矩阵

的值加1；(5)每个参考点s_i的投票过程结束后，提取出投票矩阵中最大值对应的行列坐标(m,α)，与计算得出图像场景的物体位姿，(m,α)处的值记为该姿态的分数；(6)将所有参考点计算得到的图像场景的物体位姿保留下来作为候选物体位姿。In this embodiment, the scene points calculated in step D3 are quantized to feature set F=(F ₁ , F ₂ , F ₃ , F ₄ ), the quantization method is the same as the quantization method in offline modeling step S4, and the quantization method is obtained The final 4-dimensional feature Q=(Q ₁ , Q ₂ , Q ₃ , Q ₄ ); (2) In order to reduce the impact of noise on quantization matching, this embodiment expands the quantized value Q in the following manner. set up,

i=1,2,3,4, where e _i is the quantization error of the i-th dimension, F _i is the value of each dimension of F, and Δ is the quantization interval. Take i=1 as an example, when

, Q _new ＝(Q ₁ -1,Q ₂ ,Q ₃ ,Q ₄ ); when

, Q _new ＝(Q ₁ +1,Q ₂ ,Q ₃ ,Q ₄ ); when

When , Q is not expanded; according to this method, a scene point can be quantized into 16 quantized features at most for the feature F, so as to compensate for the feature shift caused by noise; (3) Let the i-th reference point be s _i , s _i constructs n _i point pair features. For s _i , create a voting matrix. Each row of the matrix represents a point in the image scene point cloud, and each column represents a quantized rotation angle. The angle interval is generally taken as

Each coordinate (m, α) of the matrix indicates that the normal vector of point m in the image scene is rotated to be parallel to the X axis, and then translated to the origin, the connection between point m and other points needs to be rotated around the X axis by an angle α , the matrix is initialized as a matrix of all 0s; (4) Use multiple expanded Q values as key values to search in the hash table to find the model point pair features with the same key value. For each point pair feature found, calculate the point m' in the image scene and the angle α' to be rotated, and convert the voting matrix

(5) After the voting process of each reference point s _i is over, extract the row and column coordinates (m, α) corresponding to the maximum value in the voting matrix, and calculate the object pose of the image scene, (m , α) is recorded as the score of the pose; (6) The object pose of the image scene calculated from all reference points is retained as the candidate object pose.

Step D5, that is, the step of inputting the color image to extract the point cloud of the edge of the scene, specifically includes the following steps:

D501. Grayscale the color image;

D502. Use an edge detector to perform edge detection on the grayscaled image;

D503. Correspond the pixels at the edge of the image with the depth image one by one, and calculate the spatial coordinates of the pixels according to the internal parameters of the camera;

D504. Extracting the space coordinates as a scene edge point cloud.

In this embodiment, after the input color image is grayscaled, the Canny edge detector is used to perform edge detection to extract the edge pixels of the image, and the pixels at the edge are in one-to-one correspondence with the depth image, and are calculated according to the internal parameters of the camera The spatial coordinates of the pixels are obtained, and these pixels are called the scene edge point cloud.

Step D6, that is, the step of screening the candidate object poses according to the scene edge point cloud, specifically includes the following steps:

D601. According to the camera internal reference, project the three-dimensional model of the object corresponding to the pose of the candidate object to the imaging plane, and obtain the edge point cloud of the three-dimensional model;

D602. Select any point in the edge point cloud of the three-dimensional model as a reference point, and find a corresponding matching point in the scene edge point cloud, and the matching point is the point closest to the reference point;

D603. Calculate the first distance, the first distance is the distance from the matching point to the reference point, if the first distance is smaller than the second threshold, keep the matching point, otherwise discard the matching point;

D604. Calculate the ratio of the number of retained matching points to the total number of points in the edge point cloud of the 3D model, if the ratio is greater than the third threshold, keep the candidate object pose corresponding to the 3D model, otherwise discard .

In this embodiment, for each candidate object pose obtained in step D4, the 3D model corresponding to the candidate object pose is projected onto the imaging plane according to the camera internal parameters, and the edge of the corresponding 3D model under each candidate object pose is obtained Point cloud; for each point in the edge point cloud of the three-dimensional model, find the nearest point in the scene edge point cloud extracted by step D5, if the shortest distance is less than d _ε , that is, the second threshold, the first The second threshold d _ε is generally set to 0.1d _m , then the point is said to be correctly matched and called a matching point; the ratio of the number of retained matching points to the total number of points in the edge point cloud of the 3D model is counted, if If the ratio is greater than the third threshold, the candidate object pose corresponding to the 3D model is kept, otherwise discarded.

Step D7, that is, the step of clustering the screened candidate object poses to obtain multiple preliminary object poses, specifically includes the following steps:

D701. Selecting any one of the selected candidate object poses as the first candidate object pose;

D702. Calculate the distances between the first candidate object pose and other candidate object-free poses obtained through screening;

D703. Initialize the candidate object poses obtained through the screening into clusters of corresponding numbers;

D704. Clustering the screened candidate object poses according to the method of hierarchical clustering;

D705. Extract the candidate object pose with the highest voting score in each cluster to obtain multiple preliminary object poses.

In this embodiment, for the candidate object poses obtained after screening in step D6, the distance between each pair is calculated. The distance D between poses consists of two parts: displacement difference Δdist and rotation difference Δrot, where Δdist< d _cluster and Δrot<rot _cluster , that is, the displacement difference Δdist is smaller than the spatial distance threshold d _cluster between clusters, the spatial distance threshold d _cluster is generally set to one-tenth of the object diameter, and the rotation difference Δrot is smaller than the cluster The rotation angle threshold rot _cluster between classes is generally set to 30 degrees; if there are N candidate object _poses obtained after screening in step D6, each of the N poses is initialized into N clusters, according to the hierarchy The clustering method aggregates adjacent clusters together until the distance between any two clusters is greater than the threshold, and the clustering ends; the candidate object pose with the highest number of votes in each cluster is extracted, and multiple a preliminary object pose.

Finally, for the multiple preliminary object poses obtained in step D7, this embodiment further obtains the model contour point cloud under each preliminary object pose, and extracts each of the contour point clouds in step D5 to obtain the scene edge point cloud Perform ICP registration to obtain the final object pose with high accuracy.

In summary, an object pose measurement method in the embodiment of the present invention has the following advantages:

(1) The embodiment of the present invention provides a more efficient model sampling strategy, which reduces the number of point clouds, thereby reducing the amount of subsequent calculations; and retaining sufficient surface change information; (2) limiting the calculation of point pair features The distance range of time reduces the amount of point-to-feature calculations of the model and scene data, and also reduces the matching interference of too many background point clouds; (3) A quantitative expansion method is proposed to reduce the offset of noise to point-to-feature calculations (4) Introduce color image information, increase input information, and extract edge information from color images, screen candidate object poses and perform ICP registration, improve measurement accuracy, and thus for occlusion, aggregation, stacking, etc. The lower scene recognition rate is more accurate.

In this embodiment, the device for measuring the pose of an object includes a memory and a processor, the memory is used to store at least one program, and the processor is used to load the at least one program to execute the object position method of attitude measurement.

The memory can also be produced separately and used to store a computer program corresponding to the method for measuring the pose of an object. When the memory is connected to the processor, the computer program stored in it will be read and executed by the processor, so as to implement the method for measuring the pose of an object and achieve the technical effect described in the embodiment.

It should be noted that, unless otherwise specified, when a feature is called "fixed" or "connected" to another feature, it can be directly fixed and connected to another feature, or indirectly fixed and connected to another feature. on a feature. In addition, descriptions such as up, down, left, and right used in the present disclosure are only relative to the mutual positional relationship of the components of the present disclosure in the drawings. As used in this disclosure, the singular forms "a", "the", and "the" are intended to include the plural forms as well, unless the context clearly dictates otherwise. In addition, unless otherwise defined, all technical and scientific terms used in this embodiment have the same meaning as commonly understood by those skilled in the art. The terms used in the description of this embodiment are only for describing specific embodiments, not for limiting the present invention. The term "and/or" used in this embodiment includes any combination of one or more related listed items.

It should be understood that although the terms first, second, third etc. may be used in the present disclosure to describe various elements, these elements should not be limited by these terms. These terms are only used to distinguish elements of the same type from one another. For example, a first element could be termed a second element, and, similarly, a second element could be termed a first element, without departing from the scope of the present disclosure. The use of any and all examples, or exemplary language ("such as", "such as", etc.) provided in the examples is intended merely to better illuminate the examples of the invention and will not cast a shadow on the scope of the invention unless otherwise claimed impose restrictions.

It should be appreciated that embodiments of the invention may be realized or implemented by computer hardware, a combination of hardware and software, or by computer instructions stored in a non-transitory computer readable memory. The methods can be implemented in a computer program using standard programming techniques - including a non-transitory computer-readable storage medium configured with a computer program, where the storage medium so configured causes the computer to operate in a specific and predefined manner - according to the specific Methods and Figures described in the Examples. Each program can be implemented in a high-level procedural or object-oriented programming language to communicate with the computer system. However, the programs can be implemented in assembly or machine language, if desired. In any case, the language may be a compiled or interpreted language. Furthermore, the program can be run on an application specific integrated circuit programmed for this purpose.

Furthermore, operations of processes described in this embodiment may be performed in any suitable order unless otherwise indicated by this embodiment or otherwise clearly contradicted by context. The processes described in this embodiment (or variants and/or combinations thereof) can be executed under the control of one or more computer systems configured with executable instructions, and can be executed as code jointly executed on one or more processors (eg, executable instructions, one or more computer programs, or one or more applications), hardware or a combination thereof. The computer program comprises a plurality of instructions executable by one or more processors.

Further, the method can be implemented in any type of computing platform operably connected to a suitable one, including but not limited to personal computer, minicomputer, main frame, workstation, network or distributed computing environment, stand-alone or integrated computer platform, or communicate with charged particle tools or other imaging devices, etc. Aspects of the invention can be implemented as machine-readable code stored on a non-transitory storage medium or device, whether removable or integrated into a computing platform, such as a hard disk, optically read and/or written storage medium, RAM, ROM, etc., such that they are readable by a programmable computer, when the storage medium or device is read by the computer, can be used to configure and operate the computer to perform the processes described herein. Additionally, the machine-readable code, or portions thereof, may be transmitted over a wired or wireless network. The invention described in this embodiment includes these and other different types of non-transitory computer-readable storage media when such media include instructions or programs for implementing the steps described above in conjunction with a microprocessor or other data processor. The invention also includes the computer itself when programmed according to the methods and techniques described herein.

Computer programs can be applied to input data to perform the functions described in this embodiment, thereby transforming the input data to generate output data stored to non-volatile memory. Output information may also be applied to one or more output devices such as a display. In a preferred embodiment of the invention, the transformed data represents physical and tangible objects, including specific visual depictions of physical and tangible objects produced on a display.

The above is only a preferred embodiment of the present invention, and the present invention is not limited to the above-mentioned implementation, as long as it achieves the technical effect of the present invention by the same means, within the spirit and principles of the present invention, any Any modification, equivalent replacement, improvement, etc., shall be included within the protection scope of the present invention. Various modifications and changes may be made to the technical solutions and/or implementations within the protection scope of the present invention.

Claims (9)

1. An object pose measurement method is characterized by comprising an off-line modeling stage and an on-line matching stage;

the offline modeling phase comprises:

inputting a three-dimensional model of an object, wherein the three-dimensional model comprises model point cloud coordinates and a model point cloud normal vector;

sampling the model point cloud coordinates and the model point cloud normal vector;

constructing a feature set in the model point cloud coordinates and the model point cloud normal vectors obtained by sampling, and calculating the model point pair features;

storing the extracted model point pair characteristics into a hash table;

the online matching stage comprises:

inputting a depth image, calculating scene point cloud coordinates corresponding to each pixel point of the depth image according to camera internal parameters, and calculating a scene point cloud normal vector according to the point cloud coordinates;

sampling the scene point cloud coordinates and the scene point cloud normal vector;

constructing a feature set in the scene point cloud coordinates and the scene point cloud normal vectors obtained by sampling, and calculating scene point pair features;

quantizing the extracted scene point pair features and matching the scene point pair features with the model point pair features stored in a hash table to obtain a plurality of candidate object poses;

inputting a color image and extracting a scene edge point cloud;

screening the candidate object poses according to the scene edge point cloud;

clustering candidate object poses obtained by screening to obtain a plurality of preliminary object poses;

registering the initial object pose by using an iterative closest point algorithm to obtain a final object pose;

the step of quantifying the extracted scene point pair features and matching the quantified scene point pair features with the model point pair features stored in the hash table to obtain a plurality of candidate object poses specifically includes:

quantizing the extracted scene points to obtain features;

expanding the quantization result to compensate for the characteristic offset caused by the noise;

using the expanded result values as key values, and searching model point pair characteristics with the same key values in a hash table;

acquiring a plurality of candidate object poses according to the model point pair characteristics;

the quantization result Q is augmented in the following way:

is provided with

Wherein e _i For quantization error of the ith dimension, F _i Is the value of each dimension of F, Δ is the quantization interval; when i =1, when

When Q is _new ＝(Q ₁ -1,Q ₂ ,Q ₃ ,Q ₄ ) When is coming into contact with

When is, Q _new ＝(Q ₁ +1,Q ₂ ,Q ₃ ,Q ₄ ) When is coming into contact with

When the Q is not expanded, the Q is not expanded; wherein Q is _new The quantized result after the expansion is obtained.

2. The method for measuring the pose of an object according to claim 1, wherein the step of sampling the coordinates of the model point cloud and the normal vector of the model point cloud specifically comprises:

calculating a boundary frame surrounding the model point cloud according to the model point cloud coordinates to obtain a model point cloud space;

rasterizing the model point cloud space to obtain a plurality of grids with equal sizes, wherein each grid comprises a plurality of point clouds, and each point cloud comprises a corresponding model point cloud coordinate and a model point cloud normal vector;

clustering point clouds contained in each grid according to the size of an included angle between normal vectors of the model point clouds;

and averaging the model point cloud coordinates and the model point cloud normal vectors in each cluster to obtain the model point cloud coordinates and the model point cloud normal vectors after each grid sampling.

3. The method for measuring the pose of an object according to claim 1, wherein the step of constructing a feature set in the model point cloud coordinates and the model point cloud normal vectors obtained by sampling and calculating the model point pair features specifically comprises:

constructing a K-D tree for the sampled model point cloud coordinates;

selecting a reference point, wherein the reference point is any point in a model point cloud coordinate obtained by sampling;

searching a target point in the K-D tree, wherein the distance between the target point and the reference point is less than a first threshold value;

and sequentially calculating the model point pair characteristics formed by the reference points and the target points.

4. The object pose measurement method according to claim 1, wherein the step of storing the extracted model point pair features in a hash table specifically comprises:

quantizing the extracted model points to the features;

solving a key value of the quantization result through a hash function, and taking the key value as an index of the point pair characteristics in a hash table;

point pair characteristics with the same index are stored in the same bucket of the hash table, and point pair characteristics with different indexes are stored in different buckets of the hash table.

5. The object pose measurement method according to claim 1, wherein the step of extracting a scene edge point cloud from the input color image specifically comprises:

graying the color image;

performing edge detection on the grayed image by using an edge detector;

corresponding pixels at the edge of the image to the depth image one by one, and calculating the spatial coordinates of pixel points according to camera internal parameters;

and extracting the space coordinates to be used as a scene edge point cloud.

6. The method for measuring the pose of an object according to claim 1, wherein the step of screening the candidate object poses according to the scene edge point cloud specifically comprises:

projecting the object three-dimensional model corresponding to the candidate object pose to an imaging plane according to camera internal parameters to obtain an edge point cloud of the three-dimensional model;

selecting any point from the edge point cloud of the three-dimensional model as a reference point, and finding out a corresponding matching point from the scene edge point cloud, wherein the matching point is the point closest to the reference point;

calculating a first distance, wherein the first distance is the distance from a matching point to a reference point, if the first distance is smaller than a second threshold value, the matching point is reserved, otherwise, the matching point is discarded;

and calculating the proportion of the number of the reserved matching points to the total number of the points in the edge point cloud of the three-dimensional model, if the proportion is greater than a third threshold value, reserving the candidate object pose corresponding to the three-dimensional model, and otherwise, discarding the candidate object pose.

7. The object pose measurement method according to claim 1, wherein the step of clustering candidate object poses obtained by screening to obtain a plurality of preliminary object poses specifically comprises:

selecting any one of the candidate object poses obtained by screening as a first candidate object pose;

respectively calculating the distance between the first candidate object pose and the other candidate object-free poses obtained by screening;

respectively initializing the candidate object poses obtained by screening into clusters with corresponding numbers;

clustering candidate object poses obtained by screening according to a hierarchical clustering method;

and extracting candidate object poses with the highest voting scores in each cluster to obtain a plurality of preliminary object poses.

8. An apparatus for object pose measurement, comprising a memory for storing at least one program and a processor for loading the at least one program to perform the method of any one of claims 1-7.

9. A storage medium having stored therein processor-executable instructions, which when executed by a processor, are for performing the method of any one of claims 1-7.

Images