def get_pose_error(tf_1, tf_2):
    rel_tf = transform_inverse(tf_1).dot(tf_2)
    if np.allclose(np.diag(rel_tf[0:3, 0:3]), [1.0, 1.0, 1.0]):
        angle_dist = 0.0
    else:
        angle_dist = np.arccos(((np.sum(np.diag(rel_tf[0:3, 0:3])) - 1) / 2.0))
    euclid_dist = np.linalg.norm(rel_tf[0:3, 3])
    return (euclid_dist, angle_dist)
def get_pose_error(tf_1, tf_2):
    rel_tf = transform_inverse(tf_1).dot(tf_2)
    if np.allclose(np.diag(rel_tf[0:3, 0:3]), [1.0, 1.0, 1.0]):
        angle_dist = 0.0
    else:
        angle_dist = np.arccos(((np.sum(np.diag(rel_tf[0:3, 0:3])) - 1) / 2.0))
    euclid_dist = np.linalg.norm(rel_tf[0:3, 3])
    return (euclid_dist, angle_dist)