A 6D pose estimation method and system based on an RGBD camera

Abstract

The application discloses a 6D pose estimation method and system based on an RGBD camera, relates to the technical field of computer vision processing, and comprises camera imaging calibration; camera parameter calibration based on the calibrated camera imaging; three-dimensional model reconstruction and dataset construction; target object segmentation and actual size calculation based on the three-dimensional model and the dataset; a 6D pose estimation network model is established based on the three-dimensional model and the dataset; and pose estimation is performed based on the actual size.The application has the advantages that by combining an RGB image, a depth image and point cloud data, the accuracy of 6D pose estimation is remarkably improved, object region segmentation is performed by using a SAM model, and by combining depth information and point cloud data, not only the actual size of the object is intuitively calculated, but also the size estimation precision is improved; and the introduction of the depth information makes the pose estimation system have better adaptability to objects of different sizes, shapes and surface features, and improves the robustness and accuracy of the system.

Description

Technical Field

[0001] The present invention relates to the field of computer vision processing technology, and in particular to a 6D pose estimation method and system based on an RGBD camera. Background Art

[0002] With the rapid development of computer vision and robotics, estimating the 6D pose of target objects has become a crucial prerequisite for achieving object interaction tasks. The 6D pose, consisting of the target's translational position [x, y, z] and rotational angles [Rx, Ry, Rz], describes the target's specific position and posture in three-dimensional space relative to the camera coordinate system. Traditional 6D pose estimation methods rely primarily on high-precision 3D models of the target or specific target recognition algorithms, making them difficult to achieve universality and real-time performance. In particular, using RGBD cameras in complex scenes to quickly and accurately estimate an object's 6D pose remains challenging.

[0003] In recent years, deep learning-based 6D pose estimation methods have achieved significant breakthroughs, with research mainly falling into two categories: instance-level and category-level. Instance-level methods rely on high-precision 3D models of the target, and typically a network is only applicable to a specific object. Although instance-level methods perform well in terms of accuracy, they are difficult to meet the real-time requirements of practical applications due to their lack of versatility and complex pre-processing. Category-level methods, on the other hand, attempt to predict the 6D pose of similar objects using a single network model. While they have good generalization capabilities, they still face problems such as reduced estimation accuracy due to intra-category shape differences. To address these issues, we propose a 6D pose estimation method and system based on an RGBD camera. Summary of the Invention

[0004] To address the aforementioned technical issues, a 6D pose estimation method and system based on an RGBD camera are provided. This approach addresses the aforementioned issue of instance-level methods, which rely on a high-precision 3D model of the target. Typically, a network is only applicable to a specific object. Although instance-level methods offer good accuracy, their lack of versatility and complex initial processing make them difficult to meet the real-time requirements of practical applications. Category-level methods, on the other hand, attempt to predict the 6D pose of similar objects using a single network model. While they offer good generalization capabilities, they still face the problem of reduced estimation accuracy due to intra-category shape differences.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is: a 6D pose estimation method and system based on RGBD camera, comprising:

[0006] Step 1: Camera imaging calibration;

[0007] Step 2: Calibrate camera parameters based on the calibrated camera imaging;

[0008] Step 3: 3D model reconstruction and dataset construction;

[0009] Step 4: Segment the target object and calculate its actual size based on the 3D model and dataset;

[0010] Step 5: Build a 6D pose estimation network model based on the 3D model and dataset;

[0011] Step 6: Estimate pose based on actual size.

[0012] Preferably, the camera imaging calibration includes: when light passes through the lens and is projected onto the imaging plane, distortion occurs, which is divided into radial distortion and tangential distortion. The radial distortion is divided into barrel distortion and pincushion distortion, while the tangential distortion is caused by the non-parallelism between the lens and the imaging plane during the camera installation process, which is an installation error;

[0013] The normalized coordinate calculation formula for radial distortion is as follows:

[0014] x distorted =x(1+k1r 2 );

[0015] y distorted =y(1+k1r 2 )#(1)

[0016] In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, and k1 is the distortion coefficient;

[0017] The normalized coordinate calculation formula for tangential distortion is as follows:

[0018] x distorted =x+2p1xy+p2(r 2 +2x 2 );

[0019] y distorted =y+2p2xy+p1(r 2 +2y 2 )#(2)

[0020] In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, and p1 and p2 are additional distortion parameters;

[0021] For any point p on the image, the three distortion parameters k1, p1, and p2 can be combined to find the exact coordinates of the point (x, y) in the pixel coordinate system. p The specific calculation formula for the normalized coordinates of a point is:

[0022] x distorted =x(1+k1r 2 )+2p1xy+p2(r 2 +2x 2 );

[0023] y distorted =y(1+k1r 2 )+2p2xy+p1(r 2 +2y 2 )#(3)

