Pose estimation method and device, electronic equipment and readable storage medium

Abstract

The application provides a pose estimation method and device, electronic equipment and readable storage medium, and relates to the technical field of computers. The method comprises the following steps: according to a current point cloud frame and a previous point cloud frame of the current point cloud frame, first pose transformation information of a target object between the current point cloud frame and the previous point cloud frame is calculated, and the current point cloud frame and the previous point cloud frame comprise point clouds of the target object; and according to the first pose transformation information and a previous moment pose of the target object, a current pose of the target object is calculated, wherein the previous moment pose is a pose of a previous target point cloud of the target object in the previous point cloud frame relative to an initial target point cloud in a first frame point cloud frame, and the current pose is a pose of a current target point cloud of the target object in the current point cloud frame relative to the initial target point cloud. In this way, even if the pose change of the target object from the first frame point cloud frame to the current point cloud frame is large, the current pose of the target object can still be stably solved.

Description

Technical Field

[0001] This application relates to the field of computer technology, and more specifically, to a pose estimation method, apparatus, electronic device, and readable storage medium. Background Technology

[0002] Pose estimation can be represented by Pa = T * Pb, where T is the pose to be solved, which can generally be represented as a 4*4 matrix, where the 3*3 part represents the rotation matrix and the 1*3 part in the last column represents the translation part.

[0003] Currently, when using point clouds for pose estimation, the general approach is to first detect feature points on both the source and target to construct feature descriptors. Then, random consistency sampling iteration is used to select the feature descriptors with the most consistent transformations. Next, using Pa = T * Pb, where Pa and Pb are the selected consistent features, T is calculated using least squares or other methods. The calculated T represents the relative pose between the source and target. However, when the pose changes significantly between the source and target due to a large number of point cloud frames, the consistency features become insufficient, leading to a large discrepancy between the calculated pose and the true pose, or even making it impossible to calculate the pose at all. Summary of the Invention

[0004] This application provides a pose estimation method, apparatus, electronic device, and readable storage medium, which can obtain the current pose of a target object by continuously solving the pose. This avoids the situation where the solved pose differs greatly from the true pose or even cannot be solved when the pose is solved based only on the source point cloud and the target point cloud due to the large change in pose caused by a large number of point cloud frames.

[0005] The embodiments of this application can be implemented as follows:

[0006] In a first aspect, embodiments of this application provide a pose estimation method, the method comprising:

[0007] Based on the current point cloud frame and the previous point cloud frame, the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame is calculated, wherein the current point cloud frame and the previous point cloud frame include the point cloud of the target object;

[0008] Based on the first pose transformation information and the previous pose of the target object, the current pose of the target object is calculated, wherein the previous pose is the pose of the target object in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object in the current point cloud frame relative to the initial target point cloud.

[0009] In an optional implementation, before calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame, the method further includes:

[0010] Based on the current point cloud frame and the first point cloud frame, determine whether the pose change of the target object is within a preset range;

[0011] If the pose change of the target object is within the preset range, the current pose of the target object is calculated based on the first point cloud frame and the current point cloud frame.

[0012] If the pose transformation of the target object is not within the preset range, then the step of calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame is executed based on the current point cloud frame and the previous point cloud frame.

[0013] In an optional implementation, determining whether the pose change of the target object is within a preset range based on the current point cloud frame and the first point cloud frame includes:

[0014] Based on the current point cloud frame and the first point cloud frame, the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame is calculated;

[0015] Based on the second pose transformation information and the preset range, it is determined whether the pose transformation of the target object is within the preset range.

[0016] In an optional implementation, calculating the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame based on the current point cloud frame and the first point cloud frame includes:

[0017] Obtain the current target point cloud of the target object in the current point cloud frame;

[0018] Obtain the initial target point cloud of the target object in the first frame point cloud frame;

[0019] The second pose transformation information is calculated based on the current target point cloud and the initial target point cloud.

[0020] In an optional implementation, when the second pose transformation information is calculated, the current target point cloud is obtained based on the saved current target region information.

[0021] In an optional implementation, if the pose change of the target object is within the preset range, the method further includes:

[0022] The saved current target region information is reset to the initial target region information corresponding to the first point cloud frame, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame.

[0023] In an optional implementation, calculating the current pose of the target object based on the first point cloud frame and the current point cloud frame includes:

