1+ import cv2
2+ import depthai as dai
3+ import time
4+
5+ FPS = 30
6+
7+ pipeline = dai .Pipeline ()
8+
9+ # Define a source - color camera
10+ camRgb = pipeline .create (dai .node .ColorCamera )
11+ # Since we are saving RGB frames in Script node we need to make the
12+ # video pool size larger, otherwise the pipeline will freeze because
13+ # the ColorCamera won't be able to produce new video frames.
14+ camRgb .setVideoNumFramesPool (10 )
15+ camRgb .setFps (FPS )
16+
17+ left = pipeline .create (dai .node .MonoCamera )
18+ left .setResolution (dai .MonoCameraProperties .SensorResolution .THE_400_P )
19+ left .setBoardSocket (dai .CameraBoardSocket .LEFT )
20+ left .setFps (FPS )
21+
22+ right = pipeline .create (dai .node .MonoCamera )
23+ right .setResolution (dai .MonoCameraProperties .SensorResolution .THE_400_P )
24+ right .setBoardSocket (dai .CameraBoardSocket .RIGHT )
25+ right .setFps (FPS )
26+
27+ stereo = pipeline .createStereoDepth ()
28+ stereo .initialConfig .setMedianFilter (dai .MedianFilter .KERNEL_7x7 )
29+ stereo .setLeftRightCheck (True )
30+ stereo .setExtendedDisparity (False )
31+ stereo .setSubpixel (False )
32+ left .out .link (stereo .left )
33+ right .out .link (stereo .right )
34+
35+ # Script node will sync high-res frames
36+ script = pipeline .create (dai .node .Script )
37+
38+ # Send both streams to the Script node so we can sync them
39+ stereo .disparity .link (script .inputs ["disp_in" ])
40+ camRgb .video .link (script .inputs ["rgb_in" ])
41+
42+ script .setScript ("""
43+ FPS=30
44+ import time
45+ from datetime import timedelta
46+ import math
47+
48+ # Timestamp threshold (in miliseconds) under which frames will be considered synced.
49+ # Lower number means frames will have less delay between them, which can potentially
50+ # lead to dropped frames.
51+ MS_THRESHOL=math.ceil(500 / FPS)
52+
53+ def check_sync(queues, timestamp):
54+ matching_frames = []
55+ for name, list in queues.items(): # Go through each available stream
56+ # node.warn(f"List {name}, len {str(len(list))}")
57+ for i, msg in enumerate(list): # Go through each frame of this stream
58+ time_diff = abs(msg.getTimestamp() - timestamp)
59+ if time_diff <= timedelta(milliseconds=MS_THRESHOL): # If time diff is below threshold, this frame is considered in-sync
60+ matching_frames.append(i) # Append the position of the synced frame, so we can later remove all older frames
61+ break
62+
63+ if len(matching_frames) == len(queues):
64+ # We have all frames synced. Remove the excess ones
65+ i = 0
66+ for name, list in queues.items():
67+ queues[name] = queues[name][matching_frames[i]:] # Remove older (excess) frames
68+ i+=1
69+ return True
70+ else:
71+ return False # We don't have synced frames yet
72+
73+ names = ['disp', 'rgb']
74+ frames = dict() # Dict where we store all received frames
75+ for name in names:
76+ frames[name] = []
77+
78+ while True:
79+ for name in names:
80+ f = node.io[name+"_in"].tryGet()
81+ if f is not None:
82+ frames[name].append(f) # Save received frame
83+
84+ if check_sync(frames, f.getTimestamp()): # Check if we have any synced frames
85+ # Frames synced!
86+ node.info(f"Synced frame!")
87+ node.warn(f"Queue size. Disp: {len(frames['disp'])}, rgb: {len(frames['rgb'])}")
88+ for name, list in frames.items():
89+ syncedF = list.pop(0) # We have removed older (excess) frames, so at positions 0 in dict we have synced frames
90+ node.info(f"{name}, ts: {str(syncedF.getTimestamp())}, seq {str(syncedF.getSequenceNum())}")
91+ node.io[name+'_out'].send(syncedF) # Send synced frames to the host
92+
93+
94+ time.sleep(0.001) # Avoid lazy looping
95+ """ )
96+
97+ script_out = ['disp' , 'rgb' ]
98+
99+ for name in script_out : # Create XLinkOut for disp/rgb streams
100+ xout = pipeline .create (dai .node .XLinkOut )
101+ xout .setStreamName (name )
102+ script .outputs [name + '_out' ].link (xout .input )
103+
104+ with dai .Device (pipeline ) as device :
105+ device .setLogLevel (dai .LogLevel .INFO )
106+ device .setLogOutputLevel (dai .LogLevel .INFO )
107+ names = ['rgb' , 'disp' ]
108+ queues = [device .getOutputQueue (name ) for name in names ]
109+
110+ while True :
111+ print ()
112+ for q in queues :
113+ img : dai .ImgFrame = q .get ()
114+ # Display timestamp/sequence number of two synced frames
115+ print (f"[{ time .time ()} { q .getName ()} { img .getTimestamp ()} { img .getSequenceNum ()} )
0 commit comments