Object pose estimation method

Abstract

The invention discloses an object pose estimation method. The method comprises the following steps: acquiring a first image of a to-be-measured object; when the matching relationship between the first image and the two-dimensional image template meets a first condition, adjusting first point cloud information corresponding to the first image in a first plane to obtain second point cloud information; determining a second image corresponding to the second point cloud information, and calculating a matching relationship between the second image and the point cloud projection template; and obtaining initial pose information based on a matching relationship between the second image and the point cloud projection template, and performing ICP fine registration on the initial pose information to determine pose information of the to-be-measured object. According to the method, the two-dimensional image template is preliminarily matched with the first image, and then the second image is matched with the point cloud projection template, so that a relatively accurate initial pose is obtained to assist in 3D point cloud registration, falling into a local optimal solution is avoided, and registration is more accurate; according to the method, the reliability of pose registration is optimized, the number of iterations of an algorithm during registration is reduced through early-stage difference calculation, time consumption is reduced, and calculation cost is reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of object pose estimation, and in particular to an object pose estimation method. Background Art

[0002] In the rapidly developing field of computer vision, object pose detection is one of the core research topics and its importance is becoming increasingly prominent. Object pose, that is, the position and posture of an object in space, is key information for understanding the relationship between an object and its surrounding environment and achieving precise interaction. This technology is widely used in many cutting-edge fields such as robot navigation, virtual reality (VR), augmented reality (AR), etc., providing strong support for the development of intelligent and automated technologies.

[0003] In recent years, with the rapid progress of 3D sensing technology, such as the popularization of devices such as LiDAR and RGB-D cameras, object position recognition technology has ushered in unprecedented development opportunities. These high-precision and high-efficiency 3D data acquisition tools make it possible to accurately detect and identify the position of objects in a wide range of fields such as industrial automation, augmented reality experience, autonomous driving systems, and aerospace exploration, while also raising more urgent demands.

[0004] However, despite its broad application prospects, current object pose detection technology still faces many challenges. Traditional point cloud registration algorithms, such as the iterative closest point algorithm (ICP), have extremely high computational complexity when processing large-scale point cloud data, resulting in long processing time. In addition, due to the limitations of the algorithm itself, it is easy to fall into the local optimal solution, thus affecting the accuracy and stability of pose detection. On the other hand, with the rise of "deep learning" technology, researchers have begun to explore new ways to use neural network models for object pose detection. Although deep learning has shown powerful capabilities in feature extraction and pattern recognition, this method places high demands on hardware computing power and requires a large number of well-labeled training samples. In practical applications, the acquisition of high-quality training data is often costly, time-consuming and labor-intensive, which has become a major bottleneck restricting the widespread application of deep learning in the field of object pose detection.

[0005] In summary, although object pose detection technology has shown great application potential in many fields, existing methods still have shortcomings in processing efficiency, accuracy, and resource requirements. Summary of the invention

[0006] The purpose of the present invention is to provide a method for estimating the pose of an object to solve the problems in the prior art that the pose estimation method has high computational complexity and is time-consuming when processing large-scale point clouds, and the deep learning algorithm requires a large number of samples, resulting in high actual application costs.

[0007] To achieve one of the above-mentioned purposes of the invention, an embodiment of the present invention provides an object pose estimation method, the method comprising: acquiring a first image of the object to be measured; when the matching relationship between the first image and a preset two-dimensional image template satisfies a first condition, adjusting the first point cloud information corresponding to the first image on a first plane to obtain second point cloud information; determining a second image corresponding to the second point cloud information, and calculating the matching relationship between the second image and the preset point cloud projection template; obtaining initial pose information based on the matching relationship between the second image and the preset point cloud projection template, and performing ICP precise alignment on the initial pose information to determine the pose information of the object to be measured.

[0008] As a further improvement of the present invention, the method also includes: when the matching relationship between the first image and the preset two-dimensional image template satisfies a first condition, adjusting the first point cloud information corresponding to the first image on the first plane specifically includes: matching the first image with the preset two-dimensional image template and obtaining a first matching value; extracting the first point cloud information of the first image when the first matching value satisfies the first condition; and performing horizontal correction and translation transformation processing on the first point cloud information.

[0009] As a further improvement of the present invention, the method also includes: horizontal correction of the first point cloud information includes: obtaining the normal vector of the first point cloud information, and calculating the rotation angle between the two based on the normal vector of a preset direction; obtaining the rotation matrix by the Rodriguez rotation formula and rotating the first point cloud information.

[0010] As a further improvement of the present invention, the method also includes: the translation transformation of the first point cloud information includes: obtaining the center of mass of the first point cloud information after horizontal correction; calculating the translation matrix between the center of mass of the template point cloud and the center of mass of the first point cloud information and translating the first point cloud information after horizontal correction to obtain second point cloud information.

[0011] As a further improvement of the present invention, the method also includes: determining the second image corresponding to the second point cloud information specifically includes: projecting the second point cloud information into the first plane to obtain a point cloud projection; mapping the point cloud projection to a pixel coordinate system to obtain a second image.

[0012] As a further improvement of the present invention, the method also includes: calculating the matching relationship between the second image and the preset point cloud projection template specifically includes: matching the preset point cloud projection template in the second image and obtaining a second matching value; when the second matching value is greater than or equal to the first preset value, obtaining the center position and rotation angle of the matching target in the second image, the center position is the position with the highest second matching value; calculating the rotation angle difference according to the rotation angle of the matching target in the second image and the point cloud projection template; calculating the translation amount of the second image on the first plane according to the center position of the matching target in the second image and the point cloud projection template; calculating the first transformation matrix characterizing the matching relationship between the matching target in the second image and the preset point cloud projection template according to the rotation angle difference and the translation amount.

