Position estimation method, device and electronic device

Abstract

The present application discloses a posture estimation method, device and electronic device, which are applied to the posture estimation technology field to solve the problem of poor posture estimation accuracy of symmetrical plane objects in the prior art, including determining the centroid position and rotation matrix of the minimum directed bounding box based on the plane point cloud data of the symmetrical plane object; determining the maximum fitting plane with the plane normal vector and the direction of the vertical axis direction vector in the rotation matrix as the constraint condition; determining the side length direction vector of the maximum fitting plane based on the projected point cloud data of the plane point cloud data projected to the maximum fitting plane; correcting the rotation matrix based on the side length direction vector of the maximum fitting plane; and determining the target posture based on the centroid position and the corrected rotation matrix. Correcting the rotation matrix of the minimum directed bounding box by the side length direction vector of the maximum fitting plane can make the main direction always along the side length direction of the maximum fitting plane, thereby improving the posture estimation accuracy.

Description

Technical Field

[0001] The present application relates to the technical field of posture estimation, and in particular to a posture estimation method, device and electronic equipment. Background Art

[0002] At present, in the process of operating symmetrical plane objects, the traditional oriented bounding box (OBB) algorithm is usually used to solve the pose of the symmetrical plane object. In the pose solved by this method, the main direction of the symmetrical plane object may have an error with the side length direction of the symmetrical plane object. For example, when the length and width of the symmetrical plane object are relatively small, its main direction will be along the diagonal direction of the symmetrical plane object, which will cause deviations in the pose estimation of the symmetrical plane object, thereby affecting the operation accuracy of the symmetrical plane object. Summary of the invention

[0003] The present application provides a posture estimation method, device and electronic device to solve the problem of poor posture estimation accuracy for symmetrical planar objects in the prior art.

[0004] The technical solutions provided by this application are as follows:

[0005] On the one hand, the present application provides a posture estimation method, comprising:

[0006] Acquire plane point cloud data of the symmetrical plane object; wherein the plane point cloud data includes point cloud data of each original sampling point on the surface of the symmetrical plane object;

[0007] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, determine the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object;

[0008] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the maximum fitting plane corresponding to the symmetrical plane object is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint condition;

[0009] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object projected onto the projected sampling point on the maximum fitting plane, determine the side length direction vector of the maximum fitting plane;

[0010] Based on the side length direction vector of the maximum fitting plane, the rotation matrix is ​​corrected to obtain the target rotation matrix;

[0011] Determine the target pose of a symmetric planar object based on the center of mass position and the target rotation matrix.

[0012] Optionally, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, determining the centroid position and rotation matrix of the minimum directional bounding box corresponding to the symmetrical plane object includes:

[0013] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the principal component analysis algorithm is used to analyze the eigenvalue and eigenvector of the symmetrical plane object;

[0014] Based on the eigenvalues ​​and eigenvectors of the symmetric plane object, the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetric plane object are determined.

[0015] Optionally, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, determining the centroid position and rotation matrix of the minimum directional bounding box corresponding to the symmetrical plane object includes:

[0016] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the directed enclosing box solving operation is iteratively performed until it is determined that the iteration termination condition is met, and the centroid position and rotation matrix of the current directed enclosing box solved when the directed enclosing box solving operation is currently performed are determined as the centroid position and rotation matrix of the minimum directed enclosing box; wherein the directed enclosing box solving operation includes:

[0017] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the point cloud data of the projected sampling point projected onto the specified plane by each original sampling point is calculated;

[0018] Based on the point cloud data of the projected sampling points projected onto the specified plane by each original sampling point, the concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object; wherein, when the directed enclosing box solution operation is performed for the first time, the learning parameter of the concave hull algorithm is the initialization value; when the directed enclosing box solution operation is not performed for the first time, the learning parameter of the concave hull algorithm is the target value adjusted when the directed enclosing box solution operation was performed last time;

[0019] Based on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object, the principal component analysis algorithm is used to analyze the eigenvalue and eigenvector of the symmetrical plane object;

[0020] Based on the eigenvalues ​​and eigenvectors of the symmetric plane object, determine the centroid position and rotation matrix of the current directed bounding box corresponding to the symmetric plane object;

[0021] Extract the point cloud data of each contour sampling point on the box boundary of the current directional bounding box;

[0022] Performing point cloud registration on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object and the point cloud data of each contour sampling point on the box boundary of the current directed bounding box to obtain a matching score between the concave boundary of the symmetrical plane object and the box boundary of the current directed bounding box;

[0023] When it is determined that the matching score is less than the set score threshold, the learning parameters of the concave hull algorithm are adjusted to the target value based on the difference between the concave boundary of the symmetric plane object and the box boundary of the current oriented bounding box.

[0024] Optionally, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the maximum fitting plane corresponding to the symmetrical plane object is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint condition, including:

[0025] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the plane fitting operation is iteratively performed until it is determined that the current inner point ratio calculated when the plane fitting operation is currently performed is not less than the set inner point ratio threshold, and the current fitting plane fitted when the plane fitting operation is currently performed is determined as the maximum fitting plane of the symmetrical plane object; wherein the plane fitting operation includes:

[0026] From each original sampling point on the surface of the symmetrical plane object, a set number of original sampling points are randomly selected as candidate sampling points;

[0027] Based on the point cloud data of each candidate sampling point, the current fitting plane is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint;

[0028] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the distance from each original sampling point on the surface of the symmetrical plane object to the current fitting plane is calculated;

[0029] Each original sampling point whose distance is less than a set distance threshold is determined as an inner point, and based on the number of inner points and the number of each original sampling point on the surface of the symmetrical plane object, a current inner point ratio is calculated.

[0030] Optionally, determining the side length direction vector of the maximum fitting plane based on point cloud data of projected sampling points on the maximum fitting plane projected from each original sampling point on the surface of the symmetrical plane object includes:

[0031] Based on the point cloud data of the projected sampling points on the maximum fitting plane projected from each original sampling point on the surface of the symmetrical plane object, each contour line of the maximum fitting plane is extracted;

[0032] Based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line, the side length direction vector of the maximum fitting plane is calculated.

[0033] Optionally, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object projected onto the projected sampling point on the maximum fitting plane, each contour line of the maximum fitting plane is extracted, including:

[0034] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the point cloud data of the projected sampling point projected onto the maximum fitting plane by each original sampling point is calculated;

[0035] Based on the point cloud data of the projected sampling points projected onto the maximum fitting plane by each original sampling point, the concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the maximum fitting plane;

[0036] The point cloud data of each contour sampling point is extracted by straight line extraction to obtain the contour lines of the maximum fitting plane.

