Six-dimensional pose estimation method and device, electronic equipment and storage medium

Abstract

The application relates to the technical field of six-dimensional pose estimation, and discloses a six-dimensional pose estimation method and device, electronic equipment and a storage medium, wherein the method comprises the following steps: collecting left and right eye images through a binocular camera, detecting a target object by using a neural network and outputting key point pixel coordinates and visibility, generating a dense disparity map by using a stereo matching algorithm and restoring three-dimensional coordinates of the key points, selecting an effective key point set with visibility greater than a threshold, and estimating the six-dimensional pose of the target object according to the effective key point set. The application can effectively exclude the interference of occluded key points and obtain accurate depth information under complex illumination, so that the stability and precision of the six-dimensional pose estimation are improved.

Description

Technical Field

[0001] This invention relates to the fields of computer vision and robot perception technology, and in particular to a six-dimensional pose estimation method, device, electronic device, and storage medium. Background Technology

[0002] In the fields of robotic automation and human-computer interaction, accurately acquiring the position and orientation of a target object in three-dimensional space is a prerequisite for achieving tasks such as dexterous grasping, assembly, and handling. Existing technical solutions mainly rely on RGB-D (Red-Green-Blue-Depth) cameras, monocular vision systems, or multi-sensor fusion architectures to acquire scene depth information and calculate object pose.

[0003] In existing technologies, when faced with complex lighting conditions, surface reflections, or partial occlusion, it is often difficult to maintain stable and high-precision six-dimensional pose tracking for a long time. Pose estimation jitter, loss of depth information, or tracking loss may occur, affecting the stability and success rate of robot end effector operation. Summary of the Invention

[0004] Based on this, it is necessary to propose a six-dimensional pose estimation method, device, electronic device and storage medium for the existing six-dimensional pose estimation problem.

[0005] A six-dimensional pose estimation method, the method comprising: The system acquires left and right eye images using a pre-set binocular camera. The system detects target objects in the left or right eye image using a preset first neural network model, and outputs the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable from the current viewpoint. Based on the left and right eye images, a dense disparity map is generated using a preset stereo matching algorithm; Based on the dense parallax map, the depth of each preset key point is recovered, and the three-dimensional coordinates of each preset key point in the binocular camera coordinate system are obtained. Select the preset key points whose visibility is greater than a preset threshold as valid key points to obtain a set of valid key points; Estimate the six-dimensional pose of the target object based on the set of effective key points.

[0006] Further, the step of estimating the six-dimensional pose of the target object based on the set of effective key points includes: Obtain a three-dimensional model of the target object; Establish a three-dimensional coordinate correspondence between the three-dimensional coordinates of each effective key point and the three-dimensional coordinates of the corresponding key points in the three-dimensional model; The minimum point set is randomly sampled from the set of effective key points, and the six-dimensional pose is estimated by the PnP algorithm to obtain multiple candidate six-dimensional poses; Calculate the number of interior points in the set of valid key points that match each of the candidate six-dimensional poses; The candidate six-dimensional pose with the largest number of interior points is selected as the six-dimensional pose of the target object.

[0007] Further, the step of recovering the depth of each preset key point based on the dense disparity map to obtain the three-dimensional coordinates of each preset key point in the binocular camera coordinate system includes: Obtain the focal length and baseline distance of the stereo camera; Calculate the depth value of each preset key point based on the disparity value corresponding to each preset key point in the dense disparity map; Based on the pixel coordinates, depth values, and camera intrinsic parameter matrix of each preset key point, calculate the three-dimensional coordinates of each preset key point in the binocular camera coordinate system.

[0008] Furthermore, after the step of selecting the candidate six-dimensional pose with the most interior points as the six-dimensional pose of the target object, the method further includes: Using the six-dimensional pose as the starting point for iterative optimization, edge features and texture features of the left or right eye image are extracted through a preset second neural network model; The 3D model is projected onto the image plane based on the current pose parameters to generate a projection contour. Calculate the matching error between the projected contour and the edge of the image target; The translation and rotation parameters are iteratively adjusted based on the matching error to gradually make the projected contour coincide with the edge of the image target, and the six-dimensional pose with the highest degree of coincidence is output as the optimized six-dimensional pose.

[0009] Furthermore, after the step of estimating the six-dimensional pose of the target object based on the set of effective key points, the method further includes: The six-dimensional pose of multiple consecutive frames is input into a Kalman filter for time-series filtering to obtain a smoothed estimated pose.

[0010] Furthermore, after the step of estimating the six-dimensional pose of the target object based on the set of effective key points, the method further includes: When multiple target objects are detected, an independent Kalman filter is constructed for each target object. Based on the six-dimensional pose of each target object detected in the current frame, the Hungarian algorithm is used to associate each detection result with the existing trajectory, and each detection result is assigned a corresponding existing trajectory or marked as an unassociated detection. Detection results that are not associated with any existing trajectory are created as new trajectories, and the corresponding Kalman filter is initialized for the new trajectory; If an existing trajectory that has not been associated with any detection result for consecutive preset frames is determined to be a tracking loss, the tracking of the existing trajectory is terminated and its corresponding Kalman filter resource is released.

[0011] Furthermore, before the step of inputting the six-dimensional pose of multiple consecutive frames into a Kalman filter for time-series filtering to obtain a smoothed estimated pose, the method further includes: Obtain the shooting distance between the target object and the binocular camera, and determine whether the shooting distance is greater than a preset distance threshold; When the shooting distance is greater than a preset distance threshold, the process noise covariance matrix of the Kalman filter is increased.

[0012] A six-dimensional pose estimation device, the device comprising: The acquisition module is used to acquire left and right eye images through a preset binocular camera; The detection module is used to detect target objects in the left or right eye image using a preset first neural network model, and output the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable in the current viewpoint; The generation module is used to generate a dense disparity map based on the left and right eye images using a preset stereo matching algorithm. The recovery module is used to recover the depth of each preset key point based on the dense parallax map, and obtain the three-dimensional coordinates of each preset key point in the binocular camera coordinate system. The selection module is used to select preset key points whose visibility is greater than a preset threshold as valid key points, thereby obtaining a set of valid key points. An estimation module is used to estimate the six-dimensional pose of the target object based on the set of valid key points.

[0013] An electronic device includes a memory and a processor, the memory storing a computer program that, when executed by the processor, causes the processor to perform the following steps: The system acquires left and right eye images using a pre-set binocular camera. The system detects target objects in the left or right eye image using a preset first neural network model, and outputs the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable from the current viewpoint. Based on the left and right eye images, a dense disparity map is generated using a preset stereo matching algorithm; Based on the dense parallax map, the depth of each preset key point is recovered, and the three-dimensional coordinates of each preset key point in the binocular camera coordinate system are obtained. Select the preset key points whose visibility is greater than a preset threshold as valid key points to obtain a set of valid key points; Estimate the six-dimensional pose of the target object based on the set of effective key points.

[0014] A computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the following steps: The system acquires left and right eye images using a pre-set binocular camera. The system detects target objects in the left or right eye image using a preset first neural network model, and outputs the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable from the current viewpoint. Based on the left and right eye images, a dense disparity map is generated using a preset stereo matching algorithm; Based on the dense parallax map, the depth of each preset key point is recovered, and the three-dimensional coordinates of each preset key point in the binocular camera coordinate system are obtained. Select the preset key points whose visibility is greater than a preset threshold as valid key points to obtain a set of valid key points; Estimate the six-dimensional pose of the target object based on the set of effective key points.

