Pose estimation method, computer equipment and storage medium

Abstract

The invention discloses a pose estimation method, computer equipment and a storage medium. The method comprises the following steps: acquiring point cloud data of a current frame, and performing feature point extraction on the point cloud data to obtain at least two types of feature points; performing region division on the feature points of each category to obtain at least two categories of feature points corresponding to each region; clustering processing is carried out on the feature points, corresponding to the areas, of the categories to obtain centroid point sets corresponding to at least two categories; and performing pose estimation by using the centroid point sets corresponding to the at least two categories to obtain the current pose of the target equipment. According to the scheme, the accuracy of pose estimation can be improved.

Description

Technical Field

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

[0002] As devices become more and more intelligent, they can be equipped with sensors to realize positioning, navigation and other functions. Taking intelligent devices including mobile robots as an example, SLAM (Simultaneous Localization and Mapping) technology can be used to estimate their own motion in unknown environments and build maps of the surrounding environment, which has a wide range of applications in the fields of positioning and navigation.

[0003] At present, odometers or lidar are generally used for pose estimation. However, when the above two methods are used for pose estimation for positioning, the accuracy of pose estimation is low. Summary of the invention

[0004] The main technical problem solved by the present application is to provide a posture estimation method, a computer device and a storage medium, which can improve the accuracy of posture estimation.

[0005] In order to solve the above problems, the first aspect of the present application provides a pose estimation method, which includes: obtaining point cloud data of the current frame, and extracting feature points from the point cloud data to obtain feature points of at least two categories; dividing the feature points of each category into regions to obtain feature points of at least two categories corresponding to each region; clustering the feature points of each category corresponding to each region to obtain a set of centroid points corresponding to at least two categories; and performing pose estimation using the set of centroid points corresponding to at least two categories to obtain the current pose of the target device.

[0006] In order to solve the above-mentioned problem, the second aspect of the present application provides a computer device, which includes a memory and a processor coupled to each other, wherein program data is stored in the memory, and the processor is used to execute the program data to implement any step of the above-mentioned posture estimation method.

[0007] In order to solve the above-mentioned problem, the third aspect of the present application provides a computer-readable storage medium, which stores program data that can be executed by a processor, and the program data is used to implement any step of the above-mentioned posture estimation method.

[0008] The above scheme extracts feature points from the point cloud data of the current frame to obtain at least two categories of feature points, divides the feature points of each category into regions, obtains feature points of at least two categories corresponding to each region, clusters the feature points of each category corresponding to each region, and obtains sets of centroid points corresponding to at least two categories. This can more efficiently obtain more stable centroid points without processing all point clouds, thereby reducing the amount of calculation for pose estimation and the amount of storage for point cloud data. In addition, since it contains at least two categories of centroid point sets, and each centroid point set contains relatively stable centroid points, the centroid point sets corresponding to at least two categories can be used to perform pose estimation to obtain the current pose of the target device, thereby improving the accuracy of pose estimation.

[0009] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] In order to more clearly illustrate the technical solutions in this application, the following briefly introduces the drawings required in the description of the embodiments. Obviously, the drawings described below are only some embodiments of this application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work. Among them:

[0011] Figure 1 It is a flowchart of an embodiment of a posture estimation method of the present application;

[0012] Figure 2 This is an exemplary schematic diagram of an embodiment of the working environment of the mobile robot of the present application;

[0013] Figure 3 This application Figure 1 A schematic diagram of a flow chart of an embodiment of step S13;

[0014] Figure 4 This application Figure 1 A schematic diagram of a flow chart of an embodiment of step S14;

[0015] Figure 5 This is an example schematic diagram of a key point of an embodiment of a preset virtual image in the current frame of the present application;

[0016] Figure 6 It is a structural schematic diagram of an embodiment of a posture estimation device of the present application;

[0017] Figure 7 It is a structural schematic diagram of an embodiment of a computer device of the present application;

[0018] Figure 8 It is a structural diagram of an embodiment of a computer-readable storage medium of the present application. DETAILED DESCRIPTION

[0019] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0020] The terms "first" and "second" in this application are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of indicated technical features. Thus, the features defined as "first" and "second" can explicitly or implicitly include at least one of the features. In the description of this application, the meaning of "multiple" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally also includes steps or units that are not listed, or optionally also includes other steps or units inherent to these processes, methods, products or devices.

[0021] Reference to "embodiments" in this application means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0022] The term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there may be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the objects associated before and after are in an "or" relationship. In addition, "many" in this article means two or more than two. In addition, the term "at least one" in this article means any combination of at least two of any one or more of a plurality of, for example, including at least one of A, B, and C, can mean including any one or more elements selected from the set consisting of A, B, and C.

[0023] The present application provides the following embodiments, and each embodiment is described in detail below.

[0024] It is understandable that the pose estimation method in the present application can be executed by a computer device, and the computer device can be any device with processing capabilities, such as a target device, a computer, a server, a mobile robot, etc., and the present application does not limit this. In some possible implementations, the pose estimation method can be implemented by a processor calling program data stored in a memory.

[0025] See also Figure 1 , Figure 1 1 is a flow chart of an embodiment of a method for posture estimation of the present application. The method may include the following steps:

[0026] S11: Acquire point cloud data of the current frame, and extract feature points from the point cloud data to obtain feature points of at least two categories.

[0027] The target device may be equipped with sensors such as radar devices and odometers, which can be used for posture estimation and navigation, positioning and other functions. The target device may be a mobile robot, a vehicle and other equipment, such as a handling robot, a service robot, a sweeping robot, etc. The following description of this application takes a mobile robot as an example, and this application does not limit the target device. Among them, the radar device is used to collect point cloud data of the current frame. For example, the radar device may be a laser radar, a millimeter wave radar, etc. This application takes a laser radar as an example for description. This application does not limit the radar device.

[0028] Exemplarily, taking a mobile robot as an example, the mobile robot includes a mobile chassis (such as a motion controller, a motor, a battery, an embedded computer, an odometer, etc.) and a radar device. For example, the radar device can be a 3D (3 Dimensions) laser radar sensor. This application is not limited to this. The mobile robot can obtain point cloud data of the current frame in the target environment through the radar device. The target environment can be the environment where the mobile robot is located, such as a storage environment, a mobile area, etc.