[0037] Optionally, the maximum fitting plane is a square; based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line, the side length direction vector of the maximum fitting plane is calculated, including:

[0038] Select one contour line from the four contour lines of the maximum fitting plane as a reference contour line, and calculate the distance between the reference contour line and the remaining three contour lines of the four contour lines;

[0039] The contour line with the largest distance among the three contour lines and the reference contour line form a first set of opposite sides, and the remaining two contour lines among the three contour lines form a second set of opposite sides;

[0040] Based on the point cloud data of the intersection between one contour line in the first set of opposite sides and two contour lines in the second set of opposite sides, the side length direction vector of the maximum fitting plane is calculated.

[0041] Optionally, based on the side length direction vector of the maximum fitting plane, the rotation matrix is ​​corrected to obtain a target rotation matrix, including:

[0042] Based on the side length direction vector and the vertical axis direction vector in the rotation matrix, the horizontal axis direction vector is calculated using the vector cross multiplication right-hand rule;

[0043] The target rotation matrix is ​​determined based on the side length direction vector, the vertical axis direction vector and the horizontal axis direction vector.

[0044] On the other hand, the present application provides a posture estimation device, comprising:

[0045] A point cloud acquisition unit, used to acquire plane point cloud data of a symmetrical plane object; wherein the plane point cloud data includes point cloud data of each original sampling point on the surface of the symmetrical plane object;

[0046] A bounding box determination unit, used to determine the centroid position and rotation matrix of the minimum directional bounding box corresponding to the symmetrical plane object based on the point cloud data of each original sampling point on the surface of the symmetrical plane object;

[0047] A plane fitting unit is used to determine the maximum fitting plane corresponding to the symmetrical plane object based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, with the plane normal vector and the vertical axis direction vector in the rotation matrix being in the same direction as a constraint condition;

[0048] A correction preparation unit, for determining the side length direction vector of the maximum fitting plane based on point cloud data of projected sampling points on the maximum fitting plane projected from each original sampling point on the surface of the symmetrical plane object;

[0049] A posture correction unit, used for correcting the rotation matrix based on the side length direction vector of the maximum fitting plane to obtain a target rotation matrix;

[0050] The pose determination unit is used to determine the target pose of the symmetrical planar object based on the center of mass position and the target rotation matrix.

[0051] On the other hand, the present application provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the above-mentioned posture estimation method is implemented when the processor executes the computer program.

[0052] The beneficial effects of this application are as follows:

[0053] The present application utilizes the side length direction vector of the maximum fitting plane corresponding to the symmetrical plane object to correct the rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object, so that the main direction in the rotation matrix is ​​always along the side length direction of the symmetrical plane object, thereby reducing the posture estimation deviation of the symmetrical plane object and improving the operation accuracy of the symmetrical plane object.

[0054] Other features and advantages of the present application will be described in the subsequent description, and in part, will become apparent from the description, or will be understood by practicing the present application. The purpose and other advantages of the present application can be realized and obtained by the structures specifically pointed out in the written description, claims, and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The schematic diagram and description of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0056] Figure 1 Schematic diagram of the overview of the posture estimation method in the embodiment of the present application;

[0057] Figure 2 This is a schematic diagram of a specific process of the posture estimation method in an embodiment of the present application;

[0058] Figure 3 Schematic diagram of the estimation effect of the posture estimation method in the embodiment of the present application;

[0059] Figure 4 This is a functional structure diagram of a posture estimation device in an embodiment of the present application;

[0060] Figure 5 Schematic diagram of the hardware structure of the electronic device in the embodiment of the present application. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical solution and beneficial effects of this application clearer, the technical solution of this application will be clearly and completely described below in combination with the embodiments and drawings. Obviously, the described embodiments are only part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0062] In order to facilitate those skilled in the art to better understand the present application, the technical terms involved in the present application are briefly introduced below.

[0063] Symmetrical plane objects are objects whose appearance planes are mirrored along their central axis, such as cubes and cuboids.

[0064] Plane point cloud data is a set of points that describe the surface spatial layout of a symmetrical plane object. In this application, the plane point cloud data includes the point cloud data of each original sampling point on the surface of the plane object; the point cloud data of each original sampling point includes at least three-dimensional coordinates, and may also include additional information such as timestamp, normal vector, color information, reflection intensity, echo number, etc.; wherein the timestamp is the time information used to record the sampling time; the normal vector is a vector used to describe the local surface direction of the original sampling point in three-dimensional space, usually a unit vector; the color information is information used to describe the color attributes of the original sampling point, such as RGB value; the reflection intensity is information used to describe the intensity of the laser pulse reflected back; the echo number is a number used to distinguish multiple echoes formed when the laser pulse encounters multiple surfaces when it reaches the surface of the symmetrical plane object and reflects back.

[0065] The oriented enclosing box algorithm is an algorithm for solving the minimum oriented enclosing box of an object, its centroid and rotation matrix based on principal component analysis (PCA).

[0066] The plane fitting algorithm is an algorithm for fitting the appearance plane of an object based on the plane equation.

[0067] It should be noted that the "first", "second", etc. mentioned in this application are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that such terms can be interchanged where appropriate, so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the "and / or" mentioned in this application describes the association relationship of associated objects, indicating that three relationships may exist, for example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the objects associated before and after are in an "or" relationship.

[0068] After introducing the technical terms involved in this application, the application scenarios and design concepts of this application are briefly introduced.

[0069] In order to solve the problem that when solving the pose of a symmetrical plane object by a traditional directional bounding box algorithm during the operation of the symmetrical plane object, the main direction is along the diagonal direction of the symmetrical plane object, resulting in a deviation in the pose estimation, the present application proposes a pose estimation method based on the side length direction vector of the maximum fitting plane to correct the rotation matrix of the minimum directional bounding box, that is, after obtaining the point cloud data of each original sampling point on the surface of the symmetrical plane object, firstly, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the centroid position and rotation of the minimum directional bounding box corresponding to the symmetrical plane object are determined. Matrix, then, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the maximum fitting plane corresponding to the symmetrical plane object is determined with the constraint that the plane normal vector is the same as the direction of the vertical axis direction vector in the rotation matrix. Secondly, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object projected onto the projected sampling point on the maximum fitting plane, the side length direction vector of the maximum fitting plane is determined, and then the rotation matrix is ​​corrected based on the side length direction vector of the maximum fitting plane to obtain the target rotation matrix. Finally, based on the centroid position and the target rotation matrix, the target posture of the symmetrical plane object is determined. In this way, after solving the rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object, by using the side length direction vector of the maximum fitting plane corresponding to the symmetrical plane object, the rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object is corrected, so that the main direction represented by the rotation matrix is ​​always along the side length direction of the symmetrical plane object, thereby reducing the posture estimation deviation of the symmetrical plane object and improving the operation accuracy of the symmetrical plane object.

