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()
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