[0029] See also Figure 2 Taking the working environment of a mobile robot as an example, the mobile robot can use a radar device to generate a 3D high-precision point cloud map based on a warehouse environment. The point cloud map includes components such as shelves, goods, and supporting walls. The mobile robot can use the odometer to estimate the change in the movement of the mobile robot, and use the radar device to scan the environmental contour point cloud data for positioning and navigation. For this environment scan to obtain point cloud data, multiple point cloud data can belong to the same laser beam, that is, multiple point cloud data can be point cloud data on the same laser beam.

[0030] Then, feature point extraction is performed on the point cloud data to extract feature points of at least two categories. The classification can be performed according to the curvature of each point cloud data. For example, the at least two categories can include at least two categories such as surface category, line category, and corner category. That is, the at least two categories of feature points can include surface points of the surface category, line points of the line category, corner points of the corner category, etc. This application does not limit the category.

[0031] This application uses at least two types of feature points, including surface points of the surface category and line points of the line category, as an example for illustration. Feature points can be extracted from point cloud data in the following manner.

[0032] Get the curvature of each point cloud data. The curvature is calculated by using the current point cloud data and several other point cloud data adjacent to the same laser beam when each point cloud data is used as the current point cloud data. The several other point cloud data can be a set number of other point cloud data, and the several other point cloud data and the current point cloud data belong to the same laser beam. The difference between the current point cloud data and the other point cloud data can be obtained respectively, and the curvature of the current point cloud data can be obtained by using each difference and the set number.

[0033] In some implementations, after obtaining the point cloud data of the current frame, the curvature of each point cloud data of the current frame may be obtained by the following formula:

[0034]

[0035] Among them, C i Represents the current point cloud data p i The curvature, p i is the point cloud data on a certain laser beam in each point cloud data, p j Indicates that the current point cloud data pi belongs to several other point cloud data adjacent to the same laser beam, P s Indicates the current point cloud data p i A collection of point cloud data belonging to the same laser beam. N represents the number of other point cloud data, that is, the set number. For example, N is a natural number, such as 8, 10, 12, etc. This application does not limit N.

[0036] In some embodiments, in response to the curvature C i is greater than the first curvature threshold Cth1, the point cloud data is determined to be a line point of the line category. i If the first curvature threshold is less than the second curvature threshold Cth2, the point cloud data is determined to be a surface point of the surface category. The first curvature threshold is greater than the second curvature threshold. For example, the first curvature threshold may be 10, and the second curvature threshold may be 0.1. The present application does not limit the first curvature threshold and the second curvature threshold.

[0037] S12: Divide the feature points of each category into regions to obtain feature points of at least two categories corresponding to each region.

[0038] Each point cloud data contains its coordinates, so the extracted feature points also contain the corresponding coordinates (pix, piy), that is, the coordinates of the X-axis and Y-axis. The feature points of each category can be divided into regions according to the coordinates to obtain the feature points of each category divided into each region. For example, the feature points of the line category and the surface category can be divided into regions to obtain the feature points of the line category divided into each region, and the feature points of the surface category divided into each region can be obtained.

[0039] In some implementations, the regions are divided according to the angle direction and the radius direction collected by the radar device to obtain a plurality of regions. For example, the coordinate system of the radar device is facing the upper left front, and each point cloud data is divided into regions according to the deflection angle (i.e., the angle direction) and the horizontal radius direction (i.e., the radius direction) collected by the radar device.

[0040] Among them, the angular direction includes the angular resolution Nsec, and the radial direction includes the radial resolution Nr. That is, the size of the divided several regions M is Nr*Nsec. In this way, it can be divided into several sector-shaped regions. Exemplarily, the angular resolution N_sec is 90 degrees, and the radial resolution Nr is 30 meters. This application does not limit the angular resolution and radial resolution.

[0041] Then, the feature points of each category are divided into several regions to obtain feature points of at least two categories corresponding to each region. The feature points of each category can be divided into several regions according to the coordinates of the feature points of each category to obtain feature points of each category corresponding to each region.

[0042] In some implementations, feature points of each category may be used as current feature points, for example, line points of the line category and surface points of the surface category may be used as current feature points in sequence, so as to be divided into corresponding regions according to the coordinates of the current feature points.

[0043] The radius information is obtained by using the first preset relationship between the coordinates of the current feature point and the first value of the radius resolution; and the angle information is obtained by using the second preset relationship between the coordinates of the current feature point and the second value of the angle resolution. Among them, the first preset relationship includes a square root relationship. The second preset relationship includes an inverse tangent relationship, etc. This application is not limited to this. The radius information and the angle information can represent the index position of the area to be divided. Based on the radius information and the angle information, the current feature point is divided into the corresponding area. In this way, each category can be divided into regions separately, and each feature point can be divided into corresponding regions to obtain feature points of at least two categories corresponding to each region.

[0044] In some implementations, the radius information and angle information may be calculated using the coordinates of the current feature point through the following formula, that is, the index position of the divided area may be obtained:

[0045]

[0046] Among them, pix and piy represent the current coordinate point p i The coordinates of r idx Indicates radius information, sec idx represents angle information, Nr represents radius resolution, and Nsec represents angle resolution.

[0047] Through the above method, each feature point can be divided into a corresponding area more accurately to obtain the feature points of the corresponding area of ​​each category.

[0048] S13: clustering the feature points of each region corresponding to each category to obtain a set of centroid points corresponding to at least two categories.

[0049] By clustering the feature points contained in each region of each category in turn, the clustering results of each region corresponding to each category can be obtained. Then, based on the clustering results, the centroid points corresponding to each region can be obtained, thereby obtaining a set of centroid points of each category. For example, since the line points of the line category directly extracted are unstable and divergent, the line points that appear in the current frame may not appear in the next frame. Therefore, the line category and the surface category can be clustered separately to obtain the clustering results of each category. Based on the clustering results of each category, the set of centroid points of the line category and the set of centroid points of the surface category can be obtained.

[0050] In the clustering process, a clustering algorithm may be used for clustering processing. For example, the clustering algorithms include: K-means algorithm, DBSCAN algorithm, hierarchical clustering, spectral clustering, etc. This application does not limit the clustering processing.

