def register_all_clouds(self):
'''register all the pointclouds found in the given folder
Returns: the registered clouds
'''
cloud_list = self.file_saver.get_cloud_list(self.input_folder_path)
print('registration in progress...')
global_transform = np.identity(4)
# perform iterative registration
for index in range(len(cloud_list) - 1):
source = pcl.load(cloud_list[index])
target = pcl.load(cloud_list[index + 1])
estimation, pair_transform = self.register_pair_clouds(
source, target)
result = self.map_cloud_operation(estimation, global_transform,
np.dot)
# update the global transformation
global_transform = np.dot(global_transform, pair_transform)
self.file_saver.save_point_cloud(point_cloud=result,
file_name=str(index))
return source
def complete_tactile_pcd_file_and_save(depth_pcd_filename,
tactile_pcd_filename, completion_method,
**kwargs):
"""
:param depth_pcd_filename:
:param tactile_pcd_filename:
:param completion_method:
:param kwargs:
:return:
"""
suffix = kwargs.get("suffix", "")
depth_cloud = pcl.load(depth_pcd_filename)
tactile_cloud = pcl.load(tactile_pcd_filename)
depth_points = depth_cloud.to_array()
tactile_points = tactile_cloud.to_array()
if completion_method not in TACTILE_COMPLETION_METHODS:
raise ValueError(
"completion_method must be a tactile completion method defined in this module"
)
plydata = completion_method(depth_points, tactile_points, **kwargs)
ply_filename = depth_pcd_filename.replace(".pcd", suffix + ".ply")
plydata.write(open(ply_filename, 'wb'))
def main():
# // Loading first scan of room.
# pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
# {
# PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
# return (-1);
# }
# std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
target_cloud = pcl.load('room_scan1.pcd')
print('Loaded ' + str(target_cloud.size) + ' data points from room_scan1.pcd')
# // Loading second scan of room from new perspective.
# pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
# {
# PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
# return (-1);
# }
# std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
input_cloud = pcl.load('room_scan2.pcd')
print('Loaded ' + str(input_cloud.size) + ' data points from room_scan2.pcd')
# // Filtering input scan to roughly 10% of original size to increase speed of registration.
# pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
# pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
# approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
# approximate_voxel_filter.setInputCloud (input_cloud);
# approximate_voxel_filter.filter (*filtered_cloud);
# std::cout << "Filtered cloud contains " << filtered_cloud->size () << " data points from room_scan2.pcd" << std::endl;
##
approximate_voxel_filter = input_cloud.make_ApproximateVoxelGrid()
approximate_voxel_filter.set_leaf_size (0.2, 0.2, 0.2)
filtered_cloud = approximate_voxel_filter.filter ()
print('Filtered cloud contains ' + str(filtered_cloud.size) + ' data points from room_scan2.pcd')
def main():
#通过pcl.load加载pcd文件
cloud2 = pcl.load("/home/nvidia/projects/D435i/nanshan.pcd")
cloud1 = pcl.load("/home/nvidia/projects/D435i/nanshan.pcd")
cloud2 = np.array(cloud2) #这里转换成数组是为了进行一个平移，能够看出来两个点云的差异
print(cloud2.shape)
for x in range(cloud2.shape[0]):
cloud2[x] = cloud2[x] + 2
cloud1 = np.array(cloud1)
#如何将数组类型转换成点云所需要的数据类型
cloud2 = pcl.PointCloud(cloud2)
cloud1 = pcl.PointCloud(cloud1)
visualcolor1 = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud2, 0, 255, 0)
visualcolor2 = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud1, 255, 0, 0)
vs = pcl.pcl_visualization.PCLVisualizering
vss1 = pcl.pcl_visualization.PCLVisualizering() #初始化一个对象，这里是很重要的一步
vs.AddPointCloud_ColorHandler(vss1,
cloud1,
visualcolor2,
id=b'cloud',
viewport=0)
vs.AddPointCloud_ColorHandler(vss1,
cloud2,
visualcolor1,
id=b'cloud1',
viewport=0)
v = True
while not vs.WasStopped(vss1):
vs.Spin(vss1)
def anterior(self):
self.cont -= 1
print("Actualmente se esta representando Cloud_yaw_frame_number_" +
str(self.cont + 1) + " y Cloud_yaw_frame_number_" +
str(self.cont))
cloud_source = pcl.load('cloud_yaw_frame_number_' +
str(self.cont + 1) + '.pcd')
points_array = cloud_source.to_array()
self.points3d_draw = points3d(points_array[:, 0:1],
points_array[:, 1:2],
points_array[:, 2:3],
points_array[:, 2:3],
mode='sphere',
scale_mode='none',
scale_factor=self.scale_factor,
reset_zoom=False,
figure=self.scene2.mayavi_scene)
cloud_source = pcl.load('cloud_yaw_frame_number_' + str(self.cont) +
'.pcd')
points_array = cloud_source.to_array()
self.points3d_draw = points3d(points_array[:, 0],
points_array[:, 1],
points_array[:, 2],
color=(1, 1, 1),
mode='sphere',
scale_mode='none',
scale_factor=self.scale_factor,
reset_zoom=False,
figure=self.scene2.mayavi_scene)
def main():
# PCL Visualizer to view the pointcloud
viewer = pcl.pcl_visualization.PCLVisualizering()
# File i/o
if (len(sys.argv) > 1):
cloud = pcl.load(sys.argv[1])
else:
cloud = pcl.load("square.pcd")
# Display Text
hello = "Hello World!"
viewer.AddText(str.encode(hello), 10, 50, bytes(1), 0)
size = "Cloud Size = " + str(cloud.size)
viewer.AddText(str.encode(size), 10, 20, bytes(2), 0)
# Viewer
viewer.AddPointCloud(cloud, bytes(0))
viewer.SetPointCloudRenderingProperties(
pcl.pcl_visualization.PCLVISUALIZER_POINT_SIZE, 1.5, bytes(0))
color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud, 255, 255, 255)
viewer.AddPointCloud_ColorHandler(cloud, color, bytes(1), 0)
# Start/Stop
viewer.Spin()
viewer.RemovePointCloud(bytes(0), 0)
def add_example_to_batch(self, batch_x, batch_y, i):
random_line = random.choice(self.lines)
x_filepath, y_filepath = random_line.replace("\n", "").split(", ")
x_np_pts = pcl.load(x_filepath).to_array().astype(int)
x_mask = (x_np_pts[:, 0], x_np_pts[:, 1], x_np_pts[:, 2])
y_np_pts = pcl.load(y_filepath).to_array().astype(int)
y_mask = (y_np_pts[:, 0], y_np_pts[:, 1], y_np_pts[:, 2])
batch_y[i, :, :, :, :][y_mask] = 1
batch_x[i, :, :, :, :][x_mask] = 1
def _tactile_completion_test(completion_method, completion_name, smoothed):
depth_cloud = pcl.load('resources/depth_cloud_cf.pcd')
depth_points = depth_cloud.to_array()
tactile_cloud = pcl.load('resources/tactile_cf.pcd')
tactile_points = tactile_cloud.to_array()
# Testing depth completions only
ply_data = time_fn(completion_method, depth_points, tactile_points)
smoothed_str = ["unsmoothed", "smoothed"][smoothed]
ply_data.write(open("resources/pringles_{}_{}.ply".format(completion_name, smoothed_str), 'w'))
def get_pointxyz(self, cls, ds_type='ycb'):
if ds_type == "ycb":
cls = self.get_cls_name(cls, ds_type)
if cls in self.ycb_cls_ptsxyz_dict.keys():
return self.ycb_cls_ptsxyz_dict[cls]
ptxyz_ptn = os.path.join(
self.config.ycb_root,
'models',
'{}/points.xyz'.format(cls),
)
pointxyz = np.loadtxt(ptxyz_ptn.format(cls), dtype=np.float32)
self.ycb_cls_ptsxyz_dict[cls] = pointxyz
return pointxyz
elif ds_type == 'openDR':
ptxyz_pth = os.path.join(
'/home/ahmad3/PVN3D/pvn3d/datasets/openDR/openDR_dataset/models',
'obj_' + str(cls) + '.ply')
#pointxyz = self.ply_vtx(ptxyz_pth)
pointxyz = np.asarray(pcl.load(ptxyz_pth))
dellist = [j for j in range(0, len(pointxyz))]
dellist = random.sample(dellist, len(pointxyz) - 2000)
pointxyz = np.delete(pointxyz, dellist, axis=0)
self.lm_cls_ptsxyz_dict[cls] = pointxyz
return pointxyz
elif ds_type == 'CrankSlider':
ptxyz_pth = os.path.join(
'/home/ahmad3/PVN3D/pvn3d/datasets/CrankSlider/CrankSlider_dataset/models',
'obj_' + str(cls) + '.ply')
#pointxyz = self.ply_vtx(ptxyz_pth)
pointxyz = np.asarray(pcl.load(ptxyz_pth))
dellist = [j for j in range(0, len(pointxyz))]
dellist = random.sample(dellist, len(pointxyz) - 2000)
pointxyz = np.delete(pointxyz, dellist, axis=0)
self.lm_cls_ptsxyz_dict[cls] = pointxyz
return pointxyz
else:
ptxyz_pth = os.path.join(
'datasets/linemod/Linemod_preprocessed/models',
'obj_%02d.ply' % cls)
pointxyz = self.ply_vtx(ptxyz_pth) / 1000.0
dellist = [j for j in range(0, len(pointxyz))]
dellist = random.sample(dellist, len(pointxyz) - 2000)
pointxyz = np.delete(pointxyz, dellist, axis=0)
self.lm_cls_ptsxyz_dict[cls] = pointxyz
return pointxyz
def setUp(self):
self.pc = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"table_scene_mug_stereo_textured.pcd")
def load_pcd(self, filepath):
pcd = pcl.load(filepath)
# gt_np shape is (#pts, 3)
temp = pcd.to_array()
pcd_np = np.ones((temp.shape[0], 4))
pcd_np[:, 0:3] = temp
return pcd_np.T
def load(path, format=None, load_rgb=True):
"""
Read a pointcloud file.
Supports LAS and CSV files, and lets PCD and PLY files be
read by python-pcl.
Arguments:
path : string
Filename.
format : string, optional
File format: "PLY", "PCD", "LAS", "CSV",
or None to detect the format from the file extension.
load_rgb : bool
Whether RGB is loaded for PLY and PCD files. For LAS files, RGB is
always read.
Returns:
pc : pcl.PointCloud
"""
if format == 'las' or format is None and path.endswith('.las'):
pc = _load_las(path)
elif format == 'las' or format is None and path.endswith('.csv'):
pc = _load_csv(path)
else:
_check_readable(path)
pc = pcl.load(path, format=format, loadRGB=load_rgb)
return pc
def main():
# cloud = pcl.load_XYZRGB(
# './examples/pcldata/tutorials/table_scene_mug_stereo_textured.pcd')
# cloud = pcl.load("Tile_173078_LD_010_017_L22.obj")
# cloud = pcl.load('./table_scene_mug_stereo_textured.pcd')
cloud = pcl.load('./rabbit.pcd')
# cloud = pcl.load('./projData.pcd')
# Centred the data
# centred = cloud - np.mean(cloud, 0)
# # print(centred)
# ptcloud_centred = pcl.PointCloud()
# ptcloud_centred.from_array(centred)
# ptcloud_centred = pcl.load("Tile_173078_LD_010_017_L22.obj")
visual = pcl.pcl_visualization.CloudViewing()
# PointXYZ
# visual.ShowMonochromeCloud(ptcloud_centred, b'cloud')
visual.ShowMonochromeCloud(cloud, b'cloud')
# visual.ShowGrayCloud(ptcloud_centred, b'cloud')
# visual.ShowColorCloud(ptcloud_centred, b'cloud')
# visual.ShowColorACloud(ptcloud_centred, b'cloud')
v = True
while v:
v = not(visual.WasStopped())
def capture_RealSense(name):
points = rs.points()
success, frames = pipeline.try_wait_for_frames(timeout_ms=10)
depth_frame = frames.get_depth_frame()
other_frame = frames.first(other_stream)
color = frames.get_color_frame()
depth_frame = decimate.process(depth_frame)
if state.postprocessing:
for f in filters:
depth_frame = f.process(depth_frame)
points = pc.calculate(depth_frame)
pc.map_to(other_frame)
ply = name + ".ply"
pcd = name + ".pcd"
points.export_to_ply(ply, color)
clouding = pcl.load(ply)
pcl.save(clouding, pcd)
print("Export Reussi")
def pasre_data(self):
cloud_raw = pcl.load(self.pcd_path)
cloud_np = np.asarray(cloud_raw)
cloud_np = np.delete(cloud_np, -1, axis=1)
# fileter out zeros points
dis = np.sqrt(cloud_np[:, 0]**2 + cloud_np[:, 1]**2)
cloud_np = cloud_np[dis > 0.05]
# cloud_np = np.load(self.pcd_path)
if not self.test:
anns_list = []
with open(self.ann_path, 'r') as ann_f:
anns = list(ann_f)
for line in anns:
line = line.split('\t')[1:10]
if len(line) == 9:
line = list(map(float, line))
else:
continue
line = np.asarray(line)
line = line[[1, 2, 4, 5, 8]]
line[4] = line[4] if 0 <= line[4] < 180 else line[4] + 180
if min(line[0:2]) > 0.6 and max(line[0:2]) < 8 \
and (1 <= line[0]*line[1] <= 40):
anns_list.append(line)
if len(anns_list) == 0:
anns_list = None
anns_np = None
else:
anns_np = np.array(anns_list)
else:
anns_np = None
self.anns_np = anns_np
self.cloud_np = cloud_np
def test_pcd_read():
TMPL = """# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH %(npts)d
HEIGHT 1
VIEWPOINT 0.1 0 0.5 0 1 0 0
POINTS %(npts)d
DATA ascii
%(data)s"""
a = np.array(np.mat(SEGDATA, dtype=np.float32))
npts = a.shape[0]
tmp_file = tempfile.mkstemp(suffix='.pcd')[1]
with open(tmp_file, "w") as f:
f.write(TMPL % {"npts": npts, "data": SEGDATA.replace(";", "")})
p = pcl.load(tmp_file)
assert p.width == npts
assert p.height == 1
for i, row in enumerate(a):
pt = np.array(p[i])
ssd = sum((row - pt) ** 2)
assert ssd < 1e-6
assert_array_equal(p.sensor_orientation,
np.array([0, 1, 0, 0], dtype=np.float32))
assert_array_equal(p.sensor_origin,
np.array([.1, 0, .5, 0], dtype=np.float32))
def load(path, format=None, load_rgb=True):
"""
Read a pointcloud file.
Supports LAS and CSV files, and lets PCD and PLY files be
read by python-pcl.
Arguments:
path : string
Filename.
format : string, optional
File format: "PLY", "PCD", "LAS", "CSV",
or None to detect the format from the file extension.
load_rgb : bool
Whether RGB is loaded for PLY and PCD files. For LAS files, RGB is
always read.
Returns:
pc : pcl.PointCloud
"""
if format == 'las' or format is None and path.endswith('.las'):
pc = _load_las(path)
elif format == 'las' or format is None and path.endswith('.csv'):
pc = _load_csv(path)
else:
_check_readable(path)
pc = pcl.load(path, format=format, loadRGB=load_rgb)
return pc
def test_pcd_read():
TMPL = """
# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH %(npts)d
HEIGHT 1
VIEWPOINT 0 0 0 0 1 0 0
POINTS %(npts)d
DATA ascii
%(data)s"""
a = np.array(np.mat(SEGDATA, dtype=np.float32))
npts = a.shape[0]
with open("/tmp/test.pcd", "w") as f:
f.write(TMPL % {"npts": npts, "data": SEGDATA.replace(";", "")})
p = pcl.load("/tmp/test.pcd")
assert p.width == npts
assert p.height == 1
for i, row in enumerate(a):
pt = np.array(p[i])
ssd = sum((row - pt)**2)
assert ssd < 1e-6
def main():
cloud = pcl.load(
'/python-pcl/examples/pcldata/tutorials/table_scene_lms400.pcd')
print("cloud points : " + str(cloud.size))
vg = cloud.make_voxel_grid_filter()
vg.set_leaf_size(0.0029, 0.0029, 0.0029)
cloud_filtered = vg.filter()
print('point size after voxel_grid_filter: ' + str(cloud_filtered.size))
# cloud_filtered = pcl.PointCloud(cloud_filtered_blob.to_array())
# print('PointCloud after filtering: ' +
# str(cloud_filtered.width * cloud_filtered.height) + ' data points.')
tree = cloud_filtered.make_kdtree()
segment = cloud_filtered.make_EuclideanClusterExtraction()
segment.set_ClusterTolerance(0.005)
segment.set_MinClusterSize(100)
segment.set_MaxClusterSize(25000)
segment.set_SearchMethod(tree)
cluster_indices = segment.Extract()
# print(cluster_indices)
print('cluster_indices : ' + str(len(cluster_indices)) + " count.")
cloud_cluster = pcl.PointCloud()
for j, indices in enumerate(cluster_indices):
# print('indices = ' + str(len(indices)))
points = np.zeros((len(indices), 3), dtype=np.float32)
for i, indice in enumerate(indices):
points[i][0] = cloud_filtered[indice][0]
points[i][1] = cloud_filtered[indice][1]
points[i][2] = cloud_filtered[indice][2]
cloud_cluster.from_array(points)
def main(args):
# Should be an XYZRGB pointcloud.
file_list = npy.load(str(sys.argv[1]))
for i in range(1):
for j in range(len(file_list[i])):
print(i, j)
pc = pcl.load(file_list[i][j])
# Kill Z values beyond 1.8 m
passthrough_filter = pc.make_passthrough_filter()
passthrough_filter.set_filter_field_name('z')
# passthrough_filter.set_filter_limits(0,1.8)
passthrough_filter.set_filter_limits(0.5, 1.8)
pc = passthrough_filter.filter()
# Remove outliers
outlier_filter = pc.make_statistical_outlier_filter()
outlier_filter.set_mean_k(50)
outlier_filter.set_std_dev_mul_thresh(2)
pc = outlier_filter.filter()
# Save both a pointcloud and an array.
pcl.save(pc,
"D{0}_PCD/PointCloud_{1}.pcd".format(i + 2, j),
binary=True)
npy.save("D{0}_PCD/PointCloud_{1}.npy".format(i + 2, j),
pc.to_array())
def __getitem__(self, idx):
# BGR image
filename = self.image_files[idx]
print('filename = ', filename)
im = cv2.imread(filename)
if cfg.TRAIN.CHROMATIC and cfg.MODE == 'TRAIN' and np.random.rand(
1) > 0.1:
im = chromatic_transform(im)
if cfg.TRAIN.ADD_NOISE and cfg.MODE == 'TRAIN' and np.random.rand(
1) > 0.1:
im = add_noise(im)
im_tensor = torch.from_numpy(im) / 255.0
im_tensor_bgr = im_tensor.clone()
im_tensor_bgr = im_tensor_bgr.permute(2, 0, 1)
im_tensor -= self._pixel_mean
image_blob = im_tensor.permute(2, 0, 1)
# Label
labels_filename = filename.replace('image_color', 'annotation')
foreground_labels = util_.imread_indexed(labels_filename)
foreground_labels = self.process_label(foreground_labels)
label_blob = torch.from_numpy(foreground_labels).unsqueeze(0)
index = filename.find('OSD')
sample = {
'image_color': image_blob,
'image_color_bgr': im_tensor_bgr,
'label': label_blob,
'filename': filename[index + 4:]
}
# Depth image
if cfg.INPUT == 'DEPTH' or cfg.INPUT == 'RGBD':
pcd_filename = filename.replace('image_color', 'pcd')
pcd_filename = pcd_filename.replace('png', 'pcd')
print('pcd_filename = ', pcd_filename)
pcloud = pcl.load(pcd_filename).to_array()
pcloud[np.isnan(pcloud)] = 0
xyz_img = pcloud.reshape((self._height, self._width, 3))
depth_blob = torch.from_numpy(xyz_img).permute(2, 0, 1)
sample['depth'] = depth_blob
# # Depth image
# if cfg.INPUT == 'DEPTH' or cfg.INPUT == 'RGBD':
# pcd_filename = filename.replace('image_color', 'pcd')
# pcd_filename = pcd_filename.replace('png', 'pcd')
# # pcl replaced with open3d
# pcloud = o3d.io.read_point_cloud(pcd_filename)
# pcloud = np.asarray(pcloud)
# print(np.isnan(pcloud))
# pcloud[np.isnan(pcloud)] = 0
# xyz_img = pcloud.reshape((self._height, self._width, 3))
# depth_blob = torch.from_numpy(xyz_img).permute(2, 0, 1)
# sample['depth'] = depth_blob
return sample
def setUp(self):
self.pc = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"table_scene_mug_stereo_textured.pcd")
def readpcd(self, filename):
if filename.split('.')[1] != 'pcd':
return False
else:
self.p = pcl.load(filename)
self.points = self.p.to_array()
return True
def main():
folder = "/home/yufeng/Temp/Scanning/"
files = []
final_cloud = None
while True:
pc_files = listdir(folder)
newcloud = False
cloud = None
for f in pc_files:
print(f)
# new point cloud
if f not in files:
newpc = pcl.load(join(folder, f))
new = process(newpc)
if cloud == None:
cloud = new
else:
cloud = cloud + new
files.append(f)
newcloud = True
if newcloud == True:
if final_cloud == None:
final_cloud = cloud
else:
final_cloud = final_cloud + cloud
visual.pcl.pcl_visualization.CloudViewing()
visual.ShowMonochromeCloud(final_cloud, b'cloud')
def main():
# pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
# cloud_filtered = pcl.
# Fill in the cloud data
# pcl::PCDReader reader;
# reader.read ("table_scene_lms400.pcd", *cloud_blob);
# std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
cloud_blob = pcl.load(
'./examples/pcldata/tutorials/table_scene_lms400.pcd')
print("PointCloud before filtering: " +
str(cloud_blob.width * cloud_blob.height) + " data points.")
# Create the filtering object: downsample the dataset using a leaf size of 1cm
# pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
# sor.setInputCloud (cloud_blob);
# sor.setLeafSize (0.01f, 0.01f, 0.01f);
# sor.filter (*cloud_filtered_blob);
sor = cloud_blob.make_voxel_grid_filter()
sor.set_leaf_size(0.01, 0.01, 0.01)
cloud_filtered_blob = sor.filter()
# Convert to the templated PointCloud
# pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
# std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
cloud_filtered = pcl.PCLPointCloud2(cloud_filtered_blob.to_array())
print('PointCloud after filtering: ' +
str(cloud_filtered.width * cloud_filtered.height) + ' data points.')
# Write the downsampled version to disk
# pcl::PCDWriter writer;
# writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
pcl.save("table_scene_lms400_downsampled.pcd", cloud_filtered)
def test_pcd_read():
TMPL = """
# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH %(npts)d
HEIGHT 1
VIEWPOINT 0 0 0 0 1 0 0
POINTS %(npts)d
DATA ascii
%(data)s"""
a = np.array(np.mat(SEGDATA, dtype=np.float32))
npts = a.shape[0]
with open("/tmp/test.pcd","w") as f:
f.write(TMPL % {"npts":npts,"data":SEGDATA.replace(";","")})
p = pcl.load("/tmp/test.pcd")
assert p.width == npts
assert p.height == 1
for i,row in enumerate(a):
pt = np.array(p[i])
ssd = sum((row - pt) ** 2)
assert ssd < 1e-6
def setUp(self):
# self.p = pcl.PointCloud(_data)
self.p = pcl.load("tests" + os.path.sep + "tutorials" + os.path.sep +
"lamppost.pcd")
# 1.8.0
# self.feat = pcl.MomentOfInertiaEstimation()
self.feat = self.p.make_MomentOfInertiaEstimation()
def proc_clouds(self, dirname, path_models_file, sac_dist_thresh):
npa_pcd_files = np.loadtxt(path_models_file, usecols=(0, ), dtype='S')
npa_pcd_points = np.loadtxt(path_models_file,
usecols=(1, ),
dtype='i4')
npa_abcd = np.loadtxt(path_models_file,
usecols=(2, 3, 4, 5),
dtype='f4')
# print npa_pcd_files
print '# of planar models found: %d' % npa_pcd_files.shape[0]
# print npa_pcd_points
# print npa_abcd
if npa_pcd_files.size == 1:
# fixme: kludge.......... npa_* becomes a scalar in this case.............
li_pcd_paths = [dirname + '/' + '0.pcd']
npa_abcd = [
npa_abcd,
]
npa_pcd_points = [
npa_pcd_points,
]
else:
li_pcd_paths = [dirname + '/' + f for f in npa_pcd_files]
# Launch a PLC viewer in background
if 1:
SnapCam.killall_pcl_viewer()
# print li_pcd_paths
Popen(['pcl_viewer'] + li_pcd_paths)
# ---- legathy code
# li_planes = glob.glob(dirname+'/*.pcd')
# if li_planes:
# # call(['pcl_viewer'] + li_planes)
# # http://stackoverflow.com/questions/1196074/starting-a-background-process-in-python
# Popen(['pcl_viewer'] + li_planes)
# Refresh planar_win with selectable planes
max_planes = 50 # at most process max_planes planes
print 'for planar_win, max_planes: %d' % max_planes
if 1:
self.planar_win.clear_items()
# print li_pcd_paths[0:max_planes]
for pcd_path, abcd, points in zip(li_pcd_paths[0:max_planes],
npa_abcd[0:max_planes],
npa_pcd_points[0:max_planes]):
# plot a planar point cloud
print pcd_path
npa_pc = np.asarray(pcl.load(pcd_path))
print npa_pc.shape
self.planar_win.append_item(
pg.opengl.GLScatterPlotItem(pos=npa_pc,
size=0.05,
color=(1.0, 1.0, 0.0, 0.5),
pxMode=False))
# plot a grid that fits the point cloud
print abcd, points
self.planar_win.add_grid(abcd, npa_pc.shape[0],
sac_dist_thresh)
def main():
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
#
# pcl::PCDReader reader;
# reader.read("pcdfilename", *cloud);
cloud = pcl.load('./examples/pcldata/tutorials/table_scene_lms400.pcd')
# std::cerr<<"PointCloud befor filtering: " << cloud->width * cloud->height << "data points ( " << pcl::getFieldsList (*cloud) << ").";
# print('PointCloud befor filtering: ' + str(cloud.width * cloud.height) + 'data points ( ' + pcl.getFieldsList (cloud) + ').')
# pcl::VoxelGrid<pcl::PointXYZ> sor;
# sor.setInputCloud(cloud);
# sor.setLeafSize(0.1f, 0.1f, 0.1f);
# sor.filter(*cloud_filtered);
sor = cloud.make_voxel_grid_filter()
sor.set_leaf_size(0.01, 0.01, 0.01)
cloud_filtered = sor.filter()
# std::cerr<<"PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << "data points (" << pcl::getFieldsList(*cloud_filtered) <<").";
# print('PointCloud after filtering: ' + str(cloud_filtered.width * cloud_filtered.height) + 'data points ( ' + pcl.getFieldsList (cloud) + ').')
# pcl::PCDWriter writer;
# writer.write("savefilename", *cloud_filtered, false);
pcl.save(cloud_filtered, 'table_scene_lms400_voxelfilter.pcd')
def getDataFromPcd(path, cfg):
points = pcl.load(path).to_array()
points = np.matmul(cfg.rotMat, points.T).T
vgcfg = cfg.voxel_generator
# create voxel generator
vg = VoxelGenerator(
voxel_size=vgcfg.voxel_size,
point_cloud_range=vgcfg.range,
max_num_points=vgcfg.max_points_in_voxel,
max_voxels=vgcfg.max_voxel_num,
)
grid_size = vg.grid_size
voxels, coordinates, num_points = vg.generate(points)
num_voxels = np.array([voxels.shape[0]], dtype=np.int64)
data = dict(points=points,
voxels=voxels,
shape=grid_size,
num_points=num_points,
num_voxels=num_voxels,
coordinates=coordinates)
return data
def load_gt_np_pc(filepath):
gt_pcd = pcl.load(filepath)
# gt_np shape is (#pts, 3)
gt_np_temp = gt_pcd.to_array()
gt_np_pc = np.ones((gt_np_temp.shape[0], 4))
gt_np_pc[:, 0:3] = gt_np_temp
return gt_np_pc
def test_pcd_read():
TMPL = """# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH %(npts)d
HEIGHT 1
VIEWPOINT 0.1 0 0.5 0 1 0 0
POINTS %(npts)d
DATA ascii
%(data)s"""
a = np.array(np.mat(SEGDATA, dtype=np.float32))
npts = a.shape[0]
tmp_file = tempfile.mkstemp(suffix='.pcd')[1]
with open(tmp_file, "w") as f:
f.write(TMPL % {"npts": npts, "data": SEGDATA.replace(";", "")})
p = pcl.load(tmp_file)
assert p.width == npts
assert p.height == 1
for i, row in enumerate(a):
pt = np.array(p[i])
ssd = sum((row - pt) ** 2)
assert ssd < 1e-6
assert_array_equal(p.sensor_orientation,
np.array([0, 1, 0, 0], dtype=np.float32))
assert_array_equal(p.sensor_origin,
np.array([.1, 0, .5, 0], dtype=np.float32))
def readpcd(self,filename):
if filename.split('.')[1] != 'pcd':
return False
else:
self.p = pcl.load(filename)
self.points = self.p.to_array()
return True
def __init__(self):
# Internal USE Variables - Modify with Consultation
self.init = False
self.tf2Buffer = tf2_ros.Buffer()
self.tf2TL = tf2_ros.TransformListener(self.tf2Buffer)
self.record_once = True
self.start_pt = None
# Publishers
self.route_pub = rospy.Publisher("path", Path, queue_size=1)
self.pc2_pub = rospy.Publisher("laser_2d/display",
PointCloud2,
queue_size=1)
self.viz_points_pub = rospy.Publisher("table/viz/pathPt",
Marker,
queue_size=1)
# Subscriber
self.pc2_sub = rospy.Subscriber("laser_2d/merged_cloud",
PointCloud2,
self.pc2CB,
queue_size=1)
# Service Server
self.find_charger_service = rospy.Service("/find_charger", Trigger,
self.find_charger)
#
self.target_cloud = pcl.load(
"/home/samuel/atlas80evo-ws/src/self_charging/pcd/charger_unit.pcd"
)
def composeFrustrum(objects, calib_dir, pcd_dir, img_real_id):
if img_real_id == '000000':
pcd_id = '0'
else:
pcd_id = img_real_id.lstrip('0') # without 0
cloud = pcl.load(os.path.join(pcd_dir, '{0}.pcd'.format(pcd_id)))
points_array = np.asarray(cloud)
Tr_velo_to_cam, R0_rect, P2 = readCalibration(calib_dir, img_real_id)
filtered_points_array = [[] for x in range(len(objects))]
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 all objects
for index in range(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[index].append(points_array[ind])
filtered_points_array = [x for x in filtered_points_array if x != []]
flat_list = [item for sublist in filtered_points_array for item in sublist]
return filtered_points_array, flat_list
def setUp(self):
self.octree = pcl.OctreePointCloudSearch(0.1)
self.p = pcl.load("tests" + os.path.sep + "tutorials" + os.path.sep +
"table_scene_mug_stereo_textured_noplane.pcd")
self.octree.set_input_cloud(self.p)
self.octree.define_bounding_box()
self.octree.add_points_from_input_cloud()
def testSave(self):
# for ext in ["pcd", "ply"]:
# Mac ply read/write NG
for ext in ["pcd"]:
d = os.path.join(self.tmpdir, "foo." + ext)
pcl.save(self.p, d)
p = pcl.load(d)
self.assertEqual(self.p.size, p.size)
def setUp(self):
self.p = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"table_scene_lms400.pcd")
self.fil = self.p.make_ApproximateVoxelGrid()
def setUp(self):
self.p = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"bunny.pcd")
self.kp = self.p.make_HarrisKeypoint3D()
def setUp(self):
# self.p = pcl.PointCloud(_data)
self.p = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"table_scene_mug_stereo_textured.pcd")
# self.feat = pcl.IntegralImageNormalEstimation(self.p)
self.feat = self.p.make_IntegralImageNormalEstimation()
def load_pcd(path):
# TODO: load ply file, for the soma mesh
"""
load point cloud form pcd file
@param path: the whole path to the file
@type path: string
@return: the point cloud
@rtype: pcl::PointCloud2
"""
pc2 = pcl.load(path)
return pc2
def setUp(self):
self.octree = pcl.OctreePointCloudSearch(0.1)
self.p = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"table_scene_mug_stereo_textured_noplane.pcd")
self.octree.set_input_cloud(self.p)
self.octree.define_bounding_box()
self.octree.add_points_from_input_cloud()
def setUp(self):
# self.p = pcl.PointCloud(_data)
self.p = pcl.load(
"tests" +
os.path.sep +
"tutorials" +
os.path.sep +
"lamppost.pcd")
# 1.8.0
# self.feat = pcl.MomentOfInertiaEstimation()
self.feat = self.p.make_MomentOfInertiaEstimation()
def testLoad(self):
pc = pcl.load("tests/table_scene_mug_stereo_textured_noplane.pcd")
self.t.set_input_cloud(pc)
self.t.define_bounding_box()
self.t.add_points_from_input_cloud()
good_point = (0.035296999, -0.074322999, 1.2074)
rs = self.t.is_voxel_occupied_at_point(good_point)
self.assertTrue(rs)
bad_point = (0.5, 0.5, 0.5)
rs = self.t.is_voxel_occupied_at_point(bad_point)
self.assertFalse(rs)
voxels_len = 44
self.assertEqual(len(self.t.get_occupied_voxel_centers()), voxels_len)
self.t.delete_voxel_at_point(good_point)
self.assertEqual(len(self.t.get_occupied_voxel_centers()), voxels_len - 1)
def pull_map(): # Downloads map from database and writes it to local file
# Get map from database
# TODO: Get map from database computer
# Try to load map
try:
self.mapa = pcl.load("map.pcd")
except IOError:
# "empty" point cloud
self.mapa = pcl.PointCloud()
# Save to local file
pcl.save(self.mapa, "map.pcd")
def setUp(self):
# self.a = np.array(np.mat(SEGDATA, dtype=np.float32))
# self.p = pcl.PointCloud()
# self.p.from_array(self.a)
cloud = pcl.load('tests' + os.path.sep + 'tutorials' + os.path.sep + 'table_scene_mug_stereo_textured.pcd')
fil = cloud.make_passthrough_filter()
fil.set_filter_field_name("z")
fil.set_filter_limits(0, 1.5)
cloud_filtered = fil.filter()
seg = cloud_filtered.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_normal_distance_weight(0.1)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_max_iterations(100)
seg.set_distance_threshold(0.03)
indices, model = seg.segment()
self.p = cloud_filtered.extract(indices, negative=True)
self.segment = self.p.make_segmenter_normals(ksearch=50)
def loadPCD(filename):
"""
Load PCD file.
Returns: panda dataframe with three columns, each one representing
one coordinate (x, y, z). Each row is a sample.
"""
pointCloud = pcl.load(filename)
x = []
y = []
z = []
for sample in pointCloud:
x.append(sample[0])
y.append(sample[1])
z.append(sample[2])
df_pointCloud = pd.DataFrame()
df_pointCloud['x'] = x
df_pointCloud['y'] = y
df_pointCloud['z'] = z
return df_pointCloud
# {
# cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
# for (float x=-0.5f; x<=0.5f; x+=0.01f)
# {
# for (float y=-0.5f; y<=0.5f; y+=0.01f)
# {
# PointType point; point.x = x; point.y = y; point.z = 2.0f - y;
# point_cloud.points.push_back (point);
# }
# }
# point_cloud.width = (int) point_cloud.points.size (); point_cloud.height = 1;
# }
if len(pcd_filename_indices) != 0:
# point_cloud = pcl.load(argv[0])
point_cloud = pcl.load('test_pcd.pcd')
far_ranges_filename = 'test_pcd.pcd'
far_ranges = pcl.load_PointWithViewpoint(far_ranges_filename)
else:
setUnseenToMaxRange = True
print ('No *.pcd file given = Genarating example point cloud.\n')
count = 0
points = np.zeros((100 * 100, 3), dtype=np.float32)
# float NG
for x in range(-50, 50, 1):
for y in range(-50, 50, 1):
points[count][0] = x * 0.01
points[count][1] = y * 0.01
# Normal Estimation Using Integral Images
# http://pointclouds.org/documentation/tutorials/normal_estimation_using_integral_images.php#normal-estimation-using-integral-images
import pcl
import pcl.pcl_visualization
# cloud = pcl.load('table_scene_mug_stereo_textured.pcd')
cloud = pcl.load('./examples/pcldata/tutorials/table_scene_mug_stereo_textured.pcd')
print ('load table_scene_mug_stereo_textured.pcd')
# estimate normals
# pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
# pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
print ('make_IntegralImageNormalEstimation: ')
ne = cloud.make_IntegralImageNormalEstimation()
print ('set_NormalEstimation_Method_AVERAGE_3D_GRADIENT: ')
ne.set_NormalEstimation_Method_AVERAGE_3D_GRADIENT ()
print ('set_MaxDepthChange_Factor: ')
ne.set_MaxDepthChange_Factor(0.02)
print ('set_NormalSmoothingSize: ')
ne.set_NormalSmoothingSize(10.0)
print ('set OK')
print ('compute2 - start')
normals = ne.compute2(cloud)
print ('compute2 - end')
print (str(normals.size))
# visualize normals
viewer = pcl.pcl_visualization.PCLVisualizering()
# -*- coding: utf-8 -*-
# http://pointclouds.org/documentation/tutorials/voxel_grid.php#voxelgrid
# http://derivecv.tumblr.com/post/13631147204
# http://nisot0710.blogspot.jp/2014/09/pclvoxelgridpclpointcloud2.html
# PCLPointCloud2 is 1.7.2
import pcl
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
#
# pcl::PCDReader reader;
# reader.read("pcdfilename", *cloud);
cloud = pcl.load('./examples/pcldata/tutorials/table_scene_lms400.pcd')
# std::cerr<<"PointCloud befor filtering: " << cloud->width * cloud->height << "data points ( " << pcl::getFieldsList (*cloud) << ").";
# print('PointCloud befor filtering: ' + str(cloud.width * cloud.height) + 'data points ( ' + pcl.getFieldsList (cloud) + ').')
# pcl::VoxelGrid<pcl::PointXYZ> sor;
# sor.setInputCloud(cloud);
# sor.setLeafSize(0.1f, 0.1f, 0.1f);
# sor.filter(*cloud_filtered);
sor = cloud.make_voxel_grid_filter()
sor.set_leaf_size(0.1, 0.1, 0.1)
cloud_filtered = sor.filter()
# std::cerr<<"PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << "data points (" << pcl::getFieldsList(*cloud_filtered) <<").";
# print('PointCloud after filtering: ' + str(cloud_filtered.width * cloud_filtered.height) + 'data points ( ' + pcl.getFieldsList (cloud) + ').')
# pcl::PCDWriter writer;
# writer.write("savefilename", *cloud_filtered, false);
def setUp(self):
self.t = pcl.OctreePointCloudSearch(0.1)
pc = pcl.load("tests/table_scene_mug_stereo_textured_noplane.pcd")
self.t.set_input_cloud(pc)
self.t.define_bounding_box()
self.t.add_points_from_input_cloud()
def setUp(self):
self.p = pcl.load("tests/flydracyl.pcd")
def setUp(self):
self.p = pcl.load("tests/table_scene_mug_stereo_textured_noplane.pcd")
import pcl
import octomap
import numpy as np
p = pcl.load('/home/ros/TestM/Meowth.nvm.cmvs/Meowth.0.ply','ply')
print(p)
pcl.save(p,'/home/ros/TestM/Meowth.nvm.cmvs/Meowth.pcd','pcd')
p = pcl.load('/home/ros/TestM/Meowth.nvm.cmvs/Meowth.pcd')
fil = p.make_statistical_outlier_filter()
fil.set_mean_k (50)
fil.set_std_dev_mul_thresh (1.0)
fil.filter().to_file("/home/ros/TestM/Meowth.nvm.cmvs/inliersSparse.pcd")
def setUp(self):
self.p = pcl.load("tests" + os.path.sep + "flydracyl.pcd")
self.reg = pcl.GeneralizedIterativeClosestPoint()
def setUp(self):
self.p = pcl.load("tests" + os.path.sep + "flydracyl.pcd")
self.surf = self.p.make_moving_least_squares()
def setUp(self):
self.p = pcl.load("tests" + os.path.sep + "flydracyl.pcd")
self.surf = self.p.make_ConcaveHull()
# Removing outliers using a StatisticalOutlierRemoval filter
# http://pointclouds.org/documentation/tutorials/statistical_outlier.php#statistical-outlier-removal
import pcl
p = pcl.load("table_scene_lms400.pcd")
fil = p.make_statistical_outlier_filter()
fil.set_mean_k(50)
fil.set_std_dev_mul_thresh(1.0)
pcl.save(fil.filter(), "table_scene_lms400_inliers.pcd")
fil.set_negative(True)
pcl.save(fil.filter(), "table_scene_lms400_outliers.pcd")
def setUp(self):
self.p = pcl.load("tests" + os.path.sep + "flydracyl.pcd")
self.reg = pcl.IterativeClosestPointNonLinear()
def setUp(self):
self.p = pcl.load("tests/table_scene_mug_stereo_textured_noplane.pcd")
self.tmpdir = tempfile.mkdtemp(suffix='pcl-test')
