dcm.py 文件源码

def _omega_from_dtheta_matrix(theta):
    norm = np.linalg.norm(theta, axis=1)
    k1 = np.empty_like(norm)
    k2 = np.empty_like(norm)

    mask = norm > 1e-4
    nm = norm[mask]
    k1[mask] = (1 - np.cos(nm)) / nm**2
    k2[mask] = (nm - np.sin(nm)) / nm**3

    mask = ~mask
    nm = norm[mask]
    k1[mask] = 0.5 - nm**2 / 24
    k2[mask] = 1/6 - nm**2 / 120

    A = np.empty((norm.shape[0], 3, 3))
    skew = _skew_matrix_array(theta)
    A[:] = np.identity(3)
    A[:] -= k1[:, None, None] * skew
    A[:] += k2[:, None, None] * util.mm_prod(skew, skew)

    return A