def _quaternion_align(from_frame, to_frame, from_pts, to_pts):
    assert from_pts.shape[1] == to_pts.shape[1] == 3

    # Calculate the centroids and subtract
    from_c, to_c = from_pts.mean(axis=0), to_pts.mean(axis=0)
    from_ = from_pts - from_c
    to_ = to_pts - to_c

    # Compute the dot products
    S = np.dot(from_.T, to_)

    # Compute the magical N matrix
    N = np.array([[S[0, 0] + S[1, 1] + S[2, 2], 0., 0., 0.],
                  [S[1, 2] - S[2, 1], S[0, 0] - S[1, 1] - S[2, 2], 0., 0.],
                  [S[2, 0] - S[0, 2], S[0, 1] + S[1, 0],
                   -S[0, 0] + S[1, 1] - S[2, 2], 0.],
                  [S[0, 1] - S[1, 0], S[2, 0] + S[0, 2],
                   S[1, 2] + S[2, 1], -S[0, 0] - S[1, 1] + S[2, 2]]])

    # Compute the eigenvalues and eigenvectors
    # Use the eigenvector corresponding to the largest eigenvalue as the
    # unit quaternion defining the rotation
    eig_vals, eig_vecs = linalg.eigh(N, overwrite_a=True)
    which = np.argmax(eig_vals)
    if eig_vals[which] < 0:
        raise RuntimeError('No positive eigenvalues. Cannot do the alignment.')
    q = eig_vecs[:, which]

    # Write out the rotation
    trans = np.eye(4)
    trans[0, 0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]
    trans[0, 1] = 2.0 * (q[1] * q[2] - q[0] * q[3])
    trans[0, 2] = 2.0 * (q[1] * q[3] + q[0] * q[2])
    trans[1, 0] = 2.0 * (q[2] * q[1] + q[0] * q[3])
    trans[1, 1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3]
    trans[1, 2] = 2.0 * (q[2] * q[3] - q[0] * q[1])
    trans[2, 0] = 2.0 * (q[3] * q[1] - q[0] * q[2])
    trans[2, 1] = 2.0 * (q[3] * q[2] + q[0] * q[1])
    trans[2, 2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]

    # Now we need to generate a transformed translation vector
    trans[:3, 3] = to_c - np.dot(trans[:3, :3], from_c)
    del to_c, from_c

    # Test the transformation and print the results
    logger.info('    Quaternion matching (desired vs. transformed):')
    for fro, to in zip(from_pts, to_pts):
        rr = np.dot(trans[:3, :3], fro) + trans[:3, 3]
        diff = np.sqrt(np.sum((to - rr) ** 2))
        logger.info('    %7.2f %7.2f %7.2f mm <-> %7.2f %7.2f %7.2f mm '
                    '(orig : %7.2f %7.2f %7.2f mm) diff = %8.3f mm'
                    % (tuple(1000 * to) + tuple(1000 * rr) +
                       tuple(1000 * fro) + (1000 * diff,)))
        if diff > 1e-4:
            raise RuntimeError('Something is wrong: quaternion matching did '
                               'not work (see above)')
    return {'from': from_frame, 'to': to_frame, 'trans': trans}
Perform an alignment using the unit quaternions (modifies points)
dataset/ETHPy150Open mne-tools/mne-python/mne/io/ctf/trans.py/_quaternion_align