在python上的迭代最近点(ICP)实现 [英] Iterative Closest Point (ICP) implementation on python

问题描述

最近我一直在用python搜索ICP算法的实现，但没有结果.

I have been searching for an implementation of the ICP algorithm in python lately with no result.

根据Wikipedia文章 http://en.wikipedia.org/wiki/Iterative_closest_point ，算法步骤如下:

According to wikipedia article http://en.wikipedia.org/wiki/Iterative_closest_point, the algorithm steps are:

• 通过最近的邻居标准关联点(对于一个点云中的每个点，找到第二个点云中的最近点).

• Associate points by the nearest neighbor criteria (for each point in one point cloud find the closest point in the second point cloud).

使用均方成本函数估算变换参数(旋转和平移)(变换将使每个点与上一步中找到的匹配点最佳对齐).

Estimate transformation parameters (rotation and translation) using a mean square cost function (the transform would align best each point to its match found in the previous step).

使用估计的参数转换点.

Transform the points using the estimated parameters.

迭代(重新关联点等).

Iterate (re-associate the points and so on).

嗯，我知道ICP是一种非常有用的算法，它可用于多种应用中.但是我找不到任何内置的Python解决方案.是吗，我在这里什么都没想?

Well, I know that ICP is a very useful algorithm and it is used in a variety of applications. However I could not find any built in solution in Python. Am, I missing anything here?

推荐答案

最后，我设法使用sklearn和opencv库在Python中编写了自己的ICP实现.

Finally, I managed to write my own implementation of ICP in Python, using the sklearn and opencv libraries.

该函数采用两个数据集，即初始相对姿态估计和所需的迭代次数. 它返回一个转换矩阵，该矩阵将第一个数据集转换为第二个数据集.

The function takes two datasets, an initial relative pose estimation and the desired number of iterations. It returns a transformation matrix that transforms the first dataset to the second.

享受！

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

这篇关于在python上的迭代最近点(ICP)实现的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！