A 6D pose detection method and device, electronic equipment and storage medium

Abstract

This application discloses a 6D pose detection method, apparatus, electronic device, and storage medium. The method involves acquiring an RGB image of the target object to be tracked as a reference image and detecting a first feature point within it; acquiring the RGB image of the current frame from the camera, detecting a second feature point and matching it with the first feature point; when the number of matching points reaches a threshold, tracking these feature points in consecutive image frames; converting the two-dimensional coordinates of the tracked feature points into a first three-dimensional coordinate point set in the camera coordinate system; converting the corresponding feature points in the reference image into a second three-dimensional coordinate point set in the target object coordinate system; calculating a transformation matrix from the target object coordinate system to the camera coordinate system based on the two three-dimensional point sets; and finally, transforming the preset three-dimensional positioning points into the camera coordinate system using the transformation matrix to achieve 6D pose localization of the target object. This method eliminates the need for dedicated markers or model training, requiring only a single RGB image to achieve high-precision, real-time pose estimation.

Description

Technical Field

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

[0002] With the rapid development of robotics and autonomous systems technology, high-precision pose estimation is increasingly widely used in industrial automation, drone navigation, and other fields. Currently, vision-based pose detection systems are mainly divided into two categories: active optical positioning devices and passive visual recognition systems. Active optical systems, such as positioning devices based on infrared lasers or structured light, can achieve high-precision positioning, but their high hardware cost and complex deployment process limit their adoption in small and medium-sized robot or drone platforms. Passive visual systems, such as recognition schemes based on monocular or binocular cameras, typically rely on pre-set coded tags (e.g., AprilTag, ArUco, etc.) for target recognition and localization. While these methods offer advantages such as low cost and ease of integration, their recognition process heavily depends on specific pre-set tags, making it difficult to flexibly adapt to other patterns or natural features, thus limiting the system's applicability and flexibility.

[0003] To overcome the limitations of traditional methods, several label-free pose estimation methods based on natural features and neural networks have emerged in recent years. While these methods avoid dependence on manual labeling, they still require a large amount of labeled data for model training. The training process is time-consuming and computationally resource-intensive, making it difficult to meet the needs of robots or drones in high real-time application scenarios. It also limits their effective deployment on edge devices. Summary of the Invention

[0004] The purpose of this application is to overcome the shortcomings of existing technologies and provide a 6D pose detection method, device, electronic device and storage medium. By combining feature matching of RGB images with depth information from an RGBD camera, real-time, high-precision 6D pose estimation without pre-training and special markers is achieved, solving the problems of poor flexibility and high deployment cost caused by traditional methods relying on specific markers or large amounts of data for training.

[0005] The objective of this application is achieved through the following technical solution: Firstly, this application proposes a 6D pose detection method, including: Step S1: Use the RGB image of the target object to be tracked as a reference image, and detect the first feature point in the reference image; Step S2: Obtain the camera RGB image of the current frame and detect the second feature point in the camera RGB image. Match the first feature point with the second feature point. When the number of successfully matched feature points reaches a preset threshold, execute step S3. Step S3: Track the successfully matched feature points in a continuous sequence of camera RGB image frames; Step S4: Based on the depth information of the RGBD camera, convert the two-dimensional coordinates of the tracked feature points in the camera's RGB image into the first three-dimensional coordinate point set in the camera coordinate system; Step S5: Convert the two-dimensional feature points in the reference image corresponding to the tracked feature points into a second three-dimensional coordinate point set in the target object coordinate system, wherein the Z-axis coordinate of the second three-dimensional coordinate point set is set to zero; Step S6: Based on the first three-dimensional coordinate point set and the second three-dimensional coordinate point set, calculate the transformation matrix from the target object coordinate system to the camera coordinate system. The transformation matrix includes the rotation matrix and the translation matrix. Step S7: Transform at least one three-dimensional positioning point preset in the target object coordinate system to the camera coordinate system through a transformation matrix to achieve 6D pose positioning of the target object.