[0024] Preferably, the camera parameter calibration based on the calibrated camera imaging includes first arranging 13 ArUco calibration plates in a circular shape around the object to be detected, without duplicate marks and evenly distributed, and taking n RGB images and infrared images of the ArUco calibration plates at different positions and angles through the ROS system, and calibrating the data. The intrinsic parameter matrix and extrinsic parameter matrix of the RGB camera, depth camera, and infrared camera are obtained through calibration;

[0025] The intrinsic parameter matrix K describes the optical characteristics of the camera and reflects the camera parameters when projecting from a three-dimensional object to a two-dimensional image. The formula is as follows:

[0026]

[0027] Among them, f x and f y is the focal length of the RGB camera in the horizontal and vertical directions (unit: pixel), which reflects the magnification of the camera during imaging; c x and c y is the pixel coordinate of the principal point, that is, the position of the image center in the pixel coordinate system;

[0028] The extrinsic matrix [R, T] describes the relative position and rotation between cameras:

[0029]

[0030] Among them, [X r , Y r , Z r ] T is the three-dimensional point coordinate in the RGB camera coordinate system; [X d , Y d , Z d ] T is the three-dimensional point coordinate in the depth camera coordinate system; R is the rotation matrix, which describes the relative rotation between the two cameras; [t x , t y , t z ] Tis the translation vector, which describes the relative translation between the RGB camera and the depth camera.

[0031] Preferably, the three-dimensional reconstruction and dataset construction specifically include: performing all-round video capture of the target object to ensure that the object is fully presented from multiple angles and different distances during the shooting process to avoid occlusion, and using OpenCV to decompose the video file into a single-frame RGB image of a fixed size; then extracting the RGB image and using the COLMAP tool to create a dataset, and then performing SIFT feature extraction and matching, and then running the SFM method to generate a sparse point cloud model. During the SFM process, COLMAP will automatically estimate the camera pose of each frame of the image, specifically through triangulation and multi-view geometry analysis, and use image feature matching to calculate the camera's external parameters (rotation matrix and translation vector), thereby determining the camera's position and orientation in three-dimensional space. Generate a depth map through the PatchMatch algorithm, and then perform deep fusion to generate the final dense point cloud model; crop and annotate the generated point cloud model, and construct the object's coordinate system relative to the camera, thereby generating a dataset containing 6D pose annotations.

[0032] Preferably, the target object segmentation and actual size calculation based on the three-dimensional model and the data set includes: ROS synchronizes the received RGB image, depth image and point cloud data through a message filter; using Matplotlib and RectangleSelector tools, allowing the user to interactively select the area where the target object is located in the RGB image and select a rectangular area through the mouse, and the system will record the coordinates of the rectangular area as the input for segmentation; using the pre-trained SAM model to segment the object, and using the SAM model to segment the area according to the rectangular box input selected by the user to obtain a segmentation mask; using the camera's intrinsic parameter matrix to project the point cloud into the RGB image coordinate system, calculate the position of each point in the image, and calculate the projection coordinates in the image coordinate system through the camera's intrinsic parameter formula:

[0033]

[0034]

[0035] Among them, f x and f y are the camera focal length, c x and c y are the principal point coordinates (the principal point coordinates represent the projection position of the optical axis (that is, the light from the center point of the lens) on the imaging plane in the image coordinate system; in the intrinsic parameter matrix, the principal point coordinates are used to correct the lens offset to ensure that the coordinates on the image accurately reflect the imaging situation of the camera), x, y, z are the coordinates of the 3D point cloud;

[0036] By cross-matching the projected coordinates with the segmentation mask, the point cloud data belonging to the segmentation area is filtered out, and the maximum Euclidean distance between them is calculated and saved:

[0037]

[0038] Among them, x i ,y i , z i and x j ,y j , z j are the coordinates of any two points in the point cloud.

[0039] Preferably, the establishing of a 6D pose estimation network model based on a three-dimensional model and a data set includes:

[0040] The RGB image and depth image are aligned and normalized, and the ResNet network is used to extract features from the multi-scale RGB image and depth image respectively to obtain multi-scale image features and depth information. Then, the feature maps of the receptive fields of the RGB image features and depth image features at different scales are obtained by using the dilated convolution with different dilation rates. The position attention mechanism and the convolution block attention mechanism are combined to finally fuse the features.

[0041] The specific calculation formula for the receptive field of the dilated convolution is:

