Position and orientation estimation of a rigid body: Rigid body localization
A Procrustes Problem on the Stiefel Manifold