[0015] The beneficial effects of this invention are as follows: By acquiring left and right eye images through a pre-set binocular camera, and utilizing a first neural network model to detect the key point pixel coordinates and visibility of the target object, the three-dimensional coordinates of the key points are recovered using a dense disparity map generated by a stereo matching algorithm. This allows for the selection of a set of valid key points with visibility greater than a preset threshold to estimate the six-dimensional pose of the target object. By introducing a key point visibility quantification mechanism and selecting only high-visibility key points for pose calculation, interference from occluded or low-confidence key points is effectively eliminated, thus enabling reliable initial pose estimation even in partially occluded scenarios. Simultaneously, the dense depth information recovered based on binocular stereo matching replaces the measurement method of traditional active depth sensors, allowing the system to acquire accurate three-dimensional geometric constraints even in complex lighting environments such as strong light and reflections, avoiding pose drift caused by depth data gaps or noise. This effectively solves the problems of unstable and low-accuracy six-dimensional pose estimation in non-ideal environments and under occlusion conditions in existing technologies, improving the robustness and adaptability of the robot's visual perception system in dynamic and complex scenes, and providing high-quality spatial pose data support for subsequent precise grasping and stable tracking by robotic arms. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] in: Figure 1 This is an application environment diagram of a six-dimensional pose estimation method in one embodiment; Figure 2 Here is a flowchart of a six-dimensional pose estimation method in one embodiment; Figure 3 This is a structural block diagram of a six-dimensional pose estimation device in one embodiment; Figure 4 This is a structural block diagram of an electronic device in one embodiment. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] Figure 1This is a diagram illustrating the application environment of a six-dimensional pose estimation method in one embodiment. (Refer to...) Figure 1 This six-dimensional pose estimation method is applied to a six-dimensional pose estimation system. The six-dimensional pose estimation system includes a terminal 110 and a server 120. The terminal 110 and server 120 are connected via a network. The terminal 110 can be a desktop terminal or a mobile terminal; the mobile terminal can be at least one of a mobile phone, tablet computer, or laptop computer. The server 120 can be a standalone server or a server cluster consisting of multiple servers.

[0020] like Figure 2 As shown, in one embodiment, a six-dimensional pose estimation method is provided. This method can be applied to both terminals and servers; this embodiment uses terminal application as an example. The six-dimensional pose estimation method specifically includes the following steps: S1: Acquire left and right eye images using a preset binocular camera; S2: Detect a target object in the left or right eye image using a preset first neural network model, and output the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable in the current viewpoint; S3: Generate a dense disparity map based on the left and right eye images using a preset stereo matching algorithm; S4: Based on the dense parallax map, recover the depth of each preset key point to obtain the three-dimensional coordinates of each preset key point in the binocular camera coordinate system; S5: Select the preset key points whose visibility is greater than the preset threshold as valid key points to obtain a set of valid key points; S6: Estimate the six-dimensional pose of the target object based on the set of valid key points.

[0021] As described in step S1 above, left and right eye images are acquired using a preset binocular camera. The binocular camera can refer to a calibrated and epipolar-corrected stereo imaging device, comprising two imaging units: a left eye camera and a right eye camera. The left and right eye images are scene image pairs acquired simultaneously by the binocular camera. During acquisition, the binocular camera first performs distortion restoration processing on the original image and then performs epipolar correction, ensuring that corresponding points in the left and right eye views lie on the same horizontal scan line, thus simplifying subsequent stereo matching calculations. Replacing the traditional RGB-D (Red-Green-Blue-Depth) sensor with a binocular system effectively avoids interference from active light sources in strong light or reflective environments, improving the system's adaptability to various complex indoor and outdoor lighting conditions.

[0022] As described in step S2 above, a preset first neural network model detects the target object in the left or right eye image, outputting the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; whereby visibility is used to characterize the degree to which the key point is observable from the current viewpoint. The first neural network model can be a pre-trained deep convolutional neural network, such as a variant architecture of Mask R-CNN (Mask Region-based Convolutional Neural Network) or YOLO (You Only LookOnce) series, used to perform object detection, instance segmentation, and key point localization tasks. Preset key points can be feature points with geometric characteristics defined on the surface of the target object; their number can be dynamically adjusted according to the complexity of the object, for example, 64 for regular tableware and 128 for complex mechanical parts. Pixel coordinates can be the two-dimensional position (u,v) of the key point on the image plane. Visibility is a continuous value ranging from 0 to 1, used to quantify the probability that the key point is not occluded and is clearly discernible from the current shooting viewpoint. Visibility is obtained through direct regression from the output branch of the first neural network model. During the training phase, the model uses sample data labeled with occlusion states for supervised learning, thereby learning to determine whether keypoints are occluded by the object itself or other obstacles. Visibility serves as a basis for subsequent selection of reliable data. By introducing a visibility mechanism, the confidence level of each keypoint can be quantified, effectively eliminating the interference of severely occluded or blurred keypoints on pose estimation. For example, when the target object is a water glass with the handle facing away from the camera, the model predicts that the visibility of keypoints in the handle area may be only 0.1, while the visibility of keypoints at the edge of the glass rim can reach 0.95. Sample data can be obtained by rendering the 3D model from different viewpoints and simulating occlusion to generate training data, or by using weakly supervised learning to learn visibility prediction from partially labeled data.

[0023] The backbone of the first neural network model uses ResNet-50 as the feature extraction backbone. After the input image is processed by the backbone network to extract multi-scale feature maps, they are fed into the keypoint coordinate regression branch and the visibility prediction branch, respectively. Each target object has 128 preset 3D keypoints. The keypoints are defined as follows: 128 3D points are uniformly sampled on the surface of the object's CAD model using the farthest pointsampling (FPS) algorithm, ensuring that the keypoints cover the overall geometry of the object. The visibility prediction branch and the keypoint coordinate regression branch share the backbone network features, and two independent prediction heads are derived from the feature map output by the backbone network: Coordinate regression head: For each preset key point, regress its 2D coordinates (u, v) in the image pixel coordinate system.

[0024] Visibility prediction head: For each preset keypoint, outputs a scalar value, which is mapped to the [0,1] interval using a Sigmoid activation function, and serves as the visibility of that keypoint. The closer the visibility value is to 1, the more observable the keypoint is; the closer it is to 0, the more obscured or unobservable it is.

[0025] The two prediction heads share the features of the backbone network, which can reduce computational overhead and enable visibility prediction to benefit from the semantic features of objects learned in the coordinate regression task.

[0026] Since existing 6D object pose datasets typically do not include visibility annotations at the keypoint level, this invention employs an automatic annotation method based on CAD model rendering to generate visibility labels: Obtain labeled poses: Obtain the 6D pose labels (translation vectors and rotation matrices) of the objects corresponding to each image from the training dataset.

[0027] Rendering visibility: Based on the labeled pose, the CAD model of the object is projected onto the image plane. The z-buffer technology of graphics rendering is used to determine whether each preset 3D key point is visible. If the key point is located in front of the object and is not occluded by the object itself or other objects, it is marked as visible (label=1); otherwise, it is marked as invisible (label=0).

[0028] Generate binary labels: Generate a binary visibility label v_gt∈{0,1} for each keypoint.

[0029] The total loss function of a neural network consists of two parts: 1. Keypoint coordinate regression loss The difference between the 2D keypoint coordinates predicted by the supervisory network and the true projected coordinates is analyzed using either Smooth L1 Loss or Mean Squared Error Loss. L_coord=SmoothL1(p_pred, p_gt) Where p_pred represents the pixel coordinates of the keypoints predicted by the network, and p_gt represents the actual pixel coordinates obtained by projecting the 3D keypoints onto the image plane based on the labeled pose.

[0030] 2. Visibility Prediction Loss The output of the visibility prediction branch is supervised using binary cross-entropy loss: L_vis=BCE(v_pred,v_gt) Where v_pred is the visibility predicted by the network (the probability value after Sigmoid mapping), and v_gt is the binary visibility label generated based on CAD rendering.

[0031] Total loss: L_total L_coord+λ×L_vis Where λ is the hyperparameter for balancing the two loss terms, which can be set to λ = 0.1-1.0.

[0032] During the training phase, the 6D pose annotations of the target object are first obtained from the training dataset. Based on the annotated poses, 128 preset 3D keypoints on the object's CAD model are projected onto the image plane to obtain the true pixel coordinates p_gt for each keypoint. Simultaneously, the z-buffer technique of graphics rendering is used to determine the visibility of each keypoint: if the keypoint is located in front of the object and is not occluded, the visibility label v_gt=1; otherwise, v_gt=0.

[0033] The network takes the left eye image after epipolar correction as input. After feature extraction by the ResNet-50 backbone network, it outputs the predicted coordinates p_pred and predicted visibility v_pred of 128 keypoints through the coordinate regression head and visibility prediction head, respectively. The total loss function is L_total=SmoothL1(p_pred,p_gt)+λ×BCE(v_pred,v_gt), where λ is set to 0.5. The network is trained end-to-end using the Adam optimizer with an initial learning rate of 1e-4 and a batch size of 8.

