dcm.py 文件源码

def _from_rotvec_array(rv):
    norm = np.linalg.norm(rv, axis=1)
    norm2 = norm ** 2
    norm4 = norm2 ** 2

    k1 = np.empty_like(norm2)
    k2 = np.empty_like(norm2)

    small = norm2 < 1e-6
    k1[small] = 1 - norm2[small] / 6 + norm4[small] / 120
    k2[small] = 0.5 - norm2[small] / 24 + norm4[small] / 720

    big = ~small
    k1[big] = np.sin(norm[big]) / norm[big]
    k2[big] = (1 - np.cos(norm[big])) / norm2[big]

    skew = _skew_matrix_array(rv)
    skew_squared = np.einsum('...ij,...jk->...ik', skew, skew)
    identity = np.empty_like(skew)
    identity[:] = np.identity(3)
    return (identity +
            k1[:, np.newaxis, np.newaxis] * skew +
            k2[:, np.newaxis, np.newaxis] * skew_squared)