add transformation helpers

Matrix[16] <-> Pose6d (yaw,pitch,roll,x,y,z)
from t3d/src/matrix.c and t3d/src/euler.c