A robust occlusion-adaptive cross-modal 6d pose estimation method

Abstract

This invention discloses an occlusion-robust adaptive cross-modal 6D pose estimation method, relating to the fields of machine vision, 3D pose estimation, robot perception, and deep learning. The method includes: obtaining the observation quality index of the current frame; extracting RGB texture features and point cloud geometric features and performing weighted fusion to obtain cross-modal fused features; determining the effective spatial correspondence set between the current frame and historical frames, and combining this with the tracking state of the current frame to obtain the initial 6D pose of the current frame; adjusting the capacity of a preset dynamic pose pool to correct the initial 6D pose of the current frame, obtaining optimized pose parameters; calculating the occlusion perception factor and point-level registration weights, iteratively updating the optimized pose parameters, and outputting the final 6D pose parameters. This invention can maintain the pose estimation results using stable visible regions even when the target is partially visible or the occlusion boundary is complex.

Description

Technical Field

[0001] This invention relates to the fields of machine vision, 3D pose estimation, robot perception and deep learning, and more specifically, to an occlusion-robust adaptive cross-modal 6D pose estimation method. Background Technology

[0002] 6D pose estimation is used to determine the 3D translation and 3D rotation of a target object in 3D space, and is a key technology in robot grasping, industrial assembly, and spatial perception systems. In practical applications, target objects are often in complex environments such as occlusion, weak texture, lighting variations, lack of depth, or cluttered backgrounds, which can lead to degradation of image features, point cloud features, and inter-frame matching relationships, seriously affecting the accuracy and robustness of pose estimation.

[0003] Existing 6D pose estimation methods mainly include geometric matching-based methods and deep learning-based methods. Geometric matching-based methods rely on keypoint, template, or point cloud registration, and perform well when the target's visible area is sufficient and the depth quality is good. However, they are prone to matching failures under conditions of occlusion, weak texture, or local missing features. Deep learning-based methods can utilize RGB images, depth maps, and point clouds for feature learning. However, when the quality of different modalities is uneven, noise from one modality can easily propagate to the fused features, reducing the overall estimation performance.

[0004] Some existing methods directly concatenate RGB features with point cloud features at the feature ends or fuse them using a fixed ratio, failing to dynamically adjust the contributions of different modalities based on the visibility, depth validity, and matching confidence of the current frame. Some time-based pose estimation methods mainly rely on the pose of the previous frame for recursion, lacking constraints on the consistency of poses across multiple historical frames, which can easily lead to cumulative drift over long-term operation. Furthermore, existing point cloud registration processes mostly focus on geometric distance metrics, rarely considering color consistency and occlusion region confidence simultaneously. When the target geometry is similar, there is severe local occlusion, or depth points are missing, incorrect registration results are easily obtained.

[0005] In real-world scenarios such as robot grasping, industrial inspection, and warehouse sorting, occlusion is usually not a singular phenomenon but rather occurs simultaneously with abrupt changes in illumination, localized reflections of objects, depth holes, motion blur, and background interference. In such cases, textured regions in the RGB image may remain clear while the depth map is partially missing; conversely, the depth point cloud may remain stable, but the RGB segmentation boundaries may shift due to occlusion. If fixed-weight fusion is still used, erroneous features from low-reliability modes are easily introduced into the pose estimation process. Furthermore, continuous-frame pose estimation is not merely a matter of single-frame detection accuracy. If a mismatch or short-term occlusion occurs in the current frame, the pose result recursively derived from the previous frame may be continuously amplified, leading to pose jumps, cumulative drift, or even tracking loss. Therefore, a more stable constraint relationship needs to be established between the current frame's observation information and historical multi-frame pose information, enabling the system to maintain pose continuity by leveraging historical consistency when information in the current frame is insufficient.

[0006] No effective solutions have yet been proposed to address the problems in the relevant technologies. Summary of the Invention

[0007] To address the problems in related technologies, this invention proposes an occlusion-robust adaptive cross-modal 6D pose estimation method to overcome the aforementioned technical problems in existing related technologies.

[0008] Therefore, the specific technical solution adopted by the present invention is as follows:

[0009] An occlusion-robust adaptive cross-modal 6D pose estimation method, comprising:

[0010] S1. Obtain the observation quality index of the current frame; where the observation quality index includes the target RGB image sub-image, target depth map, target mask, effective depth point set, target visible ratio, effective depth point ratio and target mask integrity;

[0011] S2. Based on the target RGB image sub-image and effective depth point set in the observation quality index, extract RGB texture features and point cloud geometric features and perform weighted fusion to obtain cross-modal fusion features;

[0012] S3. Based on cross-modal fusion features, determine the effective spatial correspondence set between the current frame and historical frames, and combine the tracking state of the current frame to obtain the initial 6D pose of the current frame;

[0013] S4. Based on the tracking state of the current frame, adjust the capacity of the preset dynamic pose pool, calculate the participation weight of each historical frame based on the adjustment result, and use the participation weight to generate rotation residual and translation residual to correct the initial 6D pose of the current frame and obtain the optimized pose parameters.

[0014] S5. Calculate the occlusion perception factor and point-level registration weights, and iteratively update the optimized pose parameters with the pre-constructed registration objective function as the target, and output the final 6D pose parameters to achieve adaptive cross-modal 6D pose estimation.

[0015] The beneficial effects of this invention are as follows:

[0016] 1. This invention identifies visible and occluded regions through an occlusion perception factor and dynamically adjusts the contribution of different regions during registration optimization to reduce the impact of occlusion points, mismatched points, and depth anomalies on the final pose. Even when the target is locally visible or the occlusion boundary is complex, the pose estimation result can still be maintained by utilizing a stable visible region.

[0017] 2. This invention adaptively adjusts the weights of RGB texture features and point cloud geometric features through modal confidence, avoiding performance degradation caused by fixed fusion ratios in scenarios with changes in lighting, missing depth, weak textures, or sparse point clouds, and enabling the fused features to dynamically adapt to changes in the quality of the current frame.

[0018] 3. This invention uses a dynamic pose pool to correct the initial pose of the current frame by utilizing multi-frame historical poses, cross-modal fusion features, point cloud features, matching confidence, and registration residual information; and embeds the kinematic deviation between the relative pose of historical frames and the predicted relative pose into the attention participation weight to suppress pose jumps and cumulative drift caused by single-frame mismatches, local occlusion, and relative pose recursion.

[0019] 4. This invention completes continuous pose estimation based on RGB-D observation data, target template, reference point cloud, category prior or initial pose; at the same time, it allows the introduction of 3D model or labeled pose as auxiliary reference during the training, initialization or evaluation stage, which can be adapted to various application scenarios such as robot operation, industrial assembly, automatic detection, warehouse sorting and augmented reality, and can flexibly adjust modal weights and pose pool strategies according to different sensor quality. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments 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.

[0021] Figure 1 This is a flowchart of an occlusion-robust adaptive cross-modal 6D pose estimation method according to an embodiment of the present invention;

[0022] Figure 2This is a schematic diagram of the overall process of an occlusion-robust adaptive cross-modal 6D pose estimation method according to an embodiment of the present invention.

[0023] Figure 3 This is a schematic diagram of the adaptive cross-modal feature fusion network in an occlusion-robust adaptive cross-modal 6D pose estimation method according to an embodiment of the present invention.

[0024] Figure 4 This is a schematic diagram of the structure of the improved PSPNet-GCN network in an occlusion-robust adaptive cross-modal 6D pose estimation method according to an embodiment of the present invention.

[0025] Figure 5 This is a schematic diagram of the dynamic attitude pool optimization module in an occlusion-robust adaptive cross-modal 6D pose estimation method according to an embodiment of the present invention.

[0026] Figure 6 This is a schematic diagram of the module composition of an occlusion-robust adaptive cross-modal 6D pose estimation system according to an embodiment of the present invention.

[0027] In the picture:

[0028] 11-Image acquisition and preprocessing steps, 12-Adaptive cross-modal feature fusion steps, 13-Relative pose calculation and initial absolute pose estimation steps, 14-Dynamic pose pool optimization steps, 15-Occlusion-aware registration optimization and final output steps.

[0029] 21-RGB feature extraction branch, 22-point cloud feature extraction branch, 23-modal confidence calculation unit, 24-cross-modal fusion unit, 25-cross-modal fusion feature output end;

[0030] 31-PSPNet backbone network, 32-Feature pyramid pooling layer, 33-Graph structure construction, 34-GCN graph convolution module, 35-RGB texture feature output end;

[0031] 41-Attitude pool storage module, 42-Historical pose consistency calculation unit, 43-Temporal attention module, 44-Pose correction calculation unit, 45-Optimized pose output terminal;

[0032] 51-Image acquisition and preprocessing module, 52-Adaptive cross-modal feature fusion module, 53-Relative pose calculation module, 54-Dynamic pose pool optimization module, 55-Occlusion-aware registration optimization module, 56-Storage module, 57-Model training module. Detailed Implementation

[0033] To further illustrate the various embodiments, the present invention provides accompanying drawings, which are part of the disclosure of the present invention. These drawings are mainly used to illustrate the embodiments and can be used in conjunction with the relevant descriptions in the specification to explain the operating principles of the embodiments. With reference to these drawings, those skilled in the art should be able to understand other possible implementation methods and the advantages of the present invention.

[0034] According to an embodiment of the present invention, an occlusion-robust adaptive cross-modal 6D pose estimation method is provided.

[0035] The present invention will now be further described in conjunction with the accompanying drawings and specific embodiments, such as... Figures 1-2 As shown, the occlusion-robust adaptive cross-modal 6D pose estimation method according to an embodiment of the present invention includes:

[0036] S1. Obtain the observation quality index of the current frame; where the observation quality index includes the target RGB image sub-image, target depth map, target mask, effective depth point set, target visible ratio, effective depth point ratio and target mask integrity;

