# Is there a quick way to speed up ICP in python using a cached KD-tree

I am currently using ICP to match 2 point clouds. These point clouds evolve in time, so I have to repeat this process many times. I am using a standard KD tree from scipy for my nearest neighbor lookup. Each point cloud has roughly 500 points each. I am looking at using a cached KD tree but I am unsure about how to turn a KD tree into a cached KD tree. Is there an implementation in Scipy? SVD in python seems fast enough for my needs at the moment, but I would also be open to any known faster SVD solvers. My current code for the ICP is:

>tree=spatial.KDTree(TimeStep1)
>qHat=TimeStep2
> for i in range(10):
>         Phat=tree.query(qHat,eps=.15,distance_upper_bound=2)
>         H = np.dot(qHat.T, TimeStep1[Phat[1]-1,:])
>         U, S, Vt = np.linalg.svd(H)
>         R = np.dot(Vt.T, U.T)
>         if np.linalg.det(R) < 0:
>             Vt[2,:] *= -1
>             R = np.dot(Vt.T, U.T)
>             Yes.append(1)
>         Eu=rotationMatrixToEulerAngles(R)
>         T=TimeStep1[Phat[1]-1,:].T-np.dot(R,qHat.T)
>         T=TimeStep1[Phat[1]-1,:]-qHat
>         T=T.mean(axis=0)
>         qHat=np.dot(R,TimeStep2.T).T+T
>         qHat=TimeStep2+T.shape