Feature/temporal calibration plots

notebooks/plot_data_timestamps.py

@@ -0,0 +1,220 @@
+# ---
+# jupyter:
+#   jupytext:
+#     formats: ipynb,py:percent
+#     text_representation:
+#       extension: .py
+#       format_name: percent
+#       format_version: '1.3'
+#       jupytext_version: 1.11.1
+#   kernelspec:
+#     display_name: Python 3
+#     language: python
+#     name: python3
+# ---
+
+# %% [markdown]
+# # Plot Data Timestamps
+#
+# Plots sensor timestamps (cameras and IMU) against each other to look
+# for delays and other variability in timing.
+
+
+# %%
+import os
+import pandas as pd
+import matplotlib.pyplot as plt
+
+# # %matplotlib inline
+# %matplotlib notebook
+
+
+# %%
+# Define directory to VIO output csv files as well as ground truth absolute poses.
+vio_output_dir = ""
+log_kimera_endpoint = False
+
+
+# %%
+# Extract end point
+if log_kimera_endpoint:
+    output_tv = os.path.join(os.path.expandvars(vio_output_dir), "traj_vio.csv")
+    traj_vio = pd.read_csv(output_tv, sep=",", index_col=0)
+    end_point = traj_vio.index[-1]
+
+output_imu_file = os.path.join(os.path.expandvars(vio_output_dir), "imu_data.csv")
+imu_data = pd.read_csv(output_imu_file, sep=",", index_col=0)
+
+
+# %% [markdown]
+# # Gyro Measurements
+
+
+# %%
+fig = plt.figure()
+plt.plot(imu_data.index, imu_data.ang_vel_x)
+plt.plot(imu_data.index, imu_data.ang_vel_y)
+plt.plot(imu_data.index, imu_data.ang_vel_z)
+
+if log_kimera_endpoint:
+    plt.axvline(x=end_point)
+
+plt.ylabel("Gyro Measurements")
+plt.xlabel("Timestamps")
+plt.legend()
+plt.show()
+
+
+# %% [markdown]
+# # Accelerometer Measurements
+
+
+# %%
+fig = plt.figure()
+plt.plot(imu_data.index, imu_data.lin_acc_x)
+plt.plot(imu_data.index, imu_data.lin_acc_y)
+plt.plot(imu_data.index, imu_data.lin_acc_z)
+
+if log_kimera_endpoint:
+    plt.axvline(x=end_point)
+
+plt.ylabel("Accelerometer Measurements")
+plt.xlabel("Timestamps")
+plt.legend()
+plt.show()
+
+
+# %% [markdown]
+# # Sensor vs. ROS Timestamps
+
+
+# %%
+fig = plt.figure()
+plt.plot(imu_data.index, imu_data.clock)
+
+if log_kimera_endpoint:
+    plt.axvline(x=end_point)
+
+plt.xlabel("Ros time")
+plt.ylabel("IMU Timestamps")
+plt.legend()
+plt.show()
+
+
+# %% [markdown]
+# # IMU Timestamp Deltas
+
+
+# %%
+fig = plt.figure()
+
+deltas = []
+for i in range(1, len(imu_data.index)):
+    delta = imu_data.index[i] - imu_data.index[i - 1]
+    delta *= 1e-9
+    if delta < 0:
+        delta *= 100  # So we can see out-of-order timestamps
+    deltas.append(delta)
+
+plt.plot(imu_data.index[1:] * 1e-9, deltas, label="IMU deltas")
+
+plt.xlabel("Timestamps in seconds")
+plt.ylabel("Delta in seconds")
+plt.legend()
+plt.show()
+
+
+# %% [markdown]
+# # Left Camera Timestamp Deltas
+
+
+# %%
+output_infra1_deltas_file = os.path.join(
+    os.path.expandvars(vio_output_dir), "timestamps_infra1.csv"
+)
+infra1_deltas_data = pd.read_csv(output_infra1_deltas_file, sep=",", index_col=0)
+fig = plt.figure()
+
+deltas = []
+for i in range(1, len(infra1_deltas_data.index)):
+    delta = infra1_deltas_data.index[i] - infra1_deltas_data.index[i - 1]
+    delta *= 1e-9
+    if delta < 0:
+        delta *= 100  # So we can see out-of-order timestamps
+    deltas.append(delta)
+
+plt.plot(infra1_deltas_data.index[1:] * 1e-9, deltas, label="Infra1 deltas")
+
+plt.xlabel("Timestamps in seconds")
+plt.ylabel("Delta in seconds")
+plt.legend()
+plt.show()
+
+
+# %% [markdown]
+# # Right Camera Timestamp Deltas
+
+
+# %%
+output_infra2_deltas_file = os.path.join(
+    os.path.expandvars(vio_output_dir), "timestamps_infra2.csv"
+)
+infra2_deltas_data = pd.read_csv(output_infra2_deltas_file, sep=",", index_col=0)
+fig = plt.figure()
+
+deltas = []
+for i in range(1, len(infra2_deltas_data.index)):
+    delta = infra2_deltas_data.index[i] - infra2_deltas_data.index[i - 1]
+    delta *= 1e-9
+    if delta < 0:
+        delta *= 100  # So we can see out-of-order timestamps
+    deltas.append(delta)
+
+plt.plot(infra2_deltas_data.index[1:] * 1e-9, deltas, label="Infra2 deltas")
+
+plt.xlabel("Timestamps in seconds")
+plt.ylabel("Delta in seconds")
+plt.legend()
+plt.show()
+
+
+# %% [markdown]
+# # All Timestamp Deltas
+
+
+# %%
+output_infra1_deltas_file = os.path.join(
+    os.path.expandvars(vio_output_dir), "timestamps_infra1.csv"
+)
+infra1_deltas_data = pd.read_csv(output_infra1_deltas_file, sep=",", index_col=0)
+fig = plt.figure()
+
+deltas = []
+for i in range(1, len(infra1_deltas_data.index)):
+    delta = infra1_deltas_data.index[i] - infra1_deltas_data.index[i - 1]
+    delta *= 1e-9
+    if delta < 0:
+        delta *= 100  # So we can see out-of-order timestamps
+    deltas.append(delta)
+
+plt.plot(infra1_deltas_data.index[1:] * 1e-9, deltas, label="Infra1 deltas")
+
+output_infra2_deltas_file = os.path.join(
+    os.path.expandvars(vio_output_dir), "timestamps_infra2.csv"
+)
+infra2_deltas_data = pd.read_csv(output_infra2_deltas_file, sep=",", index_col=0)
+
+deltas = []
+for i in range(1, len(infra2_deltas_data.index)):
+    delta = infra2_deltas_data.index[i] - infra2_deltas_data.index[i - 1]
+    delta *= 1e-9
+    if delta < 0:
+        delta *= 100  # So we can see out-of-order timestamps
+    deltas.append(delta)
+
+plt.plot(infra2_deltas_data.index[1:] * 1e-9, deltas, label="Infra2 deltas")
+
+plt.xlabel("Timestamps in seconds")
+plt.ylabel("Delta in seconds")
+plt.legend()
+plt.show()