[0070] After introducing the application scenarios and design ideas of the present application, the technical solutions provided by the present application are described in detail below.

[0071] This application embodiment provides a method for posture estimation, see Figure 1 As shown, the general process of the posture estimation method provided in the embodiment of the present application is as follows:

[0072] Step 101: Acquire plane point cloud data of a symmetrical plane object; wherein the plane point cloud data includes point cloud data of each original sampling point on the surface of the symmetrical plane object.

[0073] In the embodiment of the present application, when obtaining the planar point cloud data of a symmetrical planar object, any of the following methods may be used but is not limited to:

[0074] (1) Laser scanning method based on laser radar, including:

[0075] Laser pulses are emitted to the surface of the symmetrical plane object by a laser radar, and the reflected signals reflected from the surface of the symmetrical plane object are measured to obtain the point cloud data of each original sampling point on the surface of the symmetrical plane object.

[0076] (2) RGB image and depth data analysis methods based on RGB-D cameras, including:

[0077] The RGB image and depth data of the surface of the symmetrical plane object are collected by an RGB-D camera composed of an RGB camera and a depth sensor. The point cloud data of each original sampling point on the surface of the symmetrical plane object are obtained by analyzing the RGB image and depth data.

[0078] (3) Optical image processing methods based on optical sensors, including:

[0079] An optical sensor is used to capture an optical image of the surface of a symmetrical plane object, and point cloud data of each original sampling point on the surface of the symmetrical plane object is obtained by analyzing the optical image.

[0080] (4) Triangulation based on two or more image acquisition devices, including:

[0081] The surface of a symmetrical plane object is photographed from different angles using two or more image acquisition devices (such as binocular cameras) to obtain surface images of the object at different angles, and the surface images of the object at different angles are processed using a triangulation algorithm to obtain point cloud data of each original sampling point on the surface of the symmetrical plane object.

[0082] (5) Data fusion of two or more sensors based on neural network models, including:

[0083] Through a pre-trained neural network model, two or more sensor data corresponding to the surface of a symmetrical plane object are fused to obtain point cloud data of each original sampling point on the surface of the symmetrical plane object; wherein the sensor data includes data collected by the above-mentioned lidar, optical sensor, RGB-D camera, binocular camera and other sensors.

[0084] Step 102: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, determine the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object.

[0085] In the embodiment of the present application, when determining the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, any of the following methods may be used but is not limited to:

[0086] (1) A one-step solution based on principal component analysis includes the following steps 1021a to 1022a:

[0087] Step 1021a: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the principal component analysis algorithm is used to analyze the eigenvalues ​​and eigenvectors of the symmetrical plane object. Specifically, the point cloud data of each original sampling point is preprocessed; wherein the preprocessing includes but is not limited to: denoising the point cloud data of each original sampling point, such as removing outliers through a filtering algorithm; and / or centering the point cloud data of each original sampling point, such as translating each original sampling point relative to its centroid so that the centroid is located at the origin; constructing a covariance matrix based on the preprocessed point cloud data of each original sampling point, and obtaining the eigenvalues ​​and eigenvectors of the symmetrical plane object by solving the eigenvalues ​​and eigenvectors of the covariance matrix.

[0088] Step 1022a: Based on the eigenvalues ​​and eigenvectors of the symmetric plane object, determine the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetric plane object. Specifically, the eigenvalues ​​of the symmetric plane object are determined as the centroid position, and the eigenvectors of the symmetric plane object are determined as the rotation matrix.

[0089] (2) A cyclic solution method based on principal component analysis includes the following steps 1021b to 1023b:

[0090] Step 1021b: iteratively perform a directed enclosing box solution operation based on the point cloud data of each original sampling point on the surface of the symmetrical plane object; wherein the directed enclosing box solution operation includes the following steps 1 to 7:

[0091] Step 1: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, calculate the point cloud data of each original sampling point projected onto the specified plane. Specifically, the following formula (1) is used to calculate the point cloud data of each original sampling point projected onto the specified plane;

[0092] ...Formula (1)

[0093] In formula (1), p proj represents the point cloud data of the projected sampling point; p represents the point cloud data of the original sampling point; n represents the specified normal vector; n T Represents the transpose of the specified normal vector; p0 represents the point cloud data of the specified sampling point; in practical applications, symmetrical plane objects that usually operate within a certain period of time are objects of specified sizes in the specified working space. Based on this, the specified normal vector can be an empirical value, and the point cloud data of the specified sampling point can also be an empirical value or the point cloud data of an original sampling point.

[0094] It is worth mentioning that in the embodiment of the present application, the point cloud data of each original sampling point can be pre-processed by denoising and / or centering, and then the above formula (1) is used to calculate the point cloud data of each original sampling point after pre-processing projected onto the specified plane, thereby improving the accuracy of the point cloud data; wherein the pre-processing method such as denoising and / or centering is the same as the above-described method, and the repeated parts will not be repeated.

[0095] Step 2: Based on the point cloud data of the projected sampling points projected onto the specified plane by each original sampling point, the concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object; wherein, when the directed enclosing box solution operation is performed for the first time, the learning parameter of the concave hull algorithm is the initialization value; when the directed enclosing box solution operation is not performed for the first time, the learning parameter of the concave hull algorithm is the target value adjusted when the directed enclosing box solution operation was performed last time. Specifically, the point cloud data of the projected sampling points projected onto the specified plane by each original sampling point is Delaunay triangulated to obtain a triangular mesh; according to the learning parameter α, the triangular meshes that do not meet the conditions are filtered out, for example, for each triangular mesh, the radius R of its circumscribed circle is calculated, and if R<α, the triangular mesh is retained; the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object is extracted from the remaining triangular meshes.

[0096] Step 3: Based on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object, the principal component analysis algorithm is used to analyze the eigenvalues ​​and eigenvectors of the symmetrical plane object. Specifically, the current covariance matrix is ​​constructed based on the point cloud data of each contour sampling point, and the eigenvalues ​​and eigenvectors of the symmetrical plane object are obtained by solving the eigenvalues ​​and eigenvectors of the current covariance matrix.

