6D pose estimation method and system based on RGBD camera

Abstract

The invention discloses a 6D pose estimation method and system based on an RGBD camera, and relates to the technical field of computer vision processing. Calibrating camera parameters based on the calibrated camera imaging; reconstructing a three-dimensional model and constructing a data set; segmenting the target object and calculating the actual size based on the three-dimensional model and the data set; establishing a 6D pose estimation network model based on the three-dimensional model and the data set; and performing pose estimation based on the actual size. The method has the advantages that the accuracy of 6D pose estimation is remarkably improved by combining the RGB image, the depth image and the point cloud data, object region segmentation is performed by using the SAM model, the actual size of the object is visually calculated by combining the depth information and the point cloud data, the precision of size estimation is improved, and the method is suitable for popularization and application. The introduction of depth information enables the pose estimation system to have better adaptability to objects with 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, 6D pose estimation of target objects has become an important prerequisite for achieving object interaction tasks. The 6D pose includes the translation position [x, y, z] and rotation angle [Rx, Ry, Rz] of the target object, which describes the specific position and posture of the target in three-dimensional space relative to the camera coordinate system. Traditional 6D pose estimation methods mainly rely on high-precision target 3D models or specific target recognition algorithms, which are difficult to achieve versatility and real-time performance. In particular, using RGBD cameras in complex scenes to quickly and accurately estimate the 6D pose of objects still faces challenges.

[0003] In recent years, 6D pose estimation methods based on deep learning have made significant breakthroughs, and the research is mainly divided into two categories: instance-level and category-level. Instance-level methods rely on high-precision 3D models of the target, and usually a network is only applicable to specific objects. Although instance-level methods perform well in terms of accuracy, they are difficult to meet the real-time requirements in practical applications due to their lack of versatility and complex pre-processing. Category-level methods attempt to predict the 6D pose of similar objects through a single network model, which has good generalization ability, but still faces problems such as reduced estimation accuracy caused by intra-class shape differences. To address the above problems, a 6D pose estimation method and system based on RGBD cameras are proposed to solve them. Summary of the invention

[0004] In order to solve the above technical problems, a 6D pose estimation method and system based on an RGBD camera is provided, which solves the problem that the current instance-level method relies on a high-precision 3D model of the target, and usually a network is only applicable to a specific object. Although the instance-level method performs well in terms of accuracy, it is difficult to meet the real-time requirements in practical applications due to its lack of versatility and complex pre-processing. The category-level method attempts to predict the 6D pose of similar objects through a single network model, which has good generalization ability, but still faces the problem of reduced estimation accuracy due to shape differences within the category.

[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 the camera parameters based on the calibrated camera imaging;

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

[0009] Step 4: Segmentation of target objects and calculation of actual size based on the 3D model and data set;

[0010] Step 5: Establish a 6D pose estimation network model based on the 3D model and data set;

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

[0012] Preferably, the camera imaging calibration includes that when light passes through the lens and is projected onto the imaging plane, distortion occurs, which is divided into radial distortion and tangential distortion, wherein radial distortion is divided into barrel distortion and pincushion distortion, and tangential distortion is caused by the non-parallelism of 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+k 1 r 2 );

[0015] y distorted =y(1+k 1 r 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, k 1 is the distortion coefficient;

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

[0018] x distorted =x+2p 1 xy+p 2 (r 2 +2x 2 );

[0019] y distorted =y+2p 2 xy+p 1 (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, p 1 、p 2 is an additional distortion parameter;

[0021] For any point p on the image, we can combine the above k 1 、p 1 、p 2 The three distortion parameters 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+k 1 r 2 )+2p 1 xy+p 2 (r 2 +2x 2 );

[0023] y distorted =y(1+k 1 r 2 )+2p 2 xy+p 1 (r 2 +2y 2 )#(3)

[0024] Preferably, the camera parameter calibration based on the calibrated camera imaging includes firstly arranging 13 ArUco calibration plates in a circular shape around the object to be detected, without repeated markings and evenly distributed, 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, the depth camera and the infrared camera are obtained by 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), reflecting 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 external parameter matrix [R, T] describes the relative position and rotation between cameras:

[0029]

[0030] Among them, [X r , Yr , Z r ] T is the three-dimensional point coordinate in the RGB camera coordinate system; [X d , Y d , Z d ] T is the coordinate of the three-dimensional point 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.

