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_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 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 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_infer_ptype(self):
c1 = pcl.create_xyz(np.random.rand(10, 3))
c2 = pcl.create_normal(np.random.rand(10, 3))
c3 = c1.append_fields(c2)
c3.infer_ptype()
assert c3.ptype == 'XYZN'
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"