[0097] Step 4: Based on the eigenvalues ​​and eigenvectors of the symmetrical plane object, determine the centroid position and rotation matrix of the current directed bounding box corresponding to the symmetrical plane object. Specifically, the eigenvector of the symmetrical plane object is used as the current principal direction, and the coordinate system is rotated using the current principal direction so that the coordinate axis of the new coordinate system is aligned with the current principal direction. The difference between the maximum and minimum values ​​along each axis direction is calculated in the new coordinate system to determine the current bounding box size; the eigenvector of the symmetrical plane object is determined as the current rotation matrix, and the eigenvalue of the symmetrical plane object is determined as the current centroid position; the current directed bounding box is reconstructed according to the current rotation matrix, the current centroid position and the current bounding box size.

[0098] Step 5: Extract the point cloud data of each contour sampling point on the box boundary of the current directional bounding box.

[0099] Step 6: Perform point cloud registration on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object and the point cloud data of each contour sampling point on the box boundary of the current directed bounding box to obtain the matching score between the concave boundary of the symmetrical plane object and the box boundary of the current directed bounding box. Specifically, based on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object and the point cloud data of each contour sampling point on the box boundary of the current directed bounding box, an iterative closest point (ICP) algorithm is used to calculate the matching score between the concave boundary of the symmetrical plane object and the box boundary of the current directed bounding box.

[0100] Step 7: When it is determined that the matching score is less than the set score threshold, the learning parameters of the concave hull algorithm are adjusted to the target value based on the difference between the concave boundary of the symmetrical plane object and the box boundary of the current oriented bounding box.

[0101] Step 1022b: Determine whether the iteration termination condition is met; if so, execute step 1023b; if not, return to step 1021b; wherein the iteration termination condition is that the matching score is not less than the set score threshold, and / or the current number of iterations is not less than the number threshold.

[0102] Step 1023b: Determine the centroid position and rotation matrix of the current directional bounding box as the centroid position and rotation matrix of the minimum directional bounding box.

[0103] In this way, by iteratively performing a directed enclosing box solution operation based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, a more accurate center of mass position and rotation matrix solution can be achieved, thereby improving the accuracy of the center of mass position and the rotation matrix.

[0104] Step 103: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the maximum fitting plane corresponding to the symmetrical plane object is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint.

[0105] In the embodiment of the present application, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the maximum fitting plane corresponding to the symmetrical plane object is determined with the plane normal vector and the vertical axis direction vector being in the same direction as the constraint condition, but not limited to the following methods:

[0106] Step 1031: Iteratively perform a plane fitting operation based on the point cloud data of each original sampling point on the surface of the symmetrical plane object; wherein the plane fitting operation includes the following steps 1 to 4:

[0107] Step 1: Randomly select a set number of original sampling points from the surface of the symmetrical plane object as candidate sampling points; wherein, among the set number of candidate sampling points selected when the plane fitting operation is currently executed, at least one candidate sampling point is different from the candidate sampling points selected when the plane fitting operation is previously executed. In practical applications, plane fitting requires at least 3 non-collinear points, based on which, the set number can be a positive integer not less than 3.

[0108] Step 2: Based on the point cloud data of each candidate sampling point, the current fitting plane is determined with the constraint that the plane normal vector is in the same direction as the vertical axis direction vector in the rotation matrix; wherein the plane equation is shown in the following formula (2):

[0109] ...Formula (2)

[0110] In formula (2), x, y, z represent the three-dimensional coordinates in the point cloud data; A, B, C, D represent the parameters of the current plane equation. Using the point cloud data of each candidate sampling point, with the plane normal vector and the vertical axis direction vector in the rotation matrix in the same direction as the constraint condition, the four parameters A, B, C, D in the current plane equation can be calculated to obtain the current fitting plane.

[0111] Step 3: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, calculate the distance from each original sampling point on the surface of the symmetrical plane object to the current fitting plane. Specifically, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the following formula (3) is used to calculate the distance from each original sampling point on the surface of the symmetrical plane object to the current fitting plane.

[0112] ...Formula (3)

[0113] In formula (3), Represents the distance from the original sampling point to the current fitting plane; A, B, C, D represent the parameters of the current plane equation; x i ,y i ,z i The 3D coordinates in the point cloud data representing the original sample points.

[0114] Step 4: Determine each original sampling point whose distance is less than the set distance threshold as an inlier, and calculate the current inlier ratio based on the number of inliers and the number of each original sampling point on the surface of the symmetrical plane object.

[0115] Step 1032: Determine whether the current inlier ratio is not less than the set inlier ratio threshold; if so, execute step 1033; if not, return to step 1031.

[0116] Step 1033: Determine the current fitting plane as the maximum fitting plane of the symmetrical plane object.

[0117] It is worth mentioning that in the embodiment of the present application, in order to further improve the efficiency and accuracy of plane fitting, in the process of executing step 1032, it is determined that the current inlier ratio is not less than the set inlier ratio threshold, and it is also possible to first determine whether the current sampling ratio is greater than the set sampling ratio threshold; if so, further execute step 1033; if not, return to step 1031; wherein the current sampling ratio is the ratio of the total number of all different candidate sampling points selected each time the plane fitting operation is performed to the total number of all original sampling points on the surface of the symmetrical plane object.

[0118] In this way, by iteratively performing a plane fitting operation based on point cloud data of each original sampling point on the surface of the symmetrical plane object, a more accurate plane fitting can be achieved, thereby improving the accuracy of the maximum fitting plane.

[0119] Step 104: Determine the side length direction vector of the maximum fitting plane based on the point cloud data of each original sampling point on the surface of the symmetrical plane object projected onto the projected sampling point on the maximum fitting plane.

[0120] In the embodiment of the present application, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object projected onto the projected sampling point on the maximum fitting plane, the side length direction vector of the maximum fitting plane can be determined by, but not limited to, the following methods:

[0121] Step 1041: extracting contour lines of the maximum fitting plane based on the point cloud data of the projected sampling points on the maximum fitting plane projected from the original sampling points on the surface of the symmetrical plane object, specifically including:

[0122] First, based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the point cloud data of each original sampling point projected onto the maximum fitting plane is calculated. Specifically, the point cloud data of each projected sampling point is calculated using the following formula (4).

[0123] ...Formula (4)

[0124] In formula (4), Represent the point cloud data of the i-th original sampling point; Represents the point cloud data of the projected sampling point corresponding to the i-th original sampling point; (A, B, C) represents the normal vector of the maximum fitting plane, and d represents the distance from the i-th original sampling point to the maximum fitting plane.

[0125] Then, based on the point cloud data of the projected sampling points projected onto the maximum fitting plane by each original sampling point, the concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the maximum fitting plane. The contour sampling point extraction process based on the concave hull algorithm is similar to the aforementioned description process, and the repeated parts will not be repeated.

[0126] Finally, straight line extraction is performed on the point cloud data of each contour sampling point to obtain the contour lines of the maximum fitting plane.

