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:

&gt;tree=spatial.KDTree(TimeStep1)
&gt;qHat=TimeStep2
&gt; for i in range(10):
&gt;         Phat=tree.query(qHat,eps=.15,distance_upper_bound=2)
&gt;         H = np.dot(qHat.T, TimeStep1[Phat[1]-1,:])
&gt;         U, S, Vt = np.linalg.svd(H)
&gt;         R = np.dot(Vt.T, U.T)
&gt;         if np.linalg.det(R) &lt; 0:
&gt;             Vt[2,:] *= -1
&gt;             R = np.dot(Vt.T, U.T)
&gt;             Yes.append(1)
&gt;         Eu=rotationMatrixToEulerAngles(R)
&gt;         T=TimeStep1[Phat[1]-1,:].T-np.dot(R,qHat.T)
&gt;         T=TimeStep1[Phat[1]-1,:]-qHat
&gt;         T=T.mean(axis=0)
&gt;         qHat=np.dot(R,TimeStep2.T).T+T
&gt;         qHat=TimeStep2+T.shape
Try using spatial.cKDTree, in my ICP implementation switching from KDTree to cKDTree 30 iterations went from ~160 seconds to ~50 seconds