Implementación del punto más cercano iterativo (ICP) en python

He estado buscando una implementación del algoritmo ICP en Python últimamente sin ningún resultado.

Según el artículo de wikipedia http://en.wikipedia.org/wiki/Iterative_closest_point , los pasos del algoritmo son:

  • Asocie los puntos según el criterio del vecino más cercano (para cada punto en una nube de puntos, encuentre el punto más cercano en la segunda nube de puntos).

  • Estime los parámetros de transformación (rotación y traslación) utilizando una función de costo cuadrado promedio (la transformación alinearía mejor cada punto con su coincidencia encontrada en el paso anterior).

  • Transforma los puntos utilizando los parámetros estimados.

    • Iterar (volver a asociar los puntos y así sucesivamente).

    Bueno, sé que ICP es un algoritmo muy útil y se utiliza en una variedad de aplicaciones. Sin embargo, no pude encontrar ninguna solución integrada en Python. Soy, me falta algo aquí?

    Finalmente, logré escribir mi propia implementación de ICP en Python, usando las bibliotecas sklearn y opencv.

    La función toma dos conjuntos de datos, una estimación de postura relativa inicial y el número deseado de iteraciones. Devuelve una matriz de transformación que transforma el primer conjunto de datos en el segundo.

    ¡Disfrutar!

    import cv2 import numpy as np import matplotlib.pyplot as plt from sklearn.neighbors import NearestNeighbors def icp(a, b, init_pose=(0,0,0), no_iterations = 13): ''' The Iterative Closest Point estimator. Takes two cloudpoints a[x,y], b[x,y], an initial estimation of their relative pose and the number of iterations Returns the affine transform that transforms the cloudpoint a to the cloudpoint b. Note: (1) This method works for cloudpoints with minor transformations. Thus, the result depents greatly on the initial pose estimation. (2) A large number of iterations does not necessarily ensure convergence. Contrarily, most of the time it produces worse results. ''' src = np.array([aT], copy=True).astype(np.float32) dst = np.array([bT], copy=True).astype(np.float32) #Initialise with the initial pose estimation Tr = np.array([[np.cos(init_pose[2]),-np.sin(init_pose[2]),init_pose[0]], [np.sin(init_pose[2]), np.cos(init_pose[2]),init_pose[1]], [0, 0, 1 ]]) src = cv2.transform(src, Tr[0:2]) for i in range(no_iterations): #Find the nearest neighbours between the current source and the #destination cloudpoint nbrs = NearestNeighbors(n_neighbors=1, algorithm='auto', warn_on_equidistant=False).fit(dst[0]) distances, indices = nbrs.kneighbors(src[0]) #Compute the transformation between the current source #and destination cloudpoint T = cv2.estimateRigidTransform(src, dst[0, indices.T], False) #Transform the previous source and update the #current source cloudpoint src = cv2.transform(src, T) #Save the transformation from the actual source cloudpoint #to the destination Tr = np.dot(Tr, np.vstack((T,[0,0,1]))) return Tr[0:2] 

    Llámalo así:

     #Create the datasets ang = np.linspace(-np.pi/2, np.pi/2, 320) a = np.array([ang, np.sin(ang)]) th = np.pi/2 rot = np.array([[np.cos(th), -np.sin(th)],[np.sin(th), np.cos(th)]]) b = np.dot(rot, a) + np.array([[0.2], [0.3]]) #Run the icp M2 = icp(a, b, [0.1, 0.33, np.pi/2.2], 30) #Plot the result src = np.array([aT]).astype(np.float32) res = cv2.transform(src, M2) plt.figure() plt.plot(b[0],b[1]) plt.plot(res[0].T[0], res[0].T[1], 'r.') plt.plot(a[0], a[1]) plt.show() 

    Aquí hay un ejemplo más para ICP: ENLACE