[0127] Step 1042: Based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line, the side length direction vector of the maximum fitting plane is calculated. Wherein, when the maximum fitting plane is a rectangle, a square, etc., based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line, the side length direction vector of the maximum fitting plane is calculated, specifically including:

[0128] First, a contour line is selected from the four contour lines of the maximum fitting plane as a reference contour line, and the distances between the reference contour line and the remaining three contour lines of the four contour lines are calculated.

[0129] Then, the contour line with the largest distance among the three contour lines and the reference contour line form a first group of opposite sides, and the remaining two contour lines among the three contour lines form a second group of opposite sides.

[0130] Finally, based on the point cloud data of the intersection between one contour line in the first set of opposite sides and two contour lines in the second set of opposite sides, the side length direction vector of the maximum fitting plane is calculated.

[0131] Step 105: Based on the side length direction vector of the maximum fitting plane, the rotation matrix is ​​corrected to obtain a target rotation matrix.

[0132] In the embodiment of the present application, when correcting the rotation matrix to obtain the target rotation matrix based on the side length direction vector of the maximum fitting plane, the following methods may be used but are not limited to:

[0133] Firstly, based on the side length direction vector and the vertical axis direction vector in the rotation matrix, the vector cross multiplication right-hand rule is used to calculate the vertical axis direction vector.

[0134] Then, based on the side length direction vector, the longitudinal axis direction vector and the vertical axis direction vector, a target rotation matrix is ​​determined. Specifically, the side length direction vector, the longitudinal axis direction vector and the vertical axis direction vector are combined into a target rotation matrix.

[0135] Step 106: Determine the target pose of the symmetrical planar object based on the center of mass position and the target rotation matrix.

[0136] In the embodiment of the present application, the target posture of the symmetrical plane object is composed of the target rotation matrix (that is, the three direction vectors of X, Y, and Z) and the center of mass position (that is, the three-dimensional coordinates of X, Y, and Z), which can also be called a rigid transformation matrix.

[0137] Taking "the symmetrical plane object is a rectangular object" as an example, the pose estimation method provided in the embodiment of the present application is further described in detail. Figure 2 As shown, the specific process of the posture estimation method provided in the embodiment of the present application is as follows:

[0138] Step 201: Obtain point cloud data of each original sampling point on the surface of the symmetrical plane object. The point cloud data of each original sampling point may be obtained in any of the above-mentioned ways, and the repeated parts will not be repeated.

[0139] Step 202: performing pre-processing such as denoising and / or centering on the point cloud data of each original sampling point. For example, outliers are removed by filtering algorithms and / or each original sampling point is translated relative to its centroid so that the centroid is located at the origin.

[0140] Step 203: Calculate the point cloud data of the projected sampling points projected onto the designated plane by the preprocessed original sampling points, wherein the designated plane is determined based on the designated normal vector and the point cloud data of the designated sampling points.

[0141] Step 204: based on the point cloud data of the projection sampling points projected onto the designated plane by the preprocessed original sampling points, iteratively perform a directed enclosing box solving operation; wherein the directed enclosing box solving operation includes the following steps 2041 to 2048:

[0142] Step 2041: Based on the pre-processed point cloud data of each original sampling point on the surface of the symmetrical plane object, calculate the point cloud data of the projected sampling points projected onto the designated plane by each of the pre-processed original sampling points.

[0143] Step 2042: Based on the point cloud data of the projection sampling points projected onto the specified plane by each of the preprocessed original sampling points, a concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object; wherein, when the directed enclosing box solution operation is performed for the first time, the learning parameters of the concave hull algorithm are initialization values; when the directed enclosing box solution operation is not performed for the first time, the learning parameters of the concave hull algorithm are target values ​​adjusted when the directed enclosing box solution operation was last performed.

[0144] Step 2043: construct a current covariance matrix based on the point cloud data of the contour sampling points corresponding to each original sampling point, and obtain the eigenvalues ​​and eigenvectors of the symmetrical plane object by solving the eigenvalues ​​and eigenvectors of the current covariance matrix.

[0145] Step 2044: Use the feature vector of the symmetrical plane object as the current principal direction, use the current principal direction to rotate the coordinate system so that the coordinate axis of the new coordinate system is aligned with the current principal direction, and then calculate the difference between the maximum and minimum values ​​along each axis direction in the new coordinate system to determine the current bounding box size.

[0146] Step 2045: Determine the eigenvector of the symmetrical plane object as the current rotation matrix, and determine the eigenvalue of the symmetrical plane object as the current center of mass position; reconstruct the current directional bounding box according to the current rotation matrix, the current center of mass position and the current bounding box size.

[0147] Step 2046: Extract the point cloud data of each contour sampling point on the box boundary of the current directional bounding box.

[0148] Step 2047: Based on the point cloud data of each contour sampling point on the concave boundary of the symmetrical planar object and the point cloud data of each contour sampling point on the box boundary of the current directed bounding box, an iterative closest point algorithm is used to calculate the matching score between the concave boundary of the symmetrical planar object and the box boundary of the current directed bounding box.

[0149] Step 2048: When it is determined that the matching score is less than the set score threshold, the learning parameters of the concave hull algorithm are adjusted to the target value based on the difference between the concave boundary of the symmetrical plane object and the box boundary of the current oriented bounding box.

[0150] Step 205: Determine whether the iteration termination condition is met; if so, execute step 206; if not, return to step 204; wherein the iteration termination condition is that the matching score is not less than the set score threshold, and / or the current iteration number is not less than the number threshold.

[0151] Step 206: The centroid position and rotation matrix of the current directed bounding box solved when the directed bounding box solving operation is currently being performed are determined as the centroid position and rotation matrix of the minimum directed bounding box.

[0152] Step 207: Iteratively perform a plane fitting operation based on the point cloud data of each original sampling point on the surface of the symmetrical plane object; wherein the plane fitting operation includes the following steps 2071 to 2074:

[0153] Step 2071: Randomly select a set number of original sampling points from the original sampling points on the surface of the symmetrical plane object as candidate sampling points; wherein, among the currently selected set number of candidate sampling points, at least one candidate sampling point is different from all previously selected candidate sampling points; and the set number is a positive integer not less than 3.

[0154] Step 2072: Based on the point cloud data of each candidate sampling point, the current fitting plane is determined with the constraint that the plane normal vector is in the same direction as the vertical axis direction vector in the rotation matrix.

[0155] Step 2073: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, calculate the distance from each original sampling point on the surface of the symmetrical plane object to the current fitting plane.

[0156] Step 2074: determine each original sampling point whose distance is less than the set distance threshold as an inlier, and calculate the current inlier ratio based on the number of inliers and the number of each original sampling point on the surface of the symmetrical plane object.

