def __init__(self,
basedir,
transform=None,
gen_func=None,
args=None,
viz=False):
self.data = [
pykitti.tracking(basedir, sequence) for sequence in sequences
]
calib_locations = [
basedir + "calib/" + sequence + '.txt' for sequence in sequences
]
self.calibs = [
read_calib_file(calib_location)
for calib_location in calib_locations
]
self.R0 = [calibs_['R_rect'] for calibs_ in self.calibs]
self.R0 = [np.reshape(R0_, [3, 3]) for R0_ in self.R0]
self.V2C = [calibs_['Tr_velo_cam'] for calibs_ in self.calibs]
self.V2C = [np.reshape(V2C_, [3, 4]) for V2C_ in self.V2C]
self.C2V = [inverse_rigid_trans(V2C_) for V2C_ in self.V2C]
self.tracklets_list = populate_train_list(basedir)
self.transform = transform
self.gen_func = gen_func
self.num_points = args.num_points
self.remove_ground = args.remove_ground
self.viz = viz
def test_plot_bevs(self):
config_path = 'cfg_mini.yml'
config = load_config(config_path)
input_config = InputConfig(config['INPUT_CONFIG'])
train_config = TrainConfig(config['TRAIN_CONFIG'])
basepath = "/Users/erikbohnsack/data/training"
tracking = pykitti.tracking(base_path=basepath, sequence="0000")
points = tracking.get_velo(0)
bev_map = np.zeros(input_config.bev_shape)
print("BEV map size: {}".format(bev_map.shape))
grid_limits = np.array([[input_config.x_min, input_config.x_max],
[input_config.y_min, input_config.y_max],
[input_config.z_min, input_config.z_max]])
grid_sizes = np.array([
input_config.x_grid_size, input_config.y_grid_size,
input_config.z_grid_size
])
start_jit = time.time()
kitti_dataset.format_to_BEV_jit(bev_map, points, grid_limits,
grid_sizes)
end_jit = time.time()
plot_BEV(bev_map)
print("Elapsed time: {}".format(end_jit - start_jit))
def populate_train_list(basedir):
tracklets_list = []
for sequence in sequences:
data = pykitti.tracking(basedir, sequence)
data_dict = {}
for i in range(len(data)):
objects = data.get_objects(i)
for obj in objects:
if (obj.type == 'DontCare'):
continue
if (obj.track_id in data_dict.keys()):
data_dict[obj.track_id].append(i)
else:
data_dict[obj.track_id] = [i]
for key in data_dict.keys():
for i in range(len(data_dict[key]) - 1):
if (data_dict[key][i + 1] - data_dict[key][i] == 1):
tracklets_list.append([
data_dict[key][i], data_dict[key][i + 1], key, sequence
])
return tracklets_list
def __init__(self, basedir, drive):
# TODO: finish configurations for kitti tracking dataset
self.data = pykitti.tracking(basedir, drive)
self.original_shape = (1242, 375)
self.operate_folder_name = 'image_02'
self.image_folder_name = 'data'
self.image_ext = 'png'
self.dets_name = 'mrcnn_dts.txt'
self.mask_folder_name = 'mrcnn_masks'
self.depthmaps_name = "depthmap/disparities_pp.npy"
self.segment_folder_name = 'masks_seg'
self.tracking_name = 'tracking.txt'
self.dets_save_name = 'dets_3d'
self.dets_viz_save_name = 'dets_3d_viz'
def read_data(kitti_tracking_dir, annotation_files):
"""
function read data, lidar scan and images for each file in list of annotation_files
assumes that annotation files are in form returned by get_list_kitti_annotation_files function
assumes that every annotation file has a corresponding data in kitti_tracking_dir
assumes dataset used in kitti tracking dataset
:param
kitti_tracking_dir: directory that has kitti tracking data: lidar scans, images
annotation_files: list of annotation files to read the corresponding data of
:return
dataset: list of dictionary items for every annotation file
dictionary items consists of:
folder_type: string to indicate with data is for training or testing
seq_number: sequence number of this data
frame_id: id of frame of data
ground_truth: annotation of data, labels
image: image of seq_number and frame_id
velo_data: lidar scan of seq_number and frame_id
"""
dataset = []
for train_val,seq_number,frame_id,ground_truth in annotation_files:
#To-do: make it generic, reading from both from training / testing tracking dataset folders
basedir = os.path.join(kitti_tracking_dir,'training')
if not (os.path.isdir(basedir)):
continue
loaded = pykitti.tracking(basedir,seq_number)
if not loaded.velo_files:
print("cannot be loaded",basedir,seq_number)
continue
image = loaded.get_cam2(int(frame_id) )
image = np.array(image)
velo_data = loaded.get_velo(int(frame_id))
if velo_data.size == 0:
print("data could be get",seq_number,frame_id)
continue
dataset.append({
'folder_type': train_val,
'seq_number': seq_number,
'frame_id': frame_id,
'ground_truth': ground_truth,
'velo_data': velo_data,
'image': image
})
return dataset
def create_temporal_bevs_np_jit(CONF):
conf = InputConfig(CONF)
if platform.system() == 'Darwin':
basepath = "/Users/erikbohnsack/data/training"
else:
basepath = "/home/mlt/data/training"
velodyne_path = os.path.join(basepath, 'velodyne')
allfiles = os.listdir(velodyne_path)
sequences = [
fname for fname in allfiles if not fname.endswith('.DS_Store')
]
grid_limits = np.array([[conf.x_min, conf.x_max], [conf.y_min, conf.y_max],
[conf.z_min, conf.z_max]])
grid_sizes = np.array(
[conf.x_grid_size, conf.y_grid_size, conf.z_grid_size])
for sequence in sequences:
tracking = pykitti.tracking(base_path=basepath, sequence=sequence)
nof_frames = len(tracking.velo_files)
bev_maps = []
for frame in tqdm(range(0, nof_frames)):
points = tracking.get_velo(frame)
bev_map = np.zeros(conf.bev_shape)
kitti_dataset.format_to_BEV_jit(bev_map, points, grid_limits,
grid_sizes)
print("Frame: {}, bev map shape {}".format(frame, bev_map.shape))
bev_maps.append(bev_map)
bev_path = os.path.join(os.path.join(basepath, 'bevs'), sequence)
if not os.path.exists(bev_path):
os.mkdir(bev_path)
for i in range(conf.num_conseq_frames - 1,
nof_frames - conf.num_conseq_frames + 1):
conc_bev_map = np.concatenate(
(bev_maps[i - conf.num_conseq_frames + 1:i + 1]), 2)
print("Conc bev map shape {}".format(conc_bev_map.shape))
assert np.shape(
conc_bev_map)[2] == conf.num_conseq_frames * conf.bev_shape[2]
# For saving/loading pytorch tensors:
# torch.save(tensor, 'file.pt') and torch.load('file.pt')
filename = os.path.join(bev_path, str(i).zfill(6) + '.pt')
torch.save(torch.from_numpy(conc_bev_map), filename)
def create_temporal_bevs(CONF):
conf = InputConfig(CONF)
if platform.system() == 'Darwin':
basepath = "/Users/erikbohnsack/data/training"
else:
basepath = "/home/mlt/data/training"
velodyne_path = os.path.join(basepath, 'velodyne')
allfiles = os.listdir(velodyne_path)
sequences = [
fname for fname in allfiles if not fname.endswith('.DS_Store')
]
for sequence in sequences:
tracking = pykitti.tracking(base_path=basepath, sequence=sequence)
nof_frames = len(tracking.velo_files)
bev_maps = []
for frame in tqdm(range(0, nof_frames)):
points = tracking.get_velo(frame)
bev_map = torch.zeros(conf.bev_shape)
kitti_dataset.format_to_BEV(bev_map, points, conf)
bev_maps.append(bev_map)
bev_path = os.path.join(os.path.join(basepath, 'bevs'), sequence)
if not os.path.exists(bev_path):
os.mkdir(bev_path)
for i in range(conf.num_conseq_frames - 1,
nof_frames - conf.num_conseq_frames + 1):
conc_bev_map = torch.cat(
(bev_maps[i - conf.num_conseq_frames + 1:i + 1]), 2)
assert np.shape(
conc_bev_map)[2] == conf.num_conseq_frames * conf.bev_shape[2]
# For saving/loading pytorch tensors:
# torch.save(tensor, 'file.pt') and torch.load('file.pt')
filename = os.path.join(bev_path, str(i).zfill(6) + '.pt')
torch.save(conc_bev_map, filename)
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 read_data(self):
"""
function read data, lidar scan and images for each file in list of annotation_files
assumes that annotation files are in form returned by get_annoatation_files function
assumes that every annotation file has a corresponding data in kitti_tracking_dir
assumes dataset used in kitti tracking dataset
:param
kitti_tracking_dir: directory that has kitti tracking data: lidar scans, images
annotation_files: list of annotation files to read the corresponding data of
:return
dataset: list of dictionary items for every annotation file
dictionary items consists of:
folder_type: string to indicate with data is for training or testing
seq_number: sequence number of this data
frame_id: id of frame of data
ground_truth: annotation of data, labels
image: image of seq_number and frame_id
velo_data: lidar scan of seq_number and frame_id
"""
# read zhang annotation files
self.get_lidar_annotation_files()
for annotation_file in tqdm(self.lidar_annotation_files):
train_val = annotation_file['train_val']
seq_number = annotation_file["sequence_number"]
frame_id = annotation_file["frame_id"]
lidar_ground_truth = annotation_file["lidar_ground_truth"]
#To-do: make it generic, reading from both from training / testing tracking dataset folders
# basedir = os.path.join(kitti_tracking_dir,'training')
basedir = self.kitti_tracking_dir / 'training'
if not basedir.exists():
print("kitti tracking directory does not exist {}".format(
str(basedir)))
break
loaded = pykitti.tracking(str(basedir), seq_number)
if not loaded.velo_files:
print("cannot be loaded", str(basedir), seq_number)
continue
image = loaded.get_cam2(int(frame_id))
image = np.array(image)
velo_data = loaded.get_velo(int(frame_id))
tr_velo_matrix = self.get_translation_rotation_Velo_matrix(
seq_number)
R_rect_00_matrix = self.get_rectified_cam0_coord(seq_number)
P_rect_matrix = self.get_projection_rect(seq_number, mode='02')
if velo_data.size == 0:
print("data could be get", seq_number, frame_id)
continue
self.dataset.append({
'folder_type': train_val,
'seq_number': seq_number,
'frame_id': frame_id,
'lidar_ground_truth': lidar_ground_truth,
'velo_data': velo_data,
'image': image,
"tr_velo_matrix": tr_velo_matrix,
"R_rect_00_matrix": R_rect_00_matrix,
"P_rect_matrix": P_rect_matrix,
})
return self.dataset
import pykitti
__author__ = "Lee Clement"
__email__ = "*****@*****.**"
# Change this to the directory where you store KITTI data
basedir = "/mnt/sakuradata2/datasets/kitti/tracking"
# Specify the dataset to load
sequence = "0000"
# Optionally, specify the frame range to load
frame_range = range(0, 20, 5)
# Load the data
dataset = pykitti.tracking(basedir, sequence, frame_range)
# Load image data
dataset.load_rgb(format='cv2') # Loads images as uint8 with BGR ordering
dataset.load_oxts()
# Do some stereo processing
stereo = cv2.StereoBM_create(numDisparities=64, blockSize=15)
disp_rgb = stereo.compute(
cv2.cvtColor(dataset.rgb[0].left, cv2.COLOR_BGR2GRAY),
cv2.cvtColor(dataset.rgb[0].right, cv2.COLOR_BGR2GRAY))
# Display some data
f, ax = plt.subplots(2, 1) #, figsize=(15, 5))
ax[0].imshow(cv2.cvtColor(dataset.rgb[0].left, cv2.COLOR_BGR2RGB))
# Chose which visualization library to use: "mayavi" or "matplotlib"
VISLIB = "mayavi"
# VISLIB = "matplotlib"
# Raw Data directory information
basedir = '/home/dodo_brain/kitti_data/'
date = '2011_09_26'
drive = '0005'
# Optionally, specify the frame range to load
# since we are only visualizing one frame, we will restrict what we load
# Set to None to use all the data
frame_range = range(150, 151, 1)
# Load the data
dataset = pykitti.tracking(basedir, date, drive, frame_range)
# Load Lidar Data
dataset.load_velo() # Each scan is a Nx4 array of [x,y,z,reflectance]
# Plot only the ith frame (out of what has been loaded)
i = 0
velo = dataset.velo[i]
if VISLIB == "mayavi":
# Plot using mayavi -Much faster and smoother than matplotlib
import mayavi.mlab
fig = mayavi.mlab.figure(bgcolor=(0, 0, 0), size=(640, 360))
mayavi.mlab.points3d(
velo[:, 0], # x
