diff --git a/depthai-core b/depthai-core index ea1531bb9..6cf184041 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit ea1531bb92717a11fff2c123b3dc654d014f3c28 +Subproject commit 6cf184041d3446ba9706a8893d620bcaf0f5ac71 diff --git a/utilities/cam_test.py b/utilities/cam_test.py index f92bfe4a3..418535dbd 100755 --- a/utilities/cam_test.py +++ b/utilities/cam_test.py @@ -45,19 +45,22 @@ def socket_type_pair(arg): socket, type = arg.split(',') - if not (socket in ['rgb', 'left', 'right', 'camd', 'came', 'camf']): raise ValueError("") - if not (type in ['m', 'mono', 'c', 'color']): raise ValueError("") + if not (socket in ['rgb', 'left', 'right', 'cama', 'camb', 'camc', 'camd', 'came', 'camf']): raise ValueError("") + if not (type in ['m', 'mono', 'c', 'color', 't', 'tof']): raise ValueError("") is_color = True if type in ['c', 'color'] else False - return [socket, is_color] + is_tof = True if type in ['t', 'tof'] else False + return [socket, is_color, is_tof] + parser = argparse.ArgumentParser() parser.add_argument('-cams', '--cameras', type=socket_type_pair, nargs='+', - default=[['rgb', True], ['left', False], ['right', False], ['camd', True]], - help="Which camera sockets to enable, and type: c[olor] / m[ono]. " + default=[['rgb', True, False], ['left', False, False], + ['right', False, False], ['camd', True, False]], + help="Which camera sockets to enable, and type: c[olor] / m[ono] / t[of]. " "E.g: -cams rgb,m right,c . Default: rgb,c left,m right,m camd,c") parser.add_argument('-mres', '--mono-resolution', type=int, default=800, choices={480, 400, 720, 800, 1200, 4000, 4224}, help="Select mono camera resolution (height). Default: %(default)s") -parser.add_argument('-cres', '--color-resolution', default='1080', choices={'720', '800', '1080', '1200', '1500', '1520', '1560', '4000', '4k', '5mp', '12mp', '13mp', '48mp'}, +parser.add_argument('-cres', '--color-resolution', default='1080', choices={'720', '800', '962', '1080', '1200', '1500', '1520', '1560', '4000', '4k', '5mp', '12mp', '13mp', '48mp'}, help="Select color camera resolution / height. Default: %(default)s") parser.add_argument('-rot', '--rotate', const='all', choices={'all', 'rgb', 'mono'}, nargs="?", help="Which cameras to rotate 180 degrees. All if not filtered") @@ -69,15 +72,27 @@ def socket_type_pair(arg): help="Make OpenCV windows resizable. Note: may introduce some artifacts") parser.add_argument('-raw', '--enable-raw', default=False, action="store_true", help='Enable the RAW camera streams') +parser.add_argument('-tofraw', '--tof-raw', action='store_true', + help="Show just ToF raw output instead of post-processed depth") +parser.add_argument('-tofamp', '--tof-amplitude', action='store_true', + help="Show also ToF amplitude output alongside depth") +parser.add_argument('-tofcm', '--tof-cm', action='store_true', + help="Show ToF depth output in centimeters, capped to 255") +parser.add_argument('-tofmedian', '--tof-median', choices=[0,3,5,7], default=5, type=int, + help="ToF median filter kernel size") +parser.add_argument('-rgbprev', '--rgb-preview', action='store_true', + help="Show RGB `preview` stream instead of full size `isp`") args = parser.parse_args() cam_list = [] cam_type_color = {} +cam_type_tof = {} print("Enabled cameras:") -for socket, is_color in args.cameras: +for socket, is_color, is_tof in args.cameras: cam_list.append(socket) cam_type_color[socket] = is_color - print(socket.rjust(7), ':', 'color' if is_color else 'mono') + cam_type_tof[socket] = is_tof + print(socket.rjust(7), ':', 'tof' if is_tof else 'color' if is_color else 'mono') print("DepthAI version:", dai.__version__) print("DepthAI path:", dai.__file__) @@ -86,27 +101,22 @@ def socket_type_pair(arg): 'rgb' : dai.CameraBoardSocket.CAM_A, 'left' : dai.CameraBoardSocket.CAM_B, 'right': dai.CameraBoardSocket.CAM_C, + 'cama' : dai.CameraBoardSocket.CAM_A, + 'camb' : dai.CameraBoardSocket.CAM_B, + 'camc' : dai.CameraBoardSocket.CAM_C, 'camd' : dai.CameraBoardSocket.CAM_D, 'came' : dai.CameraBoardSocket.CAM_E, 'camf' : dai.CameraBoardSocket.CAM_F, } -cam_socket_to_name = { - 'RGB' : 'rgb', - 'LEFT' : 'left', - 'RIGHT': 'right', - 'CAM_A': 'rgb', - 'CAM_B': 'left', - 'CAM_C': 'right', - 'CAM_D': 'camd', - 'CAM_E': 'came', - 'CAM_F': 'camf', -} rotate = { 'rgb' : args.rotate in ['all', 'rgb'], 'left' : args.rotate in ['all', 'mono'], 'right': args.rotate in ['all', 'mono'], + 'cama': args.rotate in ['all', 'rgb'], + 'camb': args.rotate in ['all', 'mono'], + 'camc': args.rotate in ['all', 'mono'], 'camd' : args.rotate in ['all', 'rgb'], 'came' : args.rotate in ['all', 'rgb'], 'camf' : args.rotate in ['all', 'rgb'], @@ -125,6 +135,7 @@ def socket_type_pair(arg): color_res_opts = { '720': dai.ColorCameraProperties.SensorResolution.THE_720_P, '800': dai.ColorCameraProperties.SensorResolution.THE_800_P, + '962': dai.ColorCameraProperties.SensorResolution.THE_1280X962, '1080': dai.ColorCameraProperties.SensorResolution.THE_1080_P, '1200': dai.ColorCameraProperties.SensorResolution.THE_1200_P, '1500': dai.ColorCameraProperties.SensorResolution.THE_2000X1500, @@ -149,7 +160,10 @@ def __init__(self, window_size=30): def update(self, timestamp=None): if timestamp == None: timestamp = time.monotonic() count = len(self.dq) - if count > 0: self.fps = count / (timestamp - self.dq[0]) + try: + if count > 0: self.fps = count / (timestamp - self.dq[0]) + except ZeroDivisionError: + pass self.dq.append(timestamp) def get(self): @@ -163,20 +177,69 @@ def get(self): control = pipeline.createXLinkIn() control.setStreamName('control') +xinTofConfig = pipeline.createXLinkIn() +xinTofConfig.setStreamName('tofConfig') + cam = {} +tof = {} xout = {} xout_raw = {} +xout_tof_amp = {} +xout_tof_isp = {} # TMP dummy, raw-only seems to hang on ColorCamera streams = [] +tofConfig = {} for c in cam_list: + tofEnableRaw = False xout[c] = pipeline.createXLinkOut() xout[c].setStreamName(c) streams.append(c) - if cam_type_color[c]: + if cam_type_tof[c]: + cam[c] = pipeline.create(dai.node.ColorCamera) # .Camera + cam[c].setResolution(dai.ColorCameraProperties.SensorResolution.THE_1280X962) + if 1: # ColorCamera with 'raw' only would hang, stream 'isp' as well... + xout_tof_isp[c] = pipeline.createXLinkOut() + name = 'dummy_tof_isp_' + c + xout_tof_isp[c].setStreamName(name) + streams.append(name) + cam[c].isp.link(xout_tof_isp[c].input) + if args.tof_raw: + tofEnableRaw = True + else: + tof[c] = pipeline.create(dai.node.ToF) + cam[c].raw.link(tof[c].input) + tof[c].depth.link(xout[c].input) + xinTofConfig.out.link(tof[c].inputConfig) + tofConfig = tof[c].initialConfig.get() + if 0: # From RVC2, TODO implement + tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN + tofConfig.depthParams.avgPhaseShuffle = False + tofConfig.depthParams.minimumAmplitude = 3.0 + + if args.tof_median == 0: + tofConfig.depthParams.median = dai.MedianFilter.MEDIAN_OFF + elif args.tof_median == 3: + tofConfig.depthParams.median = dai.MedianFilter.KERNEL_3x3 + elif args.tof_median == 5: + tofConfig.depthParams.median = dai.MedianFilter.KERNEL_5x5 + elif args.tof_median == 7: + tofConfig.depthParams.median = dai.MedianFilter.KERNEL_7x7 + + tof[c].initialConfig.set(tofConfig) + if args.tof_amplitude: + amp_name = 'tof_amplitude_' + c + xout_tof_amp[c] = pipeline.create(dai.node.XLinkOut) + xout_tof_amp[c].setStreamName(amp_name) + streams.append(amp_name) + tof[c].amplitude.link(xout_tof_amp[c].input) + elif cam_type_color[c]: cam[c] = pipeline.createColorCamera() cam[c].setResolution(color_res_opts[args.color_resolution]) cam[c].setIspScale(1, args.isp_downscale) #cam[c].initialControl.setManualFocus(85) # TODO - cam[c].isp.link(xout[c].input) + if args.rgb_preview: + cam[c].preview.link(xout[c].input) + else: + cam[c].isp.link(xout[c].input) else: cam[c] = pipeline.createMonoCamera() cam[c].setResolution(mono_res_opts[args.mono_resolution]) @@ -196,13 +259,13 @@ def get(self): if rotate[c]: cam[c].setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) cam[c].setFps(args.fps) - if args.enable_raw: + if args.enable_raw or tofEnableRaw: raw_name = 'raw_' + c xout_raw[c] = pipeline.create(dai.node.XLinkOut) xout_raw[c].setStreamName(raw_name) - if args.enable_raw: - streams.append(raw_name) + streams.append(raw_name) cam[c].raw.link(xout_raw[c].input) + #cam[c].setRawOutputPacked(False) # TODO implement if 0: print("=== Using custom camera tuning, and limiting RGB FPS to 10") @@ -219,10 +282,11 @@ def get(self): print(f' -socket {p.socket.name:6}: {p.sensorName:6} {p.width:4} x {p.height:4} focus:', end='') print('auto ' if p.hasAutofocus else 'fixed', '- ', end='') print(*[type.name for type in p.supportedTypes]) - socket_name = cam_socket_to_name[p.socket.name] - cam_name[socket_name] = p.sensorName + cam_name[p.socket.name] = p.sensorName if args.enable_raw: - cam_name['raw_'+socket_name] = p.sensorName + cam_name['raw_'+p.socket.name] = p.sensorName + if args.tof_amplitude: + cam_name['tof_amplitude_'+p.socket.name] = p.sensorName print('USB speed:', device.getUsbSpeed().name) @@ -243,6 +307,7 @@ def get(self): fps_capt[c] = FPS() controlQueue = device.getInputQueue('control') + tofCfgQueue = device.getInputQueue('tofConfig') # Manual exposure/focus set step EXP_STEP = 500 # us @@ -284,6 +349,9 @@ def get(self): chroma_denoise = 0 control = 'none' + jet_custom = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) + jet_custom[0] = [0, 0, 0] + print("Cam:", *[' ' + c.ljust(8) for c in cam_list], "[host | capture timestamp]") capture_list = [] @@ -294,9 +362,19 @@ def get(self): fps_host[c].update() fps_capt[c].update(pkt.getTimestamp().total_seconds()) width, height = pkt.getWidth(), pkt.getHeight() + frame = pkt.getCvFrame() + cam_skt = c.split('_')[-1] + if cam_type_tof[cam_skt] and not (c.startswith('raw_') or c.startswith('tof_amplitude_') or c.startswith('dummy')): + if args.tof_cm: + # pixels represent `cm`, capped to 255. Value can be checked hovering the mouse + frame = (frame // 10).clip(0, 255).astype(np.uint8) + else: + frame = (frame.view(np.int16).astype(float)) + frame = cv2.normalize(frame, frame, alpha=255, beta=0, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) + frame = cv2.applyColorMap(frame, jet_custom) capture = c in capture_list if capture: - capture_file_info = ('capture_' + c + '_' + cam_name[c] + capture_file_info = ('capture_' + c + '_' + cam_name[cam_socket_opts[cam_skt].name] + '_' + str(width) + 'x' + str(height) + '_exp_' + str(int(pkt.getExposureTime().total_seconds()*1e6)) + '_iso_' + str(pkt.getSensitivity()) @@ -306,7 +384,7 @@ def get(self): ) capture_list.remove(c) print() - if c.startswith('raw_'): + if c.startswith('raw_') or c.startswith('tof_amplitude_'): # Resize is done to skip the +50 black lines we get with RVC3. # TODO: handle RAW10/RAW12 to work with getFrame/getCvFrame payload = pkt.getData().view(np.uint16).copy() @@ -319,7 +397,7 @@ def get(self): # Full range for display, use bits [15:6] of the 16-bit pixels frame = payload * (1 << 6) # Debayer color for preview/png - if cam_type_color[c.split('_')[-1]]: + if cam_type_color[cam_skt]: # See this for the ordering, at the end of page: # https://docs.opencv.org/4.5.1/de/d25/imgproc_color_conversions.html # TODO add bayer order to ImgFrame getType() @@ -331,7 +409,6 @@ def get(self): filename = capture_file_info + '_NV12.yuv' print('Saving:', filename) payload.tofile(filename) - frame = pkt.getCvFrame() if capture: filename = capture_file_info + '.png' print('Saving:', filename) @@ -347,15 +424,15 @@ def get(self): elif key == ord('c'): capture_list = streams.copy() capture_time = time.strftime('%Y%m%d_%H%M%S') - elif key == ord('x'): - ctrl = dai.CameraControl() - stopped = not stopped - print("Streaming", "stop" if stopped else "start") - if stopped: - ctrl.setStopStreaming() - else: - ctrl.setStartStreaming() - controlQueue.send(ctrl) + elif key == ord('g') and tof: + f_mod = dai.RawToFConfig.DepthParams.TypeFMod.MAX if tofConfig.depthParams.freqModUsed == dai.RawToFConfig.DepthParams.TypeFMod.MIN else dai.RawToFConfig.DepthParams.TypeFMod.MIN + print("ToF toggling f_mod value to:", f_mod) + tofConfig.depthParams.freqModUsed = f_mod + tofCfgQueue.send(tofConfig) + elif key == ord('h') and tof: + tofConfig.depthParams.avgPhaseShuffle = not tofConfig.depthParams.avgPhaseShuffle + print("ToF toggling avgPhaseShuffle value to:", tofConfig.depthParams.avgPhaseShuffle) + tofCfgQueue.send(tofConfig) elif key == ord('t'): print("Autofocus trigger (and disable continuous)") ctrl = dai.CameraControl() @@ -423,17 +500,29 @@ def get(self): if floodIntensity < 0: floodIntensity = 0 device.setIrFloodLightBrightness(floodIntensity) - elif key >= 0 and chr(key) in '34567890[]': - if key == ord('3'): control = 'awb_mode' - elif key == ord('4'): control = 'ae_comp' - elif key == ord('5'): control = 'anti_banding_mode' - elif key == ord('6'): control = 'effect_mode' - elif key == ord('7'): control = 'brightness' - elif key == ord('8'): control = 'contrast' - elif key == ord('9'): control = 'saturation' - elif key == ord('0'): control = 'sharpness' - elif key == ord('['): control = 'luma_denoise' - elif key == ord(']'): control = 'chroma_denoise' + elif key >= 0 and chr(key) in '34567890[]p': + if key == ord('3'): + control = 'awb_mode' + elif key == ord('4'): + control = 'ae_comp' + elif key == ord('5'): + control = 'anti_banding_mode' + elif key == ord('6'): + control = 'effect_mode' + elif key == ord('7'): + control = 'brightness' + elif key == ord('8'): + control = 'contrast' + elif key == ord('9'): + control = 'saturation' + elif key == ord('0'): + control = 'sharpness' + elif key == ord('['): + control = 'luma_denoise' + elif key == ord(']'): + control = 'chroma_denoise' + elif key == ord('p'): + control = 'tof_amplitude_min' print("Selected control:", control) elif key in [ord('-'), ord('_'), ord('+'), ord('=')]: change = 0 @@ -482,4 +571,9 @@ def get(self): chroma_denoise = clamp(chroma_denoise + change, 0, 4) print("Chroma denoise:", chroma_denoise) ctrl.setChromaDenoise(chroma_denoise) + elif control == 'tof_amplitude_min' and tof: + amp_min = clamp(tofConfig.depthParams.minimumAmplitude + change, 0, 50) + print("Setting min amplitude(confidence) to:", amp_min) + tofConfig.depthParams.minimumAmplitude = amp_min + tofCfgQueue.send(tofConfig) controlQueue.send(ctrl)