[0042] RF i+1 =RF i +(k`-1)×S i #(8)

[0043]

[0044] Where RF i+1 Represents the receptive field of the current layer, RF i represents the receptive field of the previous layer, k represents the size of the convolution kernel, S i Represents the product of the stride lengths of all previous layers; ∏ represents cumulative multiplication, STride i Represents the step size of the i-th layer, and formula (9) represents the product of all the step sizes of the first i layers, which is used to consider the scaling effect after multi-layer convolution when calculating the receptive field. By multiplying the step size layer by layer, the area size of the receptive field of the current layer covering the input image can be calculated.

[0045] Preferably, the position attention mechanism specifically includes:

[0046] First, the feature map A (C*H*W) is passed through three convolutional layers to obtain three feature maps {B, C, D} with a shape of C*H*W. Then the shape is reconstructed to C*N, where C represents the number of channels, H represents the matrix height, W represents the matrix width, and N = H×W represents the number of pixels. The transpose of the matrix C and B is multiplied together, and then the attention feature map S (N*N) is obtained through softmax. The transpose of the matrix D and S is multiplied together, and the shape of the resulting matrix is ​​reconstructed to C×H×W and then multiplied by the scale factor α. Finally, it is added to the input matrix A to obtain the final output matrix E. The calculation formula is as follows:

[0047]

[0048] Where S ji Represents each element of the S matrix, indicating the influence of position i on position j, E j represents each element in E, α is the scale factor, D i represents the eigenvalue of position i in the feature map D, A j Represents the eigenvalue of position j in the feature map A, and the element is used to calculate each element E in the final output matrix E j , thereby generating a weight-optimized feature map; B i Represents the feature vector of the i-th position in the feature map B obtained after extraction by the convolution layer, C j Represents the feature vector of the i-th position in the feature map C; the two feature vectors are calculated by the dot product operation to calculate the similarity between them, thereby obtaining the element S in the attention feature matrix S ji , which represents the influence of position j on position i.

[0049] The convolutional block attention mechanism is composed of a channel attention module and a spatial attention module in series. The input feature map is first subjected to average pooling and maximum pooling respectively, and then the channel weights are learned through a shared multi-layer perceptron and a Sigmoid activation function. The results are then subjected to average pooling and maximum pooling respectively, and then a 7×7 dilated convolution and a Sigmoid activation function are used to learn the weights of each point in space. Finally, a weight-optimized feature map is generated, and the perspective image with the highest correlation is selected as the final reference perspective based on the weighted feature map.

[0050] Preferably, the depth image is combined with the preliminary estimated pose (step five describes the establishment of a 6D pose estimation network model, the use of multi-scale feature extraction and attention mechanism, the RGB image and the depth image are processed, so as to obtain a preliminary 6D pose estimation result of the object; specifically, the network model is based on the three-dimensional model and the data set, and first obtains the preliminary translation vector and rotation matrix of the object. This preliminary estimated pose is then combined with the pixel depth value of the depth image in the sixth step, and is iteratively optimized multiple times to finally obtain an accurate 6D pose), the actual position of the object is updated using the depth value of each pixel in the depth image, and the distance between the object and the camera is calculated, and the preliminary estimated pose is iteratively optimized multiple times, and the pose is adjusted to minimize the error to obtain the final 6D pose, including the translation vector and rotation matrix of the object.

[0051] Preferably, the pose estimation based on actual size is specifically as follows: the actual size of the object obtained by point cloud computing in step four is loaded to accurately estimate the 6D pose, and the estimation result includes the translation position (x, y, z) and rotation angle (Rx, Ry, Rz) of the object, and the accuracy of the pose estimation is further optimized in combination with the depth information and size information (by using the depth information and size information in the depth image to adjust and correct the pose estimation, the robustness and accuracy of the system can be improved), thereby adjusting the pose and size of the reference object in the dataset, and the 6D pose estimation network loads the adjusted dataset for pose estimation and pose refinement.

[0052] Preferably, a 6D pose system based on an RGBD camera is used to implement the above-mentioned 6D pose estimation method, comprising:

[0053] A storage module, wherein the computer is used to store the ROS system for controlling the robot's motion, the three-dimensional model, the 6D pose annotated data set, the pre-trained SAM model, and the 6D pose estimation network model;

[0054] An RGB camera, which captures a video of the object to be measured;

[0055] An image processing module includes an OpenCV tool to decompose video files into single-frame RGB images of fixed size, a COLMAP tool to create a data set, and SIFT to perform feature extraction and matching on the RGB images;

[0056] A calculation module, the calculation module is used to store calculation formulas and perform calculations;

[0057] An interactive module, which allows the user to interactively select the target object in the RGB image and frame a rectangular area using the mouse, and displays the 6D pose;

[0058] Image analysis module, which analyzes the depth value of each pixel in the depth image, determines the distance between the object and the camera, and obtains the final 6D pose by adjusting the pose to minimize the error

[0059] The control module is used to realize data transmission and information interaction between modules. The control module includes an information receiving unit and a control unit. The information receiving unit is used to receive image data processed by the image processing module, and the control unit is used to call data in the storage module.

[0060] Compared with the prior art, the advantages of the present invention are:

[0061] 1. This invention significantly improves the accuracy of 6D pose estimation by combining RGB images, depth images, and point cloud data. By using the SegmentAnything Model (SAM) to segment object regions and combining depth information with point cloud data, it not only intuitively calculates the actual size of objects but also improves the accuracy of size estimation. The introduction of depth information makes the pose estimation system more adaptable to objects of varying sizes, shapes, and surface features, and it performs well in complex scenes and under varying lighting conditions, improving the robustness and accuracy of the system.

[0062] 2. By capturing video and using the COLMAP tool to generate sparse point clouds and reference image pose data, this invention reduces reliance on CAD models of target objects or direct measurements of real-world dimensions. This innovation significantly simplifies the dataset construction process and improves data acquisition efficiency. Multi-view capture ensures object integrity and comprehensive coverage, resulting in a more accurate and versatile dataset suitable for a variety of application scenarios. Compared to existing systems that rely on specific object models, this invention offers advantages in flexibility and adaptability.

[0063] 3. The pose estimation network model used in the present invention has excellent feature extraction capabilities. It performs feature processing on RGB images and depth images through a multi-scale convolutional network, and combines depth information to capture object features at different scales, further improving the accuracy of detection and estimation. The viewpoint selection mechanism can select the optimal perspective from multi-angle images and combine depth information to make pose estimation more accurate. The pose refinement module, which has undergone multiple iterations of optimization, ensures that the estimation results remain stable and accurate even when dealing with complex rotations and tilts of objects. In particular, after combining depth information, the system's performance in three-dimensional space has been significantly improved, and it can better handle complex poses and spatial changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 is a flow chart of the present invention;

[0065] Figure 2 is a system block diagram of the present invention;

[0066] Figure 3 This is a pose estimation graph of an embodiment of the present invention. DETAILED DESCRIPTION

[0067] The following description is intended to disclose the present invention so that those skilled in the art can implement the present invention. The preferred embodiments described below are merely examples, and those skilled in the art may conceive of other obvious variations.

[0068] Reference Figure 1-2 As shown, a 6D pose estimation method and system based on RGBD camera, including:

[0069] Step 1: Camera imaging calibration;

[0070] Camera imaging calibration involves the distortion of light when it passes through the lens and is projected onto the imaging plane. This distortion is divided into radial distortion and tangential distortion. Radial distortion is divided into barrel distortion and pincushion distortion, while tangential distortion is caused by the non-parallelism between the lens and the imaging plane during camera installation and is considered an installation error.

[0071] The normalized coordinate calculation formula for radial distortion is as follows:

[0072] x distorted =x(1+k1r 2 );

[0073] y distorted =y(1+k1r 2 )#(1)

[0074] In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, and k1 is the distortion coefficient;

[0075] The normalized coordinate calculation formula for tangential distortion is as follows:

[0076] y distorted =x+2p1xy+p2(r 2 +2x 2 );

[0077] y distorted =y+2p2xp+p1(r 2 +2y 2 )#(2)

[0078] In the formula, (x distorted ,y distorted) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, and p1 and p2 are additional distortion parameters;

[0079] For any point p on the image, the three distortion parameters k1, p1, and p2 can be combined to find the exact coordinates of the point (x, y) in the pixel coordinate system. The specific calculation formula for the normalized coordinates of point p is:

[0080] x distorted =x(1+k1r 2 )+2p1xy+p2(r 2 +2x 2 );

[0081] y distorted =y(1+k1r 2 )+2p2xy+p1(r 2 +2y 2 )#(3)

[0082] By correcting distortion, especially radial distortion and tangential distortion, the image quality can be significantly improved, making the shape of objects in the image closer to reality, which is crucial for subsequent image processing and pose estimation. Distortion correction reduces image distortion caused by lens distortion, thereby improving the accuracy of feature point position and shape information extracted from the image, further enhancing the accuracy of 6D pose estimation.

[0083] Step 2: Calibrate camera parameters based on the calibrated camera imaging;

[0084] Camera parameter calibration based on calibrated camera imaging involves first placing 13 ArUco calibration plates in a circular pattern around the object to be detected, with no duplicate markings and evenly spaced. Using the ROS system, n RGB and infrared images of the ArUco calibration plates are captured at different positions and angles, and the data is calibrated. This calibration yields the intrinsic and extrinsic parameter matrices for the RGB camera, depth camera, and infrared camera.

[0085] The intrinsic parameter matrix K describes the optical characteristics of the camera and reflects the camera parameters when projecting from a three-dimensional object to a two-dimensional image. The formula is as follows:

[0086]

[0087] Among them, f x and f y is the focal length of the RGB camera in the horizontal and vertical directions (unit: pixel), which reflects the magnification of the camera during imaging; c x and c y is the pixel coordinate of the principal point, that is, the position of the image center in the pixel coordinate system;

[0088] The extrinsic matrix [R, T] describes the relative position and rotation between cameras:

[0089]

[0090] Among them, [X r , Y r , Z r ] T is the three-dimensional point coordinate in the RGB camera coordinate system; [X d , Y d , Z d ] T is the three-dimensional point coordinate in the depth camera coordinate system; R is the rotation matrix, which describes the relative rotation between the two cameras; [t x , t y , t z ] T is the translation vector, which describes the relative translation between the RGB camera and the depth camera.

[0091] Using the ArUco calibration board and ROS system, rich calibration data can be obtained at different positions and angles. High-precision intrinsic and extrinsic parameter matrices are obtained through data calibration to ensure the accuracy of camera parameters. The joint calibration of RGB cameras, depth cameras, and infrared cameras can simultaneously obtain multiple types of image data, improving the versatility and flexibility of the system, reducing manual intervention, and improving calibration efficiency and reliability.

[0092] Step 3: 3D model reconstruction and dataset construction;

[0093] 3D reconstruction and dataset construction specifically include all-round video capture of the target object to ensure that the object is fully presented from multiple angles and different distances during the shooting process to avoid occlusion, and using OpenCV to decompose the video file into single-frame RGB images of fixed size; then extract the RGB image and use the COLMAP tool to create a dataset, followed by SIFT feature extraction and matching, and then run the SFM method to generate a sparse point cloud model. During the SFM process, COLMAP automatically estimates the camera pose of each frame of the image. Specifically, through triangulation and multi-view geometry analysis, the camera's extrinsic parameters (rotation matrix and translation vector) are calculated using image feature matching to determine the camera's position and orientation in 3D space. The PatchMatch algorithm generates a depth map, and then deep fusion is performed to generate the final dense point cloud model; the generated point cloud model is cropped and annotated to construct the object's coordinate system relative to the camera, thereby generating a dataset containing 6D pose annotations.

[0094] Through all-round video acquisition, the target object is fully presented from multiple angles and distances, avoiding occlusion problems and improving the completeness and accuracy of 3D reconstruction. The use of mature tools such as OpenCV and COLMAP for image processing and 3D reconstruction improves processing efficiency and result quality. The generated dense point cloud model, after cropping and annotation, can construct the object's precise coordinate system relative to the camera, providing a high-quality annotated dataset for subsequent 6D pose estimation.

[0095] Step 4: Segment the target object and calculate its actual size based on the 3D model and dataset;

[0096] The segmentation and actual size calculation of the target object based on the 3D model and dataset include: ROS synchronizes the received RGB image, depth image and point cloud data through message filters; uses Matplotlib and RectangleSelector tools to allow users to interactively select the area where the target object is located in the RGB image and select a rectangular area through mouse interaction. The system will record the coordinates of the rectangular area as the input for segmentation; uses the pre-trained SAM model for object segmentation. Based on the rectangular box input selected by the user, the SAM model is used to segment the area and obtain the segmentation mask; uses the camera's intrinsic parameter matrix to project the point cloud into the RGB image coordinate system, calculates the position of each point in the image, and calculates the projection coordinates in the image coordinate system using the camera's intrinsic parameter formula:

[0097]

[0098] Among them, f x and f y are the camera focal length, c x and c y is the principal point coordinate, x, y, z are the coordinates of the 3D point cloud;

[0099] By cross-matching the projected coordinates with the segmentation mask, the point cloud data belonging to the segmentation area is filtered out, and the maximum Euclidean distance between them is calculated and saved:

[0100]

[0101] Among them, x i ,y i , z i and x j ,y j , z j are the coordinates of any two points in the point cloud.

[0102] Through Matplotlib and RectangleSelector tools, users can directly select the target object area on the RGB image. This method is intuitive and easy to operate, which improves the efficiency and accuracy of data annotation. The pre-trained SAM model is used for object segmentation. The model can provide accurate segmentation masks based on the user-selected area, effectively distinguishing the target object from the background, and providing high-quality input for subsequent point cloud data processing. By projecting the point cloud data into the RGB image coordinate system and cross-matching it with the segmentation mask, precise alignment of the image and point cloud data is achieved, improving the accuracy of 3D reconstruction and the accuracy of calculating the actual size of the object. By calculating parameters such as the maximum Euclidean distance of the point cloud in the segmented area, the actual size of the object can be automatically calculated without manual measurement, improving the automation and efficiency of data processing.

[0103] Step 5: Build a 6D pose estimation network model based on the 3D model and dataset;

[0104] The 6D pose estimation network model based on the 3D model and data set includes:

[0105] The RGB image and depth image are aligned and normalized, and the ResNet network is used to extract features from the multi-scale RGB image and depth image respectively to obtain multi-scale image features and depth information. Then, the feature maps of the receptive fields of the RGB image features and depth image features at different scales are obtained by using the dilated convolution with different dilation rates. The position attention mechanism and the convolution block attention mechanism are combined to finally fuse the features.

[0106] The specific calculation formula for the receptive field of the dilated convolution is:

[0107] RF i+1 =RF i +(k`-1)×S i #(8)

