def NonFirstCloud(self, cloud_index): self.cloud1 = o3d.read_point_cloud( r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\2-{}_Extract.ply".format(cloud_index - 1)) self.cloud2 = o3d.read_point_cloud( r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\2-{}_Extract.ply".format(cloud_index)) # get local transformation between two lastest clouds self.posLocalTrans = self.registerLocalCloud(self.cloud1, self.cloud2) # if result is not good, drop it if self.goodResultFlag: # test for loop detection info self.detectTransLoop = np.dot(self.posLocalTrans, self.detectTransLoop) # print ("==== loop detect trans ====") # print(self.detectTransLoop) # print ("==== ==== ==== ==== ==== ====") self.posWorldTrans = np.dot(self.posWorldTrans, self.posLocalTrans) # update latest cloud self.cloud1 = copy.deepcopy(self.cloud2) self.cloud2.transform(self.posWorldTrans) o3d.write_point_cloud(r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\test.ply", self.cloud2, write_ascii=False) self.cloud_base = self.cloud_base + self.cloud2 # Down Sampling self.cloud_base = o3d.voxel_down_sample(self.cloud_base, 0.001) self.registrationCount += 1 # save PCD file to local o3d.write_point_cloud(r"C:\Users\newsmart\Desktop\NSTAlgo\3D\PCL\stitch\2\registerResult.ply", self.cloud_base, write_ascii=False)
def assignYslice(ySlice, deckTop, p2, ny, zMax, zMin, write): pierArea = [] deckArea = [] #don't forget deckX contains all the x slices of the deck red = np.diag(np.divide([255, 0, 0],255)) blue = np.diag(np.divide([0, 0, 255],255)) #deck top is in deckTop[i][row,3] and should correspond to ySlice[i][:][row,3] for k in range(len(ySlice)):#42 x slices #note, using the whole x-slice zmax as the local zmax is not################################################## #entirely accurate. If issues arise, consider finding #the local zmax for each box seperately localZmax = np.max(deckTop[k][:,2]) for i in range(ny):#20 y slices per x slice pcd_export = open3d.PointCloud() pcd_export.points = open3d.Vector3dVector(ySlice[k][i]) localZmin = np.min(ySlice[k][i][:,2]) #localZmax = np.max(ySlice[k][i][:,2]) if (localZmax-localZmin)>p2*(zMax-zMin): rgb = np.matmul(np.ones((len(ySlice[k][i]),3)),blue) pierArea.append(ySlice[k][i]) filename = "../data/step3/nDslice_" + str(len(pierArea)-1) + ".pcd" else: rgb = np.matmul(np.ones((len(ySlice[k][i]),3)),red) deckArea.append(ySlice[k][i]) filename = "../data/step3/Dslice" + str(len(deckArea)-1) + ".pcd" if write: pcd_export.colors = open3d.Vector3dVector(rgb) open3d.write_point_cloud(filename, pcd_export) return pierArea, deckArea
def input_preprocess(data_dir, id, save_dir): # rgbd -> point cloud # start_time = time.time() pcd = rgbd_to_point_cloud(data_dir, id) # print("rgbd->pcd: ", time.time() - start_time) # calculate local normal for point cloud cal_local_normal(pcd) # print("cal normal: ", time.time() - start_time) # select referenced points (default number 2048) ref_pcd = select_referenced_point(pcd) # print("select ref: ", time.time() - start_time) ref_pts = np.asarray(ref_pcd.points) # assert ref_pts.shape[0] == 2048 # collect local patch for each reference point neighbor = collect_local_neighbor(ref_pcd, pcd) # print("collect neighbor:", time.time() - start_time) # assert len(neighbor) == ref_pts.shape[0] # calculate the point pair feature for each patch local_patch = build_local_patch(ref_pcd, pcd, neighbor) # print("cal ppf for each pair:", time.time() - start_time) # save the local_patch and reference point cloud for one point cloud fragment. if not os.path.exists(save_dir): os.makedirs(save_dir) np.save(f'{save_dir}/{id}.npy', local_patch.astype(np.float32)) open3d.write_point_cloud(f"{save_dir}/{id}.pcd", ref_pcd)
def save(self, color, depth, mask, img_disp, obj_bboxes, obj_3d_centers, cloud): self.idx += 1 print "saveing {:05d}th data to file ... ".format(self.idx) index = int2str(self.idx, 5) + "_" # Write images cv2.imwrite(self.fsimage + "/" + index + self.simage + ".png", color) cv2.imwrite(self.fsdepth + "/" + index + self.sdepth + ".png", depth) cv2.imwrite(self.fsmask + "/" + index + self.smask + ".png", mask) cv2.imwrite(self.fsresimg + "/" + index + self.sresimg + ".png", img_disp) # Write bounding box and other info with open(self.fsobjects + "/" + index + self.sobjects + ".txt", 'w') as f: for i in range(len(obj_bboxes)): # object index f.write(str(i)) # image size f.write(": " + str(color.shape[0:2])) # bounding box f.write(": " + obj_bboxes[i].converToStr()) # object 3d centers f.write(": " + str(obj_3d_centers[i])) f.write("\n") # Write cloud data open3d.write_point_cloud( self.fsclouds + "/" + index + self.sclouds + ".pcd", cloud) return
def down_sample(dense_pcd_path, dense_label_path, sparse_pcd_path, sparse_label_path, voxel_size): # Skip if done if os.path.isfile(sparse_pcd_path) and ( not os.path.isfile(dense_label_path) or os.path.isfile(sparse_label_path)): print("Skipped:", file_prefix) return else: print("Processing:", file_prefix) # Inputs dense_pcd = open3d.read_point_cloud(dense_pcd_path) try: dense_labels = load_labels(dense_label_path) except: dense_labels = None # Skip label 0, we use explicit frees to reduce memory usage print("Num points:", np.asarray(dense_pcd.points).shape[0]) if dense_labels is not None: non_zero_indexes = dense_labels != 0 dense_points = np.asarray(dense_pcd.points)[non_zero_indexes] dense_pcd.points = open3d.Vector3dVector() dense_pcd.points = open3d.Vector3dVector(dense_points) del dense_points dense_colors = np.asarray(dense_pcd.colors)[non_zero_indexes] dense_pcd.colors = open3d.Vector3dVector() dense_pcd.colors = open3d.Vector3dVector(dense_colors) del dense_colors dense_labels = dense_labels[non_zero_indexes] print("Num points after 0-skip:", np.asarray(dense_pcd.points).shape[0]) # Downsample points min_bound = dense_pcd.get_min_bound() - voxel_size * 0.5 max_bound = dense_pcd.get_max_bound() + voxel_size * 0.5 sparse_pcd, cubics_ids = open3d.voxel_down_sample_and_trace( dense_pcd, voxel_size, min_bound, max_bound, False) print("Num points after down sampling:", np.asarray(sparse_pcd.points).shape[0]) open3d.write_point_cloud(sparse_pcd_path, sparse_pcd) print("Point cloud written to:", sparse_pcd_path) # Downsample labels if dense_labels is not None: sparse_labels = [] for cubic_ids in cubics_ids: cubic_ids = cubic_ids[cubic_ids != -1] cubic_labels = dense_labels[cubic_ids] sparse_labels.append(np.bincount(cubic_labels).argmax()) sparse_labels = np.array(sparse_labels) write_labels(sparse_label_path, sparse_labels) print("Labels written to:", sparse_label_path)
def cb_save(msg): global Model,tfReg #save point cloud for n,l in enumerate(Config["scenes"]): if Scene[n] is None: continue pc=o3d.PointCloud() m=Scene[n] Model[n]=m pc.points=o3d.Vector3dVector(m) o3d.write_point_cloud(Config["path"]+"/"+l+".ply",pc,True,False) pub_pcs[n].publish(np2F(m)) tfReg=[] #copy TF scene...->master... and save them for n,s in enumerate(Config["scene_frame_id"]): try: tf=tfBuffer.lookup_transform("world",s,rospy.Time()) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): pub_msg.publish("searcher::capture::lookup failure world->"+s) tf=TransformStamped() tf.header.stamp=rospy.Time.now() tf.header.frame_id="world" tf.child_frame_id=s tf.transform.rotation.w=1 path=Config["path"]+"/"+Config["master_frame_id"][n].replace('/','_')+".yaml" f=open(path,"w") f.write(yaml.dump(tflib.tf2dict(tf.transform))) f.close() tfReg.append(tf) broadcaster.sendTransform(tfReg) solver.learn(Model,Param) pub_msg.publish("searcher::model learning completed")
def removeDeckTop(notDeckX, write): deckTop = [] red = np.diag(np.divide([255, 0, 0],255)) blue = np.diag(np.divide([0, 0, 255],255)) for i in range(len(notDeckX)): #print("Length slice before: " + str(len(notDeckX[i]))) yMin = np.min(notDeckX[i][:,1]) yMax = np.max(notDeckX[i][:,1]) zMax = np.max(notDeckX[i][:,2]) deltaY = (yMax-yMin) deltaZ = 0.05*deltaY notDeckX[i] = notDeckX[i][np.argsort(notDeckX[i][:,2]), :] BR = bisect.bisect_left(notDeckX[i][:,2],zMax-deltaZ) deckTop.append(notDeckX[i][BR:,:]) notDeckX[i] = notDeckX[i][:BR,:] #print("yMin %.3f\t yMax %.3f\t deltaY %.3f\t deltaZ %.3f\t BR %.3f\t"%(yMin,yMax,deltaY,deltaZ,BR)) #print("Length slice after: " + str(len(notDeckX[i]))) if write: filename = "../data/step2_5/Dtop_" + str(len(deckTop)-1) + ".pcd" pcd_export = open3d.PointCloud() pcd_export.points = open3d.Vector3dVector(deckTop[i]) rgb = np.matmul(np.ones((len(deckTop[i]),3)),red) pcd_export.colors = open3d.Vector3dVector(rgb) open3d.write_point_cloud(filename, pcd_export) filename = "../data/step2_5/Parea_" + str(i) + ".pcd" pcd_export = open3d.PointCloud() pcd_export.points = open3d.Vector3dVector(notDeckX[i]) rgb = np.matmul(np.ones((len(notDeckX[i]),3)),blue) pcd_export.colors = open3d.Vector3dVector(rgb) open3d.write_point_cloud(filename, pcd_export) return notDeckX, deckTop
def savePCs(filename,pcs,pc): ''' saves a merged point cloud and also the separated pointclouds Args: filename String: what should the name be pcs [open3d.pointcloud]: list of all the retrieved pointclouds ''' global saveInc saveInc=saveInc+1 print(saveInc) print(filename) filename = filename+"_"+str(saveInc) #creates a file for the merged pointcloud open3d.write_point_cloud("./PC/"+filename+".ply", pc) #create a folder for the unmerged pointclouds try: os.mkdir("./PC/"+filename) except: print("PA") #save the individual pointclouds for i in range(len(pcs)): print(pcs[i]) open3d.write_point_cloud("./PC/"+filename+"/pointcloud"+str(i)+".ply", pcs[i])
def cb_save_ply(msg): d = outPn.astype(np.float32) pc = o3d.PointCloud() pc.points = o3d.Vector3dVector(d) print 'save model PC', d.dtype, d.shape o3d.write_point_cloud(Args["wd"] + "/sample.ply", pc, True, False) return
def point_cloud_txt_to_pcd(raw_dir, file_prefix): # File names txt_file = os.path.join(raw_dir, file_prefix + ".txt") pts_file = os.path.join(raw_dir, file_prefix + ".pts") pcd_file = os.path.join(raw_dir, file_prefix + ".pcd") # Skip if already done if os.path.isfile(pcd_file): print("pcd {} exists, skipped".format(pcd_file)) return # .txt to .pts # We could just prepend the line count, however, there are some intensity value # which are non-integers. print("[txt->pts]") print("txt: {}".format(txt_file)) print("pts: {}".format(pts_file)) with open(txt_file, "r") as txt_f, open(pts_file, "w") as pts_f: for line in txt_f: # x, y, z, i, r, g, b tokens = line.split() tokens[3] = str(int(float(tokens[3]))) line = " ".join(tokens) pts_f.write(line + "\n") prepend_line(pts_file, str(wc(txt_file))) # .pts -> .pcd print("[pts->pcd]") print("pts: {}".format(pts_file)) print("pcd: {}".format(pcd_file)) point_cloud = open3d.read_point_cloud(pts_file) open3d.write_point_cloud(pcd_file, point_cloud)
def convert(input_dir, obj_file_name_1, mtl_file_name_1, obj_file_name_2, mtl_file_name_2, ply_file_name, n, nosf=False, write_ascii=True, u=None, v=None, random_indices=None): materials_1 = parse_mtl(mtl_file_name_1) vertices_1, faces_1, textures_1, parts_1 = parse_obj( input_dir, obj_file_name_1, materials_1) materials_2 = parse_mtl(mtl_file_name_2) vertices_2, faces_2, textures_2, parts_2 = parse_obj( input_dir, obj_file_name_2, materials_2) if u is None or v is None or random_indices is None: points_1, colors_1, random_indices, u, v = sample( vertices_1, faces_1, textures_1, parts_1, n, input_dir, materials_1) else: points_1, colors_1, random_indices, u, v = sample( vertices_1, faces_1, textures_1, parts_1, n, input_dir, materials_1, u=u, v=v, random_indices=random_indices) points_2, colors_2, random_indices, u, v = sample( vertices_2, faces_2, textures_2, parts_2, n, input_dir, materials_2, random_indices=random_indices, u=u, v=v) flow_xyz = points_2 - points_1 pc = open3d.PointCloud() pc.points = open3d.Vector3dVector(points_1) pc.colors = open3d.Vector3dVector(colors_1) if not nosf: pc.normals = open3d.Vector3dVector(flow_xyz) open3d.write_point_cloud(ply_file_name, pc, write_ascii=write_ascii) # open3d.draw_geometries([pc]) return random_indices, u, v
def export(self,k,color): #print("I'm not done yet") filename = "../data/elements/cluster" + str(k) + "_" + str(self.x) + "," + str(self.y) + ".pcd" pcd_export = open3d.PointCloud() pcd_export.points = open3d.Vector3dVector(self.points) rgb = np.matmul(np.ones((len(self.points),3)),np.diag(color)) pcd_export.colors = open3d.Vector3dVector(rgb) open3d.write_point_cloud(filename, pcd_export)
def color_deviations(config, result, distances): """ Use the Open3d visualization function to colorize deviations after convergence """ if isinstance(result, PointCloud) == True: result = open3d.PointCloud() result.points = open3d.Vector3dVector(result.points) elif isinstance(result, numpy.ndarray) == True: result = open3d.PointCloud() result.points = open3d.Vector3dVector(result) elif isinstance(result, open3d.PointCloud) == True: pass else: raise TypeError( "Point clouds need to be of type (ndarray), (PointCloud) or (open3d.PointCloud)!" ) # Needed Paths OPEN3D_BUILD_PATH = "/home/chris/Documents/Masterarbeit/Code/virt_env/env/lib/python3.6/site-packages/Open3D/build/" OPEN3D_EXPERIMENTAL_BIN_PATH = OPEN3D_BUILD_PATH + "/bin/Experimental/" # TODO use os.findpath or something to make this dynamic DATASET_DIR = "/home/chris/Documents/Masterarbeit/Code/virt_env/env/ICP/src/bin" # Log files MY_LOG_POSTFIX = "_COLMAP_SfM.log" MY_RECONSTRUCTION_POSTFIX = "_COLMAP.ply" # File structure mvs_outpath = DATASET_DIR + config["colorize_deviations"]["output_path"] "/../../data/set1/output/evaluation/" scene = config["colorize_deviations"]["scene"] + "/" # Make directory make_dir(mvs_outpath) make_dir(mvs_outpath + "/" + scene) # New log files new_logfile = DATASET_DIR + MY_LOG_POSTFIX colmap_ref_logfile = DATASET_DIR + scene + "_COLMAP_SfM.log" mvs_file = mvs_outpath + scene + MY_RECONSTRUCTION_POSTFIX # Write the distances to bin files numpy.array(distances).astype("float64").tofile(mvs_outpath + "/" + scene + ".precision.bin") # Colorize the poincloud files with the precision and recall values open3d.write_point_cloud(mvs_outpath + "/" + scene + ".precision.ply", result) open3d.write_point_cloud(mvs_outpath + "/" + scene + ".precision.ncb.ply", result) result_n_fn = mvs_outpath + "/" + scene + ".precision.ply" eval_str_viewDT = OPEN3D_EXPERIMENTAL_BIN_PATH + "ViewDistances " + result_n_fn + " --max_distance " + str( numpy.asarray(distances).max()) + " --write_color_back --without_gui" os.system(eval_str_viewDT) result_colored = open3d.read_point_cloud(result_n_fn) open3d.draw_geometries([result_colored]) return
def write_to_point_cloud(data, colors, idx): points = np.asarray(data, dtype=np.float32) colors = np.asarray(colors, dtype=np.float32) cloud = o3.PointCloud() cloud.points = o3.Vector3dVector(points) cloud.colors = o3.Vector3dVector(colors) print(f'writing to pointcloud{idx}.pcd') o3.write_point_cloud(f'pointcloud{idx}.pcd', cloud) o3.write_point_cloud(f'pointcloud{idx}.xyzrgb', cloud)
def points_save(points, colors, root='pcds/regions', child='all', pfile=''): os.makedirs(root, exist_ok=True) os.makedirs(root + '/' + child, exist_ok=True) pcd = o3d.PointCloud() pcd.points = o3d.Vector3dVector(np.float32(points)) pcd.colors = o3d.Vector3dVector(np.float32(colors)) o3d.write_point_cloud(os.path.join(root, '%s.pcd' % pfile), pcd, write_ascii=True, compressed=True)
def ply2xyz(self, FN): fileDir = "C:/Users/Ozguc Capunaman/Documents/GitHub/InteractiveDigitalFabrication/sessions/" + self._session srcDir = fileDir + "/" + FN + ".ply" ply = open3d.read_triangle_mesh(srcDir) xyz = open3d.PointCloud() srcDir = fileDir + "/" + FN xyz.points = ply.vertices targetDir = fileDir + "/" + FN + ".xyz" open3d.write_point_cloud(targetDir, xyz)
def on_lidar_frame(self, msg): self._pc_msg_cnt += 1 if self._pc_msg_cnt % self._flags.log_every_nth_frame != 0: return # Write the lidar information. file_name = '{}carla-lidar-{}.ply'.format( self._flags.data_path, msg.timestamp.coordinates[0]) pcd = o3d.PointCloud() pcd.points = o3d.Vector3dVector(msg.point_cloud) o3d.write_point_cloud(file_name, pcd)
def ply2xyz(filename="", srcPath="../ply/", targetPath="../xyz/"): srcDir = srcPath + filename + ".ply" ply = open3d.read_triangle_mesh(srcDir) xyz = open3d.PointCloud() xyz.points = ply.vertices targetDir = targetPath + filename + ".xyz" print(targetDir) open3d.write_point_cloud(targetDir, xyz) return xyz
def postprocess(pc_path, json_path, r): # find rotation of the point cloud, so it's alligned with z axis pcd = o3d.read_point_cloud(pc_path) # find transformation of the point cloud, that will allign it with x axis pcd1 = copy.deepcopy(pcd) rot_trans_straight1 = su.calc_centre_correction(pcd1) pcd1.transform(rot_trans_straight1) rot_trans_straight2 = su.calc_centre_correction(pcd1) pcd1.transform(rot_trans_straight2) # do this twice and combine results rot_trans_straight = np.dot(rot_trans_straight2, rot_trans_straight1) # make the points start from z=0 points = np.asarray(pcd.points) z_trans = min(points[:, 2]) rot_trans_straight [2, 3] -= z_trans pcd.transform(rot_trans_straight) # transform 2d np array to 2d list l_transformation = [list(r) for r in rot_trans_straight] print(l_transformation) # append transformation to the end of json file with open(json_path, mode='r', encoding='utf-8') as feedsjson: feeds = json.load(feedsjson) with open(json_path, mode='w', encoding='utf-8') as feedsjson: entry = {'postprocessing_transformation': l_transformation} feeds.append(entry) json.dump(feeds, feedsjson) output_folder, _ = split(json_path) o3d.write_point_cloud(join(output_folder, 'stitched_straight.pcd'), pcd) points = np.asarray(pcd.points) # points to flat surface points = cart2pol_pc(points, r) # save points in the point cloud pcd.points = o3d.Vector3dVector(points) # find normals so it's displayed nicer # o3d.estimate_normals(pcd, search_param = o3d.KDTreeSearchParamHybrid( # radius = 5, max_nn = 30)) # o3d.draw_geometries([pcd]) output_path = join(output_folder, 'flatten.pcd') o3d.write_point_cloud(output_path, pcd) return output_path
def main(): args = load_args() input_x = sorted(os.listdir(args.x)) input_y = sorted(os.listdir(args.y)) for x in input_x: x = split(x, '.')[0] for y in input_y: y = split(y, '.')[0] if x == y: o3d.write_point_cloud(args.o + x, x) o3d.write_point_cloud(args.o + y, y)
def get_grasp_solution(self, req): resp = GraspSolutionResponse() if self.grasp_generator.has_solutions(): solution = self.grasp_generator.get_solution(self.sidx) resp.joint_angles = solution.joint_angles resp.base_pose = list(solution.base_pose[0]) + list( solution.base_pose[1]) for i in range(len(solution.joint_trajectory)): grasp_waypoint = GraspWaypoint() grasp_waypoint.base_pose = solution.base_trajectory[ i].p.tolist() + solution.base_trajectory[i].q.tolist() grasp_waypoint.joint_angles = solution.joint_trajectory[ i].tolist() resp.trajectory.waypoints.append(grasp_waypoint) #lift waypoint grasp_waypoint = GraspWaypoint() grasp_waypoint.joint_angles = solution.joint_trajectory[-1].tolist( ) grasp_waypoint.base_pose = solution.lift_base_pose.p.tolist( ) + solution.lift_base_pose.q.tolist() resp.trajectory.waypoints.append(grasp_waypoint) resp.cloud_min_pt = np.amin(self.point_cloud.points(), axis=0) resp.cloud_max_pt = np.amax(self.point_cloud.points(), axis=0) resp.cloud_centroid = self.point_cloud.compute_centroid() if SAVE_DATA: import open3d as o3d self.cloud_name = raw_input("Object name: ") models_path = get_aml_package_path('aml_data') filepath = models_path + '/exp_data/' + self.cloud_name + '.pcd' o3d.write_point_cloud(filepath, self.point_cloud._cloud, True) print "Cloud data saved to: ", filepath models_path = get_aml_package_path('aml_data') filepath = models_path + '/exp_data/' + self.cloud_name + "_grasps.pkl" save_data( { 'solution': solution, 'sidx': self.sidx, 'solution_list': self.grasp_generator.get_solutions() }, filepath) print "Solution data saved to: ", filepath pos = resp.base_pose[0:3] quat = resp.base_pose[3:7] self.br.sendTransform(pos, quat, rospy.Time.now(), "/target/j2n6s300_link_6", "world") return resp
def process_single_fragment(cfg, color_files, depth_files, frag_id, n_frags, intrinsic_path, out_folder): import open3d as o3d depth_only_flag = (len(color_files) == 0) n_frames = len(depth_files) intrinsic = read_intrinsic(intrinsic_path, cfg.width, cfg.height) if depth_only_flag: color_type = o3d.integration.TSDFVolumeColorType.__dict__['None'] else: color_type = o3d.integration.TSDFVolumeColorType.__dict__['RGB8'] volume = o3d.integration.ScalableTSDFVolume( voxel_length=cfg.tsdf_cubic_size / 512.0, sdf_trunc=0.04, color_type=color_type) sid = frag_id * cfg.frames_per_frag eid = min(sid + cfg.frames_per_frag, n_frames) pose_base2world = None pose_base2world_inv = None for fid in range(sid, eid): if not depth_only_flag: color_path = color_files[fid] else: color_path = None depth_path = depth_files[fid] pose_path = depth_path[:-10] + '.pose.txt' pose_cam2world = read_extrinsic(pose_path) if pose_cam2world is None: continue if fid == sid: # Use as base frame pose_base2world = pose_cam2world pose_base2world_inv = np.linalg.inv(pose_base2world) if pose_base2world_inv is None: break # Relative camera pose pose_cam2world = np.matmul(pose_base2world_inv, pose_cam2world) rgbd = read_rgbd_image(cfg, color_path, depth_path, False) volume.integrate(rgbd, intrinsic, np.linalg.inv(pose_cam2world)) if pose_base2world_inv is None: return pcloud = volume.extract_point_cloud() o3d.geometry.estimate_normals(pcloud) o3d.write_point_cloud( os.path.join(out_folder, 'cloud_bin_{}.ply'.format(frag_id)), pcloud) np.save(os.path.join(out_folder, 'cloud_bin_{}.pose.npy'.format(frag_id)), pose_base2world)
def visualize_trajectory(numDirs=8, dists=[.1, .3, .5], steps=10): dino = DinoAgent() center = dino.center print("dino is located at: " + str(center)) offset_vals = [-.1, .1] offsets = [np.array([i, j, k]) for i in offset_vals for j in offset_vals for k in offset_vals] + [np.array([0, 0, 0])] centers = [np.array(center) + offset for offset in offsets] ptcloud, sc, start_view = dino.visualize_ptcloud('greedy', 80) print("start view is: " + str(start_view)) params = getCandidateParams(start_view, numDirs, centers, dists) print("params: " + str(params)) cand_view_list = [dirToView(d, start_view, c, dist) for (d, c, dist) in params] cand_traj_pts = cand_view_list for (d, c, dist) in params: print("param: " + str((d, c, dist))) int_pts = interpolateTarget(start_view, d, steps, dist, c) pts_list = [int_pts.get() for i in range(int_pts.qsize())] print("points: " + str(pts_list)) cand_traj_pts += pts_list (greedy_dir, greedy_c, greedy_dist) = chooseGreedyAction(start_view, dists, numDirs, sc, centers) print("greedy params: d {}, c {}, dist {}".format(greedy_dir, greedy_c, greedy_dist)) greedy_traj = interpolateTarget(start_view, greedy_dir, steps, greedy_dist, greedy_c) greedy_pts_list = [greedy_traj.get() for i in range(greedy_traj.qsize())] pcd = open3d.geometry.PointCloud() pcd.points = open3d.Vector3dVector(ptcloud[:, 0:3]) pcd.paint_uniform_color([1,0.706,0]) total_ptcloud = np.vstack((ptcloud[:, 0:3], np.array(cand_traj_pts)[:, 0:3])) tpcd = open3d.geometry.PointCloud() tpcd.points = open3d.Vector3dVector(total_ptcloud) open3d.estimate_normals(tpcd, search_param = open3d.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 100)) open3d.orient_normals_to_align_with_direction(tpcd) tpcd.paint_uniform_color([1,0.706,0]) greedy_pcd = open3d.geometry.PointCloud() greedy_pcd.points = open3d.Vector3dVector(np.array(greedy_pts_list)[:, 0:3]) greedy_pcd.paint_uniform_color([0, 1, 1]) traj_pcd = open3d.geometry.PointCloud() traj_pcd.points = open3d.Vector3dVector(np.array(cand_traj_pts)[:, 0:3]) traj_pcd.paint_uniform_color([1,0.706,0]) open3d.visualization.draw_geometries([pcd, greedy_pcd]) open3d.write_point_cloud("trajectory_viz.ply", tpcd)
def colorize(input_pcd_path, input_labels_path, output_pcd_path): pcd = open3d.read_point_cloud(input_pcd_path) print("Point cloud loaded from", input_pcd_path) labels = load_labels(input_labels_path) print("Labels loaded from", input_labels_path) s = time.time() colorize_point_cloud(pcd, labels) print("time colorize_point_cloud pd", time.time() - s, flush=True) open3d.write_point_cloud(output_pcd_path, pcd) print("Output written to", output_pcd_path)
def main(radius, name): cloud = epc.compress((epc[:, -2] > radius) & (radius >= epc[:, -1]), 0) pcd = PointCloud() pcd.points = Vector3dVector(cloud[:, :3]) pcd.colors = Vector3dVector(cloud[:, 3:6]) write_point_cloud(name, pcd) print( pcd, cloud[:, -1].max(), cloud[:, -3].sum(), cloud[:, -4].max(), cloud[:, -5].sum(), ) return None
def save(self, timestamp: int, data_path: str, file_base: str): """Saves the point cloud to a file. Args: timestamp (:obj:`int`): Timestamp associated with the point cloud. data_path (:obj:`str`): Path where to save the point cloud. file_base (:obj:`str`): Base name of the file. """ import open3d as o3d file_name = os.path.join(data_path, '{}-{}.ply'.format(file_base, timestamp)) pcd = o3d.PointCloud() pcd.points = o3d.Vector3dVector(self.points) o3d.write_point_cloud(file_name, pcd)
def compare(parameter): picindex1 = 000 picindex2 = 111 print("welcome to compare") pc1, image1, indexlist1 = parameter['0'] pc2, image2, indexlist2 = parameter['1'] o3d.write_point_cloud("combine.ply", pc1 + pc2, write_ascii=True) print("image shape", image1.shape) points1 = np.array(pc1.points) points2 = np.array(pc2.points) print("point count:", len(points1)) print("point count:", len(points2)) nbrs = NearestNeighbors(n_neighbors=1, n_jobs=10).fit(points2) print("neighbor match finish") distance, indices = nbrs.kneighbors(points1) del nbrs #thersh = np.sqrt(distance) thersh = np.percentile(np.array(distance), 10) #thersh = 2e-2 #showing print("thersh is", thersh) plt.hist(distance, bins=100) plt.ylabel('Distance of neighbor points' + str(picindex1) + "+" + str(picindex2)) plt.show() #match match = [] print("count ", len(distance)) for i in range(len(distance)): if points1[i][0] + points1[i][1] + points1[i][2] == 0: continue if distance[i] < thersh: match.append([i, indices[i][0]]) #o3d.visualization.draw_geometries([pc1, pc2]) ###watch mid point ################ bs = 3 blockStat(bs, match, indexlist1, indexlist2, image1, image2) bs = 7 blockStat(bs, match, indexlist1, indexlist2, image1, image2) bs = 15 blockStat(bs, match, indexlist1, indexlist2, image1, image2) bs = 31 blockStat(bs, match, indexlist1, indexlist2, image1, image2) bs = 63 blockStat(bs, match, indexlist1, indexlist2, image1, image2) return
def save_cam_ply(cam_fn=None, show=False, with_walls=True, with_pcl=True): if cam_fn is None: house_name = '0004d52d1aeeb8ae6de39d6bd993e992_A' parsed_dir = f'{SUNCG_V1_DIR}/parsed/{house_name}' cam_fn = os.path.join(parsed_dir, 'cam') cam_fn = '/home/z/parsed/28297783bce682aac7fb35a1f35f68fa/cam' cam_pos = read_cam_pos(cam_fn) parsed_dir = os.path.dirname(cam_fn) cam_num = cam_pos.shape[0] cam_centroid = cam_pos[:, 0:3] # cam centroid box tmp = np.array([[0.1, 0.1, 0.1, 0]]) tmp = np.tile(tmp, [cam_num, 1]) cam_cen_box = cam_pos[:, 0:3] cam_cen_box = np.concatenate([cam_centroid, tmp], 1) cam_cen_box = Bbox3D.bboxes_lineset(cam_cen_box, 'Z', False, random_color=False) # cam orientations cam_ori = cam_centroid + cam_pos[:, 3:6] cam_centroid = np.expand_dims(cam_centroid, 1) cam_ori = np.expand_dims(cam_ori, 1) cam_vec = np.concatenate([cam_centroid, cam_ori], 1) cam_lines = Bbox3D.draw_lines_open3d(cam_vec, color=[200, 0, 0], show=False) pcl_fn = os.path.dirname(cam_fn) + '/pcl_camref.ply' if show: if with_pcl and os.path.exists(pcl_fn): pcl = open3d.read_point_cloud(pcl_fn) open3d.draw_geometries(cam_cen_box + [cam_lines, pcl]) if with_walls: bbox_fn = f'{parsed_dir}/object_bbox/wall.txt' walls = np.loadtxt(bbox_fn).reshape([-1, 7]) walls = world2cam_box(walls) bboxes_lineset_ls = Bbox3D.bboxes_lineset(walls, 'Y', False) open3d.draw_geometries(cam_cen_box + [cam_lines] + bboxes_lineset_ls) # save cam pos as ply pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(cam_pos[:, 0:3]) cam_ply_fn = cam_fn + '_pos.ply' open3d.write_point_cloud(cam_ply_fn, pcd)
def main_save_cloud(): global received_ros_cloud rate = rospy.Rate(100) cnt = 0 file_folder = "/home/feiyu/baxterws/src/winter_prj/mask_objects_from_rgbd/data/" file_name = "pcd_" while not rospy.is_shutdown(): if received_ros_cloud is not None: cnt += 1 open3d_cloud = convertCloudFromRosToOpen3d(received_ros_cloud) filename_to_save = file_folder + file_name + "{:03d}".format( cnt) + ".pcd" open3d.write_point_cloud(filename_to_save, open3d_cloud) print("Saving" + filename_to_save) received_ros_cloud = None # clear rate.sleep()
def main(): f_depth_x = 1.0580353759137747e+03 f_depth_y = 1.0604471766882732e+03 c_depth_x = 9.5177505245974123e+02 c_depth_y = 5.4839653660364445e+02 depth_path1 = 'Depth/2028.png' img1_depth = np.array(imread(depth_path1)) camera_matrix = np.array([[1 / f_depth_x, 0., -c_depth_x / f_depth_x], [0., 1 / f_depth_y, -c_depth_y / f_depth_y], [0., 0., 1.]]) camera_matrix_opencv = np.array([[f_depth_x, 0., c_depth_x], [0., f_depth_y, c_depth_y], [0., 0., 1.]]) # pcl1 = pcl_from_open3d(img1_RGB, img1_depth, camera_matrix) # pcl2 = pcl_from_open3d(img2_RGB, img2_depth, camera_matrix) pcl1 = pcl_from_depth_image(img1_depth, camera_matrix) o3d.write_point_cloud("pcl1.ply", pcl1)