Calculate the density based on the KD-tree based search algorithm

Require: Cloud: the input point cloud to be searched, e.g., the data for epoch 2007. Require: r: the search radius set to be 0.5 m in our study. Step 1: kdtree.setInputCloud (Cloud); (establish the kdtree for the input cloud) Step 2: for i = 1: N (N is the number of points in the input cloud) kdtree.radiusSearch(Cloud(i), r, pointIdx, pointSquaredDistance) density(i) = pointIdxRadiusSearch.size/($\pi r^2$) End