[0024] The orientation information in the second pose transformation information is cleared to zero, and the cleared second pose transformation information is used as the current pose of the target object.

[0025] In an optional implementation, calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame includes:

[0026] Obtain the current target point cloud of the target object in the current point cloud frame;

[0027] Obtain the previous target point cloud of the target object in the previous point cloud frame;

[0028] The first pose transformation information is calculated based on the current target point cloud and the previous target point cloud.

[0029] In an optional implementation, when the first pose transformation information is calculated, the current target point cloud is obtained based on the saved current target region information.

[0030] In an optional implementation, after calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame, the method further includes:

[0031] Based on the saved current target region information and the first pose transformation information, the saved current target region information is updated, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame.

[0032] Secondly, embodiments of this application provide a pose estimation device, the device comprising:

[0033] The calculation module is used to calculate the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame, wherein the current point cloud frame and the previous point cloud frame include the point cloud of the target object.

[0034] The pose estimation module is used to calculate the current pose of the target object based on the first pose transformation information and the previous pose of the target object. The previous pose is the pose of the target object in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object in the current point cloud frame relative to the initial target point cloud.

[0035] In an optional implementation, before calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame, the method further includes:

[0036] Based on the current point cloud frame and the first point cloud frame, determine whether the pose change of the target object is within a preset range;

[0037] If the pose change of the target object is within the preset range, the current pose of the target object is calculated based on the first point cloud frame and the current point cloud frame.

[0038] If the pose transformation of the target object is not within the preset range, then the step of calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame is executed based on the current point cloud frame and the previous point cloud frame.

[0039] In an optional implementation, determining whether the pose change of the target object is within a preset range based on the current point cloud frame and the first point cloud frame includes:

[0040] Based on the current point cloud frame and the first point cloud frame, the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame is calculated;

[0041] Based on the second pose transformation information and the preset range, determine whether the pose transformation of the target object is within the preset range.

[0042] In an optional implementation, calculating the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame based on the current point cloud frame and the first point cloud frame includes:

[0043] Obtain the current target point cloud of the target object in the current point cloud frame;

[0044] Obtain the initial target point cloud of the target object in the first frame point cloud frame;

[0045] The second pose transformation information is calculated based on the current target point cloud and the initial target point cloud.

[0046] In an optional implementation, when the second pose transformation information is calculated, the current target point cloud is obtained based on the saved current target region information.

[0047] In an optional implementation, if the pose change of the target object is within the preset range, the method further includes:

[0048] The saved current target region information is reset to the initial target region information corresponding to the first point cloud frame, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame.

[0049] In an optional implementation, calculating the current pose of the target object based on the first point cloud frame and the current point cloud frame includes:

[0050] The orientation information in the second pose transformation information is cleared to zero, and the cleared second pose transformation information is used as the current pose of the target object.

[0051] In an optional implementation, calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame includes:

[0052] Obtain the current target point cloud of the target object in the current point cloud frame;

[0053] Obtain the previous target point cloud of the target object in the previous point cloud frame;

[0054] The first pose transformation information is calculated based on the current target point cloud and the previous target point cloud.

[0055] In an optional implementation, when the first pose transformation information is calculated, the current target point cloud is obtained based on the saved current target region information.

[0056] In an optional implementation, after calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame, the method further includes:

[0057] Based on the saved current target region information and the first pose transformation information, the saved current target region information is updated, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame.

[0058] Thirdly, embodiments of this application provide an electronic device, including a processor and a memory, wherein the memory stores machine-executable instructions that can be executed by the processor, and the processor can execute the machine-executable instructions to implement the pose estimation method described in any of the foregoing embodiments.

[0059] Fourthly, embodiments of this application provide a readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the pose estimation method as described in any of the foregoing embodiments.

[0060] The pose estimation method, apparatus, electronic device, and readable storage medium provided in this application first calculate the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame, based on the current point cloud frame including the target object and the previous point cloud frame. Then, the current pose of the target object is calculated based on the first pose transformation information and the previous pose of the target object. The previous pose is the pose of the target object's previous target point cloud in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object's current target point cloud in the current point cloud frame relative to the initial target point cloud. The embodiments of this application can obtain the current pose of the target object by continuously solving the pose. Even if the pose of the target object changes significantly from the first point cloud frame to the current point cloud frame, the current pose of the target object can still be stably solved. This avoids the situation where the solved pose differs greatly from the true pose or even cannot be solved when the pose is solved based only on the source point cloud and the target point cloud due to the large change in pose after a large number of point cloud frames. Attached Figure Description