[0037] In this optional embodiment, S1 includes:

[0038] S11. Preprocess the acquired video sequence and output the preprocessed RGB image and preprocessed depth map of the current frame. Specifically, acquire a video sequence containing the target object through the camera, extract the RGB image and depth map of the current frame from the video sequence, and perform denoising, normalization, and depth filtering operations on the RGB image and depth map to output the preprocessed RGB image and preprocessed depth map of the current frame.

[0039] S12. Process the preprocessed RGB image and preprocessed depth map to generate an initial target mask, a target RGB image sub-image, and a target depth map. Specifically, use a target detection algorithm to process the preprocessed RGB image to generate an initial target mask, and crop the preprocessed RGB image and preprocessed depth map based on the initial target mask to obtain a target RGB image sub-image and a target depth map of the target region.

[0040] S13. Extract the boundary pixels of the initial target mask, and perform pixel-level correction on the boundary region of the initial target mask based on the depth continuity of the boundary pixels in the target depth map and the mask state of the adjacent pixels, and output the target mask.

[0041] S14. Traverse each pixel within the target area and filter out pixels that simultaneously satisfy the conditions of belonging to the target mask and having a depth value within the preset effective depth range, and use them as effective depth pixels.

[0042] S15. Using a preset camera intrinsic matrix, back-project the effective depth pixels into three-dimensional space to obtain the effective depth point set of the target object. Specifically, using a preset camera intrinsic matrix, back-project the effective depth pixels into three-dimensional space to generate an initial three-dimensional point set, and sequentially perform voxel downsampling and outlier removal operations on the initial three-dimensional point set to obtain the effective depth point set of the target object;

[0043] S16. Based on the target RGB image sub-image, target mask, and effective depth point set, calculate the target visibility ratio, effective depth point ratio, and target mask integrity.

[0044] It should be added that an RGB-D video sequence containing the target object is acquired using an RGB-D camera. The current frame image is processed for denoising, normalization, depth filtering, and target region extraction. The target object region is then obtained through object detection, semantic segmentation, or instance segmentation. Based on the target object region, the RGB image and depth map are cropped or masked to obtain a target RGB image sub-image, a target depth map, and the corresponding effective depth point set. Let the th... Frame RGB image is Depth map is The target mask is For image coordinates of pixels, when And depth value When the pixel is within the preset effective depth range, it is determined as an effective depth pixel; according to the camera intrinsic parameter matrix Backprojecting effective depth pixels into 3D points The 3D points obtained from backprojection are voxel downsampled and outlier removed to obtain an effective depth point set. For the target mask boundary region, boundary correction can be performed based on the depth continuity of boundary pixels and the mask state of adjacent pixels to reduce the possibility of occlusion boundaries being incorrectly classified into the target or background region. Based on the target mask, effective depth pixels, and effective depth point set, the target visibility ratio, effective depth point ratio, target mask integrity, local depth continuity, and image texture sharpness are calculated for subsequent modal reliability calculation, spatial correspondence filtering, and occlusion-aware registration. During the above preprocessing, image quality and depth quality information of the current frame can also be simultaneously statistically analyzed, such as target mask area, target visibility ratio, number of effective depth points, depth hole ratio, edge sharpness, and segmentation confidence. This information is not only used to generate subsequent input data but also serves as the basis for modal reliability calculation and occlusion-aware judgment, enabling the system to recognize degraded scenes from the input stage. In addition, the depth value is read from the corresponding pixel in the target depth map and determined after depth filtering, hole removal, or validity filtering.

[0045] Among them, the RGB texture features are obtained through an RGB feature extraction network with multi-scale semantic feature extraction and graph structure context enhancement functions; the RGB feature extraction network constructs a pixel neighborhood graph structure in the target image region and aggregates the spatial relationship between pixels in the target region based on the pixel neighborhood graph structure to enhance the texture expression near occlusion boundaries, local missing and weak texture regions; among them, the pixel neighborhood graph structure includes an 8-neighborhood graph structure, which is determined according to Equation (1).

[0046] (1);

[0047] In the formula, x and y are the coordinates of pixel p in the image coordinate system. and This represents the neighborhood offset; the spatial relationships between pixels in the RGB image are mined using this graph structure to enhance texture representation near occluded areas. Additionally, the target visibility ratio... ,in, This represents the number of visible target pixels in the target mask. The target projected area of ​​the reference template at the current pose or bounding box scale; effective depth point ratio. ,in, To satisfy the depth validity condition, the number of effective depth pixels, The number of pixels within the target mask; target mask integrity. ,in, For the target mask area, The area of ​​the outer region of the target detection box or reference projection.

[0048] S2. Based on the target RGB image sub-image and effective depth point set in the observation quality index, extract RGB texture features and point cloud geometric features and perform weighted fusion to obtain cross-modal fusion features;

[0049] In this optional embodiment, S2 includes:

[0050] S21. Input the target RGB image sub-image and the effective depth point set into the preset RGB feature extraction branch and point cloud feature extraction branch respectively, output RGB texture features and point cloud geometric features, and perform spatial alignment on the RGB texture features and point cloud geometric features.

[0051] S22. Divide the target region into multiple local regions, and calculate the local RGB modal fusion weight and local point cloud modal fusion weight by combining modal reliability information; wherein, the modal reliability information includes at least one of RGB segmentation confidence, texture clarity, RGB matching confidence, effective depth point ratio, point cloud completeness, or point cloud matching confidence.

[0052] In this optional embodiment, the expression for the local RGB modality confidence is:

[0053] ;

[0054] The expression for the modal confidence of a local point cloud is:

[0055] ;

[0056] The expression for the local RGB modal fusion weights is:

[0057] ;

[0058] The expression for the local point cloud modal fusion weights is:

[0059] ;

[0060] In the formula, For local RGB modal confidence; These are non-negative weighting coefficients; For the first The average RGB segmentation confidence score within a local image region; These are non-negative weighting coefficients; To normalize texture sharpness; These are non-negative weighting coefficients; For the first The average confidence level of valid RGB matching points within a local image region; For local point cloud mode confidence; These are non-negative weighting coefficients; For the first The ratio of the number of effective depth pixels to the number of target pixels within a local image region; These are non-negative weighting coefficients; For point cloud integrity; These are non-negative weighting coefficients; For the first The average confidence level of matching points within a local point cloud region; For local RGB modal fusion weights; The modal weight is the positive smoothing coefficient; Weights for local point cloud modal fusion.

[0061] S23. Based on the local RGB modal fusion weights, local point cloud modal fusion weights, spatially aligned RGB texture features and point cloud geometric features, generate local cross-modal fusion features, and fuse all local cross-modal fusion features to obtain cross-modal fusion features.

[0062] It should be added that, such as Figure 3As shown, an adaptive cross-modal feature fusion network and a cross-modal fusion feature output terminal 25 are constructed. The network includes an RGB feature extraction branch 21, a point cloud feature extraction branch 22, a modal confidence calculation unit 23, and a cross-modal fusion unit 24. In one embodiment, the RGB feature extraction branch 21 adopts an improved PSPNet-GCN network, such as... Figure 4 As shown, it includes the PSPNet backbone network 31, feature pyramid pooling layer 32, graph structure construction 33, GCN graph convolution module 34, and RGB texture feature output terminal 35. An 8-neighborhood graph structure is constructed according to equation (1) to enhance the spatial correlation expression between pixels in the target area, especially to supplement the context information near occluded edges, local missing and weak texture areas; the point cloud feature extraction branch 22 can use RandLA-Net, PointNet++, point cloud Transformer or its variants to extract the local geometry, normal changes, spatial distribution and global shape information of the target point cloud. The above RGB feature extraction branch 21 and point cloud feature extraction branch 22 are not limited to specific network names. As long as they can extract the texture context of the target area and the local / global geometric features of the point cloud respectively, and can cooperate with the modal weights output by the modal confidence calculation unit 23 to achieve adaptive fusion, they can be used as equivalent implementations.

[0063] In one specific embodiment, the input to the point cloud feature extraction branch 22 is a set of effective depth points. × Point feature matrix, It includes at least one of three-dimensional coordinates, depth values, normal vectors, color, or local density; the branch sequentially includes a coordinate normalization layer, a shared multilayer perceptron, a neighborhood construction layer, a local geometry aggregation layer, a residual or attention enhancement layer, and a global pooling layer, which output point-level geometric features and global point cloud geometric features, respectively; the point-level geometric features are spatially aligned with RGB texture features through camera intrinsic projection or nearest neighbor mapping.

[0064] Modal confidence calculation unit 23 divides the target region into several local image regions, and determines the local point cloud region corresponding to each local image region based on the target mask, camera intrinsic parameters, and effective depth point set; for the first For each local region, the local RGB modality confidence and local point cloud modality confidence are calculated according to Equation (2), the local modality fusion weights are calculated according to Equation (3), and local cross-modal fusion features are generated according to Equation (4). Each local cross-modal fusion feature is spliced, pooled, or attention-aggregated to obtain cross-modal fusion features, which serve as feature inputs for determining the spatial correspondence between the current frame and historical frames. Unlike fixed-ratio fusion or simple end splicing, this invention dynamically adjusts the fusion weights according to changes in local observation quality: when the RGB texture is affected by illumination, reflection, or occlusion, the contribution of the corresponding local point cloud geometric features is increased; when depth points are missing, depth noise increases, or the point cloud is sparse, the contribution of the corresponding local RGB texture features is increased, thereby reducing the misleading effect of a single degenerate modality on the spatial correspondence.

