diff --git a/pyboreas/eval/README.md b/pyboreas/eval/README.md index 27a0348..5c874ef 100644 --- a/pyboreas/eval/README.md +++ b/pyboreas/eval/README.md @@ -32,12 +32,14 @@ python eval/odometry_benchmark.py -h usage: odometry_benchmark.py [-h] [--pred PRED] [--gt GT] [--radar] [--no-interp] [--no-solver] optional arguments: - -h, --help show this help message and exit - --pred PRED path to prediction files - --gt GT path to groundtruth files - --radar evaluate radar odometry in SE(2) - --interp INTERP path to interpolation output, do not set if evaluating - --no-solver disable solver for built-in interpolation + -h, --help show this help message and exit + --pred PRED path to prediction files + --gt GT path to groundtruth files + --radar evaluate radar odometry in SE(2) + --interp INTERP path to interpolation output, do not set if evaluating + --processes PROCESSES + number of workers to use for built-in interpolation + --no-solver disable solver for built-in interpolation ``` The `pred` argument is the directory containing the odometry sequence files, which is `pyboreas/test/demo/pred/3d/` for this demo. @@ -47,11 +49,13 @@ The `radar` argument should be included to evaluate the 2D benchmark. The 3D ben The `interp` argument should be set as the output directory for the interpolation files. Setting this argument will change the operation of the benchmark script to interpolation mode, i.e., it will not compute the errors and output error results. Instead, it will output a `.txt` file for each of your odometry sequences interpolated at the groundtruth (lidar) timestamps. Use this argument only if you need to interpolate. +The `processes` argument sets the number of processes to use when interpolating. Setting this argument to 1 will result in no additional subprocesses being created. Default value is the CPU count of your machine. + The `no-solver` argument should be included to disable the solver for the built-in interpolation method. We use a batch optimization routine to solve for velocity estimates of each frame in order to interpolate. If the solver is disabled, the script instead interpolates with velocities approximated with finite difference. This is less accurate, but will run much faster. Suggested use is for debugging. This demo requires the built-in interpolation method. We will interpolate without the solver just for demonstration purposes (run it faster): ``` -python eval/odometry_benchmark.py --pred test/demo/pred/3d/ --gt test/demo/gt/ --interp test/demo/pred/3d/interp/ --no-solver +python eval/odometry_benchmark.py --pred test/demo/pred/3d/ --gt test/demo/gt/ --interp test/demo/pred/3d/interp/ --processes 1 --no-solver interpolating sequence boreas-2021-08-05-13-34.txt ... boreas-2021-08-05-13-34.txt took 9.404045581817627 seconds diff --git a/pyboreas/eval/odometry_benchmark.py b/pyboreas/eval/odometry_benchmark.py index 8a3d5cc..b0b3ee7 100644 --- a/pyboreas/eval/odometry_benchmark.py +++ b/pyboreas/eval/odometry_benchmark.py @@ -1,7 +1,7 @@ import argparse import os from pyboreas.utils.odometry import get_sequences, get_sequence_poses, get_sequence_poses_gt, \ - compute_kitti_metrics, compute_interpolation + compute_kitti_metrics, compute_interpolation, get_sequence_times_gt if __name__ == '__main__': # parse arguments @@ -11,6 +11,7 @@ parser.add_argument('--radar', dest='radar', action='store_true', help='evaluate radar odometry in SE(2)') parser.set_defaults(radar=False) parser.add_argument('--interp', default='', type=str, help='path to interpolation output, do not set if evaluating') + parser.add_argument('--processes', default=os.cpu_count(), type=int, help='number of workers to use for built-in interpolation') parser.add_argument('--no-solver', dest='solver', action='store_false', help='disable solver for built-in interpolation') parser.set_defaults(solver=True) args = parser.parse_args() @@ -23,20 +24,25 @@ # parse sequences seq = get_sequences(args.pred, '.txt') T_pred, times_pred, seq_lens_pred = get_sequence_poses(args.pred, seq) - T_gt, times_gt, seq_lens_gt, crop = get_sequence_poses_gt(args.gt, seq, dim) if args.interp: # if we are interpolating... # can't be the same as pred if args.interp == args.pred: raise ValueError('`interp` directory path cannot be the same as the `pred` directory path') + # get corresponding groundtruth times + times_gt, seq_lens_gt, _ = get_sequence_times_gt(args.gt, seq) + # make interp directory if it doesn't exist if not os.path.exists(args.interp): os.mkdir(args.interp) # interpolate - compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pred, seq, args.interp, args.solver) + compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pred, seq, args.interp, args.solver, args.processes) else: + # get corresponding groundtruth poses + T_gt, _, seq_lens_gt, crop = get_sequence_poses_gt(args.gt, seq, dim) + # compute errors t_err, r_err, _ = compute_kitti_metrics(T_gt, T_pred, seq_lens_gt, seq_lens_pred, seq, args.pred, dim, crop) diff --git a/pyboreas/test/test_odometry.py b/pyboreas/test/test_odometry.py index abcf534..a037a07 100644 --- a/pyboreas/test/test_odometry.py +++ b/pyboreas/test/test_odometry.py @@ -73,6 +73,7 @@ def test_module(self): dim = 3 interp = 'pyboreas/test/demo/pred/3d/interptest' solver = False + processes = 2 # make interp directory if it doesn't exist if not os.path.exists(interp): @@ -84,7 +85,7 @@ def test_module(self): T_gt, times_gt, seq_lens_gt, crop = get_sequence_poses_gt(gt, seq, dim) # interpolate - compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pred, seq, interp, solver) + compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pred, seq, interp, solver, processes) # read in interpolated sequences T_pred, times_pred, seq_lens_pred = get_sequence_poses(interp, seq) diff --git a/pyboreas/utils/odometry.py b/pyboreas/utils/odometry.py index 3126b7b..2034afb 100644 --- a/pyboreas/utils/odometry.py +++ b/pyboreas/utils/odometry.py @@ -1,9 +1,12 @@ import os +from pathlib import Path from time import time -from itertools import accumulate +from itertools import accumulate, repeat +from multiprocessing import Pool import numpy as np import matplotlib.pyplot as plt -from pyboreas.utils.utils import get_inverse_tf, rotation_error, translation_error, enforce_orthog, yawPitchRollToRot +from pyboreas.utils.utils import get_inverse_tf, rotation_error, translation_error, enforce_orthog, yawPitchRollToRot, \ + get_time_from_filename from pylgmath import Transformation from pysteam.trajectory import Time, TrajectoryInterface from pysteam.state import TransformStateVar, VectorSpaceStateVar @@ -234,7 +237,25 @@ def get_path_from_Tvi_list(Tvi_list): return path -def compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pred, seq, out_dir, solver): +def compute_interpolation_one_seq(T_pred, times_gt, times_pred, out_fname, solver): + """Interpolate for poses at the groundtruth times and write them out as txt files. + Args: + T_pred (List[np.ndarray]): List of 4x4 SE(3) transforms (fixed reference frame 'i' to frame 'v', T_vi) + times_gt (List[int]): List of times (microseconds) corresponding to T_gt + times_pred (List[int]): List of times (microseconds) corresponding to T_pred + out_fname (string): path to output file for interpolation output + solver (bool): 'True' solves velocities for built-in interpolation. 'False' we use a finite-diff. approx. + Returns: + Nothing + """ + T_query = interpolate_poses(T_pred, times_pred, times_gt, solver) # interpolate + write_traj_file(out_fname, T_query, times_gt) # write out + print(f'interpolated sequence {os.path.basename(out_fname)}, output file: {out_fname}') + + return + + +def compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pred, seq, out_dir, solver, processes): """Interpolate for poses at the groundtruth times and write them out as txt files. Args: T_pred (List[np.ndarray]): List of 4x4 SE(3) transforms (fixed reference frame 'i' to frame 'v', T_vi) @@ -249,26 +270,41 @@ def compute_interpolation(T_pred, times_gt, times_pred, seq_lens_gt, seq_lens_pr Nothing """ # get start and end indices of each sequence - indices_gt = [0] - indices_gt.extend(list(accumulate(seq_lens_gt))) - indices_pred = [0] - indices_pred.extend(list(accumulate(seq_lens_pred))) - - # loop for each sequence - for i in range(len(seq_lens_pred)): - ts = time() # start time - - # get poses and times of current sequence - T_pred_seq = T_pred[indices_pred[i]:indices_pred[i+1]] - times_gt_seq = times_gt[indices_gt[i]:indices_gt[i+1]] - times_pred_seq = times_pred[indices_pred[i]:indices_pred[i+1]] + indices_gt = tuple(accumulate(seq_lens_gt, initial=0)) + indices_pred = tuple(accumulate(seq_lens_pred, initial=0)) + + # prepare input iterators to compute_interpolation_one_seq + T_pred_seq = (T_pred[indices_pred[i]:indices_pred[i+1]] for i in range(len(seq_lens_pred))) + times_gt_seq = (times_gt[indices_gt[i]:indices_gt[i+1]] for i in range(len(seq_lens_pred))) + times_pred_seq = (times_pred[indices_pred[i]:indices_pred[i+1]] for i in range(len(seq_lens_pred))) + out_fname_seq = (os.path.join(out_dir, seq[i]) for i in range(len(seq_lens_pred))) + solver_seq = repeat(solver, len(seq_lens_pred)) + + if processes == 1: + # loop for each sequence + for i in range(len(seq_lens_pred)): + ts = time() # start time + + # get poses and times of current sequence + T_pred_i = next(T_pred_seq) + times_gt_i = next(times_gt_seq) + times_pred_i = next(times_pred_seq) + + # query predicted trajectory at groundtruth times and write out + print('interpolating sequence', seq[i], '...') + T_query = interpolate_poses(T_pred_i, times_pred_i, times_gt_i, solver) # interpolate + write_traj_file(os.path.join(out_dir, seq[i]), T_query, times_gt_i) # write out + print(seq[i], 'took', str(time() - ts), ' seconds') + print('output file:', os.path.join(out_dir, seq[i]), '\n') + else: + # compute interpolation for each sequence in parallel + with Pool(processes) as p: + ts = time() # start time - # query predicted trajectory at groundtruth times and write out - print('interpolating sequence', seq[i], '...') - T_query = interpolate_poses(T_pred_seq, times_pred_seq, times_gt_seq, solver) # interpolate - write_traj_file(os.path.join(out_dir, seq[i]), T_query, times_gt_seq) # write out - print(seq[i], 'took', str(time() - ts), ' seconds') - print('output file:', os.path.join(out_dir, seq[i]), '\n') + print(f'interpolating {len(seq_lens_pred)} sequences in parallel using {processes} workers ...') + p.starmap( + compute_interpolation_one_seq, zip(T_pred_seq, times_gt_seq, times_pred_seq, out_fname_seq, solver_seq)) + print(f'interpolation took {time() - ts:.2f} seconds\n') return @@ -427,6 +463,53 @@ def get_sequence_poses_gt(path, seq, dim): return all_poses, all_times, seq_lens, crop +def get_sequence_times_gt(path, seq): + """Retrieves a list of groundtruth (lidar) timestamps corresponding to the given sequences for 3D evaluation + Args: + path (string): directory path to root directory of Boreas dataset + seq (List[string]): list of sequence file names + Returns: + all_times (List[int]): list of times in microseconds from all sequence files + seq_lens (List[int]): list of sequence lengths + crop (List[Tuple]): sequences are cropped to prevent extrapolation, this list holds start and end indices + """ + # loop for each sequence + all_times = [] + seq_lens = [] + crop = [] + for filename in seq: + # determine path to gt file + dir = filename[:-4] # assumes last four characters are '.txt' + lfilepath = os.path.join(path, dir, 'applanix/lidar_poses.csv') # use 'lidar_poses.csv' for groundtruth + cfilepath = os.path.join(path, dir, 'applanix/camera_poses.csv') # read in timestamps of camera groundtruth + if os.path.isfile(lfilepath) and os.path.isfile(cfilepath): + # csv files exist, use them + _, times = read_traj_file_gt(lfilepath, np.identity(4), dim=3) + times_np = np.stack(times) + _, ctimes = read_traj_file_gt(cfilepath, np.identity(4), dim=3) + else: + # read timestamps from data + lpath = os.path.join(path, dir, 'lidar') # read lidar data filenames + times = [int(Path(f).stem) for f in os.listdir(lpath) if '.bin' in f] + times.sort() + times_np = np.stack(times) + + cpath = os.path.join(path, dir, 'camera') # read camera data filenames + ctimes = [int(Path(f).stem) for f in os.listdir(cpath) if '.png' in f] + ctimes.sort() + + istart = np.searchsorted(times_np, ctimes[0]) + iend = np.searchsorted(times_np, ctimes[-1]) + times = times[istart:iend] + crop += [(istart, iend)] + if times[0] < ctimes[0] or times[-1] > ctimes[-1]: + raise ValueError('Invalid start and end indices for groundtruth.') + + seq_lens.append(len(times)) + all_times.extend(times) + + return all_times, seq_lens, crop + def write_traj_file(path, poses, times): """Writes trajectory into a space-separated txt file @@ -488,9 +571,9 @@ def read_traj_file_gt(path, T_ab, dim): T_ab = enforce_orthog(T_ab) for line in lines[1:]: - pose, time = convert_line_to_pose(line, dim) - poses += [enforce_orthog(T_ab @ get_inverse_tf(pose))] # convert T_iv to T_vi and apply calibration - times += [int(time)] # microseconds + pose, time = convert_line_to_pose(line, dim) + poses += [enforce_orthog(T_ab @ get_inverse_tf(pose))] # convert T_iv to T_vi and apply calibration + times += [int(time)] # microseconds return poses, times def convert_line_to_pose(line, dim): @@ -520,4 +603,3 @@ def convert_line_to_pose(line, dim): raise ValueError('Invalid dim value in convert_line_to_pose. Use either 2 or 3.') time = int(line[0]) return T, time -