[0034] As described in step S3 above, a dense disparity map is generated based on the left and right eye images using a preset stereo matching algorithm. The stereo matching algorithm can refer to a method for calculating the disparity values ​​of corresponding pixels in the left and right eye images. In this embodiment, a deep learning-based stereo matching network, such as the IGEV (Iterative Geometry Encoding Volume) architecture, is preferred. IGEV-Stereo constructs a Combined Geometry Encoding Volume (CGEV), which simultaneously encodes the scene's geometric information, contextual information, and local matching details. The disparity map is updated through iterative indexing. Introducing the Geometry Encoding Volume (GEV) effectively solves the aforementioned problems. GEV encodes richer scene geometry and contextual information by performing 3D regularized aggregation on the cost volume. However, GEV may exhibit over-smoothing at boundaries and small details due to the 3D regularization network. Therefore, IGEV combines GEV with full pair correlation to form the Combined Geometry Encoding Volume (CGEV), which encodes both non-local geometry and contextual information while preserving local matching details. In this embodiment, the disparity map prediction module adopts an Iterative Geometry Encoding Volume (IGEV-Stereo) network architecture. First, it constructs a grouped correlation volume, grouping feature channels and calculating the correlation between groups. Then, it aggregates the grouped correlation volumes through a lightweight 3D regularization network to obtain the Geometry Encoding Volume (GEV), which encodes the geometric and contextual information of the scene. Finally, it combines the GEV with the all-pair correlation to form the Combined Geometry Encoding Volume (CGEV). The CGEV simultaneously encodes non-local geometric contextual information and local matching details. The GEV is used to regress the initial disparity map using Soft Arg-min, providing a precise starting point for ConvGRU iterations. In each iteration, the geometric features corresponding to the current disparity are indexed from the CGEV through linear interpolation and input into the ConvGRU updater for disparity map optimization. In this embodiment, the number of hidden channels in ConvGRU is set to 128, and the number of iterations is 3-8. A spatial upsampling module upsamples the low-resolution disparity map to the original image resolution, outputting a dense disparity map. A dense disparity map is a single-channel image with the same resolution as the input image, where the gray value of each pixel represents the horizontal displacement (i.e., disparity value) of that point in the left and right images. Let ), where i represents the i-th preset keypoint. The dense disparity map is calculated by inputting the corrected left and right eye images into a stereo matching network, which then constructs a cost volume and iteratively optimizes geometric encoding. This map provides atomic-level depth cues in the scene, transforming 2D image information into disparity data containing 3D geometric information. Specifically, the stereo matching algorithm searches for the optimal matching point under epipolar constraints, generating a continuous and smooth disparity distribution even in areas with weak texture or varying lighting, thanks to deep learning feature extraction capabilities. The resulting dense disparity map provides a high-resolution depth source for subsequent accurate keypoint recovery, avoiding the depth loss problem associated with traditional sparse matching.

[0035] As described in step S4 above, the depth of each preset keypoint is recovered based on the dense disparity map, thus obtaining the three-dimensional coordinates of each preset keypoint in the stereo camera coordinate system. Here, the recovered depth can refer to calculating the distance from the spatial point to the camera's optical center using the principle of triangulation and the disparity value. The three-dimensional coordinates can refer to the spatial position (X, Y, Z) of the keypoint in the stereo camera coordinate system. The depth value is obtained through the formula... The calculation yielded, where denoted as , where is the camera focal length after epipolar correction, and b is the baseline distance of the binocular camera. This represents the disparity value corresponding to the keypoint in the dense disparity map. The 3D coordinates are obtained using the formula... The calculated value is given by K, where K is the corrected camera intrinsic parameter matrix. The above outputs keypoint pixel coordinates. Keypoints in the 2D image plane are mapped to 3D physical space, constructing a sparse 3D point cloud representation of the object. By combining a high-precision dense disparity map and accurate camera intrinsics, 2D keypoints can be upscaled to 3D space, providing accurate 3D-3D correspondences for subsequent PnP (Perspective-n-Point) pose calculations. For weakly textured areas, local texture analysis or smoothing constraints can be introduced to improve the reliability of the disparity map.

[0036] As described in step S5 above, preset keypoints with visibility greater than a preset threshold are selected as valid keypoints, resulting in a set of valid keypoints. The preset threshold is an empirical value set based on occlusion tolerance in actual application scenarios, typically ranging from 0.5 to 0.8. Valid keypoints refer to those keypoints with visibility scores higher than the preset threshold, considered clearly visible in the current frame, and with reliable positions. The set of valid keypoints is a subset of all valid keypoints that meet the visibility criteria. Valid keypoints are obtained by comparing the visibility value of each keypoint output above with the preset threshold, retaining only those with high visibility. The key point, This represents a preset threshold. Initially detected keypoints undergo quality filtering to remove outliers caused by occlusion, blurring, or false detections. By setting a visibility threshold, the system can adaptively exclude unreliable observations, preventing erroneous keypoint coordinates from participating in pose estimation and introducing significant errors. For example, with a threshold of 0.6, if a keypoint's visibility prediction is 0.4 due to occlusion by a finger, that point will be removed and not included in subsequent calculations; points with a visibility of 0.85 will be retained. This filtering mechanism effectively purifies the input data, ensuring high confidence in the data sources used for pose estimation, thereby improving the accuracy of the final pose estimation.

[0037] As described in step S6 above, the six-dimensional pose of the target object is estimated based on the set of valid key points. The six-dimensional pose refers to parameters describing the position and orientation of the target object in three-dimensional space, typically including a three-dimensional translation vector. and three-dimensional rotation parameters (such as Euler angles) (or quaternions). Six-dimensional pose estimation can refer to establishing a matching relationship between the three-dimensional coordinates of the effective keypoint set and the corresponding keypoint coordinates in the standard three-dimensional model of the target object, and obtaining the object's pose parameters by solving the geometric transformation equations. This is based on the three-dimensional coordinates of each point in the effective keypoint set. Its known three-dimensional coordinates in the object's CAD model A 3D-3D correspondence is established, and then the RANSAC (Random Sample Consensus) algorithm combined with the PnP (Perspective-n-Point) algorithm is used for solving the problem. Specifically, firstly, the minimum point set is randomly sampled from the effective keypoint set, and the candidate pose is estimated using the PnP algorithm. Then, the number of interior points is calculated, and finally, the candidate pose with the largest number of interior points is selected as the six-dimensional pose of the target object. This step aims to output the final object spatial state, providing accurate spatial guidance for the robot's grasping planning. By aligning high-confidence effective keypoints with an accurate 3D model, a stable and accurate six-dimensional pose can still be calculated even under partial occlusion or noise conditions, effectively solving the instability problem of traditional monocular or RGB-D schemes in complex environments.

[0038] In one embodiment, step S6, estimating the six-dimensional pose of the target object based on the set of valid key points, includes: S601: Obtain the three-dimensional model of the target object; S602: Establish a three-dimensional coordinate correspondence between the three-dimensional coordinates of each of the effective key points and the three-dimensional coordinates of the corresponding key points in the three-dimensional model; S603: Randomly sample the minimum point set from the set of effective key points, estimate the six-dimensional pose using the PnP algorithm, and obtain multiple candidate six-dimensional poses; S604: Calculate the number of interior points in the set of valid key points that match each of the candidate six-dimensional poses; S605: Select the candidate six-dimensional pose with the most interior points as the six-dimensional pose of the target object.

[0039] As described in step S601 above, a 3D model of the target object is obtained. This 3D model can be a pre-constructed digital model containing the object's geometry and key point definitions, typically stored as a CAD (Computer-Aided Design) file, point cloud data, or a mesh. This 3D model predefines model key points that correspond one-to-one with the preset key points output by the first neural network model. Each model key point has standard 3D coordinates in the object's own coordinate system. This 3D model originates from the offline modeling stage before the robot's operation task or is loaded from a standard database, serving as a reference for pose estimation. Its function is to provide spatial structural information of the object in an ideal state, so that the observed key point coordinates can be mapped to the object's coordinate system. For example, when the target object is an industrial part, its 3D model can be a high-precision STL mesh file, marking 128 feature points, such as hole centers, corner vertices, etc., whose 3D coordinates... These are known, fixed values ​​in the model file. By introducing this 3D model, the discrete keypoints recovered in image space can be associated with the overall geometry of the object, providing the necessary geometric constraints for subsequent calculation of the object's rotation and translation parameters in the camera coordinate system.