[0108]

[0109] Where RF i+1 Represents the receptive field of the current layer, RF i represents the receptive field of the previous layer, k represents the size of the convolution kernel, S i Represents the product of the strides of all previous layers.

[0110] The ResNet network and dilated convolution are used to perform multi-scale feature extraction on RGB images and depth images, which can capture image and depth information of different scales and improve the network's adaptability to complex scenes.

[0111] The position attention mechanism specifically includes:

[0112] First, the feature map A (C*H*W) is passed through three convolutional layers to obtain three feature maps {B, C, D} with a shape of C*H*W. Then the shape is reconstructed to C*N, where C represents the number of channels, H represents the matrix height, W represents the matrix width, and N = H×W represents the number of pixels. The transpose of the matrix C and B is multiplied together, and then the attention feature map S (N*N) is obtained through softmax. The transpose of the matrix D and S is multiplied together, and the shape of the resulting matrix is ​​reconstructed to C×H×W and then multiplied by the scale factor α. Finally, it is added to the input matrix A to obtain the final output matrix E. The calculation formula is as follows:

[0113]

[0114] Where S ji Represents each element of the S matrix, indicating the influence of position i on position j, E j represents each element in E, α is the scale factor, D i is an element of D, A j is an element of A;

[0115] The convolutional block attention mechanism is composed of a channel attention module and a spatial attention module in series. The input feature map is first subjected to average pooling and maximum pooling respectively, and then the channel weights are learned through a shared multi-layer perceptron and a Sigmoid activation function. The results are then subjected to average pooling and maximum pooling respectively, and then a 7×7 dilated convolution and a Sigmoid activation function are used to learn the weights of each point in space. Finally, a weight-optimized feature map is generated, and the view image with the highest correlation is selected as the final reference view based on the weighted feature map.

