Skip to content

Commit

Permalink
Merge branch 'rvc3_develop' into rvc3_custom_segmentation
Browse files Browse the repository at this point in the history
  • Loading branch information
Matevz Morato committed Oct 17, 2023
2 parents 38f2ea3 + 44ca8f7 commit 22fe80c
Show file tree
Hide file tree
Showing 2 changed files with 147 additions and 53 deletions.
2 changes: 1 addition & 1 deletion depthai-core
Submodule depthai-core updated 3 files
+1 −1 cmake/Depthai/DepthaiDeviceKbConfig.cmake
+3 −0 include/depthai/pipeline/nodes.hpp
+22 −21 src/device/CalibrationHandler.cpp
198 changes: 146 additions & 52 deletions utilities/cam_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand All @@ -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__)
Expand All @@ -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'],
Expand All @@ -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,
Expand All @@ -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):
Expand All @@ -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])
Expand All @@ -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")
Expand All @@ -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)

Expand All @@ -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
Expand Down Expand Up @@ -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 = []
Expand All @@ -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())
Expand All @@ -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()
Expand All @@ -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()
Expand All @@ -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)
Expand All @@ -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()
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)

0 comments on commit 22fe80c

Please sign in to comment.