[0040] As described in step S602 above, a 3D coordinate correspondence is established between the 3D coordinates of each valid keypoint and the 3D coordinates of the corresponding keypoint in the 3D model. Establishing this correspondence can mean pairing each point in the obtained set of valid keypoints with a keypoint in the 3D model that has the same semantic label. The 3D coordinates of the valid keypoints are the coordinates in the binocular camera coordinate system obtained by recovering the dense disparity map from the above embodiment. In a 3D model, the 3D coordinates of the corresponding key points are coordinates in the object coordinate system. The establishment of this correspondence relies on semantic consistency during the keypoint detection stage; that is, the top-left keypoint detected in the image must match the top-left keypoint in the model. Specifically, the system iterates through the set of valid keypoints and uses the keypoint's index ID or category label to extract the corresponding coordinate data from the 3D model, forming a set of 3D-3D point pairs. For example, if the set of valid key points includes key point number 5 (representing the end of the object's handle), its coordinates in the camera frame are... Then the coordinates of the key points of model number 5 can be read directly from the 3D model. The two are then established as a correspondence. This strict semantic correspondence ensures the geometric logic correctness of subsequent pose calculations and avoids pose divergence caused by mismatched points.

[0041] As described in step S603 above, a minimum point set is randomly sampled from the effective keypoint set, and the six-dimensional pose is estimated using the Perspective-n-Point (PnP) algorithm to obtain multiple candidate six-dimensional poses. The randomly sampled minimum point set refers to the subset of point pairs randomly selected from the established 3D coordinate correspondence that satisfies the minimum number of point pairs required for the PnP algorithm to solve the problem. For a typical PnP problem, the minimum point set usually contains four pairs of non-coplanar 3D-3D corresponding points. The PnP algorithm is used to calculate the rotation matrix of the object relative to the camera based on these pairs of 3D points and their projection relationships on the image plane (or camera coordinate system). Translation vector This refers to the six-dimensional pose. Since a small number of false positives or noise points may remain in the effective keypoint set, the pose calculated from a single sampling may be biased. Therefore, it is necessary to repeat the random sampling process multiple times, generating a hypothetical six-dimensional pose each time, thus obtaining multiple candidate six-dimensional poses. For example, the system can perform 100 sampling cycles, randomly selecting 4 pairs of keypoints each time, and using EPnP (Efficient Perspective-n-Point) or DLT (Direct Linear Transform) algorithms to quickly calculate a candidate pose. This process constitutes the hypothesis generation stage of the RANSAC (Random Sample Consensus) framework, aiming to cover the possible correct pose space through diversified sampling, while diluting the impact of outliers on a single solution result.

[0042] As described in step S604 above, the number of inliers in the effective keypoint set that match each candidate six-dimensional pose is calculated. The number of inliers refers to the number of points for each candidate six-dimensional pose where, after projecting all keypoints from the 3D model onto the camera coordinate system and comparing them with the observed effective keypoints, the projection error is less than a preset tolerance threshold. Inliers are keypoints whose reprojection error with the current candidate pose model is less than a preset error threshold during the estimation of the initial six-dimensional pose using RANSAC and PnP algorithms. Inliers represent correctly matched points in the 3D-3D or 3D-2D correspondence that are consistent with the current estimated pose model. They are used to evaluate the quality of the current candidate pose model and ultimately participate in solving for the optimal pose. Specifically, using the current candidate pose... Key points of the model Transform to camera coordinate system to obtain predicted coordinates Then calculate the predicted coordinates and the observed coordinates. The Euclidean distance between them, where, The rotation matrix is ​​estimated by the k-th random sampling. The translation vector is estimated by the k-th random sampling. This represents the i-th key point in the model coordinate system. This represents the i-th keypoint in the camera coordinate system. If the distance is less than a set threshold (e.g., 5mm or pixel reprojection error less than 2 pixels), the point is determined to be an interior point; otherwise, it is an exterior point. After traversing all valid keypoints, the total number of interior points accumulated is the score of the candidate pose. During the RANSAC iteration process, after each random sampling of the minimum point set to estimate the candidate pose, all valid keypoints are traversed to calculate the reprojection error and count the number of interior points. The candidate pose with the most interior points and its corresponding interior point set are recorded. After the iteration ends, the candidate pose with the most interior points is output as the initial six-dimensional pose to the fine matching module, and its corresponding interior point set can be used for subsequent pose optimization. In this scheme, the reprojection error threshold ranges from 1 to 5 pixels, preferably 2 pixels; the maximum number of iterations ranges from 1000 to 10000, preferably 5000. For example, for the i-th keypoint... If the reprojection error of 115 out of 128 valid keypoints is within the allowable range for a candidate pose, then the number of its inliers is 115. This step effectively identifies and eliminates erroneous pose assumptions generated solely by noise points sampled by chance by quantifying the consistency between each assumed pose and the overall observation data.

[0043] As described in step S605 above, the candidate six-dimensional pose with the largest number of interior points is selected as the six-dimensional pose of the target object.

[0044] The selection of the candidate six-dimensional pose with the largest number of inliers can be achieved by sorting all generated candidate poses after the inlier count is calculated, and directly selecting the pose with the highest score (i.e., the largest number of inliers) as the final initial six-dimensional pose estimation result. This pose represents the optimal rigid body transformation parameters that can explain the spatial distribution of the most effective keypoints under the current observation data. This selection strategy is based on the maximum consensus principle, which holds that a correct pose should satisfy geometric constraints for as many observation points as possible. For example, if among 100 candidate poses, the 42nd candidate pose has 120 inliers, while the number of inliers for other candidate poses is less than 80, then the 42nd candidate pose is determined to be the six-dimensional pose of the target object. The pose determined in this way not only utilizes the reliability of high-visibility keypoints, but also further eliminates potential mismatch interference through global consistency verification, significantly improving the robustness and accuracy of the initial pose estimation, and providing a high-quality starting point for subsequent fine-match optimization.

[0045] In one embodiment, step S4, which involves recovering the depth of each preset key point based on the dense disparity map to obtain the three-dimensional coordinates of each preset key point in the binocular camera coordinate system, includes: S401: Obtain the focal length and baseline distance of the stereo camera; S402: Calculate the depth value of each preset key point based on the disparity value corresponding to each preset key point in the dense disparity map; S403: Calculate the three-dimensional coordinates of each preset key point in the binocular camera coordinate system based on the pixel coordinates, depth values, and camera intrinsic parameter matrix of each preset key point.

[0046] As described in step S401 above, the focal length and baseline distance of the binocular cameras are obtained. The focal length refers to the equivalent focal length of the left and right cameras on the imaging plane after epipolar correction, usually expressed in pixels. It reflects the scaling ratio of the camera lens projecting a three-dimensional point onto a two-dimensional image plane. The baseline distance refers to the straight-line distance between the optical centers of the left and right cameras, a core geometric parameter for depth calculation in binocular stereo vision systems. These parameters are typically obtained through high-precision calibration using a standard checkerboard calibration board before the camera leaves the factory or during the initial system deployment, and are stored as fixed constants in the system's configuration file for later use. For example, for a binocular camera module used for humanoid robot grasping tasks, its calibrated focal length fx might be 800 pixels, and the baseline distance b might be 0.12 meters. Here, the focal length is the equivalent focal length of the left and right cameras (in pixels).

