def create_graph(cloud, knn, radius, kd=None): """ Converts point cloud to graph by building neighborhood structure using kd-tree. Parameters: knn: true if using k-nn neighbors, false if using radius neighbors radius: radius if not knn, k if knn """ xyz = np.asarray(cloud.points) num_nodes = xyz.shape[0] kd = open3d.KDTreeFlann(cloud) if kd is None else kd idx, rowi = [], [] #includes self-loops if knn: for i in range(len(cloud.points)): ind = kd.search_knn_vector_3d(cloud.points[i], int(radius))[1] idx.extend(ind) rowi.extend([i] * len(ind)) else: for i in range(len(cloud.points)): ind = kd.search_radius_vector_3d(cloud.points[i], radius)[1][:RADIUS_MAX_K] idx.extend(ind) rowi.extend([i] * len(ind)) edges = np.vstack([idx, rowi]).T.tolist() offsets = xyz[rowi] - xyz[idx] G = igraph.Graph(n=num_nodes, edges=edges, directed=True, edge_attrs={'offset': offsets}) return G, kd
def batch_find_neighbors_py(query_points, support_points, query_batches, support_batches, radius, max_neighbors): num_batches = len(support_batches) # Create kdtree for each pcd in support_points kdtrees = [] start_ind = 0 for length in support_batches: pcd = make_point_cloud(support_points[start_ind:start_ind + length]) kdtrees.append(open3d.KDTreeFlann(pcd)) start_ind += length assert len(kdtrees) == num_batches # Search neigbors indices neighbors_indices_list = [] start_ind = 0 support_start_ind = 0 dustbin_ind = len(support_points) for i_batch, length in enumerate(query_batches): for i_pts, pts in enumerate(query_points[start_ind:start_ind + length]): [k, idx, dis] = kdtrees[i_batch].search_radius_vector_3d(pts, radius) if k > max_neighbors: # idx = np.random.choice(idx, max_neighbors, replace=False) # If i use random, the closest_pool will not work as expected. idx = list(idx[0:max_neighbors]) else: # if not enough neighbor points, then add dustbin index. Careful !!! idx = list(idx) + [dustbin_ind - support_start_ind ] * (max_neighbors - k) idx = np.array(idx) + support_start_ind neighbors_indices_list.append(idx) # finish one query_points, update the start_ind start_ind += int(query_batches[i_batch]) support_start_ind += int(support_batches[i_batch]) return torch.from_numpy(np.array(neighbors_indices_list)).long()
def filter_edges(pcl, radius_shearch=10, intensity=.5): pcl = pn.voxel_down_sample(pcl, radius_shearch / 5) pcl_points = np.asarray(pcl.points) pcl_tree = pn.KDTreeFlann(pcl) results = [] bar = tqdm.tqdm(total=len(pcl_points)) for i in range(len(pcl.points)): [k, idx, _] = pcl_tree.search_radius_vector_3d(pcl.points[i], radius_shearch) nearest_points = pcl_points[idx] tmp = nearest_points - pcl_points[i] scalar_product = tmp.dot(tmp.T) norm = np.sqrt(np.diag(scalar_product)) tmp = np.array([tmp[i] / norm[i] for i in range(len(tmp))])[1:] scalar_product = abs(tmp.dot(tmp.T)) if len(scalar_product) > 0: results.append(scalar_product.mean()) else: results.append(0) bar.update(1) bar.close() results = np.array(results) id_edge = np.argwhere( results < np.percentile(results, int(intensity * 100)))[:, 0] resutls = pn.PointCloud() resutls.points = pn.Vector3dVector(pcl_points[id_edge]) return resutls
def knearestneighbors(xyz, i, k=None): """ Compute the k nearest neighbors algorithm on the XYZ point cloud in the format Nx3. "i" is the index of the point (of the point cloud) used as reference. Parameters ---------- xyz : numpy.ndarray Data array with shape Nx3 i : integer Index of the reference point in xyz k : integer Number of neighbors to extract Returns ------- idx : numpy.ndarray Array of indices of the neighbors """ print("File \"" + __file__ + "\"") print( " WARNING: use \"NearestNeighbors\" class for a more efficient interface" ) pcd = open3d.PointCloud() pcd.points = open3d.Vector3dVector(xyz) pcd_tree = open3d.KDTreeFlann(pcd) [k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[i], k) return idx
def __getitem__(self, index): x = self.data[index] source_pts, source_normal = x["points_src"][:, :3], x["points_src"][:, 3:] target_pts, target_normal = x["points_ref"][:, :3], x["points_ref"][:, 3:] raw_pts = x["points_raw"][:, :3] idx = x["idx"] T = np.concatenate( [x["transform_gt"], np.array([[0, 0, 0, 1]])], axis=0) source_pc = get_pc(source_pts, source_normal, [1, 0.706, 0]) target_pc = get_pc(target_pts, target_normal, [0, 0.651, 0.929]) # 找重叠 source_tree, target_tree = o3d.KDTreeFlann(source_pc), o3d.KDTreeFlann( target_pc) match = np.zeros((source_pts.shape[0], target_pts.shape[0])) for i, pt in enumerate( np.asarray(deepcopy(source_pc).transform(T).points)): _, inds, _ = target_tree.search_radius_vector_3d(pt, 0.06) inds = np.asarray(inds) if inds.shape[0] > 1: match[i, inds] = 1 source_lrf, target_lrf = LRF(source_pc, source_tree, self.r_lrf), LRF(target_pc, target_tree, self.r_lrf) source_key_feature, target_key_feature = [], [] if self.pre_encode_path is None: for pt in source_pts: patch = source_lrf.get(pt) desc = compute(patch, self.space, self.space) desc[np.isnan(desc)] = 0 source_key_feature.append(desc) source_pre_encode = np.stack(source_key_feature, axis=0) for pt in target_pts: patch = target_lrf.get(pt) desc = compute(patch, self.space, self.space) desc[np.isnan(desc)] = 0 target_key_feature.append(desc) target_pre_encode = np.stack(target_key_feature, axis=0) else: pre_encode = np.load(self.pre_encode_path + "/" + "%d.npy" % index) source_pre_encode, target_pre_encode = pre_encode[:source_pts.shape[ 0], :], pre_encode[source_pts.shape[0]:, :] return source_pts, source_normal, source_pre_encode, target_pts, target_normal, target_pre_encode, match, raw_pts, T, idx
def upscale_dpc(dpc, dpcd, dpco): for i, (pc, pcd) in enumerate(zip(dpc, dpcd)): pc.normals = open3d.Vector3dVector( np.zeros(np.asarray(pc.points).shape)) print("Frame {}".format(i)) pcd_tree = open3d.KDTreeFlann(pcd) pcu = upscale(pc, pcd, pcd_tree) dpco.add_open3d_pc( osp.join(args.out_dir, "frame_{0:06d}.ply".format(i)), pcu) dpco.write_to_disk()
def build_ppf_input(pcd, keypts): kdtree = open3d.KDTreeFlann(pcd) keypts_id = [] for i in range(keypts.shape[0]): _, id, _ = kdtree.search_knn_vector_3d(keypts[i], 1) keypts_id.append(id[0]) neighbor = collect_local_neighbor(keypts_id, pcd, vicinity=0.3, num_points=1024) local_pachtes = build_local_patch(keypts_id, pcd, neighbor) return local_pachtes
def create_pooling_map(cloud_src, cloud_dst): """ Creates a pooling map from cloud_dst to cloud_src. Points in the denser cloud (source) are mapped to their nearest neighbor in the subsampled cloud (dest), thus no overlaps supported. """ kd_dst = open3d.KDTreeFlann(cloud_dst) indices = [ kd_dst.search_knn_vector_3d(cloud_src.points[i], 1)[1] for i in range(len(cloud_src.points)) ] poolmap = defaultdict(list) for si, di in enumerate(indices): poolmap[di[0]].append(si) return poolmap, kd_dst
def genGraph(p, r_nn): N = len(p.points) tree = open3d.KDTreeFlann(p) pts = np.array(p.points) G = nx.Graph() for i in range(N): G.add_node(i, pos=pts[i]) for i in range(N): [k, idxs, _] = tree.search_radius_vector_3d(pts[i], r_nn) elist = [(i, idx) for idx in idxs] G.add_edges_from(elist) return G
def interpolate_dense_labels(sparse_points, sparse_labels, dense_points, k=3): sparse_pcd = open3d.PointCloud() sparse_pcd.points = open3d.Vector3dVector(sparse_points) sparse_pcd_tree = open3d.KDTreeFlann(sparse_pcd) dense_labels = [] for dense_point in dense_points: result_k, sparse_indexes, _ = sparse_pcd_tree.search_knn_vector_3d( dense_point, k) knn_sparse_labels = sparse_labels[sparse_indexes] dense_label = np.bincount(knn_sparse_labels).argmax() dense_labels.append(dense_label) return dense_labels
def find_neighbors(query_points, support_points, radius, max_neighbor): pcd = make_point_cloud(support_points) kdtree = open3d.KDTreeFlann(pcd) neighbor_indices_list = [] for i, point in enumerate(query_points): [k, idx, dis] = kdtree.search_radius_vector_3d(point, radius) if k > max_neighbor: idx = np.random.choice(idx, max_neighbor, replace=False) else: # if not enough neighbor points, then add the dustbin point. idx = list(idx) + [len(support_points)] * (max_neighbor - k) neighbor_indices_list.append([idx]) neighbors = np.concatenate(neighbor_indices_list, axis=0) return torch.from_numpy(neighbors)
def p2p_one_way_np(pc1, pc2): """ pred: B*NUM_CLASSES, label: B, """ # Ensure inputs are the same shape # pc1 = to_np(dpci[0]) # pc2 = to_np(dpci[0]) kdt = open3d.KDTreeFlann(pc2) p1 = np.asarray(pc1.points) c1 = np.asarray(pc1.colors) n1 = np.asarray(pc1.normals) n2 = np.asarray(pc2.normals) n = -1 nn_xyz = np.asarray(pc1.points).copy() nn_rgb = np.asarray(pc1.colors).copy() for i, (p1_, c1_, n1_) in enumerate(zip(p1[:n], c1[:n], n1[:n])): [k, idx, _] = kdt.search_knn_vector_3d(p1_, 1) #replace by hybrid nn_xyz[i, ...] = np.asarray(pc2.points)[idx, ...] nn_rgb[i, ...] = np.asarray(pc2.colors)[idx, ...] # For each pair of points, calculate the squared error. xyz_dist = np.square(nn_xyz - p1) xyz_dist = np.sum(xyz_dist, axis=-1, keepdims=True) rgb_dist = np.square(nn_rgb - c1) rgb_dist = np.mean(rgb_dist, axis=-1, keepdims=True) xyz_min = np.min(p1[..., :3], axis=0) xyz_max = np.max(p1[..., :3], axis=0) xyz_range = xyz_max - xyz_min bounding_box_width = np.max(xyz_range) err_xyz = xyz_dist / (3 * (bounding_box_width**2)) err_rgb = rgb_dist / (255 ** 2) xyz_mean = np.mean(err_xyz) rgb_mean = np.mean(err_rgb) return xyz_mean, rgb_mean
def p2plane_np(pc1, pc2, normal): """ pred: B*NUM_CLASSES, label: B, """ # Ensure inputs are the same shape pc1p = np.asarray(pc1.points) pc2p = np.asarray(pc2.points) num_points_1 = pc1p.shape[1] num_points_2 = pc2p.shape[1] pc1p = np.expand_dims(pc1p, 1) pc2p = np.expand_dims(pc2p, 1) kdt = open3d.KDTreeFlann(pc2) p1 = np.asarray(pc1.points) c1 = np.asarray(pc1.colors) n1 = np.asarray(pc1.normals) n2 = np.asarray(pc2.normals) n = -1 nn_xyz = np.asarray(pc1.points).copy() nn_rgb = np.asarray(pc1.colors).copy() for i, (p1_, c1_, n1_) in enumerate(zip(p1[:n], c1[:n], n1[:n])): [k, idx, _] = kdt.search_knn_vector_3d(p1_, 1) #replace by hybrid nn_xyz[i, ...] = np.asarray(pc2.points)[idx, ...] nn_rgb[i, ...] = np.asarray(pc2.colors)[idx, ...] # For each pair of points, calculate the squared error. xyz_dist = np.square(nn_xyz - p1) error = np.reshape(xyz_dist, (-1, 1, 3)) normal = np.reshape(normal, (-1, 3, 1)) projected_error = np.square(np.matmul(error, normal)) projected_error = np.reshape(projected_error, (-1 )) xyz_min = np.min(p1[..., :3], axis=0) xyz_max = np.max(p1[..., :3], axis=0) xyz_range = xyz_max - xyz_min bounding_box_width = np.max(xyz_range) xyz_err = np.mean(projected_error / (3 * (bounding_box_width**2))) + 1e-20 psnr_xyz = -10 * np.log10(xyz_err) return [psnr_xyz.item()]
def snap(dpcd, dpco, dpco_dir): naming = dpco_dir + "/frame_{0:06d}.ply" for j in range(len(dpcd) - 1): print("Frame ", j) # pc1 = copy.deepcopy(dpcd[j]) # pc2 = copy.deepcopy(dpcd[j+1]) pc1 = dpcd[j] pc2 = dpcd[j + 1] pc1_dest = np.asarray(pc1.points)[...] + np.asarray(pc1.normals) kdt = open3d.KDTreeFlann(pc2) p1 = np.asarray(pc1.points) c1 = np.asarray(pc1.colors) n1 = np.asarray(pc1.normals) n2 = np.asarray(pc2.normals) n = -1 nn = pc1_dest.copy() p2_needsmap = np.ones((pc1_dest.shape[0]), dtype=bool) for i, (p1_, c1_, n1_) in enumerate(zip(pc1_dest[:n], c1[:n], n1[:n])): [k, idx, _] = kdt.search_knn_vector_3d(p1_, 1) #replace by hybrid nn[i, ...] = np.asarray(pc2.points)[idx, ...] p2_needsmap[idx] = False np.asarray(pc1.normals)[...] = nn - np.asarray(pc1.points) # Correct non attached p2s p2p = np.asarray(pc2.points) p2n = np.asarray(pc2.normals) p2_fix = np.zeros((p2_needsmap[p2_needsmap].shape[0], 3)) for i, p_ in enumerate(p2p[p2_needsmap]): [k, idx, _] = kdt.search_knn_vector_3d(p_, 2) #replace by hybrid p2_fix[i, ...] = p2p[idx[1], ...] p2n[p2_needsmap, ...] = (p2p[p2_needsmap, ...] + p2n[p2_needsmap, ...] - p2_fix) p2p[p2_needsmap, ...] = p2_fix dpco.add_open3d_pc(naming.format(j), pc1) dpco.add_open3d_pc(naming.format(j + 1), pc2) dpco.write_to_disk()
def guided_filter(pcd, radius, epsilon): kdtree = o3d.KDTreeFlann(pcd) points_copy = np.array(pcd.points) points = np.asarray(pcd.points) num_points = len(pcd.points) for i in range(num_points): k, idx, _ = kdtree.search_radius_vector_3d(pcd.points[i], radius) if k < 3: continue neighbors = points[idx, :] mean = np.mean(neighbors, 0) cov = np.cov(neighbors.T) e = np.linalg.inv(cov + epsilon * np.eye(3)) A = cov @ e b = mean - A @ mean points_copy[i] = A @ points[i] + b pcd.points = o3d.Vector3dVector(points_copy)
def cicp(pcd_source: o3d.PointCloud, pcd_target: o3d.PointCloud, base_transform: np.ndarray, max_correspond_dist_coarse, max_correspond_dist_fine): kdtree = o3d.KDTreeFlann(pcd_target) if not pcd_target.has_normals(): o3d.estimate_normals(pcd_target, search_param=o3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) x0 = np.array([0, 0, 0], dtype=float) result = least_squares(point_plane_residual, x0, jac='2-point', method='trf', loss='soft_l1', args=(base_transform, pcd_source, pcd_target, kdtree, max_correspond_dist_coarse)) x0 = result.x result = least_squares(point_plane_residual, x0, jac='2-point', method='trf', loss='soft_l1', args=(base_transform, pcd_source, pcd_target, kdtree, max_correspond_dist_fine)) x = result.x angle = x[0] t1 = x[1] t2 = x[2] transform = rotation_matrix(angle, rotation_axis) transform[:3, 3] = t1 * translation_axis1 + t2 * translation_axis2 transform = np.dot(transform, base_transform) return transform, result
def _match_kd_tree(source, target): """ For each point p_s in source, this algorithm finds the closest point p_t in target such that the the Eucledian distance between them is minimized. This version uses a KD tree for efficient search :param source: source points of shape (N_s, 3) :param target: target points of shape (N_t, 3) :return: the matching points """ n_s = source.shape[0] # initialize PointCloud from target target_pcd = open3d.PointCloud() target_pcd.points = open3d.Vector3dVector(target) # initialize KD tree from target kd_tree = open3d.KDTreeFlann(target_pcd) # initialize matching matching = np.zeros((n_s, 3), dtype=np.float) # loop over all points in source for s_i in range(n_s): _, idx, _ = kd_tree.search_knn_vector_3d(source[s_i], 1) t_i = idx[0] matching[s_i] = target[t_i] return matching
def create_graph(cloud, knn, radius, kd=None): """ Converts point cloud to graph by building neighborhood structure using kd-tree. Parameters: knn: true if using k-nn neighbors, false if using radius neighbors radius: radius if not knn, k if knn """ xyz = np.asarray(cloud.points) # save points in array num_nodes = xyz.shape[0] # number of points kd = open3d.KDTreeFlann(cloud) if kd is None else kd idx, rowi = [], [] #includes self-loops if knn: # not relevant for i in range(len(cloud.points)): ind = kd.search_knn_vector_3d( cloud.points[i], int(radius) )[1] # ind is an array with the indices of neighboring points for every point in the cloud idx.extend(ind) rowi.extend( [i] * len(ind) ) # idx contains all neighboring points; rowi contains the number of neighboring points, saved in the index of ind for later identification else: for i in range(len(cloud.points)): ind = kd.search_radius_vector_3d(cloud.points[i], radius)[1][:RADIUS_MAX_K] idx.extend(ind) rowi.extend([i] * len(ind)) edges = np.vstack( [idx, rowi]).T.tolist() # arrays with all pairs of neighboring points offsets = xyz[rowi] - xyz[idx] # distances between these points G = igraph.Graph(n=num_nodes, edges=edges, directed=True, edge_attrs={'offset': offsets}) return G, kd
cm_global = ConfusionMatrix(9) for file_prefix in map_name_to_file_prefixes[flags.set]: print("Interpolating:", file_prefix, flush=True) # Paths sparse_points_path = os.path.join(sparse_dir, file_prefix + ".pcd") sparse_labels_path = os.path.join(sparse_dir, file_prefix + ".labels") dense_points_path = os.path.join(gt_dir, file_prefix + ".pcd") dense_labels_path = os.path.join(dense_dir, file_prefix + ".labels") dense_gt_labels_path = os.path.join(gt_dir, file_prefix + ".labels") # Sparse points sparse_pcd = open3d.read_point_cloud(sparse_points_path) print("sparse_pcd loaded", flush=True) sparse_pcd_tree = open3d.KDTreeFlann(sparse_pcd) del sparse_pcd print("sparse_pcd_tree ready", flush=True) # Sparse labels sparse_labels = load_labels(sparse_labels_path) print("sparse_labels loaded", flush=True) # Dense points dense_pcd = open3d.read_point_cloud(dense_points_path) dense_points = np.asarray(dense_pcd.points) del dense_pcd print("dense_pcd loaded", flush=True) # Dense Ground-truth labels try:
def __init__(self, xyz): self.pcd = open3d.PointCloud() self.pcd.points = open3d.Vector3dVector(xyz) self.pcd_tree = open3d.KDTreeFlann(self.pcd)
def __init__(self, pc, r, space): self.tree = o3d.KDTreeFlann(pc) self.lrf = BSLRF(pc, self.tree, r) self.pts = np.asarray(pc.points) self.space = space
def pcd_down_sample(self, src, voxel_size, n_sample_points, original_idx): # # numpy -> PointCloud # cloud = o3d.geometry.PointCloud() # src = src.astype(np.float32) # cloud.points = o3d.Vector3dVector(src) original_idx = np.array([original_idx, original_idx, original_idx]).transpose() color_idx = original_idx.tolist() src.colors = o3d.Vector3dVector(color_idx) max_bound = src.get_max_bound() min_bound = src.get_min_bound() # 截取物体范围内的点 max_distance = 1.5 # m min_distance = 0.3 # m src_tree = o3d.KDTreeFlann(src) _, max_idx, _ = src_tree.search_radius_vector_3d([0, 0, 0], max_distance) _, min_idx, _ = src_tree.search_radius_vector_3d([0, 0, 0], min_distance) src = o3d.select_down_sample(src, max_idx) if len(min_idx) > 0: src_tree = o3d.KDTreeFlann(src) _, min_idx, _ = src_tree.search_radius_vector_3d([0, 0, 0], min_distance) src = o3d.select_down_sample(src, min_idx, True) _, point_idx = o3d.voxel_down_sample_and_trace(src, voxel_size=voxel_size, min_bound=min_bound, max_bound=max_bound) max_idx_in_row = np.max(point_idx, axis=1) pcd_down = o3d.select_down_sample(src, max_idx_in_row) # pcd_down_tree = o3d.KDTreeFlann(pcd_down) # _, idx, _ = pcd_down_tree.search_radius_vector_3d([0,0,0], 1500) # pcd_down = o3d.select_down_sample(pcd_down, idx) # 如果采样后的点数大于指定值, 随机减少一定数量的 n_points = len(max_idx_in_row) np.random.seed(666) if n_points > n_sample_points: n_minus = n_points - n_sample_points n_pcd_dow = n_points minus_idx = np.random.choice(n_pcd_dow, n_minus, replace=False) pcd_down = o3d.select_down_sample(pcd_down, minus_idx, True) return pcd_down elif n_points < n_sample_points: n_add = n_sample_points - n_points n_cuted_points = len(src.points) n_unsample_points = n_cuted_points - max_idx_in_row.shape[0] if n_add < n_unsample_points: # select unsampled points unsample_points = o3d.select_down_sample( src, max_idx_in_row, True) add_idx = np.random.choice(n_unsample_points, n_add, replace=False) add_points = o3d.select_down_sample(unsample_points, add_idx) pcd_down += add_points return pcd_down else: return None else: return pcd_down
def create_grid(self, points, colors, cell_size, radius, annotations): max_width = np.amax(points[:, 0]) min_width = np.amin(points[:, 0]) min_height = np.amin(points[:, 1]) max_height = np.amax(points[:, 1]) max_depth = np.amax(points[:, 2]) min_depth = np.amin(points[:, 2]) print(max_width, min_width) counter = 0 for i in range(int(min_width * cell_size), int(max_width * cell_size) + 1): for j in range( int(min_height * cell_size) - 1, int(max_height * cell_size) + 1): for k in range(int(min_depth * cell_size), int(max_depth * cell_size) + 2): if (i < int(max_width * cell_size) + 1 and j < int(max_height * cell_size) + 1 and k + 1 < int(max_depth * cell_size)): self.grid_lines.append([counter, counter + 1]) if (i < int(max_width * cell_size) + 1 and j + 1 < int(max_height * cell_size) and k + 1 < int(max_depth * cell_size)): self.grid_lines.append([ counter, counter + (int(max_height * cell_size)) + int(max_depth * cell_size) ]) if (i < int(max_width * cell_size) + 1 and j < int(max_height * cell_size) + 1 and k < int(max_depth * cell_size) + 2): self.cells.append([ i / cell_size, j / cell_size, k / cell_size, (i + 1) / cell_size, (j + 1) / cell_size, (k + int((max_depth - min_depth) * 2)) / cell_size ]) self.corners.append( ([i / cell_size, j / cell_size, k / cell_size])) counter = counter + 1 print(self.corners) print(self.cells) ######################################################################################################################################## ####################Create group for each center of cell ############################################################################# ###################################################################################################################################### pcd2 = opend.PointCloud() pcd2.points = opend.Vector3dVector(points) pcd2.colors = opend.Vector3dVector(colors) # opend.draw_geometries([pcd2]) pcd_tree = opend.KDTreeFlann(pcd2) counter = 0 nb_pt = 0 for i, c in enumerate(self.cells): center = np.array([ c[0] + (c[3] - c[0]) / 2, c[1] + (c[4] - c[1]) / 2, c[2] + (c[5] - c[2]) / 2 ]) [k, idx, _] = pcd_tree.search_radius_vector_3d(center, radius) if (np.asarray(idx).shape[0] > 256): counter = counter + 1 nb_pt = nb_pt + np.asarray(idx).shape[0] self.assigned.append(points[np.array(idx)]) self.colors.append(colors[np.array(idx)]) self.annotations.append(annotations[np.array([idx ])].squeeze(0)) self.centers.append(center) ######################################################################################################################################## ####################Fuse neigboring cells if their number of point is too low ######################################################### ######################################################################################################################################## pcd3 = opend.PointCloud() pcd3.points = opend.Vector3dVector(self.centers) pcd_tree = opend.KDTreeFlann(pcd3) deleted = [] for j, pg in enumerate(self.assigned): center = self.centers[j] [k, idx, _] = pcd_tree.search_knn_vector_3d(center, 3) if (j not in deleted): if (pg.shape[0] <= 1024): cell_values = self.assigned[idx[0]] next_cell_values = self.assigned[idx[1]] next_next_cell_values = self.assigned[idx[2]] if (cell_values.shape[0] + next_cell_values.shape[0] <= 8000): self.assigned[j] = np.concatenate( [self.assigned[j], next_cell_values], 0) self.colors[j] = np.concatenate( [self.colors[j], self.colors[idx[1]]], 0) self.annotations[j] = np.concatenate( [self.annotations[j], self.annotations[idx[1]]], 0) deleted.append(idx[1]) if (cell_values.shape[0] + next_cell_values.shape[0] <= 1024): if (cell_values.shape[0] + next_cell_values.shape[0] + next_next_cell_values.shape[0] <= 8000): self.assigned[j] = np.concatenate( [self.assigned[j], next_next_cell_values], 0) self.colors[j] = np.concatenate( [self.colors[j], self.colors[idx[2]]], 0) self.annotations[j] = np.concatenate([ self.annotations[j], self.annotations[idx[2]] ], 0) deleted.append(idx[2]) res_assigned = [] res_annotations = [] for j, p in enumerate(self.assigned): if (j not in deleted): res_assigned.append(self.assigned[j]) res_annotations.append(self.annotations[j]) self.assigned = res_assigned self.annotations = res_annotations