[0031] Preferably, the three-dimensional reconstruction and data set construction specifically include: performing all-round video acquisition 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 data set, 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 using image feature matching to calculate the camera's external parameters (rotation matrix and translation vector), thereby determining the camera's position and direction 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 data set containing 6D pose annotations.

[0032] Preferably, the segmentation and actual size calculation of the target object 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 select the area where the target object is located in the RGB image through mouse interaction and select a rectangular area, 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, according to the rectangular box input selected by the user, using the SAM model to segment the area, and 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 screened 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 the three-dimensional model and the data set includes:

[0040] The RGB image and the depth image are aligned and normalized, and the ResNet network is used to extract features from the multi-scale RGB image and the 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 the depth image features of different scales are obtained by using the dilated convolution with different dilation rates. The features are finally fused in combination with the position attention mechanism and the convolution block attention mechanism.

[0041] The specific calculation formula of 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 strides of all previous layers; Π represents cumulative multiplication, Stride irepresents 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 accumulating the step size layer by layer, the size of the area covered by the receptive field of the current layer in 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, and then the attention feature map S (N*N) is obtained through softmax. The transpose of the matrix D and S is multiplied, 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] In the formula, 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. 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 at the i-th position in the feature map B obtained after extraction by the convolution layer, C j represents the feature vector of the ith position in the feature map C; the two feature vectors are similar to each other by the dot product operation to obtain 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 connected in series. The input feature map is first subjected to average pooling and maximum pooling, and then the channel weights are learned by sharing a multi-layer perceptron and a Sigmoid activation function. The results are then subjected to average pooling and maximum pooling, and then a 7×7 dilated convolution and a Sigmoid activation function are used to learn the weights of each point in the space. Finally, a weight-optimized feature map is generated, and the view image with the highest correlation is selected as the final reference view according to the weighted feature map.

[0050] Preferably, the depth image is combined with the preliminary estimated posture (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 posture is then combined with the pixel depth value of the depth image in the sixth step, and is iteratively optimized for 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 posture is iteratively optimized for multiple times, and the posture is adjusted to minimize the error, so as to obtain the final 6D pose, including the translation vector and rotation matrix of the object.

[0051] Preferably, the pose estimation based on the actual size is specifically to load the actual size of the object obtained by point cloud computing in step 4 to accurately estimate the 6D pose, and the estimation result includes the translation position (x, y, z) and rotation angle (Rx, R y , Rz), combining depth information and size information to further optimize the accuracy of pose estimation (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), so as to adjust 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 posture refinement.

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

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

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

[0055] An image processing module, wherein the image processing module includes an OpenCV tool to decompose a video file into a single-frame RGB image of a fixed size, a COLMAP tool to create a data set, and SIFT to extract and match features of the RGB image;

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

[0057] An interactive module, wherein the interactive module is used to allow the user to select the area where the target object is located in the RGB image and select a rectangular area through mouse interaction, and to display 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] A control module, which 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. The present invention significantly improves the accuracy of 6D pose estimation by combining RGB images, depth images and point cloud data. The SegmentAnything Model (SAM) model is used to segment object regions, and combined with depth information and point cloud data, not only the actual size of the object is calculated intuitively, but also the accuracy of size estimation is improved. The introduction of depth information makes the pose estimation system more adaptable to objects of different sizes, shapes and surface features, and still performs well in complex scenes and changing lighting conditions, improving the robustness and accuracy of the system.

[0062] 2. The present invention reduces the reliance on the CAD model of the target object or direct measurement of the real size by shooting videos and using the COLMAP tool to generate sparse point clouds and reference image pose data. This innovation greatly simplifies the process of building a dataset and improves data collection efficiency. Multi-view shooting ensures the integrity and comprehensive coverage of the object, and the generated dataset is more accurate and versatile, suitable for a variety of application scenarios. Compared with existing systems that rely on specific object models, the present invention has more 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 captures object features at different scales in combination with depth information, further improving the accuracy of detection and estimation. The viewpoint selection mechanism can select the optimal viewing angle from multi-angle images, and combine depth information to make pose estimation more accurate. After multiple iterations of optimized pose refinement modules, the estimation results are still stable and accurate when processing complex rotations and tilts of objects. Especially 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; Figure 3 This is a pose estimation graph of an embodiment of the present invention. DETAILED DESCRIPTION

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

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

