-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathICP.py
80 lines (71 loc) · 2.24 KB
/
ICP.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
import numpy as np
import pandas as pd
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
from numpy.random import normal
from numpy.random import multivariate_normal
from scipy.linalg import expm, sinm, cosm
from scipy.stats import multivariate_normal
from numpy.random import choice
pclX = np.loadtxt("D:/VeNKy/Pycharm Projects/Projects/venv/Mobile Robotics/pclX.txt", dtype=float)
pclY = np.loadtxt("D:/VeNKy/Pycharm Projects/Projects/venv/Mobile Robotics/pclY.txt", dtype=float)
def EstimateCorrespondences(X,Y,t,R,dmax):
c=[]
counter=0
xtemp=[]
ytemp=[]
for i in X:
xi = i.reshape(3, 1)
k= np.transpose(np.add(np.matmul(R,xi),t))
# j= np.argmin(np.sum((Y-k)**2,axis=1),axis=0)
j= np.argmin(np.linalg.norm(Y-k,axis=1),axis=0)
d= (np.sum((Y[j] -k)**2,axis=1)[0])**0.5
if d < dmax:
c.append((counter,j))
xtemp.append(i)
ytemp.append(Y[j])
counter+=1
xc=np.array(xtemp)
yc=np.array(ytemp)
return c,xc,yc
def ComputeOptimalRigidRegistration(xc,yc,C):
K=len(C)
x_cent=(np.sum(xc,axis=0)/K).reshape(1,3)
y_cent=(np.sum(yc,axis=0)/K).reshape(1,3)
xt= xc-x_cent
yt= yc-y_cent
w=np.matmul(np.transpose(yt),xt)/K
u,sigma, vt =np.linalg.svd(w)
R= np.matmul(u,vt)
# temp=(np.matmul(R,np.transpose(x_cent)))
t=np.transpose(y_cent)-(np.matmul(R,np.transpose(x_cent)))
return (t,R)
def ICP(X,Y,t,R,dmax,itr):
for i in range(0,itr):
C,xc,yc=EstimateCorrespondences(X,Y,t,R,dmax)
t,R=ComputeOptimalRigidRegistration(xc,yc,C)
return t,R,xc,yc
R=np.array([[1,0,0],[0,1,0],[0,0,1]])
t= np.array([[0],[0],[0]])
dmax=25
itr=30
t,R,xc,yc=ICP(pclX,pclY,t,R,dmax,itr)
new_X=np.matmul(R,np.transpose(pclX))+t
R=np.array([[1,0,0],[0,1,0],[0,0,1]])
t= np.array([[0],[0],[0]])
dmax=25
itr=30
t,R,xc,yc=ICP(pclX,pclY,t,R,dmax,itr)
new_X=np.matmul(R,np.transpose(pclX))+t
fig = plt.figure()
ax = fig.add_subplot(projection='3d')
X=np.transpose(pclX)
Y=np.transpose(pclY)
ax.scatter(X[0], X[1], X[2], c='blue',s=0.6)
ax.scatter(Y[0], Y[1], Y[2], c='red',s=0.6)
ax.scatter(new_X[0], new_X[1], new_X[2], c='yellow',s=0.6)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()