[0157] Step 208: Determine whether the current inlier ratio calculated when the plane fitting operation is currently being performed is not less than the set inlier ratio threshold; if so, execute step 209; if not, return to step 207.

[0158] Step 209: Determine whether the current sampling ratio is greater than the set sampling ratio threshold; if so, execute step 210; if not, return to step 207; wherein the current sampling ratio is the ratio of the total number of all different candidate sampling points selected each time the plane fitting operation is performed to the total number of all original sampling points on the surface of the symmetrical plane object.

[0159] Step 210: Determine the current fitting plane fitted during the current plane fitting operation as the maximum fitting plane of the symmetrical plane object.

[0160] Step 211: Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, calculate the point cloud data of the projected sampling point projected onto the maximum fitting plane by each original sampling point.

[0161] Step 212: Based on the point cloud data of the projected sampling points of each original sampling point projected onto the maximum fitting plane, a concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the maximum fitting plane.

[0162] Step 213: extract straight lines from the point cloud data of each contour sampling point to obtain four contour lines of the maximum fitting plane.

[0163] Step 214: Select one contour line from the four contour lines of the maximum fitting plane as a reference contour line, and calculate the distance between the reference contour line and the remaining three contour lines of the four contour lines.

[0164] Step 215: The contour line with the largest distance among the three contour lines and the reference contour line form a first set of opposite sides, and the remaining two contour lines among the three contour lines form a second set of opposite sides.

[0165] Step 216: Calculate the side length direction vector of the maximum fitting plane based on the point cloud data of the intersection between one contour line in the first set of opposite sides and two contour lines in the second set of opposite sides.

[0166] Step 217: Based on the side length direction vector and the vertical axis direction vector in the rotation matrix, the vertical axis direction vector is calculated using the vector cross multiplication right-hand rule.

[0167] Step 218: Combine the side length direction vector, the longitudinal axis direction vector and the vertical axis direction vector into a target rotation matrix, and determine the rigid transformation matrix composed of the target rotation matrix and the center of mass position of the minimum directed bounding box as the target posture of the symmetrical plane object.

[0168] In the embodiment of the present application, on the one hand, an iterative directed bounding box algorithm is used to iteratively align the point cloud data of the outline of the directed bounding box with the point cloud data of the outline of the symmetrical plane object, and solve the minimum directed bounding box, so as to obtain a more accurate bounding box size (i.e., length and width), center of mass position and vertical axis direction vector (i.e., Z axis direction vector). On the other hand, the maximum fitting plane is used to identify the long and short sides of the symmetrical plane object, and the direction vector of the long side is determined as the horizontal axis direction vector (i.e., X axis direction vector) through the intersection between the long and short sides, and the vertical axis direction vector (i.e., Y axis direction vector) is calculated through the vector cross product that conforms to the right-hand rule, so as to obtain a more accurate target rotation matrix (i.e., X, Y, Z three direction vectors), thereby obtaining a more accurate target posture composed of the target rotation matrix (i.e., X, Y, Z three direction vectors) and the center of mass position (i.e., X, Y, Z three-dimensional coordinates). See. Figure 3 As shown, by using the long side direction vector of the maximum fitting plane to correct the rotation matrix of the minimum oriented bounding box, the main direction of the target posture can be always along the long side direction of the symmetrical plane object, thereby reducing the posture estimation deviation of the symmetrical plane object and improving the operation accuracy of the symmetrical plane object. For example, when grasping a symmetrical plane object, by using the long side direction vector of the maximum fitting plane to correct the rotation matrix of the minimum oriented bounding box, the main direction of the solved target posture can be always along the long side direction of the symmetrical plane object, thereby reducing the difficulty of grasping and improving the efficiency of grasping.

[0169] Based on the above embodiments, the present application provides a posture estimation device. Figure 4 As shown, the posture estimation device 400 provided in the embodiment of the present application at least includes:

[0170] The point cloud acquisition unit 401 is used to acquire the plane point cloud data of the symmetrical plane object; wherein the plane point cloud data includes the point cloud data of each original sampling point on the surface of the symmetrical plane object;

[0171] The bounding box determination unit 402 is used to determine the centroid position and rotation matrix of the minimum directional bounding box corresponding to the symmetrical plane object based on the point cloud data of each original sampling point on the surface of the symmetrical plane object;

[0172] A plane fitting unit 403 is used to determine the maximum fitting plane corresponding to the symmetrical plane object based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, with the plane normal vector and the vertical axis direction vector in the rotation matrix being in the same direction as a constraint condition;

[0173] A correction preparation unit 404 is used to determine the side length direction vector of the maximum fitting plane based on the point cloud data of the projected sampling points on the maximum fitting plane projected from each original sampling point on the surface of the symmetrical plane object;

[0174] The posture correction unit 405 is used to correct the rotation matrix based on the side length direction vector of the maximum fitting plane to obtain a target rotation matrix;

[0175] The pose determination unit 406 is used to determine the target pose of the symmetrical planar object based on the center of mass position and the target rotation matrix.

[0176] In a possible implementation, the bounding box determining unit 402 is used to:

[0177] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the principal component analysis algorithm is used to analyze the eigenvalue and eigenvector of the symmetrical plane object;

[0178] Based on the eigenvalues ​​and eigenvectors of the symmetric plane object, the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetric plane object are determined.

[0179] In a possible implementation, the bounding box determining unit 402 is used to:

[0180] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the directed enclosing box solving operation is iteratively performed until it is determined that the iteration termination condition is met, and the centroid position and rotation matrix of the current directed enclosing box solved when the directed enclosing box solving operation is currently performed are determined as the centroid position and rotation matrix of the minimum directed enclosing box; wherein the directed enclosing box solving operation includes:

[0181] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the point cloud data of the projected sampling point projected onto the specified plane by each original sampling point is calculated;

[0182] Based on the point cloud data of the projected sampling points projected onto the specified plane by each original sampling point, the concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object; wherein, when the directed enclosing box solution operation is performed for the first time, the learning parameter of the concave hull algorithm is the initialization value; when the directed enclosing box solution operation is not performed for the first time, the learning parameter of the concave hull algorithm is the target value adjusted when the directed enclosing box solution operation was performed last time;

[0183] Based on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object, the principal component analysis algorithm is used to analyze the eigenvalue and eigenvector of the symmetrical plane object;

[0184] Based on the eigenvalues ​​and eigenvectors of the symmetric plane object, determine the centroid position and rotation matrix of the current directed bounding box corresponding to the symmetric plane object;

[0185] Extract the point cloud data of each contour sampling point on the box boundary of the current directional bounding box;