[0006] In one possible implementation, the algorithms used for detecting feature points in steps S1 and S2 include the SIFT algorithm, the ORB algorithm, or the Super Point algorithm based on neural networks.

[0007] In one possible implementation, the algorithm used for feature point matching in step S2 includes a brute-force matching algorithm, a fast nearest neighbor search algorithm, or a combination of Super Point and Super Glue algorithms based on neural networks.

[0008] In one possible implementation, the algorithm used for feature point tracking in step S3 includes the Lucas-Kanade optical flow algorithm or the neural network-based Omnimotion algorithm.

[0009] In one possible implementation, step S4, which converts the two-dimensional coordinates of the tracked feature points in the camera RGB image into a first three-dimensional coordinate point set in the camera coordinate system, is calculated using the following formula: ; in, The image coordinates of the target pixel. The camera coordinates of the target pixel; , , The camera focal length in image coordinate system form. The horizontal physical size of a single pixel. The vertical physical size of a single pixel; The horizontal distance from the pixel coordinates of the image center to the origin of the pixel coordinate system. This represents the vertical distance from the pixel coordinates of the image center to the origin of the pixel coordinate system. This represents the depth value of the target pixel.

[0010] In one possible implementation, step S6 involves calculating the transformation matrix using the Kabsch algorithm, which includes: Calculate the mean of the first three-dimensional coordinate point set and the second three-dimensional coordinate point set, and then center them. Calculate the dot product matrix of the two centered point sets. ; dot product matrix Perform singular value decomposition to obtain the left singular vector matrix. Arrays containing singular values and the conjugate transpose of the right singular vector ; Based on the left singular vector matrix Arrays containing singular values and the conjugate transpose of the right singular vector Calculate the rotation matrix ; Calculate the rotation matrix Scale scaling factor ; Based on rotation matrix and scale scaling factor Calculate the translation matrix .

[0011] In one possible implementation, the method further includes: A three-dimensional point in the coordinate system of the target object By transforming the matrix Transform to the camera coordinate system to obtain the reprojection point. ; Get the reprojection point in the current frame Corresponding measured three-dimensional coordinates ; Calculate reprojection points Compared with the measured three-dimensional coordinates Error between ; If error Greater than the preset error threshold If the tracking fails, the process returns to step S1 to re-perform feature point matching and tracking.

[0012] Secondly, this application proposes a 6D pose detection device, the device comprising: The detection module is used to use the RGB image of the target object to be tracked as a reference image and detect the first feature point in the reference image; The matching module is used to acquire the camera RGB image of the current frame, detect the second feature point in the camera RGB image, and match the first feature point with the second feature point; The tracking module is used to track successfully matched feature points in a continuous sequence of camera RGB image frames; The coordinate transformation module is used to convert the two-dimensional coordinates of the tracked feature points in the RGB image of the camera into the first three-dimensional coordinate point set in the camera coordinate system based on the depth information of the RGBD camera. The feature point conversion module is used to convert the two-dimensional feature points in the reference image corresponding to the tracked feature points into a second three-dimensional coordinate point set in the target object coordinate system, wherein the Z-axis coordinate of the second three-dimensional coordinate point set is set to zero; The calculation module is used to calculate the transformation matrix from the target object coordinate system to the camera coordinate system based on the first three-dimensional coordinate point set and the second three-dimensional coordinate point set. The transformation matrix includes the rotation matrix and the translation matrix. The matrix transformation module is used to transform at least one three-dimensional positioning point preset in the target object coordinate system to the camera coordinate system through a transformation matrix, thereby realizing the 6D pose positioning of the target object.

[0013] Thirdly, this application also proposes a computer device comprising a processor and a memory, wherein the memory stores a computer program, which is loaded and executed by the processor to implement the 6D pose detection method as described in any of the first aspects.

