ground seg and clustering based on kitti velodyne data, and a additional optimized kdtree for knn and radius nn search