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_indexing(self):
cloud = pcl.create_xyz(np.random.rand(10, 3))
# test getitem
subcloud = cloud[0]
assert isinstance(subcloud, pcl.PointCloud)
assert len(subcloud) == 1
subcloud = cloud[:2]
assert isinstance(subcloud, pcl.PointCloud)
assert len(subcloud) == 2
subcloud = cloud[[1, 2]]
assert isinstance(subcloud, pcl.PointCloud)
assert len(subcloud) == 2
subcloud = cloud[np.array([1, 2])]
assert isinstance(subcloud, pcl.PointCloud)
assert len(subcloud) == 2
subcloud = cloud['x']
assert isinstance(subcloud, np.ndarray)
assert len(subcloud) == 10
subcloud = cloud[['x', 'y']]
assert isinstance(subcloud, np.ndarray)
assert len(subcloud) == 10
# test set
cloud[0] = (1, 2, 3)
assert cloud[0]['x'] == 1.
cloud[:2] = [(2, 3, 4), (3, 4, 5)]
assert cloud[0]['x'] == 2.
assert cloud[1]['x'] == 3.
cloud[[1, 2]] = [(4, 5, 6), (5, 6, 7)]
assert cloud[1]['x'] == 4.
assert cloud[2]['x'] == 5.
cloud['x'] = np.arange(10)
assert cloud[0]['x'] == 0
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_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_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"