[0014] Fourthly, this application also proposes a computer-readable storage medium storing a computer program that is loaded and executed by a processor to implement the 6D pose detection method as described in any of the first aspects.

[0015] The main solution and its various further alternatives described above can be freely combined to form multiple solutions, all of which are solutions that can be adopted and are claimed in this application; furthermore, the (non-conflicting alternatives) can also be freely combined with each other and with other alternatives. Those skilled in the art, after understanding the solution of this application, will realize from the prior art and common general knowledge that there are many combinations, all of which are technical solutions to be protected in this application, and will not be exhaustively listed here.

[0016] This application discloses a 6D pose detection method, apparatus, electronic device, and storage medium. The method involves acquiring an RGB image of the target object to be tracked as a reference image and detecting a first feature point within it; acquiring the RGB image of the current frame from the camera, detecting a second feature point and matching it with the first feature point; when the number of matching points reaches a threshold, tracking these feature points in consecutive image frames; based on the depth information of the RGBD camera, converting the two-dimensional coordinates of the tracked feature points into a first three-dimensional coordinate point set in the camera coordinate system; converting the corresponding feature points in the reference image into a second three-dimensional coordinate point set in the target object coordinate system; calculating the transformation matrix from the target object coordinate system to the camera coordinate system based on the two three-dimensional point sets; and finally, transforming the preset three-dimensional positioning points into the camera coordinate system using the transformation matrix to achieve 6D pose localization of the target object. This method does not rely on dedicated markers or model training and only requires a single RGB image to achieve high-precision, real-time pose estimation. Attached Figure Description

[0017] To more clearly illustrate the technical solutions of the embodiments of this application, 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 this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 A flowchart illustrating a 6D pose detection method proposed in an embodiment of this application is shown. Detailed Implementation

[0019] The following specific examples illustrate the implementation of this application. Those skilled in the art can easily understand other advantages and effects of this application from the content disclosed in this specification. This application can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this application. It should be noted that, unless otherwise specified, the following embodiments and features in the embodiments can be combined with each other.

[0020] Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0021] In existing technologies, vision-based pose detection systems can generally be divided into two categories: active optical positioning devices and passive visual recognition systems. Active optical systems (such as those based on infrared lasers or structured light) can achieve high-precision positioning, but their hardware costs are high and deployment is complex, limiting their widespread adoption in small and medium-sized robots or drone platforms. Passive visual systems (such as those based on monocular or binocular cameras) mostly use pre-set coded tags (such as AprilTag, ArUco, etc.) for object recognition and localization. Although these methods are low-cost and easy to integrate, they heavily rely on pre-defined specific tags and cannot flexibly select other patterns for detection.

[0022] To overcome the limitations of traditional methods, some label-free pose estimation methods based on natural features and neural networks have emerged in recent years. Although these methods avoid dependence on manual labeling, they still require a large amount of labeled data for model training, which is time-consuming and computationally resource-intensive. This makes them unsuitable for real-time applications such as robots or drones and also limits their deployment on edge devices.

[0023] Therefore, in order to solve the above-mentioned technical problems, this application proposes a 6D pose detection method, device, electronic device and storage medium, which can be widely used in industrial robot grasping, UAV autonomous navigation and positioning, AGV guidance, augmented reality interaction and other scenarios. It can achieve high-precision real-time pose estimation without relying on pre-designed special markers (such as ArUco, QR code, etc.) or model training or offline learning of the target object.

[0024] Please refer to Figure 1 , Figure 1 This paper illustrates a flowchart of a 6D pose detection method proposed in an embodiment of this application. The method includes: Step S1: Use the RGB image of the target object to be tracked as a reference image and detect the first feature point in the reference image.