[0061] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0062] Figure 1 A block diagram illustrating an electronic device provided in an embodiment of this application;

[0063] Figure 2One of the flowcharts for the pose estimation method provided in the embodiments of this application;

[0064] Figure 3 A schematic diagram showing the pose estimation method provided in this application and existing pose estimation methods;

[0065] Figure 4 for Figure 2 A flowchart illustrating the sub-steps included in step S120;

[0066] Figure 5 A second schematic flowchart illustrating the pose estimation method provided in this application embodiment;

[0067] Figure 6 for Figure 5 A flowchart illustrating the sub-steps included in step S110;

[0068] Figure 7 One of the block diagrams of the pose estimation device provided in the embodiments of this application;

[0069] Figure 8 This is a second block diagram of the pose estimation device provided in the embodiments of this application.

[0070] Icons: 100 - Electronic device; 110 - Memory; 120 - Processor; 130 - Communication unit; 200 - Pose estimation device; 210 - Judgment module; 220 - Calculation module; 230 - Pose estimation module. Detailed Implementation

[0071] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0072] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0073] It should be noted that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0074] The following detailed description of some embodiments of this application is provided in conjunction with the accompanying drawings. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0075] Please refer to Figure 1 , Figure 1 This is a block diagram of an electronic device 100 provided in an embodiment of this application. The electronic device 100 may be, but is not limited to, a computer, a server, etc. The electronic device 100 may include a memory 110, a processor 120, and a communication unit 130. The memory 110, processor 120, and communication unit 130 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other through one or more communication buses or signal lines.

[0076] The memory 110 is used to store programs or data. The memory 110 may be, but is not limited to, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), etc.

[0077] The processor 120 is used to read / write data or programs stored in the memory 110 and execute corresponding functions. For example, the memory 110 stores a pose estimation device 200, which includes at least one software functional module that can be stored in the memory 110 in the form of software or firmware. The processor 120 executes various functional applications and data processing by running the software programs and modules stored in the memory 110, such as the pose estimation device 200 in the embodiments of this application, thereby implementing the pose estimation method in the embodiments of this application.

[0078] The communication unit 130 is used to establish a communication connection between the electronic device 100 and other communication terminals through the network, and to send and receive data through the network.

[0079] It should be understood that, Figure 1 The structure shown is only a schematic diagram of the electronic device 100. The electronic device 100 may also include components that are larger than... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown. Figure 1 The components shown can be implemented using hardware, software, or a combination thereof.

[0080] Please refer to Figure 2 , Figure 2 This is one of the flowcharts illustrating the pose estimation method provided in this application embodiment. The method can be applied to electronic device 100. The specific flow of the pose estimation method is described in detail below. In this embodiment, the method may include steps S120 to S130.

[0081] Step S120: Calculate the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame.

[0082] The current point cloud frame and the previous point cloud frame can be point cloud frames within a point cloud frame sequence. This point cloud frame sequence can be a sequence of point cloud frames that has already been acquired, or it can be a sequence of point cloud frames that is currently being acquired, meaning that the number of point cloud frames in the sequence will continuously increase. The current point cloud frame is located after the previous point cloud frame, meaning that the time sequence of the previous point cloud frame is before the time sequence of the current point cloud frame. The current point cloud frame is the point cloud frame currently being processed. Both the current point cloud frame and the previous point cloud frame include the target object, that is, they include a specific observation object, in other words, they include a specific observation target.

[0083] The first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame can be calculated based on the current point cloud frame and the previous point cloud frame. In other words, the first pose transformation information represents the relative pose of the target object in the current point cloud frame compared to its pose in the previous point cloud frame; that is, it represents the pose of the current target point cloud in the current point cloud frame relative to its pose in the previous target point cloud frame.

[0084] Optionally, the first pose transformation information can be obtained through algorithms such as ICP (Iterative Closest Point) and NDT (Normal Distribution Transform).

[0085] Step S130: Calculate the current pose of the target object based on the first pose transformation information and the previous pose of the target object.

