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