[0013] As a further improvement of the present invention, the method also includes: obtaining initial pose information based on the matching relationship between the second image and the preset point cloud projection template, and performing ICP precise alignment on the initial pose information to determine the pose information of the object to be measured, which specifically includes: calculating the first transformation matrix of the matching target in the second image within the first plane according to the matching relationship between the second image and the point cloud projection template; adjusting the template point cloud to obtain the initial pose information; and using ICP precise alignment to adjust the initial pose information to determine the pose information of the object to be measured.

[0014] As a further improvement of the present invention, the method also includes: when the matching relationship between the first image and the preset two-dimensional image template does not meet the first condition, calculating the second transformation matrix according to the center position and rotation angle of the matching target in the first image and the two-dimensional image template; adjusting the template point cloud according to the second transformation matrix to obtain initial pose information; and using a precise registration algorithm to adjust the initial pose information to determine the pose information of the object to be measured.

[0015] As a further improvement of the present invention, the method also includes: calculating the second transformation matrix according to the center position and rotation angle of the matching target and the two-dimensional image template in the first image, specifically including: calculating the rotation angle difference according to the rotation angle of the matching target and the two-dimensional image template in the first image; calculating the translation of the first image on the first plane according to the center position of the matching target and the two-dimensional image template in the first image; calculating the second transformation matrix according to the rotation angle difference and the translation.

[0016] As a further improvement of the present invention, the method also includes: the first image includes an RGB image and a depth map, and the first condition is: the first matching value between the RGB image and the two-dimensional image template is less than or equal to a second preset value; the point cloud data in the depth map is extracted to form the first point cloud information; if multiple objects to be tested are superimposed, the objects to be tested are segmented so that each object to be tested corresponds to a first image, and the segmentation step includes: extracting the first point cloud information in the depth map, calculating the normal vector of each point cloud data in the first point cloud information; mapping the specified component in the normal vector to a preset range and saving it as a grayscale image; calculating the pixel gradient of the grayscale image, and when the pixel gradient value is greater than a set threshold, setting the pixel value of the corresponding position in the depth map to 0; performing edge contour extraction on the depth map to obtain an area corresponding to each object to be tested, and then obtaining the first image of each object to be tested.

[0017] As a further improvement of the present invention, the method further includes: the establishment of the template point cloud includes: shooting and acquiring the template workpiece image, extracting the point cloud information of the template workpiece, and performing horizontal correction and decentralization processing on the point cloud information of the template workpiece to obtain the template point cloud;

[0018] The establishment of a point cloud projection template includes: obtaining each corresponding pixel point by projecting each point cloud in the template point cloud; calculating the minimum and maximum values ​​of each pixel point, and calculating the minimum width and minimum height of the required projection image according to the minimum and maximum values; based on the minimum width and the minimum height, creating a single-channel grayscale image with a preset size and mapping each pixel point to the single-channel grayscale image; setting each pixel point value in the single-channel grayscale image to a preset pixel value to obtain a projection image; extracting the contour features of the projection image to obtain a point cloud projection template and obtaining the center position and rotation angle of the point cloud projection template.

[0019] Compared with the prior art, the present invention has the following beneficial effects: the present invention performs preliminary matching between a two-dimensional image template and a first image, and can adjust the template point cloud based on a first condition to reduce the difference with the first point cloud information, thereby effectively reducing the possibility of falling into a local optimal solution during registration, so as to facilitate subsequent registration to be more accurate; the present invention effectively reduces the number of algorithm iterations during point cloud registration, reduces time consumption, and reduces computational costs through early matching relationship calculations. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is a flow chart of a method for estimating an object pose in one embodiment of the present invention.

[0021] Figure 2 This is a flowchart of adjusting the first point cloud information in one embodiment of the present invention.

[0022] Figure 3 Schematic diagram of horizontal correction in one embodiment of the present invention.

[0023] Figure 4 It is a flowchart of determining the second image and calculating the matching relationship with the point cloud projection template in one embodiment of the present invention.

[0024] Figure 5 It is a flow chart of registering a preset template point cloud to determine the position and posture information of an object to be measured in one embodiment of the present invention.

[0025] Figure 6 It is a schematic diagram of adjusting the template point cloud to align the posture information of the object to be measured in one embodiment of the present invention.

[0026] Figure 7 This is a flowchart executed when the matching relationship between the first image and the preset two-dimensional image template does not meet the first condition in one embodiment of the present invention.

[0027] Figure 8 It is a flow chart of segmenting multiple superimposed objects to be detected in one embodiment of the present invention.

[0028] Fig. 9 This is a flowchart of dilating a first image according to one embodiment of the present invention.

[0029] Fig.10 It is a schematic diagram of the first image expansion in one embodiment of the present invention. DETAILED DESCRIPTION

[0030] The present invention will be described in detail below in conjunction with the specific embodiments shown in the accompanying drawings. However, these embodiments do not limit the present invention, and any structural, methodological, or functional changes made by a person skilled in the art based on these embodiments are all within the scope of protection of the present invention.

[0031] In the following implementation manner of the present invention, in order to facilitate understanding of the embodiments of the present application, the following points are first explained.

