[PYTHON] 파이썬에서 ICP (Iterative Closest Point) 구현

파이썬에서 ICP (Iterative Closest Point) 구현

요즘 파이썬에서 ICP 알고리즘의 구현을 찾지 못했습니다.

위키 백과 문서 http://en.wikipedia.org/wiki/Iterative_closest_point에 따르면 알고리즘 단계는 다음과 같습니다.

음, ICP는 매우 유용한 알고리즘이며 다양한 응용 분야에서 사용됩니다. 그러나 파이썬에서 어떤 솔루션도 내장되어 있지 않습니다. 내가 여기에 아무것도 없다고?

해결법

1. ==============================

   1.마지막으로, 필자는 sklearn과 opencv 라이브러리를 사용하여 파이썬에서 ICP 구현을 직접 작성했습니다.

   마지막으로, 필자는 sklearn과 opencv 라이브러리를 사용하여 파이썬에서 ICP 구현을 직접 작성했습니다.

   이 함수는 두 개의 데이터 세트, 초기 상대 포즈 추정 및 원하는 반복 횟수를 사용합니다. 첫 번째 데이터 집합을 두 번째 데이터 집합으로 변환하는 변환 행렬을 반환합니다.

   즐겨!

    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([a.T], copy=True).astype(np.float32)
       dst = np.array([b.T], 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]
   

   이것을 다음과 같이 부르십시오.

   #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([a.T]).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()
   

2. ==============================

   2.ICP에 대한 또 다른 예가 있습니다 : LINK

   ICP에 대한 또 다른 예가 있습니다 : LINK

from https://stackoverflow.com/questions/20120384/iterative-closest-point-icp-implementation-on-python by cc-by-sa and MIT license