[0065] The modal reliability information includes at least one of RGB segmentation confidence, texture sharpness, RGB matching confidence, effective depth point ratio, point cloud completeness, and point cloud matching confidence; for the k-th local image region and its corresponding local point cloud region, the local RGB modal confidence and local point cloud modal confidence are calculated according to equation (2), and the local modal fusion weight is calculated according to equation (3): (2); (3); In the formula, Numbering for local areas This represents the total number of local regions, and ; For the first The average RGB segmentation confidence score within a local image region. This refers to the normalized texture sharpness obtained based on the image gradient, Laplacian response, or local contrast of the local image region. This represents the average confidence level of valid RGB matching points within the local image region; For the first The ratio of the number of effective depth pixels to the number of target pixels within a local image region. The point cloud completeness is determined based on the number of effective points in the corresponding local point cloud region, the neighborhood density, or the coverage ratio of the reference point cloud. This represents the average confidence level of the matched points within the corresponding local point cloud region; to as well as to These are the non-negative weight coefficients obtained through preset or training. , is the modal weight positive smoothing coefficient in equation (3), which is usually taken as a small value. It is used to maintain the numerical stability of the modal fusion weight calculation when the local RGB modal confidence and the local point cloud modal confidence are both low or zero, and to make the RGB modal fusion weight and the point cloud modal fusion weight satisfy the normalization constraint, i.e. . No. The cross-modal fusion features of each local region and the cross-modal fusion features obtained by aggregating the cross-modal fusion features of each local region are generated according to equation (4): (4);

[0066] In the formula, and These are the first two digits after spatial alignment and dimension mapping, respectively. The RGB texture features of a local image region and the point cloud geometric features of its corresponding local point cloud region. For the first Cross-modal fusion features of local regions and The first RGB modal fusion weights and point cloud modal fusion weights for a local region; This indicates splicing, pooling, or attention aggregation operations. To determine cross-modal fusion features for spatial correspondence; when RGB confidence decreases or target occlusion worsens, increase the fusion weight of point cloud geometric features; when the depth missing ratio increases or the number of effective points is below the threshold, increase the fusion weight of RGB texture features.

[0067] S3. Based on cross-modal fusion features, determine the effective spatial correspondence set between the current frame and historical frames, and combine the tracking state of the current frame to obtain the initial 6D pose of the current frame;

[0068] In this optional embodiment, S3 includes:

[0069] S31. Utilize cross-modal fusion features to determine the spatial correspondence between the current frame and historical frames, and filter the spatial correspondence according to the filtering criteria to obtain a set of valid spatial correspondences; wherein, the filtering criteria include at least one of matching confidence, depth validity, spatial distribution uniformity, reprojection error or random sampling consistency inlier ratio;

[0070] S32. Based on the effective spatial correspondence set and the tracking state of the current frame, obtain the initial 6D pose of the current frame; wherein, the tracking state includes the continuous tracking frame state, the first frame state, and the re-initialization frame state.

[0071] In this optional embodiment, S32 includes:

[0072] When the current frame is in the continuous tracking frame state, the relative rotation and relative translation from the historical frame to the current frame are calculated using the effective spatial correspondence set as input, the relative pose of the adjacent frame is generated, and the relative pose of the adjacent frame is compounded and recursively combined with the pre-acquired absolute pose of the historical frame to obtain the initial 6D pose of the current frame.

[0073] When the current frame is in the first frame state or the reinitialized frame state, the initial absolute pose of the current frame is calculated based on the observation information of the current frame, and the initial absolute pose is used as the initial 6D pose.

[0074] It should be added that, based on cross-modal fusion features, the spatial correspondence between the current frame and historical frames is determined. This spatial correspondence can be a 2D keypoint matching relationship, a 3D point cloud correspondence, a 2D-3D keypoint correspondence, or a dense pixel-point cloud correspondence. By combining depth information, the 2D matching points are transformed into 3D space, resulting in the point sets of the current frame and historical frames. When determining the spatial correspondence, this invention prioritizes using the adaptively fused features for matching, rather than relying solely on single information from RGB texture or point cloud geometry. This maintains matching quality even when texture is not obvious but the geometric structure is stable, and also preserves the continuity of the correspondence even when depth is locally lacking but image edges are clear. The matching results can also output the number of matches, matching distribution, reprojection error, or matching confidence, which can be used for subsequent weight adjustments in dynamic pose pooling and occlusion-aware registration.

[0075] In the process of determining spatial correspondence and solving relative pose, one or more of the following modules can be used: feature matching network, dense correspondence estimation, keypoint matching, 2D-3D correspondence solving, 3D rigid body registration, or robust estimation. These modules can be implemented by trained neural networks, regularized geometry solvers, or existing matching / registration algorithms. In specific embodiments, the LoFTR algorithm, dense feature matching algorithm, keypoint matching algorithm, or a combination thereof can be used as the method for obtaining the spatial correspondence between the current frame and historical frames. After obtaining the spatial correspondence, it is filtered based on at least one of the following: matching confidence, depth validity, spatial distribution uniformity, reprojection error, and RANSAC inlier ratio. For the filtered 2D matching points, 3D backprojection is performed using the target depth map and camera intrinsic parameters to obtain the 3D correspondence points between the current frame and historical frames. For continuously tracked frames, the relative poses of adjacent frames are obtained based on the filtered 3D correspondence points using the Umeyama algorithm, SVD registration, PnP, RANSAC, ICP, or a combination thereof. This is then combined with the absolute poses of historical frames to obtain the initial 6D pose of the current frame. For the first frame or a re-initialized frame, if the matching confidence of the current frame is lower than a preset threshold, the number of valid correspondence points is lower than a preset number, the reprojection error is higher than a preset threshold, or an abnormal jump in the pose of the current frame relative to the historical motion trend is detected, then the initial absolute pose is obtained based on the current frame observation information through keypoint PnP, template matching, a pre-trained pose network, manual calibration, or an external localization module. The initial 6D pose, spatial correspondence, and matching confidence of the current frame are simultaneously used as inputs, and the optimized pose parameters are used as the initial pose for registration. When the matching confidence of the current frame is high and the target motion is continuous, the initial 6D pose is obtained recursively using the relative poses of adjacent frames to maintain the real-time performance of the tracking process. When there is heavy occlusion, insufficient number of matching points, sudden pose changes, or the need for re-initialization, keypoint point-to-point (PnP), template matching, pre-trained pose networks, manual calibration, or external localization modules can be introduced to obtain more reliable initialization results. All of the above initialization methods are used in conjunction with the spatial correspondence determined by cross-modal fusion features, rather than being used as isolated algorithms.

[0076] For continuously tracked frames, the spatial correspondence between the current frame and historical frames is determined based on cross-modal fusion features, and is filtered according to at least one of the following: matching confidence, depth validity, spatial distribution uniformity, reprojection error, and RANSAC (random sample consensus) inlier ratio. The relative poses of adjacent frames are calculated based on the filtered spatial correspondences and depth information, and then combined with the absolute poses of historical frames to obtain the initial 6D pose of the current frame. For the first frame or a re-initialized frame, the initial absolute pose is obtained based on the current frame observation information through at least one of the following: keypoint PnP (perspective n-point algorithm), template matching, pre-trained pose network, manual calibration, or external localization module. When the Umeyama algorithm is used to calculate the relative poses of adjacent frames in continuously tracked frames, the 3D correspondence point matrices of historical frames and the current frame are assumed to be... and , , The shapes are all ,in, For point cloud dimensions, Given the corresponding number of points, the centroid, variance, covariance, and relative pose of the two point clouds are calculated according to equations (5) to (8): (5); (6); (7); (8);

[0077] In the formula, and The centroids of X and Y are respectively. Let X be the variance. Let U be the covariance matrix. V is the matrix obtained by performing singular value decomposition on the covariance matrix. The diagonal matrix is ​​used to avoid reflection transformation; no scale factor is introduced in the 6D pose estimation of this invention, and equation (8) adopts the rigid body pose calculation form without scale term; For relative rotation matrices, Let be the relative translation vector. For the historical frame corresponding point set The dispersion statistics can be used for corresponding point quality evaluation or scale estimation extensions; however, they are not used for scale factor calculation in the 6D pose estimation of this invention.

[0078] The initial 6D pose of the current frame in a series of frames is calculated according to equation (9): (9);

[0079] In the formula, The absolute pose matrix of the historical frames from the target object coordinate system to the historical frame camera coordinate system; This is the relative pose matrix from the historical frame camera coordinate system to the current frame camera coordinate system; The initial 6D pose matrix for the current frame is the distance from the target object coordinate system to the current frame camera coordinate system; the relative pose matrix includes the relative rotation matrix. and relative translation vector For the first frame or the reinitialized frame, Equation (9) is not used for recursion. Instead, the initial absolute pose is obtained through at least one of the following: keypoint PnP, template matching, pre-trained pose network, manual calibration, or external localization module. In Equation (9), the superscript 0 indicates the initial pose estimation of the current frame and does not indicate the coordinate system number.

[0080] S4. Based on the tracking state of the current frame, adjust the capacity of the preset dynamic pose pool, calculate the participation weight of each historical frame based on the adjustment result, and use the participation weight to generate rotation residual and translation residual to correct the initial 6D pose of the current frame and obtain the optimized pose parameters.

[0081] In this optional embodiment, S4 includes:

[0082] S41. Update the preset dynamic attitude pool according to the tracking state of the current frame;

[0083] In this optional embodiment, S41 includes:

[0084] When the current frame is in the first frame state or the reinitialized frame state, the initial 6D pose, cross-modal fusion features, local RGB modal confidence, local point cloud modal confidence, reprojection error, registration residual and target visibility ratio of the current frame are written into the preset dynamic pose pool as the first historical frame record, and the updated dynamic pose pool is obtained.

[0085] When the current frame is in the continuous tracking frame state, the reliability score of the current frame is calculated based on the local RGB modal confidence, local point cloud modal confidence, reprojection error, registration residual, and pose continuity. The reliability score is then compared with a preset threshold. If the reliability score is not lower than the preset threshold, the data corresponding to the current frame is written into the dynamic attitude pool to obtain the updated dynamic attitude pool. If the reliability score is lower than the preset threshold, the current frame is marked as a target reliable frame. If the number of consecutive target reliable frames reaches the preset reinitialization threshold, the reinitialization process is triggered to obtain the updated dynamic attitude pool.

