An accurate and fast algorithm for the determination of the rigid body pose by three point position data