def downsample(cloud: o3d.geometry.PointCloud,
v_size: float,
with_trace=False):
"""
Downsample the point cloud with a voxel grid with the defined voxel size
Parameters
----------
cloud: o3d.geometry.PointCloud
Cloud to be downsampled.
v_size: float
Voxel size to downsample into.
with_trace: bool
Select recording of point cloud index before downsampling.
Returns
----------
cloud: o3d.geometry.PointCloud
Downsampled point cloud
"""
if with_trace is True:
[cloud, indices
] = cloud.voxel_down_sample_and_trace(v_size,
min_bound=[0, 0, 0],
max_bound=[1000, 1000, 1000])
return [cloud, indices]
else:
cloud = cloud.voxel_down_sample(v_size)
return cloud
def preprocess_point_cloud(
pcd: o3d.geometry.PointCloud, voxel_size: np.float64
) -> {o3d.geometry.PointCloud, o3d.pipelines.registration.Feature}:
print(":: Downsample with a voxel size %.3f." % voxel_size)
pcd_down = pcd.voxel_down_sample(voxel_size)
radius_normal = voxel_size * 2
print(":: Estimate normal with search radius %.3f." % radius_normal)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
# KDTreeSearchParamHybrid: radius - search radius, max_nn - at maximum,
# max_nn neighbours will be searched
radius_feature = voxel_size * 5
print(":: Compute FPFH feature with search radius %.3f.\n" %
radius_feature)
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature,
max_nn=100))
# print(pcd_fpfh, '\n')
return pcd_down, pcd_fpfh
def filter_ground(self, pcd: o3d.geometry.PointCloud, max_dist: float = -1, height_threshold=0.1,
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)
# height and distance filter
# 0 -> left and right | 1 -> up and down | 2 = close and far
points_of_interest = np.where((points[:, 1] < 0.3))
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: o3d.geometry.PointCloud = pcd.select_by_index(inliers)
pcd = pcd.voxel_down_sample(0.01)
return pcd
def downsample(
self, point_cloud: open3d.geometry.PointCloud
) -> open3d.geometry.PointCloud:
voxel_down_pcd = point_cloud.voxel_down_sample(voxel_size=1.7)
return voxel_down_pcd.uniform_down_sample(every_k_points=2)