[0116] The introduction of position attention mechanism and convolutional block attention mechanism can automatically learn and focus on important areas and channels in the image, improve the pertinence and effectiveness of feature extraction, further improve the accuracy of pose estimation, fuse feature maps of different scales, and combine the results of the attention mechanism to generate weight-optimized feature maps, making the network more intelligent and efficient in the feature extraction and pose estimation process.

[0117] Combining the depth image with the preliminary estimated pose, the depth value of each pixel in the depth image is used to update the actual position of the object, and the distance between the object and the camera is calculated. The preliminary estimated pose is iteratively optimized multiple times, and the pose is adjusted to minimize the error to obtain the final 6D pose, including the object's translation vector and rotation matrix. The depth value of each pixel in the depth image is used to update the actual position of the object, making the pose estimation closer to the real scene. The optimized pose result is fed back to the dataset, and the pose and size of the reference object are adjusted to form a closed-loop optimization process.

[0118] Step 6: Estimate pose based on actual size.

[0119] Specifically, pose estimation based on actual size involves loading the actual size of the object obtained through point cloud computing in step 4 to accurately estimate the 6D pose. The estimation results include the object's translation position (x, y, z) and rotation angle (Rx, Ry, Rz). The accuracy of pose estimation is further optimized by combining depth information and size information. The pose and size of the reference object in the dataset are adjusted accordingly. The 6D pose estimation network loads the adjusted dataset for pose estimation and pose refinement.