[0025] First, the user needs to provide an RGB image of the target object to be tracked as a reference for subsequent pose estimation. This image should clearly show the surface pattern of the object. After the reference image is read into memory, the feature point detection process is started. In this step, the system uses a feature extraction algorithm to perform a global scan and analysis of the reference image to identify and locate key pixels with significant distinguishability in the image. These points are defined as first feature points, and the output is a feature description set consisting of multiple first feature points for the reference image.

[0026] Step S2: Obtain the camera RGB image of the current frame and detect the second feature point in the camera RGB image. Match the first feature point with the second feature point. When the number of successfully matched feature points reaches a preset threshold, execute step S3.

[0027] The algorithm used for feature point matching in step S2 includes brute-force matching algorithm, fast nearest neighbor search algorithm, or a combination of Super Point and Super Glue algorithm based on neural networks.

[0028] The RGBD camera is invoked to capture a real-time RGB image stream of the scene in a loop. For each frame of the acquired RGB image from the camera, a feature detection process is performed to identify key pixels in the current image as second feature points. Subsequently, a feature matching process is initiated to establish an accurate correspondence between the second feature points of the current frame and the first feature points of the reference image.

[0029] To achieve efficient and reliable matching, this step offers a variety of algorithm options, including traditional methods such as computationally intuitive brute-force matching and more efficient fast nearest neighbor search algorithms, as well as advanced matching strategies based on neural networks, such as the SuperPoint and SuperGlue combined algorithm, to ensure excellent matching performance in different application scenarios. When the number of successfully matched feature point pairs reaches a preset threshold (e.g., 6 pairs), it indicates that the target object has been preliminarily and reliably identified in the current field of view. The system then determines that the match is successful and triggers the subsequent feature point tracking process.

[0030] The algorithms used for detecting feature points in steps S1 and S2 include SIFT algorithm, ORB algorithm, or Super Point algorithm based on neural network.

[0031] Step S3: Track the successfully matched feature points in a continuous sequence of RGB image frames from the camera.

[0032] The algorithm used for feature point tracking in step S3 includes the Lucas-Kanade optical flow algorithm or the Omnimotion algorithm based on neural networks.

[0033] To leverage the temporal continuity of video sequences and avoid the enormous computational overhead of re-performing global feature detection and matching for each frame, thus achieving efficient real-time pose estimation, the currently matched feature point set will be tracked in subsequent consecutive camera RGB image frame sequences. To achieve robust and efficient feature tracking, this invention provides a variety of algorithm options. A preferred approach is to use the Lucas-Kanade optical flow algorithm. This method, based on the assumption of constant local brightness and minimal motion in the image, iterates to solve for the displacement vector of feature points between adjacent frames, offering advantages such as low computational cost and high speed, making it ideal for resource-constrained embedded platforms. Another approach is to employ advanced neural network-based methods, such as the Omnimotion algorithm. These methods can model more complex motion patterns and appearance changes using deep learning models, exhibiting stronger adaptability and higher tracking accuracy in situations involving rapid motion, motion blur, and partial occlusion.

[0034] Step S4: Based on the depth information of the RGBD camera, convert the two-dimensional coordinates of the tracked feature points in the camera's RGB image into the first three-dimensional coordinate point set in the camera coordinate system.

[0035] Step S4, which converts the two-dimensional coordinates of the tracked feature points in the camera's RGB image into the first three-dimensional coordinate point set in the camera coordinate system, uses the following formula: ; in, The image coordinates of the target pixel. The camera coordinates of the target pixel; , , The camera focal length in image coordinate system form. The horizontal physical size of a single pixel. The vertical physical size of a single pixel; The horizontal distance from the pixel coordinates of the image center to the origin of the pixel coordinate system. This represents the vertical distance from the pixel coordinates of the image center to the origin of the pixel coordinate system. This represents the depth value of the target pixel.