[0051] In some embodiments, see Figure 3 , step S13 of the above embodiment can be further expanded. Clustering is performed on the feature points of each region corresponding to each category to obtain a set of centroid points corresponding to at least two categories. This embodiment may include the following steps:

[0052] S131: Each area corresponding to each category is taken as the current area.

[0053] Each category is clustered separately, and for each category, each region can be used as the current region in turn to cluster each region. For example, each region of the line category and the surface category is used as the current region to cluster the feature points of each region.

[0054] S132: Clustering the feature points in the current area to obtain a number of sub-clusters.

[0055] By clustering the features of the current region, several sub-clusters can be obtained, which are the clustering results of the current region. You can use multi-threading to cluster each region. For example, enable multi-threading to cluster the line points or surface points of each region.

[0056] In some embodiments, clustering can be performed in the following manner:

[0057] Add a feature point in the current area to a sub-cluster, and use the sub-cluster as a constructed sub-cluster. For example, the first feature point can be placed in a cluster container to add the feature point to a sub-cluster and form a sub-cluster independently. And this sub-cluster can be used as a constructed sub-cluster {Gi}.

[0058] Other feature points of the current area are taken as current feature points in turn, that is, when new feature points of the current area are added to the cluster container in turn, they can be taken as current feature points in turn. Get whether the current feature point and the constructed sub-cluster {Gi} meet the clustering condition. The clustering condition includes that the distance between the current feature point and the cluster center of the constructed sub-cluster {Gi} is less than the distance threshold Gth. The cluster center represents the center point of the feature points contained in the constructed sub-cluster {Gi}. In some application scenarios, after multiple other feature points are clustered as current feature points, the constructed sub-cluster {Gi} can contain multiple constructed sub-clusters.

[0059] In some embodiments, if the distance between the current feature point and the cluster center of the constructed sub-cluster {Gi}, for example, the constructed sub-cluster {Gx} is less than the distance threshold Gth, it is determined that the current feature point and the constructed sub-cluster {Gx} meet the clustering condition. In response to the current feature point and the constructed sub-cluster {Gx} meeting the clustering condition, the current feature point is added to the constructed sub-cluster {Gx}, and the constructed sub-cluster {Gi} is updated. For example, the cluster center of the constructed sub-cluster {Gk} is updated.

[0060] In some embodiments, if the distance between the current feature point and the cluster center of the constructed subcluster {Gi} is not less than the distance threshold Gth, it is determined that the current feature point and the constructed subcluster {Gi} do not meet the clustering condition. In response to the current feature point and the constructed subcluster not meeting the clustering condition, a new subcluster {Gk} is created, and the current feature point is added to the newly created subcluster {Gk}, that is, the current feature point is placed in the cluster container corresponding to the newly created subcluster {Gk}, and the newly created subcluster {Gk} is used as the constructed subcluster. In addition, the constructed subcluster {Gi} can be updated. For example, the cluster center of this constructed subcluster {Gi} is updated. This is used for clustering the subsequent current feature point.

[0061] By looping through the above steps, the feature points in the current region are clustered and several sub-clusters are obtained, so that the feature points in each region can be clustered.

[0062] In some embodiments, after the feature points of each region are clustered, that is, after the feature points of the current region are clustered to obtain a number of sub-clusters, the sub-clusters may be subjected to a preset process to obtain a number of updated sub-clusters. The preset process includes: deleting sub-clusters that meet the first condition, or merging sub-clusters that meet the second condition. This process may traverse each sub-cluster, merge or delete each sub-cluster, etc., so as to obtain a more accurate clustering result.

[0063] In some implementations, the first condition includes that the number of feature points included in the sub-cluster is less than a first preset number. For example, if the number of feature points included in the sub-cluster is too small, less than the first preset number, the sub-cluster can be directly deleted.

[0064] In some embodiments, the second condition includes at least one of the following: the number of feature points contained in at least two sub-clusters is not less than a first preset number, the distance between the cluster centers of at least two sub-clusters is less than a preset center threshold Hth, and the statistical values ​​of each feature point of at least two sub-clusters are less than a preset statistical threshold. Among them, at least two sub-clusters can represent any two sub-clusters or multiple sub-clusters. The preset statistical threshold is less than the preset center threshold Hth, and the preset statistical threshold is a preset ratio of the preset center threshold Hth. For example, the preset statistical threshold is 0.6*Hth. The statistical value of each feature point can be the mean, median, etc. in the x-dimension and the y-dimension. The x-dimension can represent the x-coordinate of the feature point, and the y-dimension can represent the y-coordinate of the feature point. This application does not limit the statistical value. This application takes the mean as an example for explanation. If the mean of each feature point of two sub-clusters in the x-dimension and the y-dimension is less than the preset statistical threshold 0.6*Hth, the two sub-clusters can be merged.

[0065] Exemplarily, if the number of feature points contained in two sub-clusters is not less than a first preset number, it is determined whether the distance between the cluster centers of the two sub-clusters is less than a preset center threshold Hth, and / or it is determined whether the statistical values ​​of each feature point of the two sub-clusters are less than a preset statistical threshold. If the distance between the cluster centers is less than the preset center threshold Hth, and / or the statistical values ​​of each feature point are less than the preset statistical threshold, the two sub-clusters are merged. In some application scenarios, in order to perform clustering more accurately, if the distance between the cluster centers is less than the preset center threshold Hth, and the statistical values ​​of each feature point are less than the preset statistical threshold, the two sub-clusters are merged.

[0066] In some embodiments, at least two categories of feature points include line points of the line category and surface points of the surface category. Both the surface category and the line category can be clustered in the above manner to obtain sub-clusters of each category and each region. Different from the line category, when the category is the surface category, the second condition for merging at least two sub-clusters is different during the preset processing. For example, the second condition further adds the following conditions, and the second condition also includes at least one of the following: the normal vectors of the surface points included in at least two sub-clusters are parallel or opposite, and any surface point on the plane to which one sub-cluster belongs is on the plane to which another sub-cluster belongs.

