FIX: Normalize gives nan if norm is zero
TEST: Add test for rigid metric

Need test before refactoring metric to support non-rigid deformations too