[0186] Performing point cloud registration on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object and the point cloud data of each contour sampling point on the box boundary of the current directed bounding box to obtain a matching score between the concave boundary of the symmetrical plane object and the box boundary of the current directed bounding box;

[0187] When it is determined that the matching score is less than the set score threshold, the learning parameters of the concave hull algorithm are adjusted to the target value based on the difference between the concave boundary of the symmetric plane object and the box boundary of the current oriented bounding box.

[0188] In a possible implementation, the plane fitting unit 403 is used to:

[0189] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the plane fitting operation is iteratively performed until it is determined that the current inner point ratio calculated when the plane fitting operation is currently performed is not less than the set inner point ratio threshold, and the current fitting plane fitted when the plane fitting operation is currently performed is determined as the maximum fitting plane of the symmetrical plane object; wherein the plane fitting operation includes:

[0190] From each original sampling point on the surface of the symmetrical plane object, a set number of original sampling points are randomly selected as candidate sampling points;

[0191] Based on the point cloud data of each candidate sampling point, the current fitting plane is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint;

[0192] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the distance from each original sampling point on the surface of the symmetrical plane object to the current fitting plane is calculated;

[0193] Each original sampling point whose distance is less than a set distance threshold is determined as an inner point, and based on the number of inner points and the number of each original sampling point on the surface of the symmetrical plane object, a current inner point ratio is calculated.

[0194] In a possible implementation, the correction preparation unit 404 is used to:

[0195] Based on the point cloud data of the projected sampling points on the maximum fitting plane projected from each original sampling point on the surface of the symmetrical plane object, each contour line of the maximum fitting plane is extracted;

[0196] Based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line, the side length direction vector of the maximum fitting plane is calculated.

[0197] In a possible implementation, the correction preparation unit 404 is used to:

[0198] Based on the point cloud data of each original sampling point on the surface of the symmetrical plane object, the point cloud data of the projected sampling point projected onto the maximum fitting plane by each original sampling point is calculated;

[0199] Based on the point cloud data of the projected sampling points projected onto the maximum fitting plane by each original sampling point, the concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the maximum fitting plane;

[0200] The point cloud data of each contour sampling point is extracted by straight line extraction to obtain the contour lines of the maximum fitting plane.

[0201] In a possible implementation manner, the maximum fitting plane is a square; the correction preparation unit 404 is used to:

[0202] Select one contour line from the four contour lines of the maximum fitting plane as a reference contour line, and calculate the distance between the reference contour line and the remaining three contour lines of the four contour lines;

[0203] The contour line with the largest distance among the three contour lines and the reference contour line form a first set of opposite sides, and the remaining two contour lines among the three contour lines form a second set of opposite sides;

[0204] Based on the point cloud data of the intersection between one contour line in the first set of opposite sides and two contour lines in the second set of opposite sides, the side length direction vector of the maximum fitting plane is calculated.

[0205] In a possible implementation, the posture correction unit 405 is used to:

[0206] Based on the side length direction vector and the vertical axis direction vector in the rotation matrix, the horizontal axis direction vector is calculated using the vector cross multiplication right-hand rule;

[0207] The target rotation matrix is ​​determined based on the side length direction vector, the vertical axis direction vector and the horizontal axis direction vector.

[0208] It should be noted that the principle of solving the technical problem by the posture estimation device 400 provided in the embodiment of the present application is similar to the posture estimation method provided in the embodiment of the present application. Therefore, the implementation of the posture estimation device 400 provided in the embodiment of the present application can refer to the implementation of the posture estimation method provided in the embodiment of the present application, and the repeated parts will not be repeated.

[0209] After introducing the posture estimation method and device provided in the embodiments of the present application, the electronic device provided in the embodiments of the present application is briefly introduced next.

[0210] See also Figure 5 As shown, the electronic device 500 provided in the embodiment of the present application includes at least a processor 501, a memory 502, and a computer program stored in the memory 502 and executable on the processor 501. When the processor 501 executes the computer program, the posture estimation method provided in the embodiment of the present application is implemented.

[0211] The memory 502 may include a readable storage medium in the form of a volatile memory, such as a random access memory (RAM) 5021 and / or a cache memory 5022 , and may also include a read-only memory (ROM) 5023 and / or a program tool 5025 having a group (at least one) of program modules 5024 .

[0212] The electronic device 500 may also include a bus 503 connecting different components (including the processor 501 and the memory 502). In addition, the electronic device 500 may communicate with one or more external devices 504 (such as a keyboard, a computer, a router, a modem, etc.), and such communication may be performed through an input / output (I / O) interface 505. In addition, the electronic device 500 may also communicate with one or more networks (such as a local area network, a wide area network, a public network, etc.) through a network adapter 506. Figure 5 As shown, the network adapter 506 communicates with other modules of the electronic device 500 via the bus 503 .

[0213] It should be noted that Figure 5 The electronic device 500 shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0214] In addition, the embodiment of the present application further provides a computer-readable storage medium, which stores computer instructions, and when the computer instructions are executed by the processor, the pose estimation method provided in the embodiment of the present application is implemented. Specifically, the computer instructions can be built-in or installed in the processor, so that the processor can implement the pose estimation method provided in the embodiment of the present application by executing the built-in or installed computer instructions.

[0215] In addition, the posture estimation method provided in the embodiment of the present application can also be implemented as a computer program product, which includes a program code. When the program code is run on a processor, it implements the posture estimation method provided in the embodiment of the present application.

[0216] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they are aware of the basic creative concepts. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present application.

[0217] Obviously, those skilled in the art can make various changes and modifications to the embodiments of the present application without departing from the spirit and scope of the embodiments of the present application. Thus, if these modifications and variations of the embodiments of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A pose estimation method, characterized in that: include: Acquire plane point cloud data of a symmetrical plane object; wherein the plane point cloud data includes point cloud data of each original sampling point on the surface of the symmetrical plane object; Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, determining the centroid position and rotation matrix of the minimum directional bounding box corresponding to the symmetrical plane object; Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, a maximum fitting plane corresponding to the symmetrical plane object is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint condition; Determine the side length direction vector of the maximum fitting plane based on point cloud data of each of the original sampling points on the surface of the symmetrical plane object projected onto the projected sampling points on the maximum fitting plane; Based on the side length direction vector of the maximum fitting plane, correcting the rotation matrix to obtain a target rotation matrix; Based on the center of mass position and the target rotation matrix, a target pose of the symmetrical planar object is determined.

