├── .gitignore ├── requirements.txt ├── README.md ├── main.py └── icp.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/* 2 | __pycache__ -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | matplotlib==3.2.1 2 | numpy==1.18.2 3 | scikit-learn==0.22.1 4 | scipy==1.4.1 -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Iterative Closest Point (ICP) Algorithm in Python 2 | An implementation of Iterative Closest Point Algorithm in Python based on *Besl, P.J. & McKay, N.D. 1992, 'A Method for Registration of 3-D Shapes', IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, IEEE Computer Society*. 3 | 4 | 5 | ## Usage 6 | The code can be run as follows: 7 | ``` 8 | git clone https://github.com/iitaakash/icp_python.git 9 | pip3 install -r requirements.txt 10 | python3 main.py 11 | ``` 12 | 13 | For using ICP on your dataset see the icp.py file. The usage is as follows: 14 | 15 | `(R, t) = IterativeClosestPoint(source_pts, target_pts, tau)` where R and t are the estimated rotation and translation using ICP between the source points and the target points. tau is the threshold to terminate the algorithm. It terminates when the change in RMSE is less than tau between two successive iterations. 16 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | from mpl_toolkits.mplot3d import Axes3D 2 | import matplotlib.pyplot as plt 3 | from scipy.spatial.transform import Rotation as Rot 4 | import numpy as np 5 | import random 6 | import time 7 | 8 | from icp import * 9 | 10 | # num random points 11 | N = 100 12 | X = np.random.rand(3,N) * 100.0 13 | 14 | # comment below for using random points 15 | X = np.loadtxt("data/bunny.txt") 16 | X = X[::50,0:3].T 17 | N = X.shape[1] 18 | 19 | # random rotation and translation 20 | t = np.random.rand(3,1) * 25.0 21 | 22 | theta = np.random.rand() * 20 23 | phi = np.random.rand() * 20 24 | psi = np.random.rand() * 20 25 | 26 | R = Rot.from_euler('zyx', [theta, phi, psi], degrees = True) 27 | 28 | R = R.as_matrix() 29 | # print("Input Rotation : \n{}".format(R)) 30 | # print("Input Translation : \n{}".format(t)) 31 | 32 | # select a subset percentage of points 33 | subset_percent = 40 34 | Ns = int(N * (subset_percent/100.0)) 35 | index = random.sample(list(np.arange(N)), Ns) 36 | P = X[:, index] 37 | 38 | # apply inverse transformation 39 | P = ApplyInvTransformation(P, R, t) 40 | 41 | # ICP algorithm 42 | start = time.time() 43 | Rr, tr, num_iter = IterativeClosestPoint(source_pts = P, target_pts = X, tau = 10e-6) 44 | end = time.time() 45 | 46 | print("Time taken for ICP : {}".format(end - start)) 47 | print("num_iterations: {}".format(num_iter)) 48 | # print("Rotation Estimated : \n{}".format(Rr)) 49 | # print("Translation Estimated : \n{}".format(tr)) 50 | 51 | # calculate error: 52 | Re, te = CalcTransErrors(R, t, Rr, tr) 53 | print("Rotational Error : {}".format(Re)) 54 | print("Translational Error : {}".format(te)) 55 | 56 | # transformed new points 57 | Np = ApplyTransformation(P, Rr, tr) 58 | 59 | 60 | # visual output 61 | fig = plt.figure() 62 | ax = fig.add_subplot(111, projection='3d') 63 | ax.scatter(X[0,:], X[1,:], X[2,:], marker='o', alpha = 0.2, label="input target points") 64 | ax.scatter(P[0,:], P[1,:], P[2,:], marker='^', label="input source points") 65 | ax.scatter(Np[0,:], Np[1,:], Np[2,:], marker='x', label="transformed source points") 66 | ax.set_xlabel('X Label') 67 | ax.set_ylabel('Y Label') 68 | ax.set_zlabel('Z Label') 69 | ax.legend() 70 | plt.show() 71 | -------------------------------------------------------------------------------- /icp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from sklearn.neighbors import KDTree 3 | 4 | 5 | def IterativeClosestPoint(source_pts, target_pts, tau=10e-6): 6 | ''' 7 | This function implements iterative closest point algorithm based 8 | on Besl, P.J. & McKay, N.D. 1992, 'A Method for Registration 9 | of 3-D Shapes', IEEE Transactions on Pattern Analysis and Machine 10 | Intelligence, vol. 14, no. 2, IEEE Computer Society. 11 | 12 | inputs: 13 | source_pts : 3 x N 14 | target_pts : 3 x M 15 | tau : threshold for convergence 16 | Its the threshold when RMSE does not change comapred to the previous 17 | RMSE the iterations terminate. 18 | 19 | outputs: 20 | R : Rotation Matrtix (3 x 3) 21 | t : translation vector (3 x 1) 22 | k : num_iterations 23 | ''' 24 | 25 | k = 0 26 | current_pts = source_pts.copy() 27 | last_rmse = 0 28 | t = np.zeros((3, 1)) 29 | R = np.eye(3, 3) 30 | 31 | # iteration loop 32 | while True: 33 | neigh_pts = FindNeighborPoints(current_pts, target_pts) 34 | (R, t) = RegisterPoints(source_pts, neigh_pts) 35 | current_pts = ApplyTransformation(source_pts, R, t) 36 | rmse = ComputeRMSE(current_pts, neigh_pts) 37 | # print("iteration : {}, rmse : {}".format(k,rmse)) 38 | 39 | if np.abs(rmse - last_rmse) < tau: 40 | break 41 | last_rmse = rmse 42 | k = k + 1 43 | 44 | return (R, t, k) 45 | 46 | 47 | # Computes the root mean square error between two data sets. 48 | # here we dont take mean, instead sum. 49 | def ComputeRMSE(p1, p2): 50 | return np.sum(np.sqrt(np.sum((p1-p2)**2, axis=0))) 51 | 52 | 53 | # applies the transformation R,t on pts 54 | def ApplyTransformation(pts, R, t): 55 | return np.dot(R, pts) + t 56 | 57 | # applies the inverse transformation of R,t on pts 58 | def ApplyInvTransformation(pts, R, t): 59 | return np.dot(R.T, pts - t) 60 | 61 | # calculate naive transformation errors 62 | def CalcTransErrors(R1, t1, R2, t2): 63 | Re = np.sum(np.abs(R1-R2)) 64 | te = np.sum(np.abs(t1-t2)) 65 | return (Re, te) 66 | 67 | 68 | # point cloud registration between points p1 and p2 69 | # with 1-1 correspondance 70 | def RegisterPoints(p1, p2): 71 | u1 = np.mean(p1, axis=1).reshape((3, 1)) 72 | u2 = np.mean(p2, axis=1).reshape((3, 1)) 73 | pp1 = p1 - u1 74 | pp2 = p2 - u2 75 | W = np.dot(pp1, pp2.T) 76 | U, _, Vh = np.linalg.svd(W) 77 | R = np.dot(U, Vh).T 78 | if np.linalg.det(R) < 0: 79 | Vh[2, :] *= -1 80 | R = np.dot(U, Vh).T 81 | t = u2 - np.dot(R, u1) 82 | return (R, t) 83 | 84 | 85 | # function to find source points neighbors in 86 | # target based on KDTree 87 | def FindNeighborPoints(source, target): 88 | n = source.shape[1] 89 | kdt = KDTree(target.T, leaf_size=30, metric='euclidean') 90 | index = kdt.query(source.T, k=1, return_distance=False).reshape((n,)) 91 | return target[:, index] 92 | --------------------------------------------------------------------------------