[0047] As described in step S402 above, the depth value of each preset keypoint is calculated based on the disparity value corresponding to each preset keypoint in the dense disparity map. The disparity value can refer to the difference in horizontal coordinates of the imaging position of the same spatial point in the left and right eye images, and this value is inversely proportional to the distance from the spatial point to the camera. The depth value can refer to the physical distance of the preset keypoint from the camera's optical center along the camera's optical axis. Specifically, the system first extracts the disparity values ​​corresponding to the pixel coordinates (u, v) of each preset keypoint determined above from the dense disparity map of the above embodiment. Subsequently, the triangulation formula of binocular imaging was used. Calculate the depth of each key point During this process, if the disparity value corresponding to a keypoint is 0 or invalid (e.g., it is in an occluded area), the depth calculation for that point will be marked as failed or filtered out in subsequent steps. For example, when the disparity value of a keypoint... 40 pixels, focal length The calculated depth is as follows: 800 pixels, baseline b = 0.12 meters. The distance is (800 × 0.12) / 40 = 2.4 meters. This depth recovery mechanism based on geometric optics can directly convert parallax information in a two-dimensional image into physically meaningful three-dimensional distance information, effectively overcoming the deficiency of monocular vision in lacking depth constraints.

[0048] As described in step S403 above, the three-dimensional coordinates of each preset keypoint in the binocular camera coordinate system are calculated based on the pixel coordinates, depth values, and camera intrinsic parameter matrix of each preset keypoint. Here, the pixel coordinates are the two-dimensional positions (u_i, v_i) of the preset keypoints on the left-eye image plane; the camera intrinsic parameter matrix K is a 3×3 matrix describing the internal geometry of the camera, containing parameters such as focal length and principal point coordinates, used to establish a linear transformation relationship between the pixel coordinate system and the camera coordinate system; the three-dimensional coordinates can refer to the spatial position of the keypoint in the binocular camera coordinate system. Specifically, the system utilizes the back-projection principle of a pinhole camera model to construct equations. First, the pixel coordinates are converted into normalized planar coordinates by multiplying them by the inverse of the camera intrinsic matrix to obtain a normalized vector. Then, this vector is multiplied by the depth value calculated above, thereby scaling to obtain the final three-dimensional spatial coordinates.

[0049] In one embodiment, after step S605 of selecting the candidate six-dimensional pose with the most interior points as the six-dimensional pose of the target object, the method further includes: S6061: Using the six-dimensional pose as the starting point for iterative optimization, the edge features and texture features of the left or right eye image are extracted through a preset second neural network model; S6062: Project the 3D model onto the image plane according to the current pose parameters to generate a projection contour; S6063: Calculate the matching error between the projected contour and the edge of the image target; S6064: Iteratively adjust the translation and rotation parameters according to the matching error, so that the projected contour gradually coincides with the edge of the image target, and output the six-dimensional pose with the highest degree of coincidence as the optimized six-dimensional pose.

[0050] As described in step S6061 above, the six-dimensional pose is used as the starting point for iterative optimization. Edge and texture features of the left or right eye image are extracted using a pre-defined second neural network model. The six-dimensional pose can refer to the initial pose estimation result obtained after the PnP algorithm and interior point filtering, which includes the rotation matrix and translation vector of the target object relative to the binocular camera coordinate system. This initial pose serves as the initialization parameter for the iterative optimization process, providing a baseline state for subsequent fine alignment. The second neural network model is a deep learning network specifically designed for extracting high-frequency detail features from images. Its structure can be built based on a convolutional neural network, used to capture the contour boundaries and surface texture information of objects from monocular images. Specifically, the model receives either the left or right eye image as input and outputs a feature map containing edge response maps and texture descriptors. For example, when the target object is an industrial part with complex geometry, the second neural network model can suppress background noise, accurately extract the outer contour edges and surface scratches of the part, and maintain feature stability even under uneven lighting conditions. By extracting these highly discriminative edge and texture features, a reliable metric can be provided for subsequently calculating the matching error between the projected contour and the real image, thus overcoming the problem that traditional methods cannot effectively converge in weakly textured regions. To meet the real-time requirements of humanoid robot embedded platforms, this solution can implement the following lightweight design and inference acceleration for the IGEV stereo matching network: (i) Lightweight Feature Extraction Network: MobileNet V2 is used as the backbone network for feature extraction. Depthwise separable convolutions are used instead of standard convolutions to extract multi-scale features at 1 / 4, 1 / 8, and 1 / 16 resolutions. Cost volumes are constructed on low-resolution feature maps to reduce computational cost.

[0051] (ii) Lightweight Cost Volume: A lightweight 3D U-Net is used to regularize the cost volume, resulting in a geometrical encoding volume (GEV). The GEV is combined with All-pairs Correlation to form a combined geometrical encoding volume (CGEV), which encodes non-local geometric context information while avoiding the high computational cost of simple 3D cost aggregation. The cost volume is constructed at 1 / 4 resolution, and the full-resolution disparity map is recovered through upsampling.

[0052] (III) Iterative Updater Optimization: The ConvGRU-based updater uses Soft Arg-min to initialize the disparity map, and converges in only 3-8 iterations. The number of hidden channels in ConvGRU is set to 128, achieving a balance between accuracy and efficiency. An early termination strategy is adopted during inference, terminating the process when the change in the disparity map is less than a threshold after two consecutive iterations.

[0053] (iv) Model compression: During the deployment phase, structured pruning (removing redundant channels in units of filters), FP16 / INT8 quantization, and knowledge distillation (using the large model as the teacher and the lightweight model as the student) are performed sequentially on the trained full-precision model to improve the inference speed by 3-5 times while keeping the accuracy loss controllable.

[0054] (v) Inference engine optimization: Using inference optimization frameworks such as TensorRT or TFLite, the model is optimized for graphs, operators are fused and low-precision inference is accelerated, and the model is deployed on the embedded GPU or NPU carried by the robot.

[0055] As described in step S6062 above, the 3D model is projected onto the image plane based on the current pose parameters to generate a projected contour. The current pose parameters can refer to the dynamically updated six-dimensional pose data during the iteration process, with the initial six-dimensional pose being the initial pose. The 3D model can refer to a computer-aided design (CAD) model of the target object or a pre-constructed high-precision point cloud model, which contains the geometric vertex information and normal vector information of the object's surface. The process of generating the projected contour involves transforming the vertex coordinates in the 3D model to the camera coordinate system based on the current rotation matrix and translation vector, and then mapping them to the 2D image plane through the camera intrinsic parameter matrix. The virtual 3D model mesh is projected as a closed curve or polygonal contour on the image. For example, if the current pose parameters indicate that the object has rotated 15 degrees around the Z-axis, the projected contour will also undergo corresponding rotational deformation on the image plane, accurately reflecting the theoretical appearance boundary of the object from the current viewpoint. The purpose of this step is to transform abstract 3D geometric information into a 2D form that can be directly compared with image observation data, establishing a reference standard for calculating matching errors.

[0056] As described in step S6063 above, the matching error between the projected contour and the image target edge is calculated. The image target edge can refer to the actual object edge features extracted from the real-world image using the second neural network model. The matching error is a numerical indicator that quantifies the degree of overlap between the projected contour and the real edge, used to evaluate the accuracy of the current pose parameters. It can be calculated using the sum of squared distances from points to curves, the reciprocal of the intersection-over-union (IoU) ratio of the overlapping contour regions, or a gradient-based correlation measure. Specifically, the system iterates through the sampling points on the projected contour, calculates the Euclidean distance from each sampling point to its nearest neighbor image target edge, and sums or averages all distance values ​​to obtain the total error value. For example, when the projected contour completely covers the real object edge in the image, the matching error approaches zero; conversely, if there is a significant offset between the two, the error value will increase. This error value not only reflects the magnitude of the pose estimation deviation but also constitutes the objective function for subsequent iterative adjustments, guiding the optimization algorithm to search for the optimal solution in the direction of error reduction.