[0068] Step 1: Camera imaging calibration;

[0069] Camera imaging calibration includes the distortion phenomenon that occurs when light passes through the lens and is projected onto the imaging plane. This distortion 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.

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

[0071] x distorted =x(1+k 1 r 2 );

[0072] y distorted =y(1+k 1 r 2 )#(1)

[0073] In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, k 1 is the distortion coefficient;

[0074] The normalized coordinate calculation formula of tangential distortion is as follows:

[0075] x distorted =x+2p 1 xy+p 2 (r 2 +2x 2 );

[0076] y distorted =y+2p 2 xy+p 1 (r 2 +2y 2 )#(2)

[0077] In the formula, (x distorted ,y distorted ) is the corrected standard coordinate, point (x, y) is the original coordinate that produces distortion, p 1 、p 2 is an additional distortion parameter;

[0078] For any point p on the image, we can combine the above k 1 、p 1 、p 2 The three distortion parameters 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:

[0079] x distorted =x(1+k 1 r 2 )+2p 1 xy+p 2 (r 2 +2x 2 );

[0080] y distorted =y(1+k 1 r 2 )+2p 2 xy+p 1 (r 2 +2y 2 )#(3)

[0081] By correcting distortion, especially radial distortion and tangential distortion, the quality of the image 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 the image distortion caused by lens distortion, thereby improving the accuracy of the feature point position and shape information extracted from the image, and further enhancing the accuracy of 6D pose estimation.

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

[0083] The camera parameter calibration based on the calibrated camera imaging includes: firstly, 13 ArUco calibration plates are arranged in a circle around the object to be detected, without repeated marks and evenly distributed, and n RGB images and infrared images of the ArUco calibration plates are taken at different positions and angles through the ROS system, and the data are calibrated. The intrinsic and extrinsic matrix of the RGB camera, depth camera and infrared camera are obtained through calibration;

[0084] 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:

[0085]

[0086] Among them, f x and f y is the focal length of the RGB camera in the horizontal and vertical directions (unit: pixel), reflecting 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;

[0087] The external parameter matrix [R, T] describes the relative position and rotation between cameras:

[0088]

[0089] 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 coordinate of the three-dimensional point 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.

[0090] 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 can be 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, which improves the versatility and flexibility of the system, reduces manual intervention, and improves calibration efficiency and reliability.

[0091] Step 3: 3D model reconstruction and data set construction;

[0092] The three-dimensional reconstruction and data set construction specifically include: all-round video acquisition of the target object, ensuring 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 extract the RGB image and use the COLMAP tool to create a data set, and then perform SIFT feature extraction and matching, and then run 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 direction 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 coordinate system of the object relative to the camera, thereby generating a data set containing 6D pose annotations.

[0093] Through all-round video acquisition, the target object is fully presented from multiple angles and different distances, avoiding occlusion problems and improving the integrity and accuracy of 3D reconstruction. Mature tools such as OpenCV and COLMAP are used for image processing and 3D reconstruction, which improves processing efficiency and result quality. The generated dense point cloud model can be cropped and labeled to construct the precise coordinate system of the object relative to the camera, providing a high-quality labeled data set for subsequent 6D pose estimation.

[0094] Step 4: Segmentation of target objects and calculation of actual size based on the 3D model and data set;

[0095] The segmentation and actual size calculation of the target object based on the 3D model and data set include: ROS synchronizes the received RGB image, depth image and point cloud data through message filters; using Matplotlib and RectangleSelector tools, let the user select the target object area in the RGB image through mouse interaction and select a rectangular area, the system will record the coordinates of the rectangular area as the input for segmentation; use the pre-trained SAM model for object segmentation, according to the rectangular box input selected by the user, use the SAM model to segment the area and obtain the segmentation mask; use 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:

[0096]

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

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

[0099]

[0100] 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.

[0101] 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 distinguish the target object from the background, and provide 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, the image and point cloud data are accurately aligned, which improves the accuracy of 3D reconstruction and the accuracy of the actual size calculation 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, which improves the automation and efficiency of data processing.

[0102] Step 5: Establish a 6D pose estimation network model based on the 3D model and data set;

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

