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
def remove_by_z_threshold(pcd: o3d.geometry.PointCloud,
z_threshold: float) -> o3d.geometry.PointCloud:
"""Remove points that have z-value below some threshold
Arguments:
pcd: The point cloud to filter
z_threshold: Threshold for z value
Returns:
A new filtered point cloud
"""
xyz = np.asarray(pcd.points)
rgb = np.asarray(pcd.colors)
floor_mask = xyz[:, 2] > z_threshold
xyz = xyz[floor_mask]
rgb = rgb[floor_mask]
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
pcd.colors = o3d.utility.Vector3dVector(rgb)
return pcd
def set_pcd_color(pcd: o3d.geometry.PointCloud, colors: np.ndarray):
pcd.colors = o3d.utility.Vector3dVector(colors)
return pcd