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]