pythonでの反復近接点（ICP）実装
最近、python）でICPアルゴリズムの実装を検索しましたが、結果はありませんでした。
ウィキペディアの記事 http://en.wikipedia.org/wiki/Iterative_closest_point によると、アルゴリズムの手順は次のとおりです。
最近傍基準によってポイントを関連付けます（1つのポイントクラウド内の各ポイントについて、2番目のポイントクラウド内の最も近いポイントを見つけます）。
平均二乗コスト関数を使用して、変換パラメーター（回転と変換）を推定します（変換は、各ポイントを前の手順で見つかった一致に最もよく合わせます）。
推定されたパラメーターを使用してポイントを変換します。
反復します（ポイントを再度関連付けるなど）。
ICPは非常に便利なアルゴリズムであり、さまざまなアプリケーションで使用されています。しかし、Pythonの組み込みソリューションは見つかりませんでした。あ、ここで何か欠けている？
最後に、sklearnおよびopencvライブラリーを使用して、独自のICP実装をPythonで作成することができました。
この関数は、2つのデータセット、初期相対ポーズ推定と必要な反復回数を受け取ります。最初のデータセットを2番目のデータセットに変換する変換行列を返します。
楽しい！
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 3.7および関連する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()
ICPのもう1つの例を次に示します。 [〜＃〜] link [〜＃〜]