def write(dir, datadict, keys=[]):
# limited to 2d float arrays now
if not os.path.exists(dir):
os.makedirs(dir)
for key, val in list(datadict.items()):
if key in ['coords', 'ma_coords_in', 'ma_coords_out']:
if key in keys or len(keys) == 0:
fname = os.path.join(dir, key + '.pcd')
pc = pypcd.make_xyz_point_cloud(datadict[key])
pc.save_pcd(fname)
def write_pcd(dir, datadict, keys=[]): # limited to 2d float arrays now if not os.path.exists(dir): os.makedirs(dir) for key,val in list(datadict.items()): if key in ['coords', 'ma_coords_in', 'ma_coords_out']: if key in keys or len(keys)==0: fname = os.path.join(dir,key+'.pcd') pc = pypcd.make_xyz_point_cloud(datadict[key]) pc.save_pcd(fname)
def readNextFrame(self):
result = self.data.readNextFrame()
self.X = np.frombuffer(
result['x'], dtype='uint16'
) #Gives a Key to a library to pull the appropriate data
#self.X = self.X.reshape(imageHeight,imageWidth)
self.Y = np.frombuffer(result['y'], dtype='uint16')
self.Z = np.frombuffer(result['z'], dtype='uint16')
print(self.Z)
print(num / 3)
self.L = np.array([self.X, self.Y, self.Z])
print(len(self.L[1]))
self.pc = pypcd.make_xyz_point_cloud(self.L)
pypcd.save_point_cloud(self.pc, "apcd.pcd")
def readNextFrame(self):
result = self.data.readNextFrame()
#print(result.keys())
self.Amplitude = np.frombuffer(result['amplitude'], dtype='uint8')
self.Amplitude = self.Amplitude.reshape(imageHeight, imageWidth)
self.Distance = np.frombuffer(result['distance'], dtype='uint16')
self.Distance = self.Distance.reshape(imageHeight, imageWidth)
self.illuTemp = 20.0
# page 23 of manual,
# xyz data is 16-bit signed ints
# [mm]
self.X = np.frombuffer(
result['x'], dtype='int16'
) #Gives a Key to a library to pull the appropriate data
self.Y = np.frombuffer(result['y'], dtype='int16')
self.Z = np.frombuffer(result['z'], dtype='int16')
#self.L=np.concatenate([self.X,self.Y,self.Z])
#for i in range(len(self.X)):
# self.L[i] = (self.X[i],self.Y[i],self.Z[i])
self.L = [self.X, self.Y, self.Z]
self.L = np.transpose(self.L)
#self.L = self.L.astype(np.float32)
#self.pc_data = self.L.view(np.dtype([('x', np.int16), ('y', np.int16),('z', np.int16)]))
# print(self.pc_data)
# print(len(self.pc_data))
self.pc = pypcd.make_xyz_point_cloud(self.L)
#print(self.pc.pc_data[:,:]) #This indicates that somewhere something is only taking the first column of a thing
currentDT = datetime.datetime.now()
timestamp = str(currentDT.hour) + "_" + str(
currentDT.minute) + "_" + str(currentDT.second) + "_" + str(
currentDT.microsecond)
#Convert micro_sec to sec
timestampTOF = result['diagnostic']['timeStamp'] / 1e6
### Storing in memory instead
#pypcd.save_point_cloud(self.pc, "../examples/TOF_PointClouds/testframe_" + timestamp + ".pcd")
return self.pc, timestampTOF, self.Amplitude
def visualize_pointclouds():
# Generate data if needed
data_gen = modelnet.ModelnetData(pointcloud_size=CLOUD_SIZE)
# Modelnet generator
index = 0
for clouds, labels in data_gen.generate_representative_batch(
train=False,
instances_number=1,
shuffle_points=True,
jitter_points=True,
rotate_pointclouds=False,
rotate_pointclouds_up=True,
sampling_method=SAMPLING_METHOD):
# Save as pcd
cloud_out = pypcd.make_xyz_point_cloud(clouds[15])
if index < 10:
cloud_out.save_pcd("flowerpot_0" + str(index) + ".pcd")
else:
cloud_out.save_pcd("flowerpot_" + str(index) + ".pcd")
index = index + 1
def readNextFrame(self):
result = self.data.readNextFrame()
self.Amplitude = np.frombuffer(result['amplitude'], dtype='uint16')
self.Amplitude = self.Amplitude.reshape(imageHeight, imageWidth)
self.Distance = np.frombuffer(result['distance'], dtype='uint16')
self.Distance = self.Distance.reshape(imageHeight, imageWidth)
self.illuTemp = 20.0
# page 23 of manual,
# xyz data is 16-bit signed ints
# [mm]
self.X = np.frombuffer(result['x'], dtype='int16')
self.Y = np.frombuffer(result['y'], dtype='int16')
self.Z = np.frombuffer(result['z'], dtype='int16')
self.L = [self.X, self.Y, self.Z]
self.L = np.transpose(self.L)
self.pc = pypcd.make_xyz_point_cloud(self.L)
# IF YOU HAVE SET UP NTP SYNCING ON THE ToF camera, then the
# framegrabber will return the synced unix epoch time. so convert it
# back to UTC and bamarooni you got time
#
# if you encounter a "year is out of range" error the timestamp
# may be in milliseconds, try `ts /= 1000` in that case
unix_timestamp = result['timeStamp']
# get the local timezone
local_timezone = tzlocal.get_localzone()
local_time = datetime.fromtimestamp(unix_timestamp, local_timezone)
fmt = '%Y_%m_%d_%H_%M_%S.%f'
timestampTOF = local_time.strftime(fmt)
return self.pc, timestampTOF
pcd_filepath_out_unif = '../data/pcd/modelnet_unif.pcd'
pcd_filepath_out_rand = '../data/pcd/modelnet_rand.pcd'
pcd_filepath_out_grph = '../data/pcd/modelnet_grph.pcd'
data_gen = modelnet_data.ModelnetData(2048)
for X, Y in data_gen.generate_random_batch(True):
# Downsample
output_pointcloud_raw_uni = pointcloud_downsample.uniform(X[idx])
output_pointcloud_raw_grp = pointcloud_downsample.via_graphs(X[idx])
print "OLD SHAPE : ", X[idx].shape
print "NEW SHAPE [UNIFORM]: ", output_pointcloud_raw_uni.shape
print "NEW SHAPE [VIA_GRP]: ", output_pointcloud_raw_grp.shape
# Save as pcd (original)
cloud_in = pypcd.make_xyz_point_cloud(X[idx])
cloud_in.save_pcd(pcd_filepath_in)
# Save as pcd (uniform)
cloud_out = pypcd.make_xyz_point_cloud(output_pointcloud_raw_uni)
cloud_out.save_pcd(pcd_filepath_out_unif)
# Save as pcd (via_graph)
cloud_out = pypcd.make_xyz_point_cloud(output_pointcloud_raw_grp)
cloud_out.save_pcd(pcd_filepath_out_grph)
# Save as pcd (random)
cloud_point_idxs = np.arange(len(X[idx]))
cloud_randm_idxs = np.random.choice(cloud_point_idxs, 128, replace=False)
cloud_out_rand = X[idx][cloud_randm_idxs]
cloud_out_rand = pypcd.make_xyz_point_cloud(cloud_out_rand)
def projectActivations():
new_c = np.genfromtxt('filtered_counts.csv', delimiter=',', dtype='int')
samples = 512
ids_target = np.nonzero(new_c >= samples)[0]
path = os.path.abspath('/home/er13827/space/testing/tmp.csv')
pos = path.rfind('/')
descriptor_id = 13
file_descriptor = path[:pos] + '/tmp' + str(descriptor_id) + '.csv'
labels = np.genfromtxt(file_descriptor,
dtype='str',
skip_header=1,
delimiter=',')
print('Affordances in descriptor %d' % labels.shape[0])
# fig = plt.figure(figsize=(6, 6))
# plt.ion()
# ax = fig.add_subplot(111, projection='3d')
for i in range(ids_target.size - 1, -1, -1):
interaction = ids_target[i]
#recover activations
name = '../data/activations/OriginalActivations_' + str(
interaction) + '.npy'
activations = np.load(name)
aff_dir = labels[interaction, 0]
query_object = labels[interaction, 2]
data_file = path[:pos] + "/" + aff_dir + "/ibs_full_" + labels[
interaction, 1] + "_" + query_object + ".txt"
print(data_file)
with open(data_file) as f:
content = f.readlines()
# you may also want to remove whitespace characters like `\n` at the end of each line
content = [x.strip() for x in content]
scene_file = content[0].split(":")[1]
tmp = content[8].split(":")[1]
datapoint = tmp.split(',')
test_point = np.expand_dims(np.asarray([float(x) for x in datapoint]),
axis=0)
data_file = path[:pos] + "/" + aff_dir + "/" + scene_file
#training_cloud=getTrainingScene(data_file,scene_file)
#translate activations using test_point
activations = activations + test_point
field = path[:pos] + '/' + aff_dir + '/' + labels[
interaction, 1] + '_' + query_object + '_field_clean.pcd'
#read original sample lines
one_sample_file = path[:
pos] + '/' + aff_dir + '/ibs_sample_512_' + labels[
interaction,
1] + '_' + query_object + '_better.pcd'
sample_data = load_pcd_data(one_sample_file, cols=(0, 1, 2, 3, 4, 5))
#print(sample_data[-1,:])
if not os.path.exists(field):
print('Field not found %s' % (field))
continue
data, _, normals = load_pcd_data_binary(field)
#search the NN in the field for every activation point
print('Building tree')
kdt = KDTree(data, metric='euclidean')
print('Searching for %d activations NN' % (activations.shape[0]))
toExtract = np.zeros((activations.shape[0], 1), dtype=np.int32)
for k in range(activations.shape[0]):
anActivation = np.expand_dims(activations[k, :], axis=0)
_, ind = kdt.query(anActivation, k=1)
toExtract[k, 0] = ind[0, 0]
#remove repeated ids
non_repeated = np.unique(toExtract)
print('To extract %d, actually %d' %
(toExtract.shape[0], non_repeated.shape[0]))
new_sample = np.empty(
(non_repeated.shape[0] + 2, activations.shape[1]))
new_sample_normals = np.empty(
(non_repeated.shape[0] + 2, activations.shape[1]))
norm_mags = np.empty((non_repeated.shape[0], 1))
new_sample[:non_repeated.size, :] = data[non_repeated, :]
new_sample_normals[:non_repeated.size, :] = normals[non_repeated, :]
norm_mags[:, 0] = np.linalg.norm(normals[non_repeated, :], axis=1)
sorted_norms = np.argsort(norm_mags[:, 0])
new_sample = new_sample[sorted_norms, ...]
new_sample_normals = new_sample_normals[sorted_norms, ...]
# field_ids=np.arange(data.shape[0])
# np.random.shuffle(field_ids)
# field_ids=field_ids[:5000]
#ax.scatter(training_cloud[:2048,0],training_cloud[:2048,1],training_cloud[:2048,2],s=1,c='r')
# ax.scatter(new_sample[:,0],new_sample[:,1],new_sample[:,2],s=5,c='b')
# ax.scatter(data[field_ids[:],0],data[field_ids[:],1],data[field_ids[:],2],s=1,c='c')
# ax.scatter(activations[:,0],activations[:,1],activations[:,2],s=5,c='g')
# plt.pause(1)
# plt.draw()
# ax.clear()
new_sample[-2, :] = sample_data[-2, :3]
new_sample[-1, :] = sample_data[-1, :3]
new_sample_normals[-1, :] = sample_data[-1, 3:]
new_sample_normals[-2, :] = sample_data[-2, 3:]
actual_data_array = np.zeros(new_sample_normals.shape[0],
dtype={
'names':
('x', 'y', 'z', 'v1', 'v2', 'v3'),
'formats':
('f4', 'f4', 'f4', 'f4', 'f4', 'f4')
})
actual_data_array['x'] = new_sample[:, 0]
actual_data_array['y'] = new_sample[:, 1]
actual_data_array['z'] = new_sample[:, 2]
actual_data_array['v1'] = new_sample_normals[:, 0]
actual_data_array['v2'] = new_sample_normals[:, 1]
actual_data_array['v3'] = new_sample_normals[:, 2]
#new_cloud_data=np.concatenate((new_sample,new_sample_normals),axis=1)
new_cloud = pypcd.PointCloud.from_array(actual_data_array)
nname = '../data/activations/ibs_sample_cnn_sample_' + labels[
interaction, 1] + '_' + labels[interaction, 2] + '.pcd'
new_cloud.save_pcd(nname, compression='ascii')
del new_cloud
activations_cloud = pypcd.make_xyz_point_cloud(activations)
nname = '../data/activations/activations_' + str(interaction) + '.pcd'
activations_cloud.save_pcd(nname)
print("Done")
#tsp = rospy.Time()
#pyrosmsg.print_time(tsp)
#
#hdr = pyrosmsg.make_header(32)
#print hdr
#
#pyrosmsg.print_header_seq(hdr)
#
#print
#cloud = pyrosmsg.make_pc2(32)
#print cloud
#
#print
pc_data = np.random.random((4, 3)).astype('float32')
pc = pypcd.make_xyz_point_cloud(pc_data)
print 'pc.width:', pc.width
pc_msg = pc.to_msg()
print type(pc_msg)
print 'pc_msg.width:', pc_msg.width
pyrosmsg.print_centroid(pc_msg)
print pc_data.mean(0)
#
#print 'ci'
#
#ci = CameraInfo()
#print ci
#pyrosmsg.print_cam_info(ci)
import pypcd
import numpy as np
# filepaths
npy_filepath = '../data/synthetic/train/0001_01.npy'
pcd_filepath = '../data/pcd/0000_01.pcd'
augment = False
# load
a = np.load(npy_filepath)
# augment
if augment:
dist = a[1:] - a[:-1]
new_points = []
for idx in range(dist.shape[0]):
act_dist = dist[idx]
for ins in range(1, 10):
incr = act_dist * ins / 10
new_points.append(a[idx] + incr)
if augment:
b = np.array(new_points)
c = np.concatenate((a, b), axis=0)
else:
c = a
# Save as pcd
cloud_out = pypcd.make_xyz_point_cloud(c)
cloud_out.save_pcd(pcd_filepath)
import laspy
import numpy as np
import pypcd
filename = 'data_las/den.las'
inFile = laspy.file.File(filename, mode='r')
inHeader = laspy.header.HeaderManager(inFile.header, inFile.reader)
points_count = inHeader.count
x_offset, y_offset, z_offset = inHeader.offset
x_scale, y_scale, z_scale = inHeader.scale
print("count: {0}".format(points_count))
points = []
step = 50
for i in range(0, points_count, step):
point = [
inFile.X[i] * x_scale + x_offset, inFile.Y[i] * y_scale + y_offset,
inFile.Z[i] * z_scale + z_offset
]
points.append(point)
points = np.array(points)
pc = pypcd.make_xyz_point_cloud(points)
pc.save_pcd('forest2_convert.pcd')