implemented OrthogonalizeTranslation and cpp tests