[0032] RGB image is a three-channel image, which consists of data of three channels: red, green, and blue in the same rows and columns. In RGB mode, an intensity value in the range of 0 to 255 is assigned to the RGB components of each pixel in the image. By adjusting the intensity values ​​of these three colors, almost all colors that human vision can perceive can be produced. For example, the R value of pure red is 255, and the G and B values ​​are 0; the R, G, and B values ​​of gray are equal (except 0 and 255); the R, G, and B values ​​of white are all 255; and the R, G, and B values ​​of black are all 0.

[0033] Depth images are images used to represent the three-dimensional information of a scene. In a depth image, the value of each pixel represents the depth or distance of the pixel in the visual scene, usually expressed in grayscale or color levels from near to far.

[0034] ROI (Region of Interest) is the part of an image that needs special attention or processing. In image processing, the ROI area is usually determined by manual selection, automatic detection, or algorithm generation. The ROI area can be any shape such as rectangle, circle, polygon, etc., depending on the application scenario and requirements.

[0035] Point cloud data is a massive collection of points that express the spatial distribution and surface characteristics of the target in the same spatial reference system. After obtaining the spatial coordinates of each sampling point on the surface of the object, a collection of points is obtained, which is called a "point cloud". The scanned data is recorded in the form of points, each of which contains three-dimensional coordinates. Each point includes parameters to determine the position (e.g., X, Y, Z), color (e.g., R, G, B or Y, U, V) and possible other attributes (such as transparency, reflectivity, acquisition time, etc.).

[0036] Decentralization is a preprocessing step that transforms point cloud data from the original coordinate system into a local coordinate system with a specific point (usually the center or center of mass of the object) as the origin. This process helps reduce the influence of global position information on gesture recognition, allowing the algorithm to focus more on the shape and posture of the object.

[0037] A normal vector is a vector perpendicular to any tangent line on a plane (or surface). In three-dimensional space, a normal vector is usually used to describe the directionality of a surface. For point cloud data, the calculation of normal vectors is particularly important because it helps us understand the spatial relationship between points in the point cloud and the shape of the surface.

[0038] The pixel coordinate system is a two-dimensional coordinate system on the image, which is used to define the position of each pixel on the image. In one embodiment, the pixel coordinate system takes the upper left corner of the image as the origin, the X axis extends from left to right, and the Y axis extends from top to bottom.

[0039] Pixel spatial relationships refer to the connections and distances between pixels, including the neighborhood, adjacency, connection, and connectivity of pixels, as well as the distance calculation between pixels.

[0040] In one embodiment of the present invention, a method for estimating an object posture is provided.

[0041] The object pose estimation method can be applied to computer vision, robot navigation, virtual reality, augmented reality and other fields. In particular, in the field of robot navigation, accurate object pose detection is the key to achieving autonomous operation of the robot. The present invention provides a fast, stable and accurate object pose estimation method, which helps to improve the operating efficiency and accuracy of the robot in the field of robotics. In the field of virtual reality and augmented reality, the pose information of the object is an important link in achieving seamless integration of the real and virtual worlds, and the application of the present invention can provide a more realistic user experience. In addition, the present invention can also be applied to intelligent manufacturing, logistics warehousing, security monitoring and other fields, meeting the market demand for efficient, accurate and intelligent object pose detection.

[0042] Before estimating the object’s pose, the object must be identified and detected to obtain its image. Figure 1 As shown, the object pose estimation method provided by the present invention comprises the following steps:

[0043] Step S1: Acquire a first image of the object to be measured.

[0044] In one embodiment, the first image includes an RGB image and a depth image.

[0045] The first image includes RGB data and point cloud data of the object to be measured, wherein the RGB data comes from the RGB image and the point cloud data comes from the depth image.

[0046] It is determined whether a matching relationship between the first image and a preset two-dimensional image template satisfies a first condition.

[0047] The preset two-dimensional image template extracts the two-dimensional contour of the object from the RGB image of the template workpiece and stores it as a template.

[0048] The matching relationship between the first image and the preset two-dimensional image template is determined, that is, the two-dimensional image template is searched for a match in the first image. Methods such as shape context matching, contour point matching or feature-based matching can be used to determine the matching relationship.

[0049] The first condition is that the first matching value between the RGB image and the two-dimensional image template is less than or equal to a second preset value. In one embodiment, the second preset value is set to 60%. If the first matching value is less than the second preset value, it means that the first pose information cannot be obtained accurately when the first image is matched with the two-dimensional image template.

[0050] In one embodiment, it is determined whether a matching relationship between an RGB image in the first image and a preset two-dimensional image template satisfies a first condition.

[0051] In one embodiment, the contour edge of the depth map is extracted to obtain a template workpiece to form an image with a ROI area, and the ROI area is filled with RGB data to determine whether the matching relationship with the preset two-dimensional image template meets the first condition.

[0052] In one embodiment, the contour edge of the depth map is extracted to form an image with a ROI area, and the image is expanded and filled with RGB data to determine whether the matching relationship with the preset two-dimensional image template meets the first condition.

[0053] Edge detection algorithms are used to identify edges in depth images. For example, the Canny edge detector can identify areas in the image where brightness or color changes significantly, thereby determining the edges of objects. Continuous sequences of edge pixels are identified to form the outline of the object. Based on the extracted outline, a polygon or rectangular area is created to surround the outline to form an ROI area.

[0054] Before extracting the edge contour of the depth image, the depth image may be denoised to reduce interference caused by sensor noise, illumination changes, etc. The image may also be smoothed to reduce fluctuations in the depth data and make the edges clearer.

