Correct TOdomToMap to fit dimp1

Correct TOdomToMap to fit dimp1 (dimension of input data)
We need to do this because we could have called 'CorrectPose' which always returns a [4][4] matrix