def data_frame_to_point_cloud(df):
""" create a PointCloud object from a dataframe.
"""
pc_data = df.to_records(index=False)
md = {
'version': .7,
'fields': [],
'size': [],
'count': [],
'width': 0,
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': 0,
'type': [],
'data': 'binary_compressed'
}
md['fields'] = df.columns.tolist()
for field in md['fields']:
type_, size_ = pypcd.numpy_type_to_pcd_type[pc_data.dtype.fields[field]
[0]]
md['type'].append(type_)
md['size'].append(size_)
# TODO handle multicount
md['count'].append(1)
md['width'] = len(pc_data)
md['points'] = len(pc_data)
pc = pypcd.PointCloud(md, pc_data)
return pc
def make_xyzit_point_cloud(xyz_i_t):
""" Make a pointcloud object from PointXYZIT message, as in Pointcloud.proto.
message PointXYZIT {
optional float x = 1 [default = nan];
optional float y = 2 [default = nan];
optional float z = 3 [default = nan];
optional uint32 intensity = 4 [default = 0];
optional uint64 timestamp = 5 [default = 0];
}
"""
md = {
'version': .7,
'fields': ['x', 'y', 'z', 'intensity', 'timestamp'],
'count': [1, 1, 1, 1, 1],
'width': len(xyz_i_t),
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': len(xyz_i_t),
'type': ['F', 'F', 'F', 'U', 'U'],
'size': [4, 4, 4, 4, 8],
'data': 'binary_compressed'
}
typenames = []
for t, s in zip(md['type'], md['size']):
np_type = pypcd.pcd_type_to_numpy_type[(t, s)]
typenames.append(np_type)
np_dtype = np.dtype(zip(md['fields'], typenames))
pc_data = convert_xyzit_pb_to_array(xyz_i_t, data_type=np_dtype)
pc = pypcd.PointCloud(md, pc_data)
return pc
def make_xyzi_point_cloud(xyzl, label_type='f'):
""" Make XYZL point cloud from numpy array.
TODO i labels?
"""
md = {
'version': .7,
'fields': ['x', 'y', 'z', 'intensity'],
'count': [1, 1, 1, 1],
'width': len(xyzl),
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': len(xyzl),
'data': 'ASCII'
}
if label_type.lower() == 'f':
md['size'] = [4, 4, 4, 4]
md['type'] = ['F', 'F', 'F', 'F']
elif label_type.lower() == 'u':
md['size'] = [4, 4, 4, 1]
md['type'] = ['F', 'F', 'F', 'U']
else:
raise ValueError('label type must be F or U')
# TODO use .view()
xyzl = xyzl.astype(np.float32)
dt = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32),
('intensity', np.float32)])
pc_data = np.rec.fromarrays(
[xyzl[:, 0], xyzl[:, 1], xyzl[:, 2], xyzl[:, 3]], dtype=dt)
pc = pypcd.PointCloud(md, pc_data)
return pc
def make_contidata_point_cloud(self, contiobs):
"""
Make a pointcloud object from contiradar message, as conti_radar.proto.
message ContiRadarObs {
// x axis ^
// | longitude_dist
// |
// |
// |
// lateral_dist |
// y axis |
// <----------------
// ooooooooooooo //radar front surface
optional apollo.common.Header header = 1;
// longitude distance to the radar; (+) = forward; unit = m
optional double longitude_dist = 4;
// lateral distance to the radar; (+) = left; unit = m
optional double lateral_dist = 5;
.......
}
"""
md = {
'version': .7,
'fields': ['x', 'y', 'z', 'timestamp'],
'count': [1, 1, 1, 1],
'width': len(contiobs),
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': len(contiobs),
'type': ['F', 'F', 'F', 'F'],
'size': [4, 4, 4, 8],
'data': 'binary'
}
typenames = []
for t, s in zip(md['type'], md['size']):
np_type = pypcd.pcd_type_to_numpy_type[(t, s)]
typenames.append(np_type)
np_dtype = np.dtype(list(zip(md['fields'], typenames)))
pc_data = self.convert_contiobs_pb_to_array(contiobs,
data_type=np_dtype)
pc = pypcd.PointCloud(md, pc_data)
return pc
def from_array(npc):
dt = np.dtype([("x", np.float32, (1, )), ("y", np.float32, (1, )),
("z", np.float32, (1, ))])
pc_data = np.ndarray((npc.shape[0], ), dtype=dt)
for i in range(npc.shape[0]):
pc_data['x'][i] = npc[i, 0]
pc_data['y'][i] = npc[i, 1]
pc_data['z'][i] = npc[i, 2]
md = {
'version': .7,
'fields': [],
'size': [],
'count': [],
'width': 0,
'height': 1,
'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
'points': 0,
'type': [],
'data': 'binary_compressed'
}
for i in range(npc.shape[1]):
md['type'].append("F")
md['size'].append(4)
md['count'].append(1)
md['fields'] = ["x", "y", "z"]
# pc_data = np.array({"x": npc[:, 0],"y": npc[:, 1], "z": npc[:, 2]})
print("ckpt 0")
md['width'] = len(npc)
md['points'] = len(npc)
print(len(npc))
print(len(pc_data))
print(pc_data.shape)
print(npc.shape)
print(pc_data['x'].shape)
pc = pypcd.PointCloud(md, pc_data)
return pc
def from_array(arr):
""" create a PointCloud object from an array.
"""
pc_data = arr.copy()
md = {
'version': .7,
'fields': ['x', 'y', 'z', 'label', 'object'],
'size': [4, 4, 4, 4, 4],
'count': [],
'width': 0,
'height': 1,
'viewpoint': [0, 0, 0, 1, 0, 0, 0],
'points': 0,
'type': ['F', 'F', 'F', 'I', 'I'],
'data': 'binary_compressed'
}
for field in md['fields']:
md['count'].append(1)
md['width'] = len(pc_data)
md['points'] = len(pc_data)
pc = pypcd.PointCloud(md, pc_data)
return pc
(cloud_target[:, 2] != 0)]
cloud_target = cloud_target[:, :3]
metadata_target = {
"version": .7,
"fields": ['x', 'y', 'z'],
"size": [4, 4, 4],
"count": [1, 1, 1],
"width": len(cloud_target),
"height": 1,
"viewpoint": [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
"points": len(cloud_target),
"type": ['F', 'F', 'F'],
"data": 'binary'
}
pc_target = pypcd.PointCloud(metadata_target, cloud_target)
pc_target.save_pcd(folder + 'target.pcd')
if save_filtered_bag_as_npy:
cloud_save = []
# Source ------------------------------------------------------
for i in range(len(cloud_scans)):
cloud_source = cloud_scans[i]
cloud_source = cloud_source.astype(np.float32)
# Filter out dummy values
cloud_source = cloud_source[((cloud_source[:, 0] != 0) |
(cloud_source[:, 1] != 0)) |
(cloud_source[:, 2] != 0)]
#print cloud_source[:,4]
# Original ------------------------------------------------
cloud_source_original = cloud_source[:, :3]
