force orthogonal matrix before ICP