[0086] Wherein, the previous pose is the pose of the target object in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame; that is, it represents the relative pose of the target object in the previous point cloud frame relative to the initial pose of the target object in the first point cloud frame. Similarly, the current pose is the pose of the target object in the current point cloud frame relative to the initial target point cloud; that is, it represents the relative pose of the target object in the current point cloud frame relative to the initial pose of the target object. The previous pose of the target object can be transformed according to the first pose transformation information to obtain the current pose.

[0087] It is worth noting that the current pose will be used as the new previous pose in the next moment; that is, when a new current pose is obtained based on the next point cloud frame, the current pose is the new previous pose.

[0088] By continuously executing the above method, the pose of the target object relative to the initial pose at each moment can be obtained. When the current pose at a certain moment is needed, the calculated current pose at that moment can be output. When the current pose at a certain moment is not needed, the calculated current pose at that moment can be saved for calculating the current pose at the next moment. Here, the aforementioned moments refer to the moments when the point cloud frames are acquired.

[0089] The following is combined with Figure 3 This paper provides examples to illustrate the pose estimation method provided in this application and existing pose estimation methods.

[0090] Assume the point cloud frame sequence includes point cloud frames 1-k (i.e., including the first point cloud frame, the second point cloud frame, etc.), corresponding to a pose sequence T1-Tk, where T1 represents the initial pose (i.e., the pose of the target object in the first point cloud frame), and Tk represents the pose relative to T1. Furthermore, assume that Tk needs to be calculated.

[0091] When the target object moves continuously to Tk, the conventional pose estimation method directly obtains Tk from point cloud frame 1 and point cloud frame K through feature detection, feature description, feature matching and filtering, and pose estimation. However, when there is a significant change between Tk and T1, the obtained consistent features are insufficient, or even nonexistent. This will inevitably lead to significant problems in pose estimation, and may even fail to meet the conditions for pose estimation altogether. The above-mentioned process of continuously estimating pose can be represented by the following formula: Tk = T1² * T2³ * T3⁴ * … * T(k-1)k.

[0092] In this scheme, the relative pose T12 is calculated based on point cloud frames 1 and 2, and this relative pose T12 is T2; the relative pose T23 is calculated based on point cloud frames 2 and 3, and then T3 is calculated based on the relative poses T2 and T23; subsequently, the relative pose T34 is calculated based on point cloud frames 3 and 4, and then T4 is calculated based on the relative poses T34 and T3. This process continues until the relative pose of the target object's point cloud in point cloud frame k is obtained compared to its point cloud in point cloud frame 1.

[0093] The embodiments of this application can stably solve the pose of the target object at any time by continuously solving the pose. This avoids the situation where the solved pose differs greatly from the true pose or even cannot be solved when the pose is solved based only on the source point cloud and the target point cloud due to the large number of point cloud frames that are separated.

[0094] Optionally, in this embodiment, if both the current point cloud frame and the previous point cloud frame include only the point cloud of the target object, the first pose transformation information can be directly calculated based on the current point cloud frame and the previous point cloud frame.

[0095] Optionally, in this embodiment, if the current point cloud frame and the previous point cloud frame do not only include the point cloud of the target object, it can be achieved through... Figure 4 The first pose transformation information is obtained in the manner shown. Please refer to... Figure 4 , Figure 4 for Figure 2A flowchart illustrating the sub-steps included in step S120. In this embodiment, step S120 may include sub-steps S121 to S123. Sub-steps S121 and S122 may be executed sequentially or simultaneously, without specific limitations.

[0096] Sub-step S121: Obtain the current target point cloud of the target object in the current point cloud frame.

[0097] Sub-step S122: Obtain the previous target point cloud of the target object in the previous point cloud frame.

[0098] Sub-step S123: Calculate the first pose transformation information based on the current target point cloud and the previous target point cloud.

[0099] In this embodiment, the current point cloud of the target object can be obtained from the current point cloud frame by methods such as object recognition based on object features or specifying a specific region. Similarly, the previous target point cloud of the target object can be obtained from the previous target point cloud by methods such as object recognition based on object features or specifying a specific region. The methods for obtaining the current point cloud and the previous target point cloud can be the same or different, depending on the actual needs.

