From 0a5b3b94fb1430fc5e9b4551b2ac5c70f59ec1ae Mon Sep 17 00:00:00 2001 From: Jerry Ylilammi Date: Fri, 16 Aug 2024 15:26:58 +0300 Subject: [PATCH] Support synchornizing enu/gt orientation with video --- sync-imu-video.py | 14 +++++++++++++- utils/gt_to_angular_velocity.py | 14 +++++++++++--- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/sync-imu-video.py b/sync-imu-video.py index 65f32e4..bcf5075 100644 --- a/sync-imu-video.py +++ b/sync-imu-video.py @@ -13,6 +13,11 @@ from scipy import interpolate +def simulate_imu_data(filename): + from utils.gt_to_angular_velocity import simulate_angular_velocity + return simulate_angular_velocity(filename) + + class OpticalFlowComputer: def __init__(self, fn, args): self.cap = cv.VideoCapture(fn) @@ -165,6 +170,7 @@ def findMinimumError(gyro_time, gyro_angular_speed, fn, fn_range, offsets): p.add_argument('--resize_width', type=int, default=200) p.add_argument('--output', help="data.jsonl with frame timestamp shifted to match gyroscope timestamps") p.add_argument('--maxOffset', help="Maximum offset between gyro and frame times in seconds", type=float, default=5.0) + p.add_argument("--simulate_imu", help="Simulates imu signal from groundTruth/GPS poses", action="store_true") args = p.parse_args() @@ -183,7 +189,7 @@ def findMinimumError(gyro_time, gyro_angular_speed, fn, fn_range, offsets): data = slurpJson(args.data) prevFrameTime = None for entry in data: - if "sensor" in entry and entry["sensor"]["type"] == "gyroscope": + if not args.simulate_imu and "sensor" in entry and entry["sensor"]["type"] == "gyroscope": gyroTimes.append(entry["time"]) gyroSpeed.append(np.linalg.norm(entry["sensor"]["values"])) elif "frames" in entry: @@ -191,6 +197,12 @@ def findMinimumError(gyro_time, gyro_angular_speed, fn, fn_range, offsets): frameTimes.append((prevFrameTime + entry["time"]) / 2.0) prevFrameTime = entry["time"] + if args.simulate_imu: + simulatedGyro = simulate_imu_data(args.data) + gyroTimes = simulatedGyro[:,0] + xyz = simulatedGyro[:, 1:4] + gyroSpeed = np.linalg.norm(xyz, axis=1) + if args.max_frames > 0: frameTimes = frameTimes[args.skip_first_n_frames : args.skip_first_n_frames + args.max_frames] gyroSpeed = [gyroSpeed[i] for i, t in enumerate(gyroTimes) if t >= frameTimes[0] and t <= frameTimes[-1]] diff --git a/utils/gt_to_angular_velocity.py b/utils/gt_to_angular_velocity.py index 957ed7a..848acf6 100644 --- a/utils/gt_to_angular_velocity.py +++ b/utils/gt_to_angular_velocity.py @@ -33,13 +33,14 @@ def compute_angular_velocities(gt, key): angular_speed = [] euler_ori = [] time = [] + orientationKey = "orientation" if "orientation" in prev[key] else "enuOrientation" for index, cur in enumerate(gt): if index == 0: continue dt = cur["time"] - prev["time"] if dt == 0.0: continue angular, speed = angular_velocity( - ori_to_quat(prev[key]["orientation"]), - ori_to_quat(cur[key]["orientation"]), + ori_to_quat(prev[key][orientationKey]), + ori_to_quat(cur[key][orientationKey]), dt ) time.append((prev["time"] + cur["time"]) * .5) @@ -53,10 +54,17 @@ def compute_angular_velocities(gt, key): return (np.array(time).T, np.array(angular_velocities), np.array(euler_ori), np.array(angular_speed)) -def simulate_angular_velocity(input, gtName="groundTruth"): +def simulate_angular_velocity(input, gtName=None): data = [] + validGtNames = ["groundTruth", "gps"] with open(input) as f: for line in f.readlines(): + if gtName == None: + for n in validGtNames: + if n in line: + gtName = n + break + if not gtName: continue if not gtName in line: continue data.append(json.loads(line)) gt_time, gt_angular, gt_euler, gt_angular_speed = compute_angular_velocities(data, gtName)