[0086] S42. Calculate the capacity adjustment factor of the dynamic attitude pool, and determine the current capacity of the dynamic attitude pool by combining the preset lower capacity limit and upper capacity limit, and adjust the capacity of the updated dynamic attitude pool based on the current capacity.

[0087] S43. Based on the historical frames in the dynamic attitude pool after capacity adjustment, the motion trend features of each historical frame are extracted using the self-attention mechanism, and the attention score of each historical frame is calculated through cross-attention. After normalizing the attention score, the participation weight of each historical frame is obtained.

[0088] S44. Use participation weights to perform weighted aggregation of historical frames in the dynamic pose pool to generate temporal aggregation features. Based on the temporal aggregation features, predict rotation residuals and translation residuals. Then, use the rotation residuals and translation residuals to correct the initial 6D pose and obtain the optimized pose parameters.

[0089] It should be added that, such as Figure 5 As shown, the system includes a pose pool storage module 41, a historical pose consistency calculation unit 42, a temporal attention module 43, a pose correction calculation unit 44, and an optimized pose output terminal 45. Specifically, the dynamic pose pool is used to store pose information, fusion features, matching confidence, reprojection error, registration residual, and time index. In the first frame or reinitialization frame, the initial 6D pose of the current frame, cross-modal fusion features, matching confidence, reprojection error, registration residual, and target visibility ratio are written into the dynamic pose pool as the first historical frame record. For subsequent frames, the system receives the output initial 6D pose of the current frame, spatial correspondence, and matching confidence, and calculates the reliability score of the current frame based on the current frame's matching confidence, reprojection error, registration residual, and pose continuity. When the reliability score is not lower than the preset threshold, the data corresponding to the current frame is written into the dynamic attitude pool; when the reliability score is lower than the preset threshold, the current frame is not written into the dynamic attitude pool, it is marked as a low reliability frame (i.e., the target reliable frame), and the weight of its observation information in the temporal optimization is reduced. Historical frames with higher reliability scores and smaller kinematic deviations are called first to assist in correcting the initial pose of the current frame; if the number of consecutive low reliability frames reaches the preset reinitialization threshold, the reinitialization process is triggered. The capacity of the dynamic attitude pool is adaptively adjusted according to Equations (10) and (11); when the target motion is stable and the observation quality of the current frame is high, the size of the attitude pool is reduced to improve real-time performance; when occlusion is severe, the matching confidence is reduced, or the consistency of historical poses is reduced, the size of the attitude pool is increased to enhance temporal constraints. For historical frames participating in temporal optimization, the motion trend of historical frames is first extracted by self-attention, and then the correlation between the current frame and historical frames is calculated by cross-attention; the attention score and participation weight are calculated according to Equation (13). The rotation residual and translation residual are predicted by temporal aggregation features, and the optimized pose parameters are obtained according to Equation (12). The optimized pose parameters and historical pose consistency information are used as the initial pose and temporal constraint information for color-geometric joint registration. Furthermore, the reprojection error is included. Based on the initial or optimized 6D pose of the current frame, this is the statistical value of the pixel distance between the reference 3D point projected onto the image plane of the current frame and its corresponding pixel coordinates; registration residual. This represents the weighted spatial distance statistics of the corresponding 3D points after pose transformation.

[0090] The capacity of the dynamic pose pool and the weights of historical frames are jointly determined by the target motion velocity, the proportion of the target visible in the current frame, the proportion of effective depth points, the matching confidence in the current frame, the registration residuals of historical frames, historical pose consistency, and kinematic bias. Pose continuity. The reliability score is obtained by normalizing the rotation and translation deviations between the initial 6D pose of the current frame and the pose predicted from the most recent reliable historical frames. The reliability score is calculated based on the local RGB mode confidence, local point cloud mode confidence, and... , and The result is obtained by normalizing and weighting at least two of the following: matching confidence, modal confidence, and pose continuity are positive indicators, while reprojection error and registration residual are negative indicators.

[0091] To avoid confusion between capacity symbols, historical frame indices, and the number of corresponding points, the dynamic attitude pool capacity is uniformly denoted as... The value range is 4 to 8; the dynamic attitude pool capacity is adaptively adjusted according to the target motion speed, the target visibility ratio in the current frame, the effective depth point ratio, the current frame matching confidence, the historical frame registration residual, and the historical pose consistency. Specifically, the attitude pool capacity adjustment factor is calculated according to formula (10), and the dynamic attitude pool capacity is determined according to formula (11). When the target motion speed is low, the target visibility ratio is high, the effective depth point ratio is high, the current frame matching confidence is high, and the historical pose consistency is good, a smaller capacity is taken; when the target motion speed increases, the target visibility ratio decreases, the effective depth point ratio decreases, the current frame matching confidence decreases, the historical frame registration residual increases, or the historical pose consistency decreases, the dynamic attitude pool capacity is increased; for the first For each historical frame involved in the temporal optimization, a reliability score is obtained based on its matching confidence, reprojection error, registration residual and pose continuity. The kinematic bias is obtained based on the deviation between the relative pose of the historical frame and the predicted relative pose estimated by the most recent reliable historical frame. The dynamic pose pool corrects the initial 6D pose of the current frame according to Equation (12), and the attention score and participation weight of the historical frame are determined according to Equation (13).

[0092] (10); (11); (12); (13);

[0093] Among them, in equation (10) This is the attitude pool capacity adjustment factor; , , , , and These are the normalized current frame target visibility, depth validity, matching confidence, historical pose consistency, target motion velocity, and historical registration residual indices, respectively. , , , , and These are the non-negative weight coefficients obtained through pre-setting or training. This indicates numerical truncation. In equation (11) For the dynamic attitude pool capacity, and These are the lower and upper limits of capacity, respectively. This indicates rounding down. In equation (12)... The optimized pose matrix, This is the initial pose matrix for the current frame. For pose residual transformation, and These are the rotation residual and the translation residual, respectively. In equation (13) For historical frame indexing, Here, H is the normalized summation index for historical frames, and H is the set of historical frames participating in the time-series optimization. For the first Attention score for each historical frame This is the initial pose lookup vector for the current frame. For the first A historical frame key vector, For query vector dimensions, For the first Reliability score for each historical frame. For the first Kinematic bias of a historical frame and These are the weights for the reliability term and the kinematic constraint term, respectively. For the logarithmic term, numerical stability constant, For the first The participation weights of each historical frame. The reliability score of the current frame is obtained by normalized weighting of at least two of the following: current frame matching confidence, target visibility, depth effectiveness, reprojection error, registration residual, and pose continuity; among which, matching confidence, target visibility, and depth effectiveness are positive indicators, while reprojection error, registration residual, and pose jump degree are negative indicators.

[0094] Furthermore, for historical frames participating in temporal optimization, the reliability score of the historical frame is calculated using a normalization method that is the same as or compatible with the reliability score of the current frame. Kinematic bias is determined by the rotational and translational biases between the relative pose of the historical frame and the relative pose predicted by the motion model of the most recent reliable historical frame. The motion model of the most recent reliable historical frame adopts a uniform velocity model: the relative pose between the poses of the two most recent reliable historical frames is used as the predicted relative pose of the current frame; when there are fewer than two reliable historical frames, the relative pose between the pose of the most recent reliable historical frame and the initial pose of the current frame is used as the degradation prediction. Kinematic bias is obtained by weighted summation after normalization of rotation angle bias and displacement distance bias, and is therefore dimensionless when used as input for attention scoring. Historical pose consistency is determined by the pose distance between the predicted pose of the current frame obtained from reliable historical frames in the dynamic pose pool and the initial or optimized pose of the current frame, and is normalized to a preset interval; the higher the historical pose consistency, the higher the weight of reliable historical frames in temporal optimization and registration constraints.

[0095] S5. Calculate the occlusion perception factor and point-level registration weights, and iteratively update the optimized pose parameters with the pre-constructed registration objective function as the target, and output the final 6D pose parameters to achieve adaptive cross-modal 6D pose estimation.

[0096] In this optional embodiment, S5 includes:

[0097] S51. The occlusion perception factor is calculated based on the target visibility ratio, the effective depth point ratio, and the target mask integrity.

[0098] S52. Based on the spatial correspondence, target mask and target depth map, determine whether each pair of spatial correspondence points between the current frame and the historical frame is located within the visible area of ​​the target mask, and whether the depth value in the target depth map meets the preset depth validity condition, and mark the spatial correspondence points that pass through at the same time as valid spatial correspondence points.

[0099] S53. Set a visibility status flag for each point in the effective space, and calculate the point-level registration weight of each point in the effective space in combination with the occlusion perception factor.

[0100] S54. Based on point-level registration weights, construct a registration objective function that includes a weighted geometric registration loss function, a weighted color registration loss function, an occlusion constraint loss function, and a historical pose consistency loss function. Using the registration objective function as the target, iteratively update the optimized pose parameters through a nonlinear least squares method until the preset number of iterations is reached, and output the final 6D pose parameters.

[0101] In this optional embodiment, the expression for the registration objective function is:

[0102] ;

[0103] The expression for the weighted geometric registration loss function is:

[0104] ;

[0105] The expression for the weighted color registration loss function is:

[0106] ;

[0107] The expression for the occlusion constraint loss function is:

[0108] ;

[0109] The expression for the historical pose consistency loss function is:

[0110] ;

