def points_spliting(points, pcl_fn): if NO_SPLIT: wall_fn = pcl_fn.replace('pcl_camref.ply','object_bbox/wall.txt') walls = np.loadtxt(wall_fn).reshape([-1,7]) ceil_fn = pcl_fn.replace('pcl_camref.ply','object_bbox/ceiling.txt') ceilings = np.loadtxt(ceil_fn).reshape([-1,7]) points = forcely_crop_scene(points, walls, ceilings) points_splited = [points] else: splited_vidx, block_size = IndoorData.split_xyz(points[:,0:3], IndoorData._block_size0.copy(), IndoorData._block_stride_rate, IndoorData._min_pn_inblock) if splited_vidx[0] is None: points_splited = [points] else: points_splited = [np.take(points, vidx, axis=0) for vidx in splited_vidx] pnums0 = [p.shape[0] for p in points_splited] #print(pnums0) points_splited = [random_sample_pcl(p, IndoorData._num_points, only_reduce=True) for p in points_splited] show = False if show: pcds = [points2pcd_open3d(points) for points in points_splited] #open3d.draw_geometries(pcds) for pcd in pcds: open3d.draw_geometries([pcd]) return points_splited
def icp_rabbit(datalist): A1, A2, A3, A4, A5, A6, A7, A8, A9, A10 = datalist o3d.draw_geometries([A8, A9]) print("start A3->A2") A8, R9 = icp(A8, A9) o3d.draw_geometries([A8, A9]) return
def main(): dm = DataManagement() after = dt(2018, 7, 23, 14, 0, 0) before = dt(2018, 7, 23, 14, 1, 0) datetimes = dm.get_datetimes_in(after, before) print(datetimes) assert len(datetimes) == 1 datetime = datetimes[0] data_path = dm.get_save_directory(datetime) data_path = os.path.join(data_path, 'objects') print(os.listdir(data_path)) ob_name = 'keyboard' ob_path = os.path.join(data_path, ob_name) # just get one files = os.listdir(ob_path) filename = files[0] csv_path = os.path.join(ob_path, filename) np_pc = np.loadtxt(csv_path, delimiter=',') pc = o3.PointCloud() pc.points = o3.Vector3dVector(np_pc) o3.draw_geometries([pc]) pass
def draw_open3d(source, target, transformation): """ Draws the registration result in seperate window with open3d visualizer (GDAL) """ source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) if isinstance(source_temp, PointCloud) == True: pcd_source = open3d.PointCloud() pcd_source.points = open3d.Vector3dVector(source_temp.points) elif isinstance(source_temp, numpy.ndarray) == True: pcd_source = open3d.PointCloud() pcd_source.points = open3d.Vector3dVector(source_temp) elif isinstance(source_temp, open3d.PointCloud) == True: pcd_source = copy.deepcopy(source_temp) else: raise TypeError("Point clouds need to be of type (ndarray), (PointCloud) or (open3d.PointCloud)!") if isinstance(target_temp, PointCloud) == True: pcd_target = open3d.PointCloud() pcd_target.points = open3d.Vector3dVector(target_temp.points) elif isinstance(target_temp, numpy.ndarray) == True: pcd_target = open3d.PointCloud() pcd_target.points = open3d.Vector3dVector(target_temp) elif isinstance(target_temp, open3d.PointCloud) == True: pcd_target = copy.deepcopy(target_temp) else: raise TypeError("Point clouds need to be of type (ndarray) or (PointCloud)!") # Paint with uniform color pcd_source.paint_uniform_color([1, 0.706, 0]) pcd_target.paint_uniform_color([0, 0.651, 0.929]) pcd_source.transform(transformation) open3d.draw_geometries([pcd_source, pcd_target]) return
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) open3d.draw_geometries([source_temp, target_temp])
def open3dVis(self, depth, normal=None, color=None): """Visualize 3d map points from eigen depth Args: depth: depth map normal: normal map color: rgb image """ points = get3dpoints(depth, color) points = np.asarray(points) pcd = PointCloud() pcd.points = Vector3dVector(points) if color is not None: color = np.reshape(color, [-1, 3]) pcd.colors = Vector3dVector(color / 255.) if normal is not None: normal = np.reshape(normal, [-1, 3]) else: _, normal, _ = self.compute_local_planes(points[:, 0], points[:, 1], points[:, 2]) normal = np.reshape(normal, [-1, 3]) pcd.normals = Vector3dVector(normal) draw_geometries([pcd])
def icp_rabbit(datalist): A1, A2, A3, A4, A5, A6, A7, A8, A9 = datalist #o3d.draw_geometries([A1,A2,A3,A4,A5,A6,A7,A8,A9]) train = [A1, A2, A3, A4, A5, A6, A7, A8, A9] train.remove(A1) print("start A2->A1") A2, R1 = icp(A2, A1) o3d.draw_geometries([A1, A2]) rotate_elements(train.remove(A2), R1) print("start A3->A2") A3, R2 = icp(A3, A2) rotate_elements([A4, A5, A6, A7, A8, A9], R2) o3d.draw_geometries([A1, A2, A3]) print("start A4->A3") A4, R3 = icp(A4, A3) rotate_elements([A5, A6], R3) o3d.draw_geometries([A1, A2, A3, A4]) print("start A5->A4") A5, R4 = icp(A5, A4) rotate_elements([A6], R4) o3d.draw_geometries([A1, A2, A3, A4, A5]) print("start A6->A5") A6, R5 = icp(A6, A5) o3d.draw_geometries([A1, A2, A3, A4, A5, A6]) return
def ViewPLY(path): # 読み込み pointcloud = open3d.read_point_cloud(path) # ダウンサンプリング # 法線推定できなくなるので不採用 pointcloud = open3d.voxel_down_sample(pointcloud, 10) # 法線推定 open3d.estimate_normals(pointcloud, search_param=open3d.KDTreeSearchParamHybrid( radius=20, max_nn=30)) # 法線の方向を視点ベースでそろえる open3d.orient_normals_towards_camera_location(pointcloud, camera_location=np.array( [0., 10., 10.], dtype="float64")) # 可視化 open3d.draw_geometries([pointcloud]) # numpyに変換 points = np.asarray(pointcloud.points) normals = np.asarray(pointcloud.normals) print("points:{}".format(points.shape[0])) X, Y, Z = Disassemble(points) # OBB生成 _, _, length = buildOBB(points) print("OBB_length: {}".format(length)) return points, X, Y, Z, normals, length
def visualize_pc(*args): """ Visualize of list of point clouds :param *args: a list of Open3D objects, that are point clouds """ point_clouds = [points_to_pc(obj) for obj in args] op3.draw_geometries([*point_clouds])
def main(): system('') # Enable VT100 Emulation for colors in Windows # color_cube.py header modified so that Open3D can read the colors (eg. 'diffuse_red' -> 'r') mesh = open3d.read_triangle_mesh('input/extra/color_cube_mod.ply') he_mesh = HalfEdgeDS(mesh) print('Displaying Original Mesh...') print('Vertices:\x1B[32m', len(he_mesh.vertices), '\x1B[0mEgdes:\x1B[32m', int(len(he_mesh.halfedges) / 2), '\x1B[0mFaces:\x1B[32m', len(he_mesh.faces), '\x1B[0m') mesh.compute_vertex_normals() open3d.draw_geometries([mesh]) for i in range(5): print('Deleting random edge.') deletion_index = randint(0, len(he_mesh.halfedges) - 1) delete_halfedge(he_mesh, he_mesh.halfedges[deletion_index]) print('Displaying Mesh with {} random edge(s) deleted...'.format(i + 1)) print('Vertices:\x1B[32m', len(he_mesh.vertices), '\x1B[0mEgdes:\x1B[32m', int(len(he_mesh.halfedges) / 2), '\x1B[0mFaces:\x1B[32m', len(he_mesh.faces), '\x1B[0m') mesh = he_mesh.convert_to_IFS() mesh.compute_vertex_normals() if (i == 0): open3d.write_triangle_mesh('output/hw3p5a.ply', mesh) open3d.draw_geometries([mesh])
def main(): depth_paths, T, pose_paths = getData(args.shapeid) n = len(depth_paths) print('found %d clean depth images...' % n) intrinsic = np.array([[525.0,0,319.5],[0,525.0,239.5],[0,0,1]]) np.random.seed(816) indices = np.random.permutation(n) print(indices[:100]) #indices = sorted(indices) make_dirs(PATH_MAT.format(args.shapeid, 0)) import open3d pcd_combined = open3d.PointCloud() for i, idx in enumerate(indices): import ipdb; ipdb.set_trace() print('%d / %d' % (i, len(indices))) mesh = Mesh.read(depth_paths[idx], mode='depth', intrinsic = intrinsic) pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(mesh.vertex.T) pcd.transform(inverse(T[idx])) #pcd = open3d.voxel_down_sample(pcd, voxel_size=0.02) pcd_combined += pcd pcd_combined = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02) sio.savemat(PATH_MAT.format(args.shapeid, i), mdict={ 'vertex': mesh.vertex, 'validIdx_rowmajor': mesh.validIdx, 'pose': T[idx], 'depth_path': depth_paths[idx], 'pose_path': pose_paths[idx]}) if i <= 50 and i >= 40: pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02) open3d.draw_geometries([pcd_combined_down]) pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02) open3d.draw_geometries([pcd_combined_down])
def draw_all_keypoints( point_cloud, keypoint, # type: np.ndarray, keypoint_size=0.01): """ Draw all the keypoint associated with the point_cloud (mesh) :param point_cloud: open3d point cloud :param keypoint: np.ndarray in the shape of (3, n_keypoint) :param keypoint_size: the radius of the keypoint :return: None """ n_keypoint = keypoint.shape[1] geometries = [] geometries.append(point_cloud) for i in range(n_keypoint): # Type conversion keypoint_i = [] for k in range(3): keypoint_i.append(keypoint[k, i]) # Insert into geometries geometries.append(get_keypoint_sphere(keypoint_i, keypoint_size)) # The drawing command open3d.draw_geometries(geometries)
def view_local_maps(seq): sequence = dataset.sequence(seq) seqdir = os.path.join(result_dir, '{:03d}'.format(seq)) with np.load(os.path.join(seqdir, localmapfile), allow_pickle=True) as data: for i, map in enumerate(data['maps']): T_w_m = sequence.poses[map['imid']].dot(T_cam0_mc).dot(T_mc_m) mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0]) mapboundsvis.transform(T_w_m) polemap = [] for poleparams in map['poleparams']: x, y, zs, ze, a, _ = poleparams pole = util.create_wire_box([a, a, ze - zs], color=[1.0, 1.0, 0.0]) T_m_p = np.identity(4) T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs] pole.transform(T_w_m.dot(T_m_p)) polemap.append(pole) accucloud = o3.PointCloud() for j in range(map['istart'], map['iend']): velo = sequence.get_velo(j) cloud = o3.PointCloud() cloud.points = o3.Vector3dVector(velo[:, :3]) cloud.colors = o3.Vector3dVector( util.intensity2color(velo[:, 3])) cloud.transform(sequence.poses[j].dot( sequence.calib.T_cam0_velo)) accucloud.points.extend(cloud.points) accucloud.colors.extend(cloud.colors) o3.draw_geometries([accucloud, mapboundsvis] + polemap)
def plot_point_cloud(pts4D, colours4D=[], verbose=False): """Create a point cloud of all 3D points found so far :param pts4D: a list of 4D (homogeneous) points to be plotted :param colours4D: a list of colour extracted from the image for each point in pts4D. Must be the same length as pts4D. :param verbose: as in pipeline() """ # convert from homogeneous coordinates to 3D pts3D = pts4D[:, :3] / np.repeat(pts4D[:, 3], 3).reshape(-1, 3) # Plot with open3d plot_list = [] if len(colours4D) == 0: # Plot all points red pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(pts3D) pcd.paint_uniform_color([1, 0, 0]) plot_list.append(pcd) # If colours are given, use these else: # Plot each point with its given colour pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(pts3D) pcd.colors = open3d.Vector3dVector(colours4D) plot_list.append(pcd) # Print the number of 3D points plotted print("Plotted {} 3D points".format(len(pts3D))) # Draw the point cloud open3d.draw_geometries(plot_list)
def visualize_correspondence_smoothness(points, correspondence, max_error, min_error): """ Visualize smoothness of predicted correspondences Args: points: o3d.geometry.TriangleMesh, contains N points. correspondence: [N] """ pcd = getPointCloud(points) model = helper.loadSMPLModels()[0] pcd.points = o3d.utility.Vector3dVector(np.array(pcd.points)+np.array([1.0,0,0]).reshape((1, 3))) v = np.array(pcd.points) N = v.shape[0] tree = NN(n_neighbors=20).fit(v) dists, indices = tree.kneighbors(v) target = model.verts[correspondence, :] # [N, 3] centers = target[indices, :].mean(axis=1) # [N, 3] diff_norms = np.square(target[indices, :] - centers[:, np.newaxis, :]).sum(axis=-1).mean(axis=1) # [N] diff_norms[np.where(diff_norms > max_error)] = max_error diff_norms[np.where(diff_norms < min_error)] = min_error import ipdb; ipdb.set_trace() max_indices = np.argsort(diff_norms)[-1000:] edges = getEdges(v[max_indices, :], target[max_indices, :]) colors = (diff_norms-min_error)/(max_error - min_error) r = np.outer(np.ones(N), np.array([1.0, 0, 0])) # [N, 3] b = np.outer(np.ones(N), np.array([0., 0, 1.0])) # [N, 3] colors = b*(1.0-colors[:, np.newaxis])+r*(colors[:, np.newaxis]) pcd.points = o3d.utility.Vector3dVector(colors) o3d.draw_geometries([pcd, getTriangleMesh(model.verts, model.faces), edges])
def draw_registration_result(source, target): if isinstance(source, PointCloud): tmp = source source = o3d.PointCloud() source.points = o3d.Vector3dVector(tmp.position) elif isinstance(source, np.ndarray): tmp = source source = o3d.PointCloud() source.points = o3d.Vector3dVector(tmp) if isinstance(target, PointCloud): tmp = target target = o3d.PointCloud() target.points = o3d.Vector3dVector(tmp.position) elif isinstance(target, np.ndarray): tmp = target target = o3d.PointCloud() target.points = o3d.Vector3dVector(tmp) 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]) o3d.draw_geometries([source_temp, target_temp], window_name='Open3D', width=1920, height=1080, left=0, top=0)
def view_local_maps(sessionname): sessiondir = os.path.join(pynclt.resultdir, sessionname) session = pynclt.session(sessionname) maps = np.load(os.path.join(sessiondir, get_localmapfile()))['maps'] for i, map in enumerate(maps): print('Map #{}'.format(i)) mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0]) mapboundsvis.transform(map['T_w_m']) polevis = [] for poleparams in map['poleparams']: x, y, zs, ze, a = poleparams[:5] pole = util.create_wire_box( [a, a, ze - zs], color=[1.0, 1.0, 0.0]) T_m_p = np.identity(4) T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs] pole.transform(map['T_w_m'].dot(T_m_p)) polevis.append(pole) accucloud = o3.PointCloud() for j in range(map['istart'], map['iend']): points, intensities = session.get_velo(j) cloud = o3.PointCloud() cloud.points = o3.Vector3dVector(points) cloud.colors = o3.Vector3dVector( util.intensity2color(intensities / 255.0)) cloud.transform(session.T_w_r_odo_velo[j]) accucloud.points.extend(cloud.points) accucloud.colors.extend(cloud.colors) o3.draw_geometries([accucloud, mapboundsvis] + polevis)
def draw_registration_result(src, trgt): source = open3d.geometry.PointCloud() source.points = open3d.utility.Vector3dVector(src) target = open3d.geometry.PointCloud() target.points = open3d.utility.Vector3dVector(trgt) source.paint_uniform_color([1, 0.706, 0]) target.paint_uniform_color([0, 0.651, 0.929]) open3d.draw_geometries([source, target])
def compare_with_room(self): pc_room = o3.read_point_cloud('static_data/room_A.ply') pcd = self.get_pcd() P = np.loadtxt('static_data/T.csv', delimiter=',') pcd.transform(P) o3.draw_geometries([pc_room, pcd])
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 visualize_wireframe(annos): """visualize wireframe """ colormap = np.array(colormap_255) / 255 junctions = np.array([item['coordinate'] for item in annos['junctions']]) _, junction_pairs = np.where(np.array(annos['lineJunctionMatrix'])) junction_pairs = junction_pairs.reshape(-1, 2) # extract hole lines lines_holes = [] for semantic in annos['semantics']: if semantic['type'] in ['window', 'door']: for planeID in semantic['planeID']: lines_holes.extend( np.where(np.array( annos['planeLineMatrix'][planeID]))[0].tolist()) lines_holes = np.unique(lines_holes) # extract cuboid lines cuboid_lines = [] for cuboid in annos['cuboids']: for planeID in cuboid['planeID']: cuboid_lineID = np.where( np.array(annos['planeLineMatrix'][planeID]))[0].tolist() cuboid_lines.extend(cuboid_lineID) cuboid_lines = np.unique(cuboid_lines) cuboid_lines = np.setdiff1d(cuboid_lines, lines_holes) # visualize junctions connected_junctions = junctions[np.unique(junction_pairs)] connected_colors = np.repeat(colormap[0].reshape(1, 3), len(connected_junctions), axis=0) junction_set = open3d.PointCloud() junction_set.points = open3d.Vector3dVector(connected_junctions) junction_set.colors = open3d.Vector3dVector(connected_colors) # visualize line segments line_colors = np.repeat(colormap[5].reshape(1, 3), len(junction_pairs), axis=0) # color holes if len(lines_holes) != 0: line_colors[lines_holes] = colormap[6] # color cuboids if len(cuboid_lines) != 0: line_colors[cuboid_lines] = colormap[2] line_set = open3d.LineSet() line_set.points = open3d.Vector3dVector(junctions) line_set.lines = open3d.Vector2iVector(junction_pairs) line_set.colors = open3d.Vector3dVector(line_colors) open3d.draw_geometries([junction_set, line_set])
def main(): pc_files = ["input/bunny-pts.ply", "input/mobius_3t.xyz"] # files storing point cloud data for i in range(2): print("\nReading and Displaying", pc_files[i]) pcd = open3d.read_point_cloud( pc_files[i]) # read point cloud data from file print(pcd) # print details of point cloud open3d.draw_geometries([pcd]) # display point cloud
def np_to_pcd(np_pc, draw=False): pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(np_pc[:, :3]) pcd.colors = open3d.Vector3dVector(np_pc[:, 3:] / 255.0) if draw: open3d.draw_geometries([pcd]) return pcd
def visualize_fitting(model, raw): pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(raw) pcd.paint_uniform_color(color(0)) mesh = o3d.geometry.TriangleMesh() mesh.vertices = o3d.utility.Vector3dVector(model.verts) mesh.triangles = o3d.utility.Vector3iVector(model.faces) o3d.draw_geometries([mesh, pcd])
def plot_flow(x, t, phi, grid_size, t_sample): """ Plot the ground truth points, and the reconstructed patch by meshing the domain [0, 1]^2 and lifting the mesh to R^3 :param x: The ground truth points we are trying to fit :param t: The sample positions used to fit the mesh :param phi: The fitted neural network :param grid_size: The number of sample positions per axis in the meshgrid :return: None """ # I'm doing the input here so you don't crash if you never use this function and don't have OpenGL import open3d with torch.no_grad(): mesh_samples = embed_3d( torch.from_numpy(meshgrid_vertices(grid_size)).to(x), t[0, 2]) mesh_faces = meshgrid_face_indices(grid_size) mesh_vertices = phi(mesh_samples)[:, 0:3] recon_vertices = phi(t)[:, 0:3] gt_color = np.array([0.1, 0.7, 0.1]) recon_color = np.array([0.7, 0.1, 0.1]) mesh_color = np.array([0.1, 0.1, 0.7]) curve_color = np.array([0.2, 0.2, 0.5]) pcloud_gt = open3d.PointCloud() pcloud_gt.points = open3d.Vector3dVector(phi.invert(x).cpu().numpy()) pcloud_gt.paint_uniform_color(gt_color) pcloud_recon = open3d.PointCloud() pcloud_recon.points = open3d.Vector3dVector( recon_vertices.cpu().numpy()) pcloud_recon.paint_uniform_color(recon_color) mesh_recon = open3d.TriangleMesh() mesh_recon.vertices = open3d.Vector3dVector( mesh_vertices.cpu().numpy()) mesh_recon.triangles = open3d.Vector3iVector(mesh_faces) mesh_recon.compute_vertex_normals() mesh_recon.paint_uniform_color(mesh_color) pc_initial = open3d.PointCloud() pc_initial.points = open3d.Vector3dVector(t.cpu().numpy()) pc_initial.paint_uniform_color(curve_color) flow_ode = open3d.LineSet() # print(x.shape) # print(t.shape) flow = get_Lines(phi, t[::t_sample, :], 15) flow_ode.points, flow_ode.lines = open3d.Vector3dVector(flow[0]), \ open3d.Vector2iVector(flow[1]) # flow_ode.colors = open3d.Vector3dVector(curve_color) open3d.draw_geometries( [pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode])
def show(xyz, XYZ): pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(xyz) obs = [pcd] for ifor in range(XYZ.shape[0]): sp = o3d.create_mesh_sphere(0.05).transform(pose(XYZ[ifor, :])) obs.append(sp) o3d.draw_geometries(obs)
def draw_registration_result(source, target, transformation): # display results having two point clouds and transformation for source 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) mesh_frame = o3d.create_mesh_coordinate_frame(size = 200, origin = [0, 0, 0]) o3d.draw_geometries([source_temp, target_temp, mesh_frame])
def visualize_points_with_labels(points, labels, cmap=None, lut=6): if cmap is None: cmap = plt.get_cmap("hsv", lut) cmap = np.asarray([cmap(i) for i in range(lut)])[:, :3] point_cloud = open3d.PointCloud() point_cloud.points = open3d.Vector3dVector(points) assert len(labels) == len(points) point_cloud.colors = open3d.Vector3dVector(cmap[labels]) open3d.draw_geometries([point_cloud])
def show_mesh(vertices, triangle, color=[0, 0, 0], only_genmesh=False): mesh = open3d.TriangleMesh() mesh.vertices = open3d.Vector3dVector(vertices) mesh.triangles = open3d.Vector3iVector(triangle) mesh.paint_uniform_color(color) centroid = np.mean(vertices, 0) mesh_frame = open3d.create_mesh_coordinate_frame(size=1.6, origin=centroid) if not only_genmesh: open3d.draw_geometries([mesh, mesh_frame]) return mesh
def save_local_maps(sessionname, visualize=False): print(sessionname) session = pynclt.session(sessionname) util.makedirs(session.dir) istart, imid, iend = get_map_indices(session) maps = [] with progressbar.ProgressBar(max_value=len(iend)) as bar: for i in range(len(iend)): scans = [] for idx, val in enumerate(range(istart[i], iend[i])): xyz, _ = session.get_velo(val) scan = o3.geometry.PointCloud() scan.points = o3.utility.Vector3dVector(xyz) scans.append(scan) T_w_mc = util.project_xy( session.T_w_r_odo_velo[imid[i]].dot(T_r_mc)) T_w_m = T_w_mc.dot(T_mc_m) T_m_w = util.invert_ht(T_w_m) T_w_r = session.T_w_r_odo_velo[istart[i]:iend[i]] T_m_r = np.matmul(T_m_w, T_w_r) occupancymap = mapping.occupancymap(scans, T_m_r, mapshape, mapsize) poleparams = poles.detect_poles(occupancymap, mapsize) if visualize: cloud = o3.geometry.PointCloud() for T, scan in zip(T_w_r, scans): s = copy.copy(scan) s.transform(T) cloud.points.extend(s.points) mapboundsvis = util.create_wire_box(mapextent, [0.0, 0.0, 1.0]) mapboundsvis.transform(T_w_m) polevis = [] for j in range(poleparams.shape[0]): x, y, zs, ze, a = poleparams[j, :5] pole = util.create_wire_box([a, a, ze - zs], color=[1.0, 1.0, 0.0]) T_m_p = np.identity(4) T_m_p[:3, 3] = [x - 0.5 * a, y - 0.5 * a, zs] pole.transform(T_w_m.dot(T_m_p)) polevis.append(pole) o3.draw_geometries(polevis + [cloud, mapboundsvis]) map = { 'poleparams': poleparams, 'T_w_m': T_w_m, 'istart': istart[i], 'imid': imid[i], 'iend': iend[i] } maps.append(map) bar.update(i) np.savez(os.path.join(session.dir, get_localmapfile()), maps=maps)