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 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_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 pcl_xyzi2xyzrgb(cloud):
array = cloud.to_ndarray()
x = np.array([arr[0] for arr in array])
y = np.array([arr[1] for arr in array])
z = np.array([arr[2] for arr in array])
inten = np.array([arr[3] for arr in array])
# print(inten.max())
# print(inten.min())
r, g, b = rgbmap(inten)
xyzrgb = np.stack([x, y, z, r, g, b], axis=-1)
# print(xyzrgb.shape)
cloudrgb = pcl.create_xyzrgb(xyzrgb)
return cloudrgb
def test_creators(self):
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_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_normal([[1,2,3,3], [4,5,6,6]])
assert np.all(cloud.normal == np.array([[1,2,3], [4,5,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"
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)
dep = cv2.imread(dep_file, cv2.IMREAD_ANYCOLOR | cv2.IMREAD_ANYDEPTH)
dep = dep.astype(np.float32) / 256
dep = cv2.resize(dep, img_size)
dep_flat = dep.reshape(1, -1)
xyz = dep_flat * id_pts_unit
print(xyz.shape)
rgb_file = os.path.join(rgb_root, rgb_file_list[i])
bgr = cv2.imread(rgb_file)
rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)
rgb = cv2.resize(rgb, img_size)
rgb_flat = rgb.transpose((2, 0, 1)).reshape((3, -1))
print(rgb_flat.shape)
xyzrgb = np.concatenate([xyz, rgb_flat], axis=0)
xyzrgb = xyzrgb.transpose((1, 0))
cloud = pcl.create_xyzrgb(xyzrgb)
print(xyzrgb.shape)
print(xyzrgb.dtype)
pcd_name = "{}.pcd".format(dep_file.split(".")[0])
pcl.io.save_pcd(pcd_name, cloud)
# break
## back projection
## save
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"