[0111] In the formula, The rotation matrix to be optimized during the registration stage; The translation vector to be optimized during the registration stage; To register the objective function; To block the perception factor Adjusted weighted geometric registration loss weights; To block the perception factor; The weighted geometric registration loss function; To block the perception factor Adjusted weighted color registration loss weights; The weighted color registration loss function; To block the perception factor Adjusted occlusion constraint loss weights; The loss function is defined by occlusion constraints. To block the perception factor Adjust the weight of historical pose consistency loss; Let the historical pose consistency loss function be used. This represents the number of points corresponding to the effective space. Index of the corresponding point in the effective space; For the first Point-level registration weights for each corresponding point; For the first Visible status flags for each corresponding point; The source is a 3D point; The target is a three-dimensional point; The color values ​​of the current frame's RGB image; For camera projection functions; The color value of a historical frame or reference frame; The corresponding pixel coordinates in the reference image; For robust penalty functions; For the first Occlusion constraint residuals for each point in the effective space; For reliable historical frame indexing; A reliable set of historical frame indexes for participating in historical pose consistency constraints; For the first The historical consistency loss weight of a reliable historical frame; For Lie algebra mappings; For the first The predicted pose of the current frame is obtained by predicting the current frame using a reliable historical frame through a motion model; The pose matrix of the current frame is formed by the rotation matrix and translation vector during the registration stage.

[0112] It should be added that the occlusion perception factor is calculated according to Equation (14) based on the target's visible ratio, effective depth point ratio, and target mask integrity. The system receives the target's visible proportion, effective depth point proportion, and target mask integrity, along with their spatial correspondence, and uses the optimized pose parameters as the initial pose for registration. The optimized pose parameters are represented as a homogeneous pose matrix. Its rotation and translation components are used as initial values ​​for the registration iteration, and the variables to be optimized in the registration stage are denoted as... and For each spatially corresponding point between the current frame and historical frames, first determine whether the spatially corresponding point is located within the visible area defined by the target mask, and then determine whether its depth value meets the preset depth validity condition; subsequently, based on the color difference between corresponding pixels, the geometric distance between corresponding 3D points, the matching confidence of the spatially corresponding point, and the local depth continuity, determine the point-level registration weight of the spatially corresponding point. For spatial corresponding points located at occlusion boundaries, depth abrupt changes, or with matching confidence below a preset threshold, their contribution to color and geometric errors is reduced; for spatial corresponding points within the visible region that are depth-continuous, color-consistent, and have high matching confidence, their point-level registration weight is increased. The occlusion perception factor is used not only to describe the overall visibility of the target, but also to synchronously adjust the point-level registration weights of different points, different regions, and different matching pairs, as well as the weights of the loss terms in equations (15) to (19). When the occlusion perception factor is small, the weights of occlusion boundaries, depth abrupt changes, color anomalies, and low-confidence matching points are reduced, and the relative weight of the reliable historical pose consistency constraint is increased; when the occlusion perception factor is large, the weights of the geometric and color joint constraints of visible, depth-continuous, and color-consistent regions are increased. A weighted geometric registration loss is constructed. Weighted color registration loss Occlusion constraint loss Loss of consistency with historical pose Registration objective function The registration rotation matrix and translation vector are updated using nonlinear least squares, gradient descent, ICP variants, or neural network optimization. Iteration stops when the rotation and translation changes in two consecutive iterations are both less than a preset threshold, the decrease in the registration objective function is less than a preset threshold, or the maximum number of iterations is reached, and the final 6D pose parameters are output.

[0113] Among them, occlusion perception factor Calculate according to formula (14): (14);

[0114] In the formula, This is the ratio of the number of visible target points to the total number of target reference points. It is the ratio of the number of effective depth pixels in the target area to the number of target pixels. This is the ratio of the current target mask area to the target's expected projected area or the area of ​​the detection region; , and These are the non-negative weight coefficients obtained through preset or training processes, and + + =1; To block the perceptual factors, A smaller value indicates more severe target occlusion or observation degradation. The occlusion perception factor is used to reduce the contributions of occluded regions, low-confidence matching points, and depth anomalies, and to adjust the weights of geometric consistency constraints, color consistency constraints, occlusion constraints, and historical pose consistency constraints. The total number of target reference points, target reference points, reference point cloud, or expected projected area of ​​the target is obtained from at least one of the following: target CAD model, template point cloud, historical keyframe reconstructed point cloud, category prior point cloud, reference observation model established during the initialization phase, or statistical model of the detection area; when a complete CAD model is not available, historical keyframe point cloud, template point cloud, or category prior point cloud is used as the reference model.

[0115] During the online inference phase, the registration objective function is constructed according to equation (15). The registration objective function includes the weighted geometric registration loss shown in equation (16). The weighted color registration loss shown in equation (17) The occlusion constraint loss shown in equation (18) The historical pose consistency loss shown in equation (19) Among them, the occlusion perception factor is used to adjust the point-level registration weights of the corresponding points in the effective space as well as the weights of each loss term. The weighted geometric registration loss is used to constrain the spatial consistency between the corresponding 3D points. The weighted color registration loss is used to constrain the color consistency between the corresponding pixels. The occlusion constraint loss is used to reduce the error contribution of occlusion regions, depth anomalies and low-confidence matching points. The historical pose consistency loss is used to constrain the motion continuity between the current frame pose and the current frame pose predicted based on reliable historical frames in the dynamic pose pool.

[0116] (15); (16); (17); (18); (19);

[0117] In the formula, The rotation matrix to be optimized during the registration stage. The translation vector to be optimized during the registration stage. To register the overall objective function, , , and These are weighted geometric registration loss, weighted color registration loss, occlusion constraint loss, and historical pose consistency loss, respectively. , , and These are the occlusion perception factors. Adjusted loss weights; The number of points corresponding to the effective space. For the index of the corresponding point in the effective space, For the first Point-level registration weights for each corresponding point For the source 3D point, For the target three-dimensional point, For the first The visible status flags of each corresponding point This indicates that it is visible and reliable. This indicates obstruction or low reliability. The color values ​​of the current frame's RGB image. For historical or reference frame color values, For camera projection function, For the corresponding pixel coordinates in the reference image, For robust penalty functions, For the first The predicted pose of the current frame is obtained by predicting the pose of a reliable historical frame using a motion model. The pose matrix of the current frame is composed of the rotation matrix and translation vector from the registration stage. This represents the Lie algebra mapping. The point-level registration weights are determined according to equation (20): (20);

[0118] In the formula, The global visibility coefficient is adjusted by the occlusion perception factor. For the target mask constraint term, For deep validity items, For local depth continuity, To match the confidence level, The normalized value for color difference. The normalized value of the geometric residual. and is a non-negative adjustment coefficient. The historical pose consistency loss is preferably the pose distance between the current frame registration pose and the current frame predicted pose obtained by the motion model from reliable historical frames in the dynamic pose pool, rather than directly constraining the current frame pose to be equal to any historical frame pose; when the occlusion perception factor decreases, the relative weight of this historical pose consistency loss is increased. In addition, to avoid mixing point-level weights with the corresponding point indices in the effective space, the point-level registration weights in equation (20) uniformly adopt the subscript m. The target mask constraint term is represented; the historical pose consistency loss in equation (19) is based on the predicted pose of the current frame and does not directly constrain the pose of the current frame to be equal to the pose of a certain historical frame. and Can be followed Increase and increase, Can be followed The values ​​are reduced and increased to enhance reliable historical constraints when shading is heavy.

[0119] Example 1: An occlusion-robust adaptive cross-modal 6D pose estimation method, such as... Figure 2 As shown, the specific steps are as follows:

[0120] Step 1: Image Acquisition and Preprocessing (i.e.) Figure 2 Step 11):

[0121] An RGB-D camera is used to acquire RGB-D video sequences of the target object. The RGB images are denoised, brightness normalized, and the target region is cropped. The depth map is subjected to hole filling, depth thresholding, and outlier removal. A target mask is obtained using a semantic segmentation network or an object detection network, and the target RGB sub-image, target depth map, and effective depth point set are generated using the target mask. The target mask can be determined by instance segmentation, semantic segmentation, or a combination of object detection boxes and depth thresholding. For the depth map, the effective depth pixels are back-projected into a 3D point cloud based on camera intrinsics, and outliers exceeding the target depth range are removed. For pixels near the target boundary, mask dilation or erosion operations can be combined to retain more potentially visible areas, reducing over-cropping of occlusion boundaries.

[0122] The image acquisition and preprocessing module 51 can also generate statistics for reliability assessment, such as the ratio of the number of effective depth points to the number of pixels in the target region, the ratio of the target mask area to the detection box area, the mean image gradient, the keypoint response intensity, and the point cloud neighborhood density. These statistics can be used as inputs for subsequent modality weights, occlusion perception factors, and matching confidence.

[0123] Step 2, Adaptive cross-modal feature fusion (i.e.) Figure 2 Step 12):

[0124] In a specific embodiment, the RGB feature extraction branch 21 adopts a PSPNet backbone network 31 containing a multi-scale pooling structure, and introduces a GCN graph convolution module 34 after the high-level semantic features; the GCN graph structure is constructed according to equation (1). The point cloud feature extraction branch 22 converts the target depth map into a point cloud, and extracts the point cloud geometric features using random sampling, neighborhood search, and multi-scale aggregation. During the RGB feature extraction process, the PSPNet structure is used to obtain multi-scale semantic context information, and the GCN graph convolution module 34 is used to establish neighborhood associations between pixels in the target region, so that pixels near the occlusion boundary can obtain supplementary expressions from the surrounding visible pixels. This design is beneficial to maintaining the continuity of texture features when only part of the target is visible. During the point cloud feature extraction process, the target point cloud can be sampled, neighborhood searched, and local feature aggregated to extract local geometric structures, distance relationships between points, and global shape context. The geometric features output by the point cloud branch and the texture features output by the RGB branch are aligned on the same target region or corresponding point set, providing a basis for subsequent adaptive fusion. Further, the modal confidence calculation unit 23 divides the target region into Each local image region is analyzed, and the corresponding local point cloud region is determined based on the target mask, camera intrinsic parameters, and effective depth point set. For the first local image region... For each local region, calculate the local RGB modal confidence and local point cloud modal confidence according to Equation (2), obtain the local modal fusion weight according to Equation (3), and perform weighted fusion on the local RGB texture features and local point cloud geometric features after spatial alignment and dimensional mapping according to Equation (4). Each local cross-modal fusion feature is formed by splicing, pooling or attention aggregation, and is used as the input for determining the spatial correspondence in step 3.