[0100] It is worth noting that, if the point cloud of the target object in a certain point cloud frame has already been obtained, when needed, the point cloud of the target object in that point cloud frame can be obtained again through new calculations; alternatively, it can be used directly without repeated calculations. For example, if the previous target point cloud has already been obtained, it can be directly called upon. For instance, if time t3 is after time t2, when calculating the current pose corresponding to time t3, the current target point cloud at time t2 is used as the previous target point cloud, which can be used directly without additional calculations to obtain the previous target point cloud, thereby reducing the computational load.

[0101] As one possible implementation, current target region information can be stored, which is used to extract the point cloud of the target object from the current point cloud frame. When it is necessary to calculate the first pose transformation information, the current target point cloud can be obtained from the current point cloud frame based on the stored current target region information. In this way, the current target point cloud can be obtained quickly, thereby accelerating the speed of obtaining the current pose.

[0102] Given the current target point cloud and the previous target point cloud, algorithms such as ICP and NDT can be used to calculate the first pose transformation information based on the current target point cloud and the previous target point cloud.

[0103] If the first pose transformation information is obtained, the pose of the previous moment can be transformed according to the first pose transformation information to obtain the current pose of the target object.

[0104] To facilitate rapid acquisition of the current target point cloud subsequently, the saved current target region information can be updated based on the saved current target region information and the first pose transformation information, after obtaining the first pose transformation information. The updated current target region information is used to extract the point cloud of the target object from the next point cloud frame. This update process can be represented as: ROI_new = ROI_old * T(k-1)k, where ROI_old represents the current target region information before the update, T(k-1)k represents the first pose transformation information, and ROI_new represents the current target region information after the update.

[0105] The inventors of this application have discovered through research that continuous pose estimation is prone to angle drift. To reduce this, an alternative method can be adopted. Figure 5 The method shown is used for pose estimation. Please refer to... Figure 5 , Figure 5 This is a second schematic flowchart illustrating the pose estimation method provided in this application embodiment. In this embodiment, before step S120, the method may further include step S110.

[0106] Step S110: Based on the current point cloud frame and the first point cloud frame, determine whether the pose change of the target object is within a preset range.

[0107] In this embodiment, the determination of whether to obtain the current pose through steps S110 to S120 can be made based on the magnitude of the pose change of the target object between the current point cloud frame and the first point cloud frame. Specifically, change data of the corresponding type can be calculated based on the data type corresponding to the preset range, using the current point cloud frame and the first point cloud frame. Then, the pose change is judged based on the change data to determine whether it falls within the preset range. The change data reflects the pose change. For example, if the preset range only requires changes in the heading angle, only the actual change in the heading angle can be calculated, and then compared with the preset range. Other methods can also be used to complete the above judgment, which are not specifically limited here.

[0108] As one possible implementation method, it can be achieved through Figure 6 The method shown determines whether the pose change of the target object is within a preset range. Please refer to... Figure 6 , Figure 6 for Figure 5A flowchart illustrating the sub-steps included in step S110. In this embodiment, step S110 may include sub-steps S111 to S112.

[0109] Sub-step S111: Based on the current point cloud frame and the first point cloud frame, calculate the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame.

[0110] In this embodiment, when both the current point cloud frame and the first point cloud frame include only the point cloud of the target object, the second pose transformation information can be directly calculated based on the current point cloud frame and the first point cloud frame.

[0111] If the current point cloud frame and the first point cloud frame do not only include the point cloud of the target object, the current target point cloud of the target object in the current point cloud frame and the initial target point cloud of the target object in the first point cloud frame can be obtained. Then, by using the ICP algorithm, NDT algorithm, etc., the second pose transformation information can be calculated based on the current target point cloud and the initial target point cloud.

[0112] Specifically, the current point cloud of the target object can be obtained from the current point cloud frame, and the initial target point cloud of the target object can be obtained from the first point cloud frame, through methods such as object recognition based on object features or specifying a specific region. The methods for obtaining the current point cloud and the initial target point cloud can be the same or different, depending on the actual needs.

[0113] It is worth noting that if the point cloud of the target object in a certain point cloud frame has already been obtained, it can be used directly when needed without repeated calculation. For example, if the initial target point cloud has already been calculated, it can be directly called when calculating the second pose transformation information.

[0114] As one possible implementation, the current target region information can be stored. When calculating the second pose transformation information, the current target point cloud is obtained based on the stored current target region information; that is, the current target point cloud is obtained from the current point cloud frame according to the stored current target region information. In this way, the current target point cloud can be obtained quickly.