notebooks/plot_frontend.py

@@ -25,6 +25,7 @@
 import copy
 import pandas as pd
 import numpy as np
+import scipy
 from scipy.spatial.transform import Rotation as R
 
 import logging
@@ -59,7 +60,7 @@
 )
 
 # %matplotlib inline
-# # %matplotlib notebookw_T_bi
+# # %matplotlib notebook
 import matplotlib.pyplot as plt
 
 # %% [markdown]
@@ -282,7 +283,10 @@ def get_max(attrib):
     gt_angles_timestamps.append(traj_ref_rel.timestamps[i])
     # rotation matrix to axisangle
     rotm = traj_ref_rel._poses_se3[i][0:3, 0:3]
-    r = R.from_dcm(rotm)
+    if scipy.version.version >= "1.4.0":
+        r = R.from_matrix(rotm)
+    else:
+        r = R.from_dcm(rotm)
 
     rot_vec = r.as_rotvec()
     gt_angles.append(np.linalg.norm(rot_vec))
@@ -413,3 +417,135 @@ def get_max(attrib):
 plot_metric(rpe_rot, "Stereo Ransac RPE Rotation Part (degrees)", figsize=(18, 10))
 plot_metric(rpe_tran, "Stereo Ransac RPE Translation Part (meters)", figsize=(18, 10))
 plt.show()