[0055] When the matching relationship between the first image and the preset two-dimensional image template meets the first condition, step S2 is performed: first point cloud information corresponding to the first image is adjusted on the first plane according to the matching relationship to obtain second point cloud information. The first point cloud information represents the point cloud data in the depth map.

[0056] The matching relationship between the first image and the preset two-dimensional image template includes a rotation angle relationship and a horizontal displacement relationship between the first image and the two-dimensional image template.

[0057] In one embodiment, the first point cloud information is rotated and translated to obtain the second point cloud information.

[0058] like Figure 2 As shown, adjusting the first point cloud information corresponding to the first image on the first plane according to the matching relationship to obtain the second point cloud information specifically includes:

[0059] Step S2.1: Match the first image with a preset two-dimensional image template and obtain a first matching value.

[0060] Step S2.2: When the first matching value satisfies the first condition, the first point cloud information of the first image is extracted. The extracted first point cloud information can be defined as p t (Contains N points).

[0061] In one embodiment, extracting first point cloud information of the first image includes:

[0062] Extract the depth map to find each pixel (μ, v) in the i-th (i∈n) region, whose depth value is z c , converted to a spatial point p by the following formula ω (x ω ,y ω , z ω ):

[0063]

[0064] The point cloud is obtained as All point clouds are collected as That is, it is represented as the first point cloud information.

[0065] Step S2.3: Horizontally correct and translate the first point cloud information.

[0066] Correcting the first point cloud information horizontally includes:

[0067] Obtain the normal vector of the first point cloud information, and calculate the rotation angle between the two according to the normal vector of the preset direction. Define the normal vector of the first point cloud information, that is, the normal vector of the first point cloud information Defines the normal vector for a preset direction Calculated and The rotation angle between In one embodiment, the preset direction is a known positive direction of the z-axis.

[0068] The normal vector of the first point cloud information may be obtained by using the least squares method, the RANSAC method, or the PCA method.

[0069] The rotation matrix is ​​obtained by the Rodriguez rotation formula and the first point cloud information is rotated.

[0070] Among them, the unit vector can be defined Unit Vector R is a real number. In one embodiment,

[0071] The unit vector Rotate by angle θ and use the Rodriguez rotation formula to get the rotation matrix R q , the rotation matrix is ​​R q The calculation formula is:

[0072]

[0073] Thus, the rotation matrix obtained by the Rodriguez rotation formula is R q First point cloud information Do horizontal correction and define the first point cloud information after horizontal correction as like Figure 3 The figure shows a schematic diagram of horizontal correction.

[0074] In one embodiment, a fitting algorithm may be used to obtain the normal vector of the first point cloud information.

[0075] In other embodiments, a non-fitting algorithm may be used to obtain the normal vector of the first point cloud information, such as using principal component analysis (PCA).

[0076] The translation transformation of the first point cloud information includes:

[0077] Obtain the first point cloud information after horizontal correction The center of mass is C1(m x , m y , m z ).

[0078] Among them, m x The first point cloud information The average value of the x-coordinate, m y The first point cloud information The average value of the coordinates in the y direction, m z The first point cloud information The average value of the coordinates in the z direction.

[0079] According to the centroid calculation of the template point cloud and the first point cloud information The translation matrix between the centroid and the first point cloud information after horizontal correction Translation processing: Translation processing is to perform transformation processing through a translation matrix.

[0080] The template point cloud is defined as p m , the center of mass is M(0, 0, 0).

[0081] Define the translation matrix as T1. The translation matrix T1 is calculated as:

[0082]

[0083] In this way, the first point cloud information Make a translation T1 to move its center of mass C1 to point M, and obtain the first point cloud information after translation, which is defined as

[0084] The first point cloud information after the above-mentioned horizontal correction and translation transformation processing Recorded as the second point cloud information.

[0085] Step S3: determining a second image corresponding to the second point cloud information, and calculating a matching relationship between the second image and a preset point cloud projection template.

[0086] The second image is a projection image of the second point cloud information. The second image and the point cloud projection template are matched to determine their matching relationship. The matching method is as described above and will not be described here.

[0087] The matching relationship between the second image and the point cloud projection template includes a rotation relationship and a horizontal displacement relationship between the second image and the point cloud projection template.

[0088] Step S4: obtaining initial posture information based on the matching relationship between the second image and a preset point cloud projection template, and performing ICP precise registration on the initial posture information to determine the posture information of the object to be measured.

[0089] The template point cloud is generated according to known three-dimensional information of the template workpiece. The point cloud projection template is a projection image of the template point cloud in the first plane.

[0090] The template point cloud is adjusted through the matching relationship between the second image and the point cloud projection template and the transformation relationship between the first point cloud information and the second point cloud information so as to determine the initial posture information of the object to be measured, and then the template point cloud is registered using ICP precise registration to determine the final posture information of the object to be measured.

[0091] In the above-mentioned implementation, the known template point cloud is used to adjust the template point cloud based on the calculated transformation relationship, thereby reducing the difference with the first point cloud information, effectively reducing the possibility of falling into a local optimal solution during alignment, and making the alignment more accurate; effectively reducing the number of algorithm iterations during alignment, reducing time consumption, and reducing computational costs.

[0092] In one embodiment, the ICP fine registration adopts a 3D point cloud ICP (Iterative Closest Point) fine registration algorithm.