[0115] Sub-step S112: Based on the second pose transformation information and the preset range, determine whether the pose transformation of the target object is within the preset range.

[0116] Having obtained the second pose transformation information, the actual change corresponding to the preset range can be determined based on the second pose transformation information. Then, by comparing the actual change with the preset range, it can be determined whether the pose transformation of the target object is within the preset range, that is, whether the pose transformation is small. Wherein, if the actual change is within the preset range, it means that the pose transformation of the target object is within the preset range; if the actual change is not within the preset range, it means that the pose transformation of the target object is not within the preset range.

[0117] The data type and specific data range corresponding to the preset range can be determined based on actual needs and are not specifically limited here. For example, the preset range can be set as a preset heading angle change range. The actual heading angle change value can be calculated based on the second pose transformation information. If the actual heading angle change value is within the preset heading angle change range, then the pose transformation of the target object is determined to be within the preset range; if the actual heading angle change value is not within the preset heading angle change range, then the pose transformation of the target object is determined to be outside the preset range.

[0118] For example, a preset range can be set, including a preset heading angle change range and a preset position distance range. The actual heading angle change value and the actual position distance value can be calculated based on the second pose transformation information. If the actual heading angle change value is within the preset heading angle change range and the actual position distance value is within the preset position distance range, then the pose transformation of the target object is determined to be within the preset range. If the actual heading angle change value is not within the preset heading angle change range, and / or the actual position distance value is not within the preset position distance range, then the pose transformation of the target object is determined to be outside the preset range. When the preset range includes multiple data types, the above specific judgment method is only an example. The specific method can be determined based on actual needs. For example, if the actual change of one type of data is within the corresponding preset data range, then the pose transformation of the target object can be determined to be within the preset range.

[0119] like Figure 5 As shown, if the pose change of the target object is not within the preset range, steps S120 to S130 can be executed sequentially to obtain the current pose of the target object.

[0120] like Figure 5 As shown, if the pose change of the target object is within the preset range, step S140 can be executed.

[0121] Step S140: Calculate the current pose of the target object based on the first point cloud frame and the current point cloud frame.

[0122] Optionally, when the pose transformation of the target object is within the preset range, the second pose transformation information can be directly used as the current pose of the target object. Alternatively, the direction information in the second pose transformation information can be cleared to zero, that is, the pose rotation part can be cleared to zero, and the second pose transformation information after clearing to zero can be used as the current pose of the target object.

[0123] To facilitate rapid acquisition of the current target point cloud, if the pose transformation of the target object is within the preset range, the saved current target region information can be reset to the initial target region information corresponding to the first point cloud frame. The updated current target region information is used to extract the point cloud of the target object from the next point cloud frame. Optionally, the current target region information can be reset to the initial target region information by clearing the direction information in the second pose transformation information.

[0124] If the specific pose of the target object in the first point cloud frame is also obtained, the pose of the target object in the coordinate system of the initial pose can be calculated based on the current pose and the initial pose of the target object.

[0125] It is worth noting that initial target region information can be pre-set. Based on this initial target region information, an initial target point cloud is extracted from the first point cloud frame, and a second target point cloud is extracted from the second point cloud frame based on the same initial target region information. Then, the relative pose T12 is calculated based on the initial target point cloud and the second target point cloud. Afterward, the initial target region information can be updated in the aforementioned manner to obtain the current target region information and the current pose.

[0126] The above pose estimation method will be illustrated with examples below.

[0127] S1, System Initialization:

[0128] a) Set the identity matrix to the initial pose matrix of the target object;

[0129] b. Set the initial ROI boundary of the target object;

[0130] c. Based on the initialized ROI boundary, the initial target point cloud of the target object is obtained by cropping from the first point cloud frame, and the initial target point cloud is stored as the matching point cloud for subsequent attitude correction.

[0131] Then, S2-S7 are executed in a loop.

[0132] S2, obtain the current point cloud frame including the target object.

[0133] S3, based on the saved current ROI boundary, crop the current target point cloud of the target object from the current point cloud frame.

[0134] Specifically, when the current point cloud frame is the second point cloud frame, the saved current ROI boundary is the initialized ROI boundary. When the previous point cloud frame is not the second point cloud frame, the saved current ROI boundary is the updated ROI boundary.

