问题
I have been searching for an implementation of the ICP algorithm in python lately with no result.
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).
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?
回答1:
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.
Enjoy!
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]
Call it like this:
#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:
I made an update from an existing project optimised for real-time video.
The code work for python 3.7 and the associate opencv librairie.
def icp(a, b,
max_time = 1
):
import cv2
import numpy
import copy
import pylab
import time
import sys
import sklearn.neighbors
import scipy.optimize
def res(p,src,dst):
T = numpy.matrix([[numpy.cos(p[2]),-numpy.sin(p[2]),p[0]],
[numpy.sin(p[2]), numpy.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = numpy.size(src,0)
xt = numpy.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = numpy.zeros(numpy.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]
r = numpy.sum(numpy.square(d[:,0])+numpy.square(d[:,1]))
return r
def jac(p,src,dst):
T = numpy.matrix([[numpy.cos(p[2]),-numpy.sin(p[2]),p[0]],
[numpy.sin(p[2]), numpy.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = numpy.size(src,0)
xt = numpy.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = numpy.zeros(numpy.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]
dUdth_R = numpy.matrix([[-numpy.sin(p[2]),-numpy.cos(p[2])],
[ numpy.cos(p[2]),-numpy.sin(p[2])]])
dUdth = (src*dUdth_R.T).A
g = numpy.array([ numpy.sum(2*d[:,0]),
numpy.sum(2*d[:,1]),
numpy.sum(2*(d[:,0]*dUdth[:,0]+d[:,1]*dUdth[:,1])) ])
return g
def hess(p,src,dst):
n = numpy.size(src,0)
T = numpy.matrix([[numpy.cos(p[2]),-numpy.sin(p[2]),p[0]],
[numpy.sin(p[2]), numpy.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = numpy.size(src,0)
xt = numpy.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = numpy.zeros(numpy.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]
dUdth_R = numpy.matrix([[-numpy.sin(p[2]),-numpy.cos(p[2])],[numpy.cos(p[2]),-numpy.sin(p[2])]])
dUdth = (src*dUdth_R.T).A
H = numpy.zeros([3,3])
H[0,0] = n*2
H[0,2] = numpy.sum(2*dUdth[:,0])
H[1,1] = n*2
H[1,2] = numpy.sum(2*dUdth[:,1])
H[2,0] = H[0,2]
H[2,1] = H[1,2]
d2Ud2th_R = numpy.matrix([[-numpy.cos(p[2]), numpy.sin(p[2])],[-numpy.sin(p[2]),-numpy.cos(p[2])]])
d2Ud2th = (src*d2Ud2th_R.T).A
H[2,2] = numpy.sum(2*(numpy.square(dUdth[:,0])+numpy.square(dUdth[:,1]) + d[:,0]*d2Ud2th[:,0]+d[:,0]*d2Ud2th[:,0]))
return H
t0 = time.time()
init_pose = (0,0,0)
src = numpy.array([a.T], copy=True).astype(numpy.float32)
dst = numpy.array([b.T], copy=True).astype(numpy.float32)
Tr = numpy.array([[numpy.cos(init_pose[2]),-numpy.sin(init_pose[2]),init_pose[0]],
[numpy.sin(init_pose[2]), numpy.cos(init_pose[2]),init_pose[1]],
[0, 0, 1 ]])
print("src",numpy.shape(src))
print("Tr[0:2]",numpy.shape(Tr[0:2]))
src = cv2.transform(src, Tr[0:2])
p_opt = numpy.array(init_pose)
T_opt = numpy.array([])
error_max = sys.maxsize
first = False
while not(first and time.time() - t0 > max_time):
distances, indices = sklearn.neighbors.NearestNeighbors(n_neighbors=1, algorithm='auto',p = 3).fit(dst[0]).kneighbors(src[0])
p = scipy.optimize.minimize(res,[0,0,0],args=(src[0],dst[0, indices.T][0]),method='Newton-CG',jac=jac,hess=hess).x
T = numpy.array([[numpy.cos(p[2]),-numpy.sin(p[2]),p[0]],[numpy.sin(p[2]), numpy.cos(p[2]),p[1]]])
p_opt[:2] = (p_opt[:2]*numpy.matrix(T[:2,:2]).T).A
p_opt[0] += p[0]
p_opt[1] += p[1]
p_opt[2] += p[2]
src = cv2.transform(src, T)
Tr = (numpy.matrix(numpy.vstack((T,[0,0,1])))*numpy.matrix(Tr)).A
error = res([0,0,0],src[0],dst[0, indices.T][0])
if error < error_max:
error_max = error
first = True
T_opt = Tr
p_opt[2] = p_opt[2] % (2*numpy.pi)
return T_opt, error_max
def main():
import cv2
import numpy
import random
import matplotlib.pyplot
n1 = 100
n2 = 75
bruit = 1/10
center = [random.random()*(2-1)*3,random.random()*(2-1)*3]
radius = random.random()
deformation = 2
template = numpy.array([
[numpy.cos(i*2*numpy.pi/n1)*radius*deformation for i in range(n1)],
[numpy.sin(i*2*numpy.pi/n1)*radius for i in range(n1)]
])
data = numpy.array([
[numpy.cos(i*2*numpy.pi/n2)*radius*(1+random.random()*bruit)+center[0] for i in range(n2)],
[numpy.sin(i*2*numpy.pi/n2)*radius*deformation*(1+random.random()*bruit)+center[1] for i in range(n2)]
])
T,error = icp(data,template)
dx = T[0,2]
dy = T[1,2]
rotation = numpy.arcsin(T[0,1]) * 360 / 2 / numpy.pi
print("T",T)
print("error",error)
print("rotation°",rotation)
print("dx",dx)
print("dy",dy)
result = cv2.transform(numpy.array([data.T], copy=True).astype(numpy.float32), T).T
matplotlib.pyplot.plot(template[0], template[1], label="template")
matplotlib.pyplot.plot(data[0], data[1], label="data")
matplotlib.pyplot.plot(result[0], result[1], label="result: "+str(rotation)+"° - "+str([dx,dy]))
matplotlib.pyplot.legend(loc="upper left")
matplotlib.pyplot.axis('square')
matplotlib.pyplot.show()
if __name__ == "__main__":
main()
来源：https://stackoverflow.com/questions/20120384/iterative-closest-point-icp-implementation-on-python