def pcl_from_np_single(xyz, rgb=None, intensity=None, normal=None):
"""accept np.ndarray N*C
intensity can have channel dimension or not."""
### to desired shape and scale
assert xyz.shape[1] == 3
if rgb is not None:
assert rgb.shape[1] == 3
if rgb.max() <= 1:
rgb = rgb * 255
if intensity is not None:
if intensity.ndim == 1:
intensity = intensity.reshape(-1, 1)
assert intensity.shape[1] == 3
if intensity.max() <= 1:
intensity = intensity * 255
if normal is not None:
assert normal.shape[1] == 3
### construct pcl objects
if rgb is not None:
xyz_rgb = np.concatenate((xyz, rgb), axis=1)
cloud = pcl.create_xyzrgb(xyz_rgb)
elif intensity is not None:
xyz_inten = np.concatenate((xyz, intensity), axis=1)
cloud = pcl.create_xyzi(xyz_inten)
else:
cloud = pcl.create_xyz(xyz)
if normal is not None:
cloud_nm = pcl.create_normal(normal)
cloud = cloud.append_fields(cloud_nm)
return cloud
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_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_3d_semantic(self):
seq = "2013_05_28_drive_0000_sync"
idx = selection or random.randint(0, len(self.oloader))
cloud = self.oloader.lidar_data((seq, idx), names="velo", bypass=True)
pose = self.oloader.pose((seq, idx), bypass=True)
calib = self.oloader.calibration_data((seq, idx))
cloud = calib.transform_points(cloud[:, :3],
frame_to="pose",
frame_from="velo")
cloud = cloud.dot(pose.orientation.as_matrix().T) + pose.position
labels = np.load(
os.path.join(kitti360_location, "data_3d_semantics", seq,
"indexed", "%010d.npz" % idx))
color_cloud = pcl.create_xyzrgb(
np.concatenate([cloud[:, :3], labels["rgb"].view('4u1')[:, :3]],
axis=1))
semantic_cloud = pcl.create_xyzl(
np.concatenate([cloud[:, :3], labels["semantic"].reshape(-1, 1)],
axis=1))
instance_cloud = pcl.create_xyzl(
np.concatenate([cloud[:, :3], labels["instance"].reshape(-1, 1)],
axis=1))
distance = os.path.join(kitti360_location, "data_3d_semantics", seq,
"indexed", "%010d.dist.npy" % idx)
distance = np.load(distance)
print("Index:", idx, ", MAX distance", np.max(distance))
distance_cloud = pcl.create_xyzi(
np.concatenate(
[cloud[:, :3], distance.reshape(-1, 1)], axis=1))
# gt = pcl.io.load_ply("/media/jacob/Storage/Datasets/kitti360/data_3d_semantics/2013_05_28_drive_0000_sync/static/000834_001286.ply")
# semantic_cloud = pcl.create_xyzl(np.concatenate([gt.xyz, gt.to_ndarray()['semantic'].reshape(-1, 1)], axis=1))
# instance_cloud = pcl.create_xyzl(np.concatenate([gt.xyz, gt.to_ndarray()['instance'].reshape(-1, 1)], axis=1))
pcl.io.save_pcd("instance.pcd", instance_cloud, binary=True)
pcl.io.save_pcd("semantic.pcd", semantic_cloud, binary=True)
pcl.io.save_pcd("distance.pcd", distance_cloud, binary=True)
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()
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_creators(self):
# RGB points actually contains the rgba field
cloud = pcl.create_rgb([[10, 20, 30, 30], [40, 50, 60, 60]])
assert np.all(
cloud.argb == np.array([[30, 10, 20, 30], [60, 40, 50, 60]]))
assert cloud.ptype == "RGB"
cloud = pcl.create_xyz([[1, 2, 3], [4, 5, 6]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert cloud.ptype == "XYZ"
cloud = pcl.create_xyzi([[1, 2, 3, 1], [4, 5, 6, 4]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert cloud.ptype == "XYZI"
cloud = pcl.create_xyzl([[1, 2, 3, 1], [4, 5, 6, 4]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert cloud.ptype == "XYZL"
cloud = pcl.create_xyzrgb([[1, 2, 3, 1, 2, 3], [4, 5, 6, 4, 5, 6]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert cloud.ptype == "XYZRGB"
cloud = pcl.create_xyzrgba([[1, 2, 3, 1, 2, 3, 1],
[4, 5, 6, 4, 5, 6, 4]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert cloud.ptype == "XYZRGBA"
cloud = pcl.create_xyzrgbl([[1, 2, 3, 1, 2, 3, 1],
[4, 5, 6, 4, 5, 6, 4]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert cloud.ptype == "XYZRGBL"
cloud = pcl.create_normal([[1, 2, 3, 3], [4, 5, 6, 6]])
assert np.all(cloud.normal == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud['curvature'] == [3, 6])
assert cloud.ptype == "NORMAL"
cloud = pcl.create_normal([[1, 2, 3], [4, 5, 6]])
assert np.all(cloud.normal == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud['curvature'] == 0)
assert cloud.ptype == "NORMAL"
cloud = pcl.create_xyzn([[1, 2, 3, 1, 2, 3, 1], [4, 5, 6, 4, 5, 6, 4]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud.normal == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud['curvature'] == [1, 4])
assert cloud.ptype == "XYZN"
cloud = pcl.create_xyzn([[1, 2, 3, 1, 2, 3], [4, 5, 6, 4, 5, 6]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud.normal == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud['curvature'] == 0)
assert cloud.ptype == "XYZN"
cloud = pcl.create_xyzrgbn([[1, 2, 3, 1, 2, 3, 1, 2, 3, 1],
[4, 5, 6, 4, 5, 6, 4, 5, 6, 4]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud.normal == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud['curvature'] == [1, 4])
assert cloud.ptype == "XYZRGBN"
cloud = pcl.create_xyzrgbn([[1, 2, 3, 1, 2, 3, 1, 2, 3],
[4, 5, 6, 4, 5, 6, 4, 5, 6]])
assert np.all(cloud.xyz == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud.normal == np.array([[1, 2, 3], [4, 5, 6]]))
assert np.all(cloud['curvature'] == 0)
assert cloud.ptype == "XYZRGBN"