diff --git a/draft/sync_time.py b/draft/sync_time.py index f88bf27..3239725 100644 --- a/draft/sync_time.py +++ b/draft/sync_time.py @@ -34,12 +34,16 @@ robot_ts_map = { } device_device_list: list[int] = [] +device_seq_list: list[int] = [] for k in device_host_arr: seq, encoder, host_ts, device_deivce, buffer = b2t( device_ts_map[k].open("rb").read(100) ) - device_device_list.append(device_deivce) + device_device_list.append(device_deivce * 1000) + device_seq_list.append(seq) device_device_arr = np.array(device_device_list) +device_seq_arr = np.array(device_seq_list) +device_device_zero = device_device_arr[0] robot_host_arr = np.array(sorted(robot_ts_map.keys())) robot_device_arr = np.array([robot_ts_map[k]["device_ts"] for k in robot_host_arr]) camera_host_arr = np.array(sorted(camera_ts_map.keys())) @@ -81,49 +85,49 @@ print("device_host - robot_device", (device_host_diff - robot_device_diff) / 10* print("device_host - camera_host", (device_host_diff - camera_host_diff) / 10**9) print("device_deivce - robot_device", (device_deivce_diff - robot_device_diff) / 10**9) +device_device_cut = device_device_arr - device_device_zero -x = (device_host_arr - device_host_zero) / 10**9 -y = np.zeros_like(x) -plt.scatter(x, y) +robot_device_cut = ( + robot_device_arr[robot_host_id0 : robot_host_id1 + 1] + - robot_device_arr[robot_host_id0] + - robot_d0 +) -x = (robot_host_arr[robot_host_id0 : robot_host_id1 + 1] - device_host_zero) / 10**9 -y = np.zeros_like(x) + 1 -plt.scatter(x, y) +camera_host_cut = ( + camera_host_arr[camera_host_id0 : camera_host_id1 + 1] - device_host_zero +) -x = (camera_host_arr[camera_host_id0 : camera_host_id1 + 1] - device_host_zero) / 10**9 -y = np.zeros_like(x) + 2 -plt.scatter(x, y) -plt.show() +# x = (device_host_arr - device_host_zero) / 10**9 +# y = np.zeros_like(x) +# plt.scatter(x, y) -# for i, a in enumerate(ass): +# x = (robot_host_arr[robot_host_id0 : robot_host_id1 + 1] - device_host_zero) / 10**9 +# y = np.zeros_like(x) + 1 +# plt.scatter(x, y) -# try: -# # a - func(a, list(robot_ts_map.keys())) -# print(i, a-func(a, list(robot_ts_map.keys()))) -# except Exception: -# print(i,a) -# pass -# a = 174945200000000000 -# print(func(a, list(robot_ts_map.keys()))) +# x = camera_host_cut / 10**9 +# y = np.zeros_like(x) + 2 +# plt.scatter(x, y) -# print(ass[1766-1:1766+2]) -# seq, encoder, host_ts, device_ts, buffer = b2t(device_ts_map[1749444627751252600].read_bytes()) -# print(seq) -# seq, encoder, host_ts, device_ts, buffer = b2t(device_ts_map[1749442951214858600].read_bytes()) -# print(seq) -# seq, encoder, host_ts, device_ts, buffer = b2t(device_ts_map[1749444583738329700].read_bytes()) -# print(seq) +# x = device_device_cut / 10**9 +# y = np.zeros_like(x) + 3 +# plt.scatter(x, y) -# 1766 1749442951214858600 -# 2271 1749442950915344400 -# 2272 1749442950944313600 -# 2273 1749442950961272400 -# 2274 1749442950993404700 -# 2275 1749442951056585700 -# 2276 1749442951061834900 -# 2277 1749442951119567900 -# 2278 1749442951139249100 -# 2279 1749442951144567800 -# 2280 1749442951149875300 -# 2281 1749442951169837900 -# 2282 1749442951197964400 +# x = robot_device_cut / 10**9 +# y = np.zeros_like(x) + 4 +# plt.scatter(x, y) + +# plt.show() + +for i, k in enumerate(device_device_cut): + res1, idx1, offset1 = match(k, robot_device_cut) + res2, idx2, offset2 = match(k, camera_host_cut) + + r = robot_ts_map[robot_host_arr[idx1 + robot_host_id0]] + c = camera_ts_map[camera_host_arr[idx2 + camera_host_id0]] + print( + offset1 * 10**-9, + # (device_host_arr[i] - r["host_ts"]) * 10**-9, + offset2 * 10**-9, + # (device_host_arr[i] - camera_host_arr[idx2 + camera_host_id0]) * 10**-9, + )