[0093] 3D point cloud ICP (Iterative Closest Point) precise registration is an algorithm that minimizes the distance between two point clouds through iterative calculation, thereby optimizing the transformation matrix (including rotation matrix and translation vector) from one point cloud to another.

[0094] like Figure 4 As shown, in one embodiment of the present invention, determining the second image corresponding to the second point cloud information specifically includes:

[0095] Step S3.1: Project the second point cloud information onto the first plane to obtain a point cloud projection.

[0096] The second point cloud information representing the three-dimensional information Project the point cloud onto a two-dimensional plane to obtain the projection of the point cloud on the two-dimensional plane.

[0097] In one embodiment, a point cloud projection is obtained by projecting along the z-axis direction representing the height onto the XOY plane, wherein the X-axis and the Y-axis are directions representing the length and the width, and a two-dimensional point cloud projection with a height of 0 is obtained by projection.

[0098] Step S3.2: Project the point cloud into a pixel coordinate system to obtain a second image.

[0099] Mapping is the second point cloud information The corresponding image is obtained by projecting the two-dimensional plane onto the pixel coordinate system.

[0100] The second point cloud information Each point (x j ,y j ,z j ) The position of each pixel in the projected image (μ j ,v j );

[0101]

[0102] In the formula Re x ,Re y The pixel accuracy in the X and Y directions respectively.

[0103] Calculate the minimum and maximum values ​​of each pixel. That is, calculate μ j The minimum value μ min , maximum value μ max and v j The minimum value v min , maximum value v max .

[0104] Calculate the minimum width W of the second image min =μ max -μ min , minimum height H min =v max -v min , with width = 1.2*W min and height = 1.2*H min Create a single-channel grayscale image with all pixel values ​​​​as 0, the center of the image is (width / 2, height / 2), and each pixel point (μ j ,v j ) is mapped into the image:

[0105]

[0106] All The pixel value is set to the preset value 255, and the second image Q is obtained. proj .

[0107] By performing the above steps, we can obtain the second image Q proj .

[0108] In one embodiment of the present invention, calculating the matching relationship between the second image and the preset point cloud projection template specifically includes:

[0109] Step S3.3: Match the preset point cloud projection template in the second image and obtain a second matching value.

[0110] The second matching value represents the similarity between the second image feature and the point cloud projection template.

[0111] Step S3.4: When the second matching value is greater than or equal to the first preset value, the center position and rotation angle of the matching target in the second image are obtained, wherein the center position is the position with the highest second matching value.

[0112] In one embodiment, the first preset value is set to 60%. The center position of the matching target in the second image is defined as The rotation angle is β t .

[0113] Step S3.5: Calculate the rotation angle difference according to the rotation angles of the matching target in the second image and the point cloud projection template.

[0114] Define the center position of the point cloud projection template M1 and the rotation angle β m That is, the rotation angle of the second image matching target is subtracted from the rotation angle of the point cloud projection template M1: the rotation angle difference β = β t -β m .

[0115] Step S3.6: Calculate the translation amount of the second image on the first plane according to the center position of the matching target in the second image and the point cloud projection template.

[0116] The first plane is the XOY plane. The translation amount includes the translation amount in the X-axis direction of the pixel coordinate system and the translation amount in the Y-axis direction of the pixel coordinate system.

[0117] The amount of translation in the X-axis direction The amount of translation along the Y axis The X-axis represents the length direction of the image, and the Y-axis represents the width direction of the image. x is the pixel accuracy in the x direction, Re yThe pixel precision in the y direction.

[0118] Step S3.7: Calculate a first transformation matrix representing the matching relationship between the second image matching target and a preset point cloud projection template according to the rotation angle difference and the translation amount.

[0119] First transformation matrix

[0120] like Figure 5 As shown, in one embodiment of the present invention, initial pose information is obtained based on the matching relationship between the second image and the preset point cloud projection template, and ICP precise registration is performed on the initial pose information to determine the pose information of the object to be measured, specifically including:

[0121] Step S4.1: The horizontal correction and translation transformation of the second point cloud information obtained according to the first point cloud information includes a rotation matrix and a translation matrix.

[0122] Rotation Matrix

[0123]

[0124] Translation Matrix

[0125] Step S4.2: Calculate a first transformation matrix of a matching target in the second image within a first plane according to a matching relationship between the second image and the point cloud projection template.

[0126] First transformation matrix

[0127] Step S4.3: Adjust the template point cloud to obtain initial pose information.

[0128] Specifically, the template point cloud is adjusted by a rotation matrix, a translation matrix and a first transformation matrix to obtain initial pose information.

[0129] The second transformation matrix is ​​formed by the rotation matrix, the translation matrix and the first transformation matrix. Specifically, the second transformation matrix is ​​obtained by multiplying the inverse matrix of the rotation matrix, the inverse matrix of the translation matrix and the first transformation matrix.

[0130] The second transformation matrix H0=R1 -1 T1 -1 H1. Adjust the template point cloud p through the second transformation matrix m , recorded as the new template point cloud p m′ , with the new point cloud template p m′ The pose of is taken as the initial pose information.

[0131] Step S4.4: Use ICP fine registration to adjust the initial pose information to determine the pose information of the object to be measured.

[0132] like Figure 6 As shown, the 3D point cloud ICP precise registration algorithm in the PCL library is used to calculate the target matrix H from the initial pose information to the pose information of the object to be measured. icp Among them, the target point cloud p in the figure t It is the first point cloud information of the object to be measured, that is, the true position and posture of the object to be measured.

