def draw_bbox(point_cloud): x_max, y_max, z_max = np.max(point_cloud, axis=0) x_min, y_min, z_min = np.min(point_cloud, axis=0) bb1 = [x_max, y_min, z_max] bb2 = [x_max, y_max, z_max] bb3 = [x_max, y_max, z_min] bb4 = [x_max, y_min, z_min] bb5 = [x_min, y_min, z_max] bb6 = [x_min, y_max, z_max] bb7 = [x_min, y_max, z_min] bb8 = [x_min, y_min, z_min] bbox_v = np.array([bb1, bb2, bb3, bb4, bb5, bb6, bb7, bb8], dtype=np.float32) bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0], [4, 5], [5, 6], [6, 7], [7, 4], [0, 4], [1, 5], [2, 6], [3, 7]] # 某条线由哪两个点构成 colors = [[0, 0, 1] for _ in range(len(bbox_lines))] # Default blue # 构建bbox bbox = open3d.LineSet() bbox.lines = open3d.Vector2iVector(bbox_lines) bbox.colors = open3d.Vector3dVector(colors) bbox.points = open3d.Vector3dVector(bbox_v) return bbox
def test_benchmark(): vector_size = int(2e6) x = np.random.randint(10, size=(vector_size, 3)).astype(np.float64) print("\nopen3d.Vector3dVector:", x.shape) start_time = time.time() y = open3d.Vector3dVector(x) print("open3d -> numpy: %.6fs" % (time.time() - start_time)) start_time = time.time() z = np.asarray(y) print("numpy -> open3d: %.6fs" % (time.time() - start_time)) np.testing.assert_allclose(x, z) print("\nopen3d.Vector3iVector:", x.shape) x = np.random.randint(10, size=(vector_size, 3)).astype(np.int32) start_time = time.time() y = open3d.Vector3iVector(x) print("open3d -> numpy: %.6fs" % (time.time() - start_time)) start_time = time.time() z = np.asarray(y) print("numpy -> open3d: %.6fs" % (time.time() - start_time)) np.testing.assert_allclose(x, z) print("\nopen3d.Vector2iVector:", x.shape) x = np.random.randint(10, size=(vector_size, 2)).astype(np.int32) start_time = time.time() y = open3d.Vector2iVector(x) print("open3d -> numpy: %.6fs" % (time.time() - start_time)) start_time = time.time() z = np.asarray(y) print("numpy -> open3d: %.6fs" % (time.time() - start_time)) np.testing.assert_allclose(x, z)
def bounding_box(bound, color): bound_x = bound[0] bound_y = bound[1] bound_z = bound[2] point_1 = [bound_x, bound_y, bound_z] point_2 = [-bound_x, bound_y, bound_z] point_3 = [-bound_x, -bound_y, bound_z] point_4 = [bound_x, -bound_y, bound_z] point_5 = [bound_x, -bound_y, -bound_z] point_6 = [bound_x, bound_y, -bound_z] point_7 = [-bound_x, bound_y, -bound_z] point_8 =[-bound_x, -bound_y, -bound_z] points = [point_1, point_2, point_3, point_4, point_5, point_6, point_7, point_8] lines = [[0, 1], [1, 2], [2, 3], [3, 0], [0, 5], [1, 6], [2, 7], [3, 4], [4, 5], [5, 6], [6, 7], [7, 4]] line_color = [color for i in range(len(lines))] line_set = o3d.LineSet() line_set.points = o3d.Vector3dVector(points) line_set.lines = o3d.Vector2iVector(lines) line_set.colors = o3d.Vector3dVector(line_color) return line_set
def create_boundingbox(locs, dims, rots, color): num_box = locs.shape[0] boxes = [] linesets = [] origin = [0.5, 0.5, 0.5] corners = box3d_visual_utils.corners_nd(dims) corners = box3d_visual_utils.rotation_3d_in_axis(corners, rots, axis=2) corners += locs.reshape([-1, 1, 3]) lines = [[0, 1], [0, 3], [2, 1], [2, 3], [4, 5], [4, 7], [6, 5], [6, 7], [0, 4], [1, 5], [2, 6], [3, 7]] colors = [color for i in range(len(lines))] linesets = [] for i in range(num_box): lineset = open3d.LineSet() lineset.points = open3d.Vector3dVector(corners[i]) lineset.lines = open3d.Vector2iVector(lines) lineset.colors = open3d.Vector3dVector(colors) linesets.append(lineset) return linesets
def markersCallback(self, markers_msg): stamp = markers_msg.header.stamp confidence = [] marker_pts = [] bundle_pts = [] for marker in markers_msg.markers: if(marker.id in self.marker_centers.keys()): q = marker.pose.pose.orientation pt = marker.pose.pose.position trans_mat = tf.transformations.quaternion_matrix([q.x,q.y,q.z,q.w]) trans_mat[:3,3] = [pt.x, pt.y, pt.z] corners = self.tag_corners.dot(trans_mat.T) #marker_poses[marker.id] = trans_mat #confidence.append(marker.confidence) marker_pts.append([pt.x, pt.y, pt.z]) bundle_pts.append(list(self.marker_centers[marker.id])) marker_pts.extend(corners[:,:3]) bundle_pts.extend(list(self.marker_corners[marker.id])) pts_src = open3d.PointCloud() pts_src.points = open3d.Vector3dVector(marker_pts) pts_tgt = open3d.PointCloud() pts_tgt.points = open3d.Vector3dVector(bundle_pts) corres = open3d.Vector2iVector(np.tile(np.arange(len(marker_pts)),[2,1]).T) ransac_res = open3d.registration_ransac_based_on_correspondence(pts_src, pts_tgt, corres, self.max_corres_dist) trans_ransac = ransac_res.transformation self.tf_broadcaster.sendTransform(trans_ransac[:3,3], tf.transformations.quaternion_from_matrix(trans_ransac), marker.header.stamp, marker.header.frame_id, self.frame_id, )
def camera_shape_create(): ''' 生成相机的外形 Returns: ''' triangle_points = 0.05 * np.array([[1, 1, 2], [1, -1, 2], [-1, -1, 2],[-1, 1,2],[0, 0, 0]], dtype=np.float32) lines = [[0, 1], [1, 2], [2, 3],[0,3],[0,4],[1,4],[2,4],[3,4]] # Right leg colors = [[0, 0, 1] for i in range(len(lines))] # Default blue # 定义三角形的三个角点 point_pcd = open3d.geometry.PointCloud() # 定义点云 point_pcd.points = open3d.Vector3dVector(triangle_points) # 定义三角形三条连接线 line_pcd = open3d.LineSet() line_pcd.lines = open3d.Vector2iVector(lines) line_pcd.colors = open3d.Vector3dVector(colors) line_pcd.points = open3d.Vector3dVector(triangle_points) open3d.write_line_set("../data/testDate/a.ply",line_pcd) vis = open3d.Visualizer() vis.create_window(window_name='Open3D_1', width=600, height=600, left=10, top=10, visible=True) vis.get_render_option().point_size = 20 vis.add_geometry(line_pcd) while True: vis.update_geometry() vis.poll_events() vis.update_renderer() cv2.waitKey(100)
def o3d_pca_geometry(vectors, center): """ Generates open3d lineset objects for the pca vectors created around the center of the 3D pointcloud. """ # separate each vector vx = vectors[0] vy = vectors[1] vz = vectors[2] # set 4 points, the end points of each vector and the centroid points = [center, vx, vy, vz] # set the lines to go from center to each point lines = [ [0, 1], [0, 2], [0, 3], ] #each vector is red, green blue are PCA1, PCA2 and PCA3 respectivley. colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] #create open3d linesets. line_set = o3d.LineSet() line_set.points = o3d.Vector3dVector(points) line_set.lines = o3d.Vector2iVector(lines) line_set.colors = o3d.Vector3dVector(colors) return line_set
def show_desktop_pcd(self, isshow=True): self.desktop_3d_point = self.define_triangle() # self.triangle_points_2d, triangle_depth = open3d_tools.convert_point3D_2D(self.desktop_3d_point, # self.camera_conf.camera_intrinsic, # self.camera_conf.depth_scale) self.triangle_points_2d, triangle_depth2 = self.cam.convert_point3d_2d( self.desktop_3d_point, self.camera_conf.depth_scale) lines = [[0, 1], [1, 2], [2, 0]] # Right leg colors = [[0, 0, 1] for i in range(len(lines))] # Default blue # 定义三角形的三个角点 point_pcd = open3d.geometry.PointCloud() # 定义点云 point_pcd.points = open3d.Vector3dVector(self.desktop_3d_point) point_pcd.transform(self.camera_conf.flip_transform) # 定义三角形三条连接线 desktop_line_pcd = open3d.LineSet() desktop_line_pcd.lines = open3d.Vector2iVector(lines) desktop_line_pcd.colors = open3d.Vector3dVector(colors) desktop_line_pcd.points = open3d.Vector3dVector(self.desktop_3d_point) desktop_line_pcd.transform(self.camera_conf.flip_transform) if isshow: self.vis.add_geometry(desktop_line_pcd) self.vis.add_geometry(point_pcd)
def main(): # 读取点云数据并显示 init_points = np.loadtxt('./data/airplane_0001.txt', delimiter=',').astype(np.float32)[:, 0:3] pointcloud = open3d.PointCloud() pointcloud.points = open3d.utility.Vector3dVector(init_points) # open3d.visualization.draw_geometries([pointcloud],window_name='init_pointcloud') # 用PCA分析点云主方向 w, v = PCA(init_points) # print(v[:, 0:2]) point_cloud_vector = v[:, 0] # 点云主方向对应的向量 print('the main orientation of this pointcloud is: ', point_cloud_vector) pca_v = np.array([[0, 0, 0], v[:, 0], v[:, 1], v[:, 2]], dtype=np.float32) pca_lines = [[0, 1], [0, 2], [0, 3]] colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] # RGB # 定义PCA的三条线 pca_line = open3d.LineSet() pca_line.lines = open3d.Vector2iVector(pca_lines) pca_line.colors = open3d.Vector3dVector(colors) pca_line.points = open3d.Vector3dVector(pca_v) # 显示PCA # open3d.visualization.draw_geometries([pointcloud, pca_line],window_name='show_pca',height=500,width=600) # 显示投影点云 # pca_v = np.hstack((v[:, 0:2], np.zeros((3,1)))) # pca_points = np.dot(init_points, pca_v) # pointcloud.points = open3d.utility.Vector3dVector(pca_points) # open3d.visualization.draw_geometries([pointcloud, pca_line],window_name='show_pca_2',height=500,width=600) # 循环计算每个点的法向量 pointcloud_tree = open3d.geometry.KDTreeFlann(pointcloud) # 创建一个KDTree对象 points = np.array(pointcloud.points) normals = [] search_radius = 0.1 # 领域半径 search_num = 100 # 球域内最大点的数量 start = time.time() for i in range(points.shape[0]): _, index, _ = pointcloud_tree.search_hybrid_vector_3d( points[i, :], search_radius, search_num) set = points[index, :] _, set_v = PCA(set) normals.append(set_v[:, 2]) print('use time:', time.time() - start) normals = np.array(normals, dtype=np.float64) pointcloud.normals = open3d.utility.Vector3dVector(normals) open3d.visualization.draw_geometries([pointcloud], window_name='show_normals', height=500, width=600)
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 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 create_wire_box(edgelengths, color=[0.0, 0.0, 0.0]): lineset = o3.LineSet() x, y, z = edgelengths lineset.points = o3.Vector3dVector([[0, 0, 0], [x, 0, 0], [0, y, 0], [x, y, 0], [0, 0, z], [x, 0, z], [0, y, z], [x, y, z]]) lineset.lines = o3.Vector2iVector([[0, 1], [1, 3], [3, 2], [2, 0], [0, 4], [1, 5], [3, 7], [2, 6], [4, 5], [5, 7], [7, 6], [6, 4]]) lineset.colors = o3.Vector3dVector(np.tile(color, [len(lineset.lines), 1])) return lineset
def update_camera_coordinates(self, camera_coordinates): self.camera_representation.points = pn.Vector3dVector( camera_coordinates) self.camera_representation.lines = pn.Vector2iVector([[0, 1], [0, 2], [0, 3]]) self.vis.add_geometry(self.camera_representation) self.vis.update_geometry() self.vis.poll_events() self.vis.update_renderer() return
def create_line_set_bones(joints, joint_line): # Draw the 24 bones (lines) connecting 25 joints # The lines below is the kinematic tree that defines the connection between parent and child joints colors = [[0, 0, 1] for i in range(24)] # Default blue line_set = open3d.LineSet() line_set.lines = open3d.Vector2iVector(joint_line) line_set.colors = open3d.Vector3dVector(colors) line_set.points = open3d.Vector3dVector(joints) return line_set
def refine_registration(source, target, corrs): start = time.time() p2p = o3d.TransformationEstimationPointToPoint() result = p2p.compute_transformation(source, target, o3d.Vector2iVector(corrs)) elapsed_time = time.time() - start return result, elapsed_time
def update_trace(self, camera_coordinates_history): origines = [x[0] for x in camera_coordinates_history] self.camera_trace.points = pn.Vector3dVector(origines) self.camera_trace.lines = \ pn.Vector2iVector([[i,i+1] for i in range(len(origines)-1)]) self.vis.add_geometry(self.camera_trace) self.vis.update_geometry() self.vis.poll_events() self.vis.update_renderer() return
def create_line_set_bones(joints): # Draw the 24 bones (lines) connecting 25 joints # The lines below is the kinematic tree that defines the connection between parent and child joints lines = [[0, 1], [1, 20], [20, 2], [2, 3], # Spine [20, 4], [4, 5], [5, 6], [6, 7], [7, 21], [7, 22], # Left arm and hand [20, 8], [8, 9], [9, 10], [10, 11], [11, 23], [11, 24], # Right arm and hand [0, 12], [12, 13], [13, 14], [14, 15], # Left leg [0, 16], [16, 17], [17, 18], [18, 19]] # Right leg colors = [[0, 0, 1] for i in range(24)] # Default blue line_set = open3d.LineSet() line_set.lines = open3d.Vector2iVector(lines) line_set.colors = open3d.Vector3dVector(colors) line_set.points = open3d.Vector3dVector(joints) return line_set
def load_snapshot(sessionname): cloud = o3.PointCloud() trajectory = o3.LineSet() with np.load(os.path.join(resultdir, sessionname, snapshotfile)) as data: cloud.points = o3.Vector3dVector(data['points']) cloud.colors = o3.Vector3dVector( util.intensity2color(data['intensities'] / 255.0)) trajectory.points = o3.Vector3dVector(data['trajectory']) lines = np.reshape(range(data['trajectory'].shape[0] - 1), [-1, 1]) \ + [0, 1] trajectory.lines = o3.Vector2iVector(lines) trajectory.colors = o3.Vector3dVector( np.tile([0.0, 0.5, 0.0], [lines.shape[0], 1])) return cloud, trajectory
def o3d_trace_rays(cam_pose, max_range=5.5, fov_h_angle=0.34 * np.pi, fov_v_angle=0.25 * np.pi, angle_gap=0.01 * np.pi, color=[0.5, 1, 0.5]): ray_set = open3d.LineSet() cam_forward = np.array([0, 0, 1]) cam_left = np.array([-1, 0, 0]) cam_up = np.array([0, -1, 0]) points = [] lines = [] points.append(cam_pose[:3, 3].tolist()) # prepare the list of view angles fov_h_angle_list = np.arange(start=fov_h_angle / 2, stop=-fov_h_angle / 2, step=-angle_gap) fov_h_angle_list = np.append(fov_h_angle_list, -fov_h_angle / 2) # ensure the last value is included fov_v_angle_list = np.arange(start=fov_v_angle / 2, stop=-fov_v_angle / 2, step=-angle_gap) fov_v_angle_list = np.append(fov_v_angle_list, -fov_v_angle / 2) # ensure the last value is included for angle_v in fov_v_angle_list: up_offset = cam_up * np.tan(angle_v) for angle_h in fov_h_angle_list: left_offset = cam_left * np.tan(angle_h) direction_unit = cam_forward + up_offset + left_offset direction_unit = direction_unit / np.linalg.norm(direction_unit) direction_unit = np.dot(cam_pose[:3, :3], direction_unit) point = cam_pose[:3, 3] + direction_unit * max_range points.append(point.tolist()) for i in range(len(points) - 1): line = [0, i + 1] lines.append(line) colors = [color for i in range(len(lines))] # get vector camera_look_at # set ray_set ray_set.points = open3d.Vector3dVector(points) ray_set.lines = open3d.Vector2iVector(lines) ray_set.colors = open3d.Vector3dVector(colors) return ray_set
def trimesh_to_open3d(src): if isinstance(src, trimesh.Trimesh): dst = open3d.TriangleMesh() dst.vertices = open3d.Vector3dVector(src.vertices) dst.triangles = open3d.Vector3iVector(src.faces) vertex_colors = np.zeros((len(src.vertices), 3), dtype=float) for face, face_color in zip(src.faces, src.visual.face_colors): vertex_colors[face] += face_color[:3] / 255.0 # uint8 -> float indices, counts = np.unique(src.faces.flatten(), return_counts=True) vertex_colors[indices] /= counts[:, None] dst.vertex_colors = open3d.Vector3dVector(vertex_colors) dst.compute_vertex_normals() elif isinstance(src, trimesh.PointCloud): dst = open3d.PointCloud() dst.points = open3d.Vector3dVector(src.vertices) if src.colors: colors = src.colors colors = (colors[:, :3] / 255.0).astype(float) dst.colors = open3d.Vector3dVector(colors) elif isinstance(src, trimesh.scene.Camera): dst = open3d.PinholeCameraIntrinsic( width=src.resolution[0], height=src.resolution[1], fx=src.K[0, 0], fy=src.K[1, 1], cx=src.K[0, 2], cy=src.K[1, 2], ) elif isinstance(src, trimesh.path.Path3D): lines = [] for entity in src.entities: for i, j in zip(entity.points[:-1], entity.points[1:]): lines.append((i, j)) lines = np.vstack(lines) points = src.vertices dst = open3d.LineSet() dst.lines = open3d.Vector2iVector(lines) dst.points = open3d.Vector3dVector(points) elif isinstance(src, list): dst = [trimesh_to_open3d(x) for x in src] else: raise ValueError("Unsupported type of src: {}".format(type(src))) return dst
def triangle_pcd(): ''' 定义三角形的点云 :return: ''' triangle_points = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=np.float32) lines = [[0, 1], [1, 2], [2, 0]] # Right leg colors = [[0, 0, 1] for i in range(len(lines))] # Default blue # 定义三角形的三个角点 point_pcd = open3d.geometry.PointCloud() # 定义点云 point_pcd.points = open3d.Vector3dVector(triangle_points) # 定义三角形三条连接线 line_pcd = open3d.LineSet() line_pcd.lines = open3d.Vector2iVector(lines) line_pcd.colors = open3d.Vector3dVector(colors) line_pcd.points = open3d.Vector3dVector(triangle_points) return line_pcd, point_pcd
def display_surface_normals(filename, size): mesh = open3d.read_triangle_mesh(filename) mesh.paint_uniform_color([0, 0, 0.353]) vertices = np.asarray(mesh.vertices) triangles = vertices[np.array(mesh.triangles)] # calculate normals normal_lines = [] for tri in triangles: u = tri[1] - tri[0] v = tri[2] - tri[0] normal = normalize(np.cross(u, v), size) center = (tri[0] + tri[1] + tri[2]) / 3 normal_lines.append(center) normal_lines.append(center + normal) lines = [[i, i + 1] for i in range(0, len(normal_lines), 2)] colors = [[1, 0, 0] for i in range(len(lines))] line_set = open3d.LineSet() line_set.points = open3d.Vector3dVector(normal_lines) line_set.lines = open3d.Vector2iVector(lines) line_set.colors = open3d.Vector3dVector(colors) open3d.draw_geometries([mesh, line_set])
def transformation_matrix(source_points, target_points): """ Compute the transformation matrix between to set of matching points. Parameters: source_points (numpy.arrays): the source points array target_points (numpy.arrays): the target points array matching the source Returns: transformation_matrix (numpy.arrays) : The 4D transformation matrix from source to target """ assert (len(source_points) >= 3 and len(target_points) == len(source_points)) source = pn.PointCloud() source.points = pn.Vector3dVector(source_points) target = pn.PointCloud() target.points = pn.Vector3dVector(target_points) corr = np.array(2 * [range(len(source_points))]).T p2p = pn.TransformationEstimationPointToPoint() trans = p2p.compute_transformation(source, target, pn.Vector2iVector(corr)) return trans
def draw_lidar_label(points, labels): boxes = [utils.compute_box3d(x) for x in labels] lines = [[0, 1], [1, 2], [2, 3], [3, 0], [0, 4], [1, 5], [2, 6], [3, 7], [4, 5], [5, 6], [6, 7], [7, 4]] colors = [[1, 0, 0] for i in range(len(lines))] pcd = o3d.PointCloud() pcd.points = o3d.Vector3dVector(points) vis = o3d.visualization.Visualizer() vis.create_window() vis.add_geometry(pcd) for i, box in enumerate(boxes): if labels[i].cls == 'DontCare': continue lineset = o3d.geometry.LineSet() lineset.points = o3d.Vector3dVector(box) lineset.lines = o3d.Vector2iVector(lines) lineset.colors = o3d.Vector3dVector(colors) vis.add_geometry(lineset) vis.run() vis.destroy_window()
# u maps to the x-axis # v maps to the y-axis # n maps to the z-axis M_eye = np.eye(4) eye_t = np.array([0., 0., 0]) M_eye[:3, 3] = eye_t coord = open3d.create_mesh_coordinate_frame(0.4) coord1 = open3d.create_mesh_coordinate_frame(0.3) coord2 = open3d.create_mesh_coordinate_frame(0.3) mesh_sphere = open3d.geometry.create_mesh_sphere(radius=0.1) # open3d.draw_geometries([coord, coord1, coord2, mesh_sphere])#, coord2, coord3]) line_set = open3d.geometry.LineSet() line_set.lines = open3d.Vector2iVector([[0, 1]]) line_set.points = open3d.Vector3dVector([eye_t, eye_t]) vis = open3d.visualization.Visualizer() vis.create_window() vis.add_geometry(line_set) vis.add_geometry(coord) vis.add_geometry(coord1) # vis.add_geometry(coord2) vis.add_geometry(mesh_sphere) I4 = np.eye(4) R_y_90 = euler2mat(*np.deg2rad([0, 90, 0])) start_t = np.array([3.0, 2.0, 0.]) start_M = np.eye(4)
def drawGraph(p, G): graph = open3d.LineSet() graph.points = p_light.points graph.lines = open3d.Vector2iVector(G.edges) open3d.draw_geometries([graph])
def run_test(input_array): open3d_array = open3d.Vector2iVector(input_array) output_array = np.asarray(open3d_array) np.testing.assert_allclose(input_array, output_array)
def main(): parser = argparse.ArgumentParser( description='DBSCAN segmentation algorithm') parser.add_argument("--file", dest="filename", required=True, help='Provide a .pcd file') parser.add_argument("--eps", dest="eps", required=True, type=float, help='radius threshold', default=1) parser.add_argument("--minPts", dest="minPoints", required=True, type=int, help='clustering threshold points', default=50) parser.add_argument("--kitti", dest="useKitti", required=True) parser.add_argument("--sklearn", dest="useSklearn", default="True") args = parser.parse_args() # Loading Data using open3D pcd = op.read_point_cloud(args.filename) # Load data here if (args.useSklearn == str(True)): try: import sklearn.neighbors import dbscan_2 as dbscan downsampleRatio = np.array(pcd.points).shape[0] / 1e4 except Exception as e: print(e) print( "Please 'pip3 install scikit-learn, else set --sklearn False") exit() else: import dbscan downsampleRatio = np.array(pcd.points).shape[0] / 1e3 # Sample down the data if large input is given pcd = op.uniform_down_sample(pcd, every_k_points=int(downsampleRatio)) # Converting pcd into python array pcdArray = np.asarray(pcd.points) ground_list = [] above_ground = [] if (args.useKitti == "True"): # Ground plane segregation for point in pcdArray: if point[2] > -1.5: # considering points above -1.5 m of LiDAR as above the ground above_ground.append(point) else: ground_list.append(point) ground = np.asarray(ground_list) above_ground = np.asarray(above_ground) # Clustering algorithm using DBSCAN. Getting all the labels for segmentation labels = dbscan.DBSCAN(above_ground, args.eps, args.minPoints) labels = np.array(labels) # Array to store the segments pcdArrayList = [] # Color list (Randomly called to color clusters) colorList = [[0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [1, 0, 0]] # Creating cluster cloud segments for label in range(len(set(labels))): newPcdArray = above_ground[np.where(labels == label)[0]] # Converting the 3D points into open3D based structure pcdPoints = op.Vector3dVector(newPcdArray) newPcd = op.PointCloud() newPcd.points = pcdPoints color = colorList[np.random.randint(6)] colorArr = np.full((newPcdArray.shape[0], 3), color) newPcd.colors = op.Vector3dVector(colorArr) pcdArrayList.append(newPcd) # The labels have noises (-1), which is not a cluster if newPcdArray.size: # Get the extremities of the point cloud segment data x_max = max(newPcdArray[:, 0]) x_min = min(newPcdArray[:, 0]) y_max = max(newPcdArray[:, 1]) y_min = min(newPcdArray[:, 1]) z_max = max(newPcdArray[:, 2]) z_min = min(newPcdArray[:, 2]) # Create a bounding box around the point cloud cube = [[x_min, y_min, z_min], [x_max, y_min, z_min], [x_min, y_max, z_min], [x_max, y_max, z_min], [x_min, y_min, z_max], [x_max, y_min, z_max], [x_min, y_max, z_max], [x_max, y_max, z_max]] # The below creates relations to join cube points to form bounding boz lines = [[0, 1], [0, 2], [1, 3], [2, 3], [4, 5], [4, 6], [5, 7], [6, 7], [0, 4], [1, 5], [2, 6], [3, 7]] # Creeate line object line_set = op.LineSet() # Convert list to open3D objects line_set.points = op.Vector3dVector(cube) line_set.lines = op.Vector2iVector(lines) colors = [[0, 0, 0] for i in range(len(lines))] line_set.colors = op.Vector3dVector(colors) # Adding lines to the point cloud for visualization pcdArrayList.extend([line_set]) if (args.useKitti == "True"): # Now adding the ground plane back newPcd = op.PointCloud() newPcd.points = pcdPoints newPcd.points = op.Vector3dVector(ground) newPcd.paint_uniform_color([0.5, 0.5, 0.5]) # Add ground plan to the existing list pcdArrayList.extend([newPcd]) # Display the results op.draw_geometries(pcdArrayList)
if VISUALIZATION_LEVEL == 1: print("Configure the viewpoint as you want and press [q]") calib = dataset.get_calib(0) cam_points_in_img_with_rgb = dataset.get_cam_points_in_image_with_rgb( 0, calib=calib) vis = open3d.Visualizer() vis.create_window() pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(cam_points_in_img_with_rgb.xyz) pcd.colors = open3d.Vector3dVector(cam_points_in_img_with_rgb.attr[:, 1:4]) line_set = open3d.LineSet() graph_line_set = open3d.LineSet() box_corners = np.array([[0, 0, 0]]) box_edges = np.array([[0, 0]]) line_set.points = open3d.Vector3dVector(box_corners) line_set.lines = open3d.Vector2iVector(box_edges) graph_line_set.points = open3d.Vector3dVector(box_corners) graph_line_set.lines = open3d.Vector2iVector(box_edges) vis.add_geometry(pcd) vis.add_geometry(line_set) vis.add_geometry(graph_line_set) ctr = vis.get_view_control() ctr.rotate(0.0, 3141.0, 0) vis.run() color_map = np.array([(211, 211, 211), (255, 0, 0), (255, 20, 147), (65, 244, 101), (169, 244, 65), (65, 79, 244), (65, 181, 244), (229, 244, 66)], dtype=np.float32) color_map = color_map / 255.0 gt_color_map = { 'Pedestrian': (0, 255, 255),
def animate_flow(x, t, phi, grid_size, t_sample, mesh0=None): import open3d with torch.no_grad(): if mesh0 == None: mesh_samples = embed_3d( torch.from_numpy(meshgrid_vertices(grid_size)).to(x), t[0, 2]) print(mesh_samples.shape) mesh_faces = meshgrid_face_indices(grid_size) mesh_vertices = phi(mesh_samples)[:, 0:3] else: mesh_samples = torch.from_numpy(mesh0[0]).to(x) print("sample", mesh_samples.shape) mesh_faces = mesh0[1][:, 0:3] 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(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, :], 30) print(len(flow[0])) flow_ode.points, flow_ode.lines = open3d.Vector3dVector(flow[0]), \ open3d.Vector2iVector(flow[1]) # flow_ode.colors = open3d.Vector3dVector(curve_color) # vis = open3d.Visualizer() # vis.create_window() # vis.remove_geometry(flow_ode) # for geom in [pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode]: # vis.add_geometry(geometry=geom) # # for i in range(10): # # vis.remove_geometry(flow_ode) # # vis.remove_geometry(flow_ode) # vis.update_geometry() # vis.update_renderer() # vis.poll_events() store_frames = [] temp = mesh_samples store_frames.append(temp) for j in range(0, Nframes - 1): print(j) tj = ts[j] tnext = ts[j + 1] temp = phi.flow(tj, tnext, temp)[:, 0:3] store_frames.append(temp) def next_frame(vis): # ctr = vis.get_view_control() # ctr.rotate(10.0, 0.0) # global i, ts global i, ts, mesh0, Nframes if i >= Nframes: return True print(i) # if mesh0 == None: # mesh_faces = meshgrid_face_indices(grid_size) # else: # mesh_faces =mesh0[1] mesh_vertices = store_frames[i] i += 1 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) vis.update_geometry() vis.update_renderer() return False def revert(vis): global i i = 0 return False key_to_callback = {} key_to_callback[ord(",")] = next_frame key_to_callback[ord(".")] = revert open3d.draw_geometries_with_key_callbacks( [pcloud_gt, pcloud_recon, mesh_recon, pc_initial, flow_ode], key_to_callback)