A 6D Object Pose Estimation Algorithm for Autonomous Docking with Improved Maximal Cliques

Accurate 6D object pose estimation is critical for autonomous docking. To address the inefficiencies and inaccuracies associated with maximal cliques-based pose estimation methods, we propose a fast 6D pose estimation algorithm that integrates feature space and space compatibility constraints. The algorithm reduces the graph size by employing Laplacian filtering to resample high-frequency signal nodes. Then, the truncated Chamfer distance derived from fusion features and spatial compatibility constraints is used to evaluate the accuracy of candidate pose alignment between source and reference point clouds, and the optimal pose transformation matrix is selected for 6D pose estimation. Finally, a point-to-plane ICP algorithm is applied to refine the 6D pose estimation for autonomous docking. Experimental results demonstrate that the proposed algorithm achieves recall rates of 94.5%, 62.2%, and 99.1% on the 3DMatch, 3DLoMatch, and KITTI datasets, respectively. On the autonomous docking dataset, the algorithm yields rotation and localization errors of 0.96° and 5.82 cm, respectively, outperforming existing methods and validating the effectiveness of our approach.