[0133] According to the target matrix H icp The second transformation matrix H0 is used to calculate the position and posture information of the object to be measured. Specifically, the target matrix H icp The matrix H is obtained by multiplying the second transformation matrix H0. final , the matrix H final That is the posture information (real posture) of the object to be measured.

[0134] Finally, the template point cloud p is obtained m Transformation to the exact first point cloud information:

[0135] H final =H icp H0.

[0136] H final That is, the true pose of the first point cloud information.

[0137] PCL (Point Cloud Library) is a large cross-platform open source C++ programming library specifically designed for processing 2D / 3D images and point cloud data. PCL implements a large number of point cloud-related general algorithms and efficient data structures, covering point cloud acquisition, filtering, segmentation, registration, retrieval, feature extraction, recognition, tracking, surface reconstruction, visualization and other aspects.

[0138] like Figure 7 As shown, in one embodiment of the present invention, when judging whether the difference between the first image and the preset two-dimensional image template satisfies the first condition, if the first condition is not satisfied, that is, the matching value between the RGB image and the two-dimensional image template is greater than the second preset value, then jump to the following steps:

[0139] Step S5: Calculate a second transformation matrix according to the center position and rotation angle of the first image matching target and the two-dimensional image template.

[0140] Define the center position of the first image matching target Rotation angle α t . Define the center position of the 2D image template Rotation angle α m .

[0141] The rotation angle difference is calculated based on the rotation angles of the first image matching target and the two-dimensional image template. That is, the rotation angle of the first image matching target is subtracted from the rotation angle of the two-dimensional image template: the rotation angle difference α=α t -α m .

[0142] The translation amount of the first image on the first plane is calculated according to the center position of the first image matching target and the two-dimensional image template. The first plane is the XOY plane.

[0143] The amount of translation in the X-axis direction Re x The pixel precision in the x direction.

[0144] The amount of translation along the Y axis Re y The pixel precision in the y direction.

[0145] A second transformation matrix is ​​calculated according to the rotation angle difference and the translation amount.

[0146] Second transformation matrix

[0147] Step S6: Adjust the template point cloud according to the second transformation matrix to obtain initial pose information.

[0148] Step S7: Perform ICP precision registration on the initial posture information to determine the posture information of the object to be measured.

[0149] like Figure 8 As shown, in one embodiment of the present invention, if a plurality of objects to be detected are superimposed, the objects to be detected are segmented so that each object to be detected corresponds to a first image, and the segmentation step includes:

[0150] Step T1: extract the first point cloud information in the depth map, and calculate the normal vector of each point cloud data in the first point cloud information.

[0151] Define the normal vector (d x ,d y ,d z ).

[0152] Step T2: Map the specified component in the normal vector to a preset range and save it as a grayscale image.

[0153] In one embodiment, the specified component is a component representing height, ie, a known z-axis.

[0154] In one embodiment, the preset range is 0-255.

[0155] In one embodiment, the grayscale image is an 8-bit single-channel grayscale image.

[0156] Calculate the normal vector (d) for each pixel in the depth image x ,d y ,d z ), and the z-axis component of the normal vector d z Mapped to the range of 0-255 and saved as an 8-bit single-channel grayscale image (CV_8UC1).

[0157] Among them, for the above steps, it can be further understood that mapping the z-axis component of the normal vector to the range of 0-255 is a quantization and encoding of the normal vector information. By mapping the z-axis component of the normal vector to the range of 0-255, it means converting it into a grayscale value, representing a certain characteristic of the point in space (such as orientation or height, etc.). In this way, part of the three-dimensional information of the point cloud data can be retained in the two-dimensional image, thereby facilitating subsequent image analysis and processing.

[0158] Saving the processed normal vector z-axis component as a grayscale image in CV_8UC1 format means converting the three-dimensional point cloud data into a two-dimensional grayscale image.

[0159] CV_8UC1 represents an 8-bit unsigned integer single-channel image, that is, a grayscale image.

[0160] Step T3: Calculate the pixel gradient of the grayscale image, and when the pixel gradient value is greater than a set threshold, set the pixel value of the corresponding position in the depth map to 0.

[0161] Step T4: extract edge contours from the depth map to obtain a region corresponding to each object to be measured, and then obtain a first image of each object to be measured.

[0162] Based on the above implementation, the image gradient of the grayscale image is calculated by using the method of calculating the pixel gradient of the image. First, a threshold is set, and the pixel value at the position where the gradient value is greater than the threshold is set to 0; thus, the edge position of the plane is set to 0 due to the large gradient, and each plane is divided; finally, the edge contour is extracted to obtain multiple independent areas, which are the first image (ROI area) of each object to be measured.

[0163] In one embodiment of the present invention, before target segmentation, the depth map is filtered to appropriately reduce noise, fill gaps in some invalid points, and other operations.

[0164] like Fig. 9 and Fig.10 As shown, in one embodiment of the present invention, acquiring a first image of the object to be measured includes:

[0165] Step L1: Acquire a region of interest of the object to be measured and expand the first image by a preset pixel unit.

[0166] In one embodiment, the first image is expanded by 5 pixel units.

[0167] Dilation is a basic operation in morphological image processing, which can make the white area (usually representing the foreground or target) in the image expand outward. The dilation operation is performed by defining a structural element (usually a small rectangle, circle or other shape), then sliding the structural element over the image, and assigning the maximum value (for binary images, usually 1 or white) in the area covered by the structural element to the pixel at the center of the structural element.

