def background_removal(
self, point_cloud: open3d.geometry.PointCloud
) -> open3d.geometry.PointCloud:
plane, inliers = point_cloud.segment_plane(distance_threshold=0.005,
ransac_n=4,
num_iterations=250)
return point_cloud.select_down_sample(inliers, invert=True)
def filter_ground(self,
pcd: o3d.geometry.PointCloud,
max_dist: float = 2,
height_threshold=0.5,
ransac_dist_threshold=0.01,
ransac_n=3,
ransac_itr=100) -> o3d.geometry.PointCloud:
"""
Find ground from point cloud by first selecting points that are below the (car's position + a certain threshold)
Then it will take only the points that are less than `max_dist` distance away
Then do RANSAC on the resulting point cloud.
Note that this function assumes that the ground will be the largest plane seen after filtering out everything
above the vehicle
Args:
pcd: point cloud to be parsed
max_dist: maximum distance
height_threshold: additional height padding
ransac_dist_threshold: RANSAC distance threshold
ransac_n: RANSAC starting number of points
ransac_itr: RANSAC number of iterations
Returns:
point cloud that only has the ground.
"""
points = np.asarray(pcd.points)
colors = np.asarray(pcd.colors)
points_of_interest = np.where(
(points[:, 1] < self.vehicle.transform.location.y +
height_threshold) & (points[:, 2] < max_dist))
points = points[points_of_interest]
colors = colors[points_of_interest]
pcd.points = o3d.utility.Vector3dVector(points)
pcd.colors = o3d.utility.Vector3dVector(colors)
plane_model, inliers = pcd.segment_plane(
distance_threshold=ransac_dist_threshold,
ransac_n=ransac_n,
num_iterations=ransac_itr)
pcd = pcd.select_by_index(inliers)
return pcd