[0036] After successfully tracking feature points, the 2D image coordinates are mapped to 3D physical space, providing spatial geometric data for subsequent pose calculation. The system obtains the pixel coordinates of each tracked feature point by accessing the depth image synchronously provided by the RGBD camera. The corresponding precise depth value Subsequently, based on the pinhole camera model, the transformation from two-dimensional to three-dimensional was completed using the following coordinate back projection formula: , Represents the coordinates of the feature point in the image pixel coordinate system; and It is determined by the camera's physical focal length F and pixel size , The normalized focal lengths that are jointly determined transform pixel coordinates into units of measurement in the camera coordinate system. and These are the principal point coordinates of the camera, representing the intersection of the camera's optical axis and the image plane. Depth value. This is directly used as the Z-axis coordinate of the point in the camera coordinate system. The X and Y coordinates are calculated using the principle of similar triangles, that is, the normalized image coordinates are scaled using depth values. Finally, all the tracked feature points, after being transformed according to their correspondence, together constitute the first three-dimensional coordinate point set representing the spatial position of key points on the object's surface in the camera coordinate system.

[0037] Step S5: Convert the two-dimensional feature points in the reference image corresponding to the tracked feature points into a second three-dimensional coordinate point set in the target object coordinate system, wherein the Z-axis coordinate of the second three-dimensional coordinate point set is set to zero.

[0038] First, the first feature point detected from the reference image in step S1 and successfully matched with the current frame is extracted. This feature point is then converted from a two-dimensional image pixel into a three-dimensional point in the target object coordinate system, forming a second set of three-dimensional coordinate points. The conversion principle is based on a core assumption: since the reference image is a basic planar object and is used as a template, the surface of the object can be ideally regarded as a plane located at Z=0.

[0039] The two-dimensional coordinates (X-axis and Y-axis coordinates) of each feature point in the reference image pixel coordinate system are directly retained, while its Z-axis coordinate in the object coordinate system is uniformly preset to zero. Through this operation, feature points that originally only had image position information are successfully given a definition in the object's own three-dimensional space, thus constructing a second three-dimensional coordinate point set in the object coordinate system that corresponds one-to-one with the first three-dimensional coordinate point set in the camera coordinate system obtained in step S4. The spatial transformation relationship between these two point sets describes the pose and position of the target object relative to the camera.

[0040] Step S6: Based on the first three-dimensional coordinate point set and the second three-dimensional coordinate point set, calculate the transformation matrix from the target object coordinate system to the camera coordinate system. The transformation matrix includes the rotation matrix and the translation matrix.

[0041] Calculate the transformation matrix from the 3D points of the object to be tracked to the 3D points in the RGBD camera space coordinates. The transformation matrix can be solved using the Kabsch algorithm or the least squares method.

[0042] In step S6, the method for calculating the transformation matrix is ​​the Kabsch algorithm, which includes: Calculate the mean of the first three-dimensional coordinate point set and the second three-dimensional coordinate point set, and then center them. Calculate the dot product matrix of the two centered point sets. ; dot product matrix Perform singular value decomposition to obtain the left singular vector matrix. Arrays containing singular values and the conjugate transpose of the right singular vector ; Based on the left singular vector matrix Arrays containing singular values and the conjugate transpose of the right singular vector Calculate the rotation matrix ; Calculate the rotation matrix Scale scaling factor ; Based on rotation matrix and scale scaling factor Calculate the translation matrix .

[0043] If the transformation matrix is ​​solved using the Kabsch algorithm, the solution process is as follows: Assume there are two sets of points that correspond one-to-one. and , Represents the original point set, The point set representing the result of the matrix transformation needs to be solved using the Kabsch algorithm. arrive The transformation matrix.

[0044] First, solve each problem separately. and Mean of all points: ,in, Represents a point set The mean, Represents a point set The mean; Represents a point set The i-th point in Represents a point set The i-th point in the set; n represents the point set. or point set The total number of points in the set. Next, the original point set... and Centralized processing: ,in, Represents the centralized point set , Represents the centralized point set Then solve it. and dot product : , for Transpose it. Then... Perform singular value decomposition to obtain , , : , It is a left singular vector matrix; An array containing singular values; Let be the conjugate transpose of the right singular vector, since the point set and All are real number matrices, therefore The conjugate transpose of V is the transpose of V.