[0125] When the When the RGB segmentation confidence of a local image region is low, the texture clarity is reduced, or the keypoint matching confidence is lowered, the system determines that the reliability of the local RGB modality has decreased, and accordingly increases the fusion weight of the corresponding local point cloud geometric features. When the proportion of effective depth points in the corresponding local point cloud region decreases, the point cloud integrity is reduced, or the point cloud matching confidence is insufficient, the system determines that the reliability of the local point cloud modality has decreased, and accordingly increases the fusion weight of the local RGB texture features. The local fusion results, after aggregation, are used both for spatial correspondence matching between the current frame and historical frames, and as fusion features recorded in historical frames in the dynamic pose pool, so that modal reliability information continues to act on steps 3 and 4 through fusion features and historical frame weights.

[0126] Step 3: Relative pose calculation and initial absolute pose estimation (i.e.) Figure 2 Step 13 in the middle).

[0127] Based on the cross-modal fusion features obtained through local adaptive fusion and aggregation, keypoint matching, dense matching, or feature association networks are used to determine the spatial correspondence between the current frame and historical frames. Three-dimensional points are obtained by backprojecting depth values ​​onto two-dimensional matching points, and valid correspondences are selected based on the spatial distribution of matching points, matching confidence, reprojection error, and the proportion of RANSAC inliers. For continuously tracked frames, the relative poses of the selected three-dimensional corresponding points are solved using SVD registration, the Umeyama algorithm, PnP, RANSAC, ICP, or a combination thereof. In one embodiment, the Umeyama relative pose is calculated according to equations (5) to (8), and the initial absolute pose of the current frame is inferred by combining the absolute poses of historical frames with equation (9). For the first frame or a re-initialized frame, equation (9) is not used for recursion; instead, the initial absolute pose is obtained based on the current frame observation information through keypoint PnP, template matching, pre-trained pose networks, manual calibration, or an external localization module. During the matching process, correspondences can be selected based on the spatial distribution of matching points, matching confidence, reprojection error, and the proportion of RANSAC inliers. For 2D matching points, depth values ​​can be back-projected to 3D points; for 2D points lacking depth, they can be retained as 2D-3D constraints or their weight in 3D registration can be reduced. Through the above processing, RGB correspondence can still be used to solve pose even when some depth is missing.

[0128] Step 4, Dynamic attitude pool optimization (i.e.) Figure 2 Step 14):

[0129] A dynamic pose pool is set up, whose recorded items include historical pose, historical point cloud or fusion features, historical frame matching confidence, reprojection error, registration residual, historical frame reliability score and time index. After receiving the initial 6D pose, spatial correspondence and matching confidence of the current frame output in step 3, the reliability score of the current frame is calculated first based on the matching confidence, reprojection error, registration residual and pose continuity; when the reliability score meets the preset threshold, it is written into the dynamic pose pool; when the reliability score is lower than the preset threshold, it is not written into the dynamic pose pool, and the initial pose of the current frame is corrected by the temporal consistency constraint of reliable historical frames; when the number of consecutive low reliable frames reaches the preset reinitialization threshold, reinitialization is triggered. The capacity of the dynamic pose pool is determined according to Equations (10) and (11). For the first time participating in the temporal optimization For each historical frame, a reliability score is determined based on its matching confidence, reprojection error, registration residual, and pose continuity. The kinematic bias is determined based on the deviation between the relative pose of the historical frame and the predicted relative pose estimated from the most recent reliable historical frame. The encoded features of the initial pose of the current frame are used as the query vector, and the encoded features of the pose of the historical frame, the fused features, and the point cloud features are used as the key vector and the value vector, respectively. The motion trend of the historical frame is first extracted using self-attention, and then the correlation between the current frame and the historical frame is calculated using cross-attention. The attention score and participation weight are calculated according to Equation (13) to simultaneously consider feature similarity, historical frame reliability, and kinematic consistency. The rotation residual and translation residual are predicted from the temporal aggregated features, and the optimized pose parameters are obtained according to Equation (12).

[0130] Step 5: Occlusion-aware registration optimization and final output (i.e.) Figure 2 Step 15 in the middle).

[0131] Calculate the occlusion perception factor according to equation (14) And the optimized pose parameters output in step 4 As the initial pose, for each corresponding point in space, its visibility state is first determined based on the target mask, and then the point-level registration weights are determined based on depth validity, color consistency, local depth continuity, and matching confidence. The occlusion perception factor simultaneously adjusts the point-level registration weights and the weighted geometric registration loss in equations (15) to (19). Weighted color registration loss Occlusion constraint loss Loss of consistency with historical pose The weights are calculated. The final 6D pose parameters are output after iterative optimization. When the occlusion perception factor is large, it indicates that the visible area of ​​the target is sufficient, and the geometric error and color error of the visible, depth continuous, and color consistent areas can be appropriately increased to constrain the target. When the occlusion perception factor is small, it indicates that the target is occluded to a high degree. At this time, the point-level registration weights of the occlusion boundary and abnormal points are reduced, and the relative weights of the reliable historical pose consistency constraint are increased. This avoids overfitting a small number of incorrect matching points during the optimization process. Among them, the point-level registration weights can be determined according to Equation (20). The historical pose consistency loss preferably constrains the difference between the current frame registration pose and the current frame predicted pose obtained based on the reliable historical frame motion model, thereby avoiding excessive constraints on the moving target.

[0132] Example 2: An occlusion-robust adaptive cross-modal 6D pose estimation system, such as Figure 6 As shown, it includes:

[0133] (1) Image acquisition and preprocessing module 51: used to acquire RGB-D image sequences, perform denoising, normalization, detection and segmentation, depth filtering, 3D back projection and point cloud screening on RGB images and depth maps, and output target RGB sub-image, target depth map, target mask and effective depth point set; at the same time, output quality information and measurement information such as target mask integrity, effective depth point ratio, texture clarity and segmentation confidence, and provide input for modal reliability calculation and occlusion perception judgment. (2) Adaptive cross-modal feature fusion module 52: used to extract RGB texture features and point cloud geometric features respectively, divide the target area into local image areas and determine the corresponding local point cloud areas, calculate local RGB modal confidence, local point cloud modal confidence and local modal fusion weight, and output the cross-modal fusion features obtained by aggregation. This module can dynamically adjust the contribution of the two types of local features according to the current frame RGB quality and depth quality, and output the fusion result to the relative pose calculation module 53 and the dynamic pose pool optimization module 54. (3) Relative pose calculation module 53: used to determine the candidate spatial correspondence between the current frame and the historical frame based on the cross-modal fusion features, filter the effective correspondence and output the spatial correspondence and matching confidence; for continuously tracked frames, the relative pose and initial 6D pose are calculated by relative pose recursion between adjacent frames, SVD registration, PnP, RANSAC, ICP or their combination; for the first frame or re-initialized frame, the initial absolute pose is obtained based on the observation information of the current frame. (4) Dynamic pose pool optimization module 54: used to store the pose, cross-modal fusion features, point cloud features, matching confidence, reprojection error, registration residual and time index of reliable historical frames, and update the dynamic pose pool according to the reliability score of the current frame; this module determines the attention participation weight based on the reliability score of the historical frames and kinematic deviation, calculates the pose correction amount of the current frame through self-attention and cross-attention, and suppresses pose jumps that are inconsistent with the predicted motion trend by kinematic constraints. (5) Occlusion-aware registration optimization module 55: It is used to calculate the occlusion perception factor based on the target visibility ratio, effective depth point ratio and target mask integrity, and uses the optimized pose parameters output by the dynamic pose pool optimization module 54 as the initial pose. It determines the point-level registration weight based on the visibility state, depth effectiveness, color consistency, local depth continuity and matching confidence of the corresponding points in space. This module adjusts the point-level registration weight and the loss weights of geometric error, color error, occlusion error and historical pose consistency error with the occlusion perception factor to improve the robustness of the final registration result. (6) Storage module 56: It is used to store historical poses, point cloud features, matching confidence, reprojection error, registration residuals, model parameters and pose trajectory records, and provides historical data support for the relative pose calculation module 53, the dynamic pose pool optimization module 54 and the occlusion-aware registration optimization module 55.(7) Model Training Module 57: Used to train the feature extraction network, modal weight calculation network, and dynamic pose pooling optimization network; training samples can include occlusion simulation, illumination perturbation, depth missing, point cloud outliers, motion blur, and pose perturbation to improve the robustness of the model in complex scenes. During training, ablation experiments can be used to verify the contributions of adaptive cross-modal fusion, dynamic pose pooling optimization, and occlusion-aware registration optimization to accuracy and stability.

[0134] Experimental verification:

[0135] To further verify the effectiveness of this invention in occluded scenarios, the training samples, test samples, occlusion degree division, repeated experiment methods, and evaluation metrics were uniformly set in the experimental verification. In one experimental example, an RGB-D image sequence containing occluded targets was selected as the experimental data. Approximately 70% of the samples were used as the training set, approximately 10% as the validation set, and approximately 20% as the test set. All control groups used the same number of training samples, test samples, input resolution, batch size, training epochs, learning rate strategy, data augmentation methods, and evaluation scripts to ensure the objectivity of the comparison results. To reduce the impact of random initialization and sample partitioning on the experimental results, each group of experiments was repeatedly trained at least three times under the same hardware environment, using a fixed random seed or multiple random seeds for training. The final result is expressed as the average of multiple experiments, with the standard deviation provided when necessary.