[0067] That is to say, when the category is a face category, the second condition also includes at least one of the following: the number of feature points contained in at least two sub-clusters is not less than a first preset number, the distance between the cluster centers of at least two sub-clusters is less than the preset center threshold, the statistical values ​​of the feature points of at least two sub-clusters are less than the preset statistical threshold, the normal vectors of the face points contained in at least two sub-clusters are parallel or opposite, and any face point on the plane to which one sub-cluster belongs is on the plane to which another sub-cluster belongs.

[0068] By clustering the regions of the line category and the surface category in the above manner, several sub-clusters of each category and each region can be obtained, and more accurate clustering results can also be obtained.

[0069] S133: Based on the plurality of sub-clusters, determine the centroid point of each sub-cluster to obtain a plurality of centroid points of the current region; integrate the plurality of centroid points of each region corresponding to each category to obtain a set of centroid points corresponding to at least two categories.

[0070] After obtaining several sub-clusters of each region, the centroids of the sub-clusters are obtained. The cluster centroid is the mass center. In the clustering algorithm, the centroid can be used to represent the center position of each sub-cluster (cluster). According to this method, several centroids can be obtained for each region. For each category, the centroids of each category corresponding to each region are combined to obtain a set of centroids corresponding to at least two categories.

[0071] In some embodiments, it can be determined whether a plurality of sub-clusters in the current region meet a preset stability condition. The preset stability condition includes that the number of laser beams associated with the sub-clusters is greater than a second preset number. If the number of laser beams associated with the sub-clusters is greater than the second preset number, it is determined that the preset stability condition is met.

[0072] When several sub-clusters satisfy the preset stability conditions, the cluster centers of the sub-clusters that satisfy the preset stability conditions can be obtained in response to the fact that several sub-clusters in the current area satisfy the preset stability conditions. The cluster centers can represent the center points of their sub-clusters, and the cluster centers are stable centroid points. Thus, the centroid points of multiple sub-clusters are obtained as the centroid points of the current area.

[0073] By using the above method, the centroid points of each category can be obtained, and the centroid point set of each category can be obtained comprehensively, so that more stable centroid points can be efficiently extracted for subsequent pose estimation to improve the accuracy of pose estimation. For example, for the line category, the centroid point set of the line category can be obtained {L Z}. For the face category, we can get the centroid point set of the face category {L S}.

[0074] Continue reading Figure 1 After step S13, the following step S14 may be continued.

[0075] S14: performing pose estimation using a set of centroid points corresponding to at least two categories to obtain a current pose of the target device.

[0076] The set of centroid points corresponding to at least two categories contains several centroid points. Since the extracted centroid points are more stable and efficient, the centroid points can be used for matching in the pose estimation process. For line categories, the interference of scattered line features on matching can be avoided. For surface categories, matching can be performed more accurately, thereby performing pose estimation to obtain the current pose of the target device in the current frame.

[0077] In some implementations, the centroid points included in the centroid point set can be used for matching to obtain matching point pairs, and then the target change amount can be calculated using the matching point pairs. Based on the target change amount, the current position of the target device in the current frame can be obtained.

[0078] The above scheme extracts feature points from the point cloud data of the current frame to obtain at least two categories of feature points, divides the feature points of each category into regions, obtains feature points of at least two categories corresponding to each region, clusters the feature points of each category corresponding to each region, and obtains sets of centroid points corresponding to at least two categories. This can more efficiently obtain more stable centroid points without processing all point clouds, thereby reducing the amount of calculation for pose estimation and the amount of storage for point cloud data. In addition, since it contains at least two categories of centroid point sets, and each centroid point set contains relatively stable centroid points, the centroid point sets corresponding to at least two categories can be used to perform pose estimation to obtain the current pose of the target device, thereby improving the accuracy of pose estimation.

[0079] In some embodiments, see Figure 4 , step S14 of the above embodiment can be further expanded. Using the centroid point sets corresponding to at least two categories to perform pose estimation to obtain the current pose of the target device, this embodiment may include the following steps:

[0080] S141: Projecting the centroid point sets corresponding to at least two categories into a preset virtual image respectively to obtain key points corresponding to each category.

[0081] The preset virtual image may be formed by an image pixel matrix, and the pixel value in the preset virtual image is a first pixel value, for example, the preset virtual image is a pixel matrix with a pixel value of 255. The centroid point sets corresponding to each category may be projected into the pixel matrix of the preset virtual image to obtain the key points corresponding to each category.

[0082] In some embodiments, the centroid point set corresponding to each category may include a plurality of centroid points, and may be projected to the corresponding image position in the preset virtual image according to a preset projection method to obtain the key points corresponding to each category. The pixel value of the image position of the preset virtual image is set from the first pixel value to the second pixel value; or the first pixel value of the image position and the preset neighborhood of the image position are both set to the second pixel value. The key points corresponding to each category may be obtained by extracting the key points from the image position, or the image position may be used as the key point.

[0083] In some embodiments, the preset projection method includes: using the angle information of the centroid point to determine the column of the image position, and using the radius information (i.e., height information) of the centroid point to determine the row of the image position, thereby obtaining the index of the image position from the row and column of the image position. Among them, the angle information and radius information of the centroid point can be determined by using the resolution of the preset virtual image and the coordinates of the centroid point. The resolution of the preset virtual image is Mc*Mr. Then the method for calculating the row and column of the image position of the centroid point in the preset virtual image is similar to formula (2). For example, the row resolution Mc can be substituted as the radius resolution Nr into formula (2) for calculation to obtain the row of the image position. The column resolution Mr is substituted as the angular resolution Nsec into formula (2) for calculation to obtain the column of the image position. Among them, the row resolution Mc can be the same as or different from the radius resolution Nr, and the column resolution Mr can be the same as or different from the angular resolution Nsec. The present application does not limit the resolution of the preset virtual image and the preset projection method.

[0084] In some embodiments, since the original pixel value of the image position in the preset virtual image is the first pixel value, after projection, the pixel value of the image position of the preset virtual image is set from the first pixel value to the second pixel value. The difference between the first pixel value and the second pixel value is greater than the preset pixel color difference. For example, the image position originally has a first pixel value of 255, and after projection, the pixel value of the image position is set to the second pixel value, such as 0. After projection, the coordinates of the centroid point corresponding to the image position, the coordinates at the image position, and other information can be recorded.

