Revision history [back]

I think I found the equivalent for Python, so I am leaving it here for future reference

cur_matrix = matrix.reshape(3,4)
cur_matrix_homo = np.vstack((cur_matrix, np.array([0, 0, 0, 1]))) # to homogenous coordinates

q = tf.transformations.quaternion_from_matrix(cur_matrix_homo)

p = Pose()
p.position.x = matrix[0][3]
p.position.y = matrix[1][3]
p.position.z = matrix[2][3]
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]