An Efficient Optimization Framework for 6D Pose Estimation in Terminal Vision Guidance of AUV Docking

6D pose estimation is the committed step of autonomous docking of underwater vehicles using executive guidance, and it is also an important research topic in recent years. This method provides RGB images and CAD models and calculates the pose of the object. The prediction model based on keypoints performs well in 6D object pose estimation. The method of this model is to select keypoints first, and then estimate 6D pose through keyoint estimation. Therefore, the position of selected keypoints profoundly affects the accuracy of pose estimation results. To reduce the positional errors of keypoints introduced in previous studies, we propose a new framework for using keypoints to determine pose. This framework includes a vector projection based estimation method (VPE) and a two-stage pose calculation based on reprojection to improve the accuracy of keypoint position estimation required for pose solution. The two-stage pose calculation based on reprojection first obtains the initial pose value, and then iteratively optimizes the final pose based on the reprojection distance. Compared with the original method based on RANSAC vector voting on the LINEMOD test set, the final ADD(-S) metric and 2Dprojection perform $4.2 \%$ and $3.3 \%$ better after adding our VPE.