[0085] In some embodiments, after projecting each centroid point onto a preset virtual image, a preset homogenization algorithm is used to homogenize the key points / image positions corresponding to each category to obtain the key points corresponding to each category after homogenization. The preset homogenization algorithm includes: a grid division method, a quadtree method, a topological homogenization method, etc. This application takes the quadtree method as an example, and this application does not limit the preset homogenization algorithm. By homogenizing the key points after projection, it is possible to avoid the feature distribution after projection being too dense.

[0086] In some embodiments, in order to facilitate tracking of the features of the centroid point, after the projection or homogenization process is completed, the pixel values ​​in the preset neighborhood of the image position after the projection of the centroid point can also be set to the second pixel value. That is, the image position corresponding to the projection of the centroid point and the first pixel value in the preset neighborhood of the image position can be set to the second pixel value. The preset neighborhood can be an eight-neighborhood, a four-neighborhood, etc., and the present application does not limit this. The present application takes the eight-neighborhood as an example for explanation, and the image position and the surrounding eight pixel values ​​can be set from the first pixel value to the second pixel value.

[0087] For example, see Figure 5 Taking the line category as an example, since the pixel color difference between the pixel value of the image position after the centroid point is projected (set to the second pixel value) and the pixel value of other surrounding positions (set to the first pixel value) is large, it is easier to extract the key point of the image position, that is, the key point of the centroid point projected to the preset virtual image.

[0088] S142: Tracking is performed using the key points corresponding to each category to obtain matching point pairs corresponding to each category.

[0089] The key points corresponding to each category of the current frame and the key points of the historical frame can be tracked respectively, wherein the historical frame is the frame before the current frame. The tracking method can adopt the optical flow tracking method. For example, for the same category, the key points of the current frame and the key points of the previous frame are used for tracking to obtain the matching point pairs corresponding to each category, and the matching point pairs include the first matching point of the current frame and the second matching point of the historical frame.

[0090] In some embodiments, the optical flow method can be used to track the key points of the current frame and the historical frame of each category to obtain the key point matching pairs of each category. The key point matching pairs include the first key point kp1 of the preset virtual image corresponding to the historical frame and the second key point kp2 tracked by the optical flow method in the preset virtual image corresponding to the current frame.

[0091] Then, based on the key point matching pairs (such as the first key point kp1 and the second key point kp2), the image positions of each key point matching pair can be traversed to extract the point cloud data corresponding to the image positions of the current frame and the historical frame respectively. Since the coordinates of the centroid point corresponding to the image position are recorded, for example, by traversing the image position of the second key point kp2, the corresponding 3D point cloud data can be extracted. The point cloud data of the current frame and the historical frame are used as matching point pairs. Among them, the matching point pair includes the first matching point M3D1 of the current frame and the second matching point M3D2 of the historical frame.

[0092] Exemplarily, the key point matching pair includes the first key point kp1 corresponding to the previous frame and the second key point kp2 tracked in the current frame using the optical flow method. The corresponding extracted 3D point cloud data is used as a matching point pair, that is, the first matching point M3D1 of the current frame and the second matching point M3D2 of the previous frame are obtained.

[0093] S143: Determine the target change amount between the current frame and the historical frame using the matching point pairs corresponding to each category.

[0094] Each pair of matching points can be used for pose estimation, for example, for ICP (Iterative Closest Point) pose solution to obtain the target change between the current frame and the historical frame (such as the previous frame).

[0095] In some implementations, the residuals of each category are constructed using the matching point pairs and initial changes corresponding to each category. For example, the residual of the matching point pair can be obtained by multiplying the product of the first matching point and the initial change and the difference between the second matching point.

[0096] For the matching point pairs corresponding to the same category, the corresponding matching point pair sets {di} and {qi} can be obtained. Among them, {di} represents the set of the second matching points M3D2 of the historical frame, and {qi} represents the set of the first matching points M3D1 of the current frame. The residual constructed for each category can be expressed by the following formula:

[0097]

[0098] Among them, r represents the residual, n represents the number of matching point pairs, di represents the second matching point, qi represents the first matching point, and ΔT represents the initial change.

[0099] Then, the residuals of each category are combined for joint optimization processing to obtain the target change. For example, the residuals of each category can be summed, and then the joint optimization processing can be performed to obtain the target change. For another example, the residuals of each category can be weighted summed using the weights of each category, and then the joint optimization processing can be performed to obtain the target change. Among them, the weights of the weighted residuals of each category can be the same or different. This application does not limit this.

[0100] In some embodiments, the weight corresponding to the residual of the surface category is greater than the weight corresponding to the residual of the line category. Since the at least two categories include line category and surface category. The residual of the line category and the line category residual obtained by the first weight, the residual of the surface category and the line and surface category residual obtained by the second weight can be combined to obtain a comprehensive residual; wherein the second weight is a preset multiple of the first weight, or the second weight is greater than the first weight. For example, the preset multiple can be 2 to 10 times, such as 2 times, 5 times, 6 times, 8 times, etc., and the present application does not limit the preset multiple.

[0101] Among them, the comprehensive residual can be expressed by the following formula:

[0102] R=k1r1+k2r2 Formula (4)

[0103] Wherein, R represents the comprehensive residual, r1 represents the residual of the line category, k1 represents the first weight, r2 represents the residual of the surface category, and k2 represents the second weight. Wherein, the second weight is a preset multiple λ of the first weight, for example, the range of the preset multiple λ is (2<λ<10).

[0104] Then, the comprehensive residual is subjected to joint optimization processing to obtain the target change amount. The joint optimization processing may be, for example, a minimization processing, during which the initial change amount is adjusted to obtain the target change amount. For example, the initial change amount is adjusted through multiple iterations to continuously reduce the comprehensive residual until the comprehensive residual is less than a preset residual threshold and / or the number of iterations reaches a preset number of conditions, and the adjusted initial change amount is used as the target change amount △T.

[0105] In the above method, the residual of the line category and the residual of the line category obtained by the first weight, the residual of the surface category and the residual of the line and surface category obtained by the second weight are integrated to obtain a comprehensive residual, and the comprehensive residual is jointly optimized to obtain the target change. A more accurate target change can be obtained. In addition, since the characteristics of the surface category are more stable than the characteristics of the line category, during the joint optimization, the weight of the residual of the surface category is a preset multiple of the weight of the residual of the line category, which can further improve the accuracy of the obtained target change.

