def main():
import pcl
# laspy librairy, read las file
f = file.File('28XXX10000075-18.las', mode='r')
# Store pointcloud in array
ptcloud = np.vstack((f.x, f.y, f.z)).transpose()
f.close()
# cloud = pcl.load('./examples/pcldata/tutorials/table_scene_mug_stereo_textured.pcd')
# ptcloud = cloud.to_list()
# Centred the data
ptcloud_centred = ptcloud - np.mean(ptcloud, 0)
# Simulate an intensity information between 0 and 1
ptcloud_centred = sc.append(ptcloud_centred,
np.zeros((ptcloud.shape[0], 1)),
axis=1)
for i in range(ptcloud_centred.shape[0] - 1):
ptcloud_centred[i, 3] = random.random()
p = pcl.PointCloud_PointXYZI()
p.from_array(np.array(ptcloud_centred, dtype=np.float32))
# Visualization
visual = pcl_visualization.CloudViewing()
visual.ShowGrayCloud(p, b'cloud')
v = True
while v:
v = not (visual.WasStopped())
def on_click():
f = file.File(inFile1, mode='r')
ptcloud = np.vstack((f.x, f.y, f.z)).transpose()
f.close()
# Centred the data
ptcloud_centred = ptcloud - np.mean(ptcloud, 0)
# Simulate an intensity information between 0 and 1
ptcloud_centred = sc.append(ptcloud_centred,
np.zeros((ptcloud.shape[0], 1)),
axis=1) # Ajout d'une ligne (axis=0)
for i in range(ptcloud_centred.shape[0] - 1):
ptcloud_centred[i, 3] = random.random()
p = pcl.PointCloud_PointXYZI()
p.from_array(np.array(ptcloud_centred, dtype=np.float32))
visual = pcl_visualization.CloudViewing()
visual.ShowGrayCloud(p)
def check_was_stopped():
visual.WasStopped()
root.after(100, check_was_stopped)
check_was_stopped()
def setUp(self):
rng = np.random.RandomState(42)
# Define two dense sets of points of sizes 30 and 170, resp.
a = rng.randn(100, 4).astype(np.float32)
a[:30] -= 42
self.pc = pcl.PointCloud_PointXYZI(a)
self.kd = pcl.KdTreeFLANN_PointXYZI(self.pc)
def show_lidar(liadr_path):
pc = get_lidar(liadr_path)
cloud = pcl.PointCloud_PointXYZI()
cloud.from_array(pc)
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowGrayCloud(cloud, b'cloud')
flag = True
while flag:
flag != visual.WasStopped()
def XYZ_to_XYZI(XYZ_cloud, color, use_multiple_colors=False):
XYZI_cloud = pcl.PointCloud_PointXYZI()
points_list = []
for idx, data in enumerate(XYZ_cloud):
intensity = int(color[idx]) if use_multiple_colors else int(color)
points_list.append([data[0], data[1], data[2], intensity])
XYZI_cloud.from_list(points_list)
return XYZI_cloud
def ransac_segment(pc):
cloud = pcl.PointCloud_PointXYZI()
cloud.from_array(pc.astype(np.float32))
seg = cloud.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.04)
seg.set_normal_distance_weight(0.001)
seg.set_max_iterations(100)
return seg.segment()
def setUp(self):
self.p = pcl.load("tutorials/table_scene_lms400.pcd")
self.fil = self.p.make_ApproximateVoxelGrid()
self.p2 = pcl.PointCloud_PointXYZI()
self.fil2 = self.p2.make_ApproximateVoxelGrid()
self.p3 = pcl.PointCloud_PointXYZRGB()
self.fil3 = self.p3.make_ApproximateVoxelGrid()
self.p4 = pcl.PointCloud_PointXYZRGBA()
self.fil4 = self.p4.make_ApproximateVoxelGrid()
def loader(filename):
data = np.fromfile(filename, binType)
cls = classmap[os.path.basename(filename).split('.')[0]]
P = np.vstack([data['x'], data['y'], data['z']]).T # metric units
F = data['intensity'].reshape(-1, 1)
# training data augmentation
if training:
if args.pc_augm_input_dropout > 0: # removing points here changes graph structure (unlike zeroing features)
P, F = pcu.dropout(P, F, args.pc_augm_input_dropout)
M = np.eye(3)
if args.pc_augm_scale > 1:
s = random.uniform(1 / args.pc_augm_scale, args.pc_augm_scale)
M = np.dot(transforms3d.zooms.zfdir2mat(s), M)
if args.pc_augm_rot:
angle = random.uniform(0, 2 * math.pi)
M = np.dot(transforms3d.axangles.axangle2mat([0, 0, 1], angle),
M) # z=upright assumption
if args.pc_augm_mirror_prob > 0: # mirroring x&y, not z
if random.random() < args.pc_augm_mirror_prob / 2:
M = np.dot(transforms3d.zooms.zfdir2mat(-1, [1, 0, 0]), M)
if random.random() < args.pc_augm_mirror_prob / 2:
M = np.dot(transforms3d.zooms.zfdir2mat(-1, [0, 1, 0]), M)
P = np.dot(P, M.T)
# coarsen to initial resolution (btw, axis-aligned quantization of rigidly transformed cloud adds jittering noise)
P -= np.min(
P, axis=0
) #move to positive octant (voxelgrid has fixed boundaries at axes planes)
PF = np.hstack([P, F]).astype(np.float32)
PF_filtered = pcu.voxelgrid(
pcl.PointCloud_PointXYZI(PF), pyramid_conf[0][0]
).to_array(
) # aggregates intensities too (note pcl wrapper bug: only int intensities accepted)
F = PF_filtered[:,
3] / 255 - 0.5 # laser return intensities in [-0.5,0.5]
cloud = pcl.PointCloud(
PF_filtered[:, 0:3]
) # (pcl wrapper bug: XYZI cannot query kd-tree by radius)
graphs, poolmaps = pcu.create_graph_pyramid(args, cloud, pyramid_conf)
return F, cls, graphs, poolmaps
def ros_to_pcl(ros_cloud):
""" Converts a ROS PointCloud2 message to a pcl PointXYZRGB
Args:
ros_cloud (PointCloud2): ROS PointCloud2 message
Returns:
pcl.PointCloud_PointXYZRGB: PCL XYZRGB point cloud
"""
points_list = []
for data in pc2.read_points(ros_cloud, skip_nans=True):
points_list.append([data[0], data[1], data[2], data[3]])
# pcl_data = pcl.PointCloud_PointXYZRGB()
pcl_data = pcl.PointCloud_PointXYZI()
pcl_data.from_list(points_list)
return pcl_data
def convert_pcd_to_ros_cloud(self, pcd_file, stamp, frame):
p = pcl.PointCloud_PointXYZI()
p.from_file(pcd_file)
pcl_array = p.to_array()
points_arr = np.zeros((p.size),
dtype=[('x', np.float32), ('y', np.float32),
('z', np.float32),
('intensity', np.float32)])
header = std_msgs.msg.Header()
header.stamp = stamp
header.frame_id = frame
if p.size > 0:
points_arr['x'] = pcl_array[:, 0]
points_arr['y'] = pcl_array[:, 1]
points_arr['z'] = pcl_array[:, 2]
points_arr['intensity'] = pcl_array[:, 3]
cloud_msg = ros_numpy.msgify(PointCloud2, points_arr)
cloud_msg.header = header
return cloud_msg
def find_road_plane(points):
# cloud = pcl.PointCloud()
# cloud.from_array(xyz_data[:,0:3].astype('float32'))
cloud = pcl.PointCloud_PointXYZI()
cloud.from_array(xyz_data.astype('float32'))
# fitler statistical outlier
# fil_stat = cloud.make_statistical_outlier_filter()
# fil_stat = cloud.make_statistical_outlier_filter()
# fil_stat.set_mean_k(50)
# fil_stat.set_std_dev_mul_thresh(1.0)
# cloud_filtered = fil_stat.filter()
# print "Statistical Inlier Number:", cloud_filtered.size
# find normal plane
seg = cloud.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_normal_distance_weight(0.001)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(100)
seg.set_distance_threshold(0.3)
indices, model = seg.segment()
print "Road Plane Model:", model
cloud_plane = cloud.extract(indices, negative=False)
# NG : const char* not str
# cloud_plane.to_file('table_scene_mug_stereo_textured_plane.pcd')
pcl.save(cloud_plane, 'road_plane.pcd')
print "Road plane point cloud file road_plane.pcd saved."
return cloud_plane.to_array(), np.array(indices)
for ind in range(0, points_array.shape[0]):
y = np.matrix([[points_array[ind][0]], [points_array[ind][1]],
[points_array[ind][2]], [1.0]])
Tr_y = Tr_velo_to_cam * y
if Tr_y[2] > 0:
X = P2 * R0_rect * Tr_y
#FOR LOOP FOR ALL OBJECTS
for index in range(0, len(objects)):
obj = objects[index]
if ((obj.xmin < X[0] / X[2] < obj.xmax)
and (obj.ymin < X[1] / X[2] < obj.ymax)):
filtered_points_array.append(points_array[ind])
f.write('{} \n'.format(points_array[ind]))
f.close()
# Save the Points to a PCD file if the points exist
if (len(filtered_points_array) > 0):
outcloud = pcl.PointCloud_PointXYZI(
np.array(filtered_points_array, dtype=np.float32))
#DONT CHANGE NAMES BECAUSE OF CPP CODE
pcl.save(
outcloud,
"/home/emeka/Schreibtisch/AIS/ais3d/PCD_Files1/segmented_{}.pcd"
.format(pcd_id))
print('Cloud saved')
if show_images == 1:
os.system(
"/home/emeka/Schreibtisch/AIS/deleteme/build/test_pcl {}".
format(pcd_id))
if show_images == 1:
cv2.destroyAllWindows()
[0.173687, -0.84253, -0.400481, 0.1],
[-0.874475, 0.706127, -0.117635, 0.1],
[0.908514, -0.598159, 0.744714, 0.1]],
dtype=np.float32)
cloud = pcl.load(
'/home/li/PROJECTS/TRAIN/PPCL-master/examples/tests/tutorials/table_scene_lms400.pcd'
)
print('type(cloud)', type(cloud))
data = np.array(cloud)
print('data.shape', data.shape)
data2 = np.zeros((460400, 1), dtype=np.float32)
data = np.column_stack((data, data2))
print('data.shape', data.shape)
cloud = pcl.PointCloud_PointXYZI(data)
# cloud = pcl.PointCloud_PointXYZI(_data)
# // Create the filtering object: downsample the dataset using a leaf size of 1cm
# pcl::VoxelGrid<pcl::PointXYZ> vg;
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
# vg.setInputCloud (cloud);
# vg.setLeafSize (0.01f, 0.01f, 0.01f);
# vg.filter (*cloud_filtered);
# std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
vg = cloud.make_voxel_grid_filter()
vg.set_leaf_size(0.1, 0.1, 0.1)
cloud_filtered = vg.filter()
print('cloud_filtered.size', cloud_filtered.size)
Y = K[i][:, 1:3]
p = P.polyfit(X, Y, 2)
ax_f.plot(xs + min_line_xyz[0], p[0,0] + p[1,0]*(xs) + p[2,0]*(xs)**2 + min_line_xyz[1], \
p[0,1] + p[1,1]*xs + p[2,1]*xs**2 + min_line_xyz[2])
plt.colorbar(sc)
plt.show()
if __name__ == '__main__':
xyz_data = read_points("final_project_point_cloud.fuse")
# save xyz data to pcd file
road_xyz = pcl.PointCloud_PointXYZI()
road_xyz.from_array(xyz_data.astype('float32'))
pcl.save(road_xyz, 'road_xyz.pcd')
print "Road plane segment point cloud file road_xyz.pcd saved."
print "Min X:{:15.2f}, Max X:{:15.2f}".format(np.min(xyz_data[:, 0]),
np.max(xyz_data[:, 0]))
print "Min Y:{:15.2f}, Max Y:{:15.2f}".format(np.min(xyz_data[:, 1]),
np.max(xyz_data[:, 1]))
print "Min Z:{:15.2f}, Max Z:{:15.2f}".format(np.min(xyz_data[:, 2]),
np.max(xyz_data[:, 2]))
print "Min I:{:15.2f}, Max I:{:15.2f}".format(np.min(xyz_data[:, 3]),
np.max(xyz_data[:, 3]))
# find road plane using pcl call
road_plane, road_plane_idx = find_road_plane(xyz_data)
def test_asarray(self):
p = pcl.PointCloud_PointXYZI(self.p) # copy
# old0 = p[0]
a = np.asarray(p) # view
a[:] += 6
assert_array_almost_equal(p[0], a[0])
def voxelize(pc, voxel_size):
cloud = pcl.PointCloud_PointXYZI()
cloud.from_array(pc.astype(np.float32))
sor = cloud.make_voxel_grid_filter()
sor.set_leaf_size(voxel_size, voxel_size, voxel_size)
return sor.filter().to_array()
def setUp(self):
self.p = pcl.PointCloud_PointXYZI(
np.arange(12, dtype=np.float32).reshape(3, 4))
def setUp(self):
self.p = pcl.PointCloud_PointXYZI(_data)
def test_copy():
a = np.random.randn(100, 3).astype(np.float32)
p1 = pcl.PointCloud_PointXYZI(a)
p2 = pcl.PointCloud_PointXYZI(p1)
assert_array_equal(p2.to_array(), a)
np_pc = np.array(list_pc, dtype=np.float32)
print(np_pc.shape)
# test
# save numpy array to bin or pcd
filename = str(lidar_t)
print(filename)
np_pc_bin = copy.deepcopy(np_pc)
bin_path_ = save_path + '/bin'
Path(bin_path_).mkdir(parents=True, exist_ok=True)
bin_path = bin_path_ + "/" + filename + ".bin"
np_pc_bin.astype(np.float32).tofile(bin_path)
np_pc_pcd = copy.deepcopy(np_pc)
pcd_path_ = save_path + '/pcd'
Path(pcd_path_).mkdir(parents=True, exist_ok=True)
pcd_path = pcd_path_ + "/" + filename + ".pcd"
if args.is_ml == True:
pc_rgb = pcl.PointCloud_PointXYZRGB()
pc_rgb.from_list(test_list_pc)
pcl.save(pc_rgb, pcd_path)
else:
pc_intensity = pcl.PointCloud_PointXYZI()
pc_intensity.from_array(np_pc_pcd)
pcl.save(pc_intensity, pcd_path)
print('DONE')
def setUp(self):
self.a = np.array(np.mat(SEGDATA, dtype=np.float32))
self.p = pcl.PointCloud_PointXYZI()
self.p.from_array(self.a)