def compute_cell_neighborhood(environment_pc, target_pc, side_length):
"""
Find the indices of points within a square neighbourhood for a given point of a target point cloud among the
points from an environment point cloud.
:param environment_pc: environment point cloud
:param target_pc: point cloud that contains the points at which neighborhoods are to be calculated
:param side_length: search radius for neighbors
:return: indices of neighboring points from the environment point cloud for each target point
"""
max_radius = 0.5 * math.sqrt((side_length**2) + (side_length**2))
neighbors = compute_cylinder_neighborhood(environment_pc, target_pc,
max_radius)
counter = 0 # Target and neighborhood indices are going to be out of sync in loop below.
for neighborhood_indices in neighbors:
result = []
for i, _ in enumerate(neighborhood_indices):
target_x, target_y, _ = utils.get_point(target_pc, counter)
counter += 1
neighbor_indices = neighborhood_indices[i]
result_indices = []
for j in neighbor_indices:
env_x, env_y, _ = utils.get_point(environment_pc, j)
if ((abs(target_x - env_x)) > 0.5 * side_length) or (
(abs(target_y - env_y)) > 0.5 * side_length):
continue
else:
result_indices.append(j)
result.append(result_indices)
yield result
def compute_cube_neighborhood(environment_pc, target_pc, side_length):
"""
Find the indices of points within a square neighbourhood for a given point of a target point cloud among the
points from an environment point cloud.
:param environment_pc: environment point cloud
:param target_pc: point cloud that contains the points at which neighborhoods are to be calculated
:param side_length: search radius for neighbors
:return: indices of neighboring points from the environment point cloud for each target point
"""
neighbors = compute_cell_neighborhood(environment_pc, target_pc,
side_length)
counter = 0
for neighborhood_indices in neighbors:
result = []
for i, _ in enumerate(neighborhood_indices):
_, _, target_z = utils.get_point(target_pc, counter)
counter += 1
neighbor_indices = neighborhood_indices[i]
result_indices = []
for j in neighbor_indices:
_, _, env_z = utils.get_point(environment_pc, j)
if abs(target_z - env_z) > side_length:
continue
else:
result_indices.append(j)
result.append(result_indices)
yield result
def compute_sphere_neighborhood(environment_pc, target_pc, radius):
"""
Find the indices of points within a spherical neighbourhood for a given point of a target point cloud among the
points from an environment point cloud.
:param environment_pc: environment point cloud
:param target_pc: point cloud that contains the points at which neighborhoods are to be calculated
:param radius: search radius for neighbors
:return: indices of neighboring points from the environment point cloud for each target point
"""
neighborhoods = compute_cylinder_neighborhood(environment_pc, target_pc,
radius)
counter = 0 # Target and neighborhood indices are going to be out of sync in loop below.
for neighborhood_indices in neighborhoods:
result = []
for i, _ in enumerate(neighborhood_indices):
target_x, target_y, target_z = utils.get_point(target_pc, counter)
counter += 1
result_indices = []
for j in neighborhood_indices[i]:
env_x, env_y, env_z = utils.get_point(environment_pc, j)
if abs(target_z - env_z) > radius:
continue
if (env_x - target_x)**2 + (env_y - target_y)**2 + (
env_z - target_z)**2 <= radius**2:
result_indices.append(j)
result.append(result_indices)
yield result
def test_GetPointCloudPoint(self):
""" Should not raise exception. """
pc = test_tools.generate_tiny_test_point_cloud()
x, y, z = utils.get_point(pc, 1)
self.assertEqual(2, x)
self.assertEqual(3, y)
self.assertEqual(4, z)
def extract(self, point_cloud, neighborhoods, target_point_cloud,
target_index, volume_description):
"""
Extract the feature value(s) of the point cloud at location of the target.
:param point_cloud: environment (search space) point cloud
:param neighborhoods: array of array of indices of points within the point_cloud argument
:param target_point_cloud: point cloud that contains target point
:param target_index: index of the target point in the target point cloud
:param volume_description: volume object that describes the shape and size of the search volume
:return: feature value
"""
if volume_description.TYPE != 'infinite cylinder':
raise ValueError('The volume must be a cylinder')
if target_point_cloud is None:
raise ValueError('Target point cloud required')
if target_index is None:
raise ValueError('Target point index required')
# xyz = self.get_neighborhood_positions(point_cloud, neighborhood)
xyz = get_xyz_per_neighborhood(point_cloud, neighborhoods)
n_cylinder = np.sum(xyz.mask[:, 0, :] == False, axis=1, dtype=float)
x0, y0, z0 = get_point(target_point_cloud, target_index)
xyz0 = np.column_stack((x0, y0, z0))
difference = xyz - xyz0[:, :, None]
sum_of_squares = np.sum(difference**2, 1)
n_sphere = np.sum(sum_of_squares <= volume_description.radius**2,
axis=1)
return n_sphere / n_cylinder
def _get_random_targets(self):
"""Get a random target pc."""
num_all_pc_points = len(self.point_cloud[keys.point]["x"]["data"])
rand_indices = [
random.randint(0, num_all_pc_points) for p in range(20)
]
x, y, z = utils.get_point(self.point_cloud, rand_indices)
return create_point_cloud(x, y, z)
def test_GetPointCloudPointFeature(self):
""" Should not raise exception. """
pc = test_tools.generate_tiny_test_point_cloud()
cols = 0.5 * (pc[keys.point]["x"]["data"] +
pc[keys.point]["y"]["data"])
pc[keys.point]["color"] = {"type": "double", "data": cols}
x, y, z = utils.get_point(pc, 1)
c = utils.get_attribute_value(pc, 1, "color")
self.assertEqual(c, 0.5 * (x + y))
def _assert_all_points_within_sphere(self, index_sets, target_point_cloud,
radius):
point_clouds = [
utils.copy_point_cloud(self.point_cloud, indices)
for indices in index_sets
]
n_targets = len(target_point_cloud[keys.point]["x"]["data"])
assert_equal(n_targets, len(point_clouds))
for i in range(n_targets):
target_x, target_y, target_z = utils.get_point(
target_point_cloud, i)
for j in range(len(point_clouds[i][keys.point]["x"]["data"])):
neighbor_x, neighbor_y, neighbor_z = utils.get_point(
point_clouds[i], j)
dist = np.sqrt((neighbor_x - target_x)**2 +
(neighbor_y - target_y)**2 +
(neighbor_z - target_z)**2)
self.assertTrue(dist <= radius)
def compute_sphere_neighborhood(environment_pc, target_pc, radius):
"""
Find the indices of points within a spherical neighbourhood for a given point of a target point cloud among the
points from an environment point cloud.
:param environment_pc: environment point cloud
:param target_pc: point cloud that contains the points at which neighborhoods are to be calculated
:param radius: search radius for neighbors
:return: indices of neighboring points from the environment point cloud for each target point
"""
neighborhoods = compute_cylinder_neighborhood(environment_pc, target_pc, radius)
for target_i, neighborhood_indices in enumerate(neighborhoods):
target_x, target_y, target_z = utils.get_point(target_pc, target_i)
result_indices = []
for neighbor_j in neighborhood_indices:
env_x, env_y, env_z = utils.get_point(environment_pc, neighbor_j)
if abs(target_z - env_z) > radius:
continue
if (env_x - target_x) ** 2 + (env_y - target_y) ** 2 + (env_z - target_z) ** 2 <= radius ** 2:
result_indices.append(neighbor_j)
yield result_indices
def extract(self, sourcepc, neighborhood, targetpc, targetindex, volume):
nbptsX, nbptsY, nbptsZ = utils.get_point(sourcepc, neighborhood)
matrix = np.column_stack((nbptsX, nbptsY, nbptsZ))
try:
eigenvals, eigenvecs = self._structure_tensor(matrix)
except ValueError as err:
if str(err) == 'Not enough points to compute eigenvalues/vectors.':
return [0, 0, 0]
else:
raise
return [eigenvals[0], eigenvals[1], eigenvals[2]]
def test_GetPointCloudPointFeatures(self):
""" Should not raise exception. """
pc = test_tools.generate_tiny_test_point_cloud()
cols = 0.5 * (pc[keys.point]["x"]["data"] +
pc[keys.point]["y"]["data"])
flavs = 0.5 * (pc[keys.point]["x"]["data"] -
pc[keys.point]["y"]["data"])
pc[keys.point]["color"] = {"type": "double", "data": cols}
pc[keys.point]["flavor"] = {"type": "double", "data": flavs}
x, y, z = utils.get_point(pc, 2)
c, f = utils.get_features(pc, ("color", "flavor"), 2)
self.assertEqual(c, 0.5 * (x + y))
self.assertEqual(f, 0.5 * (x - y))
def generate_targets(self,
min_x,
min_y,
max_x,
max_y,
n_tiles_side,
tile_mesh_size,
validate=True,
validate_precision=None):
"""
Generate the target point cloud.
:param min_x: Min x value of the tiling schema
:param min_y: Min y value of the tiling schema
:param max_x: Max x value of the tiling schema
:param max_y: Max y value of the tiling schema
:param n_tiles_side: Number of tiles along X and Y (tiling MUST be
square)
:param tile_mesh_size: Spacing between target points (in m). The tiles'
width must be an integer times this spacing
:param validate: If True, check if all points in the point-cloud belong
to the same tile
:param validate_precision: Optional precision threshold to determine
whether point belong to tile
"""
logger.info('Setting up the target grid')
self.grid.setup(min_x, min_y, max_x, max_y, n_tiles_side)
if any([idx is None for idx in self._tile_index]):
raise RuntimeError('Tile index not set!')
if validate:
logger.info('Checking whether points belong to cell '
'({},{})'.format(*self._tile_index))
x_all, y_all, _ = get_point(self.point_cloud, ...)
mask = self.grid.is_point_in_tile(x_all, y_all,
self._tile_index[0],
self._tile_index[1],
validate_precision)
assert np.all(mask), ('{} points belong to (a) different tile(s)'
'!'.format(len(x_all[~mask])))
logger.info('Generating target point mesh with '
'{}m spacing '.format(tile_mesh_size))
x_trgts, y_trgts = self.grid.generate_tile_mesh(
self._tile_index[0], self._tile_index[1], tile_mesh_size)
self.targets = create_point_cloud(x_trgts, y_trgts,
np.zeros_like(x_trgts))
return self
def _extract_one(source_point_cloud, neighborhood):
"""
Extract the feature value(s) of the point cloud at location of the target.
:param source_point_cloud: environment (search space) point cloud
:param neighborhood: array of indices of points within the point_cloud argument
:return:
"""
x, y, z = get_point(source_point_cloud, neighborhood)
try:
plane_estimator = fit_plane(x, y, z)
normalized = z - plane_estimator(x, y)
return np.std(normalized)
except LinAlgError:
return 0
def extract(self, source_point_cloud, neighborhood, target_point_cloud, target_index, volume_description):
"""
Extract the feature value(s) of the point cloud at location of the target.
:param source_point_cloud: environment (search space) point cloud
:param neighborhood: array of indices of points within the point_cloud argument
:param target_point_cloud: point cloud that contains target point
:param target_index: index of the target point in the target point cloud
:param volume_description: volume object describing the containing volume of the neighborhood
:return:
"""
x, y, z = get_point(source_point_cloud, neighborhood)
try:
plane_estimator = fit_plane(x, y, z)
normalized = z - plane_estimator(x, y)
return np.std(normalized)
except LinAlgError:
return 0
def extract(self, sourcepc, neighborhood, targetpc, targetindex, volume):
t2a, t2c = utils.get_features(targetpc, self.requires(), targetindex)
x, y, z = utils.get_point(targetpc, targetindex)
return t2c - t2a - z # z
def extract(self, sourcepc, neighborhood, targetpc, targetindex, volume):
t1b = utils.get_attribute_value(targetpc, targetindex,
self.requires()[0])
x, y, z = utils.get_point(targetpc, targetindex)
return [x + t1b, y + t1b, z + t1b] # x + 3z/2, y + 3z/2, 5z/2
def extract(self, sourcepc, neighborhood, targetpc, targetindex, volume):
x, y, z = utils.get_point(targetpc, targetindex)
return [0.5 * z, 1.5 * z]
def _extract_one(self, target_point_cloud, target_index):
x, y, z = utils.get_point(target_point_cloud, target_index)
return [0.5 * z, 1.5 * z]
def _extract_one(self, target_point_cloud, target_index):
t1a, t2c = utils.get_features(target_point_cloud, self.requires(),
target_index)
x, y, z = utils.get_point(target_point_cloud, target_index)
return t2c - t1a - z # this should be: 2 z
def _extract_one(self, target_point_cloud, target_index):
t1b = utils.get_attribute_value(target_point_cloud, target_index,
self.requires()[0])
x, y, z = utils.get_point(target_point_cloud, target_index)
return [x + t1b, y + t1b, z + t1b] # x + 3z/2, y + 3z/2, 5z/2