[0106] S144: Based on the historical poses of the historical frames and the target change amount, the current pose of the target device is obtained.

[0107] Since the target change can represent the relative change between the historical frame and the current frame, the current pose of the target device can be obtained by performing pose calculation based on the historical pose of the historical frame and the target change.

[0108] For example, taking the historical frame including the previous frame as an example, the historical posture T corresponding to the previous frame is used. prev And the target change ΔT, we can get the current pose T of the current frame cur . The process can be expressed by the following formula:

[0109] T cur =T prev *ΔT Formula (5)

[0110] The above scheme can perform pose estimation more accurately and obtain a more accurate current pose of the current frame.

[0111] In some embodiments, the current posture can be further estimated in combination with other sensors to obtain a more accurate current posture. Specifically, the other sensors include at least one mileage sensor, that is, an odometer. For example, the other sensors of this embodiment may include at least one of the following: IMU (Inertial Measurement Unit), wheel speed odometer, camera sensor, etc. Among them, the IMU includes an accelerometer and a gyroscope, which are mainly used to measure linear acceleration and angular velocity respectively. The wheel speed odometer tracks the rotation speed of each wheel based on the photoelectric encoder installed on the target device. The change in angle and distance of the mobile robot can be preliminarily estimated, so that the posture at the current moment can be estimated based on the change in posture before and after the moment and the posture at the previous moment, that is, the predicted posture of the current frame can be obtained. Then, the current posture and the predicted posture of the current frame by other sensors are fused to obtain the fused current posture, and a more accurate current posture can be obtained.

[0112] In some embodiments, the historical frame of the tracking process in the above embodiment is the frame before the current frame, and the historical frame includes any of the following: the previous frame or the local map, and the local map is constructed using the centroid point set of the first preset number of preceding frames. The preceding frame is a frame within a preset range before the current frame. Among them, in the case where the above embodiment uses the local map as the historical frame, the present application may also include the following embodiments, which can be specifically referred to in the following embodiments.

[0113] In order to maintain a sufficient number of tracking feature points, that is, the key points of the historical frame in the preset virtual image or the key points of the local map tracking, the present application may execute at least one of the following implementations before obtaining the current posture of the target device.

[0114] In some embodiments, a set of centroid points of various categories of preceding frames of a second preset number of frames within a preset range of the current frame is retained as a set of centroid points of the historical frame, wherein the preset range may be a range of a preset number of frames before the current frame. Then, each centroid point of each set of centroid points of the historical frame is projected to the map coordinate system, a local map is constructed, and the centroid coordinates of each centroid point in the local map are obtained. This local map may be recorded as a local centroid feature point map LF. The local map is used as a historical frame, so that the key points of the current frame are tracked using the local map as a historical frame during the execution of the above step S142. In this way, compared to tracking the previous frame, more key points can be tracked in the current frame by using this local map for tracking, so that the current posture can be solved more accurately.

[0115] In some implementations, the centroid coordinates of the local map may be converted to the coordinate system of the previous frame to obtain the corresponding converted coordinates of the centroid coordinates in the coordinate system of the previous frame. Among them, the centroid coordinates and the corresponding pose of the previous frame may be used to perform pose conversion to obtain the corresponding converted coordinates of the centroid coordinates in the coordinate system of the previous frame. This process can be expressed by the following formula:

[0116] F previ =T prev -1 *F i Formula (6)

[0117] Among them, F Drevi Indicates the corresponding conversion coordinates in the previous frame coordinate system, F i T represents the centroid coordinate i in the local map. prev Indicates the pose corresponding to the previous frame.

[0118] Determine whether the converted coordinates corresponding to each centroid point of the historical frame meet the centroid deletion condition, wherein the centroid deletion condition includes an unstable point condition or a range condition. Exemplarily, the centroid deletion condition includes that the horizontal distance length of the converted coordinate is greater than a preset length, for example, the horizontal distance length may refer to the X coordinate of the converted coordinate. For example, the preset length is 0.6, 0.8, etc., and this application takes the preset length of 0.8 as an example for explanation. If the converted coordinate F previ If the horizontal distance length is greater than the preset length, the height of the Z axis of the corresponding centroid point or point cloud data may be greater than the set height value, and it is considered that the centroid point exceeds the laser range of the radar device or is an unstable point. In response to the transformed coordinates of the centroid point satisfying the centroid deletion condition, the centroid coordinates of the corresponding centroid point can be deleted from the local map. Among them, the local map is used as a historical frame, so that the key points of the current frame are tracked using the local map as a historical frame during the execution of the above step S142. In this way, since the centroid points in the local map that exceed the laser range of the radar device or are unstable are deleted, this local map is used for tracking, which can match more accurately, track more accurate key points in the current frame, and facilitate the solution of the current posture more accurately.

[0119] After performing pose estimation using sets of centroid points corresponding to at least two categories to obtain the current pose of the target device, the sets of centroid points of each category in the current frame may be added to the local map.

[0120] Using the centroid point set of each category in the current frame, based on the current pose T of each centroid point contained in the centroid point set cur Projecting to the world coordinate system, obtaining the projection feature points corresponding to each centroid point, and adding the projection feature points to the local map to obtain an updated local map. Among them, each projection feature point can be used as the centroid coordinates of the local map.

[0121] In some implementations, the steps may be performed on the updated local map: determining whether each projected feature point of the updated local map satisfies a centroid deletion condition, and in response to the projected feature point satisfying the centroid deletion condition, deleting the projected feature point corresponding to the centroid point from the updated local map. Specifically, each centroid coordinate of the updated local map is converted to the coordinate system of the current frame to obtain the corresponding converted coordinates; in response to the converted coordinate satisfying the centroid deletion condition, deleting the centroid coordinate of the corresponding centroid point from the updated local map.

[0122] The updated local map is used as the history frame of the next frame, so that the key points of the next frame are tracked using the updated local map, which can improve the accuracy of subsequent frame matching and further improve the accuracy of pose estimation of subsequent frames.

[0123] It is understandable that in the above method of the specific implementation mode, the writing order of each step does not mean a strict execution order and does not constitute any limitation on the implementation process. The specific execution order of each step should be determined by its function and possible internal logic.

