geometry.py 文件源码

def numpy_to_transform(arr):
    from tf import transformations

    shape, rest = arr.shape[:-2], arr.shape[-2:]
    assert rest == (4,4)

    if len(shape) == 0:
        trans = transformations.translation_from_matrix(arr)
        quat = transformations.quaternion_from_matrix(arr)

        return Transform(
            translation=Vector3(*trans),
            rotation=Quaternion(*quat)
        )
    else:
        res = np.empty(shape, dtype=np.object_)
        for idx in np.ndindex(shape):
            res[idx] = Transform(
                translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
                rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
            )