[0168] In this step, all ROI areas need to be dilated, and the number of dilated pixels is 5.

[0169] In one embodiment, the dilation operation is implemented by a dilation function in an image processing library (such as OpenCV).

[0170] Step L2: Create a mask to represent the dilated area.

[0171] A mask is a two-dimensional array (or image) of the same size as the original image, used to indicate which pixels belong to the region of interest (ROI) and which pixels do not. Masks usually use two basic values: non-zero values ​​(usually white, 255) and zero values ​​(usually black, 0). Non-zero values ​​indicate regions of interest, while zero values ​​indicate regions of no interest.

[0172] The expanded ROI area is set to the non-zero value (white) of the mask, while the rest of the area is set to zero value (black). In this way, the mask accurately represents the location and shape of the region of interest.

[0173] The mask and the original image are compared and processed pixel by pixel. If a pixel value in the mask is non-zero (i.e. white), the RGB color information of the corresponding position in the original image is retained. If a pixel value in the mask is zero (i.e. black), the pixel value of the corresponding position in the original image is set to zero (or remains black, depending on the processing method).

[0174] Step L3: extracting a first image representing the expanded area by using the mask segmentation.

[0175] Specifically, RGB data in the first image is extracted, and the RGB data is filled into pixels in the mask.

[0176] In one embodiment of the present invention, establishing a two-dimensional image template includes:

[0177] Shooting and acquiring a template workpiece image, extracting contour features of the template workpiece and performing decentralized processing on the contour features;

[0178] A two-dimensional image template is established based on the contour feature, and a center position and a rotation angle of the two-dimensional image template are obtained.

[0179] The center position of the 2D image template is The rotation angle is α m .

[0180] In one embodiment of the present invention, the establishment of the template point cloud includes: photographing and acquiring an image of a template workpiece, extracting point cloud information of the template workpiece, and performing horizontal correction and decentralization on the point cloud information of the template workpiece to obtain the template point cloud.

[0181] The point cloud data template is p m .

[0182] The establishment of point cloud projection template includes:

[0183] Each point cloud in the template point cloud is projected to obtain each corresponding pixel point. Point cloud data template p m Each point (x j ,y j ,z j ) is obtained by the following formula for each pixel (μ j ,v j ).

[0184]

[0185] Among them, Re x , Re y The pixel precision in the x and y directions respectively.

[0186] Calculate the minimum and maximum values ​​of each pixel point, and calculate the minimum width and minimum height of the required projection image based on the minimum and maximum values. j , v j The minimum and maximum values ​​of min , μ max and v min 、v max . And calculate the minimum width W of the projected image min =μ max -μ min , minimum height H min =v max -v min .

[0187] Based on the minimum width and the minimum height, a single-channel grayscale image is created with a preset size and each pixel is mapped into the single-channel grayscale image.

[0188] In one embodiment, the preset size is: width width = 1.2*W min , height = 1.2*H min Size.

[0189] Create a single-channel grayscale image with a preset size where all pixel values ​​are 0. The center of the single-channel grayscale image is (width / 2, height / 2) and all pixels (μ j ,v j ) is mapped into the image:

[0190]

[0191] The value of each pixel in the single-channel grayscale image is set to a preset pixel value to obtain a projection image.

[0192] In one embodiment, the preset pixel value is 255. h ,v h )The pixel value is set to 255 to obtain the projected image.

[0193] The contour features of the projection image are extracted to obtain a point cloud projection template, and the center position and rotation angle of the point cloud projection template are acquired.

[0194] The center position of the point cloud projection template M1 is The rotation angle is β m .

[0195] In one embodiment of the present invention, a system for estimating an object's position and posture is provided. The system for estimating an object's position and posture comprises:

[0196] A camera is used to photograph the object to be measured.

[0197] A posture estimation processor is electrically connected to the camera and is used to execute the object posture estimation method as described in any of the above embodiments.

[0198] In one embodiment of the present invention, a storage medium is further provided. The storage medium stores a computer program, and when the computer program is executed by a processor, the steps in the object pose estimation method described in any one of the above embodiments are implemented.

[0199] To summarize, the object pose estimation method provided by the present invention first obtains a first matching value by matching a two-dimensional image template with a first image, and when the first matching value is less than or equal to a second preset value, projects and maps the first point cloud information to obtain second point cloud information, and uses the projection image of the second point cloud information, that is, the second image, to match the cloud projection template to calculate the matching relationship, and transforms the template point cloud according to the matching relationship and the transformation relationship of horizontal correction and translation transformation of the first point cloud information to obtain the first pose information, that is, the initial pose, and then obtains the second pose information, that is, the final target pose, by precisely aligning the first pose information.

[0200] When the first matching value is greater than the second preset value, the difference between the first image matching target and the two-dimensional image template is calculated based on the center position and rotation angle, and the template point cloud is transformed according to the difference to obtain the first pose information, i.e., the initial pose, and then the second pose information, i.e., the final target pose, is obtained by precisely aligning the first pose information.

[0201] It should be understood that although this specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each implementation mode may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

[0202] The series of detailed descriptions listed above are only specific descriptions of feasible implementation methods of the present invention. They are not intended to limit the scope of protection of the present invention. Any equivalent implementation methods or changes that do not deviate from the technical spirit of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for estimating an object pose, characterized in that: include: Acquire a first image of the object to be measured; When a matching relationship between the first image and a preset two-dimensional image template satisfies a first condition, adjusting first point cloud information corresponding to the first image on a first plane to obtain second point cloud information; Determine a second image corresponding to the second point cloud information, and calculate a matching relationship between the second image and a preset point cloud projection template; Initial pose information is obtained based on a matching relationship between the second image and a preset point cloud projection template, and ICP fine alignment is performed on the initial pose information to determine the pose information of the object to be measured.

