Accurate motion and pose estimation algorithm for rigid objects