+
+# %% [markdown]
+# ## Time-Alignment
+#
+# Show 1d relative rotation angles before and after time alignment
+
+# %%
+# grab time alignment data from debug file
+temporal_cal_file = os.path.join(
+    os.path.expandvars(vio_output_dir), "output_frontend_temporal_cal.csv"
+)
+
+do_temporal_plots = True
+try:
+    temporal_cal_df = pd.read_csv(temporal_cal_file, sep=",", index_col=False)
+except FileNotFoundError:
+    print("Invalid filepath: {}".format(temporal_cal_file))
+
+if do_temporal_plots:
+    t_imu_cam_s = temporal_cal_df.loc[temporal_cal_df.index[-1], "t_imu_cam_s"]
+    window_size = sum(temporal_cal_df["not_enough_data"])
+    print("t_imu_cam_s = {}".format(t_imu_cam_s))
+
+    temporal_marker = "+"
+
+# %%
+# Plot all data
+if do_temporal_plots:
+    fig, ax = plt.subplots(2, 1, sharex=True)
+    ax[0].plot(
+        temporal_cal_df["#timestamp_vision"] * 1.0e-9,
+        np.degrees(temporal_cal_df["vision_relative_angle_norm"]),
+        label="Camera",
+        marker=temporal_marker,
+    )
+    ax[0].plot(
+        temporal_cal_df["timestamp_imu"] * 1.0e-9,
+        np.degrees(temporal_cal_df["imu_relative_angle_norm"]),
+        label="IMU",
+        marker=temporal_marker,
+    )
+    ax[1].plot(
+        temporal_cal_df["#timestamp_vision"] * 1.0e-9,
+        np.degrees(temporal_cal_df["vision_relative_angle_norm"]),
+        label="Camera",
+        marker=temporal_marker,
+    )
+    ax[1].plot(
+        temporal_cal_df["timestamp_imu"] * 1.0e-9 + t_imu_cam_s,
+        np.degrees(temporal_cal_df["imu_relative_angle_norm"]),
+        label="IMU",
+        marker=temporal_marker,
+    )
+    ax[0].legend()
+    ax[0].set_xlabel("Time [s]")
+    ax[0].set_ylabel("Rotation Angle [degrees]")
+    ax[0].set_title("Rotation angles before time-alignment")
+    ax[1].set_ylabel("Rotation Angle [degrees]")
+    ax[1].set_title("Rotation angles after time-alignment")
+    fig.set_size_inches((9, 9))
+    plt.tight_layout()
+    plt.show()
+
+# %%
+# Plot data just in alignment window
+if do_temporal_plots:
+    fig, ax = plt.subplots(2, 1, sharex=True)
+    ax[0].plot(
+        temporal_cal_df["#timestamp_vision"].iloc[-window_size:] * 1.0e-9,
+        temporal_cal_df["vision_relative_angle_norm"].iloc[-window_size:],
+        label="Camera",
+        marker=temporal_marker,
+    )
+    ax[0].plot(
+        temporal_cal_df["timestamp_imu"].iloc[-window_size:] * 1.0e-9,
+        temporal_cal_df["imu_relative_angle_norm"].iloc[-window_size:],
+        label="IMU",
+        marker=temporal_marker,
+    )
+    ax[1].plot(
+        temporal_cal_df["#timestamp_vision"].iloc[-window_size:] * 1.0e-9,
+        temporal_cal_df["vision_relative_angle_norm"].iloc[-window_size:],
+        label="Camera",
+        marker=temporal_marker,
+    )
+    ax[1].plot(
+        temporal_cal_df["timestamp_imu"].iloc[-window_size:] * 1.0e-9 + t_imu_cam_s,
+        temporal_cal_df["imu_relative_angle_norm"].iloc[-window_size:],
+        label="IMU",
+        marker=temporal_marker,
+    )
+    ax[0].legend()
+    ax[0].set_xlabel("Time [s]")
+    ax[0].set_ylabel("Rotation Angle [degrees]")
+    ax[0].set_title("Rotation angles before time-alignment")
+    ax[1].set_ylabel("Rotation Angle [degrees]")
+    ax[1].set_title("Rotation angles after time-alignment")
+    fig.set_size_inches((9, 9))
+    plt.show()
+
+# %%
+if do_temporal_plots:
+    vision_times = temporal_cal_df["#timestamp_vision"]
+    imu_times = temporal_cal_df["timestamp_imu"]
+    N = len(vision_times)
+    delays = np.zeros(2 * N - 1)
+    for i in range(N):
+        delays[i] = (vision_times[0] - imu_times[N - 1 - i]) * 1.0e-9
+        delays[2 * N - 2 - i] = (imu_times[N - 1 - i] - vision_times[0]) * 1.0e-9
+
+    cross_corr = np.correlate(
+        temporal_cal_df["vision_relative_angle_norm"],
+        temporal_cal_df["imu_relative_angle_norm"],
+        mode="full",
+    )
+    fig, ax = plt.subplots()
+    ax.plot(delays, cross_corr, marker=temporal_marker, label="correlation")
+    extents = ax.get_ylim()
+    ax.vlines(
+        t_imu_cam_s,
+        ymin=extents[0],
+        ymax=extents[1],
+        label="Estimated delay [s]",
+        colors="k",
+        linestyles="--",
+    )
+    ax.set_ylim(extents)
+    ax.set_xlabel("t_shift_imu_cam [s]")
+    ax.set_ylabel("correlation")
+    ax.legend()
+    fig.set_size_inches((8, 8))
+    plt.show()