[0045] Will transpose and The rotation matrix can be obtained by taking the dot product of the transpose of the matrix. : , It is a rotation matrix; Calculate the rotation matrix Scale scaling : ,in, Represents a point set or point set The total number of points in the matrix. Finally, calculate the translation matrix. : Using the obtained rotation matrix Translation matrix This allows us to calculate the transformation matrix from the 3D points of the object to be tracked to the 3D points in the RGBD camera space coordinates.

[0046] Step S7: Transform at least one three-dimensional positioning point preset in the target object coordinate system to the camera coordinate system through a transformation matrix to achieve 6D pose positioning of the target object.

[0047] After obtaining the transformation matrix from the target object coordinate system to the camera coordinate system, the final pose application and output are performed. The system first calls key 3D positioning points pre-defined in the target object coordinate system. These positioning points are typically manually set according to application requirements, such as key corner points of the object boundary, the center point of the tool, or any other practically significant feature locations. Then, the complete transformation matrix calculated in step S6 is applied to these pre-defined 3D positioning points, mapping them all to the current camera coordinate system through coordinate transformation. Finally, by outputting the coordinates of these positioning points transformed to the camera coordinate system and the spatial structure they form, the system fully realizes the determination of the target object's three-axis position and three-axis orientation in three-dimensional space, i.e., 6D pose localization.

[0048] The method also includes: A three-dimensional point in the coordinate system of the target object By transforming the matrix Transform to the camera coordinate system to obtain the reprojection point. ; Get the reprojection point in the current frame Corresponding measured three-dimensional coordinates ; Calculate reprojection points Compared with the measured three-dimensional coordinates Error between ; If error Greater than the preset error threshold If the tracking fails, the process returns to step S1 to re-perform feature point matching and tracking.

[0049] Repeat steps S3 to S7 until one or more tracking points are lost or shifted. Then, start the feature point matching operation again from step 1. The method to determine if a tracking point deviates from its original position can be an error-based method. After transforming the 3D points of the object to be tracked to RGBD camera space coordinates using a transformation matrix, calculate the absolute error of each point. If the error value of one or more points exceeds a set threshold, then tracking is considered a failure, and feature point matching and tracking need to be restarted from step S1.

[0050] The method for determining whether a tracking point deviates from its original position is as follows: Take a 3D point on the object to be tracked... Through the estimated transformation matrix Transform to the current camera coordinate system to obtain its reprojected 3D points. : .

[0051] At the same time, the measured three-dimensional coordinates of the point's location can be directly observed using the RGBD camera data of the current frame. Calculate the absolute error at that point. : , This indicates the calculation of the Euclidean norm (i.e., calculating the straight-line distance between two points). For all tracked points, if the error of any one of the points... Greater than the set threshold That is, satisfying: If the tracking fails, the system needs to start feature point matching and tracking again from step S1.

[0052] Compared with the prior art, the embodiments of this application have the following beneficial effects: First, it eliminates the need for extensive data training and fine-tuning required by traditional neural networks, enabling rapid deployment that is ready to use immediately, and significantly improving the speed at which robots and other systems adapt to new work objects and environments.

[0053] Secondly, pose estimation can be started by inputting only a single RGB image of the target object, which greatly reduces the requirements for the complexity of the input data and simplifies the system operation process.

[0054] Third, it does not rely on dedicated markers such as ArUco or QR codes, but can identify and locate objects using any natural patterns on their own, greatly expanding the scope of its application scenarios.

[0055] Fourth, by integrating mature traditional vision algorithms with low-cost RGBD cameras, a cost-effective solution is provided while ensuring accuracy and avoiding expensive dedicated hardware.