2. The object pose estimation method according to claim 1, characterized in that: When the matching relationship between the first image and the preset two-dimensional image template satisfies a first condition, adjusting the first point cloud information corresponding to the first image on the first plane specifically includes: Matching the first image with a preset two-dimensional image template and obtaining a first matching value; When the first matching value satisfies a first condition, extracting first point cloud information of the first image; The first point cloud information is horizontally corrected and subjected to translation transformation.

3. The object pose estimation method according to claim 2, characterized in that: Correcting the first point cloud information horizontally includes: Obtaining the normal vector of the first point cloud information, and calculating the rotation angle between the two according to the normal vector in a preset direction; The rotation matrix is ​​obtained by the Rodriguez rotation formula and the first point cloud information is rotated.

4. The object pose estimation method according to claim 2 or 3, characterized in that: The translation transformation of the first point cloud information includes: Obtaining the centroid of the first point cloud information after horizontal correction; A translation matrix between the centroid of the template point cloud and the centroid of the first point cloud information is calculated, and the first point cloud information after horizontal correction is translated to obtain second point cloud information.

5. The object pose estimation method according to claim 1, characterized in that: Determining a second image corresponding to the second point cloud information specifically includes: Projecting the second point cloud information onto the first plane to obtain a point cloud projection; The point cloud is projected into a pixel coordinate system to obtain a second image.

6. The object pose estimation method according to claim 1, characterized in that: Calculating the matching relationship between the second image and the preset point cloud projection template specifically includes: Matching a preset point cloud projection template in the second image and obtaining a second matching value; When the second matching value is greater than or equal to the first preset value, the center position and the rotation angle of the matching target in the second image are obtained, and the center position is the position with the highest second matching value; Calculating a rotation angle difference according to the rotation angles of the matching target and the point cloud projection template in the second image; Calculate the translation amount of the second image on the first plane according to the center position of the matching target in the second image and the point cloud projection template; A first transformation matrix representing a matching relationship between a matching target in the second image and a preset point cloud projection template is calculated according to the rotation angle difference and the translation amount.

7. The object pose estimation method according to claim 2, characterized in that: Obtaining initial pose information based on the matching relationship between the second image and the preset point cloud projection template, and performing ICP fine registration on the initial pose information to determine the pose information of the object to be measured specifically includes: Calculate a first transformation matrix of a matching target in the second image within a first plane according to a matching relationship between the second image and the point cloud projection template; Adjust the template point cloud to obtain the initial pose information; The initial position and posture information is adjusted by ICP fine registration to determine the position and posture information of the object to be measured.

8. The object pose estimation method according to claim 1, characterized in that: When the matching relationship between the first image and the preset two-dimensional image template does not meet the first condition, calculating the second transformation matrix according to the center position and the rotation angle of the matching target in the first image and the two-dimensional image template; Adjust the template point cloud according to the second transformation matrix to obtain initial pose information; The initial pose information is precisely aligned with ICP to determine the pose information of the object to be measured.

9. The object pose estimation method according to claim 8, characterized in that: Calculating the second transformation matrix according to the center position and rotation angle of the matching target and the two-dimensional image template in the first image specifically includes: Calculating a rotation angle difference according to the rotation angles of the matching target in the first image and the two-dimensional image template; Calculating a translation amount of the first image on a first plane according to a center position of a matching target in the first image and the two-dimensional image template; A second transformation matrix is ​​calculated according to the rotation angle difference and the translation amount.

10. The object pose estimation method according to claim 1, characterized in that: The first image includes an RGB image and a depth image, and the first condition is that a first matching value between the RGB image and the two-dimensional image template is less than or equal to a second preset value; Extracting point cloud data in the depth map to form first point cloud information; If a plurality of objects to be tested are superimposed, the objects to be tested are segmented so that each object to be tested corresponds to a first image, and the segmentation step includes: Extract first point cloud information from the depth map, and calculate a normal vector of each point cloud data in the first point cloud information; Mapping a specified component in the normal vector into a preset range and saving it as a grayscale image; Calculating the pixel gradient of the grayscale image, and when the pixel gradient value is greater than a set threshold, setting the pixel value at the corresponding position in the depth map to 0; Edge contour extraction is performed on the depth map to obtain a region corresponding to each object to be measured, and then a first image of each object to be measured is obtained.

11. The object pose estimation method according to claim 1, characterized in that: The establishment of template point cloud includes: Shooting and acquiring an image of a template workpiece, extracting point cloud information of the template workpiece, and performing horizontal correction and decentralization processing on the point cloud information of the template workpiece to obtain a template point cloud; The establishment of point cloud projection template includes: Obtain each corresponding pixel point by projecting each point cloud in the template point cloud; Calculate the minimum and maximum values ​​of each pixel point, and calculate the minimum width and minimum height of the required projection image according to the minimum and maximum values; Based on the minimum width and the minimum height, create a single-channel grayscale image with a preset size and map each pixel into the single-channel grayscale image; Setting each pixel value in the single-channel grayscale image to a preset pixel value to obtain a projection image; The contour features of the projection image are extracted to obtain a point cloud projection template, and the center position and rotation angle of the point cloud projection template are acquired.

Images