[0120] Loading the actual size of the object obtained through point cloud computing and accurately estimating the 6D pose can further improve the accuracy and reliability of pose estimation by combining the actual physical size information of the object. Based on the results of pose estimation, the pose and size of the reference objects in the dataset are dynamically adjusted, making the dataset closer to real application scenarios.

[0121] A 6D pose system based on an RGBD camera, used to implement the above-mentioned 6D pose estimation method, comprising:

[0122] Storage module: The computer is used to store the ROS system that controls the robot's motion, the three-dimensional model, the 6D pose annotation dataset, the pre-trained SAM model, and the 6D pose estimation network model;

[0123] RGB camera, the RGB camera takes video of the object to be measured;

[0124] Image processing module, which includes OpenCV tools to decompose video files into single-frame RGB images of fixed size, COLMAP tools to create datasets, and SIFT to perform feature extraction and matching on RGB images;

[0125] Calculation module, which is used to store calculation formulas and perform calculations;

[0126] The interactive module allows users to select the target object in the RGB image and select a rectangular area by mouse interaction, and displays the 6D pose.

[0127] Image analysis module, the image analysis module analyzes the depth value of the depth image to determine the distance between the object and the camera, and adjusts the posture to minimize the error to obtain the final 6D pose;

[0128] The control module is used to realize data transmission and information interaction between modules. The control module includes an information receiving unit and a control unit. The information receiving unit is used to receive image data processed by the image processing module, and the control unit is used to call data in the storage module.

[0129] Example:

[0130] Use the kinectV1 camera to estimate the pose of a cube with a side length of 4 cm

[0131] Camera parameters

[0132] RGB camera parameters:

[0133] Resolution: 640x480

[0134] Camera matrix (intrinsic parameter matrix):

[0135] [532.38691, 0, 300.17259; 0, 532.98285, 257.18138; 0, 0, 1]

[0136] Distortion coefficient:

[0137] [0.145682, -0.265080, 0.000678, 0.003219, 0.000000]

[0138] Projection matrix:

[0139] [542.28912, 0, 301.44972, 0; 0, 544.45929, 257.53302, 0; 0, 0, 1, 0]

[0140] Depth camera parameters:

[0141] Camera matrix: 575.1859510.000000321.543912 0.000000575.026480248.782741 0.0000000.0000001.000000

[0145] Distortion coefficient:

[0146] -0.1426140.370022-0.0035360.0000690.000000

[0147] Projection matrix: 586.1904910.000000321.4226230.000000 0.000000584.348389246.9899160.000000 0.0000000.0000001.0000000.000000

[0151] The maximum point cloud distance of the average object measured at the average distance of 818.6 mm is 6.81 cm, which is actually 6.928 cm. The generated bounding box is used to select the target object, such as Figure 3 shown.

[0152] The above shows and describes the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions merely illustrate the principles of the present invention. Various changes and modifications may be made to the present invention without departing from the spirit and scope of the present invention. Such changes and modifications are intended to fall within the scope of the present invention. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. A 6D pose estimation method based on an RGBD camera, characterized by: include, Step 1: Camera imaging calibration; Step 2: Calibrate the camera parameters based on the calibrated camera imaging; Step 3: 3D model reconstruction and dataset construction: A full-scale video of the target object is captured, and the video file is decomposed into single-frame RGB images of a fixed size using OpenCV. The RGB images are extracted and a dataset is created using the COLMAP tool, followed by SIFT feature extraction and matching. The SFM algorithm is used to generate a sparse point cloud model, and the PatchMatch algorithm is used to generate a depth map, which is then subjected to depth fusion to generate the final dense point cloud model. The generated point cloud model is cropped and annotated, and the coordinate system of the object relative to the camera is constructed, thereby generating a dataset containing 6D pose annotations. Step 4: Segment the target object and calculate its actual size based on the 3D model and dataset; The step 4 includes segmenting the target object and calculating the actual size based on the three-dimensional model and the data set, and the robot operating system ROS synchronizes the received RGB image, depth image and point cloud data through a message filter; Using Matplotlib and the RectangleSelector tool, the user is allowed to interactively select the target object area in the RGB image and select a rectangular area with the mouse. The coordinates of the rectangular area are recorded as the segmentation input. The pre-trained SAM model is used for object segmentation. Based on the rectangular input selected by the user, the SAM model is used to segment the area and obtain the segmentation mask. The camera's intrinsic parameter matrix is ​​used to project the point cloud into the RGB image coordinate system, and the position of each point in the image is calculated. The projection coordinates in the image coordinate system are calculated using the camera's intrinsic parameter formula: Among them, f x and f y are the camera focal length, c x and c y is the principal point coordinate, x, y, z are the coordinates of the 3D point cloud; By cross-matching the projected coordinates with the segmentation mask, the point cloud data belonging to the segmentation area is filtered out, and the maximum Euclidean distance between them is calculated and saved: Among them, x i ,y i ,z i and x j ,y j ,z j are the coordinates of any two points in the point cloud; Step 5: Build a 6D pose estimation network model based on the 3D model and dataset: align and normalize the RGB image and depth image, and use the ResNet network to extract features from the multi-scale RGB image and depth image respectively; use dilated convolution with different dilation rates to obtain feature maps of the receptive fields of different scales of the RGB image features and depth image features, combine the position attention mechanism with the convolution block attention mechanism, and finally fuse the features to obtain a preliminary estimated pose of the object; Combine the depth image with the preliminary estimated pose, use the depth value of each pixel in the depth image to update the actual position of the object, and calculate the distance between the object and the camera. Perform multiple iterations of optimization on the preliminary estimated pose, and adjust the pose to minimize the error to obtain the final 6D pose, including the object's translation vector and rotation matrix. Step 6: Estimate pose based on actual size; Specifically, step 6 performs pose estimation based on actual size, loading the actual size of the object obtained by point cloud computing in step 4 to estimate the 6D pose. The estimation result includes the translation position (x, y, z) and rotation angle (Rx, Ry, Rz) of the object. The accuracy of the pose estimation is further optimized by combining the depth information and size information. The pose and size of the reference object in the dataset are adjusted accordingly. The 6D pose estimation network model loads the adjusted dataset for pose estimation and pose refinement.