[0056] Fifth, by combining efficient feature matching, tracking, and real-time 3D coordinate back projection based on depth information, high-precision, real-time 6D pose detection can be achieved on resource-constrained edge devices.

[0057] The following is a possible implementation of a 6D pose detection device, which is used to perform the various execution steps and corresponding technical effects of the 6D pose detection method shown in the above embodiments and possible implementations. The device includes: The detection module is used to use the RGB image of the target object to be tracked as a reference image and detect the first feature point in the reference image; The matching module is used to acquire the camera RGB image of the current frame, detect the second feature point in the camera RGB image, and match the first feature point with the second feature point; The tracking module is used to track successfully matched feature points in a continuous sequence of camera RGB image frames; The coordinate transformation module is used to convert the two-dimensional coordinates of the tracked feature points in the RGB image of the camera into the first three-dimensional coordinate point set in the camera coordinate system based on the depth information of the RGBD camera. The feature point conversion module is used to convert the two-dimensional feature points in the reference image corresponding to the tracked feature points into a second three-dimensional coordinate point set in the target object coordinate system, wherein the Z-axis coordinate of the second three-dimensional coordinate point set is set to zero; The calculation module is used to calculate the transformation matrix from the target object coordinate system to the camera coordinate system based on the first three-dimensional coordinate point set and the second three-dimensional coordinate point set. The transformation matrix includes the rotation matrix and the translation matrix. The matrix transformation module is used to transform at least one three-dimensional positioning point preset in the target object coordinate system to the camera coordinate system through a transformation matrix, thereby realizing the 6D pose positioning of the target object.

[0058] This preferred embodiment provides a computer device that can implement the steps of any embodiment of the 6D pose detection method provided in this application. Therefore, it can achieve the beneficial effects of the 6D pose detection method provided in this application. For details, please refer to the previous embodiments, which will not be repeated here.

[0059] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by instructions, or by instructions controlling related hardware. These instructions can be stored in a computer-readable storage medium and loaded and executed by a processor. Therefore, embodiments of this application provide a storage medium storing multiple instructions that can be loaded by a processor to execute the steps of any embodiment of the 6D pose detection method provided in this application.

[0060] The storage medium may include: read-only memory (ROM), random access memory (RAM), disk or optical disk, etc.

[0061] Since the instructions stored in the storage medium can execute the steps in any of the 6D pose detection method embodiments provided in this application, the beneficial effects that any of the 6D pose detection methods provided in this application can achieve can be realized. For details, please refer to the previous embodiments, which will not be repeated here.

[0062] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A 6D pose detection method, characterized in that, include: Step S1: Use the RGB image of the target object to be tracked as a reference image, and detect the first feature point in the reference image; Step S2: Obtain the camera RGB image of the current frame and detect the second feature point in the camera RGB image. Match the first feature point with the second feature point. When the number of successfully matched feature points reaches a preset threshold, execute step S3. Step S3: Track the successfully matched feature points in a continuous sequence of camera RGB image frames; Step S4: Based on the depth information of the RGBD camera, convert the two-dimensional coordinates of the tracked feature points in the camera's RGB image into the first three-dimensional coordinate point set in the camera coordinate system; Step S5: Convert the two-dimensional feature points in the reference image corresponding to the tracked feature points into a second three-dimensional coordinate point set in the target object coordinate system, wherein the Z-axis coordinate of the second three-dimensional coordinate point set is set to zero; Step S6: Based on the first three-dimensional coordinate point set and the second three-dimensional coordinate point set, calculate the transformation matrix from the target object coordinate system to the camera coordinate system. The transformation matrix includes the rotation matrix and the translation matrix. Step S7: Transform at least one three-dimensional positioning point preset in the target object coordinate system to the camera coordinate system through a transformation matrix to achieve 6D pose positioning of the target object.

2. The 6D pose detection method of claim 1, wherein, The algorithms used for detecting feature points in steps S1 and S2 include SIFT algorithm, ORB algorithm, or Super Point algorithm based on neural network.