[0136] The test samples were categorized by occlusion level based on the target visibility ratio or the integrity of the target mask. For example, when the target visibility ratio was not less than 70%, the test samples were classified as lightly occluded; when the target visibility ratio was greater than 40% but less than 70%, the test samples were classified as moderately occluded; and when the target visibility ratio was not greater than 40%, the test samples were classified as heavily occluded. For each occlusion level group, the pose estimation results of the baseline method, the method with adaptive cross-modal fusion, the method with dynamic pose pooling optimization, and the complete method were statistically analyzed to observe the contribution of different modules under different occlusion intensities. Evaluation metrics, in addition to ADD(S) < 0.1d accuracy, included rotation error, translation error, 2D reprojection error, registration residual, pose jump variables in consecutive frames, and average inference time. Among them, rotation error is used to measure the angular deviation between the estimated rotation matrix and the true rotation matrix; translation error is used to measure the Euclidean distance between the estimated translation vector and the true translation vector; 2D reprojection error is used to measure the pixel distance between the model point or reference point after the estimated pose projection and the true projection position; registration residual is used to measure the weighted error between corresponding points in the effective space after color-geometric joint registration; continuous frame pose jump variable is used to measure the smoothness of pose change between adjacent frames; and average inference time is used to measure the running efficiency of a single frame from input preprocessing to final pose output.

[0137] Through the above experimental setup, the functions of each module of the present invention can be verified respectively: the adaptive cross-modal feature fusion module 52 is mainly used to improve the quality of spatial correspondence when RGB information is degraded or depth information is missing; the dynamic pose pool optimization module 54 is mainly used to reduce pose jumps and cumulative drift in consecutive frames; the occlusion-aware registration optimization module 55 is mainly used to reduce the interference of occlusion boundary points, depth anomalies, and low-confidence matching points on the final pose. If the complete method outperforms the baseline method in terms of ADD(S) < 0.1d accuracy, rotation error, translation error, reprojection error, and consecutive frame pose jump variables on moderately and heavily occluded samples, it can be shown that the occlusion-aware factor and the dynamic pose pool make positive contributions to the pose estimation accuracy and temporal stability in complex degradation scenarios.

[0138] Considering that the main improvements of this invention focus on adaptive cross-modal fusion, dynamic pose pool optimization, and occlusion-aware registration optimization, a control group as shown in Table 1 can be set up in the experimental design. This setup does not require introducing too many experimental combinations, but it can observe the impact of each module on the accuracy and stability of pose estimation in occluded scenarios. The results in Table 1 are obtained from one experimental example. To facilitate objective comparison, each group of experiments was conducted under the same hardware environment, the same deep learning framework, the same number of training samples and test samples, the same evaluation script, and the same RGB-D input preprocessing conditions. During training, each group maintained the same image resolution, batch size, training epochs, learning rate strategy, and data augmentation method, only enabling or disabling the corresponding modules to observe their effects. Group A reflects the basic performance of fixed RGB-D fusion and single-frame estimation in occluded scenarios; Group B adds adaptive cross-modal fusion to Group A to observe the improvement effect of modal reliability weights on feature representation; Group C further adds dynamic pose pool optimization to observe the impact of historical multi-frame consistency on pose stability; Group D is the complete method to demonstrate the improvement effect of occlusion-aware color-geometric joint registration on final pose refinement.

[0139] Table 1 Ablation Experiment Record Sheet

[0140] In one experimental example, occluded scene images from the Occlusion-LineMod dataset were selected as test samples. Each control group used the same number of training samples, test samples, and occluded scene segmentation, and primarily used ADD(S) < 0.1d as the evaluation metric. To observe the impact of different modules on pose estimation results, a baseline method, a method incorporating adaptive cross-modal fusion, a method further incorporating dynamic pose pooling optimization, and the complete method were compared. Table 1 lists the control experiment settings and evaluation results used in this embodiment, where each group of experiments was conducted under the same test samples and the same evaluation metrics.

[0141] The NOCS dataset can serve as a supplementary validation dataset for class-level pose estimation. Evaluation metrics can include the 5° / 5cm pose evaluation metric, meaning that when the rotation error is no greater than 5° and the translation error is no greater than 5cm, the pose estimation for that frame is considered correct, and the proportion of correct frames, rotation error, and translation error are statistically analyzed. Since different published methods differ in input conditions, initialization methods, and evaluation scripts, this embodiment does not directly use results from different calibers as a quantitative comparison of the method of this invention.

[0142] As can be seen from the above experimental design, this invention does not rely on a single module to improve accuracy, but improves the pose estimation process step by step through three core modules: first, it improves the reliability of input features; second, it improves the stability of pose in consecutive frames; and finally, it improves the anti-interference ability of the registration optimization stage against occlusion and anomalies.

[0143] Furthermore, in the adaptive cross-modal fusion mechanism, this invention no longer concatenates RGB texture features and point cloud geometric features in a fixed ratio. Instead, it evaluates the reliability of each modality in the current frame from multiple dimensions, including RGB segmentation confidence, texture clarity, keypoint matching confidence, effective depth point ratio, point cloud completeness, and point cloud matching confidence, and generates modal fusion weights accordingly. This mechanism allows for greater reliance on point cloud geometric features when RGB information degrades, and greater reliance on RGB texture features when depth information degrades, making the fused features more adaptable to occlusion, weak texture, reflection, and depth holes. In the dynamic pose pool and kinematically constrained temporal attention optimization mechanism, the poses, point cloud features, matching confidence, and registration residuals of the most recent multiple frames are jointly stored in the dynamic pose pool. The size of the pose pool is adaptively adjusted based on the target's motion speed, target visibility ratio, effective depth point ratio, matching confidence, and historical pose consistency. Through self-attention and cross-attention mechanisms, the initial pose of the current frame interacts with historical stable frames. Furthermore, the kinematic deviation between the relative pose of historical frames and the predicted relative pose is embedded in the attention score, reducing the weight of historical frames that do not conform to the predicted motion trend. When the observation of the current frame is reliable, its role is enhanced; when the current frame is occluded or the matching confidence decreases, the constraints of reliable and kinematically consistent historical frames are strengthened, thereby suppressing pose jumps and cumulative drift. In the occlusion-aware color-geometric joint registration mechanism, the occlusion perception factor is calculated based on the target visibility ratio, the effective depth point ratio, and the target mask integrity. The point-level registration weights are determined by combining the occlusion state of corresponding spatial points, depth effectiveness, color consistency, local depth continuity, and matching confidence. The occlusion perception factor is also used to adjust the point-level registration weights, as well as the loss weights for geometric error, color error, occlusion constraint error, and historical pose consistency error. When the overall visibility of the target decreases, the impact of occlusion boundaries, depth anomalies, and low-confidence matching points on color and geometric errors is reduced, and the continuity constraint of reliable historical frames on the current frame pose optimization is enhanced, so that occlusion state judgment, historical temporal information, and color-geometric registration work together in the same optimization process.

[0144] According to another embodiment of the present invention, an occlusion-robust adaptive cross-modal 6D pose estimation system is also provided, the system comprising:

[0145] The image acquisition and preprocessing module 51 is used to obtain the observation quality indicators of the current frame; wherein, the observation quality indicators include the target RGB image sub-image, the target depth map, the target mask, the effective depth point set, the target visible ratio, the effective depth point ratio, and the target mask integrity.

[0146] The adaptive cross-modal feature fusion module 52 is used to extract RGB texture features and point cloud geometric features based on the target RGB image sub-image and effective depth point set in the observation quality index, and perform weighted fusion to obtain cross-modal fusion features;

[0147] The relative pose calculation module 53 is used to determine the effective spatial correspondence set between the current frame and the historical frames based on cross-modal fusion features, and to obtain the initial 6D pose of the current frame in combination with the tracking state of the current frame.

[0148] The dynamic pose pool optimization module 54 is used to adjust the capacity of the preset dynamic pose pool according to the tracking state of the current frame, calculate the participation weight of each historical frame based on the adjustment result, and use the participation weight to generate rotation residual and translation residual to correct the initial 6D pose of the current frame and obtain the optimized pose parameters.

[0149] The occlusion-aware registration optimization module 55 is used to calculate the occlusion-aware factor and point-level registration weights, and to iteratively update the optimized pose parameters with the pre-constructed registration objective function as the target, so as to output the final 6D pose parameters to achieve adaptive cross-modal 6D pose estimation.