[0135] S4, NDT is used to match the current target point cloud and the initial target point cloud to obtain the second pose transformation information. If the pose transformation of the target object is determined to be within a preset range based on the second pose transformation information, i.e., the pose transformation value is relatively small, the rotation part in the second pose transformation information can be directly reset to the identity matrix to obtain the current pose of the target object. Here, the current pose of the target object is the pose in the coordinate system where the initial pose matrix is ​​located. The ROI boundary saved at this time is also reset to the initialized ROI boundary.

[0136] If it is determined from the second pose transformation information that the pose transformation of the target object is not within the preset range, then proceed to step S5.

[0137] S5, use NDT to match the current target point cloud and the previous target point cloud to obtain the first pose transformation information T(k-1)k, and update the saved ROI boundary according to the first pose transformation information: ROI_new=ROI_old*T(k-1)k.

[0138] S6, calculate the current pose: Tk=T(k-1)*T(k-1)k.

[0139] S7 sends the obtained current pose to other modules for use.

[0140] It is worth noting that the current pose obtained in the above example uses the initial pose, so the current pose is the pose in the coordinate system where the initial pose matrix is ​​located; while in the aforementioned steps S110 to S140, since the initial pose is not used, the calculated current pose is the pose relative to the initial pose, which is the pose in the coordinate system with the initial pose as the origin.

[0141] To perform the corresponding steps in the above embodiments and various possible methods, an implementation of the pose estimation device 200 is given below. Optionally, the pose estimation device 200 can adopt the above-described... Figure 1 The device structure of the electronic device 100 shown. Further, please refer to... Figure 7 , Figure 7This is one of the block diagrams of the pose estimation device 200 provided in this application embodiment. It should be noted that the pose estimation device 200 provided in this embodiment has the same basic principle and technical effects as the above embodiments. For the sake of brevity, any parts not mentioned in this embodiment can be referred to the corresponding content in the above embodiments. In this embodiment, the pose estimation device 200 may include: a calculation module 220 and a pose estimation module 230.

[0142] The calculation module 220 is used to calculate the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame, based on the current point cloud frame and the previous point cloud frame. The current point cloud frame and the previous point cloud frame include the point cloud of the target object.

[0143] The pose estimation module 230 is used to calculate the current pose of the target object based on the first pose transformation information and the previous pose of the target object. The previous pose is the pose of the target object in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object in the current point cloud frame relative to the initial target point cloud.

[0144] Please refer to Figure 8 , Figure 8 This is a second block diagram of the pose estimation device 200 provided in an embodiment of this application. In this embodiment, the pose estimation device 200 may further include a judgment module 210.

[0145] The judgment module 210 is used to determine whether the pose change of the target object is within a preset range based on the current point cloud frame and the first point cloud frame.

[0146] The pose estimation module 230 is used to calculate the current pose of the target object based on the first pose change information and the previous pose of the target object when the pose change of the target object is not within the preset range.

[0147] The pose estimation module 230 is further configured to calculate the current pose of the target object based on the first point cloud frame and the current point cloud frame, when the pose change of the target object is within a preset range.

[0148] Optionally, the above modules can be stored in the form of software or firmware. Figure 1 The memory 110 shown is either stored in or embedded in the operating system (OS) of the electronic device 100, and can be used by... Figure 1The processor 120 executes the program. Meanwhile, the data and program code required to execute the above modules can be stored in the memory 110.

[0149] This application also provides a readable storage medium storing a computer program thereon, which, when executed by a processor, implements the pose estimation method described above.

[0150] In summary, embodiments of this application provide a pose estimation method, apparatus, electronic device, and readable storage medium. First, based on the current point cloud frame including the point cloud of the target object and the previous point cloud frame, the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame is calculated. Then, based on the first pose transformation information and the previous pose of the target object, the current pose of the target object is calculated. Wherein, the previous pose is the pose of the target object's previous target point cloud in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object's current target point cloud in the current point cloud frame relative to the initial target point cloud. The embodiments of this application can obtain the current pose of the target object by continuously solving the pose. Even if the pose of the target object changes significantly from the first point cloud frame to the current point cloud frame, the current pose of the target object can still be stably solved. This avoids the situation where the solved pose differs greatly from the true pose or even cannot be solved when the pose is solved based only on the source point cloud and the target point cloud due to the large change in pose after a large number of point cloud frames.