[0124] In some embodiments, the present application also provides a posture estimation device for implementing the posture estimation method of any of the above embodiments.

[0125] See also Figure 6 , Figure 6 It is a structural diagram of an embodiment of a posture estimation device of the present application.

[0126] The posture estimation device 20 includes a classification module 21, a division module 22, a clustering module 23 and an estimation module 24. The modules are interconnected.

[0127] The category module 21 is used to obtain point cloud data of the current frame, and to extract feature points from the point cloud data to obtain feature points of at least two categories.

[0128] The division module 22 is used to divide the feature points of each category into regions, and obtain feature points of at least two categories corresponding to each region.

[0129] The clustering module 23 is used to perform clustering processing on the feature points of each category corresponding to each area, and obtain a set of centroid points corresponding to at least two categories.

[0130] The estimation module 24 is used to perform posture estimation using a set of centroid points corresponding to at least two categories to obtain a current posture of the target device.

[0131] It should be noted that the posture estimation device provided in the above embodiment and the posture estimation method provided in the above embodiment belong to the same concept, wherein the specific manner in which each module and unit performs the operation has been described in detail in the method embodiment and will not be repeated here. In practical applications, the posture estimation device provided in the above embodiment can distribute the above functions to different functional modules as needed, that is, divide the internal structure of the device into different functional modules to complete all or part of the functions described above, and this is not limited here.

[0132] For the above embodiment, the present application provides a computer device, see Figure 7 , Figure 7 3 is a schematic diagram of the structure of an embodiment of a computer device of the present application. The computer device 30 includes a memory 31 and a processor 32, wherein the memory 31 and the processor 32 are coupled to each other, the memory 31 stores program data, and the processor 32 is used to execute the program data to implement the steps of any embodiment of the above-mentioned posture estimation method.

[0133] In this embodiment, the processor 32 may also be referred to as a CPU (Central Processing Unit). The processor 32 may be an integrated circuit chip having signal processing capabilities. The processor 32 may also be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other programmable logic devices, discrete gates or transistor logic devices, or discrete hardware components. A general-purpose processor may be a microprocessor or the processor 32 may also be any conventional processor, etc.

[0134] The method of the above embodiment can be implemented in the form of a computer program, so the present application proposes a computer readable storage medium, see Figure 8 , Figure 8 4 is a schematic diagram of a computer-readable storage medium of the present application. The computer-readable storage medium 40 stores program data 41 that can be run by a processor, and the program data 41 can be executed by the processor to implement the steps of any embodiment of the above-mentioned posture estimation method.

[0135] The computer-readable storage medium 40 of this embodiment can be a medium that can store program data 41, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, or it can also be a server that stores the program data 41. The server can send the stored program data 41 to other devices for execution, or it can also execute the stored program data 41 by itself.

[0136] In some embodiments, the functions or modules included in the device provided in the above embodiments of the present application can be used to execute the method described in the above method embodiments. The specific implementation can refer to the description of the above method embodiments. For the sake of brevity, the present application will not go into details here.

[0137] The above description of the various embodiments tends to emphasize the differences between the various embodiments. The same or similar aspects can be referenced to each other. For the sake of brevity, this application will not go into details here.

[0138] In the several embodiments provided in the present application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation described above is only schematic. For example, the division of modules or units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, and the indirect coupling or communication connection of devices or units can be electrical, mechanical or other forms.

[0139] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0140] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0141] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium, which is a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to perform all or part of the steps of each implementation method of the present application.

[0142] Obviously, those skilled in the art should understand that the above modules or steps of the present application can be implemented by a general computing device, they can be concentrated on a single computing device, or distributed on a network composed of multiple computing devices, and optionally, they can be implemented by a program code executable by a computing device, so that they can be stored in a computer-readable storage medium and executed by a computing device, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. In this way, the present application is not limited to any specific combination of hardware and software.

[0143] The above descriptions are merely embodiments of the present application and are not intended to limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A pose estimation method, characterized in that: include: Acquire point cloud data of the current frame, and extract feature points from the point cloud data to obtain feature points of at least two categories; Divide the feature points of each category into regions respectively to obtain feature points of at least two categories corresponding to each region; Clustering is performed on the feature points of each region corresponding to each category to obtain a set of centroid points corresponding to at least two categories; The pose is estimated using the centroid point sets corresponding to the at least two categories to obtain the current pose of the target device.

2. The method according to claim 1, characterized in that The step of dividing the feature points of each category into regions to obtain feature points of at least two categories corresponding to each region includes: The regions are divided according to the angle direction and the radius direction to obtain several regions; The feature points of each category are divided into several regions respectively, and feature points of at least two categories corresponding to each region are obtained.

3. The method according to claim 2, characterized in that The angular direction includes the angular resolution, and the radial direction includes the radial resolution; and the feature points of each category are divided into a plurality of regions to obtain feature points of at least two categories corresponding to each region, including: The feature points of each category are respectively used as the current feature points; Obtaining radius information by using a first preset relationship between the coordinates of the current feature point and a first ratio of the radius resolution; and obtaining angle information by using a second preset relationship between the coordinates of the current feature point and a second ratio of the angle resolution; Based on the radius information and the angle information, the current feature point is divided into corresponding areas to obtain feature points of at least two categories corresponding to each area.

4. The method according to claim 1, characterized in that: The clustering process is performed on the feature points of each region corresponding to each category to obtain a set of centroid points corresponding to at least two categories, including: The regions corresponding to each category are respectively taken as the current regions; Performing clustering processing on the feature points of the current area to obtain a plurality of sub-clusters; Based on the plurality of sub-clusters, determining the centroid of each sub-cluster to obtain a plurality of centroids of the current region; A number of centroid points of each category corresponding to each area are integrated to obtain a set of centroid points corresponding to the at least two categories.

