def procrustes(anchors, X, scale=True, print_out=False):
    """ Fit X to anchors by applying optimal translation, rotation and reflection.

    Given m >= d anchor nodes (anchors in R^(m x d)), return transformation
    of coordinates X (output of EDM algorithm) optimally matching anchors in least squares sense.

    :param anchors: Matrix of shape m x d, where m is number of anchors, d is dimension of setup.
    :param X: Matrix of shape N x d, where the last m points will be used to find fit with the anchors. 
    :param scale: set to True if the point set should be scaled to match the anchors.
    
    :return: the transformed vector X, the rotation matrix, translation vector, and scaling factor.
    """
    def centralize(X):
        n = X.shape[0]
        ones = np.ones((n, 1))
        return X - np.multiply(1 / n * np.dot(ones.T, X), ones)
    m = anchors.shape[0]
    N, d = X.shape
    assert m >= d, 'Have to give at least d anchor nodes.'
    X_m = X[N - m:, :]
    ones = np.ones((m, 1))

    mux = 1 / m * np.dot(ones.T, X_m)
    muy = 1 / m * np.dot(ones.T, anchors)
    sigmax = 1 / m * np.linalg.norm(X_m - mux)**2
    sigmaxy = 1 / m * np.dot((anchors - muy).T, X_m - mux)
    try:
        U, D, VT = np.linalg.svd(sigmaxy)
    except np.LinAlgError:
        print('strange things are happening...')
        print(sigmaxy)
        print(np.linalg.matrix_rank(sigmaxy))
    #this doesn't work and doesn't seem to be necessary! (why?)
    #  S = np.eye(D.shape[0])
    #  if (np.linalg.det(U)*np.linalg.det(VT.T) < 0):
    #  print('switching')
    #  S[-1,-1] = -1.0
    #  else:
    #  print('not switching')
    #  c = np.trace(np.dot(np.diag(D),S))/sigmax
    #  R = np.dot(U, np.dot(S,VT))
    if (scale):
        c = np.trace(np.diag(D)) / sigmax
    else:
        c = np.trace(np.diag(D)) / sigmax
        if (print_out):
            print('Optimal scale would be: {}. Setting it to 1 now.'.format(c))
        c = 1.0
    R = np.dot(U, VT)
    t = muy.T - c * np.dot(R, mux.T)
    X_transformed = (c * np.dot(R, (X - mux).T) + muy.T).T
    return X_transformed, R, t, c
Fit X to anchors by applying optimal translation, rotation and reflection.

    Given m >= d anchor nodes (anchors in R^(m x d)), return transformation
    of coordinates X (output of EDM algorithm) optimally matching anchors in least squares sense.

    :param anchors: Matrix of shape m x d, where m is number of anchors, d is dimension of setup.
    :param X: Matrix of shape N x d, where the last m points will be used to find fit with the anchors. 
    :param scale: set to True if the point set should be scaled to match the anchors.
    
    :return: the transformed vector X, the rotation matrix, translation vector, and scaling factor.