def nearest_neighbor(src, dst):
    """Find the nearest (Euclidean) neighbor in dst for each point in src via KNN algorithm.
    Parameters
    ----------
    src : ndarray Nx3 
        source 3D points cloud
    dst : ndarray Nx3 
        destination 3D points cloud
    Return
    ------
    dist : ndarray
        Euclidean distances of the nearest neighbor
    indices : ndarray
        dst indices of the nearest neighbor
    """

    dist, indices = NearestNeighbors(n_neighbors=1).fit(dst).kneighbors(src, return_distance=True)
    return dist.ravel(), indices.ravel()
Esempio n. 2
0
def match_pro(x, y, iters=100, threshold=1e-10):
    #print(x.shape[0],y.shape[0])
    src = np.ones((4, x.shape[1]))
    dsc = np.ones((4, y.shape[1]))
    src[:3, :] = np.copy(x)
    dsc[:3, :] = np.copy(y)
    prev_error = 0
    for i in range(iters):
        dist, indices = NearestNeighbors(n_neighbors=1).fit(
            dsc[:3, :].T).kneighbors(src[:3, :].T, return_distance=True)
        dist = dist.ravel()
        indices = indices.ravel()
        T = get_transform(src[:3, :].T, dsc[:3, indices].T)
        src = np.dot(T, src)
        error = np.sum(dist) / dist.size
        if abs(prev_error - error) < threshold:
            break
        prev_error = error
    T_final = get_transform(x.T, src[:3, :].T)
    return T_final, dist