[0104] The RGB image and the depth image are aligned and normalized, and the ResNet network is used to extract features from the multi-scale RGB image and the 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 the depth image features of different scales are obtained by using the dilated convolution with different dilation rates. The features are finally fused in combination with the position attention mechanism and the convolution block attention mechanism.

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

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

[0107]

[0108] 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.

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

[0110] The position attention mechanism specifically includes:

[0111] 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, and then the attention feature map S (N*N) is obtained through softmax. The transpose of the matrix D and S is multiplied, 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:

[0112]

[0113] In the formula, 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;

[0114] 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, 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, and then a 7×7 dilated convolution and a Sigmoid activation function are used to learn the weights of each point in the space. Finally, a weight-optimized feature map is generated, and the view image with the highest correlation is selected as the final reference view according to the weighted feature map.

[0115] 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 process of feature extraction and pose estimation.

[0116] Combine the depth image with the preliminary estimated posture, 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 to optimize the preliminary estimated posture, and adjust the posture to minimize the error to obtain the final 6D posture, including the translation vector and rotation matrix of the object. Use the depth value of each pixel in the depth image to update the actual position of the object, so that the posture estimation is closer to the real scene. Feed the optimized posture results back to the data set, adjust the posture and size of the reference object, and form a closed-loop optimization process.

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

[0118] Specifically, the pose estimation based on actual size is to load the actual size of the object obtained by point cloud computing in step 4 to accurately estimate the 6D pose. The estimation results include 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 loads the adjusted dataset for pose estimation and posture refinement.

[0119] By loading the actual size of the object obtained through point cloud computing and accurately estimating the 6D pose, the accuracy and reliability of pose estimation can be further improved by combining the actual physical size information of the object. The pose and size of the reference object in the dataset can be dynamically adjusted according to the results of pose estimation, making the dataset closer to the real application scenario.

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

[0121] Storage module, the computer is used to store the ROS system for controlling the robot movement, the three-dimensional model, the 6D pose annotation data set, the pre-trained SAM model and the 6D pose estimation network model;

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

[0123] Image processing module: The image processing module includes OpenCV tools to decompose video files into single-frame RGB images of fixed size, COLMAP tools to create data sets, and SIFT to extract and match features of RGB images;

[0124] A calculation module, which is used to store calculation formulas and perform calculations;

[0125] The interactive module is used to allow the user to select the target object area in the RGB image and select a rectangular area through mouse interaction, and display the 6D pose;

[0126] 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 obtains the final 6D pose by adjusting the pose to minimize the error;

[0127] 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.

[0128] Example:

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

[0130] Camera parameters

[0131] RGB camera parameters:

[0132] Resolution: 640x480

[0133] Camera matrix (intrinsic matrix):

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

[0135] Distortion coefficient:

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

[0137] Projection matrix:

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

[0139] Depth camera parameters:

[0140] Camera matrix: 575.1859510.000000321.543912 0.000000575.026480248.782741 0.0000000.0000001.000000

[0144] Distortion coefficient:

[0145] -0.1426140.370022-0.0035360.0000690.000000

[0146] Projection matrix: 586.1904910.000000321.4226230.000000 0.000000584.348389246.9899160.000000 0.0000000.0000001.0000000.000000

[0150] 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.

[0151] 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 only describe the principles of the present invention. The present invention may be subject to various changes and improvements without departing from the spirit and scope of the present invention. These changes and improvements fall within the scope of the present invention. The scope of protection claimed by the present invention is defined by the attached claims and their equivalents.

Claims

1. A 6D pose estimation method based on an RGBD camera, characterized in that: 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: Perform omnidirectional video capture of the target object and use OpenCV to decompose the video file into single-frame RGB images of fixed size; extract the RGB image and use the COLMAP tool to create a dataset, and then perform SIFT feature extraction and matching; use the SFM algorithm to generate a sparse point cloud model, use the PatchMatch algorithm to generate a depth map, and then perform deep fusion to generate the final dense point cloud model; crop and annotate the generated point cloud model, and construct the coordinate system of the object relative to the camera, thereby generating a dataset containing 6D pose annotations; Step 4: Segmentation of the target object and calculation of its actual size based on the 3D model and data set; Step 5: Establish a 6D pose estimation network model based on the 3D model and the dataset: align and normalize the RGB image and the depth image, use the ResNet network to extract features from the multi-scale RGB image and the depth image respectively; use the hole convolution with different dilation rates to obtain the feature maps of the receptive fields of different scales of the RGB image features and the depth image features, combine the position attention mechanism with the convolution block attention mechanism, and finally fuse the features to obtain the preliminary estimated pose of the object; Step 6: Estimate pose based on actual size.

