ⓢⓐⓨⓔⓕ'𝓼 𝓽𝓮𝓬𝓱 𝓫𝓵𝓸𝓰
Ground Detection and Removal from Point Clouds
### 1. Introduction ------ In our last post ([Visualize LIDAR Point Clouds for a Single Instance](http://sayef.tech/post/query-in-nuscenes-dataset-visualize-lidar-point-clouds/)) of this series, we made a small video clip of an instance using nuScenes' annotation. We also used built-in methods to render 3D pointclouds onto 2D plane i.e. matplotlib figure. We got to know how the toolkit works, as in how we can retrieve information using particular tokens. In our pointclouds visualization, we saw circular clutter in the rendering, along with some outliers. If we want to implement some kind of tracking algorithm or any data fusion task, it's a lot easier if there weren't this clutter. We are going to see how we can get rid of those. ### 2. Possible Solutions ------ As we mentioned earlier, ground detection is one of the major issues for LIDAR tracking. There are several approaches to detect and remove them. One of the naive approaches could be measuring the height of the points and assuming that points within a certain range of height are on the ground. We will also try to improve the approach by removing outliers that come along the height range assumption using principal component analysis (PCA). There would occur situations where the street is steep, resulting in the wrong assumption of the ground. However, we will implement this approach first, and improve the approach later on. ### 3. Let's Code ------ - Let's choose a file from this directory `samples/LIDAR_TOP`. We can get pointclouds using `LidarPointCloud.from_file` built-in method. ``` lidar_point_cloud = LidarPointCloud.from_file("data/nuscenes/samples/LIDAR_TOP/n015-2018-07-24-11-22-45+0800__LIDAR_TOP__1532402928698048.pcd.bin") data = np.swapaxes(lidar_point_cloud.points, 0, 1)[:,:3] print(data.shape) ``` Output: `(34720, 3)` - We can render these points as we did last time using `render_height()` method. ``` lidar_point_cloud.render_height(plt.gca()) ``` ![img](http://sayef.tech/uploads/remove-ground-from-point-clouds/plot1.png) - Let's say we don't know which column corresponds to height or conventionally z-axis. We assume that the ground points have the low in z coordinate values. We will find the variances column-wise and see which column has the lowest variance, as we saw that most of the points belong to the ground. ``` z_idx = int(np.argmin(np.var(data, axis = 0))) print(z_idx) ``` Output: `2` That means the 3rd axis is the z-axis. - Create a new column for the index, we will need that for removing or processing the column values. ``` data = np.append(data, np.arange(data.shape).reshape(-1, 1), axis=1) ``` - Now, find the mean and standard deviation of the height column. Filter out all the points which are within 1.5 standard deviations from the mean. ``` data_z = data[:,z_idx] mean = np.mean(data_z) sd = np.std(data_z) data_ground = data[(data_z < mean + 1.5 * sd) & (data_z > mean - 1.5 * sd)] data_wo_ground= np.delete(data, data_filter[:,3], axis=0)[:,:3] ``` - As we have got ground-free pointclouds, we can put it back to `lidar_point_cloud.points` and render it in a 2D plane as a matplotlib figure. ``` lidar_point_cloud.points = np.swapaxes(data_wo_ground, 0, 1) lidar_point_cloud.render_height(plt.gca()) ``` ![img](http://sayef.tech/uploads/remove-ground-from-point-clouds/plot2.png) Voila! We just removed ground points based on the assumption on height. But the approach is a little aggressive as it removed some other points which belong to some important objects which we can see comparing the previous two plots. For example, the car itself has vanished, and a big rectangular shaped object to the upper left corner of the car has also vanished. We can try tweaking the height threshold range to fix this. However, we are going to show a robust approach toward outliers detection, as in separating non-ground points that were detected as ground earlier. In other words, these outliers are not ground points and we should not remove them. - We have ground points in `data_ground`. Before processing any further, let's scale the height column based on the minimum and maximum values. ``` max_z, min_z = np.max(data_ground[:, z_idx]), np.min(data_ground[:, z_idx]) data_ground[:, z_idx] = (data_ground[:, z_idx] - min_z)/(max_z - min_z) ``` We can get back the original scale using reverse formula. Don't need to worry about that now. - Now, we will find the covariance matrix which somewhat represent the linear relationship among axes. Let's say we have `m` data points and `n = 3` axes. The shape of the covariance matrix would be `3x3`. ``` covariance = np.cov(data_ground[:, :3].T) ``` - There is a theorem that eigen-decompistion maximizes the sum of the projections onto principal axes. In simple words, eigenvalues explain the variance of each principal component. That means, if we can't see how much importance any of the axes carry in the current coordinate system, we can always transform the data points in other (arbitrary) coordinate system using eigen-decompistion where eigenvalues explain how much each principal axes carry information i.e. variability in our case. It's also known as Principal Component Analysis (PCA). ``` eigen_values, eigen_vectors = np.linalg.eig(np.matrix(covariance)) ``` - Normally, we take the `argmax` of the eigenvalues to find the first principal component (i.e. eigenvector). Since we want to remove outliers, it's better to find the last principal component which least explains the variability. ``` normal_vector = eigen_vectors[np.argmin(eigen_values)] ``` - So far, we have our `m` data points and a vector which is nothing but an axis in a different coordinate system. Let's project our `m` points to that vector. The projection will be a single value for each point, as we took only one vector. So, the output is of `m*1` shape. ``` projection = normal_vector.dot(data_ground[:, :3].T) ``` - To eliminate the outliers, we couldn't decide from 3D points which one should be removed. Since we have only one point for decision now, we can easily choose a threshold. As eigenvectors are unit vector and projection lies between -1 to 1, one can try different threshold from 0 to 1 on the absolute value of projection. In our case, I experimented a little and found 0.4 is good to go. So, we declare all the values less than that are inliers and greater than that are outliers. Think it as the distance towards regular points, which in our case ground. So it should be as close to zero to be considered as inliers. ``` ground_mask = np.abs(projection) < 0.4 data_ground = np.asarray([data_ground[index] for index, a in np.ndenumerate(ground_mask) if a == True]) ``` - Interestingly, we have only inliers in data_ground which we can filter again and again for better ground detection with a for-loop. But for now, just stick with one iteration of this process. We aren't done yet. We scaled the height column, which we must transform back to the previous scale. ``` data_ground[:, z_idx] = data_ground[:, z_idx] * (max_z - min_z) + min_z ``` - Now, remove ground points from the LIDAR data and render to see the difference. ``` world = np.delete(data, data_ground[:,3], axis=0)[:,:3] lidar_point_cloud.points = np.swapaxes(world, 0, 1) lidar_point_cloud.render_height(plt.gca()) ``` ![img](http://sayef.tech/uploads/remove-ground-from-point-clouds/plot3.png) That's it! We just improved our naive approach with the PCA outlier removal technique. If we closely look at the LIDAR pointclouds, it's clear that ground points are on a plane, we don't know the height of the plane or the angle if the road is steep. But we can find that plane based on a few assumptions. The algorithm we will be approaching now is called random sample consensus, abbreviated RANSAC. The key idea is very simple. For line detection, two points are randomly chosen and scored based on some model and check if how many inliers are there. ![img](https://scikit-image.org/docs/stable/_images/sphx_glr_plot_ransac_001.png) *Figure: RANSAC line fitting example. Source: [sklearn](https://scikit-image.org/docs/stable/auto_examples/transform/plot_ransac.html)* In the example plot, it's noticeable that the blue line is a better fit than the black one. We do this over and over by sampling randomly and check the conditions if they apply, save the best result. Likewise, for the plane fitting, we can sample 3 or more points from the data since a plane can be made of a minimum of 3 points. The scoring model I am going to use here is almost like PCA, which I am not going to explain here because it's out of the scope of this article. The concept is well explained [here](http://www.cs.tau.ac.il/~turkel/imagepapers/RANSAC4Dummies.pdf). You can have a visit there. - Let's write utility functions for our RANSAC algorithm and feed our pointclouds. RANSAC code courtesy: [falcondai/py-ransac](https://github.com/falcondai/py-ransac). ``` def ransac(data, model, is_inlier, sample_size, max_iterations, goal_inliers, random_seed=75, debug=False): best_ic = 0 best_model = None random.seed(random_seed) data = list(data) for i in range(max_iterations): s = random.sample(data, int(sample_size)) m = model(s) ic = 0 for j in range(len(data)): if is_inlier(m, data[j]): ic += 1 if debug: print('Coeffs:', m, '# inliers:', ic) if ic > best_ic: best_ic = ic best_model = m if goal_inliers and ic > goal_inliers: break if debug: print('Took iterations:', i+1, 'Best model coeffs:', best_model, 'Inliers covered:', best_ic) return best_model, best_ic def augment(xyzs): axyz = np.ones((len(xyzs), 4)) axyz[:, :3] = xyzs return axyz def model(xyzs): axyz = augment(xyzs[:3]) return np.linalg.svd(axyz)[-1][-1, :] def is_inlier(coeffs, xyz, threshold): return np.abs(coeffs.dot(augment([xyz]).T)) < threshold ``` - Let's set the parameters of the `ransac` function and see how it works. ``` # maximum number of iterations max_iterations = 50 # number of points we want to sample, 3 is minimum, but we can choose more for better fitting. sample_size = 10 # distance threshold for choosing inliers threshold = 0.15 # minimum numbers of inliers we need to have, we can ignore this parameter by setting None goal_inliers = data.shape * 0.2 coeff, _ = ransac(data, estimate, lambda x, y: is_inlier(x, y, threshold), sample_size, max_iterations, goal_inliers) proj = data[:,0] * coeff + data[:,1] * coeff + data[:,2] * coeff + coeff pointclouds.points = np.swapaxes(data[np.abs(proj) > threshold], 0, 1) pointclouds.render_height(ax) pointclouds.points = np.swapaxes(data[np.abs(proj) <= threshold], 0, 1) pointclouds.render_height(ax) ``` ![img](http://sayef.tech/uploads/remove-ground-from-point-clouds/plot4.png) We have separated the colors of the ground and non-ground points, which seems pretty cool, right? ### 4. Conclusion ------ We have discussed and implemented 3 different approaches to detecting and removing the ground from point clouds. We started with thresholding in height range. Then we showed how to improve using the PCA outliers removal process and RANSAC plane fitting algorithm. There are plenty of other algorithms to deal with ground detection. It's still an ongoing research topic. In our next post, we will implement Kalman-filter for tracking an object, given that we can detect the instance from time to time.
SHARE THIS POST
Query in nuScenes Dataset: Visualize LIDAR Point Clouds for a Single Instance
Introduction to nuScenes: Dataset for Autonomous Driving
"Dynamic Graph CNN for Learning on Point Clouds" Simplified