def create_sliced_voxel_grid_2d(self,
sample_name,
source,
image_shape=None):
"""Generates a filtered 2D voxel grid from point cloud data
Args:
sample_name: image name to generate stereo pointcloud from
source: point cloud source - 'stereo', 'lidar', or 'depth'
image_shape: image dimensions [h, w], only required when
source is 'lidar' or 'depth'
Returns:
voxel_grid_2d: 3d voxel grid from the given image
"""
img_idx = int(sample_name)
ground_plane = obj_utils.get_road_plane(img_idx,
self.dataset.planes_dir)
point_cloud = self.get_point_cloud(source,
img_idx,
image_shape=image_shape)
filtered_points = self._apply_slice_filter(point_cloud, ground_plane)
# Create Voxel Grid
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(filtered_points,
self.voxel_size,
extents=self.area_extents,
ground_plane=ground_plane,
create_leaf_layout=True)
return voxel_grid_2d
def create_sliced_voxel_grid_2d(self,
sample_name,
source,
image_shape=None):
"""Generates a filtered 2D voxel grid from point cloud data
Args:
sample_name: image name to generate stereo pointcloud from
source: point cloud source, e.g. 'lidar'
image_shape: image dimensions [h, w], only required when
source is 'lidar' or 'depth'
Returns:
voxel_grid_2d: 3d voxel grid from the given image
"""
img_idx = int(sample_name)
ground_plane = obj_utils.get_road_plane(img_idx,
self.dataset.planes_dir)
# 点云->相机->像素(坐标)->图片(保留投影在图片里的点云投影坐标)
# 返回在图片里的点云三维坐标,坐标在相机坐标系下
point_cloud = self.get_point_cloud(source,
img_idx,
image_shape=image_shape)
filtered_points = self._apply_slice_filter(
point_cloud,
ground_plane) #保留在x,y,z范围里并且高度在[height_lo,height_hi]的点云
# 将point_cloud等投影到图片里，并显示
if img_idx == 23:
self._project_and_show(sample_name, point_cloud, "red",
"point_cloud")
self._project_and_show(sample_name, filtered_points.T, "red",
"filtered_points")
# Create Voxel Grid
# 将点云坐标离散化为体素voxel，相当于将空间划分为voxel_size大小的网格，统计在网格里的点云个数，网格坐标voxel_coords只保留一个。
# 创建体素网，网格内包含点云则用0表示，不包含点云则用-1表示
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(filtered_points,
self.voxel_size,
extents=self.area_extents,
ground_plane=ground_plane,
create_leaf_layout=True)
return voxel_grid_2d
def test_voxel_grid_2d_extents(self):
voxel_grid = VoxelGrid2D()
# Generate random points between xyz [[-40, 40], [-4, 4], [-30, 30]]
points = (np.random.rand(70000, 3) * [80, 8, 60]) - [40, 4, 0]
# Test bad extents
bad_extents = np.array([[-30, 30], [-3, 3], [10, 60]])
self.assertRaises(ValueError, voxel_grid.voxelize_2d, points, 0.1,
bad_extents)
extents = np.array([[-50, 50], [-5, 5], [0, 70]])
voxel_grid.voxelize_2d(points, 0.1, extents)
# Check number of divisions and leaf layout shape are correct and are
# the same. y-axis shapes should be 1
self.assertTrue((voxel_grid.num_divisions == [1000, 1, 700]).all())
self.assertTrue(voxel_grid.leaf_layout_2d.shape == (1000, 1, 700))
def create_sliced_voxel_grid_2d_jhuang(self, ground_plane, point_cloud):
"""Generates a filtered 2D voxel grid from point cloud data
Args:
ground_plane:
point_cloud:
Returns:
voxel_grid_2d: 3d voxel grid from the given image
"""
filtered_points = self._apply_slice_filter(point_cloud, ground_plane)
# Create Voxel Grid
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(filtered_points,
self.voxel_size,
extents=self.area_extents,
ground_plane=ground_plane,
create_leaf_layout=True)
return voxel_grid_2d
def test_voxelization_2d(self):
voxel_grid = VoxelGrid2D()
# Test with actual data
voxel_grid.voxelize_2d(self.test_points, 0.1)
# Test Size variable
self.assertAlmostEqual(voxel_grid.voxel_size, 0.1)
# Test Minimum Coordinates
self.assertTrue((voxel_grid.min_voxel_coord == [-400, 0, 0]).all())
# Test Maximum Coordinates
self.assertTrue((voxel_grid.max_voxel_coord == [399, 0, 699]).all())
# Test Divisions
self.assertTrue((voxel_grid.num_divisions == [800, 1, 700]).all())
# Test every entry of out put leafs
self.assertTrue(
(voxel_grid.leaf_layout_2d == self.expected_leaf_layout).all())
def test_voxel_2d_coordinate_conversion(self):
voxel_grid = VoxelGrid2D()
# Generate random points between xyz [[-40, 40], [-4, 4], [-30, 30]]
points = (np.random.rand(70000, 3) * [80, 8, 60]) - [40, 4, 0]
extents = np.array([[-50, 50], [-5, 5], [0, 70]])
voxel_grid.voxelize_2d(points, 0.1, extents)
# Left Top Corner, z = 0
coordinates = np.array([[0, 0]])
# Map spans from [-500, 500], [0, 700]
expected = np.array([500, 0])
self.assertTrue(
(voxel_grid.map_to_index(coordinates) == expected).all())
coordinates = np.array([[0, 0]]) + 0.1
# Increment of 1 grid size
expected = np.array([501, 1])
self.assertTrue(
(voxel_grid.map_to_index(coordinates) == expected).all())
# Start of Grid
coordinates = np.array([[-50, 0]])
expected = np.array([0, 0])
self.assertTrue(
(voxel_grid.map_to_index(coordinates) == expected).all())
# End of Grid
coordinates = np.array([[50, 70]])
expected = np.array([1000, 700])
self.assertTrue(
(voxel_grid.map_to_index(coordinates) == expected).all())
# Outside the grid
coordinates = coordinates + 10
expected = np.array([1000, 700])
self.assertTrue(
(voxel_grid.map_to_index(coordinates) == expected).all())
def generate_bev(self,
source,
point_cloud,
ground_plane,
area_extents,
voxel_size,
output_indices=False):
"""Generates the BEV maps dictionary. One height map is created for
each slice of the point cloud. One density map is created for
the whole point cloud.
Args:
source: point cloud source
point_cloud: point cloud (3, N) WZN: already in camera frame
ground_plane: ground plane coefficients
area_extents: 3D area extents
[[min_x, max_x], [min_y, max_y], [min_z, max_z]]
voxel_size: voxel size in m
Returns:
BEV maps dictionary
height_maps: list of height maps
density_map: density map
"""
all_points = np.transpose(point_cloud)
height_maps = []
voxel_indices_stack = []
points_in_voxel_stack = []
for slice_idx in range(self.num_slices):
height_lo = self.height_lo + slice_idx * self.height_per_division
height_hi = height_lo + self.height_per_division
slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, height_lo, height_hi)
# Apply slice filter
slice_points = all_points[slice_filter]
if len(slice_points) > 1:
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(slice_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Remove y values (all 0)
voxel_indices = voxel_grid_2d.voxel_indices[:, [0, 2]]
# Create empty BEV images
height_map = np.zeros((voxel_grid_2d.num_divisions[0],
voxel_grid_2d.num_divisions[2]))
# Only update pixels where voxels have max height values,
# and normalize by height of slices
voxel_grid_2d.heights = voxel_grid_2d.heights - height_lo
height_map[voxel_indices[:, 0], voxel_indices[:, 1]] = \
np.asarray(voxel_grid_2d.heights) / self.height_per_division
height_maps.append(height_map)
#WZN: added to keep track of all used point clouds
# output the rotated indices as the map is rotated 90deg
if output_indices:
voxel_indices_rot = np.copy(voxel_indices)
#BEV z axis is reversed, if z=70(max_extent), voxel_indices[:, 1]should be 0 at BEV image.
voxel_indices_rot[:, 1] = voxel_grid_2d.num_divisions[
2] - voxel_indices_rot[:, 1]
#voxel_indices_rot = voxel_indices_rot[:,[1,0]]
voxel_indices_stack.append(voxel_indices_rot)
#only choose one point at each voxel
points_in_voxel_stack.append(voxel_grid_2d.unique_pts)
# Rotate height maps 90 degrees
# (transpose and flip) is faster than np.rot90
height_maps_out = [
np.flip(height_maps[map_idx].transpose(), axis=0)
for map_idx in range(len(height_maps))
]
density_slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, self.height_lo,
self.height_hi)
density_points = all_points[density_slice_filter]
# Create Voxel Grid 2D
density_voxel_grid_2d = VoxelGrid2D()
density_voxel_grid_2d.voxelize_2d(density_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Generate density map
density_voxel_indices_2d = \
density_voxel_grid_2d.voxel_indices[:, [0, 2]]
density_map = self._create_density_map(
num_divisions=density_voxel_grid_2d.num_divisions,
voxel_indices_2d=density_voxel_indices_2d,
num_pts_per_voxel=density_voxel_grid_2d.num_pts_in_voxel,
norm_value=self.NORM_VALUES[source])
bev_maps = dict()
bev_maps['height_maps'] = height_maps_out
bev_maps['density_map'] = density_map
#pdb.set_trace()
if output_indices:
return bev_maps, np.vstack(voxel_indices_stack), np.vstack(
points_in_voxel_stack)
else:
return bev_maps
def generate_bev(self, source, point_cloud, ground_plane, area_extents,
voxel_size):
"""Generates the BEV maps dictionary. One height map is created for
each slice of the point cloud. One density map is created for
the whole point cloud.
Args:
source: point cloud source
point_cloud: point cloud (3, N)
ground_plane: ground plane coefficients
area_extents: 3D area extents
[[min_x, max_x], [min_y, max_y], [min_z, max_z]]
voxel_size: voxel size in m
Returns:
BEV maps dictionary
height_maps: list of height maps
density_map: density map
"""
all_points = np.transpose(point_cloud)
height_maps = []
for slice_idx in range(self.num_slices):
height_lo = self.height_lo + slice_idx * self.height_per_division
height_hi = height_lo + self.height_per_division
slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, height_lo, height_hi)
# Apply slice filter
slice_points = all_points[slice_filter]
if len(slice_points) > 1:
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(slice_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Remove y values (all 0)
voxel_indices = voxel_grid_2d.voxel_indices[:, [0, 2]]
# Create empty BEV images
height_map = np.zeros((voxel_grid_2d.num_divisions[0],
voxel_grid_2d.num_divisions[2]))
# Only update pixels where voxels have max height values,
# and normalize by height of slices
voxel_grid_2d.heights = voxel_grid_2d.heights - height_lo
height_map[voxel_indices[:, 0], voxel_indices[:, 1]] = \
np.asarray(voxel_grid_2d.heights) / self.height_per_division
height_maps.append(height_map)
# Rotate height maps 90 degrees
# (transpose and flip) is faster than np.rot90
height_maps_out = [
np.flip(height_maps[map_idx].transpose(), axis=0)
for map_idx in range(len(height_maps))
]
density_slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, self.height_lo,
self.height_hi)
density_points = all_points[density_slice_filter]
# Create Voxel Grid 2D
density_voxel_grid_2d = VoxelGrid2D()
density_voxel_grid_2d.voxelize_2d(density_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Generate density map
density_voxel_indices_2d = \
density_voxel_grid_2d.voxel_indices[:, [0, 2]]
density_map = self._create_density_map(
num_divisions=density_voxel_grid_2d.num_divisions,
voxel_indices_2d=density_voxel_indices_2d,
num_pts_per_voxel=density_voxel_grid_2d.num_pts_in_voxel,
norm_value=self.NORM_VALUES[source])
bev_maps = dict()
bev_maps['height_maps'] = height_maps_out
bev_maps['density_map'] = density_map
return bev_maps
def generate_bev(self,
source,
point_cloud,
ground_plane,
area_extents,
voxel_size):
"""Generates the BEV maps dictionary. One height map is created for
each cluster size in the dataset. One density map is created for
the whole point cloud.
Args:
source: point cloud source, only used for normalization
point_cloud: point cloud (3, N)
ground_plane: ground plane coefficients
area_extents: 3D area extents
[[min_x, max_x], [min_y, max_y], [min_z, max_z]]
voxel_size: voxel size in m
Returns:
BEV maps dictionary
height_maps: list of height maps
density_map: density map
"""
slice_filter = self.kitti_utils.create_slice_filter(
point_cloud,
area_extents,
ground_plane,
self.ground_filter_offset,
self.offset_filter_distance)
# Reshape points into N x [x, y, z]
all_points = np.transpose(point_cloud)[slice_filter]
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(
all_points, voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Remove y values (all 0)
voxel_indices_2d = voxel_grid_2d.voxel_indices[:, [0, 2]]
all_clusters = np.concatenate(self.kitti_utils.clusters)
all_std_devs = np.concatenate(self.kitti_utils.std_devs)
# Create empty BEV images
height_maps = np.zeros((len(all_clusters),
voxel_grid_2d.num_divisions[0],
voxel_grid_2d.num_divisions[2]))
# Get last element of clusters (l, w, h)
height_clusters = np.vstack(all_clusters)[:, [2]]
height_std_devs = np.vstack(all_std_devs)[:, [2]]
height_priors = gaussian_prior(voxel_grid_2d.heights,
height_clusters, height_std_devs,
self.std_dev_multiplier)
# Only update pixels where voxels have max height values
height_maps[:, voxel_indices_2d[:, 0], voxel_indices_2d[:, 1]] = \
np.asarray(height_priors)
# Rotate images 90 degrees
# (transpose and flip) is faster than np.rot90
height_maps_out = [np.flip(height_maps[map_idx].transpose(), axis=0)
for map_idx in range(len(height_maps))]
# Generate density map
density_map = self._create_density_map(
num_divisions=voxel_grid_2d.num_divisions,
voxel_indices_2d=voxel_indices_2d,
num_pts_per_voxel=voxel_grid_2d.num_pts_in_voxel,
norm_value=self.NORM_VALUES[source])
bev_maps = dict()
bev_maps['height_maps'] = height_maps_out
bev_maps['density_map'] = density_map
return bev_maps
def generate_bev(self, source, point_cloud, ground_plane, area_extents,
voxel_size):
"""Generates the BEV maps dictionary. Generates specified feature maps for each slice or the whole point cloud.
Args:
source: point cloud source
point_cloud: point cloud (3, N)
ground_plane: ground plane coefficients
area_extents: 3D area extents
[[min_x, max_x], [min_y, max_y], [min_z, max_z]]
voxel_size: voxel size in m
Returns:
BEV maps dictionary
slice_maps: list of feature maps per slice
cloud_maps: list of feature maps for the whole point cloud
"""
all_points = np.transpose(point_cloud)
slice_maps = []
cloud_maps = []
if len(self.slice_maps) > 0:
for slice_idx in range(self.num_slices):
height_lo = self.height_lo + slice_idx * self.height_per_division
height_hi = height_lo + self.height_per_division
slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, height_lo,
height_hi)
# Apply slice filter
slice_points = all_points[slice_filter]
if len(
slice_points
) >= 0: # Should probably apply the fix for empty BEV slices
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(slice_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False,
maps=self.slice_maps)
self.generate_bev_map(voxel_grid_2d, self.slice_maps,
slice_maps, height_lo, height_hi,
area_extents, source)
if len(self.cloud_maps) > 0:
cloud_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, self.height_lo,
self.height_hi)
cloud_points = all_points[cloud_filter]
if len(cloud_points >= 0):
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(cloud_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False,
maps=self.cloud_maps)
self.generate_bev_map(voxel_grid_2d, self.cloud_maps,
cloud_maps, self.height_lo,
self.height_hi, area_extents, source)
bev_maps = dict()
bev_maps['slice_maps'] = slice_maps
bev_maps['cloud_maps'] = cloud_maps
return bev_maps
def generate_bev(self, source, point_cloud, ground_plane, area_extents,
voxel_size):
"""Generates the BEV maps dictionary. One height map is created for
each slice of the point cloud. One density map is created for
the whole point cloud.
Args:
source: point cloud source
point_cloud: point cloud (3, N)
ground_plane: ground plane coefficients
area_extents: 3D area extents
[[min_x, max_x], [min_y, max_y], [min_z, max_z]]
voxel_size: voxel size in m
Returns:
BEV maps dictionary
height_maps: list of height maps
density_map: density map
"""
all_points = np.transpose(point_cloud)
height_maps = []
for slice_idx in range(self.num_slices):
height_lo = self.height_lo + slice_idx * self.height_per_division
height_hi = height_lo + self.height_per_division
slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, height_lo, height_hi)
# Apply slice filter
slice_points = all_points[slice_filter]
# ***This fixes bug where if there's no points in a slice script will crash
# Add a point if there aren't any valid ones
# This shouldn't change detections significantly
# TODO This is not the best option, would be better to allow 0 voxels with pts
if len(slice_points) == 0:
ground_plane = np.array(ground_plane)
offset_dist = (height_lo + height_hi) / 2
offset_plane = ground_plane + [0, 0, 0, -offset_dist]
new_pt = np.array(
[area_extents[0, 1], 0, area_extents[2, 1], 1])
new_pt_height = np.dot(offset_plane, new_pt.T)
new_pt[1] = new_pt_height
slice_points = np.array([new_pt[0:3]])
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(slice_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Remove y values (all 0)
voxel_indices = voxel_grid_2d.voxel_indices[:, [0, 2]]
# Create empty BEV images
height_map = np.zeros((voxel_grid_2d.num_divisions[0],
voxel_grid_2d.num_divisions[2]))
# Only update pixels where voxels have max height values,
# and normalize by height of slices
voxel_grid_2d.heights = voxel_grid_2d.heights - height_lo
height_map[voxel_indices[:, 0], voxel_indices[:, 1]] = \
np.asarray(voxel_grid_2d.heights) / self.height_per_division
height_maps.append(height_map)
# Rotate height maps 90 degrees
# (transpose and flip) is faster than np.rot90
height_maps_out = [
np.flip(height_maps[map_idx].transpose(), axis=0)
for map_idx in range(len(height_maps))
]
density_slice_filter = self.kitti_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, self.height_lo,
self.height_hi)
density_points = all_points[density_slice_filter]
# Create Voxel Grid 2D
density_voxel_grid_2d = VoxelGrid2D()
density_voxel_grid_2d.voxelize_2d(density_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Generate density map
density_voxel_indices_2d = \
density_voxel_grid_2d.voxel_indices[:, [0, 2]]
density_map = self._create_density_map(
num_divisions=density_voxel_grid_2d.num_divisions,
voxel_indices_2d=density_voxel_indices_2d,
num_pts_per_voxel=density_voxel_grid_2d.num_pts_in_voxel,
norm_value=self.NORM_VALUES[source])
bev_maps = dict()
bev_maps['height_maps'] = height_maps_out
bev_maps['density_map'] = density_map
return bev_maps
def generate_bev(self, source, point_cloud, ground_plane, area_extents,
voxel_size):
"""Generates the BEV maps dictionary. One height map is created for
each slice of the point cloud. One density map is created for
the whole point cloud.
Args:
source: point cloud source
point_cloud: point cloud (3, N)
ground_plane: ground plane coefficients
area_extents: 3D area extents
[[min_x, max_x], [min_y, max_y], [min_z, max_z]]
voxel_size: voxel size in m
Returns:
BEV maps dictionary
height_maps: list of height maps
density_map: density map
"""
all_points = np.transpose(point_cloud)
occupancy_maps = []
height_maps = []
depth_cam_voxel_coord = np.floor(depth_cam_coord / voxel_size).astype(
np.int32).reshape(-1)
# Generate height map for each slice
for slice_idx in range(
self.num_slices
): # pplp_pedestrian_panoptic.config: num_slices = 1
height_lo = self.height_lo + slice_idx * self.height_per_division
height_hi = height_lo + self.height_per_division
# Get all point cloud within 3D areas and heights (like 3D box)
slice_filter = self.panoptic_utils.create_slice_filter(
point_cloud,
area_extents, # area_extents: [-3.99, 3.99, -5.0, 3.0, 0.0, 6.995]
ground_plane,
height_lo, # -5.00 (in y direction)
height_hi) # 2.00
# Apply slice filter
slice_points = all_points[slice_filter]
# print('bev_panoptic_slices.py :: slice_points = ', slice_points)
# Create Voxel Grid 2D
voxel_grid_2d = VoxelGrid2D()
voxel_grid_2d.voxelize_2d(
slice_points,
voxel_size, # voxel_size: 0.01
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Remove y values (all 0)
# voxel_indices all positive
voxel_indices = voxel_grid_2d.voxel_indices[:, [
0, 2
]] # voxel_indices: unique indexes - only 1 point is chosen to be occupied in one voxel
# Bring cam to new origin
depth_cam_index = (depth_cam_voxel_coord -
voxel_grid_2d.min_voxel_coord).astype(int)
color_cam_index = (np.array([0, 0, 0]) / voxel_size -
voxel_grid_2d.min_voxel_coord).astype(int)
# Create empty BEV images
height_map = np.zeros((voxel_grid_2d.num_divisions[0],
voxel_grid_2d.num_divisions[2]))
# Only update pixels where voxels have max height values,
# and normalize by height of slices
voxel_grid_2d.heights = voxel_grid_2d.heights - height_lo
height_map[voxel_indices[:, 0], voxel_indices[:, 1]] = \
np.asarray(voxel_grid_2d.heights) / self.height_per_division
height_maps.append(height_map)
# Create empty BEV occupancy map
occupancy_map = np.zeros((voxel_grid_2d.num_divisions[0],
voxel_grid_2d.num_divisions[2]))
fov_pt_right_x = area_extents[0][1]
fov_pt_right_z = fov_pt_right_x / math.tan(fovx / 2)
fov_pt_left_x = -fov_pt_right_x
fov_pt_left_z = fov_pt_right_z
fov_pt_right = np.array([fov_pt_right_x, 0, fov_pt_right_z])
fov_pt_left = np.array([fov_pt_left_x, 0, fov_pt_left_z])
# print(depth_cam_index)
fov_pt_right_index = (fov_pt_right / voxel_size -
voxel_grid_2d.min_voxel_coord).astype(int)
pts_on_fov_right_line = bresenham_line.supercover_line(
(color_cam_index[0], color_cam_index[1]),
(fov_pt_right_index[0], fov_pt_right_index[2]),
occupancy_map.shape[0], occupancy_map.shape[1])
pts_on_fov_right_line_x, pts_on_fov_right_line_y = zip(
*pts_on_fov_right_line)
occupancy_map[list(pts_on_fov_right_line_x),
list(pts_on_fov_right_line_y)] = -1
fov_pt_left_index = (fov_pt_left / voxel_size -
voxel_grid_2d.min_voxel_coord).astype(int)
pts_on_fov_left_line = bresenham_line.supercover_line(
(color_cam_index[0], color_cam_index[1]),
(fov_pt_left_index[0], fov_pt_left_index[2]),
occupancy_map.shape[0], occupancy_map.shape[1])
pts_on_fov_left_line_x, pts_on_fov_left_line_y = zip(
*pts_on_fov_left_line)
occupancy_map[list(pts_on_fov_left_line_x),
list(pts_on_fov_left_line_y)] = -1
# Set the occluded points behind pedestrians
for pt in range(len(voxel_indices)):
# TODO: fix. It should be depth_cam_index
pts_on_line = bresenham_line.supercover_line(
(color_cam_index[0], color_cam_index[2]),
(voxel_indices[pt, 0], voxel_indices[pt, 1]),
occupancy_map.shape[0], occupancy_map.shape[1])
ptcloud_index = pts_on_line.index(
(voxel_indices[pt, 0], voxel_indices[pt, 1]))
occluded_pts = pts_on_line[ptcloud_index + 1:]
if len(occluded_pts) == 0: continue
occluded_xs, occluded_ys = zip(*occluded_pts)
occupancy_map[list(occluded_xs), list(occluded_ys)] = -1
# np.savetxt("/home/trinhle/fcav-projects/Panoptic/PPLP/pplp/core/bev_generators/height_maps_ogm/height_map_original.txt", occupancy_map)
for (xl, yl) in pts_on_fov_left_line:
for x in range(xl):
occupancy_map[x, yl] = -1
for (xr, yr) in pts_on_fov_right_line:
for y in range(yr):
occupancy_map[xr, y] = -1
occupancy_map[voxel_indices[:, 0], voxel_indices[:, 1]] = 1
occupancy_maps.append(occupancy_map)
# np.savetxt("/home/trinhle/fcav-projects/Panoptic/PPLP/pplp/core/bev_generators/height_maps_ogm/height_map_completed_{}.txt".format(num), occupancy_map)
# Rotate occupancy and height maps 90 degrees
# (transpose and flip) is faster than np.rot90
height_maps_out = [
np.flip(height_maps[map_idx].transpose(), axis=0)
for map_idx in range(len(height_maps))
]
occupancy_maps_out = [
np.flip(occupancy_maps[map_idx].transpose(), axis=0)
for map_idx in range(len(occupancy_maps))
]
# global viz_idx
# np.savetxt("/home/trinhle/fcav-projects/Panoptic/PPLP/pplp/core/bev_generators/height_maps_ogm/height_map_rotated_{0:04d}.txt".format(viz_idx), occupancy_maps_out[0])
# viz_idx += 1
density_slice_filter = self.panoptic_utils.create_slice_filter(
point_cloud, area_extents, ground_plane, self.height_lo,
self.height_hi)
density_points = all_points[density_slice_filter]
# Create Voxel Grid 2D
density_voxel_grid_2d = VoxelGrid2D()
density_voxel_grid_2d.voxelize_2d(density_points,
voxel_size,
extents=area_extents,
ground_plane=ground_plane,
create_leaf_layout=False)
# Generate density map
density_voxel_indices_2d = \
density_voxel_grid_2d.voxel_indices[:, [0, 2]]
density_map = self._create_density_map(
num_divisions=density_voxel_grid_2d.num_divisions,
voxel_indices_2d=density_voxel_indices_2d,
num_pts_per_voxel=density_voxel_grid_2d.num_pts_in_voxel,
norm_value=self.NORM_VALUES[source])
bev_maps = dict()
bev_maps['height_maps'] = height_maps_out
bev_maps['occupancy_maps'] = occupancy_maps_out
bev_maps['density_map'] = density_map
return bev_maps