diff --git a/bindings/python/src/pipeline/datatype/ToFConfigBindings.cpp b/bindings/python/src/pipeline/datatype/ToFConfigBindings.cpp index bc9ece20e6..0a1e55b5f9 100644 --- a/bindings/python/src/pipeline/datatype/ToFConfigBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ToFConfigBindings.cpp @@ -18,6 +18,7 @@ void bind_tofconfig(pybind11::module& m, void* pCallstack) { py::class_, Buffer, std::shared_ptr> toFConfig(m, "ToFConfig", DOC(dai, ToFConfig)); py::enum_ toFConfigProfile(toFConfig, "Profile", DOC(dai, ToFConfig, Profile)); + py::enum_ toFConfigPipeType(toFConfig, "PipeType", DOC(dai, ToFConfig, PipeType)); /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -38,6 +39,11 @@ void bind_tofconfig(pybind11::module& m, void* pCallstack) { .value("HIGH_RANGE", ToFConfig::Profile::HIGH_RANGE) .export_values(); + toFConfigPipeType.value("AUTO", ToFConfig::PipeType::AUTO) + .value("FLOOD", ToFConfig::PipeType::FLOOD) + .value("DOT", ToFConfig::PipeType::DOT) + .export_values(); + toFConfig.def(py::init<>()) .def("__repr__", &ToFConfig::str) // .def(py::init>()) @@ -58,6 +64,18 @@ void bind_tofconfig(pybind11::module& m, void* pCallstack) { .def_readwrite("enablePhaseUnwrapping", &ToFConfig::enablePhaseUnwrapping, DOC(dai, ToFConfig, enablePhaseUnwrapping)) .def_readwrite("phaseUnwrapErrorThreshold", &ToFConfig::phaseUnwrapErrorThreshold, DOC(dai, ToFConfig, phaseUnwrapErrorThreshold)) + // RVC4 / VD55H1 depth post-processing tuning (only honored on RVC4) + .def_readwrite("enableBilateralFilter", &ToFConfig::enableBilateralFilter, DOC(dai, ToFConfig, enableBilateralFilter)) + .def_readwrite("bilateralStdFactor", &ToFConfig::bilateralStdFactor, DOC(dai, ToFConfig, bilateralStdFactor)) + .def_readwrite("bilateralKernelSize", &ToFConfig::bilateralKernelSize, DOC(dai, ToFConfig, bilateralKernelSize)) + .def_readwrite("enableTemporalNoiseReduction", &ToFConfig::enableTemporalNoiseReduction, DOC(dai, ToFConfig, enableTemporalNoiseReduction)) + .def_readwrite("tnrMaxGain", &ToFConfig::tnrMaxGain, DOC(dai, ToFConfig, tnrMaxGain)) + .def_readwrite("tnrStdFactor", &ToFConfig::tnrStdFactor, DOC(dai, ToFConfig, tnrStdFactor)) + .def_readwrite("enableFlyingPixelFilter", &ToFConfig::enableFlyingPixelFilter, DOC(dai, ToFConfig, enableFlyingPixelFilter)) + .def_readwrite("flyingPixelDepthThreshold", &ToFConfig::flyingPixelDepthThreshold, DOC(dai, ToFConfig, flyingPixelDepthThreshold)) + .def_readwrite("flyingPixelMinDepthOccurrence", &ToFConfig::flyingPixelMinDepthOccurrence, DOC(dai, ToFConfig, flyingPixelMinDepthOccurrence)) + .def_readwrite("pipeType", &ToFConfig::pipeType, DOC(dai, ToFConfig, pipeType)) + .def("setMedianFilter", &ToFConfig::setMedianFilter, DOC(dai, ToFConfig, setMedianFilter)) .def("setProfilePreset", &ToFConfig::setProfilePreset, DOC(dai, ToFConfig, setProfilePreset)) diff --git a/bindings/python/src/pipeline/node/ToFBindings.cpp b/bindings/python/src/pipeline/node/ToFBindings.cpp index 465dfbc994..9f78ca1445 100644 --- a/bindings/python/src/pipeline/node/ToFBindings.cpp +++ b/bindings/python/src/pipeline/node/ToFBindings.cpp @@ -37,8 +37,10 @@ void bind_tof(pybind11::module& m, void* pCallstack) { .def_readonly("depth", &ToFBase::depth, DOC(dai, node, ToFBase, depth), DOC(dai, node, ToFBase, depth)) .def_readonly("amplitude", &ToFBase::amplitude, DOC(dai, node, ToFBase, amplitude), DOC(dai, node, ToFBase, amplitude)) .def_readonly("intensity", &ToFBase::intensity, DOC(dai, node, ToFBase, intensity), DOC(dai, node, ToFBase, intensity)) + .def_readonly("confidence", &ToFBase::confidence, DOC(dai, node, ToFBase, confidence), DOC(dai, node, ToFBase, confidence)) .def_readonly("phase", &ToFBase::phase, DOC(dai, node, ToFBase, phase), DOC(dai, node, ToFBase, phase)) .def_readonly("raw", &ToFBase::raw, DOC(dai, node, ToFBase, raw), DOC(dai, node, ToFBase, raw)) + .def_readonly("rawInput", &ToFBase::rawInput, DOC(dai, node, ToFBase, rawInput), DOC(dai, node, ToFBase, rawInput)) .def_readonly("initialConfig", &ToFBase::initialConfig, DOC(dai, node, ToFBase, initialConfig), DOC(dai, node, ToFBase, initialConfig)) .def("build", py::overload_cast>(&ToFBase::build), diff --git a/examples/python/ToF/tof_align.py b/examples/python/ToF/tof_align.py index fada11eb8c..dbf2945a9b 100644 --- a/examples/python/ToF/tof_align.py +++ b/examples/python/ToF/tof_align.py @@ -27,8 +27,12 @@ def colorizeDepth(frameDepth: np.ndarray, minDepth: float, maxDepth: float) -> n try: logDepth = np.log(frameDepth.astype(np.float32) + 1e-6) logDepth[invalidMask] = 0.0 - logDepth = np.clip(logDepth, np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6)) - depthFrameColor = np.interp(logDepth, (logDepth[~invalidMask].min(), logDepth[~invalidMask].max()), (0, 255)) + logMin, logMax = np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6) + logDepth = np.clip(logDepth, logMin, logMax) + # Map from the FIXED depth range (not the per-frame min/max) so a given depth + # always maps to the same color -- otherwise the mapping shifts every frame and + # the image flickers. + depthFrameColor = np.interp(logDepth, (logMin, logMax), (0, 255)) depthFrameColor = depthFrameColor.astype(np.uint8) depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) depthFrameColor[invalidMask] = 0 diff --git a/examples/python/ToF/tof_all_queues.py b/examples/python/ToF/tof_all_queues.py index eaffdda00e..d97e4a1a1c 100644 --- a/examples/python/ToF/tof_all_queues.py +++ b/examples/python/ToF/tof_all_queues.py @@ -17,8 +17,12 @@ def colorizeDepth(frame: np.ndarray, minDepth: float, maxDepth: float) -> np.nda try: logDepth = np.log(frame.astype(np.float32) + 1e-6) logDepth[invalidMask] = 0.0 - logDepth = np.clip(logDepth, np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6)) - colored = np.interp(logDepth, (logDepth[~invalidMask].min(), logDepth[~invalidMask].max()), (0, 255)) + logMin, logMax = np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6) + logDepth = np.clip(logDepth, logMin, logMax) + # Map from the FIXED depth range (not the per-frame min/max) so a given depth + # always maps to the same color -- otherwise the mapping shifts every frame and + # the image flickers. + colored = np.interp(logDepth, (logMin, logMax), (0, 255)) colored = colored.astype(np.uint8) colored = cv2.applyColorMap(colored, cv2.COLORMAP_JET) colored[invalidMask] = 0 diff --git a/examples/python/ToF/tof_minimal.py b/examples/python/ToF/tof_minimal.py index b5a5604837..a8a94f1581 100644 --- a/examples/python/ToF/tof_minimal.py +++ b/examples/python/ToF/tof_minimal.py @@ -17,8 +17,12 @@ def colorizeDepth(frame: np.ndarray, minDepth: float, maxDepth: float) -> np.nda try: logDepth = np.log(frame.astype(np.float32) + 1e-6) logDepth[invalidMask] = 0.0 - logDepth = np.clip(logDepth, np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6)) - colored = np.interp(logDepth, (logDepth[~invalidMask].min(), logDepth[~invalidMask].max()), (0, 255)) + logMin, logMax = np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6) + logDepth = np.clip(logDepth, logMin, logMax) + # Map from the FIXED depth range (not the per-frame min/max) so a given depth + # always maps to the same color -- otherwise the mapping shifts every frame and + # the image flickers. + colored = np.interp(logDepth, (logMin, logMax), (0, 255)) colored = colored.astype(np.uint8) colored = cv2.applyColorMap(colored, cv2.COLORMAP_JET) colored[invalidMask] = 0 diff --git a/examples/python/ToF/tof_raw_rvc4.py b/examples/python/ToF/tof_raw_rvc4.py new file mode 100644 index 0000000000..e014888981 --- /dev/null +++ b/examples/python/ToF/tof_raw_rvc4.py @@ -0,0 +1,244 @@ +#!/usr/bin/env python3 +"""Test script: capture raw depth via Camera + ToFBase pipeline, save to temp dir, verify, +then replay the saved raw sensor frames back into a fresh ToFBase node and plot the result. + +Capture pipeline: + ┌─────────────────────────┐ + │ Camera (CAM_D, ToF) │ + └──────────┬──────────────┘ + │ .raw + ▼ + ┌──────────────────────────┐ + │ ToFBase │ + └────┬──────────┬──────────┘ + │ .raw │ .depth (also .amplitude, not captured here) + ▼ ▼ + raw_q depth_q + │ │ + ▼ ▼ + raw_.npz depth_.npy <-- saved and validated + +Replay pipeline (no Camera node, raw frames fed from disk): + raw_.npz --> rawInput queue --> ToFBase --> .depth --> depth_q --> matplotlib + +Usage: + python tof_raw_rvc4.py + python tof_raw_rvc4.py --socket CAM_D --frames 5 + python tof_raw_rvc4.py --ip 192.168.1.100 +""" + +import argparse +import os +import sys +import tempfile +import time + +import matplotlib.pyplot as plt +import numpy as np + +import depthai as dai + + +WARMUP_FRAMES = 10 + + +def parse_args(): + parser = argparse.ArgumentParser(description="ToF raw depth save + verify test") + parser.add_argument("--ip", default=None, help="Device IP address (omit for USB)") + parser.add_argument("--socket", default="CAM_D", help="ToF camera board socket (default: CAM_D)") + parser.add_argument("--frames", type=int, default=3, + help="Number of depth frames to save (default: 3)") + parser.add_argument("--fwp", default=None, + help="Optional path to RVC4 firmware package (.tar.xz)") + parser.add_argument("--plot-out", default=None, + help="Path to save the replay depth plot PNG (default: /replay_depth.png)") + return parser.parse_args() + + +def build_capture_pipeline(socket: dai.CameraBoardSocket, profile): + pipeline = dai.Pipeline() + + tof_base = pipeline.create(dai.node.ToFBase) + tof_base.build(boardSocket=socket, profile=profile) + + cam = pipeline.create(dai.node.Camera) + cam.setSensorType(dai.CameraSensorType.TOF) + cam.build(boardSocket=tof_base.getBoardSocket()) + + cam.raw.link(tof_base.rawInput) + + # Save the frames actually fed into rawInput, not ToFBase's own (unreliable on RVC4) .raw passthrough. + raw_q = cam.raw.createOutputQueue() + depth_q = tof_base.depth.createOutputQueue() + + return pipeline, raw_q, depth_q + + +def build_replay_pipeline(socket: dai.CameraBoardSocket, profile): + """Pipeline with only ToFBase (no Camera) -- raw frames are pushed in from the host.""" + pipeline = dai.Pipeline() + + tof_base = pipeline.create(dai.node.ToFBase) + tof_base.build(boardSocket=socket, profile=profile) + + raw_in_q = tof_base.rawInput.createInputQueue() + depth_q = tof_base.depth.createOutputQueue() + + return pipeline, raw_in_q, depth_q + + +def verify_saved_files(out_dir: str, expected_count: int) -> bool: + depth_files = sorted(f for f in os.listdir(out_dir) if f.startswith("depth_") and f.endswith(".npy")) + + print(f"\n[Verify] Expected {expected_count} file(s), found {len(depth_files)}") + if len(depth_files) < expected_count: + print(f"[FAIL] Not enough depth files saved.") + return False + + all_ok = True + for fname in depth_files: + path = os.path.join(out_dir, fname) + size = os.path.getsize(path) + arr = np.load(path) + nonzero = int(np.count_nonzero(arr)) + status = "OK" if nonzero > 0 else "EMPTY" + print(f" {fname}: shape={arr.shape} dtype={arr.dtype} nonzero={nonzero} size={size}B [{status}]") + if nonzero == 0: + print(f" [FAIL] {fname} contains only zeros.") + all_ok = False + + return all_ok + + +def load_raw_frames(out_dir: str): + """Load raw_.npz files saved during capture, sorted by timestamp.""" + raw_files = sorted(f for f in os.listdir(out_dir) if f.startswith("raw_") and f.endswith(".npz")) + frames = [] + for fname in raw_files: + with np.load(os.path.join(out_dir, fname)) as npz: + data = npz["data"] + frame_type = getattr(dai.ImgFrame.Type, str(npz["type"])) + ts = int(fname[len("raw_"):-len(".npz")]) + frames.append((ts, data, frame_type)) + return frames + + +def replay_raw_frames(socket: dai.CameraBoardSocket, profile, out_dir: str): + """Feed saved raw frames back into a fresh ToFBase node and collect the resulting depth frames.""" + raw_frames = load_raw_frames(out_dir) + if not raw_frames: + print("[Replay] No raw frames found to replay.") + return [] + + print(f"\n[Replay] Feeding {len(raw_frames)} saved raw frame(s) back into ToFBase...") + pipeline, raw_in_q, depth_q = build_replay_pipeline(socket, profile) + + depth_frames = [] + with pipeline as p: + p.start() + for ts, data, frame_type in raw_frames: + img = dai.ImgFrame() + img.setCvFrame(data, frame_type) + raw_in_q.send(img) + + depth_msg = depth_q.get() + depth_frames.append((ts, depth_msg.getFrame())) + print(f"[Replay] ts={ts}ms -> depth={depth_frames[-1][1].shape}") + + return depth_frames + + +def plot_depth_frames(depth_frames, out_path: str): + """Plot replayed depth frames side by side and save to a PNG file.""" + n = len(depth_frames) + fig, axes = plt.subplots(1, n, figsize=(5 * n, 4), squeeze=False) + + for ax, (ts, depth) in zip(axes[0], depth_frames): + im = ax.imshow(depth, cmap="turbo") + ax.set_title(f"ts={ts}ms") + ax.axis("off") + fig.colorbar(im, ax=ax, fraction=0.046, pad=0.04) + + fig.suptitle("Replayed ToF depth (from saved raw frames)") + fig.tight_layout() + fig.savefig(out_path, dpi=150) + print(f"[Plot] Saved to: {out_path}") + + try: + plt.show() + except Exception: + pass + + +def main(): + args = parse_args() + + if args.ip: + os.environ["DEPTHAI_DEVICE_NAME_LIST"] = args.ip + if args.fwp: + os.environ["DEPTHAI_DEVICE_RVC4_FWP"] = args.fwp + + socket = getattr(dai.CameraBoardSocket, args.socket) + preset = dai.ToFConfig.Profile.HIGH_RANGE + + print(f"[Config] socket={args.socket}, frames={args.frames}") + print(f"[Config] DepthAI {dai.__version__}") + + out_dir = tempfile.mkdtemp(prefix="tof_depth_test_") + print(f"[Temp] Saving to: {out_dir}") + + pipeline, raw_q, depth_q = build_capture_pipeline(socket, preset) + + saved = 0 + frame_count = 0 + warmup_done = False + + with pipeline as p: + p.start() + print(f"[Pipeline] Running (warmup={WARMUP_FRAMES} frames)...") + + t_start = time.monotonic() + + while p.isRunning() and saved < args.frames: + depth_msg = depth_q.get() + raw_msg = raw_q.get() + frame_count += 1 + + if not warmup_done: + if frame_count >= WARMUP_FRAMES: + warmup_done = True + print(f"[Warmup] Done after {frame_count} frames. Starting capture...") + continue + + depth_data = depth_msg.getFrame() + ts = int(depth_msg.getTimestamp().total_seconds() * 1000) + path = os.path.join(out_dir, f"depth_{ts}.npy") + np.save(path, depth_data) + + raw_data = raw_msg.getFrame() + raw_path = os.path.join(out_dir, f"raw_{ts}.npz") + np.savez(raw_path, data=raw_data, type=np.array(raw_msg.getType().name)) + + saved += 1 + print(f"[Save {saved}/{args.frames}] depth={depth_data.shape} raw={raw_data.shape} ts={ts}ms") + + elapsed = time.monotonic() - t_start + print(f"[Done] {frame_count} total frames in {elapsed:.1f}s") + + passed = verify_saved_files(out_dir, args.frames) + + print("\n" + ("=" * 40)) + if passed: + print("RESULT: PASS — depth files saved and contain valid data") + else: + print("RESULT: FAIL — see errors above") + sys.exit(1) + + depth_frames = replay_raw_frames(socket, preset, out_dir) + if depth_frames: + plot_out = args.plot_out or os.path.join(out_dir, "replay_depth.png") + plot_depth_frames(depth_frames, plot_out) + + +if __name__ == "__main__": + main() diff --git a/examples/python/ToF/tof_rvc4_exposed_settings.py b/examples/python/ToF/tof_rvc4_exposed_settings.py new file mode 100644 index 0000000000..f9c5e9b7f9 --- /dev/null +++ b/examples/python/ToF/tof_rvc4_exposed_settings.py @@ -0,0 +1,480 @@ +#!/usr/bin/env python3 +"""RVC4 ToF example exercising the newly exposed VD55H1 IPP post-processing settings. + +The following ToFConfig fields are only honored on RVC4 (VD55H1) devices and map +directly onto STMicroelectronics IPP controls. Each is optional -- leaving one unset +keeps the IPP/wrapper default: + + Bilateral spatial filter: + enableBilateralFilter, bilateralStdFactor, bilateralKernelSize + Temporal noise reduction (TNR): + enableTemporalNoiseReduction, tnrMaxGain, tnrStdFactor + Flying-pixel filter: + enableFlyingPixelFilter, flyingPixelDepthThreshold, flyingPixelMinDepthOccurrence + Illumination pipe type: + pipeType (AUTO / FLOOD / DOT) + +This script builds a ToF pipeline, applies the exposed settings via the initial +config, streams the depth, amplitude, intensity and confidence outputs, and +(in --test mode) verifies that all four streams produce data with the settings +applied -- confirming the firmware accepts them. + +Requires a firmware package that implements these controls. Point to it with either +the --fwp flag or the DEPTHAI_DEVICE_RVC4_FWP environment variable, e.g.: + + export DEPTHAI_DEVICE_RVC4_FWP=/path/to/depthai-device-rvc4-fwp.tar.xz + python tof_rvc4_exposed_settings.py --ip 10.11.0.86 + +Usage: + python tof_rvc4_exposed_settings.py --ip 10.11.0.86 # live display + python tof_rvc4_exposed_settings.py --ip 10.11.0.86 --test # headless verify +""" + +import argparse +import sys + +import cv2 +import numpy as np + +import depthai as dai + + +def colorizeDepth(frame: np.ndarray, minDepth: float, maxDepth: float) -> np.ndarray: + invalidMask = frame == 0 + try: + logDepth = np.log(frame.astype(np.float32) + 1e-6) + logDepth[invalidMask] = 0.0 + logMin, logMax = np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6) + logDepth = np.clip(logDepth, logMin, logMax) + # Map from the FIXED depth range (not the per-frame min/max) so a given depth + # always maps to the same color -- otherwise the mapping shifts every frame and + # the image flickers. + colored = np.interp(logDepth, (logMin, logMax), (0, 255)) + colored = colored.astype(np.uint8) + colored = cv2.applyColorMap(colored, cv2.COLORMAP_JET) + colored[invalidMask] = 0 + except (IndexError, ValueError): + colored = np.zeros((*frame.shape, 3), dtype=np.uint8) + return colored + + +def normalizeFrame(frame: np.ndarray) -> np.ndarray: + return cv2.normalize(frame, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U) + + +# Live-tuning trackbar spec. Each tuple: (slider label, config attribute, max slider +# value, scale). The config value sent to the device is `slider_position * scale` +# (cast back to int for integer-typed fields). +# +# Ranges/defaults below follow the VD55H1 IPP filter reference (depthai-device-kb, +# TOF_IPP_FILTERS.md), which documents the underlying wrapper controls these fields +# map onto. depthai-core forwards each field to the firmware unchanged (no unit +# conversion), so the values here are exactly the IPP control values. +TUNE_PARAMS = [ + ("bilat: enable", "enableBilateralFilter", 1, 1.0), + # NR stdFactor: photometric sigma multiplier. IPP default 4.0; higher -> more + # smoothing. Slider covers 0.1..8.0 (step 0.1) so the default sits mid-range. + ("bilat: stdFactor x10", "bilateralStdFactor", 80, 0.1), + # NR kernelSize: IPP valid range {3,15} in ODD steps only (step 2); default 5. + # Even/zero values are invalid, so this slider is snapped to odd via ODD_ATTRS. + ("bilat: kernelSize", "bilateralKernelSize", 15, 1.0), + ("tnr: enable", "enableTemporalNoiseReduction", 1, 1.0), + # TNR maxGain: max temporal integration (frames blended). IPP default 16; a gain + # of 0 is meaningless so the slider is clamped to >= 1 (see MIN_SLIDER). + ("tnr: maxGain", "tnrMaxGain", 16, 1.0), + # TNR stdFactor: motion-detection sensitivity. IPP default 1.4 (mode variants 1.2). + # Slider covers 0.05..5.0 (step 0.05). + ("tnr: stdFactor x20", "tnrStdFactor", 100, 0.05), + ("fly: enable", "enableFlyingPixelFilter", 1, 1.0), + # FPC depthTh: neighbour "same-surface" depth threshold in MILLIMETRES. IPP + # default 100 mm (per-mode overrides span ~30..230 mm). Slider covers 0..300 mm. + ("fly: depthThr (mm)", "flyingPixelDepthThreshold", 300, 1.0), + # FPC minDepthOccurence: min neighbours within depthTh (out of the 5x5=25 window) + # required to keep a pixel. IPP default 23; fewer -> more aggressive removal. + ("fly: minOccurrence", "flyingPixelMinDepthOccurrence", 25, 1.0), + # phaseUnwrapErrorThreshold is a plain uint16 (not std::optional), default 100, + # and applyConfig writes it unconditionally -- so every runtime config we send + # overwrites it. Exposing it as a slider keeps that value explicit and tunable. + # (IPP treats a very large value, ~10000, as effectively disabled.) + ("phaseUnwrapErrThr", "phaseUnwrapErrorThreshold", 500, 1.0), + # pipeType is a 3-value enum, rendered as a cycling button (see ENUM_ATTRS) rather + # than a slider; maxval here is just len(choices) - 1. + ("pipe: type", "pipeType", 2, 1.0), +] + +# Config attributes that are integer-typed and must not be assigned a float. +INT_ATTRS = {"bilateralKernelSize", "tnrMaxGain", "phaseUnwrapErrorThreshold"} +BOOL_ATTRS = {"enableBilateralFilter", "enableTemporalNoiseReduction", "enableFlyingPixelFilter"} +# Enum-typed attributes: attr -> ordered tuple of dai.ToFConfig. member names. +# The slider position is the index into this tuple. +ENUM_ATTRS = {"pipeType": ("AUTO", "FLOOD", "DOT")} +# Attributes whose IPP control only accepts ODD values (rounded up to the next odd). +ODD_ATTRS = {"bilateralKernelSize"} +# Minimum allowed slider position per attribute: +# bilateralStdFactor -> IPP rejects a zero std factor (1 -> 0.1 after scaling). +# bilateralKernelSize -> IPP valid range starts at 3 (odd). +# tnrMaxGain -> a gain of 0 is meaningless. +MIN_SLIDER = {"bilateralStdFactor": 1, "bilateralKernelSize": 3, "tnrMaxGain": 1} + + +def _snap(attr: str, pos: int, maxval: int) -> int: + """Clamp a slider position to [MIN_SLIDER, maxval] and enforce odd-only attrs.""" + pos = max(MIN_SLIDER.get(attr, 0), min(maxval, pos)) + if attr in ODD_ATTRS and pos % 2 == 0: + pos = min(maxval, pos + 1) # round up to the next odd value + return pos + + +# Custom slider panel drawn entirely with cv2 primitives. OpenCV's native +# createTrackbar labels do not render on every highgui backend (notably Qt6, and +# inconsistently on others), so instead of fighting that we draw our own sliders -- +# label, track and handle -- as an image and drive them with a mouse callback. This +# renders identically on any backend. +PANEL_W = 660 +ROW_H = 42 +HEADER_H = 40 +LABEL_W = 250 +TRACK_X0 = LABEL_W + 10 +TRACK_X1 = PANEL_W - 95 +BTN_W = 90 # toggle button width (bool rows) +BTN_H = 26 # toggle button height + + +def panel_height() -> int: + return HEADER_H + ROW_H * len(TUNE_PARAMS) + + +def _button_rect(y: int) -> tuple: + """Toggle-button rectangle (x0, y0, x1, y1) centered vertically on row `y`.""" + return TRACK_X0, y - BTN_H // 2, TRACK_X0 + BTN_W, y + BTN_H // 2 + + +def _pos_to_x(pos: int, maxval: int) -> int: + return int(TRACK_X0 + (pos / maxval) * (TRACK_X1 - TRACK_X0)) + + +def _x_to_pos(x: int, maxval: int, minpos: int) -> int: + frac = (x - TRACK_X0) / (TRACK_X1 - TRACK_X0) + frac = min(1.0, max(0.0, frac)) + return max(minpos, min(maxval, int(round(frac * maxval)))) + + +def _row_center_y(row: int) -> int: + return HEADER_H + ROW_H * row + ROW_H // 2 + + +def initial_positions(cfg: dai.ToFConfig) -> list: + """Slider positions (ints) initialized from a ToFConfig.""" + positions = [] + for label, attr, maxval, scale in TUNE_PARAMS: + init = getattr(cfg, attr) + if attr in ENUM_ATTRS: + choices = ENUM_ATTRS[attr] + pos = choices.index(init.name) if init is not None else 0 + else: + pos = int(round((bool(init) if attr in BOOL_ATTRS else (init or 0)) / scale)) + positions.append(_snap(attr, pos, maxval)) + return positions + + +def panel_mouse(event, x, y, flags, state) -> None: + """Mouse callback: bool/enum rows toggle or cycle on click; slider rows follow drag. + + The row is latched in state["drag_row"] on mouse-down and reused for every + subsequent move until mouse-up, instead of being recomputed from the current y + each call -- otherwise a fast drag that drifts vertically hands control to + whichever row the cursor happens to pass over. + """ + if event == cv2.EVENT_LBUTTONUP: + state["drag_row"] = None + return + + if event == cv2.EVENT_LBUTTONDOWN: + row = (y - HEADER_H) // ROW_H + if not (0 <= row < len(TUNE_PARAMS)): + return + state["drag_row"] = row + elif event == cv2.EVENT_MOUSEMOVE and flags & cv2.EVENT_FLAG_LBUTTON: + row = state.get("drag_row") + if row is None: + return + else: + return + + _label, attr, maxval, _scale = TUNE_PARAMS[row] + + if attr in BOOL_ATTRS: + # Toggle only on the initial click (not on drag), so it doesn't flip repeatedly. + if event == cv2.EVENT_LBUTTONDOWN: + bx0, _by0, bx1, _by1 = _button_rect(_row_center_y(row)) + if bx0 <= x <= bx1: + state["pos"][row] = 0 if state["pos"][row] else 1 + return + + if attr in ENUM_ATTRS: + # Cycle through the enum's choices only on the initial click. + if event == cv2.EVENT_LBUTTONDOWN: + bx0, _by0, bx1, _by1 = _button_rect(_row_center_y(row)) + if bx0 <= x <= bx1: + state["pos"][row] = (state["pos"][row] + 1) % len(ENUM_ATTRS[attr]) + return + + state["pos"][row] = _snap(attr, _x_to_pos(x, maxval, MIN_SLIDER.get(attr, 0)), maxval) + + +def config_from_positions(positions: list) -> dai.ToFConfig: + """Build a ToFConfig from the current slider positions.""" + cfg = dai.ToFConfig() + for i, (label, attr, _maxval, scale) in enumerate(TUNE_PARAMS): + if attr in ENUM_ATTRS: + value = getattr(dai.ToFConfig.PipeType, ENUM_ATTRS[attr][positions[i]]) + else: + raw = positions[i] * scale + if attr in BOOL_ATTRS: + value = bool(raw) + elif attr in INT_ATTRS: + value = int(round(raw)) + else: + value = float(raw) + setattr(cfg, attr, value) + return cfg + + +def render_panel(positions: list) -> np.ndarray: + """Draw the slider panel: each row shows label, track, handle and live value.""" + img = np.full((panel_height(), PANEL_W, 3), 40, dtype=np.uint8) + cv2.putText(img, "Live tuning -- drag sliders, click toggles", (10, 26), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1, cv2.LINE_AA) + for i, (label, attr, maxval, scale) in enumerate(TUNE_PARAMS): + y = _row_center_y(i) + cv2.putText(img, label, (10, y + 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, + (220, 220, 220), 1, cv2.LINE_AA) + + if attr in BOOL_ATTRS: + # Render a toggle button: green "ON" / grey "OFF". + on = bool(positions[i]) + bx0, by0, bx1, by1 = _button_rect(y) + cv2.rectangle(img, (bx0, by0), (bx1, by1), + (70, 160, 70) if on else (70, 70, 70), -1) + cv2.rectangle(img, (bx0, by0), (bx1, by1), (200, 200, 200), 1) + text = "ON" if on else "OFF" + (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.55, 1) + cv2.putText(img, text, (bx0 + (BTN_W - tw) // 2, y + th // 2), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 1, cv2.LINE_AA) + continue + + if attr in ENUM_ATTRS: + # Render a cycling button showing the current enum choice; click advances it. + text = ENUM_ATTRS[attr][positions[i]] + bx0, by0, bx1, by1 = _button_rect(y) + cv2.rectangle(img, (bx0, by0), (bx1, by1), (170, 110, 60), -1) # BGR: blue + cv2.rectangle(img, (bx0, by0), (bx1, by1), (200, 200, 200), 1) + (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + cv2.putText(img, text, (bx0 + (BTN_W - tw) // 2, y + th // 2), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA) + continue + + cv2.line(img, (TRACK_X0, y), (TRACK_X1, y), (90, 90, 90), 2, cv2.LINE_AA) + hx = _pos_to_x(positions[i], maxval) + cv2.circle(img, (hx, y), 7, (120, 220, 120), -1, cv2.LINE_AA) + raw = positions[i] * scale + shown = int(round(raw)) if attr in INT_ATTRS else f"{raw:.3g}" + cv2.putText(img, str(shown), (TRACK_X1 + 8, y + 5), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (120, 220, 120), 1, cv2.LINE_AA) + return img + + +def draw_depth_hover(display: np.ndarray, depth_raw: np.ndarray, mouse: dict) -> None: + """Overlay the depth value (mm) under the mouse cursor onto the depth display.""" + x, y = mouse["x"], mouse["y"] + h, w = depth_raw.shape[:2] + if not (0 <= x < w and 0 <= y < h): + return + d = int(depth_raw[y, x]) + text = f"({x},{y}) {d} mm" if d > 0 else f"({x},{y}) no depth" + cv2.drawMarker(display, (x, y), (255, 255, 255), cv2.MARKER_CROSS, 12, 1, cv2.LINE_AA) + (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + cv2.rectangle(display, (5, 5), (15 + tw, 15 + th), (0, 0, 0), -1) + cv2.putText(display, text, (10, 10 + th), cv2.FONT_HERSHEY_SIMPLEX, 0.5, + (255, 255, 255), 1, cv2.LINE_AA) + + +def parse_args(): + p = argparse.ArgumentParser(description="RVC4 ToF exposed IPP settings example/test") + p.add_argument("--ip", default=None, help="Device IP address (e.g. 10.11.0.86)") + p.add_argument("--socket", default="AUTO", help="ToF board socket (default: AUTO)") + p.add_argument("--test", action="store_true", + help="Headless verification: capture N frames, check all streams produce data, then exit") + p.add_argument("--frames", type=int, default=20, + help="Frames to check in --test mode (default: 20)") + return p.parse_args() + + +def build_exposed_config() -> dai.ToFConfig: + """A ToFConfig with all newly exposed RVC4 IPP post-processing controls set.""" + cfg = dai.ToFConfig() + + # Values below are the VD55H1 IPP defaults (see TOF_IPP_FILTERS.md); tweak to taste. + + # Bilateral spatial filter -- stdFactor default 4.0, kernelSize odd in {3..15}. + cfg.enableBilateralFilter = True + cfg.bilateralStdFactor = 4.0 + cfg.bilateralKernelSize = 5 + + # Temporal noise reduction (TNR) -- maxGain default 16, stdFactor default 1.4. + cfg.enableTemporalNoiseReduction = True + cfg.tnrMaxGain = 16 + cfg.tnrStdFactor = 1.4 + + # Flying-pixel filter -- depthThreshold is in MILLIMETRES (IPP default 100 mm, + # per-mode ~30..230 mm); minDepthOccurrence default 23 (of the 5x5=25 window). + cfg.enableFlyingPixelFilter = True + cfg.flyingPixelDepthThreshold = 100.0 + cfg.flyingPixelMinDepthOccurrence = 23.0 + + # Phase-unwrap error threshold. Plain uint16 (default 100), always applied -- set + # it explicitly so runtime configs don't silently reset it. + cfg.phaseUnwrapErrorThreshold = 100 + + # Illumination pipe type. Optional -- unset keeps the IPP default (AUTO); set + # explicitly here for demonstration. + cfg.pipeType = dai.ToFConfig.PipeType.AUTO + + return cfg + + +def describe(cfg: dai.ToFConfig) -> str: + return ( + " bilateral: enable={} stdFactor={} kernelSize={}\n" + " TNR: enable={} maxGain={} stdFactor={}\n" + " flyingPixel: enable={} depthThreshold={} minDepthOccurrence={}\n" + " phaseUnwrapErrorThreshold={}\n" + " pipeType={}".format( + cfg.enableBilateralFilter, cfg.bilateralStdFactor, cfg.bilateralKernelSize, + cfg.enableTemporalNoiseReduction, cfg.tnrMaxGain, cfg.tnrStdFactor, + cfg.enableFlyingPixelFilter, cfg.flyingPixelDepthThreshold, + cfg.flyingPixelMinDepthOccurrence, cfg.phaseUnwrapErrorThreshold, + cfg.pipeType, + ) + ) + + +def main() -> int: + args = parse_args() + + minDepth, maxDepth = 100, 7000 + socket = getattr(dai.CameraBoardSocket, args.socket) + device = dai.Device(dai.DeviceInfo(args.ip)) if args.ip else None + + pipeline = dai.Pipeline(device) if device is not None else dai.Pipeline() + + tof = pipeline.create(dai.node.ToF) + # The mid-range profile enum moved across depthai versions; support both. + if hasattr(dai.ToFConfig, "Profile"): + tof.build(boardSocket=socket, profile=dai.ToFConfig.Profile.MID_RANGE) + else: + tof.build(boardSocket=socket, presetMode=dai.ImageFiltersPresetMode.TOF_MID_RANGE) + + cfg = build_exposed_config() + tof.setInitialConfig(cfg) + print("Applying exposed RVC4 ToF settings via initial config:") + print(describe(cfg)) + + inputConfigQueue = tof.tofBaseInputConfig.createInputQueue() + outputQueues = { + "depth": tof.depth.createOutputQueue(maxSize=4, blocking=False), + "amplitude": tof.amplitude.createOutputQueue(maxSize=4, blocking=False), + "intensity": tof.intensity.createOutputQueue(maxSize=4, blocking=False), + "confidence": tof.confidence.createOutputQueue(maxSize=4, blocking=False), + } + + with pipeline as p: + dev = p.getDefaultDevice() + if dev.getPlatform() != dai.Platform.RVC4: + print(f"ERROR: this example targets RVC4 devices, got {dev.getPlatform()}", file=sys.stderr) + return 2 + + p.start() + # Also exercise the runtime path: re-send the same config after start. + inputConfigQueue.send(cfg) + + if args.test: + # Count frames per stream in which at least one non-zero pixel was produced. + got = {name: 0 for name in outputQueues} + for i in range(args.frames): + line = [f"frame {i:2d}:"] + for name, queue in outputQueues.items(): + frame = queue.get().getCvFrame() + nonzero = int(np.count_nonzero(frame)) + if nonzero > 0: + got[name] += 1 + pct = 100.0 * nonzero / frame.size if frame.size else 0.0 + line.append(f"{name}={pct:5.1f}%") + print(" ".join(line)) + + print() + all_ok = True + for name, count in got.items(): + ok = count >= max(1, args.frames // 2) + all_ok = all_ok and ok + print(f" {name:10s}: {count}/{args.frames} frames with data [{'OK' if ok else 'FAIL'}]") + print("\nRESULT: PASS -- exposed settings accepted, all streams produced." if all_ok + else "\nRESULT: FAIL -- one or more streams had too little data.") + return 0 if all_ok else 1 + + # Live tuning: trackbars live in their own window so their labels are readable + # and the image windows stay undistorted. Moving any slider re-sends an updated + # ToFConfig to the running node via tofBaseInputConfig. + # Live tuning via a self-drawn slider panel (see render_panel/panel_mouse), + # so the labels render on any OpenCV backend. Dragging a slider re-sends an + # updated ToFConfig to the running node via tofBaseInputConfig. + tune_window = "tuning" + cv2.namedWindow(tune_window, cv2.WINDOW_AUTOSIZE) + panel_state = {"pos": initial_positions(cfg), "drag_row": None} + cv2.setMouseCallback(tune_window, panel_mouse, panel_state) + last_sent = None + + # Track the cursor over the depth window so we can show the depth value there. + cv2.namedWindow("depth") + mouse = {"x": -1, "y": -1} + cv2.setMouseCallback("depth", lambda e, x, y, f, m: m.update(x=x, y=y), mouse) + + latest = {} # name -> most recent raw cv frame + + print("Streaming depth, amplitude, intensity, confidence.") + print("In the 'tuning' window: drag sliders and click the ON/OFF buttons to tune live.") + print("Hover over the 'depth' window to read the depth (mm) under the cursor. " + "Press 'q' to quit.") + while p.isRunning(): + # Push a new config only when the sliders actually changed. + current = config_from_positions(panel_state["pos"]) + key = tuple(getattr(current, attr) for _, attr, _, _ in TUNE_PARAMS) + if key != last_sent: + inputConfigQueue.send(current) + last_sent = key + print("Applied:", " ".join(f"{a}={getattr(current, a)}" for _, a, _, _ in TUNE_PARAMS)) + cv2.imshow(tune_window, render_panel(panel_state["pos"])) + + for name, queue in outputQueues.items(): + frame = queue.tryGet() + if frame is not None: + latest[name] = frame.getCvFrame() + + # Redraw depth every iteration (even without a new frame) so the hover + # readout follows the cursor smoothly. + if "depth" in latest: + depth_display = colorizeDepth(latest["depth"], minDepth, maxDepth) + draw_depth_hover(depth_display, latest["depth"], mouse) + cv2.imshow("depth", depth_display) + for name in ("amplitude", "intensity", "confidence"): + if name in latest: + cv2.imshow(name, normalizeFrame(latest[name])) + + if cv2.waitKey(1) == ord("q"): + break + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/include/depthai/pipeline/datatype/ToFConfig.hpp b/include/depthai/pipeline/datatype/ToFConfig.hpp index caca5ea019..cb236fefdc 100644 --- a/include/depthai/pipeline/datatype/ToFConfig.hpp +++ b/include/depthai/pipeline/datatype/ToFConfig.hpp @@ -17,6 +17,15 @@ class ToFConfig : public Buffer { HIGH_RANGE, }; + /** + * ToF illumination pipe type (RVC4 / VD55H1 only). + */ + enum class PipeType : uint32_t { + AUTO = 0, ///< Pick flood/dot from the sensor's detected illumination type. + FLOOD = 1, ///< Force flood illumination pipe. + DOT = 2, ///< Force dot illumination pipe. + }; + Profile profile = Profile::MID_RANGE; /** * Set kernel size for depth median filtering, or disable @@ -72,6 +81,62 @@ class ToFConfig : public Buffer { */ std::optional enablePhaseUnwrapping; + /* + * RVC4 / VD55H1 depth post-processing tuning. + * + * The fields below map directly onto STMicroelectronics VD55H1 IPP controls + * and are only honored on RVC4 devices. Each is optional: leaving a field + * unset (std::nullopt) keeps the IPP/wrapper default for that control, so an + * unmodified ToFConfig reproduces the previous hardcoded behavior. + */ + + /* + * Bilateral spatial filter. false => filter bypassed. + */ + std::optional enableBilateralFilter; + /* + * Bilateral filter standard-deviation factor (strength). + */ + std::optional bilateralStdFactor; + /* + * Bilateral filter kernel size. + */ + std::optional bilateralKernelSize; + + /* + * Temporal noise reduction (TNR). false => filter bypassed. + */ + std::optional enableTemporalNoiseReduction; + /* + * TNR maximum gain. + */ + std::optional tnrMaxGain; + /* + * TNR standard-deviation factor (strength). + */ + std::optional tnrStdFactor; + + /* + * Flying-pixel filter. false => filter bypassed. + */ + std::optional enableFlyingPixelFilter; + /* + * Flying-pixel filter depth threshold. + */ + std::optional flyingPixelDepthThreshold; + /* + * Flying-pixel filter minimum depth occurrence: minimum number of neighbouring + * pixels at a similar depth required for a pixel to be kept (lower => more + * aggressive removal). Maps to the IPP "min depth occurence" parameter. + */ + std::optional flyingPixelMinDepthOccurrence; + + /* + * ToF illumination pipe type. AUTO (default) selects flood/dot from the sensor's + * detected illumination; FLOOD/DOT force it. Unset keeps the IPP default (AUTO). + */ + std::optional pipeType; + /** * Construct ToFConfig message. */ @@ -107,7 +172,17 @@ class ToFConfig : public Buffer { enableWiggleCorrection, enablePhaseUnwrapping, phaseUnwrappingLevel, - phaseUnwrapErrorThreshold); + phaseUnwrapErrorThreshold, + enableBilateralFilter, + bilateralStdFactor, + bilateralKernelSize, + enableTemporalNoiseReduction, + tnrMaxGain, + tnrStdFactor, + enableFlyingPixelFilter, + flyingPixelDepthThreshold, + flyingPixelMinDepthOccurrence, + pipeType); }; } // namespace dai diff --git a/include/depthai/pipeline/node/ToF.hpp b/include/depthai/pipeline/node/ToF.hpp index 96fda12af7..abaab224cb 100644 --- a/include/depthai/pipeline/node/ToF.hpp +++ b/include/depthai/pipeline/node/ToF.hpp @@ -28,12 +28,6 @@ class ToFBase : public DeviceNodeCRTP { protected: Properties& getProperties(); - /** - * Input for raw sensor frames used by the RVC4 host implementation. - * This stays internal to the node group API, but must remain on the base - * node so the auto-created ToF camera can be linked in the pipeline schema. - */ - Input rawInput{*this, {"rawInput", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::ImgFrame, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; public: ToFBase() = default; @@ -59,6 +53,13 @@ class ToFBase : public DeviceNodeCRTP { Output phase{*this, {"phase", DEFAULT_GROUP, {{{DatatypeEnum::ImgFrame, true}}}}}; Output raw{*this, {"raw", DEFAULT_GROUP, {{{DatatypeEnum::ImgFrame, true}}}}}; + /** + * Input for raw sensor frames used by the RVC4 host implementation. + * When using ToFBase directly (instead of the ToF node group), link a + * Camera node's raw output here to feed the ToF processing pipeline. + */ + Input rawInput{*this, {"rawInput", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::ImgFrame, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + /** * Build with a specific board socket */ @@ -74,7 +75,6 @@ class ToFBase : public DeviceNodeCRTP { private: friend class ToF; - bool isBuilt = false; uint32_t maxWidth = 0; uint32_t maxHeight = 0;