Support synchornizing enu/gt orientation with video

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,14 +189,20 @@ 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:
             if prevFrameTime != None:
                 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]]

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)