3. The 6D pose detection method of claim 1, wherein, The algorithm used for feature point matching in step S2 includes brute-force matching algorithm, fast nearest neighbor search algorithm, or a combination of Super Point and Super Glue algorithm based on neural network.

4. The 6D pose detection method of claim 1, wherein, The algorithm used for feature point tracking in step S3 includes the Lucas-Kanade optical flow algorithm or the Omnimotion algorithm based on neural networks.

5. The 6D pose detection method of claim 1, wherein, Step S4, which converts the two-dimensional coordinates of the tracked feature points in the camera's RGB image into the first three-dimensional coordinate point set in the camera coordinate system, uses the following formula: in, The image coordinates of the target pixel. The camera coordinates of the target pixel; , , The camera focal length in image coordinate system form. The horizontal physical size of a single pixel. The vertical physical size of a single pixel; The horizontal distance from the pixel coordinates of the image center to the origin of the pixel coordinate system. This represents the vertical distance from the pixel coordinates of the image center to the origin of the pixel coordinate system. This represents the depth value of the target pixel.

6. The 6D pose detection method of claim 1, wherein, In step S6, the method for calculating the transformation matrix is ​​the Kabsch algorithm, including: Calculate the mean of the first three-dimensional coordinate point set and the second three-dimensional coordinate point set, and then center them. computing a dot product matrix of the two centered point sets ; Dot product matrix Singular value decomposition is performed to obtain a left singular vector matrix , an array containing singular values , and a conjugate transpose matrix of right singular vectors ; Based on the left singular vector matrix Arrays containing singular values and the conjugate transpose of the right singular vector Calculate the rotation matrix ; Calculate the rotation matrix Scale scaling factor ; Based on a rotation matrix and a scale factor Computing a translation matrix .

7. The 6D pose detection method of claim 1, wherein, The method further includes: a three-dimensional point in the target object coordinate system by the transformation matrix converted to the camera coordinate system to obtain a re-projection point ; Get the reprojection point in the current frame Corresponding measured three-dimensional coordinates ; Computing re-projection points Error between the measured three-dimensional coordinates Error between the measured three-dimensional coordinates ; If the error is greater than a preset error threshold , it is determined that the tracking fails, and the process returns to step S1 to perform feature point matching and tracking again.

8. A 6D pose detection apparatus, characterized in that The device includes: The detection module is used to use the RGB image of the target object to be tracked as a reference image and detect the first feature point in the reference image; The matching module is used to acquire the camera RGB image of the current frame, detect the second feature point in the camera RGB image, and match the first feature point with the second feature point; The tracking module is used to track successfully matched feature points in a continuous sequence of camera RGB image frames; The coordinate transformation module is used to convert the two-dimensional coordinates of the tracked feature points in the RGB image of the camera into the first three-dimensional coordinate point set in the camera coordinate system based on the depth information of the RGBD camera. The feature point conversion module is used to convert the two-dimensional feature points in the reference image corresponding to the tracked feature points into a second three-dimensional coordinate point set in the target object coordinate system, wherein the Z-axis coordinate of the second three-dimensional coordinate point set is set to zero; The calculation module is used to calculate the transformation matrix from the target object coordinate system to the camera coordinate system based on the first three-dimensional coordinate point set and the second three-dimensional coordinate point set. The transformation matrix includes the rotation matrix and the translation matrix. The matrix transformation module is used to transform at least one three-dimensional positioning point preset in the target object coordinate system to the camera coordinate system through a transformation matrix, thereby realizing the 6D pose positioning of the target object.

9. A computer device, characterized in that, The computer device includes a processor and a memory, the memory storing a computer program, which is loaded and executed by the processor to implement the 6D pose detection method as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which is loaded and executed by a processor to implement the 6D pose detection method as described in any one of claims 1-7.

Images