def gen_weights_and_poses(sequence, fast_threshold=150):
dataset = pykitti.odometry(basedir_odom, sequence, imformat='cv2')
print("Dataset loaded")
T_calib = dataset.calib.T_cam0_velo
T_calib = np.linalg.inv(T_calib)
x = []
y = []
z = []
w = []
camera_normal = np.zeros((0, 3))
# print("Dateset lengths: ", sum(1 for _ in dataset.cam2), sum(1 for _ in dataset.poses))
count = 0
for T in dataset.poses:
T = T_calib.dot(T)
x.append(T[0, 3])
y.append(T[1, 3])
z.append(T[2, 3])
camera_normal = np.vstack((camera_normal, T[0:3, 2].T))
# Didn't like the zip, had to do them separately
for img in dataset.cam2:
# find the keypoints with ORB
orb = cv2.ORB_create(nfeatures=10000, fastThreshold=fast_threshold)
kp = orb.detect(img, None)
w.append(len(kp))
# count += 1
# if count % 100 == 0:
# print("Count: ", count)
x = np.array(x)
y = np.array(y)
z = np.array(z)
yaw = np.arctan2(camera_normal[:, 1], camera_normal[:, 0])
return x, y, z, yaw, w
def __init__(self, config: dict):
# Set Ground Truth and Velocity
if config['name'] == 'kitti_raw':
self.resize_factor = config['resize_factor']
self.dataset = pykitti.raw(config['basedir'], config['date'],
config['drive'])
self.image_getter_left = iter(self.dataset.cam0)
self.velocity_getter = ([
oxt.packet.vl, oxt.packet.vu, oxt.packet.vf
] for oxt in self.dataset.oxts)
self.true_pose_getter = ([
oxt.T_w_imu[0][3], oxt.T_w_imu[1][3], oxt.T_w_imu[2][3]
] for oxt in self.dataset.oxts)
self.k = self.dataset.k = self.resize_factor * self.dataset.calib.K_cam0
self.k[2][2] = 1
self.inv_k = np.linalg.inv(self.k)
self.focal = self.k[0][0]
self.pp = (self.k[0][2], self.k[1][2])
if config['name'] == 'kitti_odometry':
self.resize_factor = config['resize_factor']
self.dataset = pykitti.odometry(config['basedir'],
config['sequence'])
self.image_getter_left = iter(self.dataset.cam0)
self.image_getter_right = iter(self.dataset.cam1)
self.true_pose_getter = ([pose[0][3], pose[1][3], pose[2][3]]
for pose in self.dataset.poses)
self.k = self.dataset.k = self.resize_factor * self.dataset.calib.K_cam0
self.k[2][2] = 1
self.inv_k = np.linalg.inv(self.k)
self.focal = self.k[0][0]
self.pp = (self.k[0][2], self.k[1][2])
def __init__(self, path2odometry, sequence):
if isinstance(sequence, int):
sequence = str(sequence).zfill(2)
self.datainfo = pykitti.odometry(path2odometry, sequence)
self.br = tf.TransformBroadcaster()
def __init__(self, cfg, base_dir, seq, frames=None):
if seq == "03" or seq not in self.raw_path.keys():
raise ValueError("%s sequence not supported" % seq)
self.cfg = cfg
self.imu_time_sync_shift = 1 # we discovered IMU seems to be delayed, so we shifted it forward
if frames:
range_start = self.raw_range[seq][0] + frames.start
range_stop = range_start + frames.stop
else:
range_start = self.raw_range[seq][0]
range_stop = self.raw_range[seq][1] + 1
self.data_raw_kitti = pykitti.raw(base_dir, self.raw_path[seq][0], self.raw_path[seq][1],
frames=range(range_start, range_stop))
self.data_imu_kitti = pykitti.raw(base_dir, self.raw_path[seq][0], self.raw_path[seq][1],
frames=range(range_start + self.imu_time_sync_shift,
range_stop + self.imu_time_sync_shift))
self.data_odom_kitti = pykitti.odometry(base_dir, seq, frames=frames)
self.data_lidar_image = LidarDataLoader(self.cfg, base_dir, seq, frames=frames)
assert (len(self.data_raw_kitti.oxts) == len(self.data_odom_kitti.poses) and
len(self.data_imu_kitti.oxts) == len(self.data_odom_kitti.poses) and
len(self.data_odom_kitti.poses) == self.data_lidar_image.num_frames)
self.imu_measurements = []
self.imu_measurements_reversed = []
self.__load_imu()
def __init__(self,
sequence,
root='E:\\share_folder\\dataset',
transform=None,
pre_transform=None):
super().__init__(root, transform, pre_transform)
self.dataset = pykitti.odometry(root, sequence, frames=None)
def _create_drive_loader(self, drive_path):
drive = op.basename(drive_path)
pose_file = op.join(self.base_path, "poses", drive + ".txt")
self.poses = np.loadtxt(pose_file)
print(
f"[_create_drive_loader] drive: {drive}, pose avail: {self.pose_avail}, depth avail: {self.depth_avail}"
)
return pykitti.odometry(self.base_path, drive)
def loadImageDataset(self):
'''Load the data. Optionally, specify the frame range to load'''
# PyKITTI object for
self.image_dataset = pykitti.odometry(self.raw_path,
self.sequence_id,
frames=range(0, 20, 2))
print("Length of dataset: %s" % str(len(self.image_dataset)))
def main(sequence, results_dir, kitti_data_dir, start_end_increment=None):
poses_name = results_dir+"/{}.txt".format(sequence)
if start_end_increment not is None: # Get range of data.
r_start = start_end_increment[0]
r_end = start_end_increment[1]
r_incr = start_end_increment[2]
plot_range = range(r_start, r_end, r_incr)
data = pykitti.odometry(kitti_data_dir, sequence, frames=plot_range)
def draw_gt(seq):
x = []
y = []
odom = pykitti.odometry(KITTI_PATH, seq)
for i in range(len(odom)):
t = se3_to_position(odom.poses[i])
x.append(t[0])
y.append(t[2])
plt.plot(x, y, color="g", label="ground truth")
def test(model,
dataloader,
device,
epoch,
draw_seq=None,
path=KITTI_PATH,
test_seq=TEST_SEQ):
model.eval()
answer = [[0.0] * 6]
gt = []
odom = pykitti.odometry(path, test_seq[0])
traj = []
cur_R = np.eye(3)
cur_t = np.zeros((3, 1))
for i in range(len(odom)):
gt.append(se3_to_position(odom.poses[i]))
for i, batch in enumerate(dataloader):
seq, _, _ = batch
seq = seq.to(device)
predicted = model(seq)
predicted = predicted.data.cpu().numpy()
if i == 0:
for pose in predicted[0]:
for i in range(len(pose)):
pose[i] = pose[i] + answer[-1][i]
answer.append(pose.tolist())
traj.append(
np.concatenate((cur_R.copy(), cur_t.copy()), axis=1).flatten())
predicted = predicted[1:]
for poses in predicted:
ang = eulerAnglesToRotationMatrix([0, answer[-1][0], 0])
location = ang.dot(poses[-1][3:])
poses[-1][3:] = location[:]
last_pose = poses[-1]
ang_full = eulerAnglesToRotationMatrix(
[last_pose[1], last_pose[0], last_pose[2]])
cur_R = ang_full @ cur_R
for j in range(len(last_pose)):
last_pose[j] = last_pose[j] + answer[-1][j]
cur_t = np.array(last_pose[3:]).reshape((3, 1))
last_pose[0] = (last_pose[0] + np.pi) % (2 * np.pi) - np.pi
answer.append(last_pose.tolist())
traj.append(
np.concatenate((cur_R.copy(), cur_t.copy()), axis=1).flatten())
if draw_seq:
np.savetxt(f"{RESULT_FOLDER}/{draw_seq}_{epoch}_pred.txt",
traj,
fmt="%1.8f")
else:
np.savetxt(f"{RESULT_FOLDER}/{DRAW_SEQ}_{epoch}_pred.txt",
traj,
fmt="%1.8f")
def __init__(self, path: str, sequence: str) -> None:
"""
Args:
path: Path to KITTI Odometry dataset
sequence: sequence number
"""
super().__init__(path, path)
self.sequence = sequence
self._kitti = pykitti.odometry(path, sequence)
self.camera_params = self.getCameraParameters()
def init_session(self, session_ind):
try:
requested_sequence_path = self.get_session_path_raw(session_ind)
if self.dataset.sequence_path == requested_sequence_path:
return
except:
pass
sequence = "%02d" % session_ind
self.dataset = pykitti.odometry(self.dataset_directory(), sequence)
def __init__(self,
frames,
sequence="08",
directory="datasets",
download=False):
sequence = Downloader(sequence, directory)
if download and not os.path.exists(os.path.join(directory, "poses")):
print("Download datasets")
sequence.download_sequence()
self._kitty_dataset = pykitti.odometry(sequence.main_dir,
sequence.sequence_id,
frames=frames)
def sequence(self, i):
if 0 <= i < 11:
sequence = pykitti.odometry(self.ododir, '{:02d}'.format(i))
sequence.poses = numpy.array(sequence.poses)
else:
drive = kittidrives.drives[i - 11]
sequence = pykitti.raw(self.rawdir, drive['date'], drive['drive'])
T_imu_cam0 = util.invert_ht(sequence.calib.T_cam0_imu)
sequence.poses = numpy.array(
[numpy.matmul(oxts.T_w_imu, T_imu_cam0) \
for oxts in sequence.oxts])
return sequence
def get_kitti_odometry(dataset, sequence):
data = pykitti.odometry(dataset, sequence)
n_scans = len(data)
T_cam_velo = data.calib.T_cam0_velo
T_velo_cam = np.linalg.inv(T_cam_velo)
gt_poses = T_velo_cam @ data.poses @ T_cam_velo
timestamps = [dt.total_seconds() for dt in data.timestamps]
# Hack: Add 1 nano sec in the first timestamp, ros python has bug in the
# implementation, this doesn't happen in the C++ one
timestamps[0] += 1e-9
return n_scans, gt_poses, timestamps, data.velo
def load_poses():
directoryL = "/home/sexy/Documents/dataset/"
list_Y = []
for i in range(11):
data = pykitti.odometry(directoryL, str(i).zfill(2))
data.load_poses()
Y = np.zeros((len(data.T_w_cam0), 3))
for j in range(len(data.T_w_cam0)):
Y[j, :] = data.T_w_cam0[j][:-1, -1]
delta_Y = np.zeros((len(data.T_w_cam0), 3))
delta_Y[1:, :] = Y[1:, :] - Y[:-1, :]
list_Y.append(delta_Y)
return list_Y
def get_datasets(args):
seq_velo_files = []
seq_poses = []
for seq in args.sequences:
data = pykitti.odometry(args.base_dir, seq, frames=None)
seq_velo_files.extend(data.velo_files)
seq_poses.extend(
[data.poses[pose_i] for pose_i in range(data.poses.shape[0])])
orig_dset = ptlk.data.datasets.KITTIVelo(seq_velo_files, seq_poses,
args.num_points, args.partition)
trainset, testset = orig_dset.split()
return trainset, testset
def __init__(
self,
seq,
original=False,
path=KITTI_PATH,
transform=kittiTransform,
left=True,
stereo=False,
):
self.odom = pykitti.odometry(path, seq)
self.transform = transform
self.left = left
self.original = original
def __init__(
self,
seq,
path=KITTI_PATH,
transform=kittiTransform,
normalizer=normalizer,
left=True,
stereo=False,
):
self.odom = pykitti.odometry(path, seq)
self.transform = transform
self.left = left
self.normalizer = normalizer
def __init__(self, cfg, base_dir, seq, frames=None):
pickles_dir = config.lidar_pickles_path
seq_data = pykitti.odometry(base_dir, seq)
self.num_frames = len(seq_data.poses)
self.data = np.zeros([
self.num_frames, cfg.input_channels, cfg.input_height,
cfg.input_width
],
dtype=np.float16)
with (open(os.path.join(pickles_dir, seq + "_range.pik"),
"rb")) as opfile:
i = 0
while True:
try:
cur_image = pickle.load(opfile)
self.data[i, 0, :, :] = cur_image
except EOFError:
break
i += 1
if i % 1000 == 0:
tools.printf("Loading lidar range seq. %s %.1f%% " %
(seq, (i / self.num_frames) * 100))
assert (i == self.num_frames)
with (open(os.path.join(pickles_dir, seq + "_intensity.pik"),
"rb")) as opfile:
i = 0
while True:
try:
cur_image = pickle.load(opfile)
self.data[i, 1, :, :] = cur_image
self.data[i, 1, :, :] = np.divide(self.data[i, 1, :, :],
255.0,
dtype=np.float16)
self.data[i, 1, :, :] = np.subtract(self.data[i, 1, :, :],
0.5,
dtype=np.float16)
except EOFError:
break
i += 1
if i % 1000 == 0:
tools.printf("Loading lidar intensity seq. %s %.1f%% " %
(seq, (i / self.num_frames) * 100))
assert (i == self.num_frames)
# select the range of frames
if frames:
self.data = self.data[frames]
self.num_frames = self.data.shape[0]
def __init__(self, N, std_w, std_v, gamma, bias_w_std):
self.N = N
self.dt = 1/20
self.max_vis = 60.0
self.std_w = std_w
self.std_v = std_v
self.bias_w_std = bias_w_std
self.gamma = gamma
# fraction of particles to project onto road
self.alpha = 0.0
# probability of detection of a point
self.rho = 0.6
# P0 is a design parameter specifying the probability that a visible map point is occluded
self.P0 = 0.2
# design PMF pr of detection d under not occluded and the map detections D
# pr_d = vector of probabilities for each class find via validation class mismatches? detector property?
self.ROOT_DIR = os.path.abspath('')
self.results_path = os.path.join(self.ROOT_DIR, "results/_results.txt")
self.instances_path = os.path.join(self.ROOT_DIR, "content/kitti_dataset/dataset/sequences/08/instances_2")
self.CNN_PMF_PATH = os.path.join(self.ROOT_DIR, 'results/PMFs')
self.CLASS_MARGINAL = os.path.join(self.ROOT_DIR, 'detection_probabilities')
self.class_marginal = np.load(self.CLASS_MARGINAL + '.npy')
self.basedir = 'content/kitti_dataset/dataset'
self.sequence = '08'
self.CNN_PMF_PATH = os.path.join(self.ROOT_DIR, 'results/PMFs')
self.cnn_pmf = np.load(self.CNN_PMF_PATH + '.npy') # one row is the pmf of the cnn detecting the class, when the gt corresponding to the row is present
self.dataset = pykitti.odometry(self.basedir, self.sequence)
self.Kmat = self.dataset.calib.P_rect_20
self.T_w0_w = np.array([[0., 0., 1., 0.],
[-1., 0., 0., 0.],
[0., -1., 0., 0.],
[0., 0., 0., 1.]])
self.T_cam0_cam2 = np.linalg.inv(self.dataset.calib.T_cam0_velo).dot(self.dataset.calib.T_cam2_velo)
with open("results.json") as json_file:
all_data = json.load(json_file)
# sorting alldata for image_id
all_data = all_data['results']
self.all_data_sort = []
for x in sorted(all_data, key=itemgetter('image_id')):
self.all_data_sort.append(x)
self.rescale_factor = 5
self.prob_mask = np.zeros((int(370 / self.rescale_factor), int(1226 / self.rescale_factor), 17))
self.previous_image = None
print(self.class_marginal)
def convert_odom(sequence,
color,
input_dir='.',
output_dir='.',
compression=rosbag.Compression.NONE):
"""Convert an odometry KITTI dataset to a bag"""
sequence_str = str(sequence).zfill(2)
kitti = pykitti.odometry(input_dir, sequence_str)
if len(kitti.timestamps) == 0:
print('Dataset is empty? Exiting.', file=sys.stderr)
sys.exit(1)
# The odom timestamps are relative, add an arbitrary datetime to make them absolute
base_timestamp = datetime(2011, 9, 26, 12, 0, 0)
timestamps = [base_timestamp + timestamp for timestamp in kitti.timestamps]
if 0 <= sequence < 10:
print(
"Odometry dataset sequence {} has ground truth information (poses)."
.format(sequence_str))
if color == 'color':
camera_nrs = (2, 3)
elif color == 'gray':
camera_nrs = (0, 1)
else:
camera_nrs = list(range(4))
# Export
bag_name = "kitti_data_odometry_{}_sequence_{}.bag".format(
color, sequence_str)
with rosbag.Bag(os.path.join(output_dir, bag_name),
'w',
compression=compression) as bag:
save_dynamic_tf(bag, timestamps, kitti.poses,
rectified_camera_frame_id)
for camera_nr in camera_nrs:
save_camera_data(bag, kitti, cameras[camera_nr], timestamps)
print("## OVERVIEW ##")
print(bag)
def buildSE3(self, sourceImageId, destImageId):
image_range = range(sourceImageId, destImageId, 1)
dataset = pykitti.odometry(self.vo_path, self.sequence, frames=image_range)
SE3_i = next(itertools.islice(dataset.poses, sourceImageId, None))
SE3_j = next(itertools.islice(dataset.poses, destImageId, None))
R_i_trans = np.transpose(SE3_i[0:3,0:3])
R_ij = np.matmul(SE3_j[0:3,0:3],R_i_trans)
t_i_c = np.matmul(R_i_trans,SE3_i[0:3,3])
t_j_c = np.matmul(R_i_trans,SE3_j[0:3,3])
t_ij = t_j_c - t_i_c
# t_ij = np.matmul(SE3_i[0:3,0:3],t_ij) # Account for rotation of the camera
# t_ij[2] *= -1
se3 = np.identity(4)
se3[0:3,0:3] = R_ij
se3[0:3,3] = t_ij
return se3
def __init__(self, config, base_dir, seq, frames=None):
pickles_dir = os.path.join(base_dir, "sequences", "lidar_pickles")
seq_data = pykitti.odometry(base_dir, seq)
num_frames = len(seq_data.poses)
self.data = np.zeros(
[num_frames, 2, config.input_height, config.input_width],
dtype=np.float16)
with (open(os.path.join(pickles_dir, seq + "_range.pik"),
"rb")) as opfile:
i = 0
while True:
try:
cur_image = pickle.load(opfile)
self.data[i, 0, :, :] = cur_image
except EOFError:
break
i += 1
assert (i == num_frames)
with (open(os.path.join(pickles_dir, seq + "_intensity.pik"),
"rb")) as opfile:
i = 0
while True:
try:
cur_image = pickle.load(opfile)
self.data[i, 1, :, :] = cur_image
self.data[i, 1, :, :] = np.divide(self.data[i, 1, :, :],
254.0,
dtype=np.float16)
self.data[i, 1, :, :] = np.subtract(self.data[i, 1, :, :],
0.5,
dtype=np.float16)
except EOFError:
break
i += 1
assert (i == num_frames)
if frames:
self.data = self.data[frames]
def __init__(self, config):
path_to_dataset = config["path_to_dataset"]
sequence = config["seq"]
self.path_to_map_folder = config["path_to_map_folder"] + "/" + sequence
weight_paths = config["weight_paths"]
# setup kitti manager
self.kitti = pykitti.odometry(path_to_dataset, sequence)
# load the downsampled map
start_map_num =config["start_frame_num"]//300
self.map, self.map_intensity = self.load_map(self.path_to_map_folder+"/{}.pcd".format(start_map_num))
# load the transformations
velo2cam2 = torch.from_numpy(self.kitti.calib.T_cam2_velo).float().to("cuda")
self.velo2cam = velo2cam2
self.cam2velo = self.velo2cam.inverse()
# load the models into a list
self.image_shape = (384, 1280)
self.models = self.load_models(weight_paths)
# initialize a camera model used to project lidar point cloud
self.cam_params = self.get_calib_kitti(sequence).cuda()
self.camera_model = CameraModel(
focal_length=self.cam_params[:2], principal_point=self.cam_params[2:]
)
def plot_odometry_trajectory(sequence, downsample=10):
dataset = pykitti.odometry(basedir_odom, sequence)
# pose = next(iter(itertools.islice(dataset.poses, 1, None)))
# print(dataset.calib.T_cam0_velo)
T_calib = dataset.calib.T_cam2_velo
T_calib = np.linalg.inv(T_calib)
x = []
y = []
z = []
# yaw = []
camera_normal = np.zeros((0, 3))
for T in dataset.poses:
T = T_calib.dot(T)
x.append(T[0, 3])
y.append(T[1, 3])
z.append(T[2, 3])
# Yaw not useful here (it's the camera's orientation)
# yaw.append(np.arctan2(-T[0,1], T[0,0]))
camera_normal = np.vstack((camera_normal, T[0:3, 2].T))
x = np.array(x)
y = np.array(y)
z = np.array(z)
# yaw = np.array(yaw)
yaw = np.arctan2(camera_normal[:, 1], camera_normal[:, 0])
plot_traj(x, y, yaw, downsample)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(x, y, z)
inds = range(0, len(x), downsample)
# ax.quiver(x[inds], y[inds], z[inds], cos(yaw[inds]), sin(yaw[inds]), 0*yaw[inds])
ax.quiver(x[inds],
y[inds],
z[inds],
camera_normal[inds, 0],
camera_normal[inds, 1],
camera_normal[inds, 2],
color='r')
plt.axis('equal')
plt.show()
def main(dataset, out_dir, sequence, use_intensity):
"""Utility script to convert from the binary form found in the KITTI
odometry dataset to .ply files. The intensity value for each measurement is
encoded in the color channel of the output PointCloud.
If a given sequence it's specified then it assumes you have a clean copy of
the KITTI odometry benchmark, because it uses pykitti. If you only have a
folder with just .bin files the script will most likely fail.
If no sequence is specified then it blindly reads all the *.bin file in the
specified dataset directory
"""
print(
"Converting .bin scans into .ply fromat from:{orig} to:{dest}".format(
orig=dataset, dest=out_dir))
os.makedirs(out_dir, exist_ok=True)
if sequence:
# Means KITTI like format
base_path = os.path.join(out_dir, "sequences", sequence, "velodyne",
"")
os.makedirs(base_path, exist_ok=True)
data = pykitti.odometry(dataset, sequence)
velo_files = data.velo_files
scans = data.velo
else:
# Read all the *.bin scans from the dataset folder
base_path = os.path.join(out_dir, "")
os.makedirs(base_path, exist_ok=True)
velo_files = sorted(glob.glob(os.path.join(dataset, "*.bin")))
scans = yield_velo_scans(velo_files)
for points, scan_name in tqdm(zip(scans, velo_files),
total=len(velo_files)):
pcd = vel2ply(points, use_intensity)
stem = os.path.splitext(scan_name.split("/")[-1])[0]
filename = base_path + stem + ".ply"
o3d.io.write_point_cloud(filename, pcd)
def main(dataset, out_dir, sequence, n_scans, start, visualize, normals):
"""From a set of input PointClouds create one unique aggreated map cloud."""
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Info)
dataset = os.path.join(dataset, "")
dataset_name = Path(dataset).parent.name
data = pykitti.odometry(dataset, sequence)
T_cam_velo = data.calib.T_cam0_velo
T_velo_cam = np.linalg.inv(T_cam_velo)
gt_poses = T_velo_cam @ data.poses @ T_cam_velo
scans = os.path.join(dataset, "sequences", sequence, "velodyne", "")
scan_names = sorted(glob.glob(scans + "*.ply"))
# Use the whole sequence if -1 is specified
n_scans = len(scan_names) if n_scans == -1 else n_scans
print("Processing " + str(n_scans) + " in " + scans)
cloud_map = o3d.geometry.PointCloud()
for idx in tqdm(range(start, start + n_scans)):
source = preprocess_cloud(
o3d.io.read_point_cloud(scan_names[idx]),
normals=normals,
)
source.transform(gt_poses[idx])
cloud_map += source
map_name = ("gt_" + str(n_scans) + "_scans_" + sequence + "_" +
dataset_name + ".ply")
out_file = os.path.join(out_dir, map_name)
print("saving map to " + out_file)
o3d.io.write_point_cloud(out_file, cloud_map)
if visualize:
o3d.visualization.draw_geometries([cloud_map])
import pykitti __author__ = "Lee Clement" __email__ = "*****@*****.**" # Change this to the directory where you store KITTI data basedir = '/Users/leeclement/Desktop/KITTI/odometry/dataset' # Specify the dataset to load sequence = '01' # Load the data. Optionally, specify the frame range to load. # Passing imformat='cv2' will convert images to uint8 and BGR for # easy use with OpenCV. # dataset = pykitti.odometry(basedir, sequence) dataset = pykitti.odometry(basedir, sequence, frames=range(0, 20, 5)) # dataset.calib: Calibration data are accessible as a named tuple # dataset.timestamps: Timestamps are parsed into a list of timedelta objects # dataset.poses: Generator to load ground truth poses T_w_cam0 # dataset.camN: Generator to load individual images from camera N # dataset.gray: Generator to load monochrome stereo pairs (cam0, cam1) # dataset.rgb: Generator to load RGB stereo pairs (cam2, cam3) # dataset.velo: Generator to load velodyne scans as [x,y,z,reflectance] # Grab some data second_pose = next(itertools.islice(dataset.poses, 1, None)) first_gray = next(dataset.gray) first_cam1 = next(dataset.cam1) first_rgb = next(dataset.rgb) first_cam2 = next(dataset.cam2)
def run_kitti2bag():
parser = argparse.ArgumentParser(
description="Convert KITTI dataset to ROS bag file the easy way!")
# Accepted argument values
kitti_types = ["raw_synced", "odom_color", "odom_gray"]
odometry_sequences = []
for s in range(22):
odometry_sequences.append(str(s).zfill(2))
parser.add_argument("kitti_type",
choices=kitti_types,
help="KITTI dataset type")
parser.add_argument(
"dir",
nargs="?",
default=os.getcwd(),
help=
"base directory of the dataset, if no directory passed the deafult is current working directory"
)
parser.add_argument(
"-t",
"--date",
help=
"date of the raw dataset (i.e. 2011_09_26), option is only for RAW datasets."
)
parser.add_argument(
"-r",
"--drive",
help=
"drive number of the raw dataset (i.e. 0001), option is only for RAW datasets."
)
parser.add_argument(
"-s",
"--sequence",
choices=odometry_sequences,
help=
"sequence of the odometry dataset (between 00 - 21), option is only for ODOMETRY datasets."
)
args = parser.parse_args()
bridge = CvBridge()
compression = rosbag.Compression.NONE
# compression = rosbag.Compression.BZ2
# compression = rosbag.Compression.LZ4
# CAMERAS
cameras = [(0, 'camera_gray_left', '/kitti/camera_gray_left'),
(1, 'camera_gray_right', '/kitti/camera_gray_right'),
(2, 'camera_color_left', '/kitti/camera_color_left'),
(3, 'camera_color_right', '/kitti/camera_color_right')]
if args.kitti_type.find("raw") != -1:
if args.date == None:
print("Date option is not given. It is mandatory for raw dataset.")
print(
"Usage for raw dataset: kitti2bag raw_synced [dir] -t <date> -r <drive>"
)
sys.exit(1)
elif args.drive == None:
print(
"Drive option is not given. It is mandatory for raw dataset.")
print(
"Usage for raw dataset: kitti2bag raw_synced [dir] -t <date> -r <drive>"
)
sys.exit(1)
bag = rosbag.Bag("kitti_{}_drive_{}_{}.bag".format(
args.date, args.drive, args.kitti_type[4:]),
'w',
compression=compression)
kitti = pykitti.raw(args.dir, args.date, args.drive)
if not os.path.exists(kitti.data_path):
print('Path {} does not exists. Exiting.'.format(kitti.data_path))
sys.exit(1)
if len(kitti.timestamps) == 0:
print('Dataset is empty? Exiting.')
sys.exit(1)
try:
# IMU
imu_frame_id = 'imu_link'
imu_topic = '/kitti/oxts/imu'
gps_fix_topic = '/kitti/oxts/gps/fix'
gps_vel_topic = '/kitti/oxts/gps/vel'
velo_frame_id = 'velo_link'
velo_topic = '/kitti/velo'
T_base_link_to_imu = np.eye(4, 4)
T_base_link_to_imu[0:3, 3] = [-2.71 / 2.0 - 0.05, 0.32, 0.93]
# tf_static
transforms = [
('base_link', imu_frame_id, T_base_link_to_imu),
(imu_frame_id, velo_frame_id, inv(kitti.calib.T_velo_imu)),
(imu_frame_id, cameras[0][1], inv(kitti.calib.T_cam0_imu)),
(imu_frame_id, cameras[1][1], inv(kitti.calib.T_cam1_imu)),
(imu_frame_id, cameras[2][1], inv(kitti.calib.T_cam2_imu)),
(imu_frame_id, cameras[3][1], inv(kitti.calib.T_cam3_imu))
]
util = pykitti.utils.read_calib_file(
os.path.join(kitti.calib_path, 'calib_cam_to_cam.txt'))
# Export
save_static_transforms(bag, transforms, kitti.timestamps)
save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=None)
save_imu_data(bag, kitti, imu_frame_id, imu_topic)
save_gps_fix_data(bag, kitti, imu_frame_id, gps_fix_topic)
save_gps_vel_data(bag, kitti, imu_frame_id, gps_vel_topic)
for camera in cameras:
save_camera_data(bag,
args.kitti_type,
kitti,
util,
bridge,
camera=camera[0],
camera_frame_id=camera[1],
topic=camera[2],
initial_time=None)
save_velo_data(bag, kitti, velo_frame_id, velo_topic)
finally:
print("## OVERVIEW ##")
print(bag)
bag.close()
elif args.kitti_type.find("odom") != -1:
if args.sequence == None:
print(
"Sequence option is not given. It is mandatory for odometry dataset."
)
print(
"Usage for odometry dataset: kitti2bag {odom_color, odom_gray} [dir] -s <sequence>"
)
sys.exit(1)
bag = rosbag.Bag("kitti_data_odometry_{}_sequence_{}.bag".format(
args.kitti_type[5:], args.sequence),
'w',
compression=compression)
kitti = pykitti.odometry(args.dir, args.sequence)
if not os.path.exists(kitti.sequence_path):
print('Path {} does not exists. Exiting.'.format(
kitti.sequence_path))
sys.exit(1)
kitti.load_calib()
kitti.load_timestamps()
if len(kitti.timestamps) == 0:
print('Dataset is empty? Exiting.')
sys.exit(1)
if args.sequence in odometry_sequences[:11]:
print(
"Odometry dataset sequence {} has ground truth information (poses)."
.format(args.sequence))
kitti.load_poses()
try:
util = pykitti.utils.read_calib_file(
os.path.join(args.dir, 'sequences', args.sequence,
'calib.txt'))
current_epoch = (datetime.utcnow() -
datetime(1970, 1, 1)).total_seconds()
# Export
if args.kitti_type.find("gray") != -1:
used_cameras = cameras[:2]
elif args.kitti_type.find("color") != -1:
used_cameras = cameras[-2:]
save_dynamic_tf(bag,
kitti,
args.kitti_type,
initial_time=current_epoch)
for camera in used_cameras:
save_camera_data(bag,
args.kitti_type,
kitti,
util,
bridge,
camera=camera[0],
camera_frame_id=camera[1],
topic=camera[2],
initial_time=current_epoch)
finally:
print("## OVERVIEW ##")
print(bag)
bag.close()
from mpl_toolkits.mplot3d import Axes3D import pykitti __author__ = "Lee Clement" __email__ = "*****@*****.**" # Change this to the directory where you store KITTI data basedir = '/Users/leeclement/Desktop/KITTI/odometry/dataset' # Specify the dataset to load sequence = '04' # Load the data. Optionally, specify the frame range to load. # dataset = pykitti.odometry(basedir, sequence) dataset = pykitti.odometry(basedir, sequence, frames=range(0, 20, 5)) # dataset.calib: Calibration data are accessible as a named tuple # dataset.timestamps: Timestamps are parsed into a list of timedelta objects # dataset.poses: List of ground truth poses T_w_cam0 # dataset.camN: Generator to load individual images from camera N # dataset.gray: Generator to load monochrome stereo pairs (cam0, cam1) # dataset.rgb: Generator to load RGB stereo pairs (cam2, cam3) # dataset.velo: Generator to load velodyne scans as [x,y,z,reflectance] # Grab some data second_pose = dataset.poses[1] first_gray = next(iter(dataset.gray)) first_cam1 = next(iter(dataset.cam1)) first_rgb = dataset.get_rgb(0) first_cam2 = dataset.get_cam2(0)