[0150] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A robust-to-occlusion adaptive cross-modality 6D pose estimation method, characterized in that, The method includes: S1. Obtain the observation quality index of the current frame; wherein, the observation quality index includes the target RGB image sub-image, target depth map, target mask, effective depth point set, target visibility ratio, effective depth point ratio and target mask integrity; S2. Based on the target RGB image sub-image and effective depth point set in the observation quality index, extract RGB texture features and point cloud geometric features and perform weighted fusion to obtain cross-modal fusion features; S3. Based on cross-modal fusion features, determine the effective spatial correspondence set between the current frame and historical frames, and combine the tracking state of the current frame to obtain the initial 6D pose of the current frame; S4. Based on the tracking state of the current frame, adjust the capacity of the preset dynamic pose pool, calculate the participation weight of each historical frame based on the adjustment result, and use the participation weight to generate rotation residual and translation residual to correct the initial 6D pose of the current frame and obtain the optimized pose parameters. S5. Calculate the occlusion perception factor and point-level registration weight, and iteratively update the optimized pose parameters with the pre-constructed registration objective function as the objective, and output the final 6D pose parameters to achieve adaptive cross-modal 6D pose estimation. S3 includes: S31. Utilize cross-modal fusion features to determine the spatial correspondence between the current frame and historical frames, and filter the spatial correspondence according to the filtering criteria to obtain a set of valid spatial correspondences; wherein, the filtering criteria include at least one of matching confidence, depth validity, spatial distribution uniformity, reprojection error, or random sampling consistency inlier ratio; S32. Obtain the initial 6D pose of the current frame based on the effective spatial correspondence set and the tracking state of the current frame; wherein, the tracking state includes the continuous tracking frame state, the first frame state, and the re-initialization frame state; S32 includes: When the current frame is in the continuous tracking frame state, the relative rotation and relative translation from the historical frame to the current frame are calculated using the effective spatial correspondence set as input, the relative pose of the adjacent frame is generated, and the relative pose of the adjacent frame is compounded and recursively combined with the pre-acquired absolute pose of the historical frame to obtain the initial 6D pose of the current frame. When the current frame is in the first frame state or the reinitialized frame state, the initial absolute pose of the current frame is calculated based on the observation information of the current frame, and the initial absolute pose is used as the initial 6D pose. S4 includes: S41. Update the preset dynamic attitude pool according to the tracking state of the current frame; S42. Calculate the capacity adjustment factor of the dynamic attitude pool, and determine the current capacity of the dynamic attitude pool by combining the preset lower capacity limit and upper capacity limit, and adjust the capacity of the updated dynamic attitude pool based on the current capacity. S43. Based on the historical frames in the dynamic attitude pool after capacity adjustment, the motion trend features of each historical frame are extracted using a self-attention mechanism, and the attention score of each historical frame is calculated through cross-attention. After normalizing the attention scores, the participation weight of each historical frame is obtained; wherein, the expression for the attention score of the historical frame is: ； The expression for the participation weight of the historical frame is: ； In the formula, For the first Attention score for each historical frame; The initial pose lookup vector for the current frame; Indicates transpose; For the first Each historical frame key vector; For query vector dimensions; Weights for reliability items; For the first Reliability score for each historical frame; For logarithmic terms, the numerical stability constant is used. The weights of the kinematic constraint terms; For the first Kinematic bias in one historical frame; For the first The participation weight of each historical frame; Normalized summation index for historical frames; For the historical frame set that participated in timing optimization; No. Attention score for each historical frame; S44. Use participation weights to perform weighted aggregation of historical frames in the dynamic pose pool to generate temporal aggregation features. Based on the temporal aggregation features, predict rotation residuals and translation residuals. Then, use the rotation residuals and translation residuals to correct the initial 6D pose and obtain the optimized pose parameters.

2. The occlusion-robust adaptive cross-modal 6D pose estimation method according to claim 1, characterized in that, S1 includes: S11. Preprocess the acquired video sequence and output the preprocessed RGB image and preprocessed depth map of the current frame; S12. Process the preprocessed RGB image and preprocessed depth map to generate the initial target mask, the target RGB image sub-image, and the target depth map; S13. Extract the boundary pixels of the initial target mask, and perform pixel-level correction on the boundary region of the initial target mask based on the depth continuity of the boundary pixels in the target depth map and the mask state of the adjacent pixels, and output the target mask. S14. Traverse each pixel within the target area and filter out pixels that simultaneously satisfy the conditions of belonging to the target mask and having a depth value within the preset effective depth range, and use them as effective depth pixels. S15. Using the preset camera intrinsic parameter matrix, back-project the effective depth pixels to three-dimensional space to obtain the effective depth point set of the target object. S16. Based on the target RGB image sub-image, target mask, and effective depth point set, calculate the target visibility ratio, effective depth point ratio, and target mask integrity.

3. The occlusion-robust adaptive cross-modal 6D pose estimation method according to claim 1, characterized in that, S2 includes: S21. Input the target RGB image sub-image and the effective depth point set into the preset RGB feature extraction branch and point cloud feature extraction branch respectively, output RGB texture features and point cloud geometric features, and perform spatial alignment on the RGB texture features and point cloud geometric features. S22. Divide the target region into multiple local regions, and calculate the local RGB modal fusion weight and local point cloud modal fusion weight by combining modal reliability information; wherein, the modal reliability information includes at least one of RGB segmentation confidence, texture clarity, RGB matching confidence, effective depth point ratio, point cloud completeness, or point cloud matching confidence. S23. Based on the local RGB modal fusion weights, local point cloud modal fusion weights, spatially aligned RGB texture features and point cloud geometric features, generate local cross-modal fusion features, and fuse all local cross-modal fusion features to obtain cross-modal fusion features.

4. The occlusion-robust adaptive cross-modal 6D pose estimation method according to claim 3, characterized in that, The expression for the local RGB modality confidence is: ； The expression for the local point cloud modal confidence is: ； The expression for the local RGB modal fusion weights is: ； The expression for the local point cloud modal fusion weights is: ； In the formula, For local RGB modal confidence; These are non-negative weighting coefficients; For the first The average RGB segmentation confidence score within a local image region; These are non-negative weighting coefficients; To normalize texture sharpness; These are non-negative weighting coefficients; For the first The average confidence level of valid RGB matching points within a local image region; For local point cloud mode confidence; These are non-negative weighting coefficients; For the first The ratio of the number of effective depth pixels to the number of target pixels within a local image region; These are non-negative weighting coefficients; For point cloud integrity; These are non-negative weighting coefficients; For the first The average confidence level of matching points within a local point cloud region; For local RGB modal fusion weights; The modal weight is the positive smoothing coefficient; Weights for local point cloud modal fusion.

5. The occlusion-robust adaptive cross-modal 6D pose estimation method according to claim 1, characterized in that, S41 includes: When the current frame is in the first frame state or the reinitialized frame state, the initial 6D pose, cross-modal fusion features, local RGB modal confidence, local point cloud modal confidence, reprojection error, registration residual and target visibility ratio of the current frame are written into the preset dynamic pose pool as the first historical frame record, and the updated dynamic pose pool is obtained. When the current frame is in the continuous tracking frame state, the reliability score of the current frame is calculated based on the local RGB modal confidence, local point cloud modal confidence, reprojection error, registration residual, and pose continuity. The reliability score is then compared with a preset threshold. If the reliability score is not lower than the preset threshold, the data corresponding to the current frame is written into the dynamic attitude pool to obtain the updated dynamic attitude pool. If the reliability score is lower than the preset threshold, the current frame is marked as a target reliable frame. If the number of consecutive target reliable frames reaches the preset reinitialization threshold, the reinitialization process is triggered to obtain the updated dynamic attitude pool.

6. The occlusion-robust adaptive cross-modal 6D pose estimation method according to claim 1, characterized in that, S5 includes: S51. Based on the target visibility ratio, effective depth point ratio, and target mask integrity, the occlusion perception factor is calculated; wherein, the expression for the occlusion perception factor is: ； In the formula, To block the perception factor; These are non-negative weighting coefficients; This is the ratio of the number of visible target points to the total number of target reference points. These are non-negative weighting coefficients; This is the ratio of the number of effective depth pixels within the target area to the number of target pixels. These are non-negative weighting coefficients; This is the ratio of the current target mask area to the target's expected projected area. S52. Based on the spatial correspondence, target mask and target depth map, determine whether each pair of spatial correspondence points between the current frame and the historical frame is located within the visible area of ​​the target mask, and whether the depth value in the target depth map meets the preset depth validity condition, and mark the spatial correspondence points that pass through at the same time as valid spatial correspondence points. S53. Set a visibility status flag for each point corresponding to the effective space, and calculate the point-level registration weight for each point corresponding to the effective space in conjunction with the occlusion perception factor; wherein, the expression for the point-level registration weight is: ； In the formula, For point-level registration weights; Indicates numerical truncation; This is the global visibility coefficient; For target mask constraints; This is a deep validity item; This is a local depth continuity term; To match confidence levels; It is a non-negative adjustment coefficient; This represents the normalized value for color difference. It is a non-negative adjustment coefficient; This represents the normalized value of the geometric residual; S54. Based on point-level registration weights, construct a registration objective function that includes a weighted geometric registration loss function, a weighted color registration loss function, an occlusion constraint loss function, and a historical pose consistency loss function. Using the registration objective function as the target, iteratively update the optimized pose parameters through a nonlinear least squares method until the preset number of iterations is reached, and output the final 6D pose parameters.

7. The occlusion-robust adaptive cross-modal 6D pose estimation method according to claim 6, characterized in that, The expression for the registration objective function is: ； The expression for the weighted geometric registration loss function is: ； The expression for the weighted color registration loss function is: ； The expression for the occlusion constraint loss function is: ； The expression for the historical pose consistency loss function is: ； In the formula, The rotation matrix to be optimized during the registration stage; The translation vector to be optimized during the registration stage; To register the objective function; To block the perception factor Adjusted weighted geometric registration loss weights; To block the perception factor; The weighted geometric registration loss function; To block the perception factor Adjusted weighted color registration loss weights; The weighted color registration loss function; To block the perception factor Adjusted occlusion constraint loss weights; The loss function is defined by occlusion constraints. To block the perception factor Adjust the weight of historical pose consistency loss; Let the historical pose consistency loss function be used. This represents the number of points corresponding to the effective space. Index of the corresponding point in the effective space; For the first Point-level registration weights for each corresponding point; For the first Visible status flags for each corresponding point; The source is a 3D point; The target is a three-dimensional point; The color values ​​of the current frame's RGB image; For camera projection functions; The color value of a historical frame or reference frame; The corresponding pixel coordinates in the reference image; For robust penalty functions; For the first Occlusion constraint residuals for each point in the effective space; For reliable historical frame indexing; A reliable set of historical frame indexes for participating in historical pose consistency constraints; For the first The historical consistency loss weight of a reliable historical frame; For Lie algebra mappings; For the first The predicted pose of the current frame is obtained by predicting the current frame using a reliable historical frame through a motion model; The pose matrix of the current frame is formed by the rotation matrix and translation vector during the registration stage.

Images