utils_rotations.py 文件源码

def get_roto_translation_matrix(theta, rotation_axis=np.array([1,0,0]),  translation=np.array([0, 0, 0])):

    n = np.linalg.norm(rotation_axis)
    assert not np.abs(n) < 0.001, 'rotation axis too close to zero.'
    rot = rotation_axis / n

    # rodriguez formula:
    cross_prod = np.array([[0, -rot[2], rot[1]],
                           [rot[2], 0, -rot[0]],
                           [-rot[1], rot[0], 0]])
    rot_part = np.cos(theta) * np.identity(3) + np.sin(theta) * cross_prod + np.tensordot(rot, rot, axes=0)

    # transformations parameters

    rot_transl = np.identity(4)
    rot_transl[:3, :3] = rot_part
    rot_transl[:3, 3] = translation

    return rot_transl