def get_point_cloud(self, source, img_idx, image_shape=None):
""" Gets the points from the point cloud for a particular image,
keeping only the points within the area extents, and takes a slice
between self._ground_filter_offset and self._offset_distance above
the ground plane
Args:
source: point cloud source, e.g. 'lidar'
img_idx: An integer sample image index, e.g. 123 or 500
image_shape: image dimensions (h, w), only required when
source is 'lidar' or 'depth'
Returns:
The set of points in the shape (N, 3)
"""
if source == 'lidar':
# wavedata wants im_size in (w, h) order
im_size = [image_shape[1], image_shape[0]]
point_cloud = obj_utils.get_lidar_point_cloud(
img_idx,
self.dataset.calib_dir,
self.dataset.velo_dir,
im_size=im_size)
else:
raise ValueError("Invalid source {}".format(source))
return point_cloud
def get_nan_point_cloud(perspect_dir, idx):
calib_dir = perspect_dir + '/calib'
velo_dir = perspect_dir + '/velodyne'
all_points = obj_utils.get_lidar_point_cloud(idx, calib_dir, velo_dir)
# Remove nan points
nan_mask = ~np.any(np.isnan(all_points), axis=1)
point_cloud = all_points[nan_mask].T
return point_cloud
def load_samples(self, indices):
sample_dicts = []
for sample_idx in indices:
sample = self.sample_list[sample_idx]
sample_name = sample.name
if self.has_labels:
obj_labels = obj_utils.read_labels(self.label_dir,
int(sample_name))
label_classes, label_boxes_3d, label_boxes_2d = self.parse_obj_labels(
obj_labels, self.label_map)
else:
obj_labels = None
label_classes = np.zeros(1)
label_boxes_2d = np.zeros((1, 4))
label_boxes_3d = np.zeros((1, 7))
# image
cv_bgr_image = cv2.imread(self.get_rbg_image_path(
int(sample_name)))
rgb_image = cv_bgr_image[..., ::-1]
im_shape = rgb_image.shape[0:2]
image_input = rgb_image
# calibration
stereo_calib_p2 = calib_utils.read_calibration(
self.calib_dir, int(sample_name)).p2
# point cloud
# just project point to camera frame and then keep point in front of image
point_cloud = obj_utils.get_lidar_point_cloud(int(sample_name),
self.calib_dir,
self.velo_dir,
im_size=im_shape)
#################################
# Data Augmentation
#################################
if kitti_aug.AUG_FLIPPING in sample.augs:
pass
if kitti_aug.AUG_PCA_JITTER in sample.augs:
pass
sample_dict = {
constants.KEY_IMAGE_INPUT: image_input,
constants.KEY_POINT_CLOUD: point_cloud,
constants.KEY_LABEL_CLASSES: label_classes,
constants.KEY_LABEL_BOXES_2D: label_boxes_2d,
constants.KEY_LABEL_BOXES_3D: label_boxes_3d,
constants.KEY_STEREO_CALIB_P2: stereo_calib_p2
}
sample_dicts.append(sample_dict)
return sample_dicts
def estimate_ground_planes(base_dir,
dataset_config,
plane_method=0,
specific_idx=-1):
velo_dir = base_dir + 'velodyne'
plane_dir = base_dir + 'planes'
if plane_method == 1:
plane_dir = plane_dir + '_ransac'
ground_points_dir = base_dir + 'ground_points'
grid_points_dir = base_dir + 'ground_points_grid'
calib_dir = base_dir + 'calib'
files = os.listdir(velo_dir)
num_files = len(files)
file_idx = 0
#For checking ground planes
dataset = DatasetBuilder.build_kitti_dataset(dataset_config,
use_defaults=False)
kitti_utils = dataset.kitti_utils
if not os.path.exists(plane_dir):
os.makedirs(plane_dir)
#Estimate each idx
for file in files:
filepath = velo_dir + '/' + file
idx = int(os.path.splitext(file)[0])
if idx < cfg.MIN_IDX or idx > cfg.MAX_IDX:
continue
if specific_idx != -1:
idx = specific_idx
planes_file = plane_dir + '/%06d.txt' % idx
logging.debug("Index: %d", idx)
lidar_point_cloud = obj_utils.get_lidar_point_cloud(
idx, calib_dir, velo_dir)
# Reshape points into N x [x, y, z]
point_cloud = np.array(lidar_point_cloud).transpose().reshape(
(-1, 3)).T
ground_points_failed = False
if plane_method == use_ground_points:
s = loadGroundPointsFromFile(idx, ground_points_dir,
grid_points_dir)
if s.shape[0] < 4:
logging.debug("Not enough points at idx: %d", idx)
ground_points_failed = True
else:
m = estimate(s)
a, b, c, d = m
plane = loadKittiPlane(m)
#ground_points_failed = checkBadSlices(point_cloud, plane, kitti_utils)
ransac_failed = False
if plane_method == ransac or ground_points_failed:
logging.debug("PC shape: {}".format(point_cloud.shape))
points = point_cloud.T
all_points_near = points[(points[:, 0] > -3.0)
& (points[:, 0] < 3.0) &
(points[:, 1] > -3.0) &
(points[:, 1] < 0.0) &
(points[:, 2] < 20.0) &
(points[:, 2] > 2.0)]
n = all_points_near.shape[0]
logging.debug("Number of points near: %d", n)
if n < 3:
ransac_failed = True
else:
max_iterations = 100
goal_inliers = n * 0.5
m, b = run_ransac(all_points_near,
lambda x, y: is_inlier(x, y, 0.2), 3,
goal_inliers, max_iterations)
a, b, c, d = m
if plane_method == moose or ransac_failed:
plane_coeffs = estimate_plane_coeffs(point_cloud.T)
with open(planes_file, 'w+') as f:
f.write('# Plane\nWidth 4\nHeight 1\n')
if (plane_method == ransac and not ransac_failed) or \
(plane_method == use_ground_points and not ground_points_failed):
coeff_string = '%.6e %.6e %.6e %.6e' % (a, b, c, d)
else:
coeff_string = '%.6e %.6e %.6e %.6e' % (
plane_coeffs[0], plane_coeffs[1], plane_coeffs[2],
plane_coeffs[3])
f.write(coeff_string)
sys.stdout.write("\rGenerating plane {} / {}".format(
file_idx + 1, num_files))
sys.stdout.flush()
file_idx = file_idx + 1
if testing and idx == 2 or specific_idx != -1:
quit()
def is_plausible(obj, idx, detector_id, det_idx):
# Can only deny plausibility in the fov of the ego-vehicle sensors
# Filter objects to be in the sensor range, return True if out of range/fov
filtered_obj = p_utils.filter_labels([obj])
if len(filtered_obj) == 0:
return True
# Check if object is more than MAX_LIDAR_DIST away
# If yes then it is plausible since our sensors don't go that far
obj_pos = np.asanyarray(obj.t)
obj_dist = np.sqrt(np.dot(obj_pos, obj_pos.T))
if obj_dist >= cfg.MAX_LIDAR_DIST:
return True
# If it is truncated by more than half, return True
if np.abs(obj.t[2]) < np.abs(obj.t[0]):
return True
# If the center of the object is above our center, return True (only 2 degrees up)
if obj.t[1] - (obj.h / 2) < 0:
return True
# TODO - Do we want to check this or is it better to only check frustum?
# This may allow untrustworthy entities to put ground points in box
# Check if points in 3D box first, if yes it is plausible
# points_dict = points_3d.load_points_in_3d_boxes(idx, const.ego_id())
# points_in_box = points_dict[detector_id, det_idx]
# if points_in_box > 0:
# return True
# Load point cloud
velo_dir = cfg.DATASET_DIR + '/velodyne'
calib_dir = cfg.DATASET_DIR + '/calib'
pc = obj_utils.get_lidar_point_cloud(idx, calib_dir, velo_dir)
# Vis regular PC
# vis_utils.vis_pc(pc, [obj])
# Obtain unit vector to object
unit_vec_forward = obj_pos / np.linalg.norm(obj_pos)
# The size of the frustum square at the object's distance
# We want to ensure we are within the object's bounding box
half_size = min(obj.l / 4.0, obj.w / 4.0)
# camera position is origin
cam_pos = np.array([0, 0, 0])
# Calculate the up and right unit vectors for the frustum
y_up = (obj_pos[0]**2 + obj_pos[2]**2) / obj_pos[1]
vec_down = np.array([obj_pos[0], y_up, obj_pos[2]])
unit_vec_down = vec_down / np.linalg.norm(vec_down)
unit_vec_right = np.cross(unit_vec_down, unit_vec_forward)
# KITTI labels are at bottom center of 3D bounding box
obj_center = obj_pos - np.array([0, obj.h / 2, 0])
# Obtain 2D box for computing frustum boundary planes
top_left = -(half_size * unit_vec_down) - (half_size *
unit_vec_right) + obj_center
top_right = -(half_size * unit_vec_down) + (half_size *
unit_vec_right) + obj_center
bot_left = (half_size * unit_vec_down) - (half_size *
unit_vec_right) + obj_center
bot_right = (half_size * unit_vec_down) + (half_size *
unit_vec_right) + obj_center
# Compute the 4 boundary planes
plane_top = get_plane_params(cam_pos, top_left, top_right)
plane_bot = get_plane_params(cam_pos, bot_left, bot_right)
plane_right = get_plane_params(cam_pos, bot_right, top_right)
plane_left = get_plane_params(cam_pos, bot_left, top_left)
# Extend pc with ones for dot product with planes
shape = pc.shape
pc = pc.T
new_pc = np.ones((pc.shape[0], pc.shape[1] + 1))
new_pc[:, :-1] = pc
pc = new_pc
# Filter the points for each plane
pc = pc[np.dot(pc, plane_top) < 0]
pc = pc[np.dot(pc, plane_bot) > 0]
pc = pc[np.dot(pc, plane_right) > 0]
pc = pc[np.dot(pc, plane_left) < 0]
# If there are no points left then the object is not plausible
# Otherwise LiDAR should've hit something on or before the object
if pc.shape[0] == 0:
return False
# Remove extended column
pc = pc[:, 0:3]
# To visualize the resulting points
# vis_utils.vis_pc(pc.T, [obj])
# Save the current number of points before distance based culling
num_points = pc.shape[0]
# Compute if remaining points are closer or further than the object centre
point_distances = np.sqrt(np.sum(np.multiply(pc, pc), axis=1))
# Added in a safety factor of 0.25 (sometimes pedestrians are positioned poorly)
pc_closer = pc[point_distances < (obj_dist + 0.25)]
num_closer_points = pc_closer.shape[0]
# To visualize the resulting points with frustum lines
# print(pc_closer.shape)
# if pc_closer.shape[0] > 0:
# vis_utils.vis_pc(pc_closer.T, [obj], frustum_points=[top_left, top_right, bot_right, bot_left])
# Return True if > 10% of points are closer
if num_closer_points / float(num_points) > 0.1:
return True
return False
def pad_sample(self, sample):
sample = super().pad_sample(sample)
image_path = sample[constants.KEY_IMAGE_PATH]
sample_name = self.get_sample_name_from_path(image_path)
pc = obj_utils.get_lidar_point_cloud(int(sample_name),
self.calib_dir,
self.velo_dir,
im_size=None,
min_intensity=None)
p2 = sample[constants.KEY_STEREO_CALIB_P2]
h, w = sample[constants.KEY_IMAGE_INFO].astype(np.int)[:2]
label_boxes_3d = sample[constants.KEY_LABEL_BOXES_3D]
num_instances = sample[constants.KEY_NUM_INSTANCES]
depth = np.zeros((h, w), dtype=np.float32)
# 0 refers to bg
mask = np.zeros((h, w), dtype=np.int64)
for i in range(num_instances):
label_box_3d = label_boxes_3d[i]
instance_pc = pointcloud_utils.box_3d_filter(label_box_3d, pc[:3])
# draw depth
instance_depth = pointcloud_utils.cam_pts_to_rect_depth(
instance_pc[:3, :], K=p2[:3, :3], h=h, w=w)
mask[instance_depth > 0] = i + 1
depth = depth + instance_depth
# import matplotlib.pyplot as plt
# plt.imshow(mask)
# plt.show()
sample[constants.KEY_LABEL_DEPTHMAP] = depth[None]
sample[constants.KEY_LABEL_INSTANCES_MASK] = mask[None]
# boxes_3d = sample[constants.KEY_LABEL_BOXES_3D]
# boxes_2d_proj = sample[constants.KEY_LABEL_BOXES_2D]
# p2 = sample[constants.KEY_STEREO_CALIB_P2]
# cls_orients = []
# reg_orients = []
# dims = np.stack(
# [boxes_3d[:, 4], boxes_3d[:, 5], boxes_3d[:, 3]], axis=-1)
# for i in range(boxes_3d.shape[0]):
# target = {}
# target['ry'] = boxes_3d[i, -1]
# target['dimension'] = dims[i]
# target['location'] = boxes_3d[i, :3]
# corners_xy, points_3d = compute_box_3d(target, p2, True)
# # some labels for estimating orientation
# left_side_points_2d = corners_xy[[0, 3]]
# right_side_points_2d = corners_xy[[1, 2]]
# left_side_points_3d = points_3d.T[[0, 3]]
# right_side_points_3d = points_3d.T[[1, 2]]
# # which one is visible
# mid_left_points_3d = left_side_points_3d.mean(axis=0)
# mid_right_points_3d = right_side_points_3d.mean(axis=0)
# # K*T
# KT = p2[:, -1]
# K = p2[:3, :3]
# T = np.dot(np.linalg.inv(K), KT)
# C = -T
# mid_left_dist = np.linalg.norm((C - mid_left_points_3d))
# mid_right_dist = np.linalg.norm((C - mid_right_points_3d))
# if mid_left_dist > mid_right_dist:
# visible_side = right_side_points_2d
# else:
# visible_side = left_side_points_2d
# cls_orient, reg_orient = truncate_box(boxes_2d_proj[i],
# visible_side)
# cls_orient = modify_cls_orient(cls_orient, left_side_points_2d,
# right_side_points_2d)
# cls_orients.append(cls_orient)
# reg_orients.append(reg_orient)
# sample['cls_orient'] = np.stack(cls_orients, axis=0).astype(np.int32)
# sample['reg_orient'] = np.stack(reg_orients, axis=0).astype(np.float32)
sample[constants.KEY_LABEL_CLASSES] = torch.from_numpy(
sample[constants.KEY_LABEL_CLASSES]).long()
return sample
def main():
"""
Visualization of anchor filtering using 3D integral images
"""
anchor_colour_scheme = {
"Car": (0, 255, 0), # Green
"Pedestrian": (255, 150, 50), # Orange
"Cyclist": (150, 50, 100), # Purple
"DontCare": (255, 0, 0), # Red
"Anchor": (0, 0, 255), # Blue
}
# Create Dataset
dataset = DatasetBuilder.build_kitti_dataset(DatasetBuilder.KITTI_TRAINVAL)
# Options
clusters, _ = dataset.get_cluster_info()
sample_name = "000000"
img_idx = int(sample_name)
anchor_stride = [0.5, 0.5]
ground_plane = obj_utils.get_road_plane(img_idx, dataset.planes_dir)
anchor_3d_generator = grid_anchor_3d_generator.GridAnchor3dGenerator(
anchor_3d_sizes=clusters, anchor_stride=anchor_stride)
area_extents = np.array([[-40, 40], [-5, 3], [0, 70]])
# Generate anchors in box_3d format
start_time = time.time()
anchor_boxes_3d = anchor_3d_generator.generate(area_3d=area_extents,
ground_plane=ground_plane)
end_time = time.time()
print("Anchors generated in {} s".format(end_time - start_time))
point_cloud = obj_utils.get_lidar_point_cloud(img_idx, dataset.calib_dir,
dataset.velo_dir)
offset_dist = 2.0
# Filter points within certain xyz range and offset from ground plane
offset_filter = obj_utils.get_point_filter(point_cloud, area_extents,
ground_plane, offset_dist)
# Filter points within 0.2m of the road plane
road_filter = obj_utils.get_point_filter(point_cloud, area_extents,
ground_plane, 0.1)
slice_filter = np.logical_xor(offset_filter, road_filter)
point_cloud = point_cloud.T[slice_filter]
# Generate Voxel Grid
vx_grid_3d = voxel_grid.VoxelGrid()
vx_grid_3d.voxelize(point_cloud, 0.1, area_extents)
# Anchors in anchor format
all_anchors = box_3d_encoder.box_3d_to_anchor(anchor_boxes_3d)
# Filter the boxes here!
start_time = time.time()
empty_filter = \
anchor_filter.get_empty_anchor_filter(anchors=all_anchors,
voxel_grid_3d=vx_grid_3d,
density_threshold=1)
anchor_boxes_3d = anchor_boxes_3d[empty_filter]
end_time = time.time()
print("Anchors filtered in {} s".format(end_time - start_time))
# Visualize GT boxes
# Grab ground truth
ground_truth_list = obj_utils.read_labels(dataset.label_dir, img_idx)
# ----------
# Test Sample extraction
# Visualize from here
vis_utils.visualization(dataset.rgb_image_dir, img_idx)
plt.show(block=False)
image_path = dataset.get_rgb_image_path(sample_name)
image_shape = np.array(Image.open(image_path)).shape
rgb_boxes, rgb_normalized_boxes = \
anchor_projector.project_to_image_space(all_anchors, dataset,
image_shape, img_idx)
# Overlay boxes on images
anchor_objects = []
for anchor_idx in range(len(anchor_boxes_3d)):
anchor_box_3d = anchor_boxes_3d[anchor_idx]
obj_label = box_3d_encoder.box_3d_to_object_label(
anchor_box_3d, 'Anchor')
# Append to a list for visualization in VTK later
anchor_objects.append(obj_label)
for idx in range(len(ground_truth_list)):
ground_truth_obj = ground_truth_list[idx]
# Append to a list for visualization in VTK later
anchor_objects.append(ground_truth_obj)
# Create VtkAxes
axes = vtk.vtkAxesActor()
axes.SetTotalLength(5, 5, 5)
# Create VtkBoxes for boxes
vtk_boxes = VtkBoxes()
vtk_boxes.set_objects(anchor_objects, anchor_colour_scheme)
vtk_point_cloud = VtkPointCloud()
vtk_point_cloud.set_points(point_cloud)
vtk_voxel_grid = VtkVoxelGrid()
vtk_voxel_grid.set_voxels(vx_grid_3d)
# Create Voxel Grid Renderer in bottom half
vtk_renderer = vtk.vtkRenderer()
vtk_renderer.AddActor(vtk_boxes.vtk_actor)
# vtk_renderer.AddActor(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_voxel_grid.vtk_actor)
vtk_renderer.AddActor(axes)
vtk_renderer.SetBackground(0.2, 0.3, 0.4)
# Setup Camera
current_cam = vtk_renderer.GetActiveCamera()
current_cam.Pitch(170.0)
current_cam.Roll(180.0)
# Zooms out to fit all points on screen
vtk_renderer.ResetCamera()
# Zoom in slightly
current_cam.Zoom(2.5)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Setup Render Window
vtk_render_window = vtk.vtkRenderWindow()
vtk_render_window.SetWindowName("Anchors")
vtk_render_window.SetSize(900, 500)
vtk_render_window.AddRenderer(vtk_renderer)
# Setup custom interactor style, which handles mouse and key events
vtk_render_window_interactor = vtk.vtkRenderWindowInteractor()
vtk_render_window_interactor.SetRenderWindow(vtk_render_window)
vtk_render_window_interactor.SetInteractorStyle(
vtk.vtkInteractorStyleTrackballCamera())
# Render in VTK
vtk_render_window.Render()
vtk_render_window_interactor.Start() # Blocking
def main():
# Setting Paths
cam = 2
# dataset_dir = '/media/bradenhurl/hd/gta/object/'
data_set = 'training'
dataset_dir = os.path.expanduser('~') + '/wavedata-dev/demos/gta'
#dataset_dir = os.path.expanduser('~') + '/Kitti/object/'
dataset_dir = os.path.expanduser(
'~') + '/GTAData/TruPercept/object_tru_percept8/'
#Set to true to see predictions (results) from all perspectives
use_results = True
altPerspective = False
perspID = 48133
perspStr = '%07d' % perspID
altPerspect_dir = os.path.join(dataset_dir, data_set + '/alt_perspective/')
if altPerspective:
data_set = data_set + '/alt_perspective/' + perspStr
fromWiseWindows = False
useEVE = False
if fromWiseWindows:
data_set = 'object'
if useEVE:
dataset_dir = '/media/bradenhurl/hd/data/eve/'
else:
dataset_dir = '/media/bradenhurl/hd/data/'
image_dir = os.path.join(dataset_dir, data_set) + '/image_2'
velo_dir = os.path.join(dataset_dir, data_set) + '/velodyne'
calib_dir = os.path.join(dataset_dir, data_set) + '/calib'
if use_results:
label_dir = os.path.join(dataset_dir, data_set) + '/predictions'
else:
label_dir = os.path.join(dataset_dir, data_set) + '/label_2'
base_dir = os.path.join(dataset_dir, data_set)
comparePCs = False
if comparePCs:
velo_dir2 = os.path.join(dataset_dir, data_set) + '/velodyne'
tracking = False
if tracking:
seq_idx = 1
data_set = '%04d' % seq_idx
dataset_dir = '/media/bradenhurl/hd/GTAData/August-01/tracking'
image_dir = os.path.join(dataset_dir, 'images', data_set)
label_dir = os.path.join(dataset_dir, 'labels', data_set)
velo_dir = os.path.join(dataset_dir, 'velodyne', data_set)
calib_dir = os.path.join(dataset_dir, 'training', 'calib', '0000')
#Used for visualizing inferences
#label_dir = '/media/bradenhurl/hd/avod/avod/data/outputs/pyramid_people_gta_40k'
#label_dir = label_dir + '/predictions/kitti_predictions_3d/test/0.02/154000/data/'
closeView = False
pitch = 170
pointSize = 3
zoom = 1
if closeView:
pitch = 180.5
pointSize = 3
zoom = 35
image_list = os.listdir(image_dir)
fulcrum_of_points = True
use_intensity = False
img_idx = 2
print('=== Loading image: {:06d}.png ==='.format(img_idx))
print(image_dir)
image = cv2.imread(image_dir + '/{:06d}.png'.format(img_idx))
image_shape = (image.shape[1], image.shape[0])
if use_intensity:
point_cloud, intensity = obj_utils.get_lidar_point_cloud(
img_idx, calib_dir, velo_dir, ret_i=use_intensity)
else:
point_cloud = obj_utils.get_lidar_point_cloud(img_idx,
calib_dir,
velo_dir,
im_size=image_shape)
if comparePCs:
point_cloud2 = obj_utils.get_lidar_point_cloud(img_idx,
calib_dir,
velo_dir2,
im_size=image_shape)
point_cloud = np.hstack((point_cloud, point_cloud2))
# Reshape points into N x [x, y, z]
all_points = np.array(point_cloud).transpose().reshape((-1, 3))
# Define Fixed Sizes for the voxel grid
x_min = -85
x_max = 85
y_min = -5
y_max = 5
z_min = 3
z_max = 85
x_min = min(point_cloud[0])
x_max = max(point_cloud[0])
y_min = min(point_cloud[1])
y_max = max(point_cloud[1])
#z_min = min(point_cloud[2])
z_max = max(point_cloud[2])
# Filter points within certain xyz range
area_filter = (point_cloud[0] > x_min) & (point_cloud[0] < x_max) & \
(point_cloud[1] > y_min) & (point_cloud[1] < y_max) & \
(point_cloud[2] > z_min) & (point_cloud[2] < z_max)
all_points = all_points[area_filter]
#point_colours = np.zeros(point_cloud.shape[1],0)
#print(point_colours.shape)
if fulcrum_of_points:
# Get point colours
point_colours = vis_utils.project_img_to_point_cloud(
all_points, image, calib_dir, img_idx)
print("Point colours shape: ", point_colours.shape)
print("Sample 0 of colour: ", point_colours[0])
elif use_intensity:
adjusted = intensity == 65535
intensity = intensity > 0
intensity = np.expand_dims(intensity, -1)
point_colours = np.hstack(
(intensity * 255, intensity * 255 - adjusted * 255,
intensity * 255 - adjusted * 255))
print("Intensity shape:", point_colours.shape)
print("Intensity sample: ", point_colours[0])
# Create Voxel Grid
voxel_grid = VoxelGrid()
voxel_grid_extents = [[x_min, x_max], [y_min, y_max], [z_min, z_max]]
print(voxel_grid_extents)
start_time = time.time()
voxel_grid.voxelize(all_points, 0.2, voxel_grid_extents)
end_time = time.time()
print("Voxelized in {} s".format(end_time - start_time))
# Get bounding boxes
gt_detections = obj_utils.read_labels(label_dir,
img_idx,
results=use_results)
if gt_detections is None:
gt_detections = []
#perspective_utils.to_world(gt_detections, base_dir, img_idx)
#perspective_utils.to_perspective(gt_detections, base_dir, img_idx)
for entity_str in os.listdir(altPerspect_dir):
if os.path.isdir(os.path.join(altPerspect_dir, entity_str)):
perspect_detections = perspective_utils.get_detections(
base_dir,
altPerspect_dir,
img_idx,
perspID,
entity_str,
results=use_results)
if perspect_detections != None:
if use_results:
stripped_detections = trust_utils.strip_objs(
perspect_detections)
gt_detections = gt_detections + stripped_detections
else:
gt_detections = gt_detections + perspect_detections
# Create VtkPointCloud for visualization
vtk_point_cloud = VtkPointCloud()
if fulcrum_of_points or use_intensity:
vtk_point_cloud.set_points(all_points, point_colours)
else:
vtk_point_cloud.set_points(all_points)
vtk_point_cloud.vtk_actor.GetProperty().SetPointSize(pointSize)
# Create VtkVoxelGrid for visualization
vtk_voxel_grid = VtkVoxelGrid()
vtk_voxel_grid.set_voxels(voxel_grid)
COLOUR_SCHEME_PAPER = {
"Car": (0, 0, 255), # Blue
"Pedestrian": (255, 0, 0), # Red
"Bus": (0, 0, 255), #Blue
"Cyclist": (150, 50, 100), # Purple
"Van": (255, 150, 150), # Peach
"Person_sitting": (150, 200, 255), # Sky Blue
"Truck": (0, 0, 255), # Light Grey
"Tram": (150, 150, 150), # Grey
"Misc": (100, 100, 100), # Dark Grey
"DontCare": (255, 255, 255), # White
}
# Create VtkBoxes for boxes
vtk_boxes = VtkBoxes()
vtk_boxes.set_objects(gt_detections,
COLOUR_SCHEME_PAPER) #vtk_boxes.COLOUR_SCHEME_KITTI)
# Create Axes
axes = vtk.vtkAxesActor()
axes.SetTotalLength(5, 5, 5)
# Create Voxel Grid Renderer in bottom half
vtk_renderer = vtk.vtkRenderer()
vtk_renderer.AddActor(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_voxel_grid.vtk_actor)
vtk_renderer.AddActor(vtk_boxes.vtk_actor)
#vtk_renderer.AddActor(axes)
vtk_renderer.SetBackground(0.2, 0.3, 0.4)
# Setup Camera
current_cam = vtk_renderer.GetActiveCamera()
current_cam.Pitch(pitch)
current_cam.Roll(180.0)
# Zooms out to fit all points on screen
vtk_renderer.ResetCamera()
# Zoom in slightly
current_cam.Zoom(zoom)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Setup Render Window
vtk_render_window = vtk.vtkRenderWindow()
vtk_render_window.SetWindowName(
"Point Cloud and Voxel Grid, Image {}".format(img_idx))
vtk_render_window.SetSize(1920, 1080)
vtk_render_window.AddRenderer(vtk_renderer)
# Setup custom interactor style, which handles mouse and key events
vtk_render_window_interactor = vtk.vtkRenderWindowInteractor()
vtk_render_window_interactor.SetRenderWindow(vtk_render_window)
# Add custom interactor to toggle actor visibilities
vtk_render_window_interactor.SetInteractorStyle(
vis_utils.ToggleActorsInteractorStyle([
vtk_point_cloud.vtk_actor,
vtk_voxel_grid.vtk_actor,
vtk_boxes.vtk_actor,
]))
# Show image
image = cv2.imread(image_dir + "/%06d.png" % img_idx)
cv2.imshow("Press any key to continue", image)
cv2.waitKey()
# Render in VTK
vtk_render_window.Render()
vtk_render_window_interactor.Start() # Blocking
def visualize_objects_in_pointcloud(objects, COLOUR_SCHEME, dataset_dir,
img_idx, fulcrum_of_points, use_intensity,
receive_from_perspective, compare_pcs=False,
show_3d_point_count=False, show_orientation=cfg.VISUALIZE_ORIENTATION,
final_results=False, show_score=False,
compare_with_gt=False, show_image=True,
_text_positions=None, _text_labels=None):
image_dir = os.path.join(dataset_dir, 'image_2')
velo_dir = os.path.join(dataset_dir, 'velodyne')
calib_dir = os.path.join(dataset_dir, 'calib')
if compare_pcs:
fulcrum_of_points = False
print('=== Loading image: {:06d}.png ==='.format(img_idx))
print(image_dir)
image = cv2.imread(image_dir + '/{:06d}.png'.format(img_idx))
image_shape = (image.shape[1], image.shape[0])
if use_intensity:
point_cloud,intensity = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir,
ret_i=use_intensity)
else:
point_cloud = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir,
im_size=image_shape)
if compare_pcs:
receive_persp_dir = os.path.join(altPerspect_dir, '{:07d}'.format(receive_from_perspective))
velo_dir2 = os.path.join(receive_persp_dir, 'velodyne')
print(velo_dir2)
if not os.path.isdir(velo_dir2):
print("Error: cannot find velo_dir2: ", velo_dir2)
exit()
point_cloud2 = obj_utils.get_lidar_point_cloud(img_idx, calib_dir, velo_dir2,
im_size=image_shape)
#Set to true to display point clouds in world coordinates (for debugging)
display_in_world=False
if display_in_world:
point_cloud = perspective_utils.pc_to_world(point_cloud.T, receive_persp_dir, img_idx)
point_cloud2 = perspective_utils.pc_to_world(point_cloud2.T, dataset_dir, img_idx)
point_cloud = np.hstack((point_cloud.T, point_cloud2.T))
else:
point_cloud2 = perspective_utils.pc_persp_transform(point_cloud2.T, receive_persp_dir, dataset_dir, img_idx)
point_cloud = np.hstack((point_cloud, point_cloud2.T))
# Reshape points into N x [x, y, z]
all_points = np.array(point_cloud).transpose().reshape((-1, 3))
# Define Fixed Sizes for the voxel grid
x_min = -85
x_max = 85
y_min = -5
y_max = 5
z_min = 3
z_max = 85
# Comment these out to filter points by area
x_min = min(point_cloud[0])
x_max = max(point_cloud[0])
y_min = min(point_cloud[1])
y_max = max(point_cloud[1])
z_min = min(point_cloud[2])
z_max = max(point_cloud[2])
# Filter points within certain xyz range
area_filter = (point_cloud[0] > x_min) & (point_cloud[0] < x_max) & \
(point_cloud[1] > y_min) & (point_cloud[1] < y_max) & \
(point_cloud[2] > z_min) & (point_cloud[2] < z_max)
all_points = all_points[area_filter]
point_colours = None
if fulcrum_of_points:
# Get point colours
point_colours = vis_utils.project_img_to_point_cloud(all_points, image,
calib_dir, img_idx)
elif use_intensity:
adjusted = intensity == 65535
intensity = intensity > 0
intensity = np.expand_dims(intensity,-1)
point_colours = np.hstack((intensity*255,intensity*255-adjusted*255,intensity*255-adjusted*255))
# Create Voxel Grid
voxel_grid = VoxelGrid()
voxel_grid_extents = [[x_min, x_max], [y_min, y_max], [z_min, z_max]]
print(voxel_grid_extents)
start_time = time.time()
voxel_grid.voxelize(all_points, 0.2, voxel_grid_extents)
end_time = time.time()
print("Voxelized in {} s".format(end_time - start_time))
# Some settings for the initial camera view and point size
closeView = False
pitch = 170
pointSize = 2
zoom = 1
if closeView:
pitch = 180.5
pointSize = 3
zoom = 35
# Create VtkPointCloud for visualization
vtk_point_cloud = VtkPointCloud()
if point_colours is not None:
vtk_point_cloud.set_points(all_points, point_colours)
else:
vtk_point_cloud.set_points(all_points)
vtk_point_cloud.vtk_actor.GetProperty().SetPointSize(pointSize)
# Create VtkVoxelGrid for visualization
vtk_voxel_grid = VtkVoxelGrid()
vtk_voxel_grid.set_voxels(voxel_grid)
# Create VtkBoxes for boxes
vtk_boxes = VtkBoxes()
vtk_boxes.set_objects(objects, COLOUR_SCHEME, show_orientation)#vtk_boxes.COLOUR_SCHEME_KITTI)
# Create Axes
axes = vtk.vtkAxesActor()
axes.SetTotalLength(5, 5, 5)
# Create Voxel Grid Renderer in bottom half
vtk_renderer = vtk.vtkRenderer()
vtk_renderer.AddActor(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_voxel_grid.vtk_actor)
vtk_renderer.AddActor(vtk_boxes.vtk_actor)
vtk_renderer.AddActor(axes)
if _text_positions is not None:
vtk_text_labels = VtkTextLabels()
vtk_text_labels.set_text_labels(_text_positions, _text_labels)
vtk_renderer.AddActor(vtk_text_labels.vtk_actor)
vtk_renderer.SetBackground(0.2, 0.3, 0.4)
# Setup Camera
current_cam = vtk_renderer.GetActiveCamera()
current_cam.Pitch(pitch)
current_cam.Roll(180.0)
# Zooms out to fit all points on screen
vtk_renderer.ResetCamera()
# Zoom in slightly
current_cam.Zoom(zoom)
# Zoom/navigate to the desired camera view then exit
# Three lines will be output. Paste these here
# Above forward view
current_cam.SetPosition(7.512679241328601, -312.20497623371926, -130.38469206536766)
current_cam.SetViewUp(-0.01952407393317445, -0.44874501090739727, 0.893446543293314)
current_cam.SetFocalPoint(11.624950999358777, 14.835920755080867, 33.965665867613836)
# Top down view of synchronization
current_cam.SetPosition(28.384757950371405, -125.46190537888288, 63.60263366961189)
current_cam.SetViewUp(-0.02456679343399302, 0.0030507437719906913, 0.9996935358512673)
current_cam.SetFocalPoint(27.042134804730317, 15.654378427929846, 63.13899801247614)
current_cam.SetPosition(30.3869590224831, -50.28910856489952, 60.097631136698965)
current_cam.SetViewUp(-0.0237472244952177, -0.06015048799392083, 0.997906803325274)
current_cam.SetFocalPoint(27.06695416156647, 15.347824332314035, 63.97499987548391)
# current_cam.SetPosition(14.391008769593322, -120.06549828061613, -1.567028749253062)
# current_cam.SetViewUp(-0.02238762832327178, -0.1049057307562059, 0.9942301452644481)
# current_cam.SetFocalPoint(10.601112314728102, 20.237110061924664, 13.151596441968126)
# # Top down view of whole detection area
# current_cam.SetPosition(11.168659642537031, -151.97163016078756, 17.590894639193227)
# current_cam.SetViewUp(-0.02238762832327178, -0.1049057307562059, 0.9942301452644481)
# current_cam.SetFocalPoint(6.5828849321501055, 17.79452593368671, 35.400431120570865)
# Top down view of scenario
current_cam.SetPosition(2.075612197299923, -76.19063612245675, 5.948366424752178)
current_cam.SetViewUp(-0.02238762832327178, -0.1049057307562059, 0.9942301452644481)
current_cam.SetFocalPoint(-0.5129380758134061, 19.637933198314016, 16.00138547483155)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Setup Render Window
vtk_render_window = vtk.vtkRenderWindow()
vtk_render_window.SetWindowName(
"Point Cloud and Voxel Grid, Image {}".format(img_idx))
vtk_render_window.SetSize(1920, 1080)
vtk_render_window.AddRenderer(vtk_renderer)
# Setup custom interactor style, which handles mouse and key events
vtk_render_window_interactor = vtk.vtkRenderWindowInteractor()
vtk_render_window_interactor.SetRenderWindow(vtk_render_window)
# Add custom interactor to toggle actor visibilities
vtk_render_window_interactor.SetInteractorStyle(
vis_utils.ToggleActorsInteractorStyle([
vtk_point_cloud.vtk_actor,
vtk_voxel_grid.vtk_actor,
vtk_boxes.vtk_actor,
]))
# Show image
if show_image:
image = cv2.imread(image_dir + "/%06d.png" % img_idx)
cv2.imshow("Press any key to continue", image)
cv2.waitKey()
# Render in VTK
vtk_render_window.Render()
vtk_render_window_interactor.Start() # Blocking
# vtk_render_window_interactor.Initialize() # Non-Blocking
# Obtain camera positional information for repeatable views
print("current_cam.SetPosition{}".format(current_cam.GetPosition()))
print("current_cam.SetViewUp{}".format(current_cam.GetViewUp()))
print("current_cam.SetFocalPoint{}".format(current_cam.GetFocalPoint()))