[0151] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can also be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0152] In addition, the functional modules in the various embodiments of this application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0153] If the aforementioned functions are implemented as software functional modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0154] The above description is merely an optional embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A pose estimation method, characterized in that, The method includes: Based on the current point cloud frame and the first point cloud frame, determine whether the pose change of the target object is within the preset range; If the pose transformation of the target object is within the preset range, the direction information in the second pose transformation information is cleared to zero, and the cleared second pose transformation information is taken as the current pose of the target object. The second pose transformation information is the pose transformation information of the target object between the current point cloud frame and the first point cloud frame. If the pose transformation of the target object is not within the preset range, the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame is calculated based on the current point cloud frame and the previous point cloud frame. The current point cloud frame and the previous point cloud frame include the point cloud of the target object. The point cloud of the target object in the current point cloud frame is extracted from the current point cloud frame based on the saved current target region information. Based on the saved current target region information and the first pose transformation information, the saved current target region information is updated, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame. Based on the first pose transformation information and the previous pose of the target object, the current pose of the target object is calculated, wherein the previous pose is the pose of the target object in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object in the current point cloud frame relative to the initial target point cloud.

2. The method according to claim 1, characterized in that, The step of determining whether the pose change of the target object is within a preset range based on the current point cloud frame and the first point cloud frame includes: Based on the current point cloud frame and the first point cloud frame, the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame is calculated; Based on the second pose transformation information and the preset range, determine whether the pose transformation of the target object is within the preset range.

3. The method according to claim 2, characterized in that, The step of calculating the second pose transformation information of the target object between the current point cloud frame and the first point cloud frame based on the current point cloud frame and the first point cloud frame includes: Obtain the current target point cloud of the target object in the current point cloud frame; Obtain the initial target point cloud of the target object in the first frame point cloud frame; The second pose transformation information is calculated based on the current target point cloud and the initial target point cloud.

4. The method according to claim 3, characterized in that, When calculating the second pose transformation information, the current target point cloud is obtained based on the saved current target region information.

5. The method according to claim 4, characterized in that, When the pose transformation of the target object is within the preset range, the method further includes: The saved current target region information is reset to the initial target region information corresponding to the first point cloud frame, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame.

6. The method according to any one of claims 1-5, characterized in that, The step of calculating the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame includes: Obtain the current target point cloud of the target object in the current point cloud frame; Obtain the previous target point cloud of the target object in the previous point cloud frame; The first pose transformation information is calculated based on the current target point cloud and the previous target point cloud.

7. The method according to claim 6, characterized in that, When calculating the first pose transformation information, the current target point cloud is obtained based on the saved current target region information.

8. A pose estimation device, characterized in that, The device includes: The judgment module is used to determine whether the pose change of the target object is within a preset range based on the current point cloud frame and the first point cloud frame. The pose estimation module is used to, when the pose transformation of the target object is within the preset range, clear the direction information in the second pose transformation information to zero, and use the cleared second pose transformation information as the current pose of the target object, wherein the second pose transformation information is the pose transformation information of the target object between the current point cloud frame and the first point cloud frame; The calculation module is used to calculate the first pose transformation information of the target object between the current point cloud frame and the previous point cloud frame based on the current point cloud frame and the previous point cloud frame, wherein the current point cloud frame and the previous point cloud frame include the point cloud of the target object; the point cloud of the target object in the current point cloud frame is extracted from the current point cloud frame based on the saved current target region information; The calculation module is also used to update the saved current target region information according to the saved current target region information and the first pose transformation information, wherein the updated current target region information is used to extract the point cloud of the target object from the next point cloud frame. The pose estimation module is used to calculate the current pose of the target object based on the first pose transformation information and the previous pose of the target object when the pose transformation of the target object is not within the preset range. The previous pose is the pose of the target object in the previous point cloud frame relative to the initial target point cloud in the first point cloud frame, and the current pose is the pose of the target object in the current point cloud frame relative to the initial target point cloud.

9. An electronic device, characterized in that, It includes a processor and a memory, the memory storing machine-executable instructions that can be executed by the processor to implement the pose estimation method according to any one of claims 1-7.

10. A readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the pose estimation method as described in any one of claims 1-7.

Images