def apply(self, point_cloud):
bin_size = 2 * np.pi / self.__number_of_bins
histogram = [list() for _ in range(self.__number_of_bins)]
result = PointCloud(point_cloud.lidar_origin,
point_cloud.lidar_orientation)
list_of_points = point_cloud.map_frame_list
# generate histogram
for index in range(len(list_of_points)):
angle = point_cloud.get_descriptor("normals_direction", index)
bin_index = int(np.floor(angle / bin_size))
histogram[bin_index].append(index)
# remove points of the largest remaining bin to maximize entropy of resulting normal angle distribution
for _ in range(point_cloud.count - self.wished_size):
lengths = list(
map(lambda histogram_bin: len(histogram_bin), histogram))
index = lengths.index(max(lengths))
# remove random point from bin
point_index = random.randint(0, len(histogram[index]) - 1)
histogram[index].pop(point_index)
# create result point cloud
for histogram_bin in histogram:
for index in histogram_bin:
result.add_point([list_of_points[index]])
result.add_descriptor(
"normals", point_cloud.get_descriptor("normals", index))
result.add_descriptor(
"normals_direction",
point_cloud.get_descriptor("normals_direction", index))
return result
def filter(self, point_cloud):
result = PointCloud(point_cloud.lidar_origin, point_cloud.lidar_orientation)
for point in point_cloud.map_frame_list:
element = self.index_to_key_(self.get_cell_index_(point))
if not(element in self.voxel_set_):
self.voxel_set_.add(element)
result.add_point([point])
return result
def apply(self, point_cloud):
result = PointCloud(point_cloud.lidar_origin,
point_cloud.lidar_orientation)
copy_list = copy.deepcopy(point_cloud.map_frame_list)
shuffle(copy_list)
list_of_points = copy_list[0:self.wished_size]
for point in list_of_points:
result.add_point([point])
return result
def scan(self, environment, origin):
point_cloud = PointCloud(origin, 0)
for ray_angle in np.arange(self.min_angle, self.max_angle,
self.angular_resultion):
ray_angle_noise = np.random.normal(0, self.angular_variance, 1)
ray_angle += ray_angle_noise
dx = np.sin(ray_angle)
dy = np.cos(ray_angle)
ray = (origin[0] + self.max_range * dx,
origin[1] + self.max_range * dy)
ray_line = LineString([origin, ray])
# x, y = ray_line.xy
# ax.plot(x, y, color='black', linewidth=1)
intersect = ray_line.intersection(environment)
if not intersect.is_empty:
if (isinstance(intersect, shapely.geometry.point.Point)):
np_ray = np.array([
intersect.coords[0][0] - origin[0],
intersect.coords[0][1] - origin[1]
])
ray_length = np.linalg.norm(np_ray)
range_noise = np.random.normal(0, self.range_variance, 1)
point_cloud.add_point([
origin + np_ray *
((ray_length + range_noise) / ray_length)
])
elif (isinstance(intersect,
shapely.geometry.multipoint.MultiPoint)):
# get observation closest to the sensor
# print(intersect)
nearest_observation_distance = np.inf
nearest_observation = None
for p in intersect:
candidate_distance = np.linalg.norm(
np.array(origin) - np.array(p))
if candidate_distance < nearest_observation_distance:
nearest_observation = p
nearest_observation_distance = candidate_distance
np_ray = np.array([
nearest_observation.x - origin[0],
nearest_observation.y - origin[1]
])
ray_length = np.linalg.norm(np_ray)
range_noise = np.random.normal(0, self.range_variance, 1)
point_cloud.add_point([
origin + np_ray *
((ray_length + range_noise) / ray_length)
])
else:
print('Unhandled intersection type', type(intersect))
return point_cloud
def plot_image(pc: point_cloud.PointCloud, img: np.ndarray, depth_map: np.ndarray, scale=0.2, step=1):
"""
Renders pixels in 3D with depth as Z
:param pc: PointCloud object
:param img: colorized image to plot (shape (w, h, 3))
:param depth_map: depth map (shape (w, h))
:param scale: distance between points
:param step: step size of loop through pixels
:return: void
"""
for y in range(0, img.shape[1], step):
for x in range(0, img.shape[0], step):
if x >= img.shape[1] or y >= img.shape[0]:
break
pc.add_point((x * scale, y * scale, depth_map[x, y]), tuple(img[x, y]))
