def create_3d_visualizer(
point_cloud: o3d.geometry.PointCloud,
inliers: Sequence[int] = None,
visualizer_size: Sequence[int] = [800, 800],
plane_color: Sequence[float] = [1.0, 0.0, 0.0],
) -> o3d.visualization.Visualizer:
vis = o3d.visualization.VisualizerWithKeyCallback()
vis.create_window(width=visualizer_size[0],
height=visualizer_size[1],
left=0,
top=0)
mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1,
origin=np.array(
[0, 0, 0]))
if inliers is None:
o3d.visualization.draw_geometries([point_cloud, mesh])
vis.add_geometry(point_cloud)
vis.add_geometry(mesh)
else:
inlier_cloud = point_cloud.select_by_index(inliers)
inlier_cloud.paint_uniform_color(plane_color)
outlier_cloud = point_cloud.select_by_index(inliers, invert=True)
vis.add_geometry(inlier_cloud)
vis.add_geometry(outlier_cloud)
vis.add_geometry(mesh)
# Rotate view around target point cloud
view_control = vis.get_view_control()
view_control.set_front([0, 0, -1])
view_control.set_up([0, -1, 0])
view_control.set_zoom(1)
view_control.set_lookat(np.array(
point_cloud.points).mean(0)) # LookAt Point Cloud centroid
view_control.rotate(visualizer_size[0] / 4.0, -visualizer_size[1] / 4.0)
return vis
def removeHiddenPoints(pcd: o3d.geometry.PointCloud) -> o3d.geometry.PointCloud:
'''
Remove Hidden Points from Open3D pointcloud
'''
# TODO: choose exact parameters for removal.
diameter = np.linalg.norm(
np.asarray(pcd.get_max_bound()) - np.asarray(pcd.get_min_bound()))
camera = [0, 0, diameter]
radius = diameter * 100
_, pt_map = pcd.hidden_point_removal(camera, radius)
pcd = pcd.select_by_index(pt_map)
return pcd
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 cropScene(self, object: o3d.geometry.PointCloud,
scene: o3d.geometry.PointCloud,
radius: float):
"""
Select the points of the scene point cloud which are within a certain
distance from the object center.
After applying the ground truth pose to the object, the center of
the object 'c' is taken as a reference point. The resulting cloud
only keeps those points which are withtin a sphere centered in 'c'
with radius 'radius'.
Parameters
----------
object : o3d.geometry.PointCloud
Point cloud containing the object
scene : o3d.geometry.PointCloud
Point cloud containing the scene to be cropped
radius : float
Distance threshold
Returns
-------
cropped_scene : o3d.geometry.PointCloud
Cropped scene point cloud
"""
# Build the Kd-tree to search through the points of the scene pointcloud
scene_tree = o3d.geometry.KDTreeFlann(scene)
# Get the center of the object
c = object.get_center()
# Get the neighboring points (points of the scene which are closer than
# the radius to the object center)
[k, idx, _] = scene_tree.search_radius_vector_3d(c, radius)
if k == 0:
print('ERROR: cropping failed.')
else:
# Crop scene point cloud by selecting only the neighboring points
cropped_scene = scene.select_by_index(idx)
return cropped_scene