5. The method according to claim 4, characterized in that The clustering process is performed on the feature points of the current area to obtain a plurality of sub-clusters, including: Adding a feature point of the current region to a sub-cluster, and using the sub-cluster as a constructed sub-cluster; Sequentially taking other feature points of the current region as current feature points, in response to the current feature point and the constructed sub-cluster satisfying the clustering condition, adding the current feature point to the constructed sub-cluster and updating the constructed sub-cluster; or, in response to the current feature point and the constructed sub-cluster not satisfying the clustering condition, adding the current feature point to a newly created sub-cluster and taking the newly created sub-cluster as the constructed sub-cluster; until the feature points of the current region are clustered to obtain the plurality of sub-clusters; And / or, after clustering the feature points of the current area to obtain a plurality of sub-clusters, the method further comprises: Performing preset processing on the plurality of sub-clusters to obtain a plurality of updated sub-clusters; The preset processing includes: deleting sub-clusters that meet a first condition, or merging sub-clusters that meet a second condition, wherein the first condition includes that the number of feature points contained in the sub-clusters is less than a first preset number, and the second condition includes at least one of the following: the number of feature points contained in at least two sub-clusters is not less than the first preset number, the distance between the cluster centers of at least two sub-clusters is less than a preset center threshold, and the statistical values ​​of the feature points of at least two sub-clusters are less than a preset statistical threshold; And / or, determining the centroid of each sub-cluster based on the plurality of sub-clusters includes: In response to the sub-cluster of the current region satisfying a preset stability condition, a cluster center of the sub-cluster is acquired as a centroid point of the current region.

6. The method according to claim 5, characterized in that The feature points of the at least two categories include line points of the line category and surface points of the surface category; when the category is the surface category, the second condition also includes at least one of the following: the normal vectors of the surface points included in at least two sub-clusters are parallel or opposite, and any surface point on the plane to which one sub-cluster belongs is on the plane to which another sub-cluster belongs.

7. The method according to claim 1, characterized in that The step of performing pose estimation using the set of centroid points corresponding to the at least two categories to obtain the current pose of the target device includes: Respectively projecting the centroid point sets corresponding to the at least two categories into a preset virtual image to obtain key points corresponding to each category; Use the key points corresponding to each category to track and obtain the matching point pairs corresponding to each category; Using the matching point pairs corresponding to each category, determine the target change between the current frame and the historical frame; Based on the historical poses of the historical frames and the target change amount, a current pose of the target device is obtained.

8. The method according to claim 7, characterized in that Using the matching point pairs corresponding to each category, determine the target change between the current frame and the historical frame, including: Using the matching point pairs and initial changes corresponding to each category, the residuals of each category are constructed; The residuals of various categories are combined for joint optimization processing to obtain the target variation.

9. The method according to claim 8, characterized in that The at least two categories include line category and surface category; the residuals of the comprehensive categories are jointly optimized to obtain the target variation, including: The residual of the line category and the residual of the line category obtained by the first weight, the residual of the surface category and the residual of the line and surface category obtained by the second weight are integrated to obtain a comprehensive residual; wherein the second weight is a preset multiple of the first weight; The comprehensive residual is jointly optimized to obtain the target variation.

10. The method according to claim 7, characterized in that The projecting of the centroid point sets corresponding to the at least two categories into a preset virtual image to obtain key points corresponding to each category includes: A set of centroid points corresponding to each category, including a plurality of centroid points, is projected to a corresponding image position in the preset virtual image according to a preset projection method to obtain key points corresponding to each category; wherein the pixel value of the image position of the preset virtual image is set from a first pixel value to a second pixel value; or the first pixel value of the image position and a preset neighborhood of the image position are both set to the second pixel value; And / or, the tracking using key points corresponding to each category to obtain matching point pairs corresponding to each category includes: Tracking the key points of the current frame and the historical frame of each category respectively to obtain key point matching pairs of each category; Based on the key point matching pairs, respectively extracting the point cloud data of the current frame and the historical frame to obtain the matching point pairs; And / or, after projecting the centroid point sets corresponding to the at least two categories into a preset virtual image to obtain key points corresponding to each category, the method further comprises: The key points corresponding to each category are homogenized by using a preset homogenization algorithm to obtain the key points corresponding to each category after homogenization.

11. The method according to claim 1 or 7, characterized in that: After performing pose estimation using the set of centroid points corresponding to the at least two categories to obtain the current pose of the target device, the method includes: Fusing the current posture with the predicted posture of the current frame by other sensors to obtain a fused current posture; And / or, the historical frame includes any of the following: a previous frame or a local map, wherein the local map is constructed using a set of centroid points of a first preset number of preceding frames; And / or, before performing pose estimation using the set of centroid points corresponding to the at least two categories to obtain the current pose of the target device, at least one of the following steps is included: Retaining a set of centroid points of each category of the previous frames of a second preset number of frames within a preset range of the current frame as a set of centroid points of the historical frames; and projecting each centroid point of each set of centroid points of the historical frames into a map coordinate system, constructing a local map, and obtaining the centroid coordinates of each centroid point in the local map; Convert each centroid coordinate to the coordinate system of the previous frame to obtain corresponding converted coordinates; in response to the converted coordinates satisfying the centroid deletion condition, delete the centroid coordinate of the corresponding centroid point from the local map; The local map is used as the historical frame, so that the key points of the current frame are tracked using the local map; And / or, after performing pose estimation using the set of centroid points corresponding to the at least two categories to obtain the current pose of the target device, the method further comprises: Using the centroid point set of each category of the current frame, projecting the current position of each centroid point to the world coordinate system, obtaining projected feature points, and adding the projected feature points to the local map to obtain an updated local map; The updated local map is used as a historical frame, so that the key points of the next frame are tracked using the updated local map.

12. The method according to claim 1, characterized in that The extracting feature points from the point cloud data to obtain at least two types of feature points includes: Obtaining the curvature of each point cloud data; wherein the curvature is calculated by using the current point cloud data and a number of other adjacent point cloud data on the same laser beam when each point cloud data is used as the current point cloud data; In response to the curvature being greater than a first curvature threshold, the point cloud data is determined to be a line point of a line category; or, in response to the curvature being less than a second curvature threshold, the point cloud data is determined to be a surface point of a surface category.

13. A computer device, characterized in that: The invention comprises a memory and a processor coupled to each other, wherein the memory stores program data, and the processor is used to execute the program data to implement the steps of the method according to any one of claims 1 to 12.

14. A computer-readable storage medium, characterized in that: Program data that can be executed by a processor are stored, and the program data is used to implement the steps of the method according to any one of claims 1 to 12.

Images