ICP Enhancement Algorithm for 6D Pose Tracking of Household Objects

Real-time 6D pose tracking using camera images plays an important role in enabling robots to track and grasp household items. Although iterative closest point (ICP) is frequently employed as the primary method for pose tracking owing to its good performance and low resource consumption, there are several challenges such as the difficulty in dealing with flat or symmetric objects. These challenges make the ICP prone to failure in tracking scenarios. In this study, we introduce an ICP enhancement algorithm to effectively utilize the ICP for tracking household items. Our main contribution can be summarized in three ways: usage of RGB keypoints for obtaining initial pose, refinement approach for dealing with flat or rotationally symmetric objects, and self-recovery algorithm under tracking failure. We estimate the initial pose using RGB keypoints and fine-tune the weights of the loss function to achieve an accurate pose by considering the flat and rotational symmetric properties of the object. Additionally, a recovery algorithm with the Oriented FAST and Rotated BRIEF (ORB) descriptor was employed to detect and resolve tracking failures. While the performance was relatively limited for objects with low RGB texture, our algorithm achieved stable and high accuracy when sufficient RGB texture was present. We confirmed that our algorithm achieved 100% ADDs-AUC on 16 objects in YCB Video dataset.