2. The 6D pose estimation method based on an RGBD camera according to claim 1, characterized in that: The camera imaging calibration in step 1 includes the following: when light passes through the lens and is projected onto the imaging plane, distortion occurs, which is divided into radial distortion and tangential distortion. Radial distortion is divided into barrel distortion and pincushion distortion, while tangential distortion is caused by the non-parallelism between the lens and the imaging plane during the camera installation process, which is an installation error. The normalized coordinate calculation formula for radial distortion is as follows: x distorted =x(1+k1r 2 ); and distorted =y(1+k1r 2 ) In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, k1 is the distortion coefficient, and r represents the radial distance between point (x, y) and the center point of the image; The normalized coordinate calculation formula for tangential distortion is as follows: x distorted =x+2p1xy+p2(r 2 +2x 2 ); y distorted =y+2p2xy+p1(r 2 +2y 2 ) In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, and p1 and p2 are additional distortion parameters; For any point p on the image, the three distortion parameters k1, p1, and p2 are combined to find the exact coordinates of the point (x, y) in the pixel coordinate system. The specific calculation formula for the normalized coordinates of point p is: x distorted =x(1+k1r 2 )+2p1xy+p2(r 2 +2x 2 ); y distorted =y(1+k1r 2 )+2p2xy+p1(r 2 +2y 2 )。 3. The 6D pose estimation method based on an RGBD camera according to claim 1, wherein: The step 2 calibrates the camera parameters based on the calibrated camera imaging, including first arranging m ArUco calibration plates in a circular shape around the object to be detected, without repeated markings and evenly distributed, and using the robot operating system ROS to take n RGB images and infrared images of the ArUco calibration plates at different positions and angles, and calibrating the data; obtaining the intrinsic parameter matrix and extrinsic parameter matrix of the RGB camera, depth camera, and infrared camera through calibration; The formula of the internal parameter matrix K is as follows: Among them, f x and f y is the focal length of the RGB camera in the horizontal and vertical directions; c x and c y is the pixel coordinate of the principal point, that is, the position of the image center in the pixel coordinate system; The extrinsic matrix [R, T] describes the relative position and rotation between cameras: Among them, [X r ,Y r ,Z r ] T is the three-dimensional point coordinate in the RGB camera coordinate system; [X d ,Y d ,Z d ] T is the three-dimensional point coordinate in the depth camera coordinate system; R is the rotation matrix; [t x ,t y ,t z ] T is the translation vector.

4. The 6D pose estimation method based on an RGBD camera according to claim 1, wherein: In step 3, during the SFM algorithm, COLMAP automatically estimates the camera pose of each frame, that is, by triangulation and multi-view geometry analysis, the camera's extrinsic parameters are calculated using image feature matching, thereby determining the camera's position and orientation in three-dimensional space.

5. The 6D pose estimation method based on an RGBD camera according to claim 1, wherein: In step 5, the specific calculation formula for the receptive field of the dilated convolution is: RF i+1 =RF i +(k`-1)×S i Where RF i+1 Represents the receptive field of the current layer, RF i represents the receptive field of the previous layer, k represents the size of the convolution kernel, S i Represents the product of the stride lengths of all previous layers, π represents the cumulative multiplication, Stride i represents the step size of the i-th layer.

6. The 6D pose estimation method based on an RGBD camera according to claim 1, wherein: In step 5, the position attention mechanism specifically includes: First, the feature map A{C*H*W} is passed through three convolutional layers to obtain three feature maps {B, C, D} with a shape of C*H*W. Then the shape is reconstructed to C*N, where C represents the number of channels, H represents the matrix height, W represents the matrix width, and N=H×W represents the number of pixels. The transpose of the matrix C and B is multiplied together, and then the attention feature map S{N*N} is obtained through softmax. The transpose of the matrix D and S is multiplied together, and the shape of the resulting matrix is ​​reconstructed to C×H×W and then multiplied by the scale factor α. Finally, it is added to the input matrix A to obtain the final output matrix E. The calculation formula is as follows: Where S ji Represents each element of the S matrix, indicating the influence of position i on position j, E j represents each element in E, α is the scale factor, D i represents the eigenvalue of position i in the feature map D, A j represents the eigenvalue of position j in feature map A, B i Represents the feature vector of the i-th position in the feature map B obtained after extraction by the convolution layer, C j Represents the feature vector at the i-th position in the feature map C; The convolutional block attention mechanism is composed of a channel attention module and a spatial attention module connected in series. The input feature map is first subjected to average pooling and maximum pooling respectively, and then the channel weights are learned through a shared multi-layer perceptron and a Sigmoid activation function. The results are then subjected to average pooling and maximum pooling respectively, and then a 7×7 void convolution and a Sigmoid activation function are used to learn the weights of each point in space. Finally, a weight-optimized feature map is generated, and the perspective image with the highest correlation is selected as the final reference perspective based on the weighted feature map.

7. A 6D pose system based on an RGBD camera, for implementing the 6D pose estimation method according to any one of claims 1 to 6, characterized in that: include: A storage module, wherein the computer is used to store the ROS system for controlling the robot's motion, the three-dimensional model, the 6D pose annotated data set, the pre-trained SAM model, and the 6D pose estimation network model; An RGB camera, wherein the RGB camera captures a video of the object to be measured; An image processing module includes an OpenCV tool to decompose video files into single-frame RGB images of fixed size, a COLMAP tool to create a data set, and SIFT to perform feature extraction and matching on the RGB images; A calculation module, the calculation module is used to store calculation formulas and perform calculations; An interactive module, which allows the user to interactively select the target object in the RGB image and frame a rectangular area using the mouse, and displays the 6D pose; An image analysis module analyzes the depth values ​​of the depth image to determine the distance between the object and the camera, and adjusts the posture to minimize the error to obtain the final 6D pose; The control module is used to realize data transmission and information interaction between modules. The control module includes an information receiving unit and a control unit. The information receiving unit is used to receive image data processed by the image processing module, and the control unit is used to call data in the storage module.

Images