3D point cloud algorithm based on KDtree