def makeCroppedGraySequence(seq_id, num_threads=8):
src_folder = os.path.join(symlink('robotcar'), seq_id)
dst_folder = os.path.join(symlink('robotcar_cropped_gray'), seq_id)
if not os.path.exists(dst_folder):
os.makedirs(dst_folder)
# Convert images.
stereo_dir = os.path.join(src_folder, 'stereo')
for leftright in ['left', 'right']:
in_dir = os.path.join(stereo_dir, leftright)
images = utils_path.imagesFromDir(in_dir, extension='.png')
im_names = [os.path.basename(i) for i in images]
cam_model = camera_model.CameraModel(cam_model_dir, in_dir)
out_dir = os.path.join(dst_folder, 'rect', leftright)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
p = multiprocessing.Pool(num_threads)
print('Converting %s...' % leftright)
p.map(ParallelImageConverter(cam_model, in_dir, out_dir, im_names),
range(len(images)))
# K files
# Image downscaling affects camera intrinsics
f = [0.5 * x for x in cam_model.focal_length]
c = [0.5 * x for x in cam_model.principal_point]
K = np.array([[f[0], 0, c[0]], [0, f[1], c[1]], [0, 0, 1]])
np.savetxt(os.path.join(dst_folder, '%s_K.txt' % leftright), K)
times = [int(i[:16]) for i in im_names]
# Get poses everywhere.
T_W_C_file = os.path.join(dst_folder, 'T_W_C.txt')
if not os.path.exists(T_W_C_file):
print('Interpolating poses...')
insext = np.loadtxt(os.path.join(sdk_dir, 'extrinsics',
'ins.txt')).tolist()
mtx = transform.build_se3_transform(insext)
# Cx: Oxford style camera, where x looks into the image plane.
T_I_Cx = pose.fromMatrix(np.asarray(mtx)).inverse()
T_Cx_C = pose.yRotationDeg(90) * pose.zRotationDeg(90)
T_I_C = T_I_Cx * T_Cx_C
# T_W_I0: Because in Oxford, z looks down.
T_W_I0 = pose.xRotationDeg(180)
ins_path = os.path.join(src_folder, 'gps', 'ins.csv')
T_I0_I = interpolate_poses.interpolate_ins_poses(
ins_path, times, times[0])
T_I0_I = [pose.fromMatrix(np.asarray(i)) for i in T_I0_I]
T_W_C = [T_W_I0 * i * T_I_C for i in T_I0_I]
T_W_C_serialized = np.array([i.asArray().ravel() for i in T_W_C])
np.savetxt(T_W_C_file, T_W_C_serialized)
def build_pointcloud(lidar_dir,
poses_file,
extrinsics_dir,
start_time,
end_time,
origin_time=-1):
"""Builds a pointcloud by combining multiple LIDAR scans with odometry information.
Args:
lidar_dir (str): Directory containing LIDAR scans.
poses_file (str): Path to a file containing pose information. Can be VO or INS data.
extrinsics_dir (str): Directory containing extrinsic calibrations.
start_time (int): UNIX timestamp of the start of the window over which to build the pointcloud.
end_time (int): UNIX timestamp of the end of the window over which to build the pointcloud.
origin_time (int): UNIX timestamp of origin frame. Pointcloud coordinates are relative to this frame.
Returns:
numpy.ndarray: 3xn array of (x, y, z) coordinates of pointcloud
numpy.array: array of n reflectance values or None if no reflectance values are recorded (LDMRS)
Raises:
ValueError: if specified window doesn't contain any laser scans.
IOError: if scan files are not found.
"""
if origin_time < 0:
origin_time = start_time
lidar = re.search(
'(lms_front|lms_rear|ldmrs|velodyne_left|velodyne_right)',
lidar_dir).group(0)
timestamps_path = os.path.join(lidar_dir, os.pardir, lidar + '.timestamps')
timestamps = []
with open(timestamps_path) as timestamps_file:
for line in timestamps_file:
timestamp = int(line.split(' ')[0])
if start_time <= timestamp <= end_time:
timestamps.append(timestamp)
if len(timestamps) == 0:
raise ValueError("No LIDAR data in the given time bracket.")
with open(os.path.join(extrinsics_dir, lidar + '.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = build_se3_transform(
[float(x) for x in extrinsics.split(' ')])
poses_type = re.search('(vo|ins|rtk)\.csv', poses_file).group(1)
if poses_type in ['ins', 'rtk']:
with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = np.linalg.solve(
build_se3_transform([float(x) for x in extrinsics.split(' ')]),
G_posesource_laser)
poses = interpolate_ins_poses(poses_file,
timestamps,
origin_time,
use_rtk=(poses_type == 'rtk'))
else:
# sensor is VO, which is located at the main vehicle frame
poses = interpolate_vo_poses(poses_file, timestamps, origin_time)
pointcloud = np.array([[0], [0], [0], [0]])
if lidar == 'ldmrs':
reflectance = None
else:
reflectance = np.empty((0))
for i in range(0, len(poses)):
scan_path = os.path.join(lidar_dir, str(timestamps[i]) + '.bin')
if "velodyne" not in lidar:
if not os.path.isfile(scan_path):
continue
scan_file = open(scan_path)
scan = np.fromfile(scan_file, np.double)
scan_file.close()
scan = scan.reshape((len(scan) // 3, 3)).transpose()
if lidar != 'ldmrs':
# LMS scans are tuples of (x, y, reflectance)
reflectance = np.concatenate(
(reflectance, np.ravel(scan[2, :])))
scan[2, :] = np.zeros((1, scan.shape[1]))
else:
if os.path.isfile(scan_path):
ptcld = load_velodyne_binary(scan_path)
else:
scan_path = os.path.join(lidar_dir,
str(timestamps[i]) + '.png')
if not os.path.isfile(scan_path):
continue
ranges, intensities, angles, approximate_timestamps = load_velodyne_raw(
scan_path)
ptcld = velodyne_raw_to_pointcloud(ranges, intensities, angles)
reflectance = np.concatenate((reflectance, ptcld[3]))
scan = ptcld[:3]
scan = np.dot(np.dot(poses[i], G_posesource_laser),
np.vstack([scan, np.ones((1, scan.shape[1]))]))
pointcloud = np.hstack([pointcloud, scan])
pointcloud = pointcloud[:, 1:]
if pointcloud.shape[1] == 0:
raise IOError(
"Could not find scan files for given time range in directory " +
lidar_dir)
return pointcloud, reflectance
vo_data = pd.read_csv('{}/{}'.format(seq_dir, 'vo/vo.csv'), mangle_dupe_cols=True)
vo_ts = vo_data['source_timestamp']
rtk_filename = gt_dir+'/rtk.csv'
first_idx = np.where(stereo_img_ts > gt_ts[0])[0][0] + 1
last_idx = np.where(stereo_img_ts > gt_ts[-1])[0]
if last_idx != []:
last_idx = last_idx[0] - 1
else:
last_idx = len(gt_ts)-1
stereo_img_idx = list(range(first_idx, last_idx))
stereo_img_ts = list(stereo_img_ts[stereo_img_idx])
gt_poses = interpolate_ins_poses(rtk_filename, stereo_img_ts, stereo_img_ts[0], use_rtk=True)
gt_poses = [se3_to_components(p) for p in gt_poses]
gt_poses = np.array(gt_poses)
vo_filename = '{}/{}'.format(seq_dir, 'vo/vo.csv')
vo_poses = interpolate_vo_poses(vo_filename, stereo_img_ts, stereo_img_ts[0])
vo_poses = [se3_to_components(p) for p in vo_poses]
vo_poses = np.array(vo_poses)
r_w_imu_w = gt_poses[:,0:3]
rpy_w_imu_w = gt_poses[:,3:6]
plt.plot(gt_poses[:,0], gt_poses[:,1])
plt.plot(vo_poses[:,0], -vo_poses[:,1])
plt.savefig('{}_gt_traj'.format(seq))
plt.figure()
def build_pointcloud(lidar_dir, poses_file, extrinsics_dir, start_time, end_time, origin_time=-1):
"""Builds a pointcloud by combining multiple LIDAR scans with odometry information.
Args:
lidar_dir (str): Directory containing LIDAR scans.
poses_file (str): Path to a file containing pose information. Can be VO or INS data.
extrinsics_dir (str): Directory containing extrinsic calibrations.
start_time (int): UNIX timestamp of the start of the window over which to build the pointcloud.
end_time (int): UNIX timestamp of the end of the window over which to build the pointcloud.
origin_time (int): UNIX timestamp of origin frame. Pointcloud coordinates are relative to this frame.
Returns:
numpy.ndarray: 3xn array of (x, y, z) coordinates of pointcloud
numpy.array: array of n reflectance values or None if no reflectance values are recorded (LDMRS)
Raises:
ValueError: if specified window doesn't contain any laser scans.
IOError: if scan files are not found.
"""
if origin_time < 0:
origin_time = start_time
lidar = re.search('(lms_front|lms_rear|ldmrs)', lidar_dir).group(0)
timestamps_path = os.path.join(lidar_dir, os.pardir, lidar + '.timestamps')
timestamps = []
with open(timestamps_path) as timestamps_file:
for line in timestamps_file:
timestamp = int(line.split(' ')[0])
if start_time <= timestamp <= end_time:
timestamps.append(timestamp)
if len(timestamps) == 0:
raise ValueError("No LIDAR data in the given time bracket.")
with open(os.path.join(extrinsics_dir, lidar + '.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = build_se3_transform([float(x) for x in extrinsics.split(' ')])
poses_type = re.search('(vo|ins)\.csv', poses_file).group(1)
if poses_type == 'ins':
with open(os.path.join(extrinsics_dir, 'ins.txt')) as extrinsics_file:
extrinsics = next(extrinsics_file)
G_posesource_laser = np.linalg.solve(build_se3_transform([float(x) for x in extrinsics.split(' ')]),
G_posesource_laser)
poses = interpolate_ins_poses(poses_file, timestamps, origin_time)
else:
# sensor is VO, which is located at the main vehicle frame
poses = interpolate_vo_poses(poses_file, timestamps, origin_time)
pointcloud = np.array([[0], [0], [0], [0]])
if lidar == 'ldmrs':
reflectance = None
else:
reflectance = np.empty((0))
for i in range(0, len(poses)):
scan_path = os.path.join(lidar_dir, str(timestamps[i]) + '.bin')
if not os.path.isfile(scan_path):
continue
scan_file = open(scan_path)
scan = np.fromfile(scan_file, np.double)
scan_file.close()
scan = scan.reshape((len(scan) // 3, 3)).transpose()
if lidar != 'ldmrs':
# LMS scans are tuples of (x, y, reflectance)
reflectance = np.concatenate((reflectance, np.ravel(scan[2, :])))
scan[2, :] = np.zeros((1, scan.shape[1]))
scan = np.dot(np.dot(poses[i], G_posesource_laser), np.vstack([scan, np.ones((1, scan.shape[1]))]))
pointcloud = np.hstack([pointcloud, scan])
pointcloud = pointcloud[:, 1:]
if pointcloud.shape[1] == 0:
raise IOError("Could not find scan files for given time range in directory " + lidar_dir)
return pointcloud, reflectance