2. The 6D pose estimation method and system based on RGBD camera according to claim 1, characterized in that: The camera imaging calibration in step 1 includes that 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 of 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 )#(1) 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 of tangential distortion is as follows: x distorted =x+2p1xy+p2(r 2 +2x 2 ); y distorted =y+2p2xy+p1(r 2 +2y 2 )#(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)。 3. The 6D pose estimation method based on an RGBD camera according to claim 1, characterized in that: 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, taking n RGB images and infrared images of the ArUco calibration plates at different positions and angles through the robot operating system ROS, and calibrating the data; obtaining the intrinsic parameter matrix and extrinsic parameter matrix of the RGB camera, the depth camera and the 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 external parameter 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, characterized in that: In the step 3, during the SFM algorithm process, COLMAP automatically estimates the camera pose of each frame of the image, that is, through triangulation and multi-view geometry analysis, the camera's external parameters are calculated using image feature matching, thereby determining the camera's position and direction in three-dimensional space.

5. The 6D pose estimation method based on RGBD camera according to claim 1, characterized in that: The step 4 includes segmenting the target object and calculating the actual size based on the three-dimensional model and the data set, wherein the robot operating system ROS synchronizes the received RGB image, depth image and point cloud data through a message filter; Use Matplotlib and the RectangleSelector tool to allow the user to interactively select the target object in the RGB image and select a rectangular area with the mouse, and record the coordinates of the rectangular area as the input for segmentation; use the pre-trained SAM model to segment the object, and use the SAM model to segment the area according to the rectangular box input selected by the user to obtain the segmentation mask; use 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 using the camera's intrinsic parameter formula: Among them, f x and f y are the camera focal length, c x and c y are the principal point coordinates, 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 screened 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.

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

7. The 6D pose estimation method based on RGBD camera according to claim 1, characterized in that: 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, and then the attention feature map S{N*N} is obtained through softmax. The transpose of the matrix D and S is multiplied, 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: In the formula, 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 at 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, and then the channel weights are learned by sharing a multi-layer perceptron and a Sigmoid activation function. The results are then subjected to average pooling and maximum pooling, and then a 7×7 dilated convolution and a Sigmoid activation function are used to learn the weights of each point in the space. Finally, a weight-optimized feature map is generated, and the view image with the highest correlation is selected as the final reference view according to the weighted feature map.

8. The 6D pose estimation method based on RGBD camera according to claim 6, characterized in that: Combine the depth image with the preliminary estimated posture, 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 to optimize the preliminary estimated posture, and adjust the posture to minimize the error to obtain the final 6D posture, including the object's translation vector and rotation matrix.

9. The 6D pose estimation method based on RGBD camera according to claim 1, characterized in that: Specifically, step 6 performs pose estimation based on actual size, loads the actual size of the object obtained by point cloud computing in step 4 to 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 combines the depth information and size information to further optimize the accuracy of pose estimation, so as to adjust the pose and size of the reference object in the data set, and the 6D pose estimation network model loads the adjusted data set for pose estimation and posture refinement.

10. A 6D pose system based on an RGBD camera, used to implement the 6D pose estimation method according to any one of claims 1 to 9, characterized in that: include: A storage module, wherein the computer is used to store the ROS system for controlling the movement of the robot, the three-dimensional model, the data set of 6D pose annotation, the pre-trained SAM model and the 6D pose estimation network model; An RGB camera, wherein the RGB camera takes a video of the object to be measured; An image processing module, wherein the image processing module includes an OpenCV tool to decompose a video file into a single-frame RGB image of a fixed size, a COLMAP tool to create a data set, and SIFT to extract and match features of the RGB image; A calculation module, the calculation module is used to store calculation formulas and perform calculations; An interactive module, wherein the interactive module is used to allow the user to select the area where the target object is located in the RGB image and select a rectangular area through mouse interaction, and to display the 6D pose; An image analysis module, which analyzes the depth value of the depth image to determine the distance between the object and the camera, and obtains the final 6D pose by adjusting the pose to minimize the error; A control module, which 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