def mandelbulb_evolution(K=200, p=8.0, N=10, bounds=((-1.5, 1.5), (-1.5, 1.5), (-1.5, 1.5)), optimize=False): M = T.ftensor4() C = T.ftensor4() n = T.fscalar() def step(X): r = T.sqrt( T.square(X[:, :, :, 0]) + T.square(X[:, :, :, 1]) + T.square(X[:, :, :, 2])) phi = T.arctan2(X[:, :, :, 1], X[:, :, :, 0]) theta = T.arctan2( T.sqrt(T.square(X[:, :, :, 0]) + T.square(X[:, :, :, 1])), X[:, :, :, 2]) X_ = T.stack((T.pow(r, n) * T.sin(n * theta) * T.cos(n * phi), T.pow(r, n) * T.sin(n * theta) * T.sin(n * theta), T.pow(r, n) * T.cos(n * theta)), axis=-1) return X_ + C f_ = theano.function([M, C, n], step(M), allow_input_downcast=True) import open3d from scipy.misc import imresize from skimage import measure def f(X, c, q): vert_list = [] for j in range(N): print 'step {%d}' % j X = f_(X, c, q) R = np.sqrt(np.square(X[:,:,:,0]) + \ np.square(X[:,:,:,1]) + \ np.square(X[:,:,:,2])) B = 1.0 * (R < 2.0) verts, faces = measure.marching_cubes_classic(B, 0) for k in range(3): verts[:, k] = (bounds[k][1] - bounds[k][0]) * ( 1.0 * verts[:, k]) / K + bounds[k][0] faces = measure.correct_mesh_orientation(B, verts, faces) vert_list.append(verts) return vert_list x, y, z = np.meshgrid( np.linspace(bounds[0][0], bounds[0][1], K, dtype=np.float32), np.linspace(bounds[1][0], bounds[1][1], K, dtype=np.float32), np.linspace(bounds[2][0], bounds[2][1], K, dtype=np.float32)) X = np.stack((x, y, z), axis=-1) v = f(np.zeros((K, K, K, 3)), X, p) images = [] def render_pointcloud(pcd): vis = open3d.Visualizer() vis.create_window() vis.add_geometry(pcd) ctr = vis.get_view_control() ctr.rotate(202.0, 202.0) vis.update_geometry() vis.poll_events() vis.update_renderer() image = np.asarray(vis.capture_screen_float_buffer(False)) return image for k in range(len(v) - 1) + range(len(v) - 1)[::-1]: pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(np.array(v[k + 1])) open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) image = render_pointcloud(pcd) image = np.array(255 * render_pointcloud(pcd), dtype=np.uint8) images.append(image) gif_name = 'MandelbulbEvolution.gif' output_file = os.path.join(__file__.split('.')[0], gif_name) imageio.mimsave(output_file, images, duration=0.2)
if cam2_depth_array[m2, n2] > 0: x2, y2, z2 = rgbdTools.getPosition(cam2, cam2_depth_array, m2, n2) else: continue cam1_point.append([x1, y1, z1]) cam2_point.append([x2, y2, z2]) Transformation = registration.rigid_transform_3D(np.asarray(cam1_point), np.asarray(cam2_point)) print(Transformation) geometrie_added = False vis = o3d.Visualizer() vis.create_window("Pointcloud") pointcloud = o3d.PointCloud() try: time_beigin = time.time() while True: time_start = time.time() pointcloud.clear() frames1 = pipeline1.wait_for_frames() frames2 = pipeline2.wait_for_frames() aligned_frames1 = align.process(frames1) aligned_frames2 = align.process(frames2) color_frame1 = aligned_frames1.get_color_frame() depth_frame1 = aligned_frames1.get_depth_frame()
snap_steps = [int(f[:-5].split('-')[-1]) for f in os.listdir(snap_path) if f[-5:] == '.meta'] # Find which snapshot to restore chosen_step = np.sort(snap_steps)[-1] chosen_snap = os.path.join(path, 'snapshots', 'snap-{:d}'.format(chosen_step)) tester = RegTester(model, restore_snap=chosen_snap) # calculate descriptors tester.generate_descriptor(model, dataset) # Load the descriptors and estimate the transformation parameters using RANSAC src_pcd = open3d.read_point_cloud("demo_data/cloud_bin_0.ply") src_data = np.load("demo_data/cloud_bin_0.npz") src_features = open3d.registration.Feature() src_features.data = src_data["features"].T src_keypts = open3d.PointCloud() src_keypts.points = open3d.Vector3dVector(src_data["keypts"]) src_scores = src_data["scores"] tgt_pcd = open3d.read_point_cloud("demo_data/cloud_bin_1.ply") tgt_data = np.load("demo_data/cloud_bin_1.npz") tgt_features = open3d.registration.Feature() tgt_features.data = tgt_data["features"].T tgt_keypts = open3d.PointCloud() tgt_keypts.points = open3d.Vector3dVector(tgt_data["keypts"]) tgt_scores = tgt_data["scores"] result_ransac = execute_global_registration(src_keypts, tgt_keypts, src_features, tgt_features, 0.05) # First plot the original state of the point clouds draw_registration_result(src_pcd, tgt_pcd, np.identity(4))
def __init__(self, xyz): self.pcd = open3d.PointCloud() self.pcd.points = open3d.Vector3dVector(xyz) self.pcd_tree = open3d.KDTreeFlann(self.pcd)
def convert(input_dir, obj_file_name_1, mtl_file_name_1, obj_file_name_2, mtl_file_name_2, ply_sf_file_name, ply_nm_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, normals_1, random_indices, u, v = sample( vertices_1, faces_1, textures_1, parts_1, n, input_dir, materials_1) else: points_1, colors_1, normals_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, normals_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 # == SCENE FLOW == pc = open3d.PointCloud() pc.points = open3d.Vector3dVector(points_1) pc.colors = open3d.Vector3dVector(colors_1) pc.normals = open3d.Vector3dVector(flow_xyz) open3d.write_point_cloud(ply_sf_file_name, pc, write_ascii=write_ascii) # == NORMALS == pc = open3d.PointCloud() pc.points = open3d.Vector3dVector(points_1) pc.colors = open3d.Vector3dVector(colors_1) pc.normals = open3d.Vector3dVector(normals_1) open3d.write_point_cloud(ply_nm_file_name, pc, write_ascii=write_ascii) return random_indices, u, v
# Basic inference and visualization loop MAX_SAMPLES = 15 for samples in range(MAX_SAMPLES): random_index = randrange(len(test_dataset_seg)) print('[Sample {} / {}]'.format(random_index, len(test_dataset_seg))) # clean visualization visualizer.clear() clear_output() # get next sample point_set, seg = test_dataset_seg.__getitem__(random_index) # create cloud for visualization cloud = o3.PointCloud() cloud.points = o3.Vector3dVector(point_set) cloud.colors = o3.Vector3dVector(read_pointnet_colors(seg.numpy())) # perform inference in GPU points = Variable(point_set.unsqueeze(0)) points = points.transpose(2, 1) if torch.cuda.is_available(): points = points.cuda() pred_logsoft, _ = classifier(points) # move data back to cpu for visualization pred_logsoft_cpu = pred_logsoft.data.cpu().numpy().squeeze() pred_soft_cpu = np.exp(pred_logsoft_cpu) pred_class = np.argmax(pred_soft_cpu)
period = 20 o3d.set_verbosity_level(o3d.VerbosityLevel.Debug) if True: import glob pose_files = glob.glob('stream/*.pose') num_frames = len(pose_files) frame_trace = [i for i in range(num_frames) if i % period == 0] #frame_trace += [i for i in range(450, 480, 2)] #frame_trace += [445] #compareA = args.compareA #对比的两个点云 #compareB = args.compareB frame_trace = sorted(frame_trace) pcd_partial = o3d.PointCloud() last_T = None def custom_draw_geometry_with_view_tracking(mesh, generate, period): def track_view(vis): global frame, poses global num_frames, last_T global parameter ctr = vis.get_view_control() if frame == 0: params = ctr.convert_to_pinhole_camera_parameters() intrinsics = params.intrinsic extrinsics = params.extrinsic pose = np.array(
probing_points_transformed[0:3, :], buccal) front_selected_points = select_closest_point( probing_points_transformed[3:6, :], front) lingual_selected_points = select_closest_point( probing_points_transformed[6:9, :], lingual) occlusal_selected_points = select_closest_point( probing_points_transformed[9:12, :], occlusal) destination_points = np.ones(3) destination_points = np.vstack((destination_points, buccal_selected_points)) destination_points = np.vstack((destination_points, front_selected_points)) destination_points = np.vstack((destination_points, lingual_selected_points)) destination_points = np.vstack((destination_points, occlusal_selected_points)) destination_points = np.asarray(destination_points)[1:, :] # estimate surface normals at points surface_pcd = o3d.PointCloud() surface_pcd.points = o3d.Vector3dVector(surface) o3d.geometry.estimate_normals( surface_pcd, search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.6, max_nn=30)) surface_normal = np.asarray(surface_pcd.normals) print('shape of points is', np.shape(surface)) print('shape of normals is', np.shape(surface_normal)) # prepare destination points normals buccal_selected_points_idx = np.zeros(len( buccal_selected_points)) # the idx in surface to find the right normals front_selected_points_idx = np.zeros(len(front_selected_points)) lingual_selected_points_idx = np.zeros(len(lingual_selected_points)) occlusal_selected_points_idx = np.zeros(len(occlusal_selected_points))
(166, 6), dtype=float) # (X,Y,Z) + (X,Y,Z) of the normal vector at that point count = 0 with open('Pointclouds/bunny', 'r') as pcdfile: for line in pcdfile: j = 0 for word in line.split(): data_frompcd[count][j] = float(word) j = j + 1 count = count + 1 # print(data_frompcd) pcd_arr = data_frompcd[:, 0:3] # the normal vector information is stripped out original_pcd = open3d.PointCloud() original_pcd.points = open3d.Vector3dVector(pcd_arr) original_pcd.paint_uniform_color([0, 0, 1]) # entirely blue # open3d.draw_geometries([point_cloud]) # Will be used as step size variables: d_pos = 0.2 d_neg = 0.2 ####################################################################################### # Follow the normal vector to create training data outside the original surface: points_out_0 = [data_frompcd[:, 0] + d_neg * data_frompcd[:, 3]] points_out_1 = [data_frompcd[:, 1] + d_neg * data_frompcd[:, 4]] points_out_2 = [data_frompcd[:, 2] + d_neg * data_frompcd[:, 5]]
check_pc5 = np.vstack((check_pc5, landmark_fiducial4)) plot_3d_pc(check_pc5) print('check corrected ios image') new_mesh = mesh.Mesh.from_file( "G:\\My Drive\\Project\\IntraOral Scanner Registration\\IOS_scan_raw_data\\IOS Splint\\demo_scan\\2nd_splint\\scan3\\5242021-Demo_2nd_splint_trial3-lowerjaw.stl" ) original_stl_points = np.copy(new_mesh.points)[:, 0:3] original_stl_points = transpose_pc(original_stl_points, trans_rigid0) original_stl_points = transpose_pc(original_stl_points, trans_rigid) original_stl_points2 = np.copy(new_mesh.points)[:, 0:3] #original_stl_points2 = transpose_pc(original_stl_points2, trans_rigid0) original_stl_points2 = transpose_pc(original_stl_points2, trans_rigid2) pc_stl_plot = o3d.PointCloud() pc_dicom_plot = o3d.PointCloud() pc_stl_plot.points = o3d.Vector3dVector(original_stl_points) pc_stl_plot.paint_uniform_color([0, 0.651, 0.929]) # yellow pc_dicom_plot.points = o3d.Vector3dVector(original_stl_points2) pc_dicom_plot.paint_uniform_color([1, 0.706, 0]) # blue o3d.visualization.draw_geometries([pc_dicom_plot, pc_stl_plot]) # convert python ct point coordinate to Yomiplan # Y is flipped with y_data = (slice_properties[4] - surface_idx[i][0]) * slice_properties[1][0] # Z is flipped with z_data = (slice_properties[2] - z) * slice_properties[0] # In the current dicom, slice_properties are # [slice_thickness, slice_pixel_spacing, n, slice_rows, slice_columns, slice_columns, slope, intercept] # ["0.2", [0.200, 0.200], 304, 800, 800, "1.0", "0.0"] arch_ct.update_spline()
def register2Fragments(id1, id2, keyptspath, descpath, resultpath, logpath, gtLog, desc_name, inlier_ratio, distance_threshold): """ Register point cloud {id1} and {id2} using the keypts location and descriptors. """ cloud_bin_s = f'cloud_bin_{id1}' cloud_bin_t = f'cloud_bin_{id2}' write_file = f'{cloud_bin_s}_{cloud_bin_t}.rt.txt' if os.path.exists(os.path.join(resultpath, write_file)): return 0, 0, 0 source_keypts = get_keypts(keyptspath, cloud_bin_s) target_keypts = get_keypts(keyptspath, cloud_bin_t) source_desc = get_desc(descpath, cloud_bin_s, desc_name) target_desc = get_desc(descpath, cloud_bin_t, desc_name) source_desc = np.nan_to_num(source_desc) target_desc = np.nan_to_num(target_desc) # Select {num_keypts} points based on the scores. The descriptors and keypts are already sorted based on the detection score. num_keypts = 250 source_keypts = source_keypts[-num_keypts:, :] source_desc = source_desc[-num_keypts:, :] target_keypts = target_keypts[-num_keypts:, :] target_desc = target_desc[-num_keypts:, :] # Select {num_keypts} points randomly. # num_keypts = 250 # source_indices = np.random.choice(range(source_keypts.shape[0]), num_keypts) # target_indices = np.random.choice(range(target_keypts.shape[0]), num_keypts) # source_keypts = source_keypts[source_indices, :] # source_desc = source_desc[source_indices, :] # target_keypts = target_keypts[target_indices, :] # target_desc = target_desc[target_indices, :] key = f'{cloud_bin_s.split("_")[-1]}_{cloud_bin_t.split("_")[-1]}' if key not in gtLog.keys(): # skip the pairs that have less than 30% overlap. num_inliers = 0 inlier_ratio = 0 gt_flag = 0 else: # build correspondence set in feature space. corr = build_correspondence(source_desc, target_desc) # calculate the inlier ratio, this is for Feature Matching Recall. gt_trans = gtLog[key] frag1 = source_keypts[corr[:, 0]] frag2_pc = open3d.PointCloud() frag2_pc.points = open3d.utility.Vector3dVector(target_keypts[corr[:, 1]]) frag2_pc.transform(gt_trans) frag2 = np.asarray(frag2_pc.points) distance = np.sqrt(np.sum(np.power(frag1 - frag2, 2), axis=1)) num_inliers = np.sum(distance < distance_threshold) if num_inliers / len(distance) < inlier_ratio: print(key) print("num_corr:", len(corr), "inlier_ratio:", num_inliers / len(distance)) inlier_ratio = num_inliers / len(distance) gt_flag = 1 # calculate the transformation matrix using RANSAC, this is for Registration Recall. source_pcd = open3d.PointCloud() source_pcd.points = open3d.utility.Vector3dVector(source_keypts) target_pcd = open3d.PointCloud() target_pcd.points = open3d.utility.Vector3dVector(target_keypts) s_desc = open3d.registration.Feature() s_desc.data = source_desc.T t_desc = open3d.registration.Feature() t_desc.data = target_desc.T result = open3d.registration_ransac_based_on_feature_matching( source_pcd, target_pcd, s_desc, t_desc, 0.05, open3d.TransformationEstimationPointToPoint(False), 3, [open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9), open3d.CorrespondenceCheckerBasedOnDistance(0.05)], open3d.RANSACConvergenceCriteria(50000, 1000)) # write the transformation matrix into .log file for evaluation. with open(os.path.join(logpath, f'{desc_name}_{timestr}.log'), 'a+') as f: trans = result.transformation trans = np.linalg.inv(trans) s1 = f'{id1}\t {id2}\t 37\n' f.write(s1) f.write(f"{trans[0,0]}\t {trans[0,1]}\t {trans[0,2]}\t {trans[0,3]}\t \n") f.write(f"{trans[1,0]}\t {trans[1,1]}\t {trans[1,2]}\t {trans[1,3]}\t \n") f.write(f"{trans[2,0]}\t {trans[2,1]}\t {trans[2,2]}\t {trans[2,3]}\t \n") f.write(f"{trans[3,0]}\t {trans[3,1]}\t {trans[3,2]}\t {trans[3,3]}\t \n") # write the result into resultpath so that it can be re-shown. s = f"{cloud_bin_s}\t{cloud_bin_t}\t{num_inliers}\t{inlier_ratio:.8f}\t{gt_flag}" with open(os.path.join(resultpath, f'{cloud_bin_s}_{cloud_bin_t}.rt.txt'), 'w+') as f: f.write(s) return num_inliers, inlier_ratio, gt_flag
def open3d_icp_normal(pa, pb, trans_init=np.eye(4), max_distance=0.02, b_graph=False): ''' (Transformation A->B) pa : numpy array pb : numpy array return: transformation - 4x4 numpy array result - transformation - fitness - inlier_rmse - correspondence_set ''' import open3d as op3 import copy def draw_registration_result(source, target, transformation): source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) source_temp.paint_uniform_color([1, 0.706, 0]) target_temp.paint_uniform_color([0, 0.651, 0.929]) source_temp.transform(transformation) op3.draw_geometries([source_temp, target_temp]) pc1 = op3.PointCloud() pc1.points = op3.Vector3dVector(pa) pc2 = op3.PointCloud() pc2.points = op3.Vector3dVector(pb) op3.estimate_normals(pc1, search_param=op3.KDTreeSearchParamHybrid(radius=10.0, max_nn=30)) op3.estimate_normals(pc2, search_param=op3.KDTreeSearchParamHybrid(radius=10.0, max_nn=30)) # op3.draw_geometries([pc1, pc2]) # max_distance = 0.2 # trans_init = np.asarray( # [[0.862, 0.011, -0.507, 0.5], # [-0.139, 0.967, -0.215, 0.7], # [0.487, 0.255, 0.835, -1.4], # [0.0, 0.0, 0.0, 1.0]]) # trans_init = np.eye(4) # tsl = pb.mean(axis=0) - pa.mean(axis=0) # trans_init = TransformationMatrixFromEulerAngleTranslation(euler, tsl) # trans_init = transform #### # print("Apply point-to-point ICP") #### reg_p2p = op3.registration_icp(pc1, pc2, max_distance, trans_init, #### # op3.TransformationEstimationPointToPoint()) #### op3.TransformationEstimationPointToPoint(), #### op3.ICPConvergenceCriteria(max_iteration = 2000)) #### # print(reg_p2p) #### # print("Transformation is:") #### # print(reg_p2p.transformation) #### # print(EulerAngleTranslationFromTransformationMatrix(reg_p2p.transformation)) #### # print("") # print("Apply point-to-plane ICP") reg_p2l = op3.registration_icp( pc1, pc2, max_distance, trans_init, op3.TransformationEstimationPointToPlane(), op3.ICPConvergenceCriteria(max_iteration=2000)) # print(reg_p2l) # print("Transformation is:") # print(reg_p2l.transformation) # print("") if b_graph: draw_registration_result(pc1, pc2, reg_p2l.transformation) # print(reg_p2p.transformation) # print(reg_p2p.fitness) # print(reg_p2p.inlier_rmse) # print(reg_p2p.correspondence_set) return reg_p2l.transformation, reg_p2l
visi_tgt_fit = list(set(visi_tgt) - set(hf_list)) verts_gt_visi = np.array([verts_gt[ind] for ind in visi_gt]) verts_tgt_visi = np.array([verts_tgt[ind] for ind in visi_tgt]) verts_tgt_visi_nhf = np.array([verts_tgt[ind] for ind in visi_tgt_fit]) # compute scale ave_dist_gt = np.linalg.norm(np.mean(verts_gt_visi, axis=0)) ave_dist_tgt = np.linalg.norm(np.mean(verts_tgt_visi, axis=0)) scale = ave_dist_gt / ave_dist_tgt verts_tgt *= scale verts_tgt_visi *= scale verts_tgt_visi_nhf *= scale # do ICP source = open3d.PointCloud() target = open3d.PointCloud() source.points = open3d.Vector3dVector(verts_tgt_visi_nhf) target.points = open3d.Vector3dVector(verts_gt_visi) reg_p2p = open3d.registration_icp( source, target, 10, np.identity(4), open3d.TransformationEstimationPointToPoint(), open3d.ICPConvergenceCriteria(max_iteration=10000), ) trans_mat = reg_p2p.transformation verts_tgt_fit = np.zeros(verts_tgt.shape) verts_tgt_visi_fit = np.zeros(verts_tgt_visi.shape)
def preprocess_point_cloud(points, voxel_size): source = o3d.PointCloud() source.points = o3d.Vector3dVector(points) print(":: Downsample with a voxel size %.3f." % voxel_size) pcd_down = o3d.geometry.voxel_down_sample(source, voxel_size) return pcd_down.points
import open3d as op import numpy as np import subprocess #model1 = np.loadtxt(); points = np.random.rand(10000, 3) point_cloud = op.PointCloud() point_cloud.points = op.Vector3dVector(points) op.draw_geometries([point_cloud])
#SEED = [94, 267, 265] # tooth 21 #SEED = [59, 246, 265] # tooth 22 SEED = [316, 354, 265] # tooth 17 #[323, 360, 285] z_tho = 230 SRG_LOOPS = 45 CUTOFF = -200 region_candidate = region(SEED) srg_3d(region_candidate, scan, z_threshold=z_tho) region_candidate.update_final_boundary(z_threshold=z_tho) import open3d as o3d source = o3d.PointCloud() source.points = o3d.Vector3dVector(region_candidate.boundary_final) source.paint_uniform_color([0, 0.651, 0.929]) # yellow o3d.draw_geometries([source]) #plt.figure() #ax = plt.axes(projection='3d') #print('shape of boundary is', np.shape(region_candidate.boundary_final)) #Yomiplot.plot_3d_points(region_candidate.boundary_final, ax, color='green', alpha=0.3, axe_option=False) #plt.show() exit() #img2 = cv2.imread(HU) HU_square = np.square(HU)
n_size = 100 points = MakePoints(plane.f_rep, bbox=bbox, grid_step=30) p_size = points.shape[0] print(p_size) xmin, xmax, ymin, ymax, zmax, zmin = AABB noise = np.array([[Random(xmin, xmax), Random(ymin, ymax), Random(zmin, zmax)] for i in range(n_size)]) points = np.concatenate([points, noise]) size = points.shape[0] #点群をnp配列⇒open3d形式に pointcloud = open3d.PointCloud() pointcloud.points = open3d.Vector3dVector(points) # color colors3 = np.asarray(pointcloud.colors) print(colors3.shape) colors3 = np.array([[255, 130, 0] if i < p_size else [0, 0, 255] for i in range(size)]) / 255 pointcloud.colors = open3d.Vector3dVector(colors3) # 法線推定 open3d.estimate_normals(pointcloud, search_param=open3d.KDTreeSearchParamHybrid( radius=5, max_nn=100)) # 法線の方向を視点ベースでそろえる
def process_lidar_all(data_path,store_path_general,frame_val,mdy_dict,mst_s_dict,mst_v_dict,vid_to_id_map,res_scale = 4): id_vehicles_with_sensor = mst_s_dict.keys() id_vehicles = mst_v_dict.keys() observation_matrix = np.zeros([len(id_vehicles),len(id_vehicles)])*np.nan distance_matrix = np.zeros([len(id_vehicles),len(id_vehicles)]) bounding_box_cy = np.zeros([len(id_vehicles),len(id_vehicles),4]) bounding_box_3d = np.zeros([len(id_vehicles),len(id_vehicles),9,3]) world_lp = [] for ego_vid in id_vehicles_with_sensor: store_path = os.path.join(store_path_general,str(ego_vid)) ego_pid = vid_to_id_map[ego_vid] ego_s_mst = mst_s_dict[ego_vid][f_sensor_string] ego_sen_id = ego_s_mst['id'] ego_mdy_dict = mdy_dict_frame[ego_sen_id] ego_location = ego_mdy_dict['translation'] # rdu.get_sensor_data_file_path(f, SEN_LIDAR_LABEL, ego_vid, data_path, LIDAR_EXT)) input_data_path = os.path.join(data_path,str(ego_vid),str(frame_val)+'_'+str(ego_vid)+LIDAR_EXT) lidar_data = od.read_point_cloud(input_data_path) lidar_np = np.asarray(lidar_data.points) lp_w = gu.ego_to_world_sensor(lidar_np.transpose(),ego_mdy_dict) lidar_cy = lu.lidar_to_cy(lidar_np, res_scale) lidar_colored = lidar_cy lidarp_cy = PImage.fromarray(np.uint8(lidar_colored), 'RGB') print(frame_val, ego_vid) file_name = os.path.join(store_path, '%d_%d.png' % (frame_val, ego_vid)) lidarp_cy.save(file_name) world_lp +=[lp_w] world_lp_all = np.concatenate(world_lp,axis=1) # axis_point = np.zeros([3,300]) # axis_point[0,0:10] = np.arange(10) # axis_point[1, 10:20] = np.arange(10) # axis_point[2, 20:30] = np.arange(10) # x_axis = np.arange(-10.0,10.0,.2) # xy = np.meshgrid(x_axis,x_axis) # xyz_plane = np.stack(xy+[np.zeros_like(xy[0])],axis=0) # xyz_plane = xyz_plane[:].reshape([3, -1, 1]).squeeze() # world_lp_all = np.concatenate([world_lp_all,axis_point],axis=1) world_lp_all_ego = gu.world_to_ego_sensor(world_lp_all,ego_mdy_dict) # world_lp_all_ego = world_lp_all_ego.transpose() # world_lp_all_ego = np.concatenate([world_lp_all_ego,axis_point,xyz_plane],axis=1) world_lp_all_ego = world_lp_all_ego.transpose() bc = {} bc['minX'] = -80; bc['maxX'] = 80; bc['minY'] = -80; bc['maxY'] = 80 bc['minZ'] = -10; bc['maxZ'] = 10 world_clean = world_lp_all_ego world_clean = lu.removePoints(world_lp_all_ego,bc) img = lu.makeBVFeature(world_clean,None,Discretization=160.0/1024.0) img = img*255 # a = np.int8(img) # a = np.array(np.zeros([500,500,3])) # # a[400:512,400:512,1]=1 # a[400:500, 400:500, 1] = 255 # a = np.int8(a) pimg = PImage.fromarray(img.astype('uint8'), 'RGB') # b = np.asarray(pimg) pimg.show() pcd = od.PointCloud() pcd.points = od.Vector3dVector(world_clean) od.draw_geometries([pcd]) # if ego_vid == 550 and frame_val == 278260: # all_points = np.concatenate(all_points, axis=1) # # all_points = np.concatenate([points, all_points], axis=1) # pcd = od.PointCloud() # pcd.points = od.Vector3dVector(np.transpose(all_points)) # od.draw_geometries([pcd]) # img_od_cyl = od.Image(lidar_cy.astype(np.uint8)) return {'frame':frame_val,'observation':observation_matrix,'distance':distance_matrix,'bb_2d':bounding_box_cy,'bb_3d':bounding_box_3d}
import pykitti import open3d import time import numpy as np basedir = "/home/ylao/data/kitti" date = "2011_09_26" drive = "0001" pcd = open3d.PointCloud() vis = open3d.Visualizer() vis.create_window() vis.add_geometry(pcd) render_option = vis.get_render_option() render_option.point_size = 0.01 data = pykitti.raw(basedir, date, drive) to_reset_view_point = True for points_with_intensity in data.velo: points = points_with_intensity[:, :3] pcd.points = open3d.Vector3dVector(points) vis.update_geometry() if to_reset_view_point: vis.reset_view_point(True) to_reset_view_point = False vis.poll_events() vis.update_renderer() time.sleep(0.2)
def visualize_points(points): point_cloud = open3d.PointCloud() point_cloud.points = open3d.Vector3dVector(points) open3d.draw_geometries([point_cloud])
def __init__(self): self.vis_cloud = open3d.PointCloud() self.viewer = open3d.Visualizer() self.viewer.create_window() self.viewer.add_geometry(self.vis_cloud)
predict = ResNet.inference(model_path, img_path) predict = predict.reshape(185) im_name = ntpath.basename(img_path) im_name = im_name.split(im_name.split('.')[-1])[0][0:-1] out_file = out_dir + '/' + im_name np.savetxt(out_file + '_est.txt', predict, fmt="%f") est_shape, est_exp = project2bfm09(predict) est_geom = est_shape + est_exp est = open3d.PointCloud() est.points = open3d.Vector3dVector(est_geom) label_path = img_path[:-3] + 'txt' label = np.asarray(read_txt(label_path)) truth_shape, truth_exp = project2bfm09(label, use_std=False) truth_geom = truth_shape + truth_exp truth = open3d.PointCloud() truth.points = open3d.Vector3dVector(truth_geom + np.array([200000, 0, 0])) open3d.draw_geometries([est, truth])
# 6DoF pose annotator # Shuichi Akizuki, Chukyo Univ. # Email: [email protected] # import open3d as o3 import numpy as np import cv2 import copy import argparse import os import common3Dfunc as c3D from math import * """ Object model to be transformed """ CLOUD_ROT = o3.PointCloud() """ Total transformation""" all_transformation = np.identity(4) """ Step size for rotation """ step = 0.1 * pi """ Voxel size for downsampling""" voxel_size = 0.005 def get_argumets(): """ Parse arguments from command line """ parser = argparse.ArgumentParser( description='Interactive 6DoF pose annotator') parser.add_argument('--cimg', type=str,
# drives=["0095", "0001"], drives=["0095"], box_size_x=hyper_params["box_size_x"], box_size_y=hyper_params["box_size_y"], ) # Model max_batch_size = 128 # The more the better, limited by memory size predictor = PredictInterpolator( checkpoint_path=flags.ckpt, num_classes=dataset.num_classes, hyper_params=hyper_params, ) # Init visualizer dense_pcd = open3d.PointCloud() vis = open3d.Visualizer() vis.create_window() vis.add_geometry(dense_pcd) render_option = vis.get_render_option() render_option.point_size = 0.05 to_reset_view_point = True for kitti_file_data in dataset.list_file_data: timer = { "load_data": 0, "predict_interpolate": 0, "visualize": 0, "write_data": 0, "total": 0, }
# poses.append(to_rotation_matrix(R, T)) kitti = pykitti.odometry("./KITTI_ODOMETRY", "00") poses = [ torch.from_numpy(kitti.poses[i] @ kitti.calib.T_cam0_velo) for i in range(len(kitti.poses)) ] map_file = args.map first_frame = int(args.start) last_frame = min(len(poses), int(args.end)) kitti = pykitti.odometry(args.kitti_folder, sequence) if map_file is None: pc_map = [] pcl = o3.PointCloud() print("started making the map") for i in tqdm(range(first_frame, last_frame)): pc = kitti.get_velo(i) valid_indices = pc[:, 0] < -3. valid_indices = valid_indices | (pc[:, 0] > 3.) valid_indices = valid_indices | (pc[:, 1] < -3.) valid_indices = valid_indices | (pc[:, 1] > 3.) pc = pc[valid_indices].copy() intensity = pc[:, 3].copy() pc[:, 3] = 1. RT = poses[i].numpy() pc_rot = np.matmul(RT, pc.T) pc_rot = pc_rot.astype(np.float).T.copy() pcl_local = o3.PointCloud()
# rd = re(np.array(gtRots[j], dtype=np.float32).reshape(3, 3), rmat) # xyz = te((np.array(gtPoses[j], dtype=np.float32)*0.001), (otvec.T)) print('--------------------- ICP refinement -------------------') cv2.rectangle( image, (int(box[0]), int(box[1])), (int(box[0]) + int(box[2]), int(box[1]) + int(box[3])), (0, 0, 0), 3) pcd_img = create_point_cloud(image_dep, fxkin, fykin, cxkin, cykin, 0.001) pcd_img = pcd_img.reshape( (480, 640, 3))[int(box[1]):int(box[1] + box[3]), int(box[0]):int(box[0] + box[2]), :] pcd_img = pcd_img.reshape((pcd_img.shape[0] * pcd_img.shape[1], 3)) pcd_crop = open3d.PointCloud() open3d.estimate_normals( pcd_crop, search_param=open3d.KDTreeSearchParamHybrid(radius=0.02, max_nn=30)) #open3d.draw_geometries([pcd_crop]) guess = np.zeros((4, 4), dtype=np.float32) guess[:3, :3] = rmat guess[:3, 3] = itvec.T guess[3, :] = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32).T if dC == 0: pcd_model = pcd_model_1 elif dC == 1: pcd_model = pcd_model_2
classes = ["gt/gt_obj", "before/pred_obj", "after/pred_obj"] save_folder = ["graph/gt/", "graph/pred/", "graph/pred_proc/"] shiftX = 3 with open('graph/scene_name.txt', 'w') as name: for index, item in enumerate(classes): all_file = all_path(item) for file_path, file_name in all_file: cloud = [] with open(file_path, "r") as fr: for line in fr: temp = [] for s in line.split(): if s != 'v': temp.append(float(s)) cloud.append(temp) cloud = np.array(cloud) coord = cloud[:, 0:3].astype(np.float32) color = cloud[:, 3:6].astype(np.uint32) pcd = o3d.PointCloud() pcd.points = o3d.Vector3dVector(coord + shiftX * index) pcd.colors = o3d.Vector3dVector(color) #o3d.visualization.draw_geometries([pcd]) o3d.io.write_point_cloud( save_folder[index] + file_name.split('.')[0] + '.pcd', pcd) print("Write file: ", file_name.split('.')[0] + '.pcd') if index == 0: name.write(file_name[:-7] + '\n')
cam_ray_directions.append(cam_ray_direction) cam_point_cloud, depth_image = cam.simulate_camera_mesh(target_mesh,real=True, err=0) cam_point_clouds.append(cam_point_cloud) cam_depth_images.append(depth_image) cam_point_clouds = np.array(cam_point_clouds) cam_depth_images = np.array(cam_depth_images) file_name = "V_%s_fps_%s_simulation_time_%s_start_angle_%s_Z_%s_radius_%s" %(V, fps, simulation_time, start_angle, Z, radius) np.save("simulation/cam_point_clouds_" + file_name, cam_point_clouds) np.save("simulation/cam_depth_images_" + file_name, cam_depth_images) #%% PLOTING CAM POINT CLOUD source = pn.PointCloud() vis = pn.Visualizer() vis.create_window() for i in range(2): for i, point_cloud in enumerate(cam_point_clouds): time.sleep(0.42) source.points = pn.Vector3dVector(point_cloud) source.paint_uniform_color([1, 0.706, 0]) vis.add_geometry(source) vis.update_geometry() vis.poll_events() vis.update_renderer() vis.destroy_window()
def test(tr_src, train_tgt, tst_src, gt, output, mean, std): tensor_x = torch.Tensor(tst_src) tensor_y = torch.stack([torch.Tensor(i) for i in gt]) # print(tensor_y.shape) testset = utils.TensorDataset(tensor_x, tensor_y) testloader = utils.DataLoader(testset) loss = 0.0 test_size = 0 saved_data = np.zeros(gt.shape) with torch.no_grad(): for data in testloader: # print(saved_data) inputs, targets = data[0].to(device), data[1].to(device) outputs = posemlp(inputs) loss += criterion(outputs, targets[0]).item() saved_data[test_size] = outputs.cpu().numpy() test_size += 1 saved_data = saved_data * std + mean print(saved_data) print('Finished testing with average loss %.6f.', loss / test_size) gt = gt * std + mean train_tgt = train_tgt * std + mean print('Dumping data to ' + output) np.save(output, saved_data) pred_pcd = o3d.PointCloud() pred_pcd.points = o3d.Vector3dVector(saved_data) pred_pcd = o3d.voxel_down_sample(pred_pcd, voxel_size=0.05) pred_pcd.paint_uniform_color([0, 0, 1]) colors = np.zeros((saved_data.size / 3, 3)) colors[..., 2] = np.ones(saved_data.size / 3) tr_src = np.around(tr_src, decimals=4) tst_src = np.around(tst_src, decimals=4) # print(tst_src)[900:1100] # print(tr_src) # print(np.asarray(pred_pcd.colors).size) print(saved_data.size) print(np.asarray(pred_pcd.colors).size) for i in range(saved_data.size / 3): if tst_src[i] in tr_src: np.asarray(pred_pcd.colors)[i, 2] = 0 np.asarray(pred_pcd.colors)[i, 0] = 1 # print(i) # colors[i, 0] = 0 # colors[i, 1] = 0 # colors[i, 2] = 1 # pred_pcd.colors = o3d.Vector3dVector(colors) print(np.asarray(pred_pcd.colors)) gt_pcd = o3d.PointCloud() gt_pcd.points = o3d.Vector3dVector(gt) gt_pcd = o3d.voxel_down_sample(gt_pcd, voxel_size=0.05) gt_pcd.paint_uniform_color([0, 1, 0]) tr_pcd = o3d.PointCloud() tr_pcd.points = o3d.Vector3dVector(train_tgt) tr_pcd = o3d.voxel_down_sample(tr_pcd, voxel_size=0.05) tr_pcd.paint_uniform_color([1, 0, 0]) o3d.visualization.draw_geometries([pred_pcd, gt_pcd, tr_pcd])
def mandelbulb(K=200, p=8.0, N=10, extreme=1.5, optimize=False): M = T.ftensor4() C = T.ftensor4() n = T.fscalar() def step(X): r = T.sqrt( T.square(X[:, :, :, 0]) + T.square(X[:, :, :, 1]) + T.square(X[:, :, :, 2])) phi = T.arctan2(X[:, :, :, 1], X[:, :, :, 0]) theta = T.arctan2( T.sqrt(T.square(X[:, :, :, 0]) + T.square(X[:, :, :, 1])), X[:, :, :, 2]) X_ = T.stack((T.pow(r, n) * T.sin(n * theta) * T.cos(n * phi), T.pow(r, n) * T.sin(n * theta) * T.sin(n * theta), T.pow(r, n) * T.cos(n * theta)), axis=-1) return X_ + C if optimize: result, _ = theano.scan(fn=step, outputs_info=M, n_steps=N) f = theano.function([M, C, n], result, allow_input_downcast=True) else: f_ = theano.function([M, C, n], step(M), allow_input_downcast=True) def f(X, c, q): for j in range(N): print 'step {%d}' % j X = f_(X, c, q) return np.array(X) x, y, z = np.meshgrid(np.linspace(-extreme, extreme, K), np.linspace(-extreme, extreme, K), np.linspace(-extreme, extreme, K)) X = np.stack((x, y, z), axis=-1) if optimize: x_n = f(np.zeros((K, K, K, 3)), X, p)[-1] else: x_n = f(np.zeros((K, K, K, 3)), X, p) r_n = np.sqrt(np.square(x_n[:,:,:,0]) + \ np.square(x_n[:,:,:,1]) + \ np.square(x_n[:,:,:,2])) b_ = 1.0 * (r_n < 2.0) import open3d from scipy.misc import imresize from skimage import measure verts, faces = measure.marching_cubes_classic(b_, 0) faces = measure.correct_mesh_orientation(b_, verts, faces) verts = 2.0 * extreme * (np.array(verts, dtype=np.float32) / K) - extreme pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(verts) open3d.estimate_normals(pcd, search_param=open3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) global i, images i = 0 images = [] def custom_draw_geometry_with_rotation(pcd): def rotate_view(vis): ctr = vis.get_view_control() ctr.rotate(10.0, 0.0) global i, images i += 1 print i % 210, i // 210 image = np.asarray(vis.capture_screen_float_buffer()) image = np.array(255 * image, dtype=np.uint8) image = imresize(image, 0.25) if (i // 210 == 0): images.append(image) return False open3d.draw_geometries_with_animation_callback([pcd], rotate_view) custom_draw_geometry_with_rotation(pcd) gif_name = 'Mandelbulb_%d_%d.gif' % (int(p), int(N)) output_file = os.path.join(__file__.split('.')[0], gif_name) imageio.mimsave(output_file, images, duration=0.05)