Added poseMsgToEigen