2. The method for posture estimation according to claim 1, wherein: Determining the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object includes: Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, a principal component analysis algorithm is used to analyze the eigenvalues ​​and eigenvectors of the symmetrical plane object; Based on the eigenvalue and the eigenvector of the symmetric plane object, a centroid position and a rotation matrix of a minimum directional bounding box corresponding to the symmetric plane object are determined.

3. The method for posture estimation according to claim 1, wherein: Determining the centroid position and rotation matrix of the minimum directed bounding box corresponding to the symmetrical plane object based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object includes: Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, iteratively perform a directed enclosing box solving operation until it is determined that an iteration termination condition is satisfied, and determine the centroid position and rotation matrix of the current directed enclosing box solved when the directed enclosing box solving operation is currently performed as the centroid position and rotation matrix of the minimum directed enclosing box; wherein the directed enclosing box solving operation includes: Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, calculate the point cloud data of the projected sampling points projected onto the designated plane by each of the original sampling points; Based on the point cloud data of the projected sampling points projected onto the designated plane by each of the original sampling points, a concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object; wherein, when the directed enclosing box solution operation is performed for the first time, the learning parameter of the concave hull algorithm is an initialization value; when the directed enclosing box solution operation is not performed for the first time, the learning parameter of the concave hull algorithm is a target value adjusted when the directed enclosing box solution operation was last performed; Based on the point cloud data of each contour sampling point on the concave boundary of the symmetrical plane object, a principal component analysis algorithm is used to analyze the eigenvalue and eigenvector of the symmetrical plane object; Based on the eigenvalue and the eigenvector of the symmetric plane object, determine the centroid position and the rotation matrix of the current directed bounding box corresponding to the symmetric plane object; Extracting point cloud data of each contour sampling point on the box boundary of the current directional bounding box; Performing point cloud registration on the point cloud data of each of the contour sampling points on the concave boundary of the symmetrical plane object and the point cloud data of each of the contour sampling points on the box boundary of the current directional bounding box to obtain a matching score between the concave boundary of the symmetrical plane object and the box boundary of the current directional bounding box; When it is determined that the matching score is less than a set score threshold, the learning parameter of the concave hull algorithm is adjusted to a target value based on a difference between the concave boundary of the symmetrical planar object and the box boundary of the current oriented bounding box.

4. The method for posture estimation according to claim 1, wherein: Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, the maximum fitting plane corresponding to the symmetrical plane object is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint condition, including: Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, iteratively perform a plane fitting operation until it is determined that the current inlier ratio calculated when the plane fitting operation is currently performed is not less than a set inlier ratio threshold, and the current fitting plane fitted when the plane fitting operation is currently performed is determined as the maximum fitting plane of the symmetrical plane object; wherein the plane fitting operation includes: Randomly selecting a set number of original sampling points from each of the original sampling points on the surface of the symmetrical plane object as candidate sampling points; Based on the point cloud data of each of the candidate sampling points, a current fitting plane is determined with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint; Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, calculating the distance from each of the original sampling points on the surface of the symmetrical plane object to the current fitting plane; Each of the original sampling points whose distance is less than a set distance threshold is determined as an inner point, and a current inner point ratio is calculated based on the number of the inner points and the number of the original sampling points on the surface of the symmetrical plane object.

5. The method for posture estimation according to claim 1, wherein: Determining the side length direction vector of the maximum fitting plane based on point cloud data of each of the original sampling points on the surface of the symmetrical plane object projected onto the projected sampling points on the maximum fitting plane includes: Extracting each contour line of the maximum fitting plane based on point cloud data of each of the original sampling points on the surface of the symmetrical plane object projected onto the projected sampling points on the maximum fitting plane; Based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line, the side length direction vector of the maximum fitting plane is calculated.

6. The method for posture estimation according to claim 5, characterized in that: Extracting each contour line of the maximum fitting plane based on point cloud data of each of the original sampling points on the surface of the symmetrical plane object projected onto the projected sampling points on the maximum fitting plane, comprising: Based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object, calculate the point cloud data of the projected sampling points projected onto the maximum fitting plane by each of the original sampling points; Based on the point cloud data of the projected sampling points projected onto the maximum fitting plane by each of the original sampling points, a concave hull algorithm is used to extract the point cloud data of each contour sampling point on the concave boundary of the maximum fitting plane; Linear extraction is performed on the point cloud data of each of the contour sampling points to obtain contour lines of the maximum fitting plane.

7. The method for posture estimation according to claim 5, wherein: The maximum fitting plane is a square; Calculating the side length direction vector of the maximum fitting plane based on the point cloud data of the intersection points corresponding to the intersecting contour lines in each contour line includes: Selecting one contour line from the four contour lines of the maximum fitting plane as a reference contour line, and calculating the distance between the reference contour line and the remaining three contour lines of the four contour lines; The contour line with the largest distance among the three contour lines and the reference contour line form a first set of opposite sides, and the remaining two contour lines among the three contour lines form a second set of opposite sides; The side length direction vector of the maximum fitting plane is calculated based on the point cloud data of the intersection between one contour line in the first group of opposite sides and two contour lines in the second group of opposite sides.

8. The method for posture estimation according to any one of claims 1 to 7, characterized in that: Based on the side length direction vector of the maximum fitting plane, the rotation matrix is ​​corrected to obtain a target rotation matrix, including: Based on the side length direction vector and the vertical axis direction vector in the rotation matrix, the horizontal axis direction vector is calculated using the vector cross multiplication right-hand rule; The target rotation matrix is ​​determined based on the side length direction vector, the vertical axis direction vector and the horizontal axis direction vector.

9. A posture estimation device, characterized in that: include: A point cloud acquisition unit, used to acquire plane point cloud data of a symmetrical plane object; wherein the plane point cloud data includes point cloud data of each original sampling point on the surface of the symmetrical plane object; A bounding box determination unit, configured to determine the centroid position and rotation matrix of the minimum directional bounding box corresponding to the symmetrical plane object based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object; A plane fitting unit, configured to determine a maximum fitting plane corresponding to the symmetrical plane object based on the point cloud data of each of the original sampling points on the surface of the symmetrical plane object and with the plane normal vector being in the same direction as the vertical axis direction vector in the rotation matrix as a constraint condition; A correction preparation unit, configured to determine the side length direction vector of the maximum fitting plane based on point cloud data of projected sampling points on the maximum fitting plane projected from each of the original sampling points on the surface of the symmetrical plane object; A posture correction unit, used for correcting the rotation matrix based on the side length direction vector of the maximum fitting plane to obtain a target rotation matrix; The pose determination unit is used to determine the target pose of the symmetrical plane object based on the center of mass position and the target rotation matrix.

10. An electronic device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the posture estimation method according to any one of claims 1 to 8 when executing the computer program.

Images