def pcl_vis(clouds):
if isinstance(clouds, list):
for cloud in clouds:
vis = pcl.Visualizer()
vis.addPointCloud(cloud)
# if normal is not None:
# vis.addPointCloudNormals(cloud, cloud_nm)
# else:
vis.addCoordinateSystem()
vis.spin()
else:
vis = pcl.Visualizer()
vis.addPointCloud(clouds)
vis.addCoordinateSystem()
vis.spin()
def test_color_handlers(self):
viewer = pcl.Visualizer()
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer.addPointCloud(cloud, field="y", id="cloud1")
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer.addPointCloud(cloud, color=[0.8, 0.2, 0], id="cloud2")
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer.addPointCloud(
cloud,
color_handler=lambda: np.random.rand(100, 4) * 255,
id="cloud3")
cloud = np.random.rand(100, 4).astype('f4')
cloud[:, 3] *= 20 # create label fields
cloud = pcl.create_xyzl(cloud)
viewer.addPointCloud(cloud, field="label", id="cloud4")
cloud = np.random.rand(100, 6).astype('f4')
cloud[:, 3:6] *= 256 # create rgb fields
cloud = pcl.create_xyzrgb(cloud)
viewer.addPointCloud(cloud, field="rgb", id="cloud5")
cloud = np.random.rand(100, 7).astype('f4')
cloud[:, 3:7] *= 256 # create rgb fields
cloud = pcl.create_xyzrgba(cloud)
viewer.addPointCloud(cloud, field="rgba", id="cloud6")
viewer.spinOnce(time=2000)
viewer.close()
def test_add_callback(self):
def key_callback(event):
print(event)
def mouse_callback(event):
print(event)
def picking_callback(event):
print(event)
def area_callback(event):
print(event)
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer = pcl.Visualizer("Testing mouse and key events")
viewer.addPointCloud(cloud)
connection = viewer.registerKeyboardCallback(key_callback)
assert connection.connected()
connection = viewer.registerMouseCallback(mouse_callback)
assert connection.connected()
connection = viewer.registerPointPickingCallback(picking_callback)
assert connection.connected()
connection = viewer.registerAreaPickingCallback(area_callback)
assert connection.connected()
viewer.spinOnce(time=2000)
viewer.close()
def test_python_color_handler(self):
viewer = pcl.Visualizer()
cloud = pcl.PointCloud(np.random.rand(1000, 4).astype('f4'))
def rgb_handler(cloud_ref):
r = (cloud_ref.xyz[:, [0]] * 255)
g = (cloud_ref.xyz[:, [1]] * 255)
b = (cloud_ref.xyz[:, [2]] * 255)
return np.hstack([r, g, b]).astype('u1')
viewer.addPointCloud(cloud, color_handler=rgb_handler, id="cloud1")
cloud = pcl.PointCloud(np.random.rand(1000, 4).astype('f4'))
def rgba_handler(cloud_ref):
r = (cloud_ref.xyz[:, [0]] * 255)
g = (cloud_ref.xyz[:, [1]] * 255)
b = (cloud_ref.xyz[:, [2]] * 255)
a = np.full((len(cloud), 1), 200)
return np.hstack([r, g, b, a]).astype('u1')
viewer.addPointCloud(cloud, color_handler=rgba_handler, id="cloud2")
viewer.spinOnce(time=2000)
viewer.close()
def test_add_normal(self):
viewer = pcl.Visualizer()
cloud_data = np.random.rand(100, 6)
cloud_data[:, 3:] = np.clip(cloud_data[:, 3:] * 128 + 128, 0, 256)
cloud = pcl.create_xyzrgb(cloud_data)
normals = pcl.create_normal(np.random.rand(100, 4))
viewer.addPointCloud(cloud)
viewer.addPointCloudNormals(cloud, normals, level=2, scale=0.1, id="cloudn")
viewer.spinOnce(time=2000)
viewer.close()
def visualize_pcl(xyz,
rgb=None,
intensity=None,
normal=None,
filename=None,
single_batch=False,
tag=''):
"""Inputs are tensors of shape B*C*N
"""
## 1. tensor to np array
B = xyz.shape[0]
xyz_np = xyz.cpu().numpy().swapaxes(1, 2)
if rgb is not None:
rgb_np = rgb.cpu().numpy().swapaxes(1, 2) * 255
xyz_rgb = np.concatenate((xyz_np, rgb_np), axis=2)
elif intensity is not None:
intensity_np = intensity.cpu().numpy().swapaxes(1, 2)
xyz_inten = np.concatenate((xyz_np, intensity_np), axis=2)
if normal is not None:
normal_np = normal.cpu().numpy().swapaxes(1, 2)
## 2. np array to pcl cloud objects
## 3. create visualize window
for ib in range(B):
if rgb is not None:
cloud = pcl.create_xyzrgb(xyz_rgb[ib])
elif intensity is not None:
cloud = pcl.create_xyzi(xyz_inten[ib])
else:
cloud = pcl.create_xyz(xyz_np[ib])
if normal is not None:
cloud_nm = pcl.create_normal(normal_np[ib])
cloud = cloud.append_fields(cloud_nm)
# print(cloud.to_ndarray())
if filename is None:
vis = pcl.Visualizer()
if normal is not None:
vis.addPointCloudNormals(cloud, cloud_nm)
else:
vis.addPointCloud(cloud)
vis.addCoordinateSystem()
vis.spin()
else:
if single_batch:
pcl.io.save_pcd('{}{}.pcd'.format(filename, tag), cloud)
# if normal is not None:
# pcl.io.save_pcd('{}{}_normal.pcd'.format(filename, tag), cloud_nm)
else:
pcl.io.save_pcd('{}{}_{}.pcd'.format(filename, tag, ib), cloud)
def test_color_handlers(self):
viewer = pcl.Visualizer()
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer.addPointCloud(cloud, field="y", id="cloud1")
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer.addPointCloud(cloud, color=[0.8,0.2,0], id="cloud2")
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer.addPointCloud(cloud, color_handler=lambda: np.random.rand(100, 4)*255, id="cloud3")
viewer.spinOnce(time=2000)
viewer.close()
def pcl_load_viewer_fromfile(filename=None):
"""accept a .cam file generated by pcl, generate a pcl.Visualizer"""
viewer = pcl.Visualizer()
viewer.addCoordinateSystem() # optional
if filename is not None:
pos, view, focal, win_size = pcl_load_viewpoint(filename)
viewer.setCameraPosition(
pos, view, focal
) #https://github.com/PointCloudLibrary/pcl/blob/000a762bb84781a119ee0cd4ba6bdfee59325fc3/visualization/src/interactor_style.cpp
# viewer.setCameraClipDistances(0.0001,5.0)
viewer.setSize(win_size[0], win_size[1])
return viewer
def test_load_pointcloud(self):
cloud1 = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
cloud2 = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer = pcl.Visualizer()
viewer.addPointCloud(cloud1)
viewer.addCoordinateSystem()
viewer.removeCoordinateSystem()
v1 = viewer.createViewPort(0, 0, 0.5, 1)
v2 = viewer.createViewPort(0.5, 0, 1, 1)
viewer.addPointCloud(cloud1, viewport=v1, id="cloud1")
viewer.addPointCloud(cloud2, viewport=v2, id="cloud2")
viewer.spinOnce(time=1000)
viewer.close()
def test_ground_truth_visualizer_pcl(self):
idx = selection or random.randint(0, len(self.oloader))
lidar = random.choice(self.oloader.VALID_LIDAR_NAMES)
cloud = self.oloader.lidar_data(idx, lidar)
cloud = pcl.create_xyzi(cloud[:, :4])
targets = self.oloader.annotation_3dobject(idx)
calib = self.oloader.calibration_data(idx)
visualizer = pcl.Visualizer()
visualizer.addPointCloud(cloud, field="intensity")
pcl_vis(visualizer, lidar, targets, calib)
visualizer.setRepresentationToWireframeForAllActors()
visualizer.setWindowName(
"Please check whether the gt boxes are aligned!")
visualizer.spinOnce(time=5000)
visualizer.close()
def test_ground_truth_visualizer_pcl(self):
# this function is overrided since point cloud return from waymo is in vehicle frame
idx = selection or random.randint(0, len(self.oloader))
cloud = self.oloader.lidar_data(idx, "lidar_top")
cloud = pcl.create_xyzi(cloud[:, :4])
targets = self.oloader.annotation_3dobject(idx)
calib = self.oloader.calibration_data(idx)
visualizer = pcl.Visualizer()
visualizer.addPointCloud(cloud, field="intensity")
pcl_vis(visualizer, "vehicle", targets, calib)
visualizer.setRepresentationToWireframeForAllActors()
visualizer.setWindowName(
"Please check whether the gt boxes are aligned!")
visualizer.spinOnce(time=5000)
visualizer.close()
def test_add_shape(self):
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer = pcl.Visualizer()
viewer.addPointCloud(cloud)
viewer.addLine([-1,-1,-1], [1,1,1])
viewer.addArrow([1,-1,-1], [-1,1,1], line_color=[0.7,0,0], text_color=[0,3,0,0])
viewer.addSphere([0,0,0], 1)
viewer.addText("Text", -1, 1, fontsize=100)
viewer.addText3D("Text3D", [-1,1,-1], text_scale=2)
viewer.spinOnce(time=500)
viewer.updateSphere([0,0,0], 2)
viewer.spinOnce(time=500)
viewer.close()
def test_add_callback(self):
def key_callback(event):
print(event.KeyCode, event.KeySym)
def mouse_callback(event):
print(event.X, event.Y, event.Type, event.Button)
def picking_callback(event):
print(event.Point, event.PointIndex)
def area_callback(event):
print(event.PointsIndices)
cloud = pcl.PointCloud(np.random.rand(100, 4).astype('f4'))
viewer = pcl.Visualizer()
viewer.addPointCloud(cloud)
viewer.registerKeyboardCallback(key_callback)
viewer.registerMouseCallback(mouse_callback)
viewer.registerPointPickingCallback(picking_callback)
viewer.registerAreaPickingCallback(area_callback)
viewer.spinOnce(time=2000)
viewer.close()
def dataset_visualize_pcl(dataset_path: Union[str, Path],
dataset_type: str,
scene: str,
ninter_frames: int = 0):
'''
Visualize tracking dataset using PCL visualizer
:param dataset_path: path to the dataset root
:param dataset_type: type of supported datasets. Options: {kitti-raw, nuscenes, waymo}
:param scene: scene ID to be visualized
'''
import pcl
dataset_type = dataset_type.lower()
if dataset_type == "kitti-raw":
loader = KittiRawLoader(dataset_path)
elif dataset_type == "nuscenes":
loader = NuscenesLoader(dataset_path)
else:
raise ValueError("Unsupported dataset type!")
state = dict(idx=1)
vis = pcl.Visualizer()
def render_next(key):
if not (key is None or (key.KeySym == 'space' and key.keyDown())):
return
lidar_frame = loader.VALID_LIDAR_NAMES[0]
sidx = scene, state['idx']
objs = loader.annotation_3dobject(sidx)
calib = loader.calibration_data(sidx)
cloud = loader.lidar_data(sidx)[:, :4]
inter_lidar = loader.intermediate_data(sidx,
names=lidar_frame,
ninter_frames=ninter_frames)
pose = loader.pose(sidx)
for frame in inter_lidar:
lidar_ego_rt = calib.get_extrinsic(frame_from=lidar_frame)
rt = np.linalg.inv(lidar_ego_rt).dot(np.linalg.inv(pose.h**o()))\
.dot(frame.pose.h**o()).dot(lidar_ego_rt)
xyz = frame.data[:, :3].dot(rt[:3, :3].T) + rt[:3, 3]
cloud_frame = np.hstack([xyz, frame.data[:, [3]]])
cloud = np.vstack([cloud, cloud_frame])
vis.removeAllPointClouds()
vis.removeAllShapes()
vis.addPointCloud(pcl.create_xyzi(cloud[:, :4]), field="intensity")
visualize_detections(vis,
lidar_frame,
objs,
calib,
id_prefix="gt",
box_color="rainbow")
vis.setRepresentationToWireframeForAllActors()
vis.addCoordinateSystem()
state['idx'] += 1
if state['idx'] >= loader.sequence_sizes[scene]:
vis.close()
vis.registerKeyboardCallback(render_next)
render_next(None)
vis.spin()
def test_point_cloud_temporal_fusion(self):
# TODO: this fail for nuscenes and waymo
idx = selection or random.randint(0, len(self.tloader))
lidar = self.tloader.VALID_LIDAR_NAMES[0]
# load data
clouds = self.tloader.lidar_data(idx, lidar)
poses = self.tloader.pose(idx)
targets = self.tloader.annotation_3dobject(idx)
calib = self.oloader.calibration_data(idx)
cloud1, cloud2 = clouds[0][:, :4], clouds[-1][:, :4]
pose1, pose2 = poses[0], poses[-1]
targets1, targets2 = targets[0], targets[-1]
print("START", pose1)
print("END", pose2)
# create transforms
tf = TransformSet("global")
fname1, fname2 = "pose1", "pose2"
tf.set_intrinsic_map_pin(fname1)
tf.set_intrinsic_map_pin(fname2)
tf.set_extrinsic(pose1.h**o(), fname1)
tf.set_extrinsic(pose2.h**o(), fname2)
# make coordinate unified in frame
targets1 = calib.transform_objects(targets1, lidar)
targets2 = calib.transform_objects(targets2, lidar)
targets1.frame = fname1
targets2.frame = fname2
print("HOMO1", pose1.h**o())
print("HOMO2", pose2.h**o())
print(tf.get_extrinsic(fname1, fname2))
# visualize both point cloud in frame2
visualizer = pcl.Visualizer()
visualizer.addPointCloud(pcl.create_xyzi(
tf.transform_points(cloud1, frame_from=fname1, frame_to=fname2)),
field="intensity",
id="cloud1")
visualizer.addPointCloud(pcl.create_xyzi(cloud2),
field="intensity",
id="cloud2")
pcl_vis(visualizer,
fname2,
targets1,
tf,
box_color=(1, 1, 0),
id_prefix="frame1")
pcl_vis(visualizer,
fname2,
targets2,
tf,
box_color=(0, 1, 1),
id_prefix="frame2")
visualizer.setRepresentationToWireframeForAllActors()
visualizer.addCoordinateSystem()
visualizer.setWindowName(
"Please check whether the gt boxes are aligned!")
# visualizer.spinOnce(time=5000)
# visualizer.close()
visualizer.spin()
def test_add_callback(self, cloud, labelfile, database_name):
def key_callback(event):
print(event.KeyCode, event.KeySym)
def mouse_callback(event):
print(event.X, event.Y, event.Type, event.Button)
def picking_callback(event):
print(event.Point, event.PointIndex)
def area_callback(event):
print(event.PointsIndices)
def bounding_box_apollo(para_list, line_num, r, b, g):
center = np.matrix([para_list[0], para_list[1], para_list[2]
]).T * np.ones((1, 8))
size = np.matrix([[para_list[3], 0, 0], [0, para_list[4], 0],
[0, 0, para_list[5]]])
vertex = center + size * np.matrix(
[[.5, .5, -.5, -.5, .5, .5, -.5, -.5],
[.5, -.5, -.5, .5, .5, -.5, -.5, .5],
[.5, .5, .5, .5, -.5, -.5, -.5, -.5]])
for i in range(3):
viewer.addLine(vertex[:, i],
vertex[:, i + 1],
r,
b,
g,
id=str(line_num))
viewer.addLine(vertex[:, i + 4],
vertex[:, i + 5],
r,
b,
g,
id=str(4 + line_num))
viewer.addLine(vertex[:, i],
vertex[:, i + 4],
r,
b,
g,
id=str(8 + line_num))
line_num += 1
viewer.addLine(vertex[:, 3],
vertex[:, 0],
r,
b,
g,
id=str(line_num))
viewer.addLine(vertex[:, 4],
vertex[:, 7],
r,
b,
g,
id=str(4 + line_num))
viewer.addLine(vertex[:, 3],
vertex[:, 7],
r,
b,
g,
id=str(8 + line_num))
lines = 0
viewer = pcl.Visualizer()
viewer.addPointCloud(cloud)
viewer.addCoordinateSystem()
# addLine(self, p1, p2, double r=0.5, double g=0.5, double b=0.5, str id="line", int viewport=0)
# addText(self, str text, int xpos, int ypos, int fontsize = 10, double r = 1, double g = 1, double b = 1, str id = "", int viewport = 0)
file = open(labelfile, 'r')
if database_name == "apollo":
for obj_line in file:
obj_line = obj_line.split()
obj_para = [float(i) for i in obj_line[1:7]]
if obj_line[0] == "vehicle": # red
r = 1
g = 0
b = 0
bounding_box_apollo(obj_para, lines, r, b, g)
lines += 12
elif obj_line[0] == "pedestrian": # blue
r = 0
g = 1
b = 0
bounding_box_apollo(obj_para, lines, r, b, g)
lines += 12
elif obj_line[0] == "cyclist": # green
r = 0
g = 0
b = 1
bounding_box_apollo(obj_para, lines, r, b, g)
lines += 12
else: # don't care
r = 0.7
g = 0.7
b = 0.1
bounding_box_apollo(obj_para, lines, r, b, g)
lines += 12
elif database_name == "kitti":
pass
viewer.registerKeyboardCallback(key_callback)
viewer.registerMouseCallback(mouse_callback)
viewer.registerPointPickingCallback(picking_callback)
viewer.registerAreaPickingCallback(area_callback)
viewer.spinOnce(time=2e9)
viewer.close()
