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 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 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 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')