1
$\begingroup$

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
$\endgroup$
1
$\begingroup$

Try using spatial.cKDTree, in my ICP implementation switching from KDTree to cKDTree 30 iterations went from ~160 seconds to ~50 seconds

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.