복붙노트

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

PYTHON

파이썬에서 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

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