Optimal Matching of Three-Dimensional Features under Geometrical Constraints