[0057] As described in step S6064 above, the translation and rotation parameters are iteratively adjusted based on the matching error to gradually align the projected contour with the edge of the image target, outputting the six-dimensional pose with the highest degree of overlap as the optimized six-dimensional pose. The iterative adjustment can refer to the process of fine-tuning the pose parameters based on the matching error using a nonlinear optimization algorithm (such as the Gauss-Newton method, the Levenberg-Marquardt method, or a gradient descent strategy based on deep learning). The translation and rotation parameters correspond to the position coordinates of the object in three-dimensional space, respectively. and posture angle In each iteration, the algorithm calculates the gradient or Hessian matrix of the matching error with respect to the pose parameters, determining the direction and step size of the parameter updates so that the projected contour continuously moves closer to the edge of the image target. The calculated error is fed back to the parameter adjustment module, which updates the pose, reprojects, and recalculates the error until convergence conditions are met (e.g., the error is less than a preset threshold or the maximum number of iterations is reached). For example, if the overall projected contour is detected to be shifted to the right, the algorithm will automatically reduce... Components; if the contour angles do not match, adjust the rotation parameters. Finally, when the overlap between the projected contour and the edge of the image target reaches its maximum, i.e., the matching error is minimized, the iteration stops and the pose parameters at this point are output as the optimized six-dimensional pose. This edge-feature-driven fine-grained registration effectively corrects residual errors caused by occlusion or noise in the initial PnP estimation, improving the final accuracy of the pose estimation.

[0058] In one embodiment, after step S6 of estimating the six-dimensional pose of the target object based on the set of valid key points, the method further includes: S701: Input the six-dimensional pose of multiple consecutive frames into the Kalman filter for time-series filtering to obtain the smoothed estimated pose.

[0059] As described in step S701 above, the six-dimensional pose of multiple consecutive frames can refer to a sequence of position and translation vectors of the target object in three-dimensional space, estimated by previous steps and arranged in temporal order. Specifically, the six-dimensional pose includes three translation components (tx, ty, tz) and three rotation components (rx, ry, rz), derived from the solution results of the effective keypoint set in the current frame and historical frames. The Kalman filter is a recursive state estimation algorithm used to estimate the true state of a system in the presence of measurement noise by combining the system's dynamic model and observation data. In this embodiment, the Kalman filter is configured to receive the smoothed pose from the previous moment as a priori estimate and input the newly calculated six-dimensional pose of the current frame as the observation value. For example, when the binocular camera acquires images at a frequency of 30Hz, the system acquires a new set of six-dimensional pose data approximately every 33 milliseconds and immediately sends it to the filter queue. Through this continuous input mechanism, the filter can utilize the correlation in the temporal dimension to distinguish between real object motion and instantaneous jitter caused by image noise or matching errors. The smoothed estimated pose refers to the noise-suppressed six-dimensional pose data output after the Kalman filter prediction and update steps. This pose is obtained by constructing a state vector containing position, attitude, and corresponding velocity components, predicting the state based on a uniform motion model, and then correcting it by fusing the current observation values. Specifically, the filter's state vector can be extended to 12 dimensions, including not only the object's three-dimensional coordinates and Euler angles, but also the corresponding linear and angular velocity components. The filter dynamically adjusts the trust weights of the prediction model and the observation data according to the preset process noise covariance matrix and measurement noise covariance matrix. For example, when the object is detected to be stationary or in low-speed motion, the filter tends to trust the prediction value of the motion model, thereby significantly filtering out high-frequency measurement noise; while when the object undergoes rapid maneuvering, the filter increases the weight of the current observation value to ensure real-time tracking. The smoothed estimated pose obtained in this way eliminates the jumps and oscillations common in single-frame estimation, forming a continuous and stable trajectory sequence. This result provides high-precision target position information for subsequent robot arm control, effectively avoiding grasping failures or operational instability caused by pose jitter. The specific mathematical model of the Kalman filter includes the definition of the state vector, the state transition equation, the observation equation, and the method for setting the noise covariance matrix.

[0060] In one embodiment, after step S6 of estimating the six-dimensional pose of the target object based on the set of valid key points, the method further includes: S711: When multiple target objects are detected, an independent Kalman filter is constructed for each target object; S712: Based on the six-dimensional pose of each target object detected in the current frame, the Hungarian algorithm is used to associate each detection result with the existing trajectory, and each detection result is assigned a corresponding existing trajectory or marked as an unassociated detection. S713: Create a new trajectory from the detection results that are not associated with any existing trajectory, and initialize the corresponding Kalman filter for the new trajectory; S714: If an existing trajectory that has not been associated with any detection result for consecutive preset frames is determined to be a tracking loss, the tracking of the existing trajectory is terminated and the corresponding Kalman filter resource is released.

[0061] As described in step S711 above, when multiple target objects are detected, an independent Kalman filter is constructed for each target object. Multiple target objects can refer to two or more independent instances identified by the detection module in the same frame. Constructing an independent Kalman filter for each target object means that the system maintains an independent set of state vectors, error covariance matrices, and motion model parameters for each tracked object in the scene, without interference between them. The state vector of this independent filter typically contains the six-dimensional pose (translation and rotation components) of the corresponding object and its corresponding velocity components. Through this isolation mechanism, even if the observation data of one object fluctuates drastically or is temporarily lost, it will not affect the tracking stability of other objects. For example, in a humanoid robot operation scenario, if a water cup and a plate are present on the table simultaneously, the system will initialize filter F1 for the water cup and filter F2 for the plate; F1 updates its state only using the observation sequence of the water cup, and F2 updates its state only using the observation sequence of the plate. This step aims to provide an independent and parallel computational foundation for subsequent data association and trajectory prediction, effectively avoiding state confusion between multiple targets.

[0062] As described in step S712 above, based on the six-dimensional poses of each target object detected in the current frame, the Hungarian algorithm is used to associate each detection result with existing trajectories, assigning each detection result a corresponding existing trajectory or marking it as an unassociated detection. Data association refers to the process of determining the optimal matching relationship between the detection box in the current frame and historical trajectories. The Hungarian algorithm is a classic algorithm based on bipartite graph maximum matching, used to solve this type of assignment problem. Specifically, the system first uses the poses predicted by each independent Kalman filter in the previous time step to calculate the cost matrix between the predicted pose and the actual six-dimensional pose detected in the current frame. This cost is usually defined as Mahalanobis distance or Euclidean distance to measure the proximity between the predicted and observed positions. Subsequently, the Hungarian algorithm searches for the matching scheme with the minimum total cost in this cost matrix, thereby assigning a unique existing trajectory ID to each detection result, or marking a detection result as an unassociated detection when the distance between it and all existing trajectories exceeds a preset threshold. For example, assuming objects A' and B' are detected in the current frame, and the system has trajectories T1 (predicted position near A') and T2 (predicted position near B'), the algorithm will calculate the distance combinations of A'-T1, A'-T2, B'-T1, and B'-T2, ultimately determining that A' matching T1 and B' matching T2 are the optimal solutions. If a new object C' appears, and its position is far from the prediction range of T1 and T2, then C' is marked as unassociated. This step, through a globally optimal matching strategy, significantly reduces the probability of identity switching caused by object cross-movement or brief occlusion, ensuring the continuity of the trajectory.

[0063] As described in step S713 above, detection results not associated with any existing trajectory are created as new trajectories, and the corresponding Kalman filter is initialized for these new trajectories. A new trajectory can refer to a tracking sequence formed by a target object that appears newly in the current frame and cannot be matched with any historical path. For results marked as unassociated detections, the system determines them to be newly entered or rediscovered targets and immediately performs an initialization operation. This operation includes: using the current six-dimensional pose of the detection result as the initial state mean, setting the initial velocity to zero or estimating it based on the difference between previous and subsequent frames, and configuring a large initial covariance to reflect the uncertainty at the initial moment, thereby generating a new Kalman filter instance. For example, when a robotic arm places a new workpiece into the camera's field of view, if the workpiece is detected in the first frame but not matched with any old trajectory, the system immediately assigns it a new trajectory ID (e.g., ID=3) and instantiates filter F3, continuously tracking it starting from the next frame.

[0064] As described in step S714 above, an existing trajectory that has not been associated with any detection result for a consecutive preset frame is determined to be a tracking loss, the tracking of the existing trajectory is terminated, and its corresponding Kalman filter resources are released. Here, the consecutive preset frame is a configurable time window parameter used to tolerate brief detection failures or severe occlusion. When an existing trajectory fails to be successfully associated with any new detection result within several consecutive frames (e.g., 5 consecutive frames), the system determines that the target has left the field of view, is permanently occluded, or tracking has failed. At this time, the system will perform a cleanup operation: stop updating the status of the trajectory, mark it as a tracking loss or termination, and destroy its corresponding Kalman filter instance and related data structures from memory, releasing the occupied computing resources and storage space. For example, if the object corresponding to trajectory T1 is completely removed from the camera's field of view, resulting in no matching detection for 5 consecutive frames, the system will automatically delete T1 and its filter F1 to prevent the accumulation of invalid trajectories from causing excessive system load.

[0065] In one embodiment, before step S701, which involves inputting the six-dimensional pose of multiple consecutive frames into a Kalman filter for time-series filtering to obtain a smoothed estimated pose, the method further includes: S7001: Obtain the shooting distance between the target object and the binocular camera, and determine whether the shooting distance is greater than a preset distance threshold; S7002: When the shooting distance is greater than a preset distance threshold, increase the process noise covariance matrix of the Kalman filter.

[0066] As described in step S7001 above, the shooting distance between the target object and the stereo camera is obtained, and it is determined whether the shooting distance is greater than a preset distance threshold. The shooting distance can refer to the depth value Z of the target object's center point or feature point in the stereo camera coordinate system. This distance is calculated using the depth values ​​of each preset keypoint recovered from the dense disparity map. For example, the average or median depth of all keypoints in the effective keypoint set can be used as the shooting distance for the current frame. The preset distance threshold is an empirical value pre-set based on the stereo camera's baseline length, focal length, and the system's allowable measurement error range. It is used to distinguish between close-range high-precision measurement areas and long-range low signal-to-noise ratio measurement areas. For example, when the stereo camera baseline is 120mm and the focal length is 4mm, the preset distance threshold can be set to 3 meters; when the calculated shooting distance Z is greater than 3 meters, it is determined to be a long-range scene. This determination step aims to identify the uncertainty level of the current visual observation, because as the distance increases, the depth error corresponding to the same pixel error increases quadratically, leading to a significant increase in observation noise.

[0067] As described in step S7002 above, when the shooting distance is greater than a preset distance threshold, the process noise covariance matrix of the Kalman filter is increased. Increasing the process noise covariance matrix can refer to dynamically scaling the process noise covariance matrix Q in the state equation of the Kalman filter. Specifically, the process noise covariance matrix Q is usually defined as a diagonal matrix, where the diagonal elements represent the process noise intensity of each component (such as position, velocity, angle, angular velocity) in the state vector. When the shooting distance is detected to be greater than the preset threshold, the system increases its value by multiplying by a gain coefficient k greater than 1 (k>1), or by recalculating the diagonal elements of the Q matrix according to a linear or nonlinear function relationship based on the difference between the distance Z and the threshold. For example, if the baseline process noise covariance is... When the distance exceeds the threshold, it can be updated to ,in As a regulating factor, This represents the preset distance threshold. In Kalman filtering theory, the process noise covariance matrix Q reflects the system's confidence in the predictive ability of the motion model, while the observation noise covariance matrix R reflects the confidence in the sensor measurements. Increasing the Q value means reducing the filter's confidence in the prediction results of the uniform motion model, and relatively increasing the weight of new observation data. Through this mechanism, when long distances cause significant visual measurement jitter, the filter can respond more quickly to changes in observations, avoiding tracking lag or bias caused by over-smoothing.

[0068] Reference Figure 3 The present invention also provides a six-dimensional pose estimation device, the device comprising: Acquisition module 902 is used to acquire left and right eye images through a preset binocular camera; The detection module 904 is used to detect a target object in the left or right eye image using a preset first neural network model, and output the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable in the current viewpoint. The generation module 906 is used to generate a dense disparity map based on the left and right eye images using a preset stereo matching algorithm. The recovery module 908 is used to recover the depth of each preset key point based on the dense parallax map, and obtain the three-dimensional coordinates of each preset key point in the binocular camera coordinate system. The selection module 910 is used to select the preset key points whose visibility is greater than a preset threshold as valid key points, thereby obtaining a set of valid key points. The estimation module 912 is used to estimate the six-dimensional pose of the target object based on the set of valid key points.

[0069] In one embodiment, the estimation module 912 includes: A 3D model acquisition module is used to acquire a 3D model of the target object; The three-dimensional coordinate correspondence establishment submodule is used to establish a three-dimensional coordinate correspondence between the three-dimensional coordinates of each of the effective key points and the three-dimensional coordinates of the corresponding key points in the three-dimensional model. The six-dimensional pose estimation submodule is used to randomly sample the minimum point set in the set of effective key points and estimate the six-dimensional pose through the PnP algorithm to obtain multiple candidate six-dimensional poses; The in-point count calculation submodule is used to calculate the number of in-points in the set of valid keypoints that match each of the candidate six-dimensional poses; The candidate six-dimensional pose selection submodule is used to select the candidate six-dimensional pose with the most interior points as the six-dimensional pose of the target object.

[0070] In one embodiment, the recovery module 908 includes: The focal length acquisition submodule is used to acquire the focal length and baseline distance of the stereo camera; The depth value calculation submodule is used to calculate the depth value of each preset key point based on the disparity value corresponding to each preset key point in the dense disparity map. The 3D coordinate calculation submodule is used to calculate the 3D coordinates of each preset key point in the binocular camera coordinate system based on the pixel coordinates, depth values, and camera intrinsic parameter matrix of each preset key point.

[0071] In one embodiment, the estimation module 912 further includes: The feature extraction submodule is used to extract edge and texture features of the left or right eye image by taking the six-dimensional pose as the starting point for iterative optimization and using a preset second neural network model. The projection submodule is used to project the 3D model onto the image plane according to the current pose parameters, and generate a projection contour. The matching error calculation submodule is used to calculate the matching error between the projected contour and the edge of the image target; The iterative submodule is used to iteratively adjust the translation and rotation parameters according to the matching error, so that the projected contour gradually coincides with the edge of the image target, and outputs the six-dimensional pose with the highest degree of coincidence as the optimized six-dimensional pose.

[0072] In one embodiment, the six-dimensional pose estimation device further includes: The timing filtering module is used to input the six-dimensional pose of multiple consecutive frames into the Kalman filter for timing filtering to obtain a smoothed estimated pose.

[0073] In one embodiment, the six-dimensional pose estimation device further includes: The Kalman filter building module is used to build an independent Kalman filter for each target object when multiple target objects are detected. The detection result association module is used to associate each detection result with an existing trajectory based on the six-dimensional pose of each target object detected in the current frame, and to assign a corresponding existing trajectory to each detection result or mark it as an unassociated detection. The new trajectory creation module is used to create new trajectories from detection results that are not associated with any existing trajectories, and initialize the corresponding Kalman filter for the new trajectory; The determination module is used to determine that an existing trajectory that has not been associated with any detection result in consecutive preset frames is a tracking loss, terminate the tracking of the existing trajectory, and release its corresponding Kalman filter resources.

[0074] In one embodiment, the six-dimensional pose estimation device further includes: The shooting distance acquisition module is used to acquire the shooting distance between the target object and the binocular camera, and to determine whether the shooting distance is greater than a preset distance threshold. The process noise covariance matrix enlargement module is used to increase the process noise covariance matrix of the Kalman filter when the shooting distance is greater than a preset distance threshold.

[0075] Figure 4 An internal structural diagram of an electronic device in one embodiment is shown. This electronic device can specifically be a terminal or a server, and more specifically, a computer device. Figure 4 As shown, the electronic device includes a processor, a memory, and a network interface connected via a system bus. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system and may also store a computer program. When executed by the processor, this computer program enables the processor to implement a six-dimensional pose estimation method. The internal memory may also store a computer program, which, when executed by the processor, enables the processor to implement the six-dimensional pose estimation method. Those skilled in the art will understand that… Figure 4 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the electronic device to which the present application is applied. The specific electronic device may include more or fewer components than shown in the figure, or combine certain components, or have different component arrangements.

[0076] In one embodiment, an electronic device is provided, including a memory and a processor, the memory storing a computer program that, when executed by the processor, causes the processor to perform the following steps: The system acquires left and right eye images using a pre-set binocular camera. The system detects target objects in the left or right eye image using a preset first neural network model, and outputs the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable from the current viewpoint. Based on the left and right eye images, a dense disparity map is generated using a preset stereo matching algorithm; Based on the dense parallax map, the depth of each preset key point is recovered, and the three-dimensional coordinates of each preset key point in the binocular camera coordinate system are obtained. Select the preset key points whose visibility is greater than a preset threshold as valid key points to obtain a set of valid key points; Estimate the six-dimensional pose of the target object based on the set of effective key points.

[0077] By introducing a keypoint visibility quantification mechanism and selecting only high-visibility keypoints for pose estimation, interference from occluded or low-confidence keypoints is effectively eliminated, thus enabling reliable initial pose estimation even in partially occluded scenarios. Simultaneously, dense depth information recovered through binocular stereo matching replaces the measurement method of traditional active depth sensors, allowing the system to acquire accurate 3D geometric constraints even in complex lighting environments such as strong light and reflections, avoiding pose drift caused by depth data gaps or noise. This effectively solves the problems of instability and low accuracy in six-dimensional pose estimation under non-ideal environments and occlusion conditions in existing technologies, improving the robustness and adaptability of robot vision perception systems in dynamic and complex scenes, and providing high-quality spatial pose data support for subsequent precise grasping and stable tracking by robotic arms.

[0078] In one embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, causes the processor to perform the following steps: The system acquires left and right eye images using a pre-set binocular camera. The system detects target objects in the left or right eye image using a preset first neural network model, and outputs the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable from the current viewpoint. Based on the left and right eye images, a dense disparity map is generated using a preset stereo matching algorithm; Based on the dense parallax map, the depth of each preset key point is recovered, and the three-dimensional coordinates of each preset key point in the binocular camera coordinate system are obtained. Select the preset key points whose visibility is greater than a preset threshold as valid key points to obtain a set of valid key points; Estimate the six-dimensional pose of the target object based on the set of effective key points.

[0079] By introducing a keypoint visibility quantification mechanism and selecting only high-visibility keypoints for pose estimation, interference from occluded or low-confidence keypoints is effectively eliminated, thus enabling reliable initial pose estimation even in partially occluded scenarios. Simultaneously, dense depth information recovered through binocular stereo matching replaces the measurement method of traditional active depth sensors, allowing the system to acquire accurate 3D geometric constraints even in complex lighting environments such as strong light and reflections, avoiding pose drift caused by depth data gaps or noise. This effectively solves the problems of instability and low accuracy in six-dimensional pose estimation under non-ideal environments and occlusion conditions in existing technologies, improving the robustness and adaptability of robot vision perception systems in dynamic and complex scenes, and providing high-quality spatial pose data support for subsequent precise grasping and stable tracking by robotic arms.

[0080] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments described above. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0081] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0082] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this patent application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A six-dimensional pose estimation method, characterized in that, The method includes: The system acquires left and right eye images using a pre-set binocular camera. The system detects target objects in the left or right eye image using a preset first neural network model, and outputs the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable from the current viewpoint. Based on the left and right eye images, a dense disparity map is generated using a preset stereo matching algorithm; Based on the dense parallax map, the depth of each preset key point is recovered, and the three-dimensional coordinates of each preset key point in the binocular camera coordinate system are obtained. Select the preset key points whose visibility is greater than a preset threshold as valid key points to obtain a set of valid key points; Estimate the six-dimensional pose of the target object based on the set of effective key points; specifically including: acquiring a three-dimensional model of the target object; establishing a three-dimensional coordinate correspondence between the three-dimensional coordinates of each of the effective key points and the three-dimensional coordinates of the corresponding key points in the three-dimensional model; randomly sampling the minimum point set in the set of effective key points, estimating the six-dimensional pose using the PnP algorithm, and obtaining multiple candidate six-dimensional poses; calculating the number of interior points in the set of effective key points that match each of the candidate six-dimensional poses; and selecting the candidate six-dimensional pose with the largest number of interior points as the six-dimensional pose of the target object.

2. The six-dimensional pose estimation method according to claim 1, characterized in that, The step of recovering the depth of each preset key point based on the dense disparity map and obtaining the three-dimensional coordinates of each preset key point in the binocular camera coordinate system includes: Obtain the focal length and baseline distance of the stereo camera; Calculate the depth value of each preset key point based on the disparity value corresponding to each preset key point in the dense disparity map; Based on the pixel coordinates, depth values, and camera intrinsic parameter matrix of each preset key point, calculate the three-dimensional coordinates of each preset key point in the binocular camera coordinate system.

3. The six-dimensional pose estimation method according to claim 1, characterized in that, Following the step of selecting the candidate six-dimensional pose with the most interior points as the six-dimensional pose of the target object, the method further includes: Using the six-dimensional pose as the starting point for iterative optimization, edge features and texture features of the left or right eye image are extracted through a preset second neural network model; The 3D model is projected onto the image plane based on the current pose parameters to generate a projection contour. Calculate the matching error between the projected contour and the edge of the image target; The translation and rotation parameters are iteratively adjusted based on the matching error to gradually make the projected contour coincide with the edge of the image target, and the six-dimensional pose with the highest degree of coincidence is output as the optimized six-dimensional pose.

4. The six-dimensional pose estimation method according to claim 1, characterized in that, After the step of estimating the six-dimensional pose of the target object based on the set of effective key points, the method further includes: The six-dimensional pose of multiple consecutive frames is input into a Kalman filter for time-series filtering to obtain a smoothed estimated pose.

5. The six-dimensional pose estimation method according to claim 4, characterized in that, After the step of estimating the six-dimensional pose of the target object based on the set of effective key points, the method further includes: When multiple target objects are detected, an independent Kalman filter is constructed for each target object. Based on the six-dimensional pose of each target object detected in the current frame, the Hungarian algorithm is used to associate each detection result with the existing trajectory, and each detection result is assigned a corresponding existing trajectory or marked as an unassociated detection. Detection results that are not associated with any existing trajectory are created as new trajectories, and the corresponding Kalman filter is initialized for the new trajectory; If an existing trajectory that has not been associated with any detection result for consecutive preset frames is determined to be a tracking loss, the tracking of the existing trajectory is terminated and its corresponding Kalman filter resource is released.

6. The six-dimensional pose estimation method according to claim 4, characterized in that, Before the step of inputting the six-dimensional pose of multiple consecutive frames into a Kalman filter for time-series filtering to obtain a smoothed estimated pose, the method further includes: Obtain the shooting distance between the target object and the binocular camera, and determine whether the shooting distance is greater than a preset distance threshold; When the shooting distance is greater than a preset distance threshold, the process noise covariance matrix of the Kalman filter is increased.

7. A six-dimensional pose estimation device, characterized in that, The device includes: The acquisition module is used to acquire left and right eye images through a preset binocular camera; The detection module is used to detect target objects in the left or right eye image using a preset first neural network model, and output the pixel coordinates of multiple preset key points of the target object and the visibility of each preset key point; wherein, the visibility is used to characterize the degree to which the key point is observable in the current viewpoint; The generation module is used to generate a dense disparity map based on the left and right eye images using a preset stereo matching algorithm. The recovery module is used to recover the depth of each preset key point based on the dense parallax map, and obtain the three-dimensional coordinates of each preset key point in the binocular camera coordinate system. The selection module is used to select preset key points whose visibility is greater than a preset threshold as valid key points, thereby obtaining a set of valid key points. An estimation module is used to estimate the six-dimensional pose of the target object based on the set of valid key points. Specifically, it includes: acquiring a three-dimensional model of the target object; establishing a three-dimensional coordinate correspondence between the three-dimensional coordinates of each valid key point and the three-dimensional coordinates of the corresponding key point in the three-dimensional model; randomly sampling a minimum point set from the set of valid key points, estimating the six-dimensional pose using the PnP algorithm, and obtaining multiple candidate six-dimensional poses; calculating the number of interior points in the set of valid key points that match each candidate six-dimensional pose; and selecting the candidate six-dimensional pose with the largest number of interior points as the six-dimensional pose of the target object.

8. A computer-readable storage medium, characterized in that, The system contains a computer program that, when executed by a processor, causes the processor to perform the steps of the six-dimensional pose estimation method as described in any one of claims 1 to 6.

9. An electronic device, characterized in that, The device includes a memory and a processor, the memory storing a computer program that, when executed by the processor, causes the processor to perform the steps of the six-dimensional pose estimation method as described in any one of claims 1 to 6.

Images