def main():
####################
# Options
####################
odom_dir = os.path.expanduser('~/Kitti/odometry/dataset')
sequence = '03'
# max_fps = 10.0
# vtk_window_size = (1280, 720)
vtk_window_size = (960, 540)
save_images = False
point_cloud_source = 'lidar'
# point_cloud_source = 'fast'
# point_cloud_source = 'multiscale'
# first_frame_idx = None
# first_frame_pose = None
# Setup odometry dataset handler
odom_dataset = pykitti.odometry(odom_dir, sequence)
# # Check that velo length matches timestamps?
# if len(odom_dataset.velo_files) != len(odom_dataset.timestamps):
# raise ValueError('velo files and timestamps have different length!')
frame_range = (0, len(odom_dataset.timestamps))
# frame_range = (0, 100)
# frame_range = (1, 10)
# frame_range = (20, 30)
# frame_range = (440, 442)
# frame_range = (441, 442)
# frame_range = (440, 452)
# frame_range = (440, 512)
# frame_range = (500, 502)
# frame_range = (500, 512)
# for frame_idx in range(len(raw_data.timestamps)):
# for frame_idx in range(457, 459):
camera_viewpoint = 'front'
camera_viewpoint = 'elevated'
# camera_zoom = 2.2
# camera_viewpoint = (0.0, -5.0, -30.0)
# camera_fp = (0.0, 1.0, 30.0)
# viewpoint = 'front'
# camera_zoom = 0.6
# camera_pos = (0.0, 0.0, 0.0)
# camera_fp = (0.0, 0.0, 2000.0)
# vtk_window_size = (1000, 500)
# viewpoint = 'bev'
# camera_zoom = 1.0
# # camera_pos = (0.0, -15.0, -25.0)
# # camera_pos = (0.0, 0.0, 0.0)
# camera_fp = (0.0, 1.0, 30.0)
####################
# End of Options
####################
# Setup output folder
# drive_name = category + '_' + date + '_' + drive
# images_out_dir = 'outputs/point_clouds/' + drive_name + '/' + point_cloud_source + '_' + viewpoint
# os.makedirs(images_out_dir, exist_ok=True)
# max_loop_time = 1.0 / max_fps
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'Overlaid Point Cloud', vtk_window_size, vtk_renderer)
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(
vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer))
vtk_interactor.Initialize()
cam_p2 = odom_dataset.calib.P_rect_20
# Load poses
cam0_poses = odom_dataset.poses
# Setup camera
if camera_viewpoint == 'front':
cam0_curr_vtk_cam_pos = [0.0, 0.0, 0.0, 1.0]
cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0]
elif camera_viewpoint == 'elevated':
cam0_curr_vtk_cam_pos = [0.0, -5.0, -15.0, 1.0]
cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0]
else:
raise ValueError('Invalid camera_pos', camera_viewpoint)
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(2, 2, 2)
vtk_renderer.AddActor(vtk_axes)
for frame_idx in range(*frame_range):
# Point cloud actor wrapper
vtk_pc = VtkPointCloud()
vtk_pc.vtk_actor.GetProperty().SetPointSize(2)
# all_vtk_actors.append(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_pc.vtk_actor)
print('{} / {}'.format(frame_idx, len(odom_dataset.timestamps) - 1))
# Load next frame data
load_start_time = time.time()
rgb_image = np.asarray(odom_dataset.get_cam2(frame_idx))
bgr_image = rgb_image[..., ::-1]
if point_cloud_source == 'lidar':
velo_points = get_velo_points(odom_dataset, frame_idx)
# Transform point cloud to cam_0_curr frame
velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]],
constant_values=1.0,
mode='constant')
cam0_curr_pc_all_padded = odom_dataset.calib.T_cam0_velo @ velo_curr_points_padded.T
elif point_cloud_source == 'multiscale':
# depth_map_path = depth_map_dir + '/{:010d}.png'.format(frame_idx)
# depth_map = depth_map_utils.read_depth_map(depth_map_path)
# points_cam2 = depth_map_utils.get_depth_point_cloud(depth_map, cam_p).T
raise NotImplementedError()
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# Project velodyne points
projection_start_time = time.time()
if point_cloud_source == 'lidar':
# Project into image2
points_in_img2 = calib_utils.project_pc_to_image(
cam0_curr_pc_all_padded[0:3], cam_p2)
points_in_img2_int = np.round(points_in_img2).astype(np.int32)
image_filter = obj_utils.points_in_img_filter(
points_in_img2_int, bgr_image.shape)
cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter]
points_in_img_int_valid = points_in_img2_int[:, image_filter]
point_colours = bgr_image[points_in_img_int_valid[1],
points_in_img_int_valid[0]]
else:
raise ValueError('Invalid point_cloud_source', point_cloud_source)
print('projection\t', time.time() - projection_start_time)
# Get pose
cam0_ref_pose = cam0_poses[frame_idx]
tf_cam0_ref_cam0_curr = cam0_ref_pose
# print('cam0_ref_pose\n', np.round(cam0_ref_pose, 3))
cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded
# VtkPointCloud
vtk_pc_start_time = time.time()
vtk_pc.set_points(cam0_ref_pc_padded[0:3].T, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
# Display pose
vtk_pc_pose = VtkPointCloud()
vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5)
vtk_pc_pose.set_points(np.reshape(cam0_ref_pose[0:3, 3], [-1, 3]))
vtk_renderer.AddActor(vtk_pc_pose.vtk_actor)
cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr.dot(cam0_curr_vtk_cam_pos)
cam0_ref_vtk_focal_point = tf_cam0_ref_cam0_curr.dot(
cam0_curr_vtk_focal_point)
current_cam = vtk_renderer.GetActiveCamera()
vtk_renderer.ResetCamera()
current_cam.SetViewUp(0, -1, 0)
current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3])
current_cam.SetFocalPoint(*cam0_ref_vtk_focal_point[0:3])
current_cam.Zoom(0.5)
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_render_window.Render()
print('render\t\t', time.time() - render_start_time)
print('---')
print('Done')
# Keep window open
vtk_interactor.Start()
def main():
####################
# Options
####################
# drive_id = '2011_09_26_drive_0001_sync' # No moving cars, depth completion sample 0 area
# drive_id = '2011_09_26_drive_0002_sync' # depth completion sample 0
# drive_id = '2011_09_26_drive_0005_sync' # (gps bad)
# drive_id = '2011_09_26_drive_0005_sync' # (moving) (gps bad)
# drive_id = '2011_09_26_drive_0009_sync' # missing velo?
# drive_id = '2011_09_26_drive_0011_sync' # both moving, then traffic light)
# drive_id = '2011_09_26_drive_0013_sync' # both moving
# drive_id = '2011_09_26_drive_0014_sync' # both moving, sample 104 (314)
# drive_id = '2011_09_26_drive_0015_sync' # both moving
# drive_id = '2011_09_26_drive_0017_sync' # other only, traffic light, moving across
# drive_id = '2011_09_26_drive_0018_sync' # other only, traffic light, forward
# drive_id = '2011_09_26_drive_0019_sync' # both moving, some people at end, wonky gps
# drive_id = '2011_09_26_drive_0020_sync' # gps drift
# drive_id = '2011_09_26_drive_0022_sync' # ego mostly, then both, long
# drive_id = '2011_09_26_drive_0023_sync' # sample 169 (474)
# drive_id = '2011_09_26_drive_0027_sync' # both moving, fast, straight
# drive_id = '2011_09_26_drive_0028_sync' # both moving, opposite
# drive_id = '2011_09_26_drive_0029_sync' # both moving, good gps (430)
# drive_id = '2011_09_26_drive_0032_sync' # both moving, following some cars
# drive_id = '2011_09_26_drive_0035_sync' #
# drive_id = '2011_09_26_drive_0036_sync' # (long) behind red truck
# drive_id = '2011_09_26_drive_0039_sync' # ok, 1 moving
# drive_id = '2011_09_26_drive_0046_sync' # (short) only 1 moving at start
# drive_id = '2011_09_26_drive_0048_sync' # ok but short, no movement
# drive_id = '2011_09_26_drive_0052_sync' #
# drive_id = '2011_09_26_drive_0056_sync' #
# drive_id = '2011_09_26_drive_0057_sync' # (gps sinking)
# drive_id = '2011_09_26_drive_0059_sync' #
# drive_id = '2011_09_26_drive_0060_sync' #
# drive_id = '2011_09_26_drive_0061_sync' # ego only, ok, long, bumpy, some gps drift
# drive_id = '2011_09_26_drive_0064_sync' # Smart car, sample 25
# drive_id = '2011_09_26_drive_0070_sync' #
# drive_id = '2011_09_26_drive_0079_sync' #
drive_id = '2011_09_26_drive_0086_sync' # (medium) uphill
# drive_id = '2011_09_26_drive_0087_sync' #
# drive_id = '2011_09_26_drive_0091_sync' #
# drive_id = '2011_09_26_drive_0093_sync' # Sample 50 (bad)
# drive_id = '2011_09_26_drive_0106_sync' # Two cyclists on right
# drive_id = '2011_09_26_drive_0113_sync' #
# drive_id = '2011_09_26_drive_0117_sync' # (long)
# drive_id = '2011_09_28_drive_0002_sync' # campus
# drive_id = '2011_09_28_drive_0016_sync' # campus
# drive_id = '2011_09_28_drive_0021_sync' # campus
# drive_id = '2011_09_28_drive_0034_sync' #
# drive_id = '2011_09_28_drive_0037_sync' #
# drive_id = '2011_09_28_drive_0038_sync' #
# drive_id = '2011_09_28_drive_0039_sync' # busy campus, bad gps
# drive_id = '2011_09_28_drive_0043_sync' #
# drive_id = '2011_09_28_drive_0045_sync' #
# drive_id = '2011_09_28_drive_0179_sync' #
# drive_id = '2011_09_29_drive_0026_sync' #
# drive_id = '2011_09_29_drive_0071_sync' #
# drive_id = '2011_09_30_drive_0020_sync' #
# drive_id = '2011_09_30_drive_0027_sync' # (long)
# drive_id = '2011_09_30_drive_0028_sync' # (long) bad gps
# drive_id = '2011_09_30_drive_0033_sync' #
# drive_id = '2011_09_30_drive_0034_sync' #
# drive_id = '2011_10_03_drive_0042_sync' #
# drive_id = '2011_10_03_drive_0047_sync' #
raw_dir = os.path.expanduser('~/Kitti/raw')
vtk_window_size = (900, 600)
save_images = False
point_cloud_source = 'lidar'
# Load raw data
drive_date = drive_id[0:10]
drive_num_str = drive_id[17:21]
raw_data = pykitti.raw(raw_dir, drive_date, drive_num_str)
# Check that velo length matches timestamps?
if len(raw_data.velo_files) != len(raw_data.timestamps):
raise ValueError('velo files and timestamps have different length!')
frame_range = (0, len(raw_data.timestamps))
# frame_range = (0, 100)
camera_viewpoint = 'front'
camera_viewpoint = 'elevated'
####################
# End of Options
####################
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'Overlaid Point Cloud', vtk_window_size, vtk_renderer)
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(
vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer))
vtk_interactor.Initialize()
if point_cloud_source != 'lidar':
raise ValueError(
'Invalid point cloud source {}'.format(point_cloud_source))
# Load poses from ORBSLAM2
poses = np.loadtxt(raw_dir +
'/orbslam2_cam0_poses/{}/{}_cam0_poses.txt'.format(
drive_date, drive_id))
poses = poses.reshape((-1, 3, 4))
poses = np.pad(poses, ((0, 0), (0, 1), (0, 0)),
mode='constant',
constant_values=0)
poses[:, 3, 3] = 1.0
# Read calibrations
tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu
tf_imu_calib_velo_calib = transform_utils.invert_tf(
tf_velo_calib_imu_calib)
tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(1, 1, 1)
vtk_renderer.AddActor(vtk_axes)
for frame_idx in range(*frame_range):
# Point cloud actor wrapper
vtk_pc = VtkPointCloud()
vtk_pc.vtk_actor.GetProperty().SetPointSize(2)
# all_vtk_actors.append(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_pc.vtk_actor)
print('{} / {}'.format(frame_idx, len(raw_data.timestamps) - 1))
# Load next frame data
load_start_time = time.time()
rgb_image = np.asarray(raw_data.get_cam2(frame_idx))
bgr_image = rgb_image[..., ::-1]
if point_cloud_source == 'lidar':
velo_points, velo_intensities = get_velo_points(
raw_data, frame_idx)
# Transform point cloud to cam_0 frame
velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]],
constant_values=1.0,
mode='constant')
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# Project velodyne points
projection_start_time = time.time()
if point_cloud_source == 'lidar':
cam0_curr_pc_padded = tf_cam0_calib_velo_calib @ velo_curr_points_padded.T
point_colours = np.repeat(
np.round(velo_intensities * 255).astype(np.uint8),
3).reshape(-1, 3)
print('projection\t', time.time() - projection_start_time)
cam0_ref_pose = poses[frame_idx]
tf_cam0_ref_cam0_curr = cam0_ref_pose
cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded
# VtkPointCloud
vtk_pc_start_time = time.time()
vtk_pc.set_points(cam0_ref_pc_padded[0:3].T, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
vtk_pc_pose = VtkPointCloud()
vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5)
vtk_pc_pose.set_points(np.reshape(cam0_ref_pose[0:3, 3], [-1, 3]))
# vtk_renderer.AddActor(vtk_pc_pose.vtk_actor)
# Move camera
if camera_viewpoint == 'front':
cam0_curr_vtk_cam_position = [0.0, 0.0, 0.0, 1.0]
elif camera_viewpoint == 'elevated':
cam0_curr_vtk_cam_position = [0.0, -10.0, -25.0, 1.0]
elif camera_viewpoint == '':
pass
else:
raise ValueError('Invalid camera_pos', camera_viewpoint)
cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr @ cam0_curr_vtk_cam_position
cam0_curr_vtk_cam_fp = [0.0, 0.0, 20.0, 1.0]
cam0_ref_vtk_cam_fp = tf_cam0_ref_cam0_curr @ cam0_curr_vtk_cam_fp
current_cam = vtk_renderer.GetActiveCamera()
# vtk_renderer.ResetCamera()
current_cam.SetViewUp(0, -1, 0)
current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3])
current_cam.SetFocalPoint(*cam0_ref_vtk_cam_fp[0:3])
# current_cam.Zoom(0.5)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_render_window.Render()
print('render\t\t', time.time() - render_start_time)
print('---')
print('Done')
# Keep window open
vtk_interactor.Start()
def main():
"""Demo to display point clouds and 3D bounding boxes
Keys:
F1: Toggle LIDAR point cloud
F2: Toggle depth map point cloud
F3: Toggle labels
"""
##############################
# Options
##############################
# sample_name = '000050'
# sample_name = '000095'
# sample_name = '000641'
# sample_name = '000169'
# sample_name = '000191'
# sample_name = '000197'
# sample_name = '006562'
# sample_name = '004965'
sample_name = '001692'
# sample_name = '000999'
# Area extents
x_min = -40
x_max = 40
y_min = -5
y_max = 5
z_min = 0
z_max = 70
##############################
# End of Options
##############################
area_extents = np.array([[x_min, x_max], [y_min, y_max], [z_min, z_max]],
dtype=np.float32)
dataset = DatasetBuilder.build_kitti_obj_dataset(
DatasetBuilder.KITTI_TRAINVAL)
# Get sample info
image = obj_utils.get_image(sample_name, dataset.image_2_dir)
image_shape = image.shape[0:2]
frame_calib = calib_utils.get_frame_calib(dataset.calib_dir, sample_name)
# Get lidar points
lidar_point_cloud, lidar_pc_i = obj_utils.get_lidar_point_cloud(
sample_name, frame_calib, dataset.velo_dir, intensity=True)
lidar_point_cloud, area_filter = obj_utils.filter_pc_to_area(
lidar_point_cloud, area_extents)
# Filter to image
lidar_points_in_img = calib_utils.project_pc_to_image(
lidar_point_cloud, frame_calib.p2)
lidar_point_cloud, image_filter = obj_utils.filter_pc_to_image(
lidar_point_cloud, lidar_points_in_img, image_shape)
lidar_points_in_img_int = np.floor(
lidar_points_in_img[:, image_filter]).astype(np.int32)
lidar_point_colours = image[lidar_points_in_img_int[1],
lidar_points_in_img_int[0]]
# lidar_pc_i_valid = lidar_pc_i[area_filter]
# lidar_point_colours = (np.repeat(lidar_pc_i_valid, 3).reshape(-1, 3) * 200 + 25).astype(np.uint8)
# Get points from depth map
depth_point_cloud = obj_utils.get_depth_map_point_cloud(
sample_name, frame_calib, dataset.depth_dir)
depth_point_cloud, area_filter = obj_utils.filter_pc_to_area(
depth_point_cloud, area_extents)
# Filter depth map points to area
area_filter = np.reshape(area_filter, image.shape[0:2])
depth_point_colours = image[area_filter]
# Get bounding boxes
gt_objects = obj_utils.read_labels(dataset.label_dir, sample_name)
##############################
# Vtk Visualization
##############################
vtk_pc_lidar = VtkPointCloud()
vtk_pc_depth = VtkPointCloud()
vtk_pc_lidar.vtk_actor.GetProperty().SetPointSize(2)
vtk_pc_depth.vtk_actor.GetProperty().SetPointSize(2)
vtk_pc_lidar.set_points(lidar_point_cloud.T, lidar_point_colours)
vtk_pc_depth.set_points(depth_point_cloud.T, depth_point_colours)
# Create VtkBoxes for boxes
vtk_pyr_boxes = VtkPyramidBoxes()
vtk_pyr_boxes.set_objects(gt_objects, vtk_utils.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.SetBackground(0.2, 0.3, 0.4)
vtk_renderer.SetBackground(0.35, 0.45, 0.55)
# vtk_renderer.SetBackground(0.1, 0.1, 0.1)
# vtk_renderer.SetBackground(0.8, 0.8, 0.8)
vtk_renderer.AddActor(vtk_pc_lidar.vtk_actor)
vtk_renderer.AddActor(vtk_pc_depth.vtk_actor)
vtk_renderer.AddActor(vtk_pyr_boxes.vtk_actor)
# vtk_renderer.AddActor(axes)
# 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)
# current_cam.SetViewAngle(5.0)
# 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, Sample {}".format(sample_name))
# vtk_render_window.SetSize(1280, 720)
vtk_render_window.SetSize(960, 640)
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
custom_interactor = vtk_utils.ToggleActorsInteractorStyle([
vtk_pc_lidar.vtk_actor, vtk_pc_depth.vtk_actor, vtk_pyr_boxes.vtk_actor
], vtk_renderer, current_cam, axes)
vtk_render_window_interactor.SetInteractorStyle(custom_interactor)
# Render in VTK
vtk_render_window.Render()
vtk_render_window_interactor.Start() # Blocking
def main():
"""Comparison of ground truth and ORBSLAM2 poses
https://github.com/raulmur/ORB_SLAM2
"""
####################
# Options
####################
odom_dataset_dir = os.path.expanduser('~/Kitti/odometry/dataset')
# sequence = '02'
# sequence = '03'
# sequence = '08'
sequence = '09'
# sequence = '10'
# sequence = '11'
# sequence = '12'
# vtk_window_size = (1280, 720)
vtk_window_size = (960, 540)
# vtk_window_size = (400, 300)
point_cloud_source = 'lidar'
# point_cloud_source = 'fast'
# point_cloud_source = 'multiscale'
# first_frame_idx = None
# first_frame_pose = None
# Setup odometry dataset handler
odom_dataset = pykitti.odometry(odom_dataset_dir, sequence)
# # Check that velo length matches timestamps?
# if len(odom_dataset.velo_files) != len(odom_dataset.timestamps):
# raise ValueError('velo files and timestamps have different length!')
frame_range = (0, len(odom_dataset.timestamps))
# frame_range = (0, 100)
# camera_viewpoint = 'front'
camera_viewpoint = 'elevated'
# camera_viewpoint = 'bev'
buffer_size = 50
buffer_update = 10
save_screenshots = False
##############################
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'Overlaid Point Cloud', vtk_window_size, vtk_renderer)
if save_screenshots:
vtk_win_to_img_filter, vtk_png_writer = vtk_utils.setup_screenshots(
vtk_render_window)
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(
vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer))
vtk_interactor.Initialize()
cam_p2 = odom_dataset.calib.P_rect_20
# Load poses
cam0_ref_poses_orbslam = np.loadtxt(
odom_dataset_dir + '/poses_orbslam2/{}.txt'.format(sequence))
cam0_ref_poses_orbslam = np.pad(cam0_ref_poses_orbslam, ((0, 0), (0, 4)),
mode='constant')
cam0_ref_poses_orbslam[:, -1] = 1
cam0_ref_poses_orbslam = cam0_ref_poses_orbslam.reshape((-1, 4, 4))
cam0_ref_poses_gt = np.asarray(odom_dataset.poses, np.float32)
# Setup camera
if camera_viewpoint == 'front':
cam0_curr_vtk_cam_pos = [0.0, 0.0, 0.0, 1.0]
cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0]
elif camera_viewpoint == 'elevated':
cam0_curr_vtk_cam_pos = [0.0, -5.0, -15.0, 1.0]
cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0]
elif camera_viewpoint == 'bev':
# camera_zoom = 1.0
cam0_curr_vtk_cam_pos = [0.0, -50.0, 10.0, 1.0]
# camera_pos = (0.0, 0.0, 0.0)
cam0_curr_vtk_focal_point = [0.0, 0.0, 15.0, 1.0]
else:
raise ValueError('Invalid camera_pos', camera_viewpoint)
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(2, 2, 2)
vtk_renderer.AddActor(vtk_axes)
vtk_pc_poses_orbslam = VtkPointCloudGlyph()
vtk_pc_poses_gt = VtkPointCloudGlyph()
vtk_pc_poses_orbslam.vtk_actor.GetProperty().SetPointSize(5)
vtk_pc_poses_gt.vtk_actor.GetProperty().SetPointSize(5)
actor_buffer = []
for frame_idx in range(*frame_range):
print('{} / {}'.format(frame_idx, len(odom_dataset.timestamps) - 1))
# Load next frame data
load_start_time = time.time()
rgb_load_start = time.time()
bgr_image = cv2.imread(odom_dataset.cam2_files[frame_idx])
print('rgb_load', time.time() - rgb_load_start)
if point_cloud_source == 'lidar':
velo_curr_points_padded = get_velo_points(odom_dataset, frame_idx)
# Velo in cam0_curr
cam0_curr_pc_all_padded = odom_dataset.calib.T_cam0_velo @ velo_curr_points_padded.T
pass
# cam0_curr_points = cam0_curr_pc.T
elif point_cloud_source in ['fast', 'multiscale']:
# depth_map_path = depth_map_dir + '/{:010d}.png'.format(frame_idx)
# depth_map = depth_map_utils.read_depth_map(depth_map_path)
# points_cam2 = depth_map_utils.get_depth_point_cloud(depth_map, cam_p).T
raise NotImplementedError()
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# Project velodyne points
projection_start_time = time.time()
if point_cloud_source == 'lidar':
# Project into image2
points_in_img2 = calib_utils.project_pc_to_image2(
cam0_curr_pc_all_padded, cam_p2)
points_in_img2_int = np.round(points_in_img2).astype(np.int32)
image_filter = obj_utils.points_in_img_filter(
points_in_img2_int, bgr_image.shape)
cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter]
# points = points_cam2[image_mask]
points_in_img_int_valid = points_in_img2_int[:, image_filter]
point_colours = bgr_image[points_in_img_int_valid[1],
points_in_img_int_valid[0]]
else:
raise ValueError('Invalid point_cloud_source', point_cloud_source)
print('projection\t', time.time() - projection_start_time)
# Get pose
cam0_ref_pose_orbslam = cam0_ref_poses_orbslam[frame_idx]
cam0_ref_pose_gt = cam0_ref_poses_gt[frame_idx]
tf_cam0_ref_cam0_curr = cam0_ref_pose_orbslam
cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded
# VtkPointCloud
vtk_pc = VtkPointCloudGlyph()
vtk_pc.vtk_actor.GetProperty().SetPointSize(2)
vtk_pc_start_time = time.time()
vtk_pc.set_points(cam0_ref_pc_padded[0:3].T, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
# Display orbslam pose
vtk_pc_poses_orbslam.set_points(
cam0_ref_poses_orbslam[max(0, frame_idx - buffer_size):frame_idx +
1, 0:3, 3],
np.tile([255, 0, 0], [buffer_size, 1]))
# Display gt pose
vtk_pc_poses_gt.vtk_actor.GetProperty().SetPointSize(5)
vtk_pc_poses_gt.set_points(
cam0_ref_poses_gt[max(0, frame_idx - buffer_size):frame_idx + 1,
0:3, 3], np.tile([0, 255, 0], [buffer_size, 1]))
# Add vtk actors
vtk_renderer.AddActor(vtk_pc.vtk_actor)
vtk_renderer.AddActor(vtk_pc_poses_orbslam.vtk_actor)
vtk_renderer.AddActor(vtk_pc_poses_gt.vtk_actor)
cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr.dot(cam0_curr_vtk_cam_pos)
cam0_ref_vtk_focal_point = tf_cam0_ref_cam0_curr.dot(
cam0_curr_vtk_focal_point)
current_cam = vtk_renderer.GetActiveCamera()
vtk_renderer.ResetCamera()
current_cam.SetViewUp(0, -1, 0)
current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3])
current_cam.SetFocalPoint(*cam0_ref_vtk_focal_point[0:3])
current_cam.Zoom(0.5)
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_render_window.Render()
print('render\t\t', time.time() - render_start_time)
actor_buffer.append(vtk_pc.vtk_actor)
if len(actor_buffer) > buffer_size:
if frame_idx % buffer_update != 0:
actor_buffer[frame_idx - buffer_size].SetVisibility(0)
if save_screenshots:
screenshot_start_time = time.time()
screenshot_path = core.data_dir() + '/temp/{:010d}.png'.format(
frame_idx)
vtk_utils.save_screenshot(screenshot_path, vtk_win_to_img_filter,
vtk_png_writer)
print('screenshot\t', time.time() - screenshot_start_time)
print('---')
for vtk_actor in actor_buffer:
vtk_actor.SetVisibility(1)
print('Done')
# Keep window open
vtk_interactor.Start()
def main():
##############################
# Options
##############################
"""Note: Run scripts/depth_completion/save_depth_maps_raw.py first.
Demo to show depth completed point clouds
"""
raw_dir = os.path.expanduser('~/Kitti/raw')
drive_id = '2011_09_26_drive_0023_sync'
vtk_window_size = (1280, 720)
max_fps = 30.0
# point_cloud_source = 'lidar'
# fill_type = None
point_cloud_source = 'depth'
fill_type = 'multiscale'
# Load raw data
drive_date = drive_id[0:10]
drive_num_str = drive_id[17:21]
raw_data = pykitti.raw(raw_dir, drive_date, drive_num_str)
# Check that velo length matches timestamps?
if len(raw_data.velo_files) != len(raw_data.timestamps):
raise ValueError('velo files and timestamps have different length!')
frame_range = (0, len(raw_data.timestamps))
##############################
min_loop_time = 1.0 / max_fps
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'Overlaid Point Cloud', vtk_window_size, vtk_renderer)
# Setup camera
current_cam = vtk_renderer.GetActiveCamera()
current_cam.SetViewUp(0, 0, 1)
current_cam.SetPosition(0.0, -8.0, -15.0)
current_cam.SetFocalPoint(0.0, 0.0, 20.0)
current_cam.Zoom(0.7)
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(2, 2, 2)
# Setup interactor
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(
vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer, current_cam,
vtk_axes))
vtk_interactor.Initialize()
if point_cloud_source not in ['lidar', 'depth']:
raise ValueError(
'Invalid point cloud source {}'.format(point_cloud_source))
cam_p = raw_data.calib.P_rect_20
# Point cloud
vtk_pc = VtkPointCloudGlyph()
# Add actors
vtk_renderer.AddActor(vtk_axes)
vtk_renderer.AddActor(vtk_pc.vtk_actor)
for frame_idx in range(*frame_range):
loop_start_time = time.time()
print('{} / {}'.format(frame_idx, len(raw_data.timestamps) - 1))
# Load next frame data
load_start_time = time.time()
rgb_image = np.asarray(raw_data.get_cam2(frame_idx))
bgr_image = rgb_image[..., ::-1]
if point_cloud_source == 'lidar':
velo_points = get_velo_points(raw_data, frame_idx)
# Transform point cloud to cam_0 frame
velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]],
constant_values=1.0,
mode='constant')
cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T
# Project velodyne points
projection_start_time = time.time()
points_in_img = calib_utils.project_pc_to_image(
cam0_curr_pc_all_padded[0:3], cam_p)
points_in_img_int = np.round(points_in_img).astype(np.int32)
print('projection\t', time.time() - projection_start_time)
image_filter = obj_utils.points_in_img_filter(
points_in_img_int, bgr_image.shape)
cam0_curr_pc = cam0_curr_pc_all_padded[0:3, image_filter]
points_in_img_int_valid = points_in_img_int[:, image_filter]
point_colours = bgr_image[points_in_img_int_valid[1],
points_in_img_int_valid[0]]
elif point_cloud_source == 'depth':
depth_maps_dir = core.data_dir() + \
'/depth_completion/raw/{}/depth_02_{}'.format(drive_id, fill_type)
depth_map_path = depth_maps_dir + '/{:010d}.png'.format(frame_idx)
depth_map = depth_map_utils.read_depth_map(depth_map_path)
cam0_curr_pc = depth_map_utils.get_depth_point_cloud(
depth_map, cam_p)
point_colours = bgr_image.reshape(-1, 3)
# Mask to valid points
valid_mask = (cam0_curr_pc[2] != 0)
cam0_curr_pc = cam0_curr_pc[:, valid_mask]
point_colours = point_colours[valid_mask]
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# VtkPointCloud
vtk_pc_start_time = time.time()
vtk_pc.set_points(cam0_curr_pc.T, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_render_window.Render()
print('render\t\t', time.time() - render_start_time)
# Pause to keep frame rate under max
loop_run_time = time.time() - loop_start_time
print('loop\t\t', loop_run_time)
if loop_run_time < min_loop_time:
time.sleep(min_loop_time - loop_run_time)
print('---')
print('Done')
# Keep window open
vtk_interactor.Start()
def main():
####################
# Options
####################
# drive_id = '2011_09_26_drive_0001_sync' # No moving cars, depth completion sample 0 area
# drive_id = '2011_09_26_drive_0002_sync' # depth completion sample 0
# drive_id = '2011_09_26_drive_0005_sync' # (gps bad)
# drive_id = '2011_09_26_drive_0005_sync' # (moving) (gps bad)
# drive_id = '2011_09_26_drive_0009_sync' # missing velo?
# drive_id = '2011_09_26_drive_0011_sync' # both moving, then traffic light)
# drive_id = '2011_09_26_drive_0013_sync' # both moving
# drive_id = '2011_09_26_drive_0014_sync' # both moving, sample 104
# drive_id = '2011_09_26_drive_0015_sync' # both moving
# drive_id = '2011_09_26_drive_0017_sync' # other only, traffic light, moving across
# drive_id = '2011_09_26_drive_0018_sync' # other only, traffic light, forward
# drive_id = '2011_09_26_drive_0019_sync' # both moving, some people at end, wonky gps
# drive_id = '2011_09_26_drive_0020_sync' # gps drift
# drive_id = '2011_09_26_drive_0022_sync' # ego mostly, then both, long
# drive_id = '2011_09_26_drive_0023_sync' # sample 169 (long)
# drive_id = '2011_09_26_drive_0027_sync' # both moving, fast, straight
# drive_id = '2011_09_26_drive_0028_sync' # both moving, opposite
drive_id = '2011_09_26_drive_0029_sync' # both moving, good gps
# drive_id = '2011_09_26_drive_0032_sync' # both moving, following some cars
# drive_id = '2011_09_26_drive_0035_sync' #
# drive_id = '2011_09_26_drive_0036_sync' # (long) behind red truck
# drive_id = '2011_09_26_drive_0039_sync' # ok, 1 moving
# drive_id = '2011_09_26_drive_0046_sync' # (short) only 1 moving at start
# drive_id = '2011_09_26_drive_0048_sync' # ok but short, no movement
# drive_id = '2011_09_26_drive_0052_sync' #
# drive_id = '2011_09_26_drive_0056_sync' #
# drive_id = '2011_09_26_drive_0057_sync' # (gps sinking)
# drive_id = '2011_09_26_drive_0059_sync' #
# drive_id = '2011_09_26_drive_0060_sync' #
# drive_id = '2011_09_26_drive_0061_sync' # ego only, ok, long, bumpy, some gps drift
# drive_id = '2011_09_26_drive_0064_sync' # Smart car, sample 25
# drive_id = '2011_09_26_drive_0070_sync' #
# drive_id = '2011_09_26_drive_0079_sync' #
# drive_id = '2011_09_26_drive_0086_sync' # (medium) uphill
# drive_id = '2011_09_26_drive_0087_sync' #
# drive_id = '2011_09_26_drive_0091_sync' #
# drive_id = '2011_09_26_drive_0093_sync' # Sample 50 (bad)
# drive_id = '2011_09_26_drive_0106_sync' # Two cyclists on right
# drive_id = '2011_09_26_drive_0113_sync' #
# drive_id = '2011_09_26_drive_0117_sync' # (long)
# drive_id = '2011_09_28_drive_0002_sync' # campus
# drive_id = '2011_09_28_drive_0016_sync' # campus
# drive_id = '2011_09_28_drive_0021_sync' # campus
# drive_id = '2011_09_28_drive_0034_sync' #
# drive_id = '2011_09_28_drive_0037_sync' #
# drive_id = '2011_09_28_drive_0038_sync' #
# drive_id = '2011_09_28_drive_0039_sync' # busy campus, bad gps
# drive_id = '2011_09_28_drive_0043_sync' #
# drive_id = '2011_09_28_drive_0045_sync' #
# drive_id = '2011_09_28_drive_0179_sync' #
# drive_id = '2011_09_29_drive_0026_sync' #
# drive_id = '2011_09_29_drive_0071_sync' #
# drive_id = '2011_09_30_drive_0020_sync' #
# drive_id = '2011_09_30_drive_0027_sync' # (long)
# drive_id = '2011_09_30_drive_0028_sync' # (long) bad gps
# drive_id = '2011_09_30_drive_0033_sync' #
# drive_id = '2011_09_30_drive_0034_sync' #
# drive_id = '2011_10_03_drive_0042_sync' #
# drive_id = '2011_10_03_drive_0047_sync' #
raw_dir = os.path.expanduser('~/Kitti/raw')
vtk_window_size = (1280, 720)
point_cloud_source = 'lidar'
# Load raw data
drive_date = drive_id[0:10]
drive_num_str = drive_id[17:21]
raw_data = pykitti.raw(raw_dir, drive_date, drive_num_str)
# Check that velo length matches timestamps?
if len(raw_data.velo_files) != len(raw_data.timestamps):
raise ValueError('velo files and timestamps have different length!')
frame_range = (0, len(raw_data.timestamps))
# frame_range = (0, 100)
poses_source = 'gps'
# poses_source = 'orbslam2'
# camera_viewpoint = 'front'
camera_viewpoint = 'elevated'
####################
# End of Options
####################
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'Overlaid Point Cloud', vtk_window_size, vtk_renderer)
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer))
vtk_interactor.Initialize()
drive_date_dir = raw_dir + '/{}'.format(drive_date)
if point_cloud_source != 'lidar':
raise ValueError('Invalid point cloud source {}'.format(
point_cloud_source))
cam_p = raw_data.calib.P_rect_20
if poses_source == 'gps':
# Load poses from matlab
poses_path = drive_date_dir + '/{}/poses.mat'.format(drive_id)
poses_mat = scipy.io.loadmat(poses_path)
imu_ref_poses = np.asarray(poses_mat['pose'][0])
elif poses_source == 'orbslam2':
# Load poses from ORBSLAM2
imu_ref_poses = np.loadtxt(raw_data.data_path + '/poses_orbslam2.txt').reshape(-1, 3, 4)
imu_ref_poses = np.pad(imu_ref_poses, ((0, 0), (0, 1), (0, 0)),
mode='constant', constant_values=0)
imu_ref_poses[:, 3, 3] = 1.0
else:
raise ValueError('Invalid poses_source', poses_source)
# Read calibrations
tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu
tf_imu_calib_velo_calib = transform_utils.invert_tf(tf_velo_calib_imu_calib)
tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo
tf_velo_calib_cam0_calib = transform_utils.invert_tf(tf_cam0_calib_velo_calib)
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(2, 2, 2)
vtk_renderer.AddActor(vtk_axes)
for frame_idx in range(*frame_range):
# Point cloud actor wrapper
# vtk_pc = VtkPointCloud()
vtk_pc = VtkPointCloudGlyph()
vtk_pc.vtk_actor.GetProperty().SetPointSize(2)
# all_vtk_actors.append(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_pc.vtk_actor)
print('{} / {}'.format(frame_idx, len(raw_data.timestamps) - 1))
# Load next frame data
load_start_time = time.time()
rgb_image = np.asarray(raw_data.get_cam2(frame_idx))
bgr_image = rgb_image[..., ::-1]
if point_cloud_source == 'lidar':
velo_points = get_velo_points(raw_data, frame_idx)
# Transform point cloud to cam_0 frame
velo_curr_points_padded = np.pad(
velo_points, [[0, 0], [0, 1]],
constant_values=1.0, mode='constant')
# R_rect_00 already applied in T_cam0_velo
# T_cam0_velo = R_rect_00 @ T_cam0_velo_unrect
cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# Project velodyne points
projection_start_time = time.time()
if point_cloud_source == 'lidar':
points_in_img = calib_utils.project_pc_to_image(cam0_curr_pc_all_padded[0:3], cam_p)
points_in_img_int = np.round(points_in_img).astype(np.int32)
image_filter = obj_utils.points_in_img_filter(points_in_img_int, bgr_image.shape)
cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter]
points_in_img_int_valid = points_in_img_int[:, image_filter]
point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]]
print('projection\t', time.time() - projection_start_time)
# Get calibration transformations
tf_velo_calib_imu_calib = transform_utils.invert_tf(tf_imu_calib_velo_calib)
tf_cam0_calib_imu_calib = tf_cam0_calib_velo_calib @ tf_velo_calib_imu_calib
tf_imu_calib_cam0_calib = transform_utils.invert_tf(tf_cam0_calib_imu_calib)
# Get poses
imu_ref_pose = imu_ref_poses[frame_idx]
tf_imu_ref_imu_curr = imu_ref_pose
# Calculate point cloud in imu_ref frame
velo_curr_pc_padded = tf_velo_calib_cam0_calib @ cam0_curr_pc_padded
imu_curr_pc_padded = tf_imu_calib_velo_calib @ velo_curr_pc_padded
imu_ref_pc_padded = tf_imu_ref_imu_curr @ imu_curr_pc_padded
# VtkPointCloud
vtk_pc_start_time = time.time()
vtk_pc.set_points(imu_ref_pc_padded[0:3].T, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
vtk_pc_pose = VtkPointCloudGlyph()
vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5)
vtk_pc_pose.set_points(np.reshape(imu_ref_pose[0:3, 3], [-1, 3]))
vtk_renderer.AddActor(vtk_pc_pose.vtk_actor)
# Move camera
if camera_viewpoint == 'front':
imu_curr_cam0_position = tf_imu_calib_cam0_calib @ [0.0, 0.0, 0.0, 1.0]
elif camera_viewpoint == 'elevated':
imu_curr_cam0_position = tf_imu_calib_cam0_calib.dot([0.0, -5.0, -10.0, 1.0])
else:
raise ValueError('Invalid camera_pos', camera_viewpoint)
imu_ref_cam0_position = tf_imu_ref_imu_curr.dot(imu_curr_cam0_position)
imu_curr_focal_point = tf_imu_calib_cam0_calib.dot([0.0, 0.0, 20.0, 1.0])
imu_ref_focal_point = tf_imu_ref_imu_curr.dot(imu_curr_focal_point)
current_cam = vtk_renderer.GetActiveCamera()
vtk_renderer.ResetCamera()
current_cam.SetViewUp(0, 0, 1)
current_cam.SetPosition(imu_ref_cam0_position[0:3])
current_cam.SetFocalPoint(*imu_ref_focal_point[0:3])
current_cam.Zoom(0.5)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_render_window.Render()
print('render\t\t', time.time() - render_start_time)
print('---')
print('Done')
# Keep window open
vtk_interactor.Start()
def main():
##############################
# Options
##############################
# sequence_id = '0013'
# sequence_id = '0014'
sequence_id = '0015'
sequence_id = '0020'
# raw_dir = os.path.expanduser('~/Kitti/raw')
tracking_dir = os.path.expanduser('~/Kitti/tracking/training')
tracking_data = pykitti.tracking(tracking_dir, sequence_id)
max_fps = 30.0
vtk_window_size = (1280, 720)
show_pose = True
point_cloud_source = 'lidar'
buffer_size = 5
oxts_path = tracking_data.base_path + '/oxts/{}.txt'.format(sequence_id)
oxts_data = pykitti.utils.load_oxts_packets_and_poses([oxts_path])
poses = np.asarray([oxt.T_w_imu for oxt in oxts_data])
calib_path = tracking_data.base_path + '/calib/{}.txt'.format(sequence_id)
calib_data = tracking_utils.load_calib(calib_path)
camera_viewpoint = 'front'
camera_viewpoint = 'elevated'
load_images = True
# load_images = False
##############################
label_path = tracking_data.base_path + '/label_02/{}.txt'.format(
sequence_id)
gt_tracklets = tracking_utils.get_tracklets(label_path)
min_loop_time = 1.0 / max_fps
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'KITTI Tracking Demo', vtk_window_size, vtk_renderer)
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(
vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer))
vtk_interactor.Initialize()
if point_cloud_source != 'lidar':
raise ValueError(
'Invalid point cloud source {}'.format(point_cloud_source))
# cam_p = raw_data.calib.P_rect_20
cam_p = calib_data['P2'].reshape(3, 4)
r_rect = calib_data['R_rect'].reshape(3, 3)
# Read calibrations
tf_velo_calib_imu_calib = calib_data['Tr_imu_velo'].reshape(3, 4)
tf_velo_calib_imu_calib = np.vstack(
[tf_velo_calib_imu_calib,
np.asarray([0, 0, 0, 1])])
# tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu
tf_imu_calib_velo_calib = transform_utils.invert_tf(
tf_velo_calib_imu_calib)
tf_cam0_calib_velo_calib = r_rect @ calib_data['Tr_velo_cam'].reshape(3, 4)
tf_cam0_calib_velo_calib = np.vstack(
[tf_cam0_calib_velo_calib,
np.asarray([0, 0, 0, 1])])
# tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo
tf_velo_calib_cam0_calib = transform_utils.invert_tf(
tf_cam0_calib_velo_calib)
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(2, 2, 2)
vtk_renderer.AddActor(vtk_axes)
# Create vtk actors
vtk_pc_list = []
for i in range(buffer_size):
# Point cloud actor wrapper
vtk_pc = VtkPointCloudGlyph()
vtk_pc.vtk_actor.GetProperty().SetPointSize(2)
# all_vtk_actors.append(vtk_point_cloud.vtk_actor)
vtk_renderer.AddActor(vtk_pc.vtk_actor)
vtk_pc_list.append(vtk_pc)
curr_actor_idx = -1
vtk_boxes = VtkBoxes()
vtk_text_labels = VtkTextLabels()
vtk_renderer.AddActor(vtk_boxes.vtk_actor)
vtk_renderer.AddActor(vtk_text_labels.vtk_actor)
for frame_idx in range(len(poses)):
loop_start_time = time.time()
curr_actor_idx = (curr_actor_idx + 1) % buffer_size
vtk_pc = vtk_pc_list[curr_actor_idx]
for i in range(buffer_size):
if i == curr_actor_idx:
vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(3)
else:
vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(0.5)
print('{} / {}'.format(frame_idx, len(tracking_data.velo_files) - 1))
# Load next frame data
load_start_time = time.time()
if point_cloud_source == 'lidar':
velo_points, velo_points_i = get_velo_points(
tracking_data, frame_idx)
# Transform point cloud to cam_0 frame
velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]],
constant_values=1.0,
mode='constant')
cam0_curr_pc_all_padded = tf_cam0_calib_velo_calib @ velo_curr_points_padded.T
# cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# Project velodyne points
projection_start_time = time.time()
if point_cloud_source == 'lidar':
if load_images:
rgb_image = np.asarray(tracking_data.get_cam2(frame_idx))
bgr_image = rgb_image[..., ::-1]
points_in_img = calib_utils.project_pc_to_image(
cam0_curr_pc_all_padded[0:3], cam_p)
points_in_img_int = np.round(points_in_img).astype(np.int32)
image_filter = obj_utils.points_in_img_filter(
points_in_img_int, bgr_image.shape)
cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter]
points_in_img_int_valid = points_in_img_int[:, image_filter]
point_colours = bgr_image[points_in_img_int_valid[1],
points_in_img_int_valid[0]]
else:
cam0_curr_pc_padded = cam0_curr_pc_all_padded
point_colours = np.repeat(
(velo_points_i * 255).astype(np.uint8), 3).reshape(-1, 3)
# point_colours = np.full(cam0_curr_pc_padded[0:3].T.shape, [255, 255, 255])
else:
raise ValueError('Invalid point_cloud_source', point_cloud_source)
print('projection\t', time.time() - projection_start_time)
# Get calibration transformations
tf_velo_calib_imu_calib = transform_utils.invert_tf(
tf_imu_calib_velo_calib)
tf_cam0_calib_imu_calib = tf_cam0_calib_velo_calib @ tf_velo_calib_imu_calib
tf_imu_calib_cam0_calib = transform_utils.invert_tf(
tf_cam0_calib_imu_calib)
# Get poses
imu_ref_pose = poses[frame_idx]
tf_imu_ref_imu_curr = imu_ref_pose
velo_curr_pc_padded = tf_velo_calib_cam0_calib @ cam0_curr_pc_padded
imu_curr_pc_padded = tf_imu_calib_velo_calib @ velo_curr_pc_padded
imu_ref_pc_padded = tf_imu_ref_imu_curr @ imu_curr_pc_padded
# TODO: Show points in correct frame
# VtkPointCloud
vtk_pc_start_time = time.time()
vtk_pc.set_points(cam0_curr_pc_padded[0:3].T, point_colours)
# vtk_pc.set_points(imu_ref_pc_padded[0:3].T, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
# if show_pose:
# vtk_pc_pose = VtkPointCloudGlyph()
# vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5)
# vtk_pc_pose.set_points(np.reshape(imu_ref_pose[0:3, 3], [-1, 3]))
# vtk_renderer.AddActor(vtk_pc_pose.vtk_actor)
# Tracklets
frame_mask = [tracklet.frame == frame_idx for tracklet in gt_tracklets]
tracklets_for_frame = gt_tracklets[frame_mask]
obj_labels = [
tracking_utils.tracklet_to_obj_label(tracklet)
for tracklet in tracklets_for_frame
]
vtk_boxes.set_objects(obj_labels,
colour_scheme=vtk_utils.COLOUR_SCHEME_KITTI)
positions_3d = [obj_label.t for obj_label in obj_labels]
text_labels = [str(tracklet.id) for tracklet in tracklets_for_frame]
vtk_text_labels.set_text_labels(positions_3d, text_labels)
# Move camera
if camera_viewpoint == 'front':
imu_curr_cam0_position = tf_imu_calib_cam0_calib @ [
0.0, 0.0, 0.0, 1.0
]
elif camera_viewpoint == 'elevated':
imu_curr_cam0_position = tf_imu_calib_cam0_calib.dot(
[0.0, -5.0, -10.0, 1.0])
else:
raise ValueError('Invalid camera_pos', camera_viewpoint)
# imu_ref_cam0_position = tf_imu_ref_imu_curr.dot(imu_curr_cam0_position)
# imu_curr_focal_point = tf_imu_calib_cam0_calib.dot([0.0, 0.0, 20.0, 1.0])
# imu_ref_focal_point = tf_imu_ref_imu_curr.dot(imu_curr_focal_point)
current_cam = vtk_renderer.GetActiveCamera()
vtk_renderer.ResetCamera()
current_cam.SetViewUp(0, 0, 1)
current_cam.SetPosition(0.0, -8.0, -20.0)
# current_cam.SetPosition(imu_ref_cam0_position[0:3])
current_cam.SetFocalPoint(0.0, 0.0, 20.0)
# current_cam.SetFocalPoint(*imu_ref_focal_point[0:3])
# current_cam.Zoom(0.5)
# Reset the clipping range to show all points
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_render_window.Render()
print('render\t\t', time.time() - render_start_time)
# Pause to keep frame rate under max
loop_run_time = time.time() - loop_start_time
print('loop\t\t', loop_run_time)
if loop_run_time < min_loop_time:
time.sleep(min_loop_time - loop_run_time)
print('---')
print('Done')
# Keep window open
vtk_interactor.Start()
def main():
####################
# Options
####################
odom_dir = os.path.expanduser('~/Kitti/odometry/dataset')
# sequence = '01'
# sequence = '02'
# sequence = '03'
# sequence = '04'
# sequence = '05'
# sequence = '06'
# sequence = '08'
sequence = '09'
# max_fps = 10.0
# vtk_window_size = (1280, 720)
vtk_window_size = (960, 540)
save_images = False
point_cloud_source = 'lidar'
# point_cloud_source = 'fast'
# point_cloud_source = 'multiscale'
# Setup odometry dataset handler
odom_dataset = pykitti.odometry(odom_dir, sequence)
# # Check that velo length matches timestamps?
# if len(odom_dataset.velo_files) != len(odom_dataset.timestamps):
# raise ValueError('velo files and timestamps have different length!')
frame_range = (0, len(odom_dataset.timestamps))
# frame_range = (0, 200)
camera_viewpoint = 'front'
camera_viewpoint = 'elevated'
render_interval = 4
actor_buffer = []
####################
# End of Options
####################
vtk_renderer = demo_utils.setup_vtk_renderer()
vtk_render_window = demo_utils.setup_vtk_render_window(
'Sequence {}'.format(sequence), vtk_window_size, vtk_renderer)
vtk_interactor = vtk.vtkRenderWindowInteractor()
vtk_interactor.SetRenderWindow(vtk_render_window)
vtk_interactor.SetInteractorStyle(
vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer))
vtk_interactor.Initialize()
cam_p2 = odom_dataset.calib.P_rect_20
# Load poses
cam0_poses = odom_dataset.poses
# tf_cam0_calib_velo_calib = odom_dataset.calib.T_cam0_velo
# tf_velo_calib_cam0_calib = transform_utils.invert_tf(tf_cam0_calib_velo_calib)
# Setup camera
if camera_viewpoint == 'front':
cam0_curr_vtk_cam_pos = [0.0, 0.0, 0.0, 1.0]
cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0]
elif camera_viewpoint == 'elevated':
cam0_curr_vtk_cam_pos = [0.0, -5.0, -15.0, 1.0]
cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0]
else:
raise ValueError('Invalid camera_pos', camera_viewpoint)
# Create VtkAxes
vtk_axes = vtk.vtkAxesActor()
vtk_axes.SetTotalLength(1, 1, 1)
vtk_renderer.AddActor(vtk_axes)
all_render_times = []
buffer_points = []
buffer_colours = []
for frame_idx in range(*frame_range):
print('{} / {}'.format(frame_idx, len(odom_dataset.timestamps) - 1))
# Load next frame data
load_start_time = time.time()
rgb_image = np.asarray(odom_dataset.get_cam2(frame_idx))
bgr_image = rgb_image[..., ::-1]
if point_cloud_source == 'lidar':
velo_points = get_velo_points(odom_dataset, frame_idx)
# Transform point cloud to cam_0 frame
velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]],
constant_values=1.0,
mode='constant')
# Velo in cam0_curr
cam0_curr_pc_all_padded = odom_dataset.calib.T_cam0_velo @ velo_curr_points_padded.T
pass
# cam0_curr_points = cam0_curr_pc.T
elif point_cloud_source in ['fast', 'multiscale']:
# depth_map_path = depth_map_dir + '/{:010d}.png'.format(frame_idx)
# depth_map = depth_map_utils.read_depth_map(depth_map_path)
# points_cam2 = depth_map_utils.get_depth_point_cloud(depth_map, cam_p).T
raise NotImplementedError()
else:
raise ValueError('Invalid point cloud source')
print('load\t\t', time.time() - load_start_time)
# Project velodyne points
projection_start_time = time.time()
# points_in_img = calib_utils.project_pc_to_image(points_cam2.T, cam_p)
if point_cloud_source == 'lidar':
# Project into image2
points_in_img2 = calib_utils.project_pc_to_image(
cam0_curr_pc_all_padded[0:3], cam_p2)
points_in_img2_int = np.round(points_in_img2).astype(np.int32)
image_filter = obj_utils.points_in_img_filter(
points_in_img2_int, bgr_image.shape)
# image_mask = (points_in_img_int[0] >= 0) & (points_in_img_int[0] < bgr_image.shape[1]) & \
# (points_in_img_int[1] >= 0) & (points_in_img_int[1] < bgr_image.shape[0])
cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter]
# cam0_curr_points = cam0_curr_pc_padded[0:3].T
# points = points_cam2[image_mask]
points_in_img_int_valid = points_in_img2_int[:, image_filter]
point_colours = bgr_image[points_in_img_int_valid[1],
points_in_img_int_valid[0]]
else:
raise ValueError('Invalid point_cloud_source', point_cloud_source)
print('projection\t', time.time() - projection_start_time)
# Get pose
cam0_ref_pose = cam0_poses[frame_idx]
tf_cam0_ref_cam0_curr = cam0_ref_pose
# print('cam0_ref_pose\n', np.round(cam0_ref_pose, 3))
cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded
cam0_ref_points = cam0_ref_pc_padded[0:3].T
if render_interval > 1:
buffer_points.extend(cam0_ref_points)
buffer_colours.extend(point_colours)
if frame_idx % render_interval == 0 or frame_idx == (frame_range[1] -
1):
vtk_pc = VtkPointCloudGlyph()
vtk_pc.vtk_actor.GetProperty().SetPointSize(2)
vtk_renderer.AddActor(vtk_pc.vtk_actor)
vtk_pc_start_time = time.time()
if render_interval > 1:
vtk_pc.set_points(buffer_points, buffer_colours)
buffer_points = []
buffer_colours = []
else:
vtk_pc.set_points(cam0_ref_points, point_colours)
print('vtk_pc\t\t', time.time() - vtk_pc_start_time)
# Display pose
vtk_pc_pose = VtkPointCloud()
vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5)
vtk_pc_pose.set_points(np.reshape(cam0_ref_pose[0:3, 3], [-1, 3]))
vtk_renderer.AddActor(vtk_pc_pose.vtk_actor)
cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr.dot(
cam0_curr_vtk_cam_pos)
cam0_ref_vtk_focal_point = tf_cam0_ref_cam0_curr.dot(
cam0_curr_vtk_focal_point)
current_cam = vtk_renderer.GetActiveCamera()
vtk_renderer.ResetCamera()
current_cam.SetViewUp(0, -1, 0)
current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3])
current_cam.SetFocalPoint(*cam0_ref_vtk_focal_point[0:3])
current_cam.Zoom(0.5)
vtk_renderer.ResetCameraClippingRange()
# Render
render_start_time = time.time()
vtk_renderer.GetRenderWindow().Render()
print('render\t\t', time.time() - render_start_time)
all_render_times.append(time.time() - render_start_time)
print('---')
actor_buffer.append(vtk_pc.vtk_actor)
if len(actor_buffer) > 50:
if frame_idx % 10 != 0:
actor_buffer[frame_idx - 50].SetVisibility(0)
for vtk_actor in actor_buffer:
vtk_actor.SetVisibility(1)
print('Done')
import matplotlib.pyplot as plt
plt.plot(all_render_times)
plt.show(block=True)
# Keep window open
vtk_interactor.Start()