From 228d9417d3ec3a61606593d6d13e30541ae763b8 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 14 Aug 2025 21:54:41 +0200 Subject: [PATCH 001/180] Add HFR support --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 283d21cdd4..f56032ba7e 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -4,4 +4,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" # set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+93f7b75a885aa32f44c5e9f53b74470c49d2b1af") -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+14e10a9f3a31a6a5a8b08382000a1015cefcb1d0") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+8481392f2a23deae8411053724e1b76ebd110cc2") From 1b807ac2d40e4aadc32b9b1fd122828f30bfac74 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 14 Aug 2025 23:49:57 +0200 Subject: [PATCH 002/180] Add HFR examples and README.md --- examples/python/RVC4/HFR/.gitignore | 2 + examples/python/RVC4/HFR/README.md | 70 +++++++++++++++++ examples/python/RVC4/HFR/hfr_nn.py | 37 +++++++++ examples/python/RVC4/HFR/hfr_save_encoded.py | 75 +++++++++++++++++++ examples/python/RVC4/HFR/hfr_small_preview.py | 35 +++++++++ 5 files changed, 219 insertions(+) create mode 100644 examples/python/RVC4/HFR/.gitignore create mode 100644 examples/python/RVC4/HFR/README.md create mode 100644 examples/python/RVC4/HFR/hfr_nn.py create mode 100644 examples/python/RVC4/HFR/hfr_save_encoded.py create mode 100644 examples/python/RVC4/HFR/hfr_small_preview.py diff --git a/examples/python/RVC4/HFR/.gitignore b/examples/python/RVC4/HFR/.gitignore new file mode 100644 index 0000000000..e30a39b0cb --- /dev/null +++ b/examples/python/RVC4/HFR/.gitignore @@ -0,0 +1,2 @@ +*.encoded +*.mp4 \ No newline at end of file diff --git a/examples/python/RVC4/HFR/README.md b/examples/python/RVC4/HFR/README.md new file mode 100644 index 0000000000..8fac981517 --- /dev/null +++ b/examples/python/RVC4/HFR/README.md @@ -0,0 +1,70 @@ +# HFR (High Frame Rate) examples on IMX586 on the RVC4 platform +This directory contains examples demonstrating high frame rate (HFR) capabilities on the IMX586 sensor using the RVC4 platform. +The examples showcase an early preview of the capabilities. + + +## HFR resolutions +The HFR mode introduces two new resolutions that can run at a high frame-rate. +At the current time, the resolutions cannot be scaled arbitrarily nor can the FPS be varied arbitrarily — it must be one of the two supported values. +We plan to add more flexibility in the future and it's possible to use `ImageManip` in the meantime. + +The current supported resolutions are: +* 1920x1080 @ 240 FPS +* 1280x720 @ 480 FPS + + +## Setup +To set up the HFR mode both a custom version of this (DepthAI) repository and a custom version of the LuxonisOS are required. + +### Installation of LuxonisOS: +First download the HFR version of the OS [here](https://luxonisos-rvc4.fra1.digitaloceanspaces.com/OTA/full_update_luxonis_ext4-1.17.0-imx+16bc91752168bda919ce87196bf57d18fb9c43ec.zip) +``` +wget https://luxonisos-rvc4.fra1.digitaloceanspaces.com/OTA/full_update_luxonis_ext4-1.17.0-imx+16bc91752168bda919ce87196bf57d18fb9c43ec.zip +``` + +then copy the OS to the device + +``` +scp full_update_luxonis_ext4-1.17.0-imx+16bc91752168bda919ce87196bf57d18fb9c43ec.zip root@:/data/hfr_os.zip +``` + +ssh into the device and update the OS + +``` +ssh root@ +recovery --update_package=/data/hfr_os.zip --wipe_cache && reboot +``` + +### DepthAI installation +Clone this repository and checkout the `feature/imx586_hfr` branch + +``` +git clone https://github.com/luxonis/depthai-core.git +cd depthai-core +git checkout feature/imx586_hfr +``` + +and install requirements + +``` +python examples/python/install_requirements.py +``` + +The examples can then be found in the `examples/python/RVC4/HFR` directory. + +## Example descriptions +### Object Detection +The object detection [example](hfr_nn.py) demonstrates how to use the HFR mode with a YOLOv6 model for real-time object detection at **480 FPS**. + +### Small live preview +The small preview [example](hfr_small_preview.py) demonstrates how to use the HFR mode for a small live preview at **240 FPS** or **480 FPS**. + +### Video encoding +The video encoding [example](hfr_save_encoded.py) demonstrates how to use the HFR mode for video encoding at **240 FPS** or **480 FPS**. + +All three examples have a `BenchmarkIn` node included that prints the framerate and the latency. This is the expected output: +``` +[2025-08-14 23:31:49.487] [ThreadedNode] [warning] FPS: 474.3766 +[2025-08-14 23:31:49.487] [ThreadedNode] [warning] Messages took 1.0118543 s +[2025-08-14 23:31:49.487] [ThreadedNode] [warning] Average latency: 0.05904912 s +``` \ No newline at end of file diff --git a/examples/python/RVC4/HFR/hfr_nn.py b/examples/python/RVC4/HFR/hfr_nn.py new file mode 100644 index 0000000000..ef4a9a9afc --- /dev/null +++ b/examples/python/RVC4/HFR/hfr_nn.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +import depthai as dai + +FPS = 480 + +with dai.Pipeline() as pipeline: + # Download the model + nnArchivePath = dai.getModelFromZoo(dai.NNModelDescription("yolov6-nano", platform="RVC4")) + nnArchive = dai.NNArchive(nnArchivePath) + inputSize = nnArchive.getInputSize() + cameraNode = pipeline.create(dai.node.Camera).build() + + # Configure the ImageManip as in HFR mode requesting arbitrary outputs is not yet supported + cameraOutput = cameraNode.requestOutput((1280, 720), fps=FPS) + imageManip = pipeline.create(dai.node.ImageManip) + imageManip.initialConfig.setOutputSize(inputSize[0], inputSize[1]) + imageManip.setMaxOutputFrameSize(int(inputSize[0] * inputSize[1] * 3)) + imageManip.initialConfig.setFrameType(dai.ImgFrame.Type.BGR888i) + cameraOutput.link(imageManip.inputImage) + + # Configure the DetectionNetwork + detectionNetwork = pipeline.create(dai.node.DetectionNetwork) + detectionNetwork.setNNArchive(nnArchive) + imageManip.out.link(detectionNetwork.input) + + benchmarkIn = pipeline.create(dai.node.BenchmarkIn) + benchmarkIn.setRunOnHost(True) + benchmarkIn.sendReportEveryNMessages(FPS) + detectionNetwork.out.link(benchmarkIn.input) + + + qDet = detectionNetwork.out.createOutputQueue() + pipeline.start() + + while pipeline.isRunning(): + inDet: dai.ImgDetections = qDet.get() + # print(f"Got {len(inDet.detections)} nn detections ") diff --git a/examples/python/RVC4/HFR/hfr_save_encoded.py b/examples/python/RVC4/HFR/hfr_save_encoded.py new file mode 100644 index 0000000000..0dd51a5d43 --- /dev/null +++ b/examples/python/RVC4/HFR/hfr_save_encoded.py @@ -0,0 +1,75 @@ +import depthai as dai + +# Capture Ctrl+C and set a flag to stop the loop +import time +import cv2 +import threading +import signal + +PROFILE = dai.VideoEncoderProperties.Profile.H264_MAIN + +quitEvent = threading.Event() +signal.signal(signal.SIGTERM, lambda *_args: quitEvent.set()) +signal.signal(signal.SIGINT, lambda *_args: quitEvent.set()) + +SIZE = (1280, 720) +FPS = 480 + +# SIZE = (1920, 1080) +# FPS = 240 + +class VideoSaver(dai.node.HostNode): + def __init__(self, *args, **kwargs): + dai.node.HostNode.__init__(self, *args, **kwargs) + self.file_handle = open('video_hfr.encoded', 'wb') + + def build(self, *args): + self.link_args(*args) + return self + + def process(self, frame): + frame.getData().tofile(self.file_handle) + +with dai.Pipeline() as pipeline: + camRgb = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A) + output = camRgb.requestOutput(SIZE, fps=FPS) + + # ImageManip is added to workaround a limitation with VideoEncoder with native resolutions + # This limitation will be lifted in the future + imageManip = pipeline.create(dai.node.ImageManip) + imageManip.initialConfig.setOutputSize(SIZE[0], SIZE[1] + 10) # To avoid a passthrough + imageManip.setMaxOutputFrameSize(int(SIZE[0] * (SIZE[1] + 10) * 1.6)) + output.link(imageManip.inputImage) + output = imageManip.out + + benchmarkIn = pipeline.create(dai.node.BenchmarkIn) + benchmarkIn.setRunOnHost(True) + + encoded = pipeline.create(dai.node.VideoEncoder).build(output, + frameRate = FPS, + profile = PROFILE) + encoded.out.link(benchmarkIn.input) + saver = pipeline.create(VideoSaver).build(encoded.out) + + pipeline.start() + print("Started to save video to video.encoded") + print("Press Ctrl+C to stop") + timeStart = time.monotonic() + while pipeline.isRunning() and not quitEvent.is_set(): + time.sleep(1) + pipeline.stop() + pipeline.wait() + saver.file_handle.close() + +print("To view the encoded data, convert the stream file (.encoded) into a video file (.mp4) using a command below:") +print(f"ffmpeg -framerate {FPS} -i video_hfr.encoded -c copy video_hfr.mp4") + +print("If the FPS is not set correctly, you can ask ffmpeg to generate it with the command below") + +print(f""" +ffmpeg -fflags +genpts -r {FPS} -i video_hfr.encoded \\ + -vsync cfr -fps_mode cfr \\ + -video_track_timescale {FPS}00 \\ + -c:v copy \\ + video_hfr.mp4 +""") diff --git a/examples/python/RVC4/HFR/hfr_small_preview.py b/examples/python/RVC4/HFR/hfr_small_preview.py new file mode 100644 index 0000000000..c0e667fcbc --- /dev/null +++ b/examples/python/RVC4/HFR/hfr_small_preview.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +import depthai as dai +import time +import cv2 + +SIZE = (1280, 720) +FPS = 480 + +# SIZE = (1920, 1080) +# FPS = 240 +with dai.Pipeline() as pipeline: + cam = pipeline.create(dai.node.Camera).build() + benchmarkIn = pipeline.create(dai.node.BenchmarkIn) + benchmarkIn.setRunOnHost(True) + benchmarkIn.sendReportEveryNMessages(FPS) + + imageManip = pipeline.create(dai.node.ImageManip) + imageManip.initialConfig.setOutputSize(250, 250) + imageManip.setMaxOutputFrameSize(int(250* 250 * 1.6)) + + # One of the two modes can be selected + # NOTE: Generic resolutions are not yet supported through camera node when using HFR mode + output = cam.requestOutput(SIZE, fps=FPS) + + output.link(imageManip.inputImage) + imageManip.out.link(benchmarkIn.input) + + outputQueue = imageManip.out.createOutputQueue() + + pipeline.start() + while pipeline.isRunning(): + imgFrame = outputQueue.get() + assert isinstance(imgFrame, dai.ImgFrame) + cv2.imshow("frame", imgFrame.getCvFrame()) + cv2.waitKey(1) From 0cc40baa6ed7d80c5164dde7cbbc35f74565adac Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 14 Aug 2025 23:50:45 +0200 Subject: [PATCH 003/180] Remove outdated README --- examples/python/RVC4/README.md | 43 ---------------------------------- 1 file changed, 43 deletions(-) delete mode 100644 examples/python/RVC4/README.md diff --git a/examples/python/RVC4/README.md b/examples/python/RVC4/README.md deleted file mode 100644 index d9f9bc9d2b..0000000000 --- a/examples/python/RVC4/README.md +++ /dev/null @@ -1,43 +0,0 @@ -# Getting started on RVC4 - -This folder contains some examples to quickly get started on an RVC4 device. - -There are two modes of running the examples. - -### Running on a host PC (default) - -In this case install depthai library on your host PC and run the examples as normal. -To do this: - -```bash -git clone https://github.com/luxonis/depthai-core.git -cd depthai-core -git checkout v3_develop -python3 examples/python/install_requirements.py -cd examples/python/RVC4 -python3 Camera/camera_output.py # Use any of the examples in the RVC4 folder -``` - -### Running on device - -For running the examples on device you can first ssh into the device and then run the examples, where you have to remove any visualizing parts. - -For the simple `camera_output.py` example you have to do the following: - -```bash -git clone https://github.com/luxonis/depthai-core.git -cd depthai-core -git checkout v3_develop -# Copy the depthai-core folder to the device -scp -r ../depthai-core root@IP_ADDRESS://data -ssh root@IP_ADDRESS -cd /data/depthai-core -python3 -m venv .env -source .env/bin/activate -python3 -m ensurepip -python3 examples/python/install_requirements.py - -# Remove the `cv2.imshow` part from the example -cd examples/python/RVC4 -python3 example.py -``` From 735b0cd02e59839d681b6b1677f2f433260bafa0 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 13 Nov 2025 22:56:51 +0100 Subject: [PATCH 004/180] Added setCameraTuningBlobPath with support for setting the blob per camera board socket --- .../python/src/pipeline/PipelineBindings.cpp | 3 ++ include/depthai/pipeline/Pipeline.hpp | 7 ++++ .../depthai/properties/GlobalProperties.hpp | 9 +++++ src/pipeline/Pipeline.cpp | 10 +++++ tests/src/ondevice_tests/filesystem_test.cpp | 13 +++++++ utilities/cam_test.py | 37 +++++++++++++++++-- 6 files changed, 76 insertions(+), 3 deletions(-) diff --git a/bindings/python/src/pipeline/PipelineBindings.cpp b/bindings/python/src/pipeline/PipelineBindings.cpp index 02d10b3bd0..25ec857fa9 100644 --- a/bindings/python/src/pipeline/PipelineBindings.cpp +++ b/bindings/python/src/pipeline/PipelineBindings.cpp @@ -86,6 +86,8 @@ void PipelineBindings::bind(pybind11::module& m, void* pCallstack) { .def_readwrite("pipelineVersion", &GlobalProperties::pipelineVersion) .def_readwrite("cameraTuningBlobSize", &GlobalProperties::cameraTuningBlobSize, DOC(dai, GlobalProperties, cameraTuningBlobSize)) .def_readwrite("cameraTuningBlobUri", &GlobalProperties::cameraTuningBlobUri, DOC(dai, GlobalProperties, cameraTuningBlobUri)) + .def_readwrite("cameraSocketTuningBlobSize", &GlobalProperties::cameraSocketTuningBlobSize, DOC(dai, GlobalProperties, cameraSocketTuningBlobSize)) + .def_readwrite("cameraSocketTuningBlobUri", &GlobalProperties::cameraSocketTuningBlobUri, DOC(dai, GlobalProperties, cameraSocketTuningBlobUri)) .def_readwrite("xlinkChunkSize", &GlobalProperties::xlinkChunkSize, DOC(dai, GlobalProperties, xlinkChunkSize)) .def_readwrite("sippBufferSize", &GlobalProperties::sippBufferSize, DOC(dai, GlobalProperties, sippBufferSize)) .def_readwrite("sippDmaBufferSize", &GlobalProperties::sippDmaBufferSize, DOC(dai, GlobalProperties, sippDmaBufferSize)); @@ -157,6 +159,7 @@ void PipelineBindings::bind(pybind11::module& m, void* pCallstack) { py::return_value_policy::reference_internal, DOC(dai, Pipeline, getAssetManager)) .def("setCameraTuningBlobPath", &Pipeline::setCameraTuningBlobPath, py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath)) + .def("setCameraTuningBlobPath", py::overload_cast(&Pipeline::setCameraTuningBlobPath), py::arg("socket"), py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath, 2)) .def("setXLinkChunkSize", &Pipeline::setXLinkChunkSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setXLinkChunkSize)) .def("setSippBufferSize", &Pipeline::setSippBufferSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setSippBufferSize)) .def("setSippDmaBufferSize", &Pipeline::setSippDmaBufferSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setSippDmaBufferSize)) diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index 83457a05f9..f2dffdbd2a 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -22,6 +22,7 @@ #include "depthai/pipeline/PipelineSchema.hpp" #include "depthai/properties/GlobalProperties.hpp" #include "depthai/utility/RecordReplay.hpp" +#include "depthai/common/CameraBoardSocket.hpp" namespace dai { @@ -55,6 +56,7 @@ class PipelineImpl : public std::enable_shared_from_this { PipelineSchema getPipelineSchema(SerializationType type = DEFAULT_SERIALIZATION_TYPE) const; Device::Config getDeviceConfig() const; void setCameraTuningBlobPath(const fs::path& path); + void setCameraTuningBlobPath(CameraBoardSocket socket, const fs::path& path); void setXLinkChunkSize(int sizeBytes); GlobalProperties getGlobalProperties() const; void setGlobalProperties(GlobalProperties globalProperties); @@ -419,6 +421,11 @@ class Pipeline { void setCameraTuningBlobPath(const fs::path& path) { impl()->setCameraTuningBlobPath(path); } + + /// Set a camera IQ (Image Quality) tuning blob, used for specific board socket + void setCameraTuningBlobPath(CameraBoardSocket socket, const fs::path& path) { + impl()->setCameraTuningBlobPath(socket, path); + } /** * Set chunk size for splitting device-sent XLink packets, in bytes. A larger value could diff --git a/include/depthai/properties/GlobalProperties.hpp b/include/depthai/properties/GlobalProperties.hpp index 5dbc316289..094b9b70db 100644 --- a/include/depthai/properties/GlobalProperties.hpp +++ b/include/depthai/properties/GlobalProperties.hpp @@ -45,6 +45,15 @@ struct GlobalProperties : PropertiesSerializable { */ std::string cameraTuningBlobUri; + /** + * Socket specific camera tuning blob size in bytes + */ + std::unordered_map cameraSocketTuningBlobSize; + /** + * Socket specific camera tuning blob uri + */ + std::unordered_map cameraSocketTuningBlobUri; + /** * Chunk size for splitting device-sent XLink packets, in bytes. A larger value could * increase performance, with 0 disabling chunking. A negative value won't modify the diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 7076d25048..0bdd4229fa 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -349,6 +349,16 @@ void PipelineImpl::setCameraTuningBlobPath(const fs::path& path) { globalProperties.cameraTuningBlobSize = static_cast(asset->data.size()); } +void PipelineImpl::setCameraTuningBlobPath(CameraBoardSocket socket, const fs::path& path) { + std::string assetKey = "camTuning"; + assetKey += "_" + std::to_string(static_cast(socket)); + + auto asset = assetManager.set(assetKey, path); + + globalProperties.cameraSocketTuningBlobUri[socket] = asset->getRelativeUri(); + globalProperties.cameraSocketTuningBlobSize[socket] = static_cast(asset->data.size()); +} + void PipelineImpl::setXLinkChunkSize(int sizeBytes) { globalProperties.xlinkChunkSize = sizeBytes; } diff --git a/tests/src/ondevice_tests/filesystem_test.cpp b/tests/src/ondevice_tests/filesystem_test.cpp index b705112761..af04daf485 100644 --- a/tests/src/ondevice_tests/filesystem_test.cpp +++ b/tests/src/ondevice_tests/filesystem_test.cpp @@ -338,6 +338,13 @@ TEST_CASE("std::filesystem::path with AssetManager, StereoDepth") { CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(path4)); pipeline.getAssetManager().remove("camTuning"); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, string4.c_str())); + pipeline.getAssetManager().remove("camTuning_0"); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, string4)); + pipeline.getAssetManager().remove("camTuning_0"); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, path4)); + pipeline.getAssetManager().remove("camTuning_0"); + auto depth = pipeline.create(); CHECK_NOTHROW(depth->loadMeshFiles(string4.c_str(), string4.c_str())); depth->getAssetManager().remove("meshLeft"); @@ -356,6 +363,11 @@ TEST_CASE("std::filesystem::path with AssetManager, StereoDepth") { CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(widePath4)); pipeline.getAssetManager().remove("camTuning"); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, widePath4.c_str())); + pipeline.getAssetManager().remove("camTuning_0"); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, widePath4)); + pipeline.getAssetManager().remove("camTuning_0"); + CHECK_NOTHROW(depth->loadMeshFiles(widePath4.c_str(), widePath4.c_str())); depth->getAssetManager().remove("meshLeft"); depth->getAssetManager().remove("meshRight"); @@ -371,6 +383,7 @@ TEST_CASE("std::filesystem::path with AssetManager, StereoDepth") { const auto stdPath4 = std::filesystem::u8path(string4); #endif CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(stdPath4)); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, stdPath4)); CHECK_NOTHROW(depth->loadMeshFiles(stdPath4, stdPath4)); #endif diff --git a/utilities/cam_test.py b/utilities/cam_test.py index 7c9c49872f..5acef3b768 100755 --- a/utilities/cam_test.py +++ b/utilities/cam_test.py @@ -121,6 +121,24 @@ def socket_type_pair(arg): is_thermal = True if type in ['th', 'thermal'] else False return [socket, is_color, is_tof, is_thermal] +def camera_tuning_item(arg: str): + """ + Accept either: + - PATH + - SOCKET,PATH + Returns (socket_or_all, Path). + """ + # Case 1: plain path = global tuning for all cameras + if ',' not in arg: + return ("__all__", Path(arg)) + + # Case 2: socket,path + socket, path_str = arg.split(',', 1) + + if socket not in ALL_SOCKETS: + raise argparse.ArgumentTypeError(f"Unknown camera socket: {socket}") + + return (socket, Path(path_str)) parser = argparse.ArgumentParser(add_help=False) parser.add_argument('-cams', '--cameras', type=socket_type_pair, nargs='+', @@ -141,8 +159,8 @@ def socket_type_pair(arg): help="Downscale the ISP output by this factor") parser.add_argument('-rs', '--resizable-windows', action='store_true', help="Make OpenCV windows resizable. Note: may introduce some artifacts") -parser.add_argument('-tun', '--camera-tuning', type=Path, - help="Path to custom camera tuning database") +parser.add_argument('-tun', '--camera-tuning', type=camera_tuning_item, nargs='+', default=[], + help="Camera tuning database. Either a single PATH, which will apply to all cameras, or one or more SOCKET,PATH pairs. Example: -tun /path/to/tuning.db or -tun rgb,/path/to/rgb.db right,/path/to/right.db") 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', @@ -357,7 +375,20 @@ def socket_to_socket_opt(socket: dai.CameraBoardSocket) -> str: streams.append(streamName) if args.camera_tuning: - pipeline.setCameraTuningBlobPath(str(args.camera_tuning)) + if len(args.camera_tuning) == 1 and args.camera_tuning[0][0] == "__all__": + # Single tuning for all cameras + tuning_path = args.camera_tuning[0][1] + print(f'Applying camera tuning from {tuning_path} to all cameras') + pipeline.setCameraTuningBlobPath(str(tuning_path)) + else: + # Per-socket tuning + for socket, tuning_path in args.camera_tuning: + if socket == "__all__": + print(f'Cannot apply all tuning to all and specific sockets at the same time.') + exit(1) + cam_socket = cam_socket_opts[socket] + print(f'Applying camera tuning from {tuning_path} to camera socket {socket}') + pipeline.setCameraTuningBlobPath(cam_socket, str(tuning_path)) stereo = None From 4fc69cd090fc1933ca7bf0d2b4dadf1599adfbab Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 13 Nov 2025 23:59:39 +0100 Subject: [PATCH 005/180] Bumped RVC2 FW --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index f9a2999b1b..67b8f18021 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "f81edb9c2328ee1f80a6f80c3fade0af0076a3ff") +set(DEPTHAI_DEVICE_SIDE_COMMIT "82b9f02e68f9c35d201914af404f3c4f367d9056") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From ebecd7168da40a7cc4dea92100ddbd51643fdc8b Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Fri, 14 Nov 2025 12:37:20 +0100 Subject: [PATCH 006/180] Fixed bindings for setCameraTuningBlobPath function --- bindings/python/src/pipeline/PipelineBindings.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bindings/python/src/pipeline/PipelineBindings.cpp b/bindings/python/src/pipeline/PipelineBindings.cpp index 25ec857fa9..163cc30346 100644 --- a/bindings/python/src/pipeline/PipelineBindings.cpp +++ b/bindings/python/src/pipeline/PipelineBindings.cpp @@ -158,8 +158,8 @@ void PipelineBindings::bind(pybind11::module& m, void* pCallstack) { static_cast(&Pipeline::getAssetManager), py::return_value_policy::reference_internal, DOC(dai, Pipeline, getAssetManager)) - .def("setCameraTuningBlobPath", &Pipeline::setCameraTuningBlobPath, py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath)) - .def("setCameraTuningBlobPath", py::overload_cast(&Pipeline::setCameraTuningBlobPath), py::arg("socket"), py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath, 2)) + .def("setCameraTuningBlobPath", py::overload_cast(&Pipeline::setCameraTuningBlobPath), py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath)) + .def("setCameraTuningBlobPath", py::overload_cast(&Pipeline::setCameraTuningBlobPath), py::arg("socket"), py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath, 2)) .def("setXLinkChunkSize", &Pipeline::setXLinkChunkSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setXLinkChunkSize)) .def("setSippBufferSize", &Pipeline::setSippBufferSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setSippBufferSize)) .def("setSippDmaBufferSize", &Pipeline::setSippDmaBufferSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setSippDmaBufferSize)) From c5ac1ed2ede64d82ac393a6a3779e837034e3397 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Fri, 14 Nov 2025 12:37:35 +0100 Subject: [PATCH 007/180] Imprved docs on Style check --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 779bfe0208..7f99dd6a24 100644 --- a/README.md +++ b/README.md @@ -258,6 +258,10 @@ To use this target clang format must be installed, preferably clang-format-18 ``` sudo apt install clang-format-18 ``` +or using pip +``` +python -m pip install clang-format~=18.0 +``` And to apply formatting ``` From abace7bb844523c169a96b08ff5d838c4a0b058f Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Fri, 21 Nov 2025 18:40:00 +0100 Subject: [PATCH 008/180] Added per socket tuning data to serialization --- include/depthai/properties/GlobalProperties.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/depthai/properties/GlobalProperties.hpp b/include/depthai/properties/GlobalProperties.hpp index 094b9b70db..6011917624 100644 --- a/include/depthai/properties/GlobalProperties.hpp +++ b/include/depthai/properties/GlobalProperties.hpp @@ -88,6 +88,8 @@ DEPTHAI_SERIALIZE_EXT(GlobalProperties, pipelineVersion, cameraTuningBlobSize, cameraTuningBlobUri, + cameraSocketTuningBlobSize, + cameraSocketTuningBlobUri, calibData, eepromId, xlinkChunkSize, From 8f6618490c26f4befed3c421a1b1c4a480939672 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Fri, 21 Nov 2025 19:06:48 +0100 Subject: [PATCH 009/180] Bumped RVC2 FW --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 67b8f18021..104df08a6d 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "82b9f02e68f9c35d201914af404f3c4f367d9056") +set(DEPTHAI_DEVICE_SIDE_COMMIT "67c2d2856fedb2772ec66ed42f16ea08062bbcab") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 33059d46cff905eba353c428d9076f1de3a3003c Mon Sep 17 00:00:00 2001 From: Danilo Date: Fri, 3 Oct 2025 12:57:38 +0200 Subject: [PATCH 010/180] add variables for device sanitizers --- cmake/toolchain/asan-ubsan.cmake | 1 + cmake/toolchain/tsan.cmake | 2 ++ 2 files changed, 3 insertions(+) diff --git a/cmake/toolchain/asan-ubsan.cmake b/cmake/toolchain/asan-ubsan.cmake index 309ca69ce9..b5077b2cd2 100644 --- a/cmake/toolchain/asan-ubsan.cmake +++ b/cmake/toolchain/asan-ubsan.cmake @@ -9,4 +9,5 @@ set(CMAKE_SHARED_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_SHARED_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) +set(DEPTHAI_SANITIZE ON CACHE BOOL) set(_internal_flags_sanitizer) diff --git a/cmake/toolchain/tsan.cmake b/cmake/toolchain/tsan.cmake index 7af0a18f7a..5592d25ee2 100644 --- a/cmake/toolchain/tsan.cmake +++ b/cmake/toolchain/tsan.cmake @@ -9,4 +9,6 @@ set(CMAKE_SHARED_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_SHARED_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) +set(DEPTHAI_SANITIZE ON CACHE BOOL) +set(SANITIZE_THREAD ON CACHE BOOL) set(_internal_flags_sanitizer) \ No newline at end of file From 20269ccd49d2cab8676a43621b53e15e06b02eeb Mon Sep 17 00:00:00 2001 From: Danilo Date: Tue, 7 Oct 2025 10:19:32 +0200 Subject: [PATCH 011/180] fix including sanitizers --- CMakeLists.txt | 5 ++--- cmake/sanitizers/FindASan.cmake | 2 ++ cmake/sanitizers/FindTSan.cmake | 2 +- cmake/toolchain/asan-ubsan.cmake | 5 ++++- cmake/toolchain/tsan.cmake | 7 +++++-- 5 files changed, 14 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index db6e51752c..404f731ca5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.20) include(cmake/depthaiOptions.cmake) +include("${CMAKE_TOOLCHAIN_FILE}") if(WIN32) add_compile_options(/MP) @@ -64,7 +65,7 @@ else() message(STATUS "Using toolchain file: ${CMAKE_TOOLCHAIN_FILE}") endif() - +message(STATUS "CMAKE_TOOLCHAIN_FILE_3 = ${CMAKE_TOOLCHAIN_FILE}") # Create depthai project project(depthai VERSION "3.2.1" LANGUAGES CXX C) set(DEPTHAI_PRE_RELEASE_TYPE "") # Valid options are "alpha", "beta", "rc", "" @@ -1114,8 +1115,6 @@ endif() # Sanitizers ######################## if(DEPTHAI_SANITIZE) - set(SANITIZE_ADDRESS ON CACHE BOOL "Enable AddressSanitizer for sanitized targets.") - set(SANITIZE_UNDEFINED ON CACHE BOOL "Enable UndefinedBehaviorSanitizer for sanitized targets.") find_package(Sanitizers) add_sanitizers(${TARGET_CORE_NAME}) if(DEPTHAI_HAVE_OPENCV_SUPPORT AND NOT DEPTHAI_MERGED_TARGET) diff --git a/cmake/sanitizers/FindASan.cmake b/cmake/sanitizers/FindASan.cmake index 98ea7cb311..6aad8295c5 100644 --- a/cmake/sanitizers/FindASan.cmake +++ b/cmake/sanitizers/FindASan.cmake @@ -35,6 +35,8 @@ set(FLAG_CANDIDATES if (SANITIZE_ADDRESS AND (SANITIZE_THREAD OR SANITIZE_MEMORY)) + message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") + message(STATUS "SANITIZE_ADDRESS = ${SANITIZE_ADDRESS}") message(FATAL_ERROR "AddressSanitizer is not compatible with " "ThreadSanitizer or MemorySanitizer.") endif () diff --git a/cmake/sanitizers/FindTSan.cmake b/cmake/sanitizers/FindTSan.cmake index 3cba3c03b6..3624cdb13d 100644 --- a/cmake/sanitizers/FindTSan.cmake +++ b/cmake/sanitizers/FindTSan.cmake @@ -28,7 +28,7 @@ set(FLAG_CANDIDATES "-g -fsanitize=thread" ) - +message("pucpuc") # ThreadSanitizer is not compatible with MemorySanitizer. if (SANITIZE_THREAD AND SANITIZE_MEMORY) message(FATAL_ERROR "ThreadSanitizer is not compatible with " diff --git a/cmake/toolchain/asan-ubsan.cmake b/cmake/toolchain/asan-ubsan.cmake index b5077b2cd2..41cbad1b9b 100644 --- a/cmake/toolchain/asan-ubsan.cmake +++ b/cmake/toolchain/asan-ubsan.cmake @@ -9,5 +9,8 @@ set(CMAKE_SHARED_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_SHARED_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) -set(DEPTHAI_SANITIZE ON CACHE BOOL) +set(DEPTHAI_SANITIZE ON) +set(SANITIZE_ADDRESS ON) +set(SANITIZE_UNDEFINED ON) +set(SANITIZE_THREAD OFF) set(_internal_flags_sanitizer) diff --git a/cmake/toolchain/tsan.cmake b/cmake/toolchain/tsan.cmake index 5592d25ee2..5847d43dbd 100644 --- a/cmake/toolchain/tsan.cmake +++ b/cmake/toolchain/tsan.cmake @@ -1,3 +1,4 @@ +message(STATUS ">>> Toolchain loaded: ${CMAKE_CURRENT_LIST_FILE}") set(_internal_flags_sanitizer "-fno-omit-frame-pointer -fsanitize=thread") set(CMAKE_C_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_CXX_FLAGS ${_internal_flags_sanitizer}) @@ -9,6 +10,8 @@ set(CMAKE_SHARED_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_SHARED_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_MODULE_LINKER_FLAGS_INIT ${_internal_flags_sanitizer}) -set(DEPTHAI_SANITIZE ON CACHE BOOL) -set(SANITIZE_THREAD ON CACHE BOOL) +set(DEPTHAI_SANITIZE ON) +set(SANITIZE_ADDRESS OFF) +set(SANITIZE_UNDEFINED OFF) +set(SANITIZE_THREAD ON) set(_internal_flags_sanitizer) \ No newline at end of file From 1ef854264a6ceb2198f8af189092b2dd87581713 Mon Sep 17 00:00:00 2001 From: Danilo Date: Tue, 7 Oct 2025 11:46:16 +0200 Subject: [PATCH 012/180] remove debug messages --- CMakeLists.txt | 1 - cmake/sanitizers/FindTSan.cmake | 1 - 2 files changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 404f731ca5..f32c44c84a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,6 @@ else() message(STATUS "Using toolchain file: ${CMAKE_TOOLCHAIN_FILE}") endif() -message(STATUS "CMAKE_TOOLCHAIN_FILE_3 = ${CMAKE_TOOLCHAIN_FILE}") # Create depthai project project(depthai VERSION "3.2.1" LANGUAGES CXX C) set(DEPTHAI_PRE_RELEASE_TYPE "") # Valid options are "alpha", "beta", "rc", "" diff --git a/cmake/sanitizers/FindTSan.cmake b/cmake/sanitizers/FindTSan.cmake index 3624cdb13d..57e54a8a1c 100644 --- a/cmake/sanitizers/FindTSan.cmake +++ b/cmake/sanitizers/FindTSan.cmake @@ -28,7 +28,6 @@ set(FLAG_CANDIDATES "-g -fsanitize=thread" ) -message("pucpuc") # ThreadSanitizer is not compatible with MemorySanitizer. if (SANITIZE_THREAD AND SANITIZE_MEMORY) message(FATAL_ERROR "ThreadSanitizer is not compatible with " From e0d48fb94431fa9b9e432950f0ab14ebdc069c95 Mon Sep 17 00:00:00 2001 From: Danilo Date: Tue, 7 Oct 2025 15:08:06 +0200 Subject: [PATCH 013/180] add debug lines --- CMakeLists.txt | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f32c44c84a..c7cb90fe93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,12 @@ cmake_minimum_required(VERSION 3.20) include(cmake/depthaiOptions.cmake) include("${CMAKE_TOOLCHAIN_FILE}") +message(STATUS "CMAKE_TOOLCHAIN_FILE = ${CMAKE_TOOLCHAIN_FILE}") +message(STATUS "DEPTHAI_ENABLE_DEVICE_RVC4_FW = ${DEPTHAI_ENABLE_DEVICE_RVC4_FW}") +message(STATUS "DEPTHAI_SANITIZE = ${DEPTHAI_SANITIZE}") +message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") +message(STATUS "DEPTHAI_DEVICE_RVC4_VERSION = ${DEPTHAI_DEVICE_RVC4_VERSION}") + if(WIN32) add_compile_options(/MP) @@ -47,6 +53,13 @@ set(CMAKE_TOOLCHAIN_FILE "" CACHE FILEPATH "CMake toolchain path") # Create a custom toolchain to pass certain options to dependencies set(gen_toolchain "${CMAKE_CURRENT_BINARY_DIR}/generated/toolchain.cmake") +message(STATUS "CMAKE_TOOLCHAIN_FILE_2 = ${CMAKE_TOOLCHAIN_FILE}") +message(STATUS "DEPTHAI_ENABLE_DEVICE_RVC4_FW = ${DEPTHAI_ENABLE_DEVICE_RVC4_FW}") +message(STATUS "DEPTHAI_SANITIZE = ${DEPTHAI_SANITIZE}") +message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") +message(STATUS "DEPTHAI_DEVICE_RVC4_VERSION = ${DEPTHAI_DEVICE_RVC4_VERSION}") + + if(EXISTS "${gen_toolchain}" AND ("${_INTERNAL_DEPTHAI_ORIGINAL_CMAKE_TOOLCHAIN_FILE}" STREQUAL "${CMAKE_TOOLCHAIN_FILE}" OR NOT "${CMAKE_TOOLCHAIN_FILE}" STREQUAL "")) message(STATUS "Using existing generated toolchain") else() @@ -65,6 +78,7 @@ else() message(STATUS "Using toolchain file: ${CMAKE_TOOLCHAIN_FILE}") endif() +message(STATUS "CMAKE_TOOLCHAIN_FILE_3 = ${CMAKE_TOOLCHAIN_FILE}") # Create depthai project project(depthai VERSION "3.2.1" LANGUAGES CXX C) set(DEPTHAI_PRE_RELEASE_TYPE "") # Valid options are "alpha", "beta", "rc", "" @@ -690,6 +704,19 @@ if(DEPTHAI_ENABLE_DEVICE_RVC3_FW) list(APPEND RESOURCE_COMPILED_FILES ${DEPTHAI_DEVICE_KB_RESOURCE_LIST}) endif() +# --- User overlay toolchain (optional) --- +# set(DEPTHAI_USER_TOOLCHAIN "" CACHE PATH "Extra toolchain to include after vcpkg") +# if(DEPTHAI_USER_TOOLCHAIN AND EXISTS "${DEPTHAI_USER_TOOLCHAIN}") +# message(STATUS "Including user toolchain: ${DEPTHAI_USER_TOOLCHAIN}") +# include("${DEPTHAI_USER_TOOLCHAIN}") +# endif() + +message(STATUS "CMAKE_TOOLCHAIN_FILE_4 = ${CMAKE_TOOLCHAIN_FILE}") +message(STATUS "DEPTHAI_ENABLE_DEVICE_RVC4_FW = ${DEPTHAI_ENABLE_DEVICE_RVC4_FW}") +message(STATUS "DEPTHAI_SANITIZE = ${DEPTHAI_SANITIZE}") +message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") +message(STATUS "DEPTHAI_DEVICE_RVC4_VERSION = ${DEPTHAI_DEVICE_RVC4_VERSION}") + if(DEPTHAI_ENABLE_DEVICE_RVC4_FW) if(DEPTHAI_SANITIZE AND SANITIZE_THREAD) string(APPEND DEPTHAI_DEVICE_RVC4_VERSION "-tsan") From c5ef6f3f274f5258345badda92c71a93de3f053a Mon Sep 17 00:00:00 2001 From: Danilo Date: Tue, 7 Oct 2025 15:33:58 +0200 Subject: [PATCH 014/180] run santizier tests --- CMakeLists.txt | 35 ++++++----------------------------- tests/Dockerfile | 4 ++-- 2 files changed, 8 insertions(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c7cb90fe93..ff1af79a00 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,13 +1,11 @@ cmake_minimum_required(VERSION 3.20) -include(cmake/depthaiOptions.cmake) -include("${CMAKE_TOOLCHAIN_FILE}") -message(STATUS "CMAKE_TOOLCHAIN_FILE = ${CMAKE_TOOLCHAIN_FILE}") -message(STATUS "DEPTHAI_ENABLE_DEVICE_RVC4_FW = ${DEPTHAI_ENABLE_DEVICE_RVC4_FW}") -message(STATUS "DEPTHAI_SANITIZE = ${DEPTHAI_SANITIZE}") -message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") -message(STATUS "DEPTHAI_DEVICE_RVC4_VERSION = ${DEPTHAI_DEVICE_RVC4_VERSION}") - +if(CMAKE_TOOLCHAIN_FILE) + message(STATUS "Including toolchain file: ${CMAKE_TOOLCHAIN_FILE}") + include("${CMAKE_TOOLCHAIN_FILE}") +else() + message(STATUS "No toolchain file specified, skipping include.") +endif() if(WIN32) add_compile_options(/MP) @@ -53,13 +51,6 @@ set(CMAKE_TOOLCHAIN_FILE "" CACHE FILEPATH "CMake toolchain path") # Create a custom toolchain to pass certain options to dependencies set(gen_toolchain "${CMAKE_CURRENT_BINARY_DIR}/generated/toolchain.cmake") -message(STATUS "CMAKE_TOOLCHAIN_FILE_2 = ${CMAKE_TOOLCHAIN_FILE}") -message(STATUS "DEPTHAI_ENABLE_DEVICE_RVC4_FW = ${DEPTHAI_ENABLE_DEVICE_RVC4_FW}") -message(STATUS "DEPTHAI_SANITIZE = ${DEPTHAI_SANITIZE}") -message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") -message(STATUS "DEPTHAI_DEVICE_RVC4_VERSION = ${DEPTHAI_DEVICE_RVC4_VERSION}") - - if(EXISTS "${gen_toolchain}" AND ("${_INTERNAL_DEPTHAI_ORIGINAL_CMAKE_TOOLCHAIN_FILE}" STREQUAL "${CMAKE_TOOLCHAIN_FILE}" OR NOT "${CMAKE_TOOLCHAIN_FILE}" STREQUAL "")) message(STATUS "Using existing generated toolchain") else() @@ -78,7 +69,6 @@ else() message(STATUS "Using toolchain file: ${CMAKE_TOOLCHAIN_FILE}") endif() -message(STATUS "CMAKE_TOOLCHAIN_FILE_3 = ${CMAKE_TOOLCHAIN_FILE}") # Create depthai project project(depthai VERSION "3.2.1" LANGUAGES CXX C) set(DEPTHAI_PRE_RELEASE_TYPE "") # Valid options are "alpha", "beta", "rc", "" @@ -704,19 +694,6 @@ if(DEPTHAI_ENABLE_DEVICE_RVC3_FW) list(APPEND RESOURCE_COMPILED_FILES ${DEPTHAI_DEVICE_KB_RESOURCE_LIST}) endif() -# --- User overlay toolchain (optional) --- -# set(DEPTHAI_USER_TOOLCHAIN "" CACHE PATH "Extra toolchain to include after vcpkg") -# if(DEPTHAI_USER_TOOLCHAIN AND EXISTS "${DEPTHAI_USER_TOOLCHAIN}") -# message(STATUS "Including user toolchain: ${DEPTHAI_USER_TOOLCHAIN}") -# include("${DEPTHAI_USER_TOOLCHAIN}") -# endif() - -message(STATUS "CMAKE_TOOLCHAIN_FILE_4 = ${CMAKE_TOOLCHAIN_FILE}") -message(STATUS "DEPTHAI_ENABLE_DEVICE_RVC4_FW = ${DEPTHAI_ENABLE_DEVICE_RVC4_FW}") -message(STATUS "DEPTHAI_SANITIZE = ${DEPTHAI_SANITIZE}") -message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") -message(STATUS "DEPTHAI_DEVICE_RVC4_VERSION = ${DEPTHAI_DEVICE_RVC4_VERSION}") - if(DEPTHAI_ENABLE_DEVICE_RVC4_FW) if(DEPTHAI_SANITIZE AND SANITIZE_THREAD) string(APPEND DEPTHAI_DEVICE_RVC4_VERSION "-tsan") diff --git a/tests/Dockerfile b/tests/Dockerfile index c7aed0276d..1c9e0f08dc 100644 --- a/tests/Dockerfile +++ b/tests/Dockerfile @@ -103,8 +103,8 @@ ENV PATH="/workspace/venv/bin:$PATH" # Set toolchain path only if flavor != vanilla RUN if [ "$FLAVOR" != "vanilla" ]; then \ - export CMAKE_TOOLCHAIN_PATH=/workspace/cmake/toolchain/${FLAVOR}.cmake && \ - echo "Using toolchain: $CMAKE_TOOLCHAIN_PATH"; \ + export CMAKE_TOOLCHAIN_FILE=/workspace/cmake/toolchain/${FLAVOR}.cmake && \ + echo "Using toolchain: $CMAKE_TOOLCHAIN_FILE"; \ else \ echo "Using vanilla flavor (no toolchain)"; \ fi && \ From 753556111f6e7b0829e555ae1251bc5dedfb6bc8 Mon Sep 17 00:00:00 2001 From: Danilo Date: Tue, 7 Oct 2025 16:03:33 +0200 Subject: [PATCH 015/180] adding back deopthai option --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index ff1af79a00..9c91d8ed80 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 3.20) +include(cmake/depthaiOptions.cmake) if(CMAKE_TOOLCHAIN_FILE) message(STATUS "Including toolchain file: ${CMAKE_TOOLCHAIN_FILE}") include("${CMAKE_TOOLCHAIN_FILE}") From b31a3b5d1319e213e1c18abf305bdf07d890eb90 Mon Sep 17 00:00:00 2001 From: Danilo Date: Thu, 9 Oct 2025 16:58:45 +0200 Subject: [PATCH 016/180] empty commit --- tests/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/Dockerfile b/tests/Dockerfile index 1c9e0f08dc..6eba35b133 100644 --- a/tests/Dockerfile +++ b/tests/Dockerfile @@ -101,7 +101,6 @@ RUN python3 -m venv venv && \ ENV PATH="/workspace/venv/bin:$PATH" -# Set toolchain path only if flavor != vanilla RUN if [ "$FLAVOR" != "vanilla" ]; then \ export CMAKE_TOOLCHAIN_FILE=/workspace/cmake/toolchain/${FLAVOR}.cmake && \ echo "Using toolchain: $CMAKE_TOOLCHAIN_FILE"; \ From b6289b7629c0603963bebd9cf5380a12462dea41 Mon Sep 17 00:00:00 2001 From: Danilo Pejovic <115164734+danilo-pejovic@users.noreply.github.com> Date: Thu, 9 Oct 2025 17:15:12 +0200 Subject: [PATCH 017/180] Update test_child.yml --- .github/workflows/test_child.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test_child.yml b/.github/workflows/test_child.yml index 6e1a8a4e88..c5db6b82b2 100644 --- a/.github/workflows/test_child.yml +++ b/.github/workflows/test_child.yml @@ -56,7 +56,7 @@ jobs: export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc2-${{ inputs.flavor }}" exec hil_runner --capabilities depthai-core-hil --platforms 'rvc2 and rvc2' --reservation-name $RESERVATION_NAME --wait --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc2" - # Testing + # Testing linux_rvc4_test: needs: [build_docker_container] strategy: From e51a049cb069ba3e6a534a4766330fcf525f976b Mon Sep 17 00:00:00 2001 From: AljazD Date: Wed, 17 Dec 2025 11:29:29 +0100 Subject: [PATCH 018/180] Updated file size error message --- src/utility/EventsManager.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 34322b1a3c..1b9a074917 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -684,7 +684,10 @@ bool EventsManager::sendSnap(const std::string& name, } for(const auto& file : fileGroup->fileData) { if(file->size >= maxFileSizeBytes) { - logger::error("Failed to send snap, file: {} is bigger then the configured maximum size: {}", file->fileName, maxFileSizeBytes); + logger::error( + "Failed to send snap, file: {} is bigger than the current maximum file size limit: {} kB. To increase your maximum file size, contact support.", + file->fileName, + maxFileSizeBytes / 1024); return false; } } From 0b18d483f78641fe9e65fe160edaf8d9e84136d0 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 18 Dec 2025 09:09:56 +0100 Subject: [PATCH 019/180] Added callbacks for snap uploads. Device serial number is no longer user provided when sending snaps --- .../src/utility/EventsManagerBindings.cpp | 29 ++++- examples/cpp/Events/events.cpp | 37 +++++- examples/cpp/Events/events_file_group.cpp | 32 +++++- examples/python/Events/events.py | 20 +++- examples/python/Events/events_file_group.py | 20 +++- include/depthai/utility/EventsManager.hpp | 33 ++++-- src/utility/EventsManager.cpp | 107 +++++++++++++----- 7 files changed, 234 insertions(+), 44 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index e09d314413..8ddad925f5 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -7,6 +7,10 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { using namespace dai; + + // Type definitions + py::enum_ sendSnapCallbackStatus(m, "SendSnapCallbackStatus", DOC(dai, SendSnapCallbackStatus)); + /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -82,6 +86,18 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { // py::arg("nnData"), // DOC(dai, utility, FileGroup, addImageNNDataPair)); + sendSnapCallbackStatus.value("SUCCESS", SendSnapCallbackStatus::SUCCESS) + .value("FILE_BATCH_PREPARATION_FAILED", SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED) + .value("GROUP_CONTAINS_REJECTED_FILES", SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES) + .value("FILE_UPLOAD_FAILED", SendSnapCallbackStatus::FILE_UPLOAD_FAILED) + .value("SEND_EVENT_FAILED", SendSnapCallbackStatus::SEND_EVENT_FAILED); + + py::class_(m, "SendSnapCallbackResult") + .def(py::init<>()) + .def_readonly("snapName", &SendSnapCallbackResult::snapName) + .def_readonly("snapTimestamp", &SendSnapCallbackResult::snapTimestamp) + .def_readonly("status", &SendSnapCallbackResult::status); + py::class_(m, "EventsManager") .def(py::init<>()) .def(py::init(), py::arg("uploadCachedOnStart") = false) @@ -98,7 +114,6 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("name"), py::arg("tags") = std::vector(), py::arg("extras") = std::unordered_map(), - py::arg("deviceSerialNo") = "", py::arg("associateFiles") = std::vector(), DOC(dai, utility, EventsManager, sendEvent)) .def("sendSnap", @@ -106,12 +121,14 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { const std::shared_ptr, const std::vector&, const std::unordered_map&, - const std::string&)>(&EventsManager::sendSnap), + const std::function successCallback, + const std::function failureCallback)>(&EventsManager::sendSnap), py::arg("name"), py::arg("fileGroup") = std::shared_ptr(), py::arg("tags") = std::vector(), py::arg("extras") = std::unordered_map(), - py::arg("deviceSerialNo") = "", + py::arg("successCallback") = py::none(), + py::arg("failureCallback") = py::none(), DOC(dai, utility, EventsManager, sendSnap)) .def("sendSnap", static_cast>&, const std::vector&, const std::unordered_map&, - const std::string&)>(&EventsManager::sendSnap), + const std::function successCallback, + const std::function failureCallback)>(&EventsManager::sendSnap), py::arg("name"), py::arg("fileName"), py::arg("imgFrame"), py::arg("imgDetections"), py::arg("tags") = std::vector(), py::arg("extras") = std::unordered_map(), - py::arg("deviceSerialNo") = "", + py::arg("successCallback") = py::none(), + py::arg("failureCallback") = py::none(), DOC(dai, utility, EventsManager, sendSnap)); #endif } diff --git a/examples/cpp/Events/events.cpp b/examples/cpp/Events/events.cpp index 0291c634a9..23be5e2e36 100644 --- a/examples/cpp/Events/events.cpp +++ b/examples/cpp/Events/events.cpp @@ -12,6 +12,34 @@ cv::Rect frameNorm(const cv::Mat& frame, const dai::Point2f& topLeft, const dai: return cv::Rect(cv::Point(topLeft.x * width, topLeft.y * height), cv::Point(bottomRight.x * width, bottomRight.y * height)); } +// Callback functions +void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { + std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " has been successfully uploaded to the hub" + << std::endl; +} + +void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { + std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " could not have been uploaded to the hub" + << std::endl; + + switch(sendSnapResult.status) { + case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: + std::cout << "File batch preparation failed!" << std::endl; + break; + case dai::utility::SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES: + std::cout << "Snap's file group contains rejected files!" << std::endl; + break; + case dai::utility::SendSnapCallbackStatus::FILE_UPLOAD_FAILED: + std::cout << "File upload was unsuccessful!" << std::endl; + break; + case dai::utility::SendSnapCallbackStatus::SEND_EVENT_FAILED: + std::cout << "Snap could not been sent to the hub, following successful file upload!" << std::endl; + break; + default: + break; + } +} + int main() { dai::Pipeline pipeline(true); @@ -72,7 +100,14 @@ int main() { // Trigger sendSnap() if(cv::waitKey(1) == 's') { - eventsManager->sendSnap("ImageDetection", std::nullopt, inRgb, inDet, {"EventsExample", "C++"}, {{"key_0", "value_0"}, {"key_1", "value_1"}}); + eventsManager->sendSnap("ImageDetection", + std::nullopt, + inRgb, + inDet, + {"EventsExample", "C++"}, + {{"key_0", "value_0"}, {"key_1", "value_1"}}, + uploadSuccessCallback, + uploadFailureCallback); } } diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index 5d763b76df..5f82227bb2 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -12,6 +12,31 @@ cv::Rect frameNorm(const cv::Mat& frame, const dai::Point2f& topLeft, const dai: return cv::Rect(cv::Point(topLeft.x * width, topLeft.y * height), cv::Point(bottomRight.x * width, bottomRight.y * height)); } +// Callback functions +void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { + std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " has been successfully uploaded to the hub" + << std::endl; +} + +void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { + switch(sendSnapResult.status) { + case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: + std::cout << "File batch preparation failed!" << std::endl; + break; + case dai::utility::SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES: + std::cout << "Snap's file group contains rejected files!" << std::endl; + break; + case dai::utility::SendSnapCallbackStatus::FILE_UPLOAD_FAILED: + std::cout << "File upload was unsuccessful!" << std::endl; + break; + case dai::utility::SendSnapCallbackStatus::SEND_EVENT_FAILED: + std::cout << "Snap could not been sent to the hub, following successful file upload!" << std::endl; + break; + default: + break; + } +} + int main() { dai::Pipeline pipeline(true); @@ -87,7 +112,12 @@ int main() { auto fileGroup = std::make_shared(); fileGroup->addImageDetectionsPair(ss.str(), inRgb, borderDetections); - eventsManager->sendSnap("LowConfidenceDetection", fileGroup, {"EventsExample", "C++"}, {{"key_0", "value_0"}, {"key_1", "value_1"}}); + eventsManager->sendSnap("LowConfidenceDetection", + fileGroup, + {"EventsExample", "C++"}, + {{"key_0", "value_0"}, {"key_1", "value_1"}}, + uploadSuccessCallback, + uploadFailureCallback); counter++; } diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index f4ed1c385c..e070547463 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -6,6 +6,23 @@ import time +# Callback functions +def upload_success_callback(send_snap_result): + print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} has been successfully uploaded to the hub") + +def upload_failure_callback(send_snap_result): + status = send_snap_result.status + print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} could not have been uploaded to the hub") + + if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: + print("File batch preparation failed!") + elif status == dai.SendSnapCallbackStatus.GROUP_CONTAINS_REJECTED_FILES: + print("Snap's file group contains rejected files!") + elif status == dai.SendSnapCallbackStatus.FILE_UPLOAD_FAILED: + print("File upload was unsuccessful!") + elif status == dai.SendSnapCallbackStatus.SEND_EVENT_FAILED: + print("Snap could not been sent to the hub, following successful file upload!") + # Create pipeline with dai.Pipeline() as pipeline: # Set your Hub team's api-key using the environment variable DEPTHAI_HUB_API_KEY. Or use the EventsManager setToken() method. @@ -70,4 +87,5 @@ def frameNorm(frame, bbox): # Trigger sendSnap() if cv2.waitKey(1) == ord("s"): - eventMan.sendSnap("ImageDetection", None, inRgb, inDet, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}) \ No newline at end of file + eventMan.sendSnap("ImageDetection", None, inRgb, inDet, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, + upload_success_callback, upload_failure_callback) \ No newline at end of file diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index baf1181380..1eadd31434 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -5,6 +5,23 @@ import numpy as np +# Callback functions +def upload_success_callback(send_snap_result): + print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} has been successfully uploaded to the hub") + +def upload_failure_callback(send_snap_result): + status = send_snap_result.status + print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} could not have been uploaded to the hub") + + if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: + print("File batch preparation failed!") + elif status == dai.SendSnapCallbackStatus.GROUP_CONTAINS_REJECTED_FILES: + print("Snap's file group contains rejected files!") + elif status == dai.SendSnapCallbackStatus.FILE_UPLOAD_FAILED: + print("File upload was unsuccessful!") + elif status == dai.SendSnapCallbackStatus.SEND_EVENT_FAILED: + print("Snap could not been sent to the hub, following successful file upload!") + # Create pipeline with dai.Pipeline() as pipeline: # Set your Hub team's api-key using the environment variable DEPTHAI_HUB_API_KEY. Or use the EventsManager setToken() method. @@ -82,6 +99,7 @@ def frameNorm(frame, bbox): fileGroup = dai.FileGroup() fileGroup.addImageDetectionsPair(fileName, inRgb, borderDetections) - eventMan.sendSnap("LowConfidenceDetection", fileGroup, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}) + eventMan.sendSnap("LowConfidenceDetection", fileGroup, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, + upload_success_callback, upload_failure_callback) counter += 1 \ No newline at end of file diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 09ba9caa12..0ec5366c7c 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -69,6 +69,15 @@ class FileGroup { friend class EventsManager; }; +enum class SendSnapCallbackStatus { SUCCESS, FILE_BATCH_PREPARATION_FAILED, GROUP_CONTAINS_REJECTED_FILES, FILE_UPLOAD_FAILED, SEND_EVENT_FAILED }; + +struct SendSnapCallbackResult { + public: + std::string snapName; + int64_t snapTimestamp; + SendSnapCallbackStatus status; +}; + class EventsManager { public: explicit EventsManager(bool uploadCachedOnStart = false); @@ -79,14 +88,12 @@ class EventsManager { * @param name Name of the event * @param tags List of tags to send * @param extras Extra data to send - * @param deviceSerialNo Device serial number * @param associateFiles List of associate files with ids * @return bool */ bool sendEvent(const std::string& name, const std::vector& tags = {}, const std::unordered_map& extras = {}, - const std::string& deviceSerialNo = "", const std::vector& associateFiles = {}); /** * Send a snap to the events service. Snaps should be used for sending images and other files. @@ -94,14 +101,16 @@ class EventsManager { * @param fileGroup FileGroup containing FileData objects to send * @param tags List of tags to send * @param extras Extra data to send - * @param deviceSerialNo Device serial number + * @param successCallback Callback to be called when the snap is successfully uploaded to the hub + * @param failureCallback Callback to be called if the snap upload is unsuccessful * @return bool */ bool sendSnap(const std::string& name, const std::shared_ptr fileGroup, const std::vector& tags = {}, const std::unordered_map& extras = {}, - const std::string& deviceSerialNo = ""); + const std::function successCallback = nullptr, + const std::function failureCallback = nullptr); /** * Send a snap to the events service, with an ImgFrame and ImgDetections pair as files * @param name Name of the snap @@ -110,7 +119,8 @@ class EventsManager { * @param imgDetections ImgDetections to sent * @param tags List of tags to send * @param extras Extra data to send - * @param deviceSerialNo Device serial number + * @param successCallback Callback to be called when the snap is successfully uploaded to the hub + * @param failureCallback Callback to be called if the snap upload is unsuccessful * @return bool */ bool sendSnap(const std::string& name, @@ -119,7 +129,8 @@ class EventsManager { const std::optional>& imgDetections = std::nullopt, const std::vector& tags = {}, const std::unordered_map& extras = {}, - const std::string& deviceSerialNo = ""); + const std::function successCallback = nullptr, + const std::function failureCallback = nullptr); /** * Set the token for the events service. By default, the token is taken from the environment variable DEPTHAI_HUB_API_KEY * @param token Token for the events service @@ -152,8 +163,13 @@ class EventsManager { void setCacheIfCannotSend(bool cacheIfCannotSend); private: - struct SnapData { + struct EventData { std::shared_ptr event; + std::optional, std::function>> callbacks; + }; + + struct SnapData { + std::shared_ptr eventData; std::shared_ptr fileGroup; }; @@ -216,13 +232,14 @@ class EventsManager { std::string url; std::string sourceAppId; std::string sourceAppIdentifier; + std::string sourceSerialNumber; float publishInterval; bool logResponse; bool verifySsl; std::string cacheDir; bool cacheIfCannotSend; std::unique_ptr uploadThread; - std::deque> eventBuffer; + std::deque> eventBuffer; std::deque> snapBuffer; std::deque> uploadFileBatchFutures; std::mutex eventBufferMutex; diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 1b9a074917..25f0d4aad1 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -13,6 +13,7 @@ #include "Environment.hpp" #include "Logging.hpp" #include "cpr/cpr.h" +#include "depthai/device/DeviceBase.hpp" #include "depthai/schemas/Event.pb.h" namespace dai { @@ -241,6 +242,12 @@ EventsManager::EventsManager(bool uploadCachedOnStart) auto containerId = utility::getEnvAs("OAKAGENT_CONTAINER_ID", ""); sourceAppId = appId == "" ? containerId : appId; sourceAppIdentifier = utility::getEnvAs("OAKAGENT_APP_IDENTIFIER", ""); + auto connectedDevices = DeviceBase::getAllConnectedDevices(); + if(!connectedDevices.empty()) { + sourceSerialNumber = connectedDevices[0].getDeviceId(); + } else { + sourceSerialNumber = ""; + } url = utility::getEnvAs("DEPTHAI_HUB_EVENTS_BASE_URL", "https://events.cloud.luxonis.com"); token = utility::getEnvAs("DEPTHAI_HUB_API_KEY", ""); // Thread handling preparation and uploads @@ -419,6 +426,13 @@ void EventsManager::uploadFileBatch(std::deque> inputS eventBufferCondition.wait_for(lock, duration, [this]() { return stopUploadThread.load(); }); // After retrying a defined number of times, we can determine the connection is not established, cache if enabled if(retryAttempt >= uploadRetryPolicy.maxAttempts) { + for(size_t index = 0; index < inputSnapBatch.size(); ++index) { + if(inputSnapBatch.at(index)->eventData->callbacks.has_value()) { + auto event = inputSnapBatch.at(index)->eventData->event; + inputSnapBatch.at(index)->eventData->callbacks.value().second( + SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); + } + } if(cacheIfCannotSend) { cacheSnapData(inputSnapBatch); } else { @@ -444,7 +458,12 @@ void EventsManager::uploadFileBatch(std::deque> inputS std::string rejectionReason = dai::proto::event::RejectedFileGroupReason_descriptor() ->FindValueByNumber(static_cast(prepareGroupResult.rejected().reason())) ->name(); - logger::info("A group has been rejected because of {}", rejectionReason); + logger::error("A group has been rejected because of {}", rejectionReason); + if(snapData->eventData->callbacks.has_value()) { + auto event = snapData->eventData->event; + snapData->eventData->callbacks.value().second( + SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + } continue; } // Handle groups asynchronously @@ -477,7 +496,7 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: auto prepareFileResult = prepareGroupResult.files(i); if(prepareFileResult.result_case() == proto::event::FileUploadResult::kAccepted) { // Add an associate file to the event - auto associateFile = snapData->event->add_associate_files(); + auto associateFile = snapData->eventData->event->add_associate_files(); associateFile->set_id(prepareFileResult.accepted().id()); // Upload files asynchronously fileUploadResults.emplace_back(std::async( @@ -486,6 +505,11 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: return uploadFile(std::move(fileData), std::move(uploadUrl)); })); } else { + if(snapData->eventData->callbacks.has_value()) { + auto event = snapData->eventData->event; + snapData->eventData->callbacks.value().second( + SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + } return false; } } @@ -493,12 +517,17 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: for(auto& uploadResult : fileUploadResults) { if(!uploadResult.valid() || !uploadResult.get()) { logger::info("Failed to upload all of the files in the given group"); + if(snapData->eventData->callbacks.has_value()) { + auto event = snapData->eventData->event; + snapData->eventData->callbacks.value().second( + SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); + } return false; } } // Once all of the files are uploaded, the event can be sent std::lock_guard lock(eventBufferMutex); - eventBuffer.push_back(std::move(snapData->event)); + eventBuffer.push_back(std::move(snapData->eventData)); return true; } @@ -555,7 +584,7 @@ void EventsManager::uploadEventBatch() { return; } for(size_t i = 0; i < eventBuffer.size() && i < eventsPerRequest; ++i) { - eventBatch->add_events()->CopyFrom(*eventBuffer.at(i).get()); + eventBatch->add_events()->CopyFrom(*eventBuffer.at(i)->event.get()); } } std::string serializedBatch; @@ -582,6 +611,15 @@ void EventsManager::uploadEventBatch() { logger::error("Failed to send event, status code: {}", response.status_code); // In case the eventBuffer gets too full (dropped connection), cache the events or drop them if(eventBuffer.size() >= EVENT_BUFFER_MAX_SIZE) { + // failureCallback + for(size_t index = 0; index < eventBuffer.size(); ++index) { + if(!eventBuffer.at(index)->callbacks.has_value()) { + continue; + } + auto event = eventBuffer.at(index)->event; + eventBuffer.at(index)->callbacks.value().second( + SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::SEND_EVENT_FAILED}); + } if(cacheIfCannotSend) { cacheEvents(); } else { @@ -598,6 +636,14 @@ void EventsManager::uploadEventBatch() { logger::info("BatchUploadEvents response: \n{}", eventBatchUploadResults->DebugString()); } std::lock_guard lock(eventBufferMutex); + // successCallback + for(int index = 0; index < eventBatch->events_size(); ++index) { + if(!eventBuffer.at(index)->callbacks.has_value()) { + continue; + } + auto event = eventBuffer.at(index)->event; + eventBuffer.at(index)->callbacks.value().first(SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::SUCCESS}); + } eventBuffer.erase(eventBuffer.begin(), eventBuffer.begin() + eventBatch->events_size()); } } @@ -605,7 +651,6 @@ void EventsManager::uploadEventBatch() { bool EventsManager::sendEvent(const std::string& name, const std::vector& tags, const std::unordered_map& extras, - const std::string& deviceSerialNo, const std::vector& associateFiles) { // Check if the configuration and limits have already been fetched if(!configurationLimitsFetched) { @@ -624,7 +669,7 @@ bool EventsManager::sendEvent(const std::string& name, for(const auto& entry : extras) { extrasData->insert({entry.first, entry.second}); } - event->set_source_serial_number(deviceSerialNo); + event->set_source_serial_number(sourceSerialNumber); event->set_source_app_id(sourceAppId); event->set_source_app_identifier(sourceAppIdentifier); for(const auto& file : associateFiles) { @@ -638,7 +683,9 @@ bool EventsManager::sendEvent(const std::string& name, // Add event to eventBuffer std::lock_guard lock(eventBufferMutex); - eventBuffer.push_back(std::move(event)); + auto eventData = std::make_unique(); + eventData->event = std::move(event); + eventBuffer.push_back(std::move(eventData)); return true; } @@ -646,7 +693,8 @@ bool EventsManager::sendSnap(const std::string& name, const std::shared_ptr fileGroup, const std::vector& tags, const std::unordered_map& extras, - const std::string& deviceSerialNo) { + const std::function successCallback, + const std::function failureCallback) { // Check if the configuration and limits have already been fetched if(!configurationLimitsFetched) { logger::error("The configuration and limits have not been successfully fetched, snap not sent"); @@ -656,22 +704,24 @@ bool EventsManager::sendSnap(const std::string& name, // Prepare snapData auto snapData = std::make_unique(); snapData->fileGroup = fileGroup; + snapData->eventData = std::make_unique(); + snapData->eventData->callbacks.emplace(successCallback, failureCallback); // Create an event - snapData->event = std::make_unique(); - snapData->event->set_created_at(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); - snapData->event->set_name(name); - snapData->event->add_tags("snap"); + snapData->eventData->event = std::make_unique(); + snapData->eventData->event->set_created_at(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); + snapData->eventData->event->set_name(name); + snapData->eventData->event->add_tags("snap"); for(const auto& tag : tags) { - snapData->event->add_tags(tag); + snapData->eventData->event->add_tags(tag); } - auto* extrasData = snapData->event->mutable_extras(); + auto* extrasData = snapData->eventData->event->mutable_extras(); for(const auto& entry : extras) { extrasData->insert({entry.first, entry.second}); } - snapData->event->set_source_serial_number(deviceSerialNo); - snapData->event->set_source_app_id(sourceAppId); - snapData->event->set_source_app_identifier(sourceAppIdentifier); - if(!validateEvent(*snapData->event)) { + snapData->eventData->event->set_source_serial_number(sourceSerialNumber); + snapData->eventData->event->set_source_app_id(sourceAppId); + snapData->eventData->event->set_source_app_identifier(sourceAppIdentifier); + if(!validateEvent(*snapData->eventData->event)) { logger::error("Failed to send snap, validation failed"); return false; } @@ -703,7 +753,8 @@ bool EventsManager::sendSnap(const std::string& name, const std::optional>& imgDetections, const std::vector& tags, const std::unordered_map& extras, - const std::string& deviceSerialNo) { + const std::function successCallback, + const std::function failureCallback) { // Create a FileGroup and send a snap containing it auto fileGroup = std::make_shared(); if(imgDetections.has_value()) { @@ -712,7 +763,7 @@ bool EventsManager::sendSnap(const std::string& name, fileGroup->addFile(fileName, imgFrame); } - return sendSnap(name, fileGroup, tags, extras, deviceSerialNo); + return sendSnap(name, fileGroup, tags, extras, successCallback, failureCallback); } bool EventsManager::validateEvent(const proto::event::Event& inputEvent) { @@ -782,9 +833,9 @@ void EventsManager::cacheEvents() { // Create a unique directory and save the protobuf message for each event in the eventBuffer logger::info("Caching events"); std::lock_guard lock(eventBufferMutex); - for(const auto& event : eventBuffer) { + for(const auto& eventData : eventBuffer) { std::filesystem::path path(cacheDir); - path = path / ("event_" + event->name() + "_" + std::to_string(event->created_at())); + path = path / ("event_" + eventData->event->name() + "_" + std::to_string(eventData->event->created_at())); std::string eventDir = path.string(); logger::info("Caching event to {}", eventDir); if(!std::filesystem::exists(cacheDir)) { @@ -792,7 +843,7 @@ void EventsManager::cacheEvents() { } std::filesystem::create_directory(eventDir); std::ofstream eventFile(path / "event.pb", std::ios::binary); - event->SerializeToOstream(&eventFile); + eventData->event->SerializeToOstream(&eventFile); } eventBuffer.clear(); } @@ -802,7 +853,7 @@ void EventsManager::cacheSnapData(std::deque>& inputSn logger::info("Caching snapData"); for(const auto& snap : inputSnapBatch) { std::filesystem::path path(cacheDir); - path = path / ("snap_" + snap->event->name() + "_" + std::to_string(snap->event->created_at())); + path = path / ("snap_" + snap->eventData->event->name() + "_" + std::to_string(snap->eventData->event->created_at())); std::string snapDir = path.string(); logger::info("Caching snap to {}", snapDir); if(!std::filesystem::exists(cacheDir)) { @@ -810,7 +861,7 @@ void EventsManager::cacheSnapData(std::deque>& inputSn } std::filesystem::create_directory(snapDir); std::ofstream eventFile(path / "snap.pb", std::ios::binary); - snap->event->SerializeToOstream(&eventFile); + snap->eventData->event->SerializeToOstream(&eventFile); for(auto& file : snap->fileGroup->fileData) { file->toFile(path); } @@ -832,10 +883,12 @@ void EventsManager::uploadCachedData() { if(entry.path().filename().string().rfind("event", 0) == 0) { std::ifstream eventFile(entry.path() / "event.pb", std::ios::binary); + auto eventData = std::make_unique(); auto event = std::make_shared(); event->ParseFromIstream(&eventFile); std::lock_guard lock(eventBufferMutex); - eventBuffer.push_back(std::move(event)); + eventData->event = event; + eventBuffer.push_back(std::move(eventData)); // Clear entries added to the eventBuffer clearCachedData(entry.path()); @@ -851,7 +904,7 @@ void EventsManager::uploadCachedData() { fileGroup->fileData.push_back(fileData); } } - snapData->event = event; + snapData->eventData->event = event; snapData->fileGroup = fileGroup; std::lock_guard lock(snapBufferMutex); snapBuffer.push_back(std::move(snapData)); From b215f027224f3be5b66d28c0a53eff6968812700 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 18 Dec 2025 10:01:05 +0100 Subject: [PATCH 020/180] SendSnapCallbackResult now includes snap's payload as well --- .../src/utility/EventsManagerBindings.cpp | 3 +- examples/cpp/Events/events.cpp | 2 +- examples/cpp/Events/events_file_group.cpp | 5 +++- examples/python/Events/events.py | 2 +- examples/python/Events/events_file_group.py | 2 +- include/depthai/utility/EventsManager.hpp | 3 +- src/utility/EventsManager.cpp | 29 ++++++++++++++----- 7 files changed, 32 insertions(+), 14 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index 8ddad925f5..a523b2fabe 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -96,7 +96,8 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { .def(py::init<>()) .def_readonly("snapName", &SendSnapCallbackResult::snapName) .def_readonly("snapTimestamp", &SendSnapCallbackResult::snapTimestamp) - .def_readonly("status", &SendSnapCallbackResult::status); + .def_readonly("snapPayload", &SendSnapCallbackResult::snapPayload) + .def_readonly("uploadStatus", &SendSnapCallbackResult::uploadStatus); py::class_(m, "EventsManager") .def(py::init<>()) diff --git a/examples/cpp/Events/events.cpp b/examples/cpp/Events/events.cpp index 23be5e2e36..5171acad11 100644 --- a/examples/cpp/Events/events.cpp +++ b/examples/cpp/Events/events.cpp @@ -22,7 +22,7 @@ void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " could not have been uploaded to the hub" << std::endl; - switch(sendSnapResult.status) { + switch(sendSnapResult.uploadStatus) { case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: std::cout << "File batch preparation failed!" << std::endl; break; diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index 5f82227bb2..4dee76d937 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -19,7 +19,10 @@ void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) } void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - switch(sendSnapResult.status) { + std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " could not have been uploaded to the hub" + << std::endl; + + switch(sendSnapResult.uploadStatus) { case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: std::cout << "File batch preparation failed!" << std::endl; break; diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index e070547463..95bc0e87ed 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -11,7 +11,7 @@ def upload_success_callback(send_snap_result): print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} has been successfully uploaded to the hub") def upload_failure_callback(send_snap_result): - status = send_snap_result.status + status = send_snap_result.uploadStatus print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} could not have been uploaded to the hub") if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index 1eadd31434..fa05a5ba14 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -10,7 +10,7 @@ def upload_success_callback(send_snap_result): print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} has been successfully uploaded to the hub") def upload_failure_callback(send_snap_result): - status = send_snap_result.status + status = send_snap_result.uploadStatus print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} could not have been uploaded to the hub") if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 0ec5366c7c..f393860b69 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -75,7 +75,8 @@ struct SendSnapCallbackResult { public: std::string snapName; int64_t snapTimestamp; - SendSnapCallbackStatus status; + std::string snapPayload; + SendSnapCallbackStatus uploadStatus; }; class EventsManager { diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 25f0d4aad1..d51c427cd1 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -429,8 +429,10 @@ void EventsManager::uploadFileBatch(std::deque> inputS for(size_t index = 0; index < inputSnapBatch.size(); ++index) { if(inputSnapBatch.at(index)->eventData->callbacks.has_value()) { auto event = inputSnapBatch.at(index)->eventData->event; - inputSnapBatch.at(index)->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); + std::string serializedPayload; + event->SerializeToString(&serializedPayload); + inputSnapBatch.at(index)->eventData->callbacks.value().second(SendSnapCallbackResult{ + event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); } } if(cacheIfCannotSend) { @@ -461,8 +463,10 @@ void EventsManager::uploadFileBatch(std::deque> inputS logger::error("A group has been rejected because of {}", rejectionReason); if(snapData->eventData->callbacks.has_value()) { auto event = snapData->eventData->event; - snapData->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + std::string serializedPayload; + event->SerializeToString(&serializedPayload); + snapData->eventData->callbacks.value().second(SendSnapCallbackResult{ + event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } continue; } @@ -507,8 +511,10 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: } else { if(snapData->eventData->callbacks.has_value()) { auto event = snapData->eventData->event; + std::string serializedPayload; + event->SerializeToString(&serializedPayload); snapData->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } return false; } @@ -519,8 +525,10 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: logger::info("Failed to upload all of the files in the given group"); if(snapData->eventData->callbacks.has_value()) { auto event = snapData->eventData->event; + std::string serializedPayload; + event->SerializeToString(&serializedPayload); snapData->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); + SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); } return false; } @@ -617,8 +625,10 @@ void EventsManager::uploadEventBatch() { continue; } auto event = eventBuffer.at(index)->event; + std::string serializedPayload; + event->SerializeToString(&serializedPayload); eventBuffer.at(index)->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::SEND_EVENT_FAILED}); + SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::SEND_EVENT_FAILED}); } if(cacheIfCannotSend) { cacheEvents(); @@ -642,7 +652,10 @@ void EventsManager::uploadEventBatch() { continue; } auto event = eventBuffer.at(index)->event; - eventBuffer.at(index)->callbacks.value().first(SendSnapCallbackResult{event->name(), event->created_at(), SendSnapCallbackStatus::SUCCESS}); + std::string serializedPayload; + event->SerializeToString(&serializedPayload); + eventBuffer.at(index)->callbacks.value().first( + SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::SUCCESS}); } eventBuffer.erase(eventBuffer.begin(), eventBuffer.begin() + eventBatch->events_size()); } From 52dca071dcd5a77c9569b86e322d0684a25bb516 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 18 Dec 2025 12:36:46 +0100 Subject: [PATCH 021/180] SendSnapCallbackResult now contains ids for accepted events --- .../src/utility/EventsManagerBindings.cpp | 4 ++- examples/cpp/Events/events.cpp | 7 ++--- examples/cpp/Events/events_file_group.cpp | 7 ++--- examples/python/Events/events.py | 6 ++-- examples/python/Events/events_file_group.py | 6 ++-- include/depthai/utility/EventsManager.hpp | 10 ++++++- src/utility/EventsManager.cpp | 28 ++++++++++++------- 7 files changed, 42 insertions(+), 26 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index a523b2fabe..71d860f663 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -90,12 +90,14 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { .value("FILE_BATCH_PREPARATION_FAILED", SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED) .value("GROUP_CONTAINS_REJECTED_FILES", SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES) .value("FILE_UPLOAD_FAILED", SendSnapCallbackStatus::FILE_UPLOAD_FAILED) - .value("SEND_EVENT_FAILED", SendSnapCallbackStatus::SEND_EVENT_FAILED); + .value("SEND_EVENT_FAILED", SendSnapCallbackStatus::SEND_EVENT_FAILED) + .value("EVENT_REJECTED", SendSnapCallbackStatus::EVENT_REJECTED); py::class_(m, "SendSnapCallbackResult") .def(py::init<>()) .def_readonly("snapName", &SendSnapCallbackResult::snapName) .def_readonly("snapTimestamp", &SendSnapCallbackResult::snapTimestamp) + .def_readonly("snapID", &SendSnapCallbackResult::snapID) .def_readonly("snapPayload", &SendSnapCallbackResult::snapPayload) .def_readonly("uploadStatus", &SendSnapCallbackResult::uploadStatus); diff --git a/examples/cpp/Events/events.cpp b/examples/cpp/Events/events.cpp index 5171acad11..83b55f629c 100644 --- a/examples/cpp/Events/events.cpp +++ b/examples/cpp/Events/events.cpp @@ -14,13 +14,12 @@ cv::Rect frameNorm(const cv::Mat& frame, const dai::Point2f& topLeft, const dai: // Callback functions void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " has been successfully uploaded to the hub" - << std::endl; + std::cout << "Successfully uploaded Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ", " + << sendSnapResult.snapID.value_or("") << ") to the hub." << std::endl; } void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " could not have been uploaded to the hub" - << std::endl; + std::cout << "Upload of Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ") to the hub has failed." << std::endl; switch(sendSnapResult.uploadStatus) { case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index 4dee76d937..1d7ac41cdc 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -14,13 +14,12 @@ cv::Rect frameNorm(const cv::Mat& frame, const dai::Point2f& topLeft, const dai: // Callback functions void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " has been successfully uploaded to the hub" - << std::endl; + std::cout << "Successfully uploaded Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ", " + << sendSnapResult.snapID.value_or("") << ") to the hub." << std::endl; } void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - std::cout << "Snap: " << sendSnapResult.snapName << " with a timestamp: " << sendSnapResult.snapTimestamp << " could not have been uploaded to the hub" - << std::endl; + std::cout << "Upload of Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ") to the hub has failed." << std::endl; switch(sendSnapResult.uploadStatus) { case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index 95bc0e87ed..d31a300562 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -8,12 +8,12 @@ # Callback functions def upload_success_callback(send_snap_result): - print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} has been successfully uploaded to the hub") + print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapID}) to the hub.") def upload_failure_callback(send_snap_result): - status = send_snap_result.uploadStatus - print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} could not have been uploaded to the hub") + print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}) to the hub has failed.") + status = send_snap_result.uploadStatus if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: print("File batch preparation failed!") elif status == dai.SendSnapCallbackStatus.GROUP_CONTAINS_REJECTED_FILES: diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index fa05a5ba14..5e2cab0ac3 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -7,12 +7,12 @@ # Callback functions def upload_success_callback(send_snap_result): - print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} has been successfully uploaded to the hub") + print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapID}) to the hub.") def upload_failure_callback(send_snap_result): - status = send_snap_result.uploadStatus - print(f"Snap: {send_snap_result.snapName} with a timestamp: {send_snap_result.snapTimestamp} could not have been uploaded to the hub") + print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}) to the hub has failed.") + status = send_snap_result.uploadStatus if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: print("File batch preparation failed!") elif status == dai.SendSnapCallbackStatus.GROUP_CONTAINS_REJECTED_FILES: diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index f393860b69..987549a53a 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -69,12 +69,20 @@ class FileGroup { friend class EventsManager; }; -enum class SendSnapCallbackStatus { SUCCESS, FILE_BATCH_PREPARATION_FAILED, GROUP_CONTAINS_REJECTED_FILES, FILE_UPLOAD_FAILED, SEND_EVENT_FAILED }; +enum class SendSnapCallbackStatus { + SUCCESS, + FILE_BATCH_PREPARATION_FAILED, + GROUP_CONTAINS_REJECTED_FILES, + FILE_UPLOAD_FAILED, + SEND_EVENT_FAILED, + EVENT_REJECTED +}; struct SendSnapCallbackResult { public: std::string snapName; int64_t snapTimestamp; + std::optional snapID; std::string snapPayload; SendSnapCallbackStatus uploadStatus; }; diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index d51c427cd1..5aa3a7059d 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -432,7 +432,7 @@ void EventsManager::uploadFileBatch(std::deque> inputS std::string serializedPayload; event->SerializeToString(&serializedPayload); inputSnapBatch.at(index)->eventData->callbacks.value().second(SendSnapCallbackResult{ - event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); + event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); } } if(cacheIfCannotSend) { @@ -466,7 +466,7 @@ void EventsManager::uploadFileBatch(std::deque> inputS std::string serializedPayload; event->SerializeToString(&serializedPayload); snapData->eventData->callbacks.value().second(SendSnapCallbackResult{ - event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } continue; } @@ -513,8 +513,8 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + snapData->eventData->callbacks.value().second(SendSnapCallbackResult{ + event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } return false; } @@ -528,7 +528,7 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: std::string serializedPayload; event->SerializeToString(&serializedPayload); snapData->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); + SendSnapCallbackResult{event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); } return false; } @@ -628,7 +628,7 @@ void EventsManager::uploadEventBatch() { std::string serializedPayload; event->SerializeToString(&serializedPayload); eventBuffer.at(index)->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::SEND_EVENT_FAILED}); + SendSnapCallbackResult{event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::SEND_EVENT_FAILED}); } if(cacheIfCannotSend) { cacheEvents(); @@ -640,9 +640,9 @@ void EventsManager::uploadEventBatch() { } } else { logger::info("Event sent successfully"); + auto eventBatchUploadResults = std::make_unique(); + eventBatchUploadResults->ParseFromString(response.text); if(logResponse) { - auto eventBatchUploadResults = std::make_unique(); - eventBatchUploadResults->ParseFromString(response.text); logger::info("BatchUploadEvents response: \n{}", eventBatchUploadResults->DebugString()); } std::lock_guard lock(eventBufferMutex); @@ -654,8 +654,16 @@ void EventsManager::uploadEventBatch() { auto event = eventBuffer.at(index)->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - eventBuffer.at(index)->callbacks.value().first( - SendSnapCallbackResult{event->name(), event->created_at(), serializedPayload, SendSnapCallbackStatus::SUCCESS}); + if(eventBatchUploadResults->events(index).result_case() == proto::event::EventResult::kAccepted) { + eventBuffer.at(index)->callbacks.value().first(SendSnapCallbackResult{event->name(), + event->created_at(), + eventBatchUploadResults->events(index).accepted().id(), + serializedPayload, + SendSnapCallbackStatus::SUCCESS}); + } else { + eventBuffer.at(index)->callbacks.value().second( + SendSnapCallbackResult{event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::EVENT_REJECTED}); + } } eventBuffer.erase(eventBuffer.begin(), eventBuffer.begin() + eventBatch->events_size()); } From 00cdd09c95ef4f49fec96073616d5c82fdc0c5eb Mon Sep 17 00:00:00 2001 From: AljazD Date: Tue, 23 Dec 2025 10:06:37 +0100 Subject: [PATCH 022/180] Added depthai local snap ids for success/failure callbacks of sendSnap() --- .../src/utility/EventsManagerBindings.cpp | 3 +- examples/cpp/Events/events.cpp | 6 +- examples/cpp/Events/events_file_group.cpp | 6 +- examples/python/Events/events.py | 4 +- examples/python/Events/events_file_group.py | 4 +- include/depthai/utility/EventsManager.hpp | 4 +- src/utility/EventsManager.cpp | 62 +++++++++++++++---- 7 files changed, 64 insertions(+), 25 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index 71d860f663..c5e6184cad 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -97,7 +97,8 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { .def(py::init<>()) .def_readonly("snapName", &SendSnapCallbackResult::snapName) .def_readonly("snapTimestamp", &SendSnapCallbackResult::snapTimestamp) - .def_readonly("snapID", &SendSnapCallbackResult::snapID) + .def_readonly("snapLocalID", &SendSnapCallbackResult::snapLocalID) + .def_readonly("snapHubID", &SendSnapCallbackResult::snapHubID) .def_readonly("snapPayload", &SendSnapCallbackResult::snapPayload) .def_readonly("uploadStatus", &SendSnapCallbackResult::uploadStatus); diff --git a/examples/cpp/Events/events.cpp b/examples/cpp/Events/events.cpp index 83b55f629c..84fc5e507d 100644 --- a/examples/cpp/Events/events.cpp +++ b/examples/cpp/Events/events.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -15,11 +14,12 @@ cv::Rect frameNorm(const cv::Mat& frame, const dai::Point2f& topLeft, const dai: // Callback functions void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { std::cout << "Successfully uploaded Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ", " - << sendSnapResult.snapID.value_or("") << ") to the hub." << std::endl; + << sendSnapResult.snapHubID.value_or("") << ") to the hub." << std::endl; } void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - std::cout << "Upload of Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ") to the hub has failed." << std::endl; + std::cout << "Upload of Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ", " << sendSnapResult.snapLocalID + << ") to the hub has failed." << std::endl; switch(sendSnapResult.uploadStatus) { case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index 1d7ac41cdc..ab91886e29 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -15,11 +14,12 @@ cv::Rect frameNorm(const cv::Mat& frame, const dai::Point2f& topLeft, const dai: // Callback functions void uploadSuccessCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { std::cout << "Successfully uploaded Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ", " - << sendSnapResult.snapID.value_or("") << ") to the hub." << std::endl; + << sendSnapResult.snapHubID.value_or("") << ") to the hub." << std::endl; } void uploadFailureCallback(dai::utility::SendSnapCallbackResult sendSnapResult) { - std::cout << "Upload of Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ") to the hub has failed." << std::endl; + std::cout << "Upload of Snap: (" << sendSnapResult.snapName << ", " << sendSnapResult.snapTimestamp << ", " << sendSnapResult.snapLocalID + << ") to the hub has failed." << std::endl; switch(sendSnapResult.uploadStatus) { case dai::utility::SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED: diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index d31a300562..d240bf7430 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -8,10 +8,10 @@ # Callback functions def upload_success_callback(send_snap_result): - print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapID}) to the hub.") + print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapHubID}) to the hub.") def upload_failure_callback(send_snap_result): - print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}) to the hub has failed.") + print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapLocalID}) to the hub has failed.") status = send_snap_result.uploadStatus if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index 5e2cab0ac3..cae98fce0a 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -7,10 +7,10 @@ # Callback functions def upload_success_callback(send_snap_result): - print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapID}) to the hub.") + print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapHubID}) to the hub.") def upload_failure_callback(send_snap_result): - print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}) to the hub has failed.") + print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapLocalID}) to the hub has failed.") status = send_snap_result.uploadStatus if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 987549a53a..15e1b48c35 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -82,7 +82,8 @@ struct SendSnapCallbackResult { public: std::string snapName; int64_t snapTimestamp; - std::optional snapID; + std::string snapLocalID; + std::optional snapHubID; std::string snapPayload; SendSnapCallbackStatus uploadStatus; }; @@ -173,6 +174,7 @@ class EventsManager { private: struct EventData { + std::string localID; std::shared_ptr event; std::optional, std::function>> callbacks; }; diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 5aa3a7059d..dcfdcd413d 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -6,10 +6,18 @@ #include #include #include -#include #include #include +namespace { +std::string generateLocalID(int64_t timestamp) { + static std::atomic counter{0}; + std::ostringstream oss; + oss << timestamp << "_" << std::setw(6) << std::setfill('0') << counter++; + return oss.str(); +} +} // namespace + #include "Environment.hpp" #include "Logging.hpp" #include "cpr/cpr.h" @@ -431,8 +439,13 @@ void EventsManager::uploadFileBatch(std::deque> inputS auto event = inputSnapBatch.at(index)->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - inputSnapBatch.at(index)->eventData->callbacks.value().second(SendSnapCallbackResult{ - event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); + inputSnapBatch.at(index)->eventData->callbacks.value().second( + SendSnapCallbackResult{event->name(), + event->created_at(), + inputSnapBatch.at(index)->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); } } if(cacheIfCannotSend) { @@ -465,8 +478,12 @@ void EventsManager::uploadFileBatch(std::deque> inputS auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second(SendSnapCallbackResult{ - event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + snapData->eventData->callbacks.value().second(SendSnapCallbackResult{event->name(), + event->created_at(), + snapData->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } continue; } @@ -513,8 +530,12 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second(SendSnapCallbackResult{ - event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + snapData->eventData->callbacks.value().second(SendSnapCallbackResult{event->name(), + event->created_at(), + snapData->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } return false; } @@ -527,8 +548,12 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); + snapData->eventData->callbacks.value().second(SendSnapCallbackResult{event->name(), + event->created_at(), + snapData->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); } return false; } @@ -627,8 +652,12 @@ void EventsManager::uploadEventBatch() { auto event = eventBuffer.at(index)->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - eventBuffer.at(index)->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::SEND_EVENT_FAILED}); + eventBuffer.at(index)->callbacks.value().second(SendSnapCallbackResult{event->name(), + event->created_at(), + eventBuffer.at(index)->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::SEND_EVENT_FAILED}); } if(cacheIfCannotSend) { cacheEvents(); @@ -657,12 +686,17 @@ void EventsManager::uploadEventBatch() { if(eventBatchUploadResults->events(index).result_case() == proto::event::EventResult::kAccepted) { eventBuffer.at(index)->callbacks.value().first(SendSnapCallbackResult{event->name(), event->created_at(), + eventBuffer.at(index)->localID, eventBatchUploadResults->events(index).accepted().id(), serializedPayload, SendSnapCallbackStatus::SUCCESS}); } else { - eventBuffer.at(index)->callbacks.value().second( - SendSnapCallbackResult{event->name(), event->created_at(), std::nullopt, serializedPayload, SendSnapCallbackStatus::EVENT_REJECTED}); + eventBuffer.at(index)->callbacks.value().second(SendSnapCallbackResult{event->name(), + event->created_at(), + eventBuffer.at(index)->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::EVENT_REJECTED}); } } eventBuffer.erase(eventBuffer.begin(), eventBuffer.begin() + eventBatch->events_size()); @@ -705,6 +739,7 @@ bool EventsManager::sendEvent(const std::string& name, // Add event to eventBuffer std::lock_guard lock(eventBufferMutex); auto eventData = std::make_unique(); + eventData->localID = generateLocalID(event->created_at()); eventData->event = std::move(event); eventBuffer.push_back(std::move(eventData)); return true; @@ -730,6 +765,7 @@ bool EventsManager::sendSnap(const std::string& name, // Create an event snapData->eventData->event = std::make_unique(); snapData->eventData->event->set_created_at(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); + snapData->eventData->localID = generateLocalID(snapData->eventData->event->created_at()); snapData->eventData->event->set_name(name); snapData->eventData->event->add_tags("snap"); for(const auto& tag : tags) { From 8c938095b6fdd557198ee5b04b3a7bb3928d3ff0 Mon Sep 17 00:00:00 2001 From: AljazD Date: Fri, 9 Jan 2026 14:12:12 +0100 Subject: [PATCH 023/180] EventData now holds separate onSuccess & onFailure callbacks. Minor fixes --- .../src/utility/EventsManagerBindings.cpp | 5 +- examples/python/Events/events.py | 2 + examples/python/Events/events_file_group.py | 2 + include/depthai/utility/EventsManager.hpp | 3 +- src/utility/EventsManager.cpp | 117 +++++++++--------- 5 files changed, 67 insertions(+), 62 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index c5e6184cad..9a04f13cd8 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -8,9 +8,6 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { using namespace dai; - // Type definitions - py::enum_ sendSnapCallbackStatus(m, "SendSnapCallbackStatus", DOC(dai, SendSnapCallbackStatus)); - /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -86,6 +83,8 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { // py::arg("nnData"), // DOC(dai, utility, FileGroup, addImageNNDataPair)); + py::enum_ sendSnapCallbackStatus(m, "SendSnapCallbackStatus", DOC(dai, SendSnapCallbackStatus)); + sendSnapCallbackStatus.value("SUCCESS", SendSnapCallbackStatus::SUCCESS) .value("FILE_BATCH_PREPARATION_FAILED", SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED) .value("GROUP_CONTAINS_REJECTED_FILES", SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES) diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index d240bf7430..adfd776509 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -8,9 +8,11 @@ # Callback functions def upload_success_callback(send_snap_result): + assert (send_snap_result.uploadStatus == dai.SendSnapCallbackStatus.SUCCESS) print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapHubID}) to the hub.") def upload_failure_callback(send_snap_result): + assert (send_snap_result.uploadStatus != dai.SendSnapCallbackStatus.SUCCESS) print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapLocalID}) to the hub has failed.") status = send_snap_result.uploadStatus diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index cae98fce0a..44e52ea2b2 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -7,9 +7,11 @@ # Callback functions def upload_success_callback(send_snap_result): + assert (send_snap_result.uploadStatus == dai.SendSnapCallbackStatus.SUCCESS) print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapHubID}) to the hub.") def upload_failure_callback(send_snap_result): + assert (send_snap_result.uploadStatus != dai.SendSnapCallbackStatus.SUCCESS) print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapLocalID}) to the hub has failed.") status = send_snap_result.uploadStatus diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 15e1b48c35..a371291b32 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -176,7 +176,8 @@ class EventsManager { struct EventData { std::string localID; std::shared_ptr event; - std::optional, std::function>> callbacks; + std::optional> onSuccess; + std::optional> onFailure; }; struct SnapData { diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index dcfdcd413d..c7413e872c 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -250,9 +250,9 @@ EventsManager::EventsManager(bool uploadCachedOnStart) auto containerId = utility::getEnvAs("OAKAGENT_CONTAINER_ID", ""); sourceAppId = appId == "" ? containerId : appId; sourceAppIdentifier = utility::getEnvAs("OAKAGENT_APP_IDENTIFIER", ""); - auto connectedDevices = DeviceBase::getAllConnectedDevices(); - if(!connectedDevices.empty()) { - sourceSerialNumber = connectedDevices[0].getDeviceId(); + auto connectedDevice = DeviceBase::getFirstAvailableDevice(); + if(std::get<0>(connectedDevice)) { + sourceSerialNumber = std::get<1>(connectedDevice).getDeviceId(); } else { sourceSerialNumber = ""; } @@ -435,17 +435,16 @@ void EventsManager::uploadFileBatch(std::deque> inputS // After retrying a defined number of times, we can determine the connection is not established, cache if enabled if(retryAttempt >= uploadRetryPolicy.maxAttempts) { for(size_t index = 0; index < inputSnapBatch.size(); ++index) { - if(inputSnapBatch.at(index)->eventData->callbacks.has_value()) { + if(inputSnapBatch.at(index)->eventData->onFailure.has_value() && inputSnapBatch.at(index)->eventData->onFailure.value() != nullptr) { auto event = inputSnapBatch.at(index)->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - inputSnapBatch.at(index)->eventData->callbacks.value().second( - SendSnapCallbackResult{event->name(), - event->created_at(), - inputSnapBatch.at(index)->eventData->localID, - std::nullopt, - serializedPayload, - SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); + inputSnapBatch.at(index)->eventData->onFailure.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + inputSnapBatch.at(index)->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED}); } } if(cacheIfCannotSend) { @@ -474,16 +473,16 @@ void EventsManager::uploadFileBatch(std::deque> inputS ->FindValueByNumber(static_cast(prepareGroupResult.rejected().reason())) ->name(); logger::error("A group has been rejected because of {}", rejectionReason); - if(snapData->eventData->callbacks.has_value()) { + if(snapData->eventData->onFailure.has_value() && snapData->eventData->onFailure.value() != nullptr) { auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second(SendSnapCallbackResult{event->name(), - event->created_at(), - snapData->eventData->localID, - std::nullopt, - serializedPayload, - SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + snapData->eventData->onFailure.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + snapData->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } continue; } @@ -526,16 +525,16 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: return uploadFile(std::move(fileData), std::move(uploadUrl)); })); } else { - if(snapData->eventData->callbacks.has_value()) { + if(snapData->eventData->onFailure.has_value() && snapData->eventData->onFailure.value() != nullptr) { auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second(SendSnapCallbackResult{event->name(), - event->created_at(), - snapData->eventData->localID, - std::nullopt, - serializedPayload, - SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); + snapData->eventData->onFailure.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + snapData->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::GROUP_CONTAINS_REJECTED_FILES}); } return false; } @@ -544,16 +543,16 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: for(auto& uploadResult : fileUploadResults) { if(!uploadResult.valid() || !uploadResult.get()) { logger::info("Failed to upload all of the files in the given group"); - if(snapData->eventData->callbacks.has_value()) { + if(snapData->eventData->onFailure.has_value() && snapData->eventData->onFailure.value() != nullptr) { auto event = snapData->eventData->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - snapData->eventData->callbacks.value().second(SendSnapCallbackResult{event->name(), - event->created_at(), - snapData->eventData->localID, - std::nullopt, - serializedPayload, - SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); + snapData->eventData->onFailure.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + snapData->eventData->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::FILE_UPLOAD_FAILED}); } return false; } @@ -643,27 +642,27 @@ void EventsManager::uploadEventBatch() { if(response.status_code != cpr::status::HTTP_OK) { logger::error("Failed to send event, status code: {}", response.status_code); // In case the eventBuffer gets too full (dropped connection), cache the events or drop them + std::lock_guard lock(eventBufferMutex); if(eventBuffer.size() >= EVENT_BUFFER_MAX_SIZE) { // failureCallback for(size_t index = 0; index < eventBuffer.size(); ++index) { - if(!eventBuffer.at(index)->callbacks.has_value()) { + if(!eventBuffer.at(index)->onFailure.has_value() || eventBuffer.at(index)->onFailure.value() == nullptr) { continue; } auto event = eventBuffer.at(index)->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - eventBuffer.at(index)->callbacks.value().second(SendSnapCallbackResult{event->name(), - event->created_at(), - eventBuffer.at(index)->localID, - std::nullopt, - serializedPayload, - SendSnapCallbackStatus::SEND_EVENT_FAILED}); + eventBuffer.at(index)->onFailure.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + eventBuffer.at(index)->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::SEND_EVENT_FAILED}); } if(cacheIfCannotSend) { cacheEvents(); } else { logger::warn("EventBuffer is full and caching is not enabled, dropping events"); - std::lock_guard lock(eventBufferMutex); eventBuffer.clear(); } } @@ -677,26 +676,25 @@ void EventsManager::uploadEventBatch() { std::lock_guard lock(eventBufferMutex); // successCallback for(int index = 0; index < eventBatch->events_size(); ++index) { - if(!eventBuffer.at(index)->callbacks.has_value()) { - continue; - } + bool hasSuccessCallback = eventBuffer.at(index)->onSuccess.has_value() && eventBuffer.at(index)->onSuccess.value() != nullptr; + bool hasFailureCallback = eventBuffer.at(index)->onFailure.has_value() && eventBuffer.at(index)->onFailure.value() != nullptr; auto event = eventBuffer.at(index)->event; std::string serializedPayload; event->SerializeToString(&serializedPayload); - if(eventBatchUploadResults->events(index).result_case() == proto::event::EventResult::kAccepted) { - eventBuffer.at(index)->callbacks.value().first(SendSnapCallbackResult{event->name(), - event->created_at(), - eventBuffer.at(index)->localID, - eventBatchUploadResults->events(index).accepted().id(), - serializedPayload, - SendSnapCallbackStatus::SUCCESS}); - } else { - eventBuffer.at(index)->callbacks.value().second(SendSnapCallbackResult{event->name(), - event->created_at(), - eventBuffer.at(index)->localID, - std::nullopt, - serializedPayload, - SendSnapCallbackStatus::EVENT_REJECTED}); + if(eventBatchUploadResults->events(index).result_case() == proto::event::EventResult::kAccepted && hasSuccessCallback) { + eventBuffer.at(index)->onSuccess.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + eventBuffer.at(index)->localID, + eventBatchUploadResults->events(index).accepted().id(), + serializedPayload, + SendSnapCallbackStatus::SUCCESS}); + } else if(eventBatchUploadResults->events(index).result_case() == proto::event::EventResult::kRejected && hasFailureCallback) { + eventBuffer.at(index)->onFailure.value()(SendSnapCallbackResult{event->name(), + event->created_at(), + eventBuffer.at(index)->localID, + std::nullopt, + serializedPayload, + SendSnapCallbackStatus::EVENT_REJECTED}); } } eventBuffer.erase(eventBuffer.begin(), eventBuffer.begin() + eventBatch->events_size()); @@ -761,7 +759,8 @@ bool EventsManager::sendSnap(const std::string& name, auto snapData = std::make_unique(); snapData->fileGroup = fileGroup; snapData->eventData = std::make_unique(); - snapData->eventData->callbacks.emplace(successCallback, failureCallback); + snapData->eventData->onSuccess = successCallback; + snapData->eventData->onFailure = failureCallback; // Create an event snapData->eventData->event = std::make_unique(); snapData->eventData->event->set_created_at(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); @@ -952,6 +951,7 @@ void EventsManager::uploadCachedData() { } else if(entry.path().filename().string().rfind("snap", 0) == 0) { std::ifstream snapFile(entry.path() / "snap.pb", std::ios::binary); auto snapData = std::make_unique(); + auto eventData = std::make_shared(); auto event = std::make_shared(); auto fileGroup = std::make_shared(); event->ParseFromIstream(&snapFile); @@ -961,6 +961,7 @@ void EventsManager::uploadCachedData() { fileGroup->fileData.push_back(fileData); } } + snapData->eventData = eventData; snapData->eventData->event = event; snapData->fileGroup = fileGroup; std::lock_guard lock(snapBufferMutex); From 0152c278534f350c65e7bd53815903db53e0a96b Mon Sep 17 00:00:00 2001 From: AljazD Date: Mon, 12 Jan 2026 08:53:48 +0100 Subject: [PATCH 024/180] Local SnapIDs are now returned to the called when using SendSnap() --- .../src/utility/EventsManagerBindings.cpp | 30 ++++---- examples/cpp/Events/events.cpp | 22 ++++-- examples/cpp/Events/events_file_group.cpp | 18 +++-- examples/python/Events/events.py | 22 +++--- examples/python/Events/events_file_group.py | 22 +++--- include/depthai/utility/EventsManager.hpp | 42 +++++------ src/utility/EventsManager.cpp | 73 ++++++++++--------- 7 files changed, 127 insertions(+), 102 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index 9a04f13cd8..14139a62e3 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -120,12 +120,13 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("associateFiles") = std::vector(), DOC(dai, utility, EventsManager, sendEvent)) .def("sendSnap", - static_cast, - const std::vector&, - const std::unordered_map&, - const std::function successCallback, - const std::function failureCallback)>(&EventsManager::sendSnap), + static_cast (EventsManager::*)(const std::string&, + const std::shared_ptr, + const std::vector&, + const std::unordered_map&, + const std::function successCallback, + const std::function failureCallback)>( + &EventsManager::sendSnap), py::arg("name"), py::arg("fileGroup") = std::shared_ptr(), py::arg("tags") = std::vector(), @@ -134,14 +135,15 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("failureCallback") = py::none(), DOC(dai, utility, EventsManager, sendSnap)) .def("sendSnap", - static_cast&, - const std::shared_ptr, - const std::optional>&, - const std::vector&, - const std::unordered_map&, - const std::function successCallback, - const std::function failureCallback)>(&EventsManager::sendSnap), + static_cast (EventsManager::*)(const std::string&, + const std::optional&, + const std::shared_ptr, + const std::optional>&, + const std::vector&, + const std::unordered_map&, + const std::function successCallback, + const std::function failureCallback)>( + &EventsManager::sendSnap), py::arg("name"), py::arg("fileName"), py::arg("imgFrame"), diff --git a/examples/cpp/Events/events.cpp b/examples/cpp/Events/events.cpp index 84fc5e507d..65b0bbc249 100644 --- a/examples/cpp/Events/events.cpp +++ b/examples/cpp/Events/events.cpp @@ -99,14 +99,20 @@ int main() { // Trigger sendSnap() if(cv::waitKey(1) == 's') { - eventsManager->sendSnap("ImageDetection", - std::nullopt, - inRgb, - inDet, - {"EventsExample", "C++"}, - {{"key_0", "value_0"}, {"key_1", "value_1"}}, - uploadSuccessCallback, - uploadFailureCallback); + auto localSnapID = eventsManager->sendSnap("ImageDetection", + std::nullopt, + inRgb, + inDet, + {"EventsExample", "C++"}, + {{"key_0", "value_0"}, {"key_1", "value_1"}}, + uploadSuccessCallback, + uploadFailureCallback); + + if(localSnapID.has_value()) { + std::cout << "Snap with a localID: " << localSnapID.value() << " has been successfully added to the EventsManager" << std::endl; + } else { + std::cout << "Snap was not successfully added to the EventsManager" << std::endl; + } } } diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index ab91886e29..55c4a3c2c0 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -114,12 +114,18 @@ int main() { auto fileGroup = std::make_shared(); fileGroup->addImageDetectionsPair(ss.str(), inRgb, borderDetections); - eventsManager->sendSnap("LowConfidenceDetection", - fileGroup, - {"EventsExample", "C++"}, - {{"key_0", "value_0"}, {"key_1", "value_1"}}, - uploadSuccessCallback, - uploadFailureCallback); + auto localSnapID = eventsManager->sendSnap("LowConfidenceDetection", + fileGroup, + {"EventsExample", "C++"}, + {{"key_0", "value_0"}, {"key_1", "value_1"}}, + uploadSuccessCallback, + uploadFailureCallback); + + if(localSnapID.has_value()) { + std::cout << "Snap with a localID: " << localSnapID.value() << " has been successfully added to the EventsManager" << std::endl; + } else { + std::cout << "Snap was not successfully added to the EventsManager" << std::endl; + } counter++; } diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index adfd776509..132fd98b46 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -7,15 +7,15 @@ # Callback functions -def upload_success_callback(send_snap_result): - assert (send_snap_result.uploadStatus == dai.SendSnapCallbackStatus.SUCCESS) - print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapHubID}) to the hub.") +def uploadSuccessCallback(sendSnapResult): + assert (sendSnapResult.uploadStatus == dai.SendSnapCallbackStatus.SUCCESS) + print(f"Successfully uploaded Snap: ({sendSnapResult.snapName}, {sendSnapResult.snapTimestamp}, {sendSnapResult.snapHubID}) to the hub.") -def upload_failure_callback(send_snap_result): - assert (send_snap_result.uploadStatus != dai.SendSnapCallbackStatus.SUCCESS) - print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapLocalID}) to the hub has failed.") +def uploadFailureCallback(sendSnapResult): + assert (sendSnapResult.uploadStatus != dai.SendSnapCallbackStatus.SUCCESS) + print(f"Upload of Snap: ({sendSnapResult.snapName}, {sendSnapResult.snapTimestamp}, {sendSnapResult.snapLocalID}) to the hub has failed.") - status = send_snap_result.uploadStatus + status = sendSnapResult.uploadStatus if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: print("File batch preparation failed!") elif status == dai.SendSnapCallbackStatus.GROUP_CONTAINS_REJECTED_FILES: @@ -89,5 +89,9 @@ def frameNorm(frame, bbox): # Trigger sendSnap() if cv2.waitKey(1) == ord("s"): - eventMan.sendSnap("ImageDetection", None, inRgb, inDet, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, - upload_success_callback, upload_failure_callback) \ No newline at end of file + localSnapID = eventMan.sendSnap("ImageDetection", None, inRgb, inDet, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, + uploadSuccessCallback, uploadFailureCallback) + if localSnapID != None: + print(f"Snap with a localID: {localSnapID} has been successfully added to the EventsManager") + else: + print("Snap was not successfully added to the EventsManager") \ No newline at end of file diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index 44e52ea2b2..da942f9757 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -6,15 +6,15 @@ # Callback functions -def upload_success_callback(send_snap_result): - assert (send_snap_result.uploadStatus == dai.SendSnapCallbackStatus.SUCCESS) - print(f"Successfully uploaded Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapHubID}) to the hub.") +def uploadSuccessCallback(sendSnapResult): + assert (sendSnapResult.uploadStatus == dai.SendSnapCallbackStatus.SUCCESS) + print(f"Successfully uploaded Snap: ({sendSnapResult.snapName}, {sendSnapResult.snapTimestamp}, {sendSnapResult.snapHubID}) to the hub.") -def upload_failure_callback(send_snap_result): - assert (send_snap_result.uploadStatus != dai.SendSnapCallbackStatus.SUCCESS) - print(f"Upload of Snap: ({send_snap_result.snapName}, {send_snap_result.snapTimestamp}, {send_snap_result.snapLocalID}) to the hub has failed.") +def uploadFailureCallback(sendSnapResult): + assert (sendSnapResult.uploadStatus != dai.SendSnapCallbackStatus.SUCCESS) + print(f"Upload of Snap: ({sendSnapResult.snapName}, {sendSnapResult.snapTimestamp}, {sendSnapResult.snapLocalID}) to the hub has failed.") - status = send_snap_result.uploadStatus + status = sendSnapResult.uploadStatus if status == dai.SendSnapCallbackStatus.FILE_BATCH_PREPARATION_FAILED: print("File batch preparation failed!") elif status == dai.SendSnapCallbackStatus.GROUP_CONTAINS_REJECTED_FILES: @@ -101,7 +101,11 @@ def frameNorm(frame, bbox): fileGroup = dai.FileGroup() fileGroup.addImageDetectionsPair(fileName, inRgb, borderDetections) - eventMan.sendSnap("LowConfidenceDetection", fileGroup, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, - upload_success_callback, upload_failure_callback) + localSnapID = eventMan.sendSnap("LowConfidenceDetection", fileGroup, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, + uploadSuccessCallback, uploadFailureCallback) + if localSnapID != None: + print(f"Snap with a localID: {localSnapID} has been successfully added to the EventsManager") + else: + print("Snap was not successfully added to the EventsManager") counter += 1 \ No newline at end of file diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index a371291b32..186cf6c054 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -99,12 +99,12 @@ class EventsManager { * @param tags List of tags to send * @param extras Extra data to send * @param associateFiles List of associate files with ids - * @return bool + * @return LocalID of the sent Event */ - bool sendEvent(const std::string& name, - const std::vector& tags = {}, - const std::unordered_map& extras = {}, - const std::vector& associateFiles = {}); + std::optional sendEvent(const std::string& name, + const std::vector& tags = {}, + const std::unordered_map& extras = {}, + const std::vector& associateFiles = {}); /** * Send a snap to the events service. Snaps should be used for sending images and other files. * @param name Name of the snap @@ -113,14 +113,14 @@ class EventsManager { * @param extras Extra data to send * @param successCallback Callback to be called when the snap is successfully uploaded to the hub * @param failureCallback Callback to be called if the snap upload is unsuccessful - * @return bool + * @return LocalID of the sent Snap */ - bool sendSnap(const std::string& name, - const std::shared_ptr fileGroup, - const std::vector& tags = {}, - const std::unordered_map& extras = {}, - const std::function successCallback = nullptr, - const std::function failureCallback = nullptr); + std::optional sendSnap(const std::string& name, + const std::shared_ptr fileGroup, + const std::vector& tags = {}, + const std::unordered_map& extras = {}, + const std::function successCallback = nullptr, + const std::function failureCallback = nullptr); /** * Send a snap to the events service, with an ImgFrame and ImgDetections pair as files * @param name Name of the snap @@ -131,16 +131,16 @@ class EventsManager { * @param extras Extra data to send * @param successCallback Callback to be called when the snap is successfully uploaded to the hub * @param failureCallback Callback to be called if the snap upload is unsuccessful - * @return bool + * @return LocalID of the sent Snap */ - bool sendSnap(const std::string& name, - const std::optional& fileName, - const std::shared_ptr imgFrame, - const std::optional>& imgDetections = std::nullopt, - const std::vector& tags = {}, - const std::unordered_map& extras = {}, - const std::function successCallback = nullptr, - const std::function failureCallback = nullptr); + std::optional sendSnap(const std::string& name, + const std::optional& fileName, + const std::shared_ptr imgFrame, + const std::optional>& imgDetections = std::nullopt, + const std::vector& tags = {}, + const std::unordered_map& extras = {}, + const std::function successCallback = nullptr, + const std::function failureCallback = nullptr); /** * Set the token for the events service. By default, the token is taken from the environment variable DEPTHAI_HUB_API_KEY * @param token Token for the events service diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index c7413e872c..c867919e04 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -250,12 +250,13 @@ EventsManager::EventsManager(bool uploadCachedOnStart) auto containerId = utility::getEnvAs("OAKAGENT_CONTAINER_ID", ""); sourceAppId = appId == "" ? containerId : appId; sourceAppIdentifier = utility::getEnvAs("OAKAGENT_APP_IDENTIFIER", ""); - auto connectedDevice = DeviceBase::getFirstAvailableDevice(); - if(std::get<0>(connectedDevice)) { - sourceSerialNumber = std::get<1>(connectedDevice).getDeviceId(); - } else { - sourceSerialNumber = ""; - } + // auto connectedDevice = DeviceBase::getFirstAvailableDevice(); + // if(std::get<0>(connectedDevice)) { + // sourceSerialNumber = std::get<1>(connectedDevice).getDeviceId(); + // } else { + // sourceSerialNumber = ""; + // } + sourceSerialNumber = ""; url = utility::getEnvAs("DEPTHAI_HUB_EVENTS_BASE_URL", "https://events.cloud.luxonis.com"); token = utility::getEnvAs("DEPTHAI_HUB_API_KEY", ""); // Thread handling preparation and uploads @@ -701,14 +702,14 @@ void EventsManager::uploadEventBatch() { } } -bool EventsManager::sendEvent(const std::string& name, - const std::vector& tags, - const std::unordered_map& extras, - const std::vector& associateFiles) { +std::optional EventsManager::sendEvent(const std::string& name, + const std::vector& tags, + const std::unordered_map& extras, + const std::vector& associateFiles) { // Check if the configuration and limits have already been fetched if(!configurationLimitsFetched) { logger::error("The configuration and limits have not been successfully fetched, event not sent"); - return false; + return std::nullopt; } // Create an event @@ -731,28 +732,29 @@ bool EventsManager::sendEvent(const std::string& name, } if(!validateEvent(*event)) { logger::error("Failed to send event, validation failed"); - return false; + return std::nullopt; } // Add event to eventBuffer std::lock_guard lock(eventBufferMutex); auto eventData = std::make_unique(); - eventData->localID = generateLocalID(event->created_at()); + std::string localID = generateLocalID(event->created_at()); + eventData->localID = localID; eventData->event = std::move(event); eventBuffer.push_back(std::move(eventData)); - return true; + return localID; } -bool EventsManager::sendSnap(const std::string& name, - const std::shared_ptr fileGroup, - const std::vector& tags, - const std::unordered_map& extras, - const std::function successCallback, - const std::function failureCallback) { +std::optional EventsManager::sendSnap(const std::string& name, + const std::shared_ptr fileGroup, + const std::vector& tags, + const std::unordered_map& extras, + const std::function successCallback, + const std::function failureCallback) { // Check if the configuration and limits have already been fetched if(!configurationLimitsFetched) { logger::error("The configuration and limits have not been successfully fetched, snap not sent"); - return false; + return std::nullopt; } // Prepare snapData @@ -764,7 +766,8 @@ bool EventsManager::sendSnap(const std::string& name, // Create an event snapData->eventData->event = std::make_unique(); snapData->eventData->event->set_created_at(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); - snapData->eventData->localID = generateLocalID(snapData->eventData->event->created_at()); + std::string localID = generateLocalID(snapData->eventData->event->created_at()); + snapData->eventData->localID = localID; snapData->eventData->event->set_name(name); snapData->eventData->event->add_tags("snap"); for(const auto& tag : tags) { @@ -779,14 +782,14 @@ bool EventsManager::sendSnap(const std::string& name, snapData->eventData->event->set_source_app_identifier(sourceAppIdentifier); if(!validateEvent(*snapData->eventData->event)) { logger::error("Failed to send snap, validation failed"); - return false; + return std::nullopt; } if(fileGroup->fileData.size() > maxFilesPerGroup) { logger::error("Failed to send snap, the number of files in a file group {} exceeds {}", fileGroup->fileData.size(), maxFilesPerGroup); - return false; + return std::nullopt; } else if(fileGroup->fileData.empty()) { logger::error("Failed to send snap, the file group is empty"); - return false; + return std::nullopt; } for(const auto& file : fileGroup->fileData) { if(file->size >= maxFileSizeBytes) { @@ -794,23 +797,23 @@ bool EventsManager::sendSnap(const std::string& name, "Failed to send snap, file: {} is bigger than the current maximum file size limit: {} kB. To increase your maximum file size, contact support.", file->fileName, maxFileSizeBytes / 1024); - return false; + return std::nullopt; } } // Add the snap to snapBuffer std::lock_guard lock(snapBufferMutex); snapBuffer.push_back(std::move(snapData)); - return true; + return localID; } -bool EventsManager::sendSnap(const std::string& name, - const std::optional& fileName, - const std::shared_ptr imgFrame, - const std::optional>& imgDetections, - const std::vector& tags, - const std::unordered_map& extras, - const std::function successCallback, - const std::function failureCallback) { +std::optional EventsManager::sendSnap(const std::string& name, + const std::optional& fileName, + const std::shared_ptr imgFrame, + const std::optional>& imgDetections, + const std::vector& tags, + const std::unordered_map& extras, + const std::function successCallback, + const std::function failureCallback) { // Create a FileGroup and send a snap containing it auto fileGroup = std::make_shared(); if(imgDetections.has_value()) { From a95c62fac1268f815440f11e4ddf97eb275043cd Mon Sep 17 00:00:00 2001 From: AljazD Date: Mon, 12 Jan 2026 13:15:56 +0100 Subject: [PATCH 025/180] Added a nullcheck before memcpy inside bspatch.c --- src/bspatch/bspatch.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/bspatch/bspatch.c b/src/bspatch/bspatch.c index 628a76ab83..63024870f9 100644 --- a/src/bspatch/bspatch.c +++ b/src/bspatch/bspatch.c @@ -213,8 +213,10 @@ int bspatch_mem(const uint8_t* oldfile_bin, const int64_t oldfile_size, const ui } return -1; } - memcpy(new + newpos, p_decompressed_block[EXTRA_BLOCK], ctrl[1]); - p_decompressed_block[EXTRA_BLOCK] += ctrl[1]; + if(ctrl[1] > 0 && p_decompressed_block[EXTRA_BLOCK] != NULL) { + memcpy(new + newpos, p_decompressed_block[EXTRA_BLOCK], ctrl[1]); + p_decompressed_block[EXTRA_BLOCK] += ctrl[1]; + } /* Adjust pointers */ newpos += ctrl[1]; From daf2bc7272ae027e827d1d8c324328241a063845 Mon Sep 17 00:00:00 2001 From: AljazD Date: Mon, 12 Jan 2026 13:39:19 +0100 Subject: [PATCH 026/180] Fixed integer overflow inside StreamMessageParser - readIntLE() --- src/pipeline/datatype/StreamMessageParser.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 439889143d..e958f8e406 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -67,8 +67,9 @@ namespace dai { static constexpr std::array endOfPacketMarker = {0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0}; // Reads int from little endian format -inline int readIntLE(uint8_t* data) { - return data[0] + data[1] * 256 + data[2] * 256 * 256 + data[3] * 256 * 256 * 256; +inline int readIntLE(const uint8_t* data) { + return static_cast(static_cast(data[0]) | (static_cast(data[1]) << 8) | (static_cast(data[2]) << 16) + | (static_cast(data[3]) << 24)); } template From d69076d1880a06a2dbd00b63eb92fec4359b5859 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Tue, 13 Jan 2026 23:07:13 +0100 Subject: [PATCH 027/180] Add units scaling in calibrationHandler --- .../python/src/CalibrationHandlerBindings.cpp | 8 +++- include/depthai/common/DepthUnit.hpp | 3 ++ include/depthai/device/CalibrationHandler.hpp | 37 +++++++++++---- src/device/CalibrationHandler.cpp | 47 ++++++++++++++----- .../onhost_tests/calibration_handler_test.cpp | 27 ++++++++++- 5 files changed, 99 insertions(+), 23 deletions(-) diff --git a/bindings/python/src/CalibrationHandlerBindings.cpp b/bindings/python/src/CalibrationHandlerBindings.cpp index eeeb52c0f0..8f92467237 100644 --- a/bindings/python/src/CalibrationHandlerBindings.cpp +++ b/bindings/python/src/CalibrationHandlerBindings.cpp @@ -88,6 +88,7 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("srcCamera"), py::arg("dstCamera"), py::arg("useSpecTranslation") = false, + py::arg("unit") = MeasurementUnit::CENTIMETER, DOC(dai, CalibrationHandler, getCameraExtrinsics)) .def("getHousingCalibration", @@ -95,6 +96,7 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("srcCamera"), py::arg("housingCS"), py::arg("useSpecTranslation") = true, + py::arg("unit") = MeasurementUnit::CENTIMETER, DOC(dai, CalibrationHandler, getHousingCalibration)) .def("getCameraTranslationVector", @@ -102,6 +104,7 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("srcCamera"), py::arg("dstCamera"), py::arg("useSpecTranslation") = true, + py::arg("unit") = MeasurementUnit::CENTIMETER, DOC(dai, CalibrationHandler, getCameraTranslationVector)) .def("getCameraRotationMatrix", &CalibrationHandler::getCameraRotationMatrix, @@ -113,17 +116,20 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("cam1") = dai::CameraBoardSocket::CAM_C, py::arg("cam2") = dai::CameraBoardSocket::CAM_B, py::arg("useSpecTranslation") = true, + py::arg("unit") = MeasurementUnit::CENTIMETER, DOC(dai, CalibrationHandler, getBaselineDistance)) .def("getCameraToImuExtrinsics", &CalibrationHandler::getCameraToImuExtrinsics, py::arg("cameraId"), py::arg("useSpecTranslation") = false, + py::arg("unit") = MeasurementUnit::CENTIMETER, DOC(dai, CalibrationHandler, getCameraToImuExtrinsics)) .def("getImuToCameraExtrinsics", &CalibrationHandler::getImuToCameraExtrinsics, py::arg("cameraId"), py::arg("useSpecTranslation") = false, + py::arg("unit") = MeasurementUnit::CENTIMETER, DOC(dai, CalibrationHandler, getImuToCameraExtrinsics)) .def("getStereoRightRectificationRotation", @@ -235,4 +241,4 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { &CalibrationHandler::validateCalibrationHandler, py::arg("throwOnError") = true, DOC(dai, CalibrationHandler, validateCalibrationHandler)); -} \ No newline at end of file +} diff --git a/include/depthai/common/DepthUnit.hpp b/include/depthai/common/DepthUnit.hpp index fcf5b1def3..3312512786 100644 --- a/include/depthai/common/DepthUnit.hpp +++ b/include/depthai/common/DepthUnit.hpp @@ -9,6 +9,9 @@ namespace dai { */ enum class DepthUnit : int32_t { METER, CENTIMETER, MILLIMETER, INCH, FOOT, CUSTOM }; +// Alias for generic measurement units used in calibration APIs. +using MeasurementUnit = DepthUnit; + constexpr float getDepthUnitMultiplier(DepthUnit unit) { switch(unit) { case DepthUnit::METER: diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index 441b67d82a..fefc28fd4b 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -6,6 +6,7 @@ #include #include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/DepthUnit.hpp" #include "depthai/common/EepromData.hpp" #include "depthai/common/HousingCoordinateSystem.hpp" #include "depthai/common/Point2f.hpp" @@ -217,11 +218,12 @@ class CalibrationHandler { /** * Get the Camera Extrinsics object between two cameras from the calibration data if there is a linked connection - * between any two cameras then the relative rotation and translation (in centimeters) is returned by this function. + * between any two cameras then the relative rotation and translation is returned by this function. * * @param srcCamera Camera Id of the camera which will be considered as origin. * @param dstCamera Camera Id of the destination camera to which we are fetching the rotation and translation from the SrcCamera * @param useSpecTranslation Enabling this bool uses the translation information from the board design data + * @param unit Units of the returned translation (default: centimeters) * @return a transformationMatrix which is 4x4 in homogeneous coordinate system * * Matrix representation of transformation matrix @@ -233,7 +235,10 @@ class CalibrationHandler { * \end{matrix} \right ] \f] * */ - std::vector> getCameraExtrinsics(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation = false) const; + std::vector> getCameraExtrinsics(CameraBoardSocket srcCamera, + CameraBoardSocket dstCamera, + bool useSpecTranslation = false, + MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; /** * Get the transformation matrix between a camera and a chosen housing @@ -250,6 +255,7 @@ class CalibrationHandler { * transformation is requested (e.g. VESA_RIGHT, FRONT_COVER_LEFT, etc.). * @param useSpecTranslation If true, uses board-design (spec) translation values. * If false, uses calibrated translation values. + * @param unit Units of the returned translation (default: centimeters) * * @return A 4x4 homogeneous transformation matrix. * @@ -268,7 +274,8 @@ class CalibrationHandler { */ std::vector> getHousingCalibration(CameraBoardSocket srcCamera, const HousingCoordinateSystem housingCS, - bool useSpecTranslation = false) const; + bool useSpecTranslation = false, + MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; /** * Get the Camera translation vector between two cameras from the calibration data. @@ -276,9 +283,13 @@ class CalibrationHandler { * @param srcCamera Camera Id of the camera which will be considered as origin. * @param dstCamera Camera Id of the destination camera to which we are fetching the translation vector from the SrcCamera * @param useSpecTranslation Disabling this bool uses the translation information from the calibration data (not the board design data) - * @return a translation vector like [x, y, z] in centimeters + * @param unit Units of the returned translation (default: centimeters) + * @return a translation vector like [x, y, z] */ - std::vector getCameraTranslationVector(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation = true) const; + std::vector getCameraTranslationVector(CameraBoardSocket srcCamera, + CameraBoardSocket dstCamera, + bool useSpecTranslation = true, + MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; /** * Get the Camera rotation matrix between two cameras from the calibration data. @@ -302,11 +313,13 @@ class CalibrationHandler { * @param cam1 First camera * @param cam2 Second camera * @param useSpecTranslation Enabling this bool uses the translation information from the board design data (not the calibration data) - * @return baseline distance in centimeters + * @param unit Units of the returned baseline distance (default: centimeters) + * @return baseline distance */ float getBaselineDistance(CameraBoardSocket cam1 = CameraBoardSocket::CAM_C, CameraBoardSocket cam2 = CameraBoardSocket::CAM_B, - bool useSpecTranslation = true) const; + bool useSpecTranslation = true, + MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; /** * Get the Camera To Imu Extrinsics object @@ -315,6 +328,7 @@ class CalibrationHandler { * * @param cameraId Camera Id of the camera which will be considered as origin. from which Transformation matrix to the IMU will be found * @param useSpecTranslation Enabling this bool uses the translation information from the board design data + * @param unit Units of the returned translation (default: centimeters) * @return Returns a transformationMatrix which is 4x4 in homogeneous coordinate system * * Matrix representation of transformation matrix @@ -326,7 +340,9 @@ class CalibrationHandler { * \end{matrix} \right ] \f] * */ - std::vector> getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation = false) const; + std::vector> getCameraToImuExtrinsics(CameraBoardSocket cameraId, + bool useSpecTranslation = false, + MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; /** * Get the Imu To Camera Extrinsics object from the data loaded if there is a linked connection @@ -335,6 +351,7 @@ class CalibrationHandler { * * @param cameraId Camera Id of the camera which will be considered as destination. To which Transformation matrix from the IMU will be found. * @param useSpecTranslation Enabling this bool uses the translation information from the board design data + * @param unit Units of the returned translation (default: centimeters) * @return Returns a transformationMatrix which is 4x4 in homogeneous coordinate system * * Matrix representation of transformation matrix @@ -346,7 +363,9 @@ class CalibrationHandler { * \end{matrix} \right ] \f] * */ - std::vector> getImuToCameraExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation = false) const; + std::vector> getImuToCameraExtrinsics(CameraBoardSocket cameraId, + bool useSpecTranslation = false, + MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; /** * diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index f19e4ba647..2c79cdb803 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -49,6 +49,19 @@ void invertSe3Matrix4x4InPlace(std::vector>& mat) { } for(int i = 0; i < 3; ++i) mat[i][3] = newTrans[i]; } + +float getUnitScale(MeasurementUnit unit) { + // Extrinsics are stored in centimeters; DepthUnit multiplier converts meters to target. + return getDepthUnitMultiplier(unit) / 100.0f; +} + +void scaleTranslationInPlace(std::vector>& mat, MeasurementUnit unit) { + const float scale = getUnitScale(unit); + if(scale == 1.0f) return; + for(int i = 0; i < 3; ++i) { + mat[i][3] *= scale; + } +} } // namespace CalibrationHandler::ExtrinsicGraphValidationResult CalibrationHandler::validateExtrinsicGraph() const { @@ -445,7 +458,8 @@ CameraModel CalibrationHandler::getDistortionModel(CameraBoardSocket cameraId) c std::vector> CalibrationHandler::getCameraExtrinsics(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, - bool useSpecTranslation) const { + bool useSpecTranslation, + MeasurementUnit unit) const { /** * 1. Check if both camera ID exists. * 2. Check if the forward link exists from source and destination camera to origin camera. @@ -478,6 +492,7 @@ std::vector> CalibrationHandler::getCameraExtrinsics(CameraBo // Get the matrix from src to dst camera extrinsics = matMul(dstOriginMatrix, srcOriginMatrix); + scaleTranslationInPlace(extrinsics, unit); return extrinsics; } @@ -621,7 +636,8 @@ std::vector> CalibrationHandler::getHousingToHousingOrigin(co std::vector> CalibrationHandler::getHousingCalibration(CameraBoardSocket srcCamera, const HousingCoordinateSystem housingCS, - bool useSpecTranslation) const { + bool useSpecTranslation, + MeasurementUnit unit) const { // Ensure we have calibration data for the requested source camera if(eepromData.cameraData.find(srcCamera) == eepromData.cameraData.end()) { throw std::runtime_error("There is no Camera data available corresponding to the requested source cameraId"); @@ -676,12 +692,16 @@ std::vector> CalibrationHandler::getHousingCalibration(Camera // Which gives us: cam_src → housing // ------------------------------------------------------------ camToHousing = matMul(HousingToHousingOrigin, camToHousingOrigin); + scaleTranslationInPlace(camToHousing, unit); return camToHousing; } -std::vector CalibrationHandler::getCameraTranslationVector(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation) const { - std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, useSpecTranslation); +std::vector CalibrationHandler::getCameraTranslationVector(CameraBoardSocket srcCamera, + CameraBoardSocket dstCamera, + bool useSpecTranslation, + MeasurementUnit unit) const { + std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, useSpecTranslation, unit); std::vector translationVector = {0, 0, 0}; for(auto i = 0; i < 3; i++) { @@ -691,7 +711,7 @@ std::vector CalibrationHandler::getCameraTranslationVector(CameraBoardSoc } std::vector> CalibrationHandler::getCameraRotationMatrix(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera) const { - std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, false); + std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, false, MeasurementUnit::CENTIMETER); std::vector> rotationMatrix = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; for(auto i = 0; i < 3; i++) { @@ -702,8 +722,8 @@ std::vector> CalibrationHandler::getCameraRotationMatrix(Came return rotationMatrix; } -float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoardSocket cam2, bool useSpecTranslation) const { - std::vector translationVector = getCameraTranslationVector(cam1, cam2, useSpecTranslation); +float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoardSocket cam2, bool useSpecTranslation, MeasurementUnit unit) const { + std::vector translationVector = getCameraTranslationVector(cam1, cam2, useSpecTranslation, unit); float sum = 0; for(auto val : translationVector) { sum += val * val; @@ -711,13 +731,13 @@ float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoar return std::sqrt(sum); } -std::vector> CalibrationHandler::getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation) const { - std::vector> transformationMatrix = getImuToCameraExtrinsics(cameraId, useSpecTranslation); +std::vector> CalibrationHandler::getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation, MeasurementUnit unit) const { + std::vector> transformationMatrix = getImuToCameraExtrinsics(cameraId, useSpecTranslation, unit); invertSe3Matrix4x4InPlace(transformationMatrix); return transformationMatrix; } -std::vector> CalibrationHandler::getImuToCameraExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation) const { +std::vector> CalibrationHandler::getImuToCameraExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation, MeasurementUnit unit) const { if(eepromData.imuExtrinsics.rotationMatrix.size() == 0 || eepromData.imuExtrinsics.toCameraSocket == CameraBoardSocket::AUTO) { throw std::runtime_error("IMU calibration data is not available on device yet."); } else if(eepromData.cameraData.find(cameraId) == eepromData.cameraData.end()) { @@ -738,11 +758,14 @@ std::vector> CalibrationHandler::getImuToCameraExtrinsics(Cam currTransformationMatrixImu.push_back(homogeneous_vector); if(eepromData.imuExtrinsics.toCameraSocket == cameraId) { + scaleTranslationInPlace(currTransformationMatrixImu, unit); return currTransformationMatrixImu; } else { std::vector> destTransformationMatrixCurr = - getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation); - return matMul(destTransformationMatrixCurr, currTransformationMatrixImu); + getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation, MeasurementUnit::CENTIMETER); + auto result = matMul(destTransformationMatrixCurr, currTransformationMatrixImu); + scaleTranslationInPlace(result, unit); + return result; } } diff --git a/tests/src/onhost_tests/calibration_handler_test.cpp b/tests/src/onhost_tests/calibration_handler_test.cpp index 648d63854f..c230b9db76 100644 --- a/tests/src/onhost_tests/calibration_handler_test.cpp +++ b/tests/src/onhost_tests/calibration_handler_test.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include @@ -418,6 +419,30 @@ TEST_CASE("Valid extrinsics for directly linked cameras", "[getCameraExtrinsics] REQUIRE(M == I); } +TEST_CASE("Extrinsics translation scales with measurement unit", "[getCameraExtrinsics][getCameraTranslationVector][getBaselineDistance]") { + auto handler = loadValidHandler(); + + std::vector> R = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + std::vector tABcm = {100.0f, -50.0f, 25.0f}; + std::vector zeros = {0.0f, 0.0f, 0.0f}; + + handler.setCameraExtrinsics(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, R, tABcm, zeros); + + auto Mmeters = handler.getCameraExtrinsics(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, MeasurementUnit::METER); + REQUIRE(Mmeters[0][3] == Catch::Approx(1.0f).margin(1e-6)); + REQUIRE(Mmeters[1][3] == Catch::Approx(-0.5f).margin(1e-6)); + REQUIRE(Mmeters[2][3] == Catch::Approx(0.25f).margin(1e-6)); + + auto tMillimeters = handler.getCameraTranslationVector(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, MeasurementUnit::MILLIMETER); + REQUIRE(tMillimeters[0] == Catch::Approx(1000.0f).margin(1e-6)); + REQUIRE(tMillimeters[1] == Catch::Approx(-500.0f).margin(1e-6)); + REQUIRE(tMillimeters[2] == Catch::Approx(250.0f).margin(1e-6)); + + float baselineMeters = handler.getBaselineDistance(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, MeasurementUnit::METER); + float expectedMeters = std::sqrt(1.0f * 1.0f + 0.5f * 0.5f + 0.25f * 0.25f); + REQUIRE(baselineMeters == Catch::Approx(expectedMeters).margin(1e-6)); +} + TEST_CASE("Same-origin cameras return identity", "[getCameraExtrinsics]") { auto handler = loadInvalidHandler(); // A→A should also be identity @@ -1078,4 +1103,4 @@ TEST_CASE("getHousingCalibration - Disconnected origin cameras throw", "[getHous // Try to get housing calibration from CAM_A (origin 0) when housing points to origin 1 REQUIRE_THROWS_WITH(handler.getHousingCalibration(CameraBoardSocket::CAM_A, dai::HousingCoordinateSystem::FRONT_CAM_A, true), Catch::Matchers::ContainsSubstring("Missing extrinsic link from source camera to destination camera")); -} \ No newline at end of file +} From f88842e5ebfdd7f70bf4776b1028bc874f970b6f Mon Sep 17 00:00:00 2001 From: AljazD Date: Wed, 14 Jan 2026 09:38:32 +0100 Subject: [PATCH 028/180] Updated Xlink commit, schemaNameToDatatype() now uses hardcoded strings for schema names --- cmake/depthaiDependencies.cmake | 2 +- src/utility/ProtoSerialize.cpp | 14 +++++++------- tests/src/onhost_tests/replay_test.cpp | 5 +++++ 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index f608293730..7e94b24c00 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -177,7 +177,7 @@ else() FetchContent_Declare( XLink GIT_REPOSITORY https://github.com/luxonis/XLink.git - GIT_TAG c19f995d8284f1f3ceb5d47a8acfb4a5b4a866ff + GIT_TAG 7e908706c96b7a1541d9f62b519e9bd22061ebb5 ) FetchContent_MakeAvailable( diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index bce13b9226..5d984abc3f 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -115,19 +115,19 @@ ImgTransformation deserializeImgTransformation(const proto::common::ImgTransform } DatatypeEnum schemaNameToDatatype(const std::string& schemaName) { - if(schemaName == proto::encoded_frame::EncodedFrame::descriptor()->full_name()) { + if(schemaName == "dai.proto.encoded_frame.EncodedFrame") { return DatatypeEnum::EncodedFrame; - } else if(schemaName == proto::imu_data::IMUData::descriptor()->full_name()) { + } else if(schemaName == "dai.proto.imu_data.IMUData") { return DatatypeEnum::IMUData; - } else if(schemaName == proto::image_annotations::ImageAnnotations::descriptor()->full_name()) { + } else if(schemaName == "dai.proto.image_annotations.ImageAnnotations") { return DatatypeEnum::ImgAnnotations; - } else if(schemaName == proto::img_detections::ImgDetections::descriptor()->full_name()) { + } else if(schemaName == "dai.proto.img_detections.ImgDetections") { return DatatypeEnum::ImgDetections; - } else if(schemaName == proto::img_frame::ImgFrame::descriptor()->full_name()) { + } else if(schemaName == "dai.proto.img_frame.ImgFrame") { return DatatypeEnum::ImgFrame; - } else if(schemaName == proto::point_cloud_data::PointCloudData::descriptor()->full_name()) { + } else if(schemaName == "dai.proto.point_cloud_data.PointCloudData") { return DatatypeEnum::PointCloudData; - } else if(schemaName == proto::spatial_img_detections::SpatialImgDetections::descriptor()->full_name()) { + } else if(schemaName == "dai.proto.spatial_img_detections.SpatialImgDetections") { return DatatypeEnum::SpatialImgDetections; } else { throw std::runtime_error("Unknown schema name: " + schemaName); diff --git a/tests/src/onhost_tests/replay_test.cpp b/tests/src/onhost_tests/replay_test.cpp index 2259f4060f..fe6e6e3eef 100644 --- a/tests/src/onhost_tests/replay_test.cpp +++ b/tests/src/onhost_tests/replay_test.cpp @@ -14,6 +14,11 @@ constexpr unsigned int NUM_MSGS = 100; +// Disable container overflow detection for this test binary +extern "C" const char* __asan_default_options() { + return "detect_container_overflow=0"; +} + class TestHelper { public: TestHelper() { From cd7e821cbee89948bef1a848bc73fd072085f871 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 14 Jan 2026 10:05:34 +0100 Subject: [PATCH 029/180] Internal method uses as well this method --- src/pipeline/node/DynamicCalibrationNode.cpp | 4 ++-- src/rtabmap/CalibrationHandler.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pipeline/node/DynamicCalibrationNode.cpp b/src/pipeline/node/DynamicCalibrationNode.cpp index d7040572c0..633b40f64d 100644 --- a/src/pipeline/node/DynamicCalibrationNode.cpp +++ b/src/pipeline/node/DynamicCalibrationNode.cpp @@ -72,7 +72,7 @@ std::pair, std::shared_ptr DclUtils::createDclCalibration(con } for(int i = 0; i < 3; ++i) { - tvec[i] = static_cast(translationVector[i] / 100.0f); // Convert to m + tvec[i] = static_cast(translationVector[i]); } dcl::distortion_t dclDistortion; diff --git a/src/rtabmap/CalibrationHandler.cpp b/src/rtabmap/CalibrationHandler.cpp index 295694dec6..d8b8ab813c 100644 --- a/src/rtabmap/CalibrationHandler.cpp +++ b/src/rtabmap/CalibrationHandler.cpp @@ -34,7 +34,7 @@ rtabmap::StereoCameraModel CalibrationHandler::getRTABMapCameraModel(CameraBoard double fy = newCameraMatrix.at(1, 1); double cx = newCameraMatrix.at(0, 2); double cy = newCameraMatrix.at(1, 2); - double baseline = double(getBaselineDistance(right, left)) / 100.0; + double baseline = double(getBaselineDistance(right, left, true, MeasurementUnit::METER)); return rtabmap::StereoCameraModel(eepromData.boardName, fx, fy, cx, cy, baseline, localTransform, cv::Size(width, height)); } From 033a30b996dd11265926eac33fd94a0b9fc4a76b Mon Sep 17 00:00:00 2001 From: AljazD Date: Wed, 14 Jan 2026 12:17:51 +0100 Subject: [PATCH 030/180] Temporarily disabled tsan tests --- tests/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/Dockerfile b/tests/Dockerfile index 2e9dac27a2..fb3ba4af83 100644 --- a/tests/Dockerfile +++ b/tests/Dockerfile @@ -104,7 +104,7 @@ RUN --mount=type=cache,target=/opt/vcpkg-downloads \ RUN --mount=type=cache,target=/opt/vcpkg-downloads \ --mount=type=cache,target=/opt/vcpkg-bincache \ - if [ "$FLAVOR" != "vanilla" ]; then \ + if [ "$FLAVOR" != "vanilla" ] && [ "$FLAVOR" != "tsan" ]; then \ export CMAKE_TOOLCHAIN_FILE=/workspace/cmake/toolchain/${FLAVOR}.cmake && \ echo "Using toolchain: $CMAKE_TOOLCHAIN_FILE"; \ else \ From bc30fe405c5f8d762b1bf4531d4b56f674d9922b Mon Sep 17 00:00:00 2001 From: AljazD Date: Wed, 14 Jan 2026 13:54:07 +0100 Subject: [PATCH 031/180] Bump XLink commit --- cmake/depthaiDependencies.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index 7e94b24c00..0e8d7f0fe5 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -177,7 +177,7 @@ else() FetchContent_Declare( XLink GIT_REPOSITORY https://github.com/luxonis/XLink.git - GIT_TAG 7e908706c96b7a1541d9f62b519e9bd22061ebb5 + GIT_TAG 0958262cc5a45b25296ce87f552a7701d1fb8fb5 ) FetchContent_MakeAvailable( From 64c25bc0c76557cfed8102e85f7e7e4fac6f44ae Mon Sep 17 00:00:00 2001 From: AljazD Date: Wed, 14 Jan 2026 13:57:07 +0100 Subject: [PATCH 032/180] Bump FW --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 021f70eeaf..0356939a68 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+ff6848f7330bd09c1d3dbd53912b2b00690e4192") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+b6dd91b48a055a05f92a1976367564769a71f7f3") From 725cb85290fb87feed70e8c4d240380c7a74b2e8 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 15 Jan 2026 10:23:05 +0100 Subject: [PATCH 033/180] Added suppression for external libGLX_mesa.so, libstdc++.so will now be loaded in the LD_PRELOAD --- bindings/python/CMakeLists.txt | 7 ++++++- tests/lsan.supp | 1 + tests/run_tests.py | 10 ++++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) create mode 100644 tests/lsan.supp diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index e46a50b3d0..2076caf6b4 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -388,12 +388,17 @@ if(SANITIZE_ADDRESS OR SANITIZE_MEMORY OR SANITIZE_THREAD OR SANITIZE_UNDEFINED) execute_process(COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libclang_rt.asan-${CMAKE_HOST_SYSTEM_PROCESSOR}.so OUTPUT_VARIABLE LIBASAN_PATH OUTPUT_STRIP_TRAILING_WHITESPACE) elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") execute_process(COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libasan.so OUTPUT_VARIABLE LIBASAN_PATH OUTPUT_STRIP_TRAILING_WHITESPACE) + execute_process(COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so OUTPUT_VARIABLE LIBSTDCXX_PATH OUTPUT_STRIP_TRAILING_WHITESPACE) endif() # Set preload env variable if(APPLE) set(ASAN_ENVIRONMENT_VARS "DYLD_INSERT_LIBRARIES=${LIBASAN_PATH}" "ASAN_OPTIONS=leak_check_at_exit=0") elseif(UNIX) - set(ASAN_ENVIRONMENT_VARS "LD_PRELOAD=${LIBASAN_PATH}" "ASAN_OPTIONS=leak_check_at_exit=0") + if(LIBSTDCXX_PATH) + set(ASAN_ENVIRONMENT_VARS "LD_PRELOAD=${LIBASAN_PATH}:${LIBSTDCXX_PATH}" "ASAN_OPTIONS=leak_check_at_exit=0") + else() + set(ASAN_ENVIRONMENT_VARS "LD_PRELOAD=${LIBASAN_PATH}" "ASAN_OPTIONS=leak_check_at_exit=0") + endif() endif() message(STATUS "ASAN environment variables: ${ASAN_ENVIRONMENT_VARS}") endif() diff --git a/tests/lsan.supp b/tests/lsan.supp new file mode 100644 index 0000000000..aae644eef5 --- /dev/null +++ b/tests/lsan.supp @@ -0,0 +1 @@ +leak:libGLX_mesa.so diff --git a/tests/run_tests.py b/tests/run_tests.py index dbb1d00902..39eb76b37b 100644 --- a/tests/run_tests.py +++ b/tests/run_tests.py @@ -46,6 +46,16 @@ def run(self): def run_ctest(env_vars, labels, blocking=True, name=""): env = os.environ.copy() env_vars["DEPTHAI_PIPELINE_DEBUGGING"] = "1" + + # Add LSAN suppressions if the file exists + lsan_supp_path = pathlib.Path(__file__).parent / "lsan.supp" + if lsan_supp_path.exists(): + lsan_supp_path = lsan_supp_path.resolve() + if "LSAN_OPTIONS" in env: + env["LSAN_OPTIONS"] += f":suppressions={lsan_supp_path}" + else: + env["LSAN_OPTIONS"] = f"suppressions={lsan_supp_path}" + env.update(env_vars) cmd = ["ctest", "--no-tests=error", "-VV", "-L", "^ci$", "--timeout", "1000", "-C", "Release"] From 3b69eaa574a119085d47cd190a951dfa37c76e5c Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 15 Jan 2026 11:26:58 +0100 Subject: [PATCH 034/180] Updated supp file, minor fix in SpatialLocationCalculator example --- .../SpatialLocationCalculator/spatial_location_calculator.cpp | 2 +- tests/lsan.supp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/cpp/SpatialLocationCalculator/spatial_location_calculator.cpp b/examples/cpp/SpatialLocationCalculator/spatial_location_calculator.cpp index b3d9a9f1c9..b20197e475 100644 --- a/examples/cpp/SpatialLocationCalculator/spatial_location_calculator.cpp +++ b/examples/cpp/SpatialLocationCalculator/spatial_location_calculator.cpp @@ -65,7 +65,7 @@ int main() { std::vector depthValues; for(int i = 0; i < frameDepth.rows; i++) { for(int j = 0; j < frameDepth.cols; j++) { - float val = frameDepth.at(i, j); + uint16_t val = frameDepth.at(i, j); if(val > 0) depthValues.push_back(val); } } diff --git a/tests/lsan.supp b/tests/lsan.supp index aae644eef5..65aa706646 100644 --- a/tests/lsan.supp +++ b/tests/lsan.supp @@ -1 +1,3 @@ leak:libGLX_mesa.so +leak:g_malloc +leak:g_realloc From 1d69cbc6c26a312b4c9fb134e6cef4a060307228 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 15 Jan 2026 13:45:41 +0100 Subject: [PATCH 035/180] Bump FW --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 0356939a68..82ca683770 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+b6dd91b48a055a05f92a1976367564769a71f7f3") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+b6e66f044d34615b90fe754ae92904abbc3c612c") From 307b779ac25378df98b7f1e7ca6601342d186645 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 15 Jan 2026 17:43:20 +0100 Subject: [PATCH 036/180] Update tests & RVC2 Impl --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/common/ImgTransformations.hpp | 6 +++ src/pipeline/datatype/ImgTransformations.cpp | 23 ++++++++++++ src/pipeline/node/ImageAlign.cpp | 12 +++++- .../ondevice_tests/image_align_node_test.cpp | 37 ++++++++++++++----- 5 files changed, 69 insertions(+), 11 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index ce131da23b..7c5e3601ec 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "a406121b872f3502d3fc666e21441edf91707554") +set(DEPTHAI_DEVICE_SIDE_COMMIT "ecdc7175a8932be3fd2c64be3f30a75afc616459") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/common/ImgTransformations.hpp b/include/depthai/common/ImgTransformations.hpp index 62ba8bc27d..ffa3befa6f 100644 --- a/include/depthai/common/ImgTransformations.hpp +++ b/include/depthai/common/ImgTransformations.hpp @@ -240,6 +240,12 @@ struct ImgTransformation { */ dai::RotatedRect remapRectFrom(const ImgTransformation& from, dai::RotatedRect rect) const; + /** + * Check if the transformations are aligned + * @param to Transformation to compare with + */ + bool isAlignedTo(const ImgTransformation& to) const; + /** * Check if the transformations are valid. The transformations are valid if the source frame size and the current frame size are set. */ diff --git a/src/pipeline/datatype/ImgTransformations.cpp b/src/pipeline/datatype/ImgTransformations.cpp index 7a78c90f84..69e9fcaeb4 100644 --- a/src/pipeline/datatype/ImgTransformations.cpp +++ b/src/pipeline/datatype/ImgTransformations.cpp @@ -459,4 +459,27 @@ dai::RotatedRect ImgTransformation::remapRectFrom(const ImgTransformation& from, return transformed; } +bool ImgTransformation::isAlignedTo(const ImgTransformation& to) const { + if(width != to.width || height != to.height) return false; + if(this->distortionModel != to.distortionModel) return false; + auto approxEqual = [](float a, float b, float absTol = ROUND_UP_EPS, float relTol = 2 * ROUND_UP_EPS) { + return std::abs(a - b) <= (absTol + relTol * std::max(std::abs(a), std::abs(b))); + }; + + for(size_t i = 0; i < distortionCoefficients.size(); ++i) { + if(!approxEqual(distortionCoefficients[i], to.distortionCoefficients[i])) { + return false; + } + } + + std::array, 3> thisIntrinsic = this->getIntrinsicMatrix(); + std::array, 3> toIntrinsic = to.getIntrinsicMatrix(); + for(int i = 0; i < 3; ++i) { + for(int j = 0; j < 3; ++j) { + if(!approxEqual(toIntrinsic[i][j], thisIntrinsic[i][j])) return false; + } + } + return true; +} + }; // namespace dai diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index 55a36ee6e8..78295def47 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -1,5 +1,7 @@ #include "depthai/pipeline/node/ImageAlign.hpp" +#include +#include #include #include @@ -370,6 +372,13 @@ void ImageAlign::run() { inputAlignToImgFrame = *inputAlignToImg; inputAlignToTransform = inputAlignToImg->transformation; + const auto alignToDistortion = inputAlignToTransform.getDistortionCoefficients(); + const bool hasDistortion = std::any_of(alignToDistortion.begin(), alignToDistortion.end(), [](float value) { return std::abs(value) > 0.0f; }); + if(hasDistortion) { + logger->warn( + "The input connected to inputAlignTo is distorted. The aligned image will still be undistorted, meaning it won't be perfectly " + "aligned."); + } alignSourceIntrinsics = inputAlignToImg->transformation.getIntrinsicMatrix(); @@ -575,13 +584,14 @@ void ImageAlign::run() { alignedImg->setSequenceNum(inputImg->getSequenceNum()); alignedImg->transformation = inputAlignToTransform; + const auto alignToDistortion = inputAlignToTransform.getDistortionCoefficients(); + alignedImg->transformation.setDistortionCoefficients(std::vector(alignToDistortion.size(), 0.0f)); tStop = steady_clock::now(); auto runtime = duration_cast(tStop - tStart).count(); logger->trace("ImageAlign took {} ms", runtime); - alignedImg->transformation.setDistortionCoefficients({}); { auto blockEvent = this->outputBlockEvent(); outputAligned.send(alignedImg); diff --git a/tests/src/ondevice_tests/image_align_node_test.cpp b/tests/src/ondevice_tests/image_align_node_test.cpp index 209cbbdbfe..9364f36f08 100644 --- a/tests/src/ondevice_tests/image_align_node_test.cpp +++ b/tests/src/ondevice_tests/image_align_node_test.cpp @@ -1,5 +1,7 @@ +#include #include #include +#include #include #include "depthai/depthai.hpp" @@ -9,15 +11,16 @@ using namespace std::chrono; using namespace std::chrono_literals; namespace { -void runImageAlignTest(bool useDepth, bool runOnHost) { +void runImageAlignTest(bool useDepth, bool runOnHost, dai::ImgResizeMode resizeMode) { dai::Pipeline p; + auto rgbCam = p.create()->build(dai::CameraBoardSocket::CAM_A); auto leftCam = p.create()->build(dai::CameraBoardSocket::CAM_B); auto rightCam = p.create()->build(dai::CameraBoardSocket::CAM_C); std::shared_ptr stereo; auto align = p.create(); - - auto leftOut = leftCam->requestOutput({1280, 800}); - auto rightOut = rightCam->requestOutput({1280, 800}); + auto* rgbOut = rgbCam->requestOutput({1280, 640}, std::nullopt, resizeMode, std::nullopt, true); + auto* leftOut = leftCam->requestOutput({1280, 800}, std::nullopt); + auto* rightOut = rightCam->requestOutput({1280, 800}, std::nullopt); if(useDepth) { stereo = p.create(); @@ -26,8 +29,9 @@ void runImageAlignTest(bool useDepth, bool runOnHost) { stereo->depth.link(align->input); } else { leftOut->link(align->input); + rightOut->createOutputQueue(); // TODO remove once left&rgb only streaming on RVC4 is supported } - rightOut->link(align->inputAlignTo); + rgbOut->link(align->inputAlignTo); if(!useDepth) { align->initialConfig->staticDepthPlane = 0x5AB1; @@ -37,12 +41,19 @@ void runImageAlignTest(bool useDepth, bool runOnHost) { } auto alignedQueue = align->outputAligned.createOutputQueue(); + auto alignToQueue = rgbOut->createOutputQueue(); p.start(); + auto alignToFrame = alignToQueue->get(); + REQUIRE(alignToFrame != nullptr); + const auto alignToIntrinsics = alignToFrame->transformation.getIntrinsicMatrix(); + constexpr size_t N = 20; for(size_t i = 0; i < N; ++i) { auto aligned = alignedQueue->get(); REQUIRE(aligned != nullptr); + REQUIRE(aligned->transformation.isAlignedTo(alignToFrame->transformation)); + REQUIRE(aligned->getInstanceNum() == alignToFrame->getInstanceNum()); } p.stop(); } @@ -51,23 +62,31 @@ void runImageAlignTest(bool useDepth, bool runOnHost) { TEST_CASE("Test ImageAlign node image to image alignment") { bool useDepth = false; bool runOnHost = false; - runImageAlignTest(useDepth, runOnHost); + for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { + runImageAlignTest(useDepth, runOnHost, resizeMode); + } } TEST_CASE("Test ImageAlign node depth to image alignment") { bool useDepth = true; bool runOnHost = false; - runImageAlignTest(useDepth, runOnHost); + for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { + runImageAlignTest(useDepth, runOnHost, resizeMode); + } } TEST_CASE("Test ImageAlign node image to image alignment on host") { bool useDepth = false; bool runOnHost = true; - runImageAlignTest(useDepth, runOnHost); + for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { + runImageAlignTest(useDepth, runOnHost, resizeMode); + } } TEST_CASE("Test ImageAlign node depth to image alignment on host") { bool useDepth = true; bool runOnHost = true; - runImageAlignTest(useDepth, runOnHost); + for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { + runImageAlignTest(useDepth, runOnHost, resizeMode); + } } From 9541d78b6a028d1f0773c5b921e74daef98595a8 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 14 Jan 2026 21:12:20 +0100 Subject: [PATCH 037/180] Add SI convention to calibrationHandler --- .../python/src/CalibrationHandlerBindings.cpp | 12 ++--- .../python/src/pipeline/CommonBindings.cpp | 15 +++--- include/depthai/common/DepthUnit.hpp | 51 ++++++++++++++----- include/depthai/device/CalibrationHandler.hpp | 17 ++++--- src/device/CalibrationHandler.cpp | 34 +++++++------ src/pipeline/node/DynamicCalibrationNode.cpp | 2 +- src/pipeline/node/host/RGBD.cpp | 2 +- src/rtabmap/CalibrationHandler.cpp | 2 +- .../onhost_tests/calibration_handler_test.cpp | 6 +-- tests/src/onhost_tests/depth_unit_test.cpp | 16 +++--- 10 files changed, 95 insertions(+), 62 deletions(-) diff --git a/bindings/python/src/CalibrationHandlerBindings.cpp b/bindings/python/src/CalibrationHandlerBindings.cpp index 8f92467237..2df21139ab 100644 --- a/bindings/python/src/CalibrationHandlerBindings.cpp +++ b/bindings/python/src/CalibrationHandlerBindings.cpp @@ -88,7 +88,7 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("srcCamera"), py::arg("dstCamera"), py::arg("useSpecTranslation") = false, - py::arg("unit") = MeasurementUnit::CENTIMETER, + py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, CalibrationHandler, getCameraExtrinsics)) .def("getHousingCalibration", @@ -96,7 +96,7 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("srcCamera"), py::arg("housingCS"), py::arg("useSpecTranslation") = true, - py::arg("unit") = MeasurementUnit::CENTIMETER, + py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, CalibrationHandler, getHousingCalibration)) .def("getCameraTranslationVector", @@ -104,7 +104,7 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("srcCamera"), py::arg("dstCamera"), py::arg("useSpecTranslation") = true, - py::arg("unit") = MeasurementUnit::CENTIMETER, + py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, CalibrationHandler, getCameraTranslationVector)) .def("getCameraRotationMatrix", &CalibrationHandler::getCameraRotationMatrix, @@ -116,20 +116,20 @@ void CalibrationHandlerBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("cam1") = dai::CameraBoardSocket::CAM_C, py::arg("cam2") = dai::CameraBoardSocket::CAM_B, py::arg("useSpecTranslation") = true, - py::arg("unit") = MeasurementUnit::CENTIMETER, + py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, CalibrationHandler, getBaselineDistance)) .def("getCameraToImuExtrinsics", &CalibrationHandler::getCameraToImuExtrinsics, py::arg("cameraId"), py::arg("useSpecTranslation") = false, - py::arg("unit") = MeasurementUnit::CENTIMETER, + py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, CalibrationHandler, getCameraToImuExtrinsics)) .def("getImuToCameraExtrinsics", &CalibrationHandler::getImuToCameraExtrinsics, py::arg("cameraId"), py::arg("useSpecTranslation") = false, - py::arg("unit") = MeasurementUnit::CENTIMETER, + py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, CalibrationHandler, getImuToCameraExtrinsics)) .def("getStereoRightRectificationRotation", diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 019a667158..7e29db6ca1 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -75,13 +75,14 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::enum_ usbSpeed(m, "UsbSpeed", DOC(dai, UsbSpeed)); py::enum_ processorType(m, "ProcessorType"); py::enum_ detectionNetworkType(m, "DetectionNetworkType"); - py::enum_ depthUnitEnum(m, "DepthUnit", DOC(dai, DepthUnit)); - depthUnitEnum.value("METER", DepthUnit::METER) - .value("CENTIMETER", DepthUnit::CENTIMETER) - .value("MILLIMETER", DepthUnit::MILLIMETER) - .value("INCH", DepthUnit::INCH) - .value("FOOT", DepthUnit::FOOT) - .value("CUSTOM", DepthUnit::CUSTOM); + py::enum_ LengthUnitEnum(m, "LengthUnit", DOC(dai, LengthUnit)); + LengthUnitEnum.value("METER", LengthUnit::METER) + .value("CENTIMETER", LengthUnit::CENTIMETER) + .value("MILLIMETER", LengthUnit::MILLIMETER) + .value("INCH", LengthUnit::INCH) + .value("FOOT", LengthUnit::FOOT) + .value("CUSTOM", LengthUnit::CUSTOM); + m.attr("DepthUnit") = m.attr("LengthUnit"); py::enum_ yoloDecodingFamily(m, "YoloDecodingFamily"); py::enum_ serializationType(m, "SerializationType"); py::class_ detectionParserOptions(m, "DetectionParserOptions", DOC(dai, DetectionParserOptions)); diff --git a/include/depthai/common/DepthUnit.hpp b/include/depthai/common/DepthUnit.hpp index 3312512786..e80beacaec 100644 --- a/include/depthai/common/DepthUnit.hpp +++ b/include/depthai/common/DepthUnit.hpp @@ -4,31 +4,54 @@ namespace dai { +enum class SIPrefix : int32_t { GIGA, MEGA, KILO, DEKA, DEFAULT, CENTI, MILLI }; /** - * Measurement unit for depth data + * Measurement unit for depth and calibration data. */ -enum class DepthUnit : int32_t { METER, CENTIMETER, MILLIMETER, INCH, FOOT, CUSTOM }; +enum class LengthUnit : int32_t { METER, CENTIMETER, MILLIMETER, INCH, FOOT, CUSTOM }; +// Backward-compatible aliases. +using DepthUnit = LengthUnit; -// Alias for generic measurement units used in calibration APIs. -using MeasurementUnit = DepthUnit; +constexpr float getSIPrefixMultiplier(SIPrefix unit); -constexpr float getDepthUnitMultiplier(DepthUnit unit) { +constexpr float getLengthUnitMultiplier(LengthUnit unit) { switch(unit) { - case DepthUnit::METER: - return 1.0f; - case DepthUnit::CENTIMETER: - return 100.0f; - case DepthUnit::MILLIMETER: - return 1000.0f; - case DepthUnit::INCH: + case LengthUnit::METER: + return getSIPrefixMultiplier(SIPrefix::DEFAULT); + case LengthUnit::CENTIMETER: + return getSIPrefixMultiplier(SIPrefix::CENTI); + case LengthUnit::MILLIMETER: + return getSIPrefixMultiplier(SIPrefix::MILLI); + case LengthUnit::INCH: return 39.3701f; - case DepthUnit::FOOT: + case LengthUnit::FOOT: return 3.28084f; - case DepthUnit::CUSTOM: + case LengthUnit::CUSTOM: return 1.0f; // CUSTOM multiplier should be handled separately default: return 1.0f; } } +constexpr float getSIPrefixMultiplier(SIPrefix unit) { + switch(unit) { + case SIPrefix::GIGA: + return 1 / 1000000000.0f; + case SIPrefix::MEGA: + return 1 / 1000000.0f; + case SIPrefix::KILO: + return 1 / 1000.0f; + case SIPrefix::DEKA: + return 1 / 10.0f; + case SIPrefix::DEFAULT: + return 1.0f; + case SIPrefix::CENTI: + return 100.0f; + case SIPrefix::MILLI: + return 1000.0f; + default: + return 1.0f; + } +} + } // namespace dai diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index fefc28fd4b..0ef940517c 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -238,7 +238,7 @@ class CalibrationHandler { std::vector> getCameraExtrinsics(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation = false, - MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; + LengthUnit unit = LengthUnit::CENTIMETER) const; /** * Get the transformation matrix between a camera and a chosen housing @@ -275,7 +275,7 @@ class CalibrationHandler { std::vector> getHousingCalibration(CameraBoardSocket srcCamera, const HousingCoordinateSystem housingCS, bool useSpecTranslation = false, - MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; + LengthUnit unit = LengthUnit::CENTIMETER) const; /** * Get the Camera translation vector between two cameras from the calibration data. @@ -289,7 +289,7 @@ class CalibrationHandler { std::vector getCameraTranslationVector(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation = true, - MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; + LengthUnit unit = LengthUnit::CENTIMETER) const; /** * Get the Camera rotation matrix between two cameras from the calibration data. @@ -319,7 +319,7 @@ class CalibrationHandler { float getBaselineDistance(CameraBoardSocket cam1 = CameraBoardSocket::CAM_C, CameraBoardSocket cam2 = CameraBoardSocket::CAM_B, bool useSpecTranslation = true, - MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; + LengthUnit unit = LengthUnit::CENTIMETER) const; /** * Get the Camera To Imu Extrinsics object @@ -342,7 +342,7 @@ class CalibrationHandler { */ std::vector> getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation = false, - MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; + LengthUnit unit = LengthUnit::CENTIMETER) const; /** * Get the Imu To Camera Extrinsics object from the data loaded if there is a linked connection @@ -365,7 +365,7 @@ class CalibrationHandler { */ std::vector> getImuToCameraExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation = false, - MeasurementUnit unit = MeasurementUnit::CENTIMETER) const; + LengthUnit unit = LengthUnit::CENTIMETER) const; /** * @@ -696,6 +696,11 @@ class CalibrationHandler { CameraBoardSocket& originSocket) const; DEPTHAI_SERIALIZE(CalibrationHandler, eepromData); + void scaleTranslationInPlace(std::vector>& mat, LengthUnit unit) const; + + protected: + static constexpr LengthUnit eepromTranslationUnits = LengthUnit::CENTIMETER; + LengthUnit getEepromTranslationUnits() const; }; } // namespace dai diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 2c79cdb803..8158bdfbf2 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -50,19 +50,23 @@ void invertSe3Matrix4x4InPlace(std::vector>& mat) { for(int i = 0; i < 3; ++i) mat[i][3] = newTrans[i]; } -float getUnitScale(MeasurementUnit unit) { - // Extrinsics are stored in centimeters; DepthUnit multiplier converts meters to target. - return getDepthUnitMultiplier(unit) / 100.0f; +float getDistanceUnitScale(LengthUnit targetUnit, LengthUnit sourceUnit) { + return getLengthUnitMultiplier(targetUnit) / getLengthUnitMultiplier(sourceUnit); +} +} // namespace + +LengthUnit CalibrationHandler::getEepromTranslationUnits() const { + return eepromTranslationUnits; } -void scaleTranslationInPlace(std::vector>& mat, MeasurementUnit unit) { - const float scale = getUnitScale(unit); +void CalibrationHandler::scaleTranslationInPlace(std::vector>& mat, LengthUnit unit) const { + const LengthUnit myUnits = getEepromTranslationUnits(); + const float scale = getDistanceUnitScale(unit, myUnits); if(scale == 1.0f) return; for(int i = 0; i < 3; ++i) { mat[i][3] *= scale; } } -} // namespace CalibrationHandler::ExtrinsicGraphValidationResult CalibrationHandler::validateExtrinsicGraph() const { std::unordered_map originMap; @@ -459,7 +463,7 @@ CameraModel CalibrationHandler::getDistortionModel(CameraBoardSocket cameraId) c std::vector> CalibrationHandler::getCameraExtrinsics(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation, - MeasurementUnit unit) const { + LengthUnit unit) const { /** * 1. Check if both camera ID exists. * 2. Check if the forward link exists from source and destination camera to origin camera. @@ -555,7 +559,7 @@ std::vector> CalibrationHandler::getHousingToHousingOrigin(co bool useSpecTranslation, CameraBoardSocket& originSocket) const { // Define scale parameter for mm to cm conversion - constexpr float MM_TO_CM_SCALE = getDepthUnitMultiplier(DepthUnit::MILLIMETER) / getDepthUnitMultiplier(DepthUnit::CENTIMETER); + constexpr float MM_TO_CM_SCALE = getLengthUnitMultiplier(DepthUnit::MILLIMETER) / getLengthUnitMultiplier(DepthUnit::CENTIMETER); const Extrinsics& housingExtrinsics = eepromData.housingExtrinsics; @@ -637,7 +641,7 @@ std::vector> CalibrationHandler::getHousingToHousingOrigin(co std::vector> CalibrationHandler::getHousingCalibration(CameraBoardSocket srcCamera, const HousingCoordinateSystem housingCS, bool useSpecTranslation, - MeasurementUnit unit) const { + LengthUnit unit) const { // Ensure we have calibration data for the requested source camera if(eepromData.cameraData.find(srcCamera) == eepromData.cameraData.end()) { throw std::runtime_error("There is no Camera data available corresponding to the requested source cameraId"); @@ -700,7 +704,7 @@ std::vector> CalibrationHandler::getHousingCalibration(Camera std::vector CalibrationHandler::getCameraTranslationVector(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation, - MeasurementUnit unit) const { + LengthUnit unit) const { std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, useSpecTranslation, unit); std::vector translationVector = {0, 0, 0}; @@ -711,7 +715,7 @@ std::vector CalibrationHandler::getCameraTranslationVector(CameraBoardSoc } std::vector> CalibrationHandler::getCameraRotationMatrix(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera) const { - std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, false, MeasurementUnit::CENTIMETER); + std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, false, LengthUnit::CENTIMETER); std::vector> rotationMatrix = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; for(auto i = 0; i < 3; i++) { @@ -722,7 +726,7 @@ std::vector> CalibrationHandler::getCameraRotationMatrix(Came return rotationMatrix; } -float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoardSocket cam2, bool useSpecTranslation, MeasurementUnit unit) const { +float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoardSocket cam2, bool useSpecTranslation, LengthUnit unit) const { std::vector translationVector = getCameraTranslationVector(cam1, cam2, useSpecTranslation, unit); float sum = 0; for(auto val : translationVector) { @@ -731,13 +735,13 @@ float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoar return std::sqrt(sum); } -std::vector> CalibrationHandler::getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation, MeasurementUnit unit) const { +std::vector> CalibrationHandler::getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation, LengthUnit unit) const { std::vector> transformationMatrix = getImuToCameraExtrinsics(cameraId, useSpecTranslation, unit); invertSe3Matrix4x4InPlace(transformationMatrix); return transformationMatrix; } -std::vector> CalibrationHandler::getImuToCameraExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation, MeasurementUnit unit) const { +std::vector> CalibrationHandler::getImuToCameraExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation, LengthUnit unit) const { if(eepromData.imuExtrinsics.rotationMatrix.size() == 0 || eepromData.imuExtrinsics.toCameraSocket == CameraBoardSocket::AUTO) { throw std::runtime_error("IMU calibration data is not available on device yet."); } else if(eepromData.cameraData.find(cameraId) == eepromData.cameraData.end()) { @@ -762,7 +766,7 @@ std::vector> CalibrationHandler::getImuToCameraExtrinsics(Cam return currTransformationMatrixImu; } else { std::vector> destTransformationMatrixCurr = - getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation, MeasurementUnit::CENTIMETER); + getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation, LengthUnit::CENTIMETER); auto result = matMul(destTransformationMatrixCurr, currTransformationMatrixImu); scaleTranslationInPlace(result, unit); return result; diff --git a/src/pipeline/node/DynamicCalibrationNode.cpp b/src/pipeline/node/DynamicCalibrationNode.cpp index 633b40f64d..5793e8596b 100644 --- a/src/pipeline/node/DynamicCalibrationNode.cpp +++ b/src/pipeline/node/DynamicCalibrationNode.cpp @@ -72,7 +72,7 @@ std::pair, std::shared_ptr 0.0f ? (1.0f / unitPerMeter) : 1.0f; } void printDevices() { diff --git a/src/rtabmap/CalibrationHandler.cpp b/src/rtabmap/CalibrationHandler.cpp index d8b8ab813c..4404dc0ade 100644 --- a/src/rtabmap/CalibrationHandler.cpp +++ b/src/rtabmap/CalibrationHandler.cpp @@ -34,7 +34,7 @@ rtabmap::StereoCameraModel CalibrationHandler::getRTABMapCameraModel(CameraBoard double fy = newCameraMatrix.at(1, 1); double cx = newCameraMatrix.at(0, 2); double cy = newCameraMatrix.at(1, 2); - double baseline = double(getBaselineDistance(right, left, true, MeasurementUnit::METER)); + double baseline = double(getBaselineDistance(right, left, true, LengthUnit::METER)); return rtabmap::StereoCameraModel(eepromData.boardName, fx, fy, cx, cy, baseline, localTransform, cv::Size(width, height)); } diff --git a/tests/src/onhost_tests/calibration_handler_test.cpp b/tests/src/onhost_tests/calibration_handler_test.cpp index c230b9db76..011ad34072 100644 --- a/tests/src/onhost_tests/calibration_handler_test.cpp +++ b/tests/src/onhost_tests/calibration_handler_test.cpp @@ -428,17 +428,17 @@ TEST_CASE("Extrinsics translation scales with measurement unit", "[getCameraExtr handler.setCameraExtrinsics(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, R, tABcm, zeros); - auto Mmeters = handler.getCameraExtrinsics(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, MeasurementUnit::METER); + auto Mmeters = handler.getCameraExtrinsics(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, LengthUnit::METER); REQUIRE(Mmeters[0][3] == Catch::Approx(1.0f).margin(1e-6)); REQUIRE(Mmeters[1][3] == Catch::Approx(-0.5f).margin(1e-6)); REQUIRE(Mmeters[2][3] == Catch::Approx(0.25f).margin(1e-6)); - auto tMillimeters = handler.getCameraTranslationVector(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, MeasurementUnit::MILLIMETER); + auto tMillimeters = handler.getCameraTranslationVector(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, LengthUnit::MILLIMETER); REQUIRE(tMillimeters[0] == Catch::Approx(1000.0f).margin(1e-6)); REQUIRE(tMillimeters[1] == Catch::Approx(-500.0f).margin(1e-6)); REQUIRE(tMillimeters[2] == Catch::Approx(250.0f).margin(1e-6)); - float baselineMeters = handler.getBaselineDistance(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, MeasurementUnit::METER); + float baselineMeters = handler.getBaselineDistance(CameraBoardSocket::CAM_C, CameraBoardSocket::CAM_D, false, LengthUnit::METER); float expectedMeters = std::sqrt(1.0f * 1.0f + 0.5f * 0.5f + 0.25f * 0.25f); REQUIRE(baselineMeters == Catch::Approx(expectedMeters).margin(1e-6)); } diff --git a/tests/src/onhost_tests/depth_unit_test.cpp b/tests/src/onhost_tests/depth_unit_test.cpp index 10b9e3d6d4..ddbed271da 100644 --- a/tests/src/onhost_tests/depth_unit_test.cpp +++ b/tests/src/onhost_tests/depth_unit_test.cpp @@ -5,22 +5,22 @@ namespace { float toUnit(float meters, dai::DepthUnit unit) { - return meters * dai::getDepthUnitMultiplier(unit); + return meters * dai::getLengthUnitMultiplier(unit); } float toMeters(float value, dai::DepthUnit unit) { - return value / dai::getDepthUnitMultiplier(unit); + return value / dai::getLengthUnitMultiplier(unit); } } // namespace TEST_CASE("DepthUnit multipliers", "[DepthUnit]") { - REQUIRE(dai::getDepthUnitMultiplier(dai::DepthUnit::METER) == Catch::Approx(1.0f)); - REQUIRE(dai::getDepthUnitMultiplier(dai::DepthUnit::CENTIMETER) == Catch::Approx(100.0f)); - REQUIRE(dai::getDepthUnitMultiplier(dai::DepthUnit::MILLIMETER) == Catch::Approx(1000.0f)); - REQUIRE(dai::getDepthUnitMultiplier(dai::DepthUnit::INCH) == Catch::Approx(39.3701f)); - REQUIRE(dai::getDepthUnitMultiplier(dai::DepthUnit::FOOT) == Catch::Approx(3.28084f)); - REQUIRE(dai::getDepthUnitMultiplier(dai::DepthUnit::CUSTOM) == Catch::Approx(1.0f)); + REQUIRE(dai::getLengthUnitMultiplier(dai::DepthUnit::METER) == Catch::Approx(1.0f)); + REQUIRE(dai::getLengthUnitMultiplier(dai::DepthUnit::CENTIMETER) == Catch::Approx(100.0f)); + REQUIRE(dai::getLengthUnitMultiplier(dai::DepthUnit::MILLIMETER) == Catch::Approx(1000.0f)); + REQUIRE(dai::getLengthUnitMultiplier(dai::DepthUnit::INCH) == Catch::Approx(39.3701f)); + REQUIRE(dai::getLengthUnitMultiplier(dai::DepthUnit::FOOT) == Catch::Approx(3.28084f)); + REQUIRE(dai::getLengthUnitMultiplier(dai::DepthUnit::CUSTOM) == Catch::Approx(1.0f)); } TEST_CASE("DepthUnit conversions", "[DepthUnit]") { From ba557a1b18563e31c41d3292caf3d696b53b02ad Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 14 Jan 2026 23:28:42 +0100 Subject: [PATCH 038/180] Adding test for exposed API --- .../onhost_tests/calibration_handler_test.cpp | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/tests/src/onhost_tests/calibration_handler_test.cpp b/tests/src/onhost_tests/calibration_handler_test.cpp index 011ad34072..40574eb0ce 100644 --- a/tests/src/onhost_tests/calibration_handler_test.cpp +++ b/tests/src/onhost_tests/calibration_handler_test.cpp @@ -253,6 +253,30 @@ static CalibrationHandler loadHandlerWithHousingRotation() { return CalibrationHandler::fromJson(calibJson); } +static CalibrationHandler loadHandlerWithImuExtrinsics() { + dai::EepromData data; + data.cameraData[CameraBoardSocket::CAM_A]; + + data.imuExtrinsics.rotationMatrix = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}; + data.imuExtrinsics.translation = {10.0f, -5.0f, 2.5f}; + data.imuExtrinsics.specTranslation = {1.0f, 2.0f, 3.0f}; + data.imuExtrinsics.toCameraSocket = CameraBoardSocket::CAM_A; + + return CalibrationHandler(data); +} + +static std::vector> getAllUnitScales(LengthUnit baseUnit) { + const float cm = getLengthUnitMultiplier(baseUnit); + return { + {LengthUnit::METER, getLengthUnitMultiplier(LengthUnit::METER) / cm}, + {LengthUnit::CENTIMETER, getLengthUnitMultiplier(LengthUnit::CENTIMETER) / cm}, + {LengthUnit::MILLIMETER, getLengthUnitMultiplier(LengthUnit::MILLIMETER) / cm}, + {LengthUnit::INCH, getLengthUnitMultiplier(LengthUnit::INCH) / cm}, + {LengthUnit::FOOT, getLengthUnitMultiplier(LengthUnit::FOOT) / cm}, + {LengthUnit::CUSTOM, getLengthUnitMultiplier(LengthUnit::CUSTOM) / cm}, + }; +} + static CalibrationHandler loadInvalidHandler() { return CalibrationHandler::fromJson(loadCalibJson()); } @@ -1104,3 +1128,45 @@ TEST_CASE("getHousingCalibration - Disconnected origin cameras throw", "[getHous REQUIRE_THROWS_WITH(handler.getHousingCalibration(CameraBoardSocket::CAM_A, dai::HousingCoordinateSystem::FRONT_CAM_A, true), Catch::Matchers::ContainsSubstring("Missing extrinsic link from source camera to destination camera")); } + +TEST_CASE("getHousingCalibration scales translation for all units", "[getHousingCalibration][units]") { + auto handler = loadHandlerWithHousingRotation(); + auto base = handler.getHousingCalibration(CameraBoardSocket::CAM_C, dai::HousingCoordinateSystem::FRONT_CAM_A, false, LengthUnit::CENTIMETER); + + for(const auto& [unit, scale] : getAllUnitScales(LengthUnit::CENTIMETER)) { + auto result = handler.getHousingCalibration(CameraBoardSocket::CAM_C, dai::HousingCoordinateSystem::FRONT_CAM_A, false, unit); + auto expected = base; + expected[0][3] *= scale; + expected[1][3] *= scale; + expected[2][3] *= scale; + requireMatrixApproxEqual(result, expected); + } +} + +TEST_CASE("getImuToCameraExtrinsics scales translation for all units", "[getImuToCameraExtrinsics][units]") { + auto handler = loadHandlerWithImuExtrinsics(); + auto base = handler.getImuToCameraExtrinsics(CameraBoardSocket::CAM_A, false, LengthUnit::CENTIMETER); + + for(const auto& [unit, scale] : getAllUnitScales(LengthUnit::CENTIMETER)) { + auto result = handler.getImuToCameraExtrinsics(CameraBoardSocket::CAM_A, false, unit); + auto expected = base; + expected[0][3] *= scale; + expected[1][3] *= scale; + expected[2][3] *= scale; + requireMatrixApproxEqual(result, expected); + } +} + +TEST_CASE("getCameraToImuExtrinsics scales translation for all units", "[getCameraToImuExtrinsics][units]") { + auto handler = loadHandlerWithImuExtrinsics(); + auto base = handler.getCameraToImuExtrinsics(CameraBoardSocket::CAM_A, false, LengthUnit::CENTIMETER); + + for(const auto& [unit, scale] : getAllUnitScales(LengthUnit::CENTIMETER)) { + auto result = handler.getCameraToImuExtrinsics(CameraBoardSocket::CAM_A, false, unit); + auto expected = base; + expected[0][3] *= scale; + expected[1][3] *= scale; + expected[2][3] *= scale; + requireMatrixApproxEqual(result, expected); + } +} From 8795bf72eaa07ebb1823b987c6b99c7066a7abc7 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 16 Jan 2026 09:36:23 +0100 Subject: [PATCH 039/180] Terminate examples if NeuralDepth is not supported --- examples/cpp/Vpp/virtual_patern_projection.cpp | 5 +++++ examples/python/Vpp/virtual_patern_projection.py | 7 ++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/examples/cpp/Vpp/virtual_patern_projection.cpp b/examples/cpp/Vpp/virtual_patern_projection.cpp index dd91358ebc..54af653996 100644 --- a/examples/cpp/Vpp/virtual_patern_projection.cpp +++ b/examples/cpp/Vpp/virtual_patern_projection.cpp @@ -49,6 +49,11 @@ void showDepth(const cv::Mat& depthFrameIn, int main() { int fps = 20; dai::Pipeline pipeline; + auto device = pipeline.getDefaultDevice(); + if(!device->isNeuralDepthSupported()) { + std::cout << "Exiting Vpp example: device doesn't support NeuralDepth.\n"; + return 0; + } // Left / Right cameras auto monoLeft = pipeline.create()->build(dai::CameraBoardSocket::CAM_B, std::nullopt, fps); diff --git a/examples/python/Vpp/virtual_patern_projection.py b/examples/python/Vpp/virtual_patern_projection.py index 7aeeec29ff..a7ed1f954f 100644 --- a/examples/python/Vpp/virtual_patern_projection.py +++ b/examples/python/Vpp/virtual_patern_projection.py @@ -46,7 +46,12 @@ def showDepth(depthFrame, windowName="Depth", minDistance=500, maxDistance=5000, if __name__ == "__main__": fps = 20 - pipeline = dai.Pipeline() + device = dai.Device() + pipeline = dai.Pipeline(device) + + if not device.isNeuralDepthSupported(): + print("Exiting Vpp example: device doesn't support NeuralDepth.") + exit() # Left Right cameras monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B, sensorFps=fps) From 91a6232131e0150cc1bb77ab905e95bc80d9b418 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Fri, 16 Jan 2026 09:48:32 +0100 Subject: [PATCH 040/180] remove confidence check --- tests/src/onhost_tests/pipeline/datatype/imgdetections_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/src/onhost_tests/pipeline/datatype/imgdetections_test.cpp b/tests/src/onhost_tests/pipeline/datatype/imgdetections_test.cpp index 7bccdc7199..aeff51e3f5 100644 --- a/tests/src/onhost_tests/pipeline/datatype/imgdetections_test.cpp +++ b/tests/src/onhost_tests/pipeline/datatype/imgdetections_test.cpp @@ -221,7 +221,6 @@ TEST_CASE("Keypoint validation", "[ImgDetections][Keypoint]") { using namespace dai; REQUIRE_NOTHROW(Keypoint(Point3f{0.0F, 0.0F, 0.0F}, 0.1F)); - REQUIRE_THROWS_AS(Keypoint(Point3f{0.0F, 0.0F, 0.0F}, -0.1F), std::invalid_argument); } TEST_CASE("ImgDetections segmentation mask operations", "[ImgDetections][Segmentation]") { From 7cb4b0b6d06eeb692864b381cd3d3678ee3cce30 Mon Sep 17 00:00:00 2001 From: aljazkonec1 <53098513+aljazkonec1@users.noreply.github.com> Date: Fri, 16 Jan 2026 13:23:19 +0100 Subject: [PATCH 041/180] Feat: Extend NeuralNetwork build functions (#1628) * Add ImgFramCapability arguument to build * clang-tidy improvements * Add ImgFramCapability arguument to build * Apply suggestions --- .../node/DetectionNetworkBindings.cpp | 113 ++++++++++++------ .../pipeline/node/NeuralNetworkBindings.cpp | 96 ++++++++++----- .../node/SpatialDetectionNetworkBindings.cpp | 96 +++++++++------ .../pipeline/node/DetectionNetwork.hpp | 36 +++--- .../depthai/pipeline/node/NeuralNetwork.hpp | 42 +++---- .../pipeline/node/SpatialDetectionNetwork.hpp | 24 ++-- src/pipeline/node/DetectionNetwork.cpp | 32 ++--- src/pipeline/node/NeuralNetwork.cpp | 72 ++++++----- src/pipeline/node/SpatialDetectionNetwork.cpp | 22 ++-- 9 files changed, 314 insertions(+), 219 deletions(-) diff --git a/bindings/python/src/pipeline/node/DetectionNetworkBindings.cpp b/bindings/python/src/pipeline/node/DetectionNetworkBindings.cpp index b07b24e04e..bb70c5f073 100644 --- a/bindings/python/src/pipeline/node/DetectionNetworkBindings.cpp +++ b/bindings/python/src/pipeline/node/DetectionNetworkBindings.cpp @@ -4,6 +4,7 @@ #include "Common.hpp" #include "NodeBindings.hpp" +#include "depthai/capabilities/ImgFrameCapability.hpp" #include "depthai/modelzoo/Zoo.hpp" #include "depthai/pipeline/DeviceNodeGroup.hpp" #include "depthai/pipeline/Node.hpp" @@ -16,6 +17,7 @@ void bind_detectionnetwork(pybind11::module& m, void* pCallstack) { using namespace dai::node; auto detectionNetwork = ADD_NODE_DERIVED(DetectionNetwork, DeviceNodeGroup); + detectionNetwork.attr("Model") = daiNodeModule.attr("NeuralNetwork").attr("Model"); /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -50,46 +52,79 @@ void bind_detectionnetwork(pybind11::module& m, void* pCallstack) { }, DETECTION_NETWORK_BUILD_PYARGS, DETECTION_NETWORK_PYARGS) - .def("build", - py::overload_cast&, NNModelDescription, std::optional, std::optional>( - &DetectionNetwork::build), - py::arg("input"), - py::arg("model"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, DetectionNetwork, build)) - .def("build", - ([](DetectionNetwork& self, - const std::shared_ptr& input, - std::string model, - std::optional fps, - std::optional resizeMode) { return self.build(input, NNModelDescription{model}, fps, resizeMode); }), - py::arg("input"), - py::arg("model"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, DetectionNetwork, build)) - .def("build", - py::overload_cast&, const NNArchive&, std::optional, std::optional>( - &DetectionNetwork::build), - py::arg("input"), - py::arg("nnArchive"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, DetectionNetwork, build, 3)) + .def( + "build", + [](DetectionNetwork& self, + const std::shared_ptr& input, + NNModelDescription modelDesc, + std::optional fps, + std::optional resizeMode) { return self.build(input, DetectionNetwork::Model{std::move(modelDesc)}, fps, resizeMode); }, + py::arg("input"), + py::arg("model"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, DetectionNetwork, build)) + .def( + "build", + [](DetectionNetwork& self, + const std::shared_ptr& input, + const NNArchive& nnArchive, + std::optional fps, + std::optional resizeMode) { return self.build(input, DetectionNetwork::Model{nnArchive}, fps, resizeMode); }, + py::arg("input"), + py::arg("nnArchive"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, DetectionNetwork, build)) + .def( + "build", + [](DetectionNetwork& self, + const std::shared_ptr& input, + const std::string& model, + std::optional fps, + std::optional resizeMode) { return self.build(input, DetectionNetwork::Model{model}, fps, resizeMode); }, + py::arg("input"), + py::arg("model"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, DetectionNetwork, build)) + .def( + "build", + [](DetectionNetwork& self, const std::shared_ptr& input, const DetectionNetwork::Model& model, const ImgFrameCapability& capability) { + return self.build(input, model, capability); + }, + py::arg("input"), + py::arg("model"), + py::arg("capability"), + DOC(dai, node, DetectionNetwork, build, 3)) #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT - .def("build", - py::overload_cast&, NNModelDescription, std::optional>(&DetectionNetwork::build), - py::arg("input"), - py::arg("model"), - py::arg("fps") = std::nullopt, - DOC(dai, node, DetectionNetwork, build, 4)) - .def("build", - py::overload_cast&, const NNArchive&, std::optional>(&DetectionNetwork::build), - py::arg("input"), - py::arg("nnArchive"), - py::arg("fps") = std::nullopt, - DOC(dai, node, DetectionNetwork, build, 5)) + .def( + "build", + [](DetectionNetwork& self, const std::shared_ptr& input, NNModelDescription modelDesc, std::optional fps) { + return self.build(input, DetectionNetwork::Model{std::move(modelDesc)}, fps); + }, + py::arg("input"), + py::arg("model"), + py::arg("fps") = std::nullopt, + DOC(dai, node, DetectionNetwork, build, 4)) + .def( + "build", + [](DetectionNetwork& self, const std::shared_ptr& input, const std::string& model, std::optional fps) { + return self.build(input, DetectionNetwork::Model{NNModelDescription{model}}, fps); + }, + py::arg("input"), + py::arg("model"), + py::arg("fps") = std::nullopt, + DOC(dai, node, DetectionNetwork, build, 4)) + .def( + "build", + [](DetectionNetwork& self, const std::shared_ptr& input, const NNArchive& nnArchive, std::optional fps) { + return self.build(input, DetectionNetwork::Model{nnArchive}, fps); + }, + py::arg("input"), + py::arg("nnArchive"), + py::arg("fps") = std::nullopt, + DOC(dai, node, DetectionNetwork, build, 4)) #endif .def(py::init([](DETECTION_NETWORK_BUILD_ARGS, DETECTION_NETWORK_ARGS) { auto self = getImplicitPipeline()->create(); diff --git a/bindings/python/src/pipeline/node/NeuralNetworkBindings.cpp b/bindings/python/src/pipeline/node/NeuralNetworkBindings.cpp index 84a678826f..6c35cd1466 100644 --- a/bindings/python/src/pipeline/node/NeuralNetworkBindings.cpp +++ b/bindings/python/src/pipeline/node/NeuralNetworkBindings.cpp @@ -2,6 +2,7 @@ #include "Common.hpp" #include "NodeBindings.hpp" +#include "depthai/capabilities/ImgFrameCapability.hpp" #include "depthai/pipeline/Node.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/NeuralNetwork.hpp" @@ -14,6 +15,10 @@ void bind_neuralnetwork(pybind11::module& m, void* pCallstack) { py::class_> neuralNetworkProperties( m, "NeuralNetworkProperties", DOC(dai, NeuralNetworkProperties)); auto neuralNetwork = ADD_NODE(NeuralNetwork); + py::class_ neuralNetworkModel(neuralNetwork, "Model"); + neuralNetworkModel.def(py::init(), py::arg("modelDesc")) + .def(py::init(), py::arg("nnArchive")) + .def(py::init(), py::arg("model_id")); /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -66,46 +71,79 @@ void bind_neuralnetwork(pybind11::module& m, void* pCallstack) { py::arg("input"), py::arg("nnArchive"), DOC(dai, node, NeuralNetwork, build)) - .def("build", - py::overload_cast&, dai::NNModelDescription, std::optional, std::optional>( - &NeuralNetwork::build), - py::arg("input"), - py::arg("modelDesc"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, NeuralNetwork, build, 2)) - .def("build", - py::overload_cast&, dai::NNArchive, std::optional, std::optional>(&NeuralNetwork::build), - py::arg("input"), - py::arg("model"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, NeuralNetwork, build, 3)) + .def( + "build", + [](NeuralNetwork& self, + const std::shared_ptr& input, + NNModelDescription modelDesc, + std::optional fps, + std::optional resizeMode) { return self.build(input, NeuralNetwork::Model{std::move(modelDesc)}, fps, resizeMode); }, + py::arg("input"), + py::arg("modelDesc"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, NeuralNetwork, build, 2)) + .def( + "build", + [](NeuralNetwork& self, + const std::shared_ptr& input, + const NNArchive& nnArchive, + std::optional fps, + std::optional resizeMode) { return self.build(input, NeuralNetwork::Model{nnArchive}, fps, resizeMode); }, + py::arg("input"), + py::arg("model"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, NeuralNetwork, build, 2)) .def( "build", [](NeuralNetwork& self, const std::shared_ptr& input, const std::string& model, std::optional fps, - std::optional resizeMode) { return self.build(input, NNModelDescription{model}, fps, resizeMode); }, + std::optional resizeMode) { return self.build(input, NeuralNetwork::Model{model}, fps, resizeMode); }, py::arg("input"), py::arg("model"), py::arg("fps") = std::nullopt, py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, NeuralNetwork, build)) + DOC(dai, node, NeuralNetwork, build, 2)) + .def( + "build", + [](NeuralNetwork& self, const std::shared_ptr& input, const NeuralNetwork::Model& model, const ImgFrameCapability& capability) { + return self.build(input, model, capability); + }, + py::arg("input"), + py::arg("model"), + py::arg("capability"), + DOC(dai, node, NeuralNetwork, build, 3)) #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT - .def("build", - py::overload_cast&, const NNArchive&, std::optional>(&NeuralNetwork::build), - py::arg("input"), - py::arg("model"), - py::arg("fps") = std::nullopt, - DOC(dai, node, NeuralNetwork, build, 4)) - .def("build", - py::overload_cast&, const NNArchive&, std::optional>(&NeuralNetwork::build), - py::arg("input"), - py::arg("nnArchive"), - py::arg("fps") = std::nullopt, - DOC(dai, node, NeuralNetwork, build, 5)) + .def( + "build", + [](NeuralNetwork& self, const std::shared_ptr& input, const NNModelDescription modelDesc, std::optional fps) { + return self.build(input, NeuralNetwork::Model{modelDesc}, fps); + }, + py::arg("input"), + py::arg("modelDesc"), + py::arg("fps") = std::nullopt, + DOC(dai, node, NeuralNetwork, build, 4)) + .def( + "build", + [](NeuralNetwork& self, const std::shared_ptr& input, const std::string& model, std::optional fps) { + return self.build(input, NeuralNetwork::Model{NNModelDescription{model}}, fps); + }, + py::arg("input"), + py::arg("model"), + py::arg("fps") = std::nullopt, + DOC(dai, node, NeuralNetwork, build, 4)) + .def( + "build", + [](NeuralNetwork& self, const std::shared_ptr& input, const NNArchive& nnArchive, std::optional fps) { + return self.build(input, NeuralNetwork::Model{nnArchive}, fps); + }, + py::arg("input"), + py::arg("nnArchive"), + py::arg("fps") = std::nullopt, + DOC(dai, node, NeuralNetwork, build, 4)) #endif .def("setBlob", py::overload_cast(&NeuralNetwork::setBlob), py::arg("blob"), DOC(dai, node, NeuralNetwork, setBlob)) .def("setBlob", py::overload_cast(&NeuralNetwork::setBlob), py::arg("path"), DOC(dai, node, NeuralNetwork, setBlob, 2)) diff --git a/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp b/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp index 8a334c970f..f6d5f19d88 100644 --- a/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp +++ b/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp @@ -13,6 +13,7 @@ void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack) { py::class_> spatialDetectionNetworkProperties( m, "SpatialDetectionNetworkProperties", DOC(dai, SpatialDetectionNetworkProperties)); auto spatialDetectionNetwork = ADD_NODE_DERIVED(SpatialDetectionNetwork, DeviceNode); + spatialDetectionNetwork.attr("Model") = daiNodeModule.attr("NeuralNetwork").attr("Model"); /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -34,43 +35,64 @@ void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack) { // Node spatialDetectionNetwork // Build methods with DepthSource variant - .def("build", - py::overload_cast&, - const node::DepthSource&, - NNModelDescription, - std::optional, - std::optional>(&SpatialDetectionNetwork::build), - py::arg("input"), - py::arg("stereo"), - py::arg("model"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, SpatialDetectionNetwork, build)) - .def("build", - ([](SpatialDetectionNetwork& self, - const std::shared_ptr& input, - const node::DepthSource& depthSource, - std::string model, - std::optional fps, - std::optional resizeMode) { return self.build(input, depthSource, NNModelDescription{model}, fps, resizeMode); }), - py::arg("input"), - py::arg("stereo"), - py::arg("model"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, SpatialDetectionNetwork, build)) - .def("build", - py::overload_cast&, - const node::DepthSource&, - const NNArchive&, - std::optional, - std::optional>(&SpatialDetectionNetwork::build), - py::arg("input"), - py::arg("stereo"), - py::arg("nnArchive"), - py::arg("fps") = std::nullopt, - py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), - DOC(dai, node, SpatialDetectionNetwork, build, 2)) + .def( + "build", + [](SpatialDetectionNetwork& self, + const std::shared_ptr& input, + const node::DepthSource& depthSource, + NNModelDescription modelDesc, + std::optional fps, + std::optional resizeMode) { + return self.build(input, depthSource, SpatialDetectionNetwork::Model{std::move(modelDesc)}, fps, resizeMode); + }, + py::arg("input"), + py::arg("stereo"), + py::arg("model"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, SpatialDetectionNetwork, build)) + .def( + "build", + [](SpatialDetectionNetwork& self, + const std::shared_ptr& input, + const node::DepthSource& depthSource, + const NNArchive& nnArchive, + std::optional fps, + std::optional resizeMode) { + return self.build(input, depthSource, SpatialDetectionNetwork::Model{nnArchive}, fps, resizeMode); + }, + py::arg("input"), + py::arg("stereo"), + py::arg("nnArchive"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, SpatialDetectionNetwork, build)) + .def( + "build", + [](SpatialDetectionNetwork& self, + const std::shared_ptr& input, + const node::DepthSource& depthSource, + const std::string& model, + std::optional fps, + std::optional resizeMode) { return self.build(input, depthSource, SpatialDetectionNetwork::Model{model}, fps, resizeMode); }, + py::arg("input"), + py::arg("stereo"), + py::arg("model"), + py::arg("fps") = std::nullopt, + py::arg_v("resizeMode", dai::ImgResizeMode::CROP, "dai.ImgResizeMode.CROP"), + DOC(dai, node, SpatialDetectionNetwork, build)) + .def( + "build", + [](SpatialDetectionNetwork& self, + const std::shared_ptr& input, + const node::DepthSource& depthSource, + const SpatialDetectionNetwork::Model& model, + const ImgFrameCapability& capability) { return self.build(input, depthSource, model, capability); }, + py::arg("input"), + py::arg("stereo"), + py::arg("model"), + py::arg("capability"), + DOC(dai, node, SpatialDetectionNetwork, build, 2)) .def("setBlobPath", &SpatialDetectionNetwork::setBlobPath, py::arg("path"), DOC(dai, node, SpatialDetectionNetwork, setBlobPath)) .def("setNumPoolFrames", &SpatialDetectionNetwork::setNumPoolFrames, py::arg("numFrames"), DOC(dai, node, SpatialDetectionNetwork, setNumPoolFrames)) .def("setNumInferenceThreads", diff --git a/include/depthai/pipeline/node/DetectionNetwork.hpp b/include/depthai/pipeline/node/DetectionNetwork.hpp index 74d72c7f4f..0d3d9d9442 100644 --- a/include/depthai/pipeline/node/DetectionNetwork.hpp +++ b/include/depthai/pipeline/node/DetectionNetwork.hpp @@ -21,6 +21,7 @@ namespace node { class DetectionNetwork : public DeviceNodeGroup { public: DetectionNetwork(const std::shared_ptr& device); + using Model = NeuralNetwork::Model; [[nodiscard]] static std::shared_ptr create(const std::shared_ptr& device) { auto networkPtr = std::make_shared(device); @@ -35,41 +36,36 @@ class DetectionNetwork : public DeviceNodeGroup { * @returns Shared pointer to DetectionNetwork node */ std::shared_ptr build(Node::Output& input, const NNArchive& nnArchive); - std::shared_ptr build(const std::shared_ptr& input, - NNModelDescription modelDesc, - std::optional fps = std::nullopt, - std::optional resizeMode = dai::ImgResizeMode::CROP); - /** - * @brief Build DetectionNetwork node. Connect Camera output to this node's input. Also call setNNArchive() with provided NNArchive. + * @brief Build DetectionNetwork node. Connect Camera output to this node's input and configure the inference model. * @param input: Camera node - * @param nnArchive: Neural network archive + * @param model: Neural network model description, NNArchive or HubAI model id string * @param fps: Desired frames per second * @param resizeMode: Resize mode for input frames * @returns Shared pointer to DetectionNetwork node */ std::shared_ptr build(const std::shared_ptr& input, - const NNArchive& nnArchive, + const Model& model, std::optional fps = std::nullopt, std::optional resizeMode = dai::ImgResizeMode::CROP); -#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT - /* - * @brief Build DetectionNetwork node. Connect ReplayVideo output to this node's input. Also call setNNArchive() with provided model description. - * @param input: ReplayVideo node - * @param modelDesc: Neural network model description - * @param fps: Desired frames per second + + /** + * @brief Build DetectionNetwork node. Connect Camera output to this node's input and configure the inference model. + * @param input: Camera node + * @param model: Neural network model description, NNArchive or HubAI model id string + * @param capability: Camera capabilities * @returns Shared pointer to DetectionNetwork node */ - std::shared_ptr build(const std::shared_ptr& input, NNModelDescription modelDesc, std::optional fps = std::nullopt); - /** - * @brief Build DetectionNetwork node. Connect ReplayVideo output to this node's input. + std::shared_ptr build(const std::shared_ptr& input, const Model& model, const ImgFrameCapability& capability); +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + /* + * @brief Build DetectionNetwork node. Connect ReplayVideo output to this node's input and configure the inference model. * @param input: ReplayVideo node - * @param nnArchive: Neural network archive + * @param model: Neural network model description, NNArchive or HubAI model id string * @param fps: Desired frames per second * @returns Shared pointer to DetectionNetwork node */ - - std::shared_ptr build(const std::shared_ptr& input, const NNArchive& nnArchive, std::optional fps = std::nullopt); + std::shared_ptr build(const std::shared_ptr& input, const Model& model, std::optional fps = std::nullopt); #endif Subnode neuralNetwork{*this, "neuralNetwork"}; Subnode detectionParser{*this, "detectionParser"}; diff --git a/include/depthai/pipeline/node/NeuralNetwork.hpp b/include/depthai/pipeline/node/NeuralNetwork.hpp index 6a9e177f0d..a5bb2ac82e 100644 --- a/include/depthai/pipeline/node/NeuralNetwork.hpp +++ b/include/depthai/pipeline/node/NeuralNetwork.hpp @@ -1,5 +1,6 @@ #pragma once +#include "depthai/capabilities/ImgFrameCapability.hpp" #include "depthai/modelzoo/Zoo.hpp" #include "depthai/nn_archive/NNArchive.hpp" #include "depthai/nn_archive/NNArchiveVersionedConfig.hpp" @@ -25,12 +26,12 @@ class NeuralNetwork : public DeviceNodeCRTP; ~NeuralNetwork() override; /** - * @brief Build NeuralNetwork node. Connect output to this node's input. Also call setNNArchive() with provided NNArchive. - * + * @brief Build NeuralNetwork node. Connect output to this node's input and sets up the NNArchive. * @param output: Output to link * @param nnArchive: Neural network archive * @returns Shared pointer to NeuralNetwork node @@ -38,47 +39,36 @@ class NeuralNetwork : public DeviceNodeCRTP build(Node::Output& input, const NNArchive& nnArchive); /** - * @brief Build NeuralNetwork node. Connect Camera output to this node's input. Also call setNNArchive() with provided model description. + * @brief Build NeuralNetwork node. Connect Camera output to this node's input and configure the inference model. * @param input: Camera node - * @param modelDesc: Neural network model description + * @param model: Neural network model description, NNArchive or HubAI model id string * @param fps: Desired frames per second * @param resizeMode: Resize mode for input frames * * @returns Shared pointer to NeuralNetwork node */ std::shared_ptr build(const std::shared_ptr& input, - NNModelDescription modelDesc, + const Model& model, std::optional fps = std::nullopt, std::optional resizeMode = dai::ImgResizeMode::CROP); /** - * @brief Build NeuralNetwork node. Connect Camera output to this node's input. Also call setNNArchive() with provided NNArchive. + * @brief Build NeuralNetwork node. Connect Camera output to this node's input and configure the inference model. * @param input: Camera node - * @param nnArchive: Neural network archive - * @param fps: Desired frames per second - * @param resizeMode: Resize mode for input frames + * @param model: Neural network model description, NNArchive or HubAI model id string + * @param capability: Camera capabilities * @returns Shared pointer to NeuralNetwork node */ - std::shared_ptr build(const std::shared_ptr& input, - NNArchive nnArchive, - std::optional fps = std::nullopt, - std::optional resizeMode = dai::ImgResizeMode::CROP); + std::shared_ptr build(const std::shared_ptr& input, const Model& model, const ImgFrameCapability& capability); + #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT /** - * @brief Build NeuralNetwork node. Connect ReplayVideo output to this node's input. Also call setNNArchive() with provided model description. + * @brief Build NeuralNetwork node. Connect ReplayVideo output to this node's input and configure the inference model. * @param input: ReplayVideo node - * @param modelDesc: Neural network model description + * @param model: Neural network model description, NNArchive or HubAI model id string * @param fps: Desired frames per second * @returns Shared pointer to NeuralNetwork node */ - std::shared_ptr build(const std::shared_ptr& input, NNModelDescription modelDesc, std::optional fps = std::nullopt); - /** - * @brief Build NeuralNetwork node. Connect ReplayVideo output to this node's input. - * @param input: ReplayVideo node - * @param nnArchive: Neural network archive - * @param fps: Desired frames per second - * @returns Shared pointer to NeuralNetwork node - */ - std::shared_ptr build(const std::shared_ptr& input, const NNArchive& nnArchive, std::optional fps = std::nullopt); + std::shared_ptr build(const std::shared_ptr& input, const Model& model, std::optional fps = std::nullopt); #endif /** @@ -238,7 +228,9 @@ class NeuralNetwork : public DeviceNodeCRTP fps, std::optional resizeMode); + void decodeModel(const Model& model); + + ImgFrameCapability getFrameCapability(const NNArchive& nnArchive, std::optional expectedCapability = std::nullopt); std::optional nnArchive; }; diff --git a/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp b/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp index 8ca0a3801c..a189977328 100644 --- a/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp +++ b/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp @@ -31,6 +31,8 @@ using DepthSource = std::variant, std::shared_ptr { public: + using Model = NeuralNetwork::Model; + explicit SpatialDetectionNetwork(const std::shared_ptr& device) : DeviceNodeCRTP(device) #ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 @@ -102,36 +104,34 @@ class SpatialDetectionNetwork : public DeviceNodeCRTP build(const std::shared_ptr& inputRgb, const DepthSource& depthSource, - dai::NNModelDescription modelDesc, + const Model& model, std::optional fps = std::nullopt, std::optional resizeMode = std::nullopt); /** - * @brief Build SpatialDetectionNetwork node with specified depth source. Connect Camera and depth source outputs to this node's inputs. - * Also call setNNArchive() with provided NNArchive. + * @brief Build SpatialDetectionNetwork node with specified depth source. Connect Camera and depth source outputs to this node's inputs and configure the + * inference model. * @param inputRgb Camera node * @param depthSource Depth source node (StereoDepth, NeuralDepth, or ToF) - * @param nnArchive Neural network archive - * @param fps Desired frames per second - * @param resizeMode Resize mode for input color frames + * @param model: Neural network model description, NNArchive or HubAI model id string + * @param capability: Camera capabilities * @returns Shared pointer to SpatialDetectionNetwork node */ std::shared_ptr build(const std::shared_ptr& inputRgb, const DepthSource& depthSource, - const dai::NNArchive& nnArchive, - std::optional fps = std::nullopt, - std::optional resizeMode = std::nullopt); + const Model& model, + const ImgFrameCapability& capability); Subnode neuralNetwork{*this, "neuralNetwork"}; Subnode detectionParser{*this, "detectionParser"}; diff --git a/src/pipeline/node/DetectionNetwork.cpp b/src/pipeline/node/DetectionNetwork.cpp index 019226e041..71a0fc1374 100644 --- a/src/pipeline/node/DetectionNetwork.cpp +++ b/src/pipeline/node/DetectionNetwork.cpp @@ -15,6 +15,7 @@ #include "depthai/nn_archive/NNArchive.hpp" #include "nn_archive/NNArchiveVersionedConfig.hpp" #include "pipeline/DeviceNodeGroup.hpp" +#include "pipeline/node/NeuralNetwork.hpp" #include "utility/ArchiveUtil.hpp" #include "utility/ErrorMacros.hpp" #include "utility/PimplImpl.hpp" @@ -54,30 +55,29 @@ std::shared_ptr DetectionNetwork::build(Node::Output& input, c } std::shared_ptr DetectionNetwork::build(const std::shared_ptr& camera, - NNModelDescription modelDesc, + const Model& model, std::optional fps, std::optional resizeMode) { - auto nnArchive = createNNArchive(modelDesc); - return build(camera, nnArchive, fps, resizeMode); + ImgFrameCapability cap; + if(fps.has_value()) cap.fps.value = *fps; + if(resizeMode.has_value()) cap.resizeMode = *resizeMode; + return build(camera, model, cap); } -std::shared_ptr DetectionNetwork::build(const std::shared_ptr& camera, - const NNArchive& nnArchive, - std::optional fps, - std::optional resizeMode) { - neuralNetwork->build(camera, nnArchive, fps, resizeMode); - detectionParser->setNNArchive(nnArchive); +std::shared_ptr DetectionNetwork::build(const std::shared_ptr& camera, const Model& model, const ImgFrameCapability& capability) { + neuralNetwork->build(camera, model, capability); + auto nnArchive = neuralNetwork->getNNArchive(); + DAI_CHECK(nnArchive.has_value(), "NeuralNetwork NNArchive is not set after build."); + detectionParser->setNNArchive(*nnArchive); return std::static_pointer_cast(shared_from_this()); } #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT -std::shared_ptr DetectionNetwork::build(const std::shared_ptr& input, NNModelDescription modelDesc, std::optional fps) { - auto nnArchive = createNNArchive(modelDesc); - return build(input, nnArchive, fps); -} -std::shared_ptr DetectionNetwork::build(const std::shared_ptr& input, const NNArchive& nnArchive, std::optional fps) { - neuralNetwork->build(input, nnArchive, fps); - detectionParser->setNNArchive(nnArchive); +std::shared_ptr DetectionNetwork::build(const std::shared_ptr& input, const Model& model, std::optional fps) { + neuralNetwork->build(input, model, fps); + auto nnArchive = neuralNetwork->getNNArchive(); + DAI_CHECK(nnArchive.has_value(), "NeuralNetwork NNArchive is not set after build."); + detectionParser->setNNArchive(*nnArchive); return std::static_pointer_cast(shared_from_this()); } #endif diff --git a/src/pipeline/node/NeuralNetwork.cpp b/src/pipeline/node/NeuralNetwork.cpp index f6d637583a..507aba9f81 100644 --- a/src/pipeline/node/NeuralNetwork.cpp +++ b/src/pipeline/node/NeuralNetwork.cpp @@ -1,5 +1,6 @@ #include "depthai/pipeline/node/NeuralNetwork.hpp" +#include #include #include #include @@ -26,28 +27,19 @@ std::shared_ptr NeuralNetwork::build(Node::Output& input, const N } std::shared_ptr NeuralNetwork::build(const std::shared_ptr& input, - NNModelDescription modelDesc, + const Model& model, std::optional fps, std::optional resizeMode) { - // Download model from zoo - if(modelDesc.platform.empty()) { - DAI_CHECK(getDevice() != nullptr, "Device is not set."); - modelDesc.platform = getDevice()->getPlatformAsString(); - } - auto path = getModelFromZoo(modelDesc); - auto modelType = dai::model::readModelType(path); - DAI_CHECK(modelType == dai::model::ModelType::NNARCHIVE, - "Model from zoo is not NNArchive - it needs to be a NNArchive to use build(Camera, NNModelDescription, float) method"); - auto nnArchive = dai::NNArchive(path); - return build(input, nnArchive, fps, resizeMode); + ImgFrameCapability cap; + if(fps.has_value()) cap.fps.value = *fps; + if(resizeMode.has_value()) cap.resizeMode = *resizeMode; + + return build(input, model, cap); } -std::shared_ptr NeuralNetwork::build(const std::shared_ptr& input, - NNArchive nnArchive, - std::optional fps, - std::optional resizeMode) { - setNNArchive(nnArchive); - auto cap = getFrameCapability(nnArchive, fps, resizeMode); +std::shared_ptr NeuralNetwork::build(const std::shared_ptr& input, const Model& model, const ImgFrameCapability& capability) { + decodeModel(model); + ImgFrameCapability cap = getFrameCapability(*nnArchive, capability); auto* camInput = input->requestOutput(cap, false); DAI_CHECK_V(camInput != nullptr, "Camera does not have output with requested capabilities"); camInput->link(this->input); @@ -55,14 +47,12 @@ std::shared_ptr NeuralNetwork::build(const std::shared_ptr NeuralNetwork::build(const std::shared_ptr& input, NNModelDescription modelDesc, std::optional fps) { - auto nnArchive = createNNArchive(modelDesc); - return build(input, nnArchive, fps); -} +std::shared_ptr NeuralNetwork::build(const std::shared_ptr& input, const Model& model, std::optional fps) { + decodeModel(model); -std::shared_ptr NeuralNetwork::build(const std::shared_ptr& input, const NNArchive& nnArchive, std::optional fps) { - setNNArchive(nnArchive); - auto cap = getFrameCapability(nnArchive, fps, std::nullopt); + ImgFrameCapability cap; + if(fps.has_value()) cap.fps.value = *fps; + cap = getFrameCapability(*nnArchive, cap); input->setOutFrameType(cap.type.value()); if(fps.has_value()) { input->setFps(*fps); @@ -73,7 +63,25 @@ std::shared_ptr NeuralNetwork::build(const std::shared_ptr fps, std::optional resizeMode) { +void NeuralNetwork::decodeModel(const Model& model) { + std::optional nnArchive; + + if(const auto* s = std::get_if(&model)) { + NNModelDescription description; + description.model = *s; + nnArchive = createNNArchive(description); + } else if(const auto* desc = std::get_if(&model)) { + NNModelDescription tmpDesc = *desc; + nnArchive = createNNArchive(tmpDesc); + } else if(const auto* archive = std::get_if(&model)) { + nnArchive = *archive; + } + + DAI_CHECK_V(nnArchive.has_value(), "Unsupported model type passed to NeuralNetwork::build"); + setNNArchive(*nnArchive); +} + +ImgFrameCapability NeuralNetwork::getFrameCapability(const NNArchive& nnArchive, std::optional expectedCapability) { const auto& nnArchiveCfg = nnArchive.getVersionedConfig(); DAI_CHECK_V(nnArchiveCfg.getVersion() == NNArchiveConfigVersion::V1, "Only V1 configs are supported for NeuralNetwork.build method"); @@ -96,9 +104,7 @@ ImgFrameCapability NeuralNetwork::getFrameCapability(const NNArchive& nnArchive, auto inputType = configV1.model.inputs[0].preprocessing.daiType; if(inputType.has_value()) { auto convertedInputType = magic_enum::enum_cast(inputType.value()); - if(!convertedInputType.has_value()) { - DAI_CHECK_V(false, "Unsupported input type: {}", inputType.value()); - } + DAI_CHECK_V(convertedInputType.has_value(), "Unsupported input type: {}", inputType.value()); type = convertedInputType.value(); } else { if(platform == Platform::RVC2 || platform == Platform::RVC3) { @@ -110,12 +116,12 @@ ImgFrameCapability NeuralNetwork::getFrameCapability(const NNArchive& nnArchive, } } auto cap = ImgFrameCapability(); + if(expectedCapability.has_value()) { + cap = *expectedCapability; + } cap.size.value = std::pair(*inputWidth, *inputHeight); cap.type = type; - cap.fps.value = fps; - if(resizeMode.has_value()) { - cap.resizeMode = resizeMode.value(); - } + return cap; } diff --git a/src/pipeline/node/SpatialDetectionNetwork.cpp b/src/pipeline/node/SpatialDetectionNetwork.cpp index 199ecbc5df..dcf8039a8f 100644 --- a/src/pipeline/node/SpatialDetectionNetwork.cpp +++ b/src/pipeline/node/SpatialDetectionNetwork.cpp @@ -33,20 +33,26 @@ void SpatialDetectionNetwork::buildInternal() { std::shared_ptr SpatialDetectionNetwork::build(const std::shared_ptr& inputRgb, const DepthSource& depthSource, - NNModelDescription modelDesc, + const Model& model, std::optional fps, std::optional resizeMode) { - auto nnArchive = createNNArchive(modelDesc); - return build(inputRgb, depthSource, nnArchive, fps, resizeMode); + ImgFrameCapability cap; + if(fps.has_value()) cap.fps.value = *fps; + if(resizeMode.has_value()) cap.resizeMode = *resizeMode; + cap.enableUndistortion = true; // default for SpatialDetectionNetwork + return build(inputRgb, depthSource, model, cap); } std::shared_ptr SpatialDetectionNetwork::build(const std::shared_ptr& inputRgb, const DepthSource& depthSource, - const NNArchive& nnArchive, - std::optional fps, - std::optional resizeMode) { - neuralNetwork->build(inputRgb, nnArchive, fps, resizeMode); - detectionParser->setNNArchive(nnArchive); + const Model& model, + const ImgFrameCapability& capability) { + auto cap = capability; + if(!cap.enableUndistortion.has_value()) cap.enableUndistortion = true; + neuralNetwork->build(inputRgb, model, cap); + auto nnArchive = neuralNetwork->getNNArchive(); + DAI_CHECK(nnArchive.has_value(), "NeuralNetwork NNArchive is not set after build."); + detectionParser->setNNArchive(nnArchive.value()); alignDepth(depthSource, inputRgb); return std::static_pointer_cast(shared_from_this()); } From 21de83c4f83badc2b7f272afdbc1c680b2f87ae2 Mon Sep 17 00:00:00 2001 From: Jakob Socan Date: Fri, 16 Jan 2026 13:34:59 +0100 Subject: [PATCH 042/180] Added start/stop stream functionality and test on RVC4 --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- .../pipeline/node/camera_test.cpp | 51 +++++++++++++++++++ 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 1a8828d70a..edb8b685ee 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+da43540f9ace113917adc8b1b3559bd8bca724f4") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+a708e778b4aaae99b4c1c3c86f82ae8cb825d547") diff --git a/tests/src/ondevice_tests/pipeline/node/camera_test.cpp b/tests/src/ondevice_tests/pipeline/node/camera_test.cpp index 0a8e7a22e5..8416b22349 100644 --- a/tests/src/ondevice_tests/pipeline/node/camera_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/camera_test.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "depthai/capabilities/ImgFrameCapability.hpp" @@ -145,6 +146,56 @@ TEST_CASE("Multiple outputs") { } #endif +TEST_CASE("Camera start/stop stream") { + constexpr float K_FPS = 30.0f; + constexpr uint32_t K_REPORT_EVERY_N = 10; + const auto kReportTimeout = std::chrono::seconds(4); + + dai::Pipeline p; + auto camera = p.create()->build(); + camera->initialControl.setStopStreaming(); + auto* output = camera->requestFullResolutionOutput(dai::ImgFrame::Type::NV12, K_FPS); + REQUIRE(output != nullptr); + + auto benchmarkIn = p.create(); + output->link(benchmarkIn->input); + benchmarkIn->sendReportEveryNMessages(K_REPORT_EVERY_N); + auto reportQueue = benchmarkIn->report.createOutputQueue(); + auto controlQueue = camera->inputControl.createInputQueue(); + + auto waitForReport = [&](std::chrono::milliseconds timeout) { + bool timedOut = false; + auto report = reportQueue->get(timeout, timedOut); + return timedOut ? nullptr : report; + }; + + p.start(); + + auto startCtrl = std::make_shared(); + startCtrl->setStartStreaming(); + controlQueue->send(startCtrl); + + auto initialReport = waitForReport(kReportTimeout); + REQUIRE(initialReport != nullptr); + + while(reportQueue->tryGet() != nullptr) { + } + + auto stopCtrl = std::make_shared(); + stopCtrl->setStopStreaming(); + controlQueue->send(stopCtrl); + + auto next = waitForReport(kReportTimeout); + REQUIRE(next == nullptr); + + auto restartCtrl = std::make_shared(); + restartCtrl->setStartStreaming(); + controlQueue->send(restartCtrl); + + auto restartedReport = waitForReport(kReportTimeout); + REQUIRE(restartedReport != nullptr); +} + TEST_CASE("Test how default FPS is generated for a specific output") { constexpr float FPS_TO_SET = 20.0; // Create pipeline From 709429f8b5b5f3881176d7cada117febeaad201f Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 16 Jan 2026 14:31:44 +0100 Subject: [PATCH 043/180] Update the examples to use undistorted frames --- examples/cpp/RGBD/rgbd.cpp | 2 +- examples/cpp/RVC2/ImageAlign/image_align.cpp | 4 ++-- examples/cpp/RVC2/Thermal/thermal_align.cpp | 2 +- examples/python/RGBD/rgbd.py | 2 +- examples/python/RGBD/rgbd_o3d.py | 2 +- examples/python/RVC2/Thermal/thermal_align.py | 10 ++-------- .../img_transformation_test.cpp | 19 +++++++++++++++++++ tests/src/ondevice_tests/rgbd_test.cpp | 2 +- 8 files changed, 28 insertions(+), 15 deletions(-) diff --git a/examples/cpp/RGBD/rgbd.cpp b/examples/cpp/RGBD/rgbd.cpp index d4f73f2734..f8879ebfa3 100644 --- a/examples/cpp/RGBD/rgbd.cpp +++ b/examples/cpp/RGBD/rgbd.cpp @@ -71,7 +71,7 @@ int main() { auto platform = pipeline.getDefaultDevice()->getPlatform(); if(platform == dai::Platform::RVC4) { - auto* out = color->requestOutput(std::pair(640, 400), dai::ImgFrame::Type::RGB888i); + auto* out = color->requestOutput(std::pair(640, 400), dai::ImgFrame::Type::RGB888i, dai::ImgResizeMode::CROP, std::nullopt, true); out->link(rgbd->inColor); align = pipeline.create(); stereo->depth.link(align->input); diff --git a/examples/cpp/RVC2/ImageAlign/image_align.cpp b/examples/cpp/RVC2/ImageAlign/image_align.cpp index 94abcbc099..5804134bcd 100644 --- a/examples/cpp/RVC2/ImageAlign/image_align.cpp +++ b/examples/cpp/RVC2/ImageAlign/image_align.cpp @@ -50,7 +50,7 @@ int main() { sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); align->outputAligned.link(sync->inputs["aligned"]); - auto* RGBOutput = camRgb->requestOutput(std::make_pair(640, 400)); + auto* RGBOutput = camRgb->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, std::nullopt, true); RGBOutput->link(align->inputAlignTo); RGBOutput->link(sync->inputs["rgb"]); left->requestOutput(std::make_pair(640, 400))->link(align->input); @@ -102,4 +102,4 @@ int main() { } return 0; -} \ No newline at end of file +} diff --git a/examples/cpp/RVC2/Thermal/thermal_align.cpp b/examples/cpp/RVC2/Thermal/thermal_align.cpp index ba6c80e8c7..cb0ef8521e 100644 --- a/examples/cpp/RVC2/Thermal/thermal_align.cpp +++ b/examples/cpp/RVC2/Thermal/thermal_align.cpp @@ -89,7 +89,7 @@ int main() { auto sync = pipeline.create(); auto align = pipeline.create(); - auto camRgbOut = camRgb->requestOutput(COLOR_RESOLUTION, std::nullopt, dai::ImgResizeMode::CROP, FPS); + auto camRgbOut = camRgb->requestOutput(COLOR_RESOLUTION, std::nullopt, dai::ImgResizeMode::CROP, FPS, true); sync->setSyncThreshold(std::chrono::milliseconds((unsigned int)(3000.f / FPS))); diff --git a/examples/python/RGBD/rgbd.py b/examples/python/RGBD/rgbd.py index 5e15d796d6..1b9985fda0 100644 --- a/examples/python/RGBD/rgbd.py +++ b/examples/python/RGBD/rgbd.py @@ -59,7 +59,7 @@ def run(self): platform = p.getDefaultDevice().getPlatform() if platform == dai.Platform.RVC4: - out = color.requestOutput((640,400), dai.ImgFrame.Type.RGB888i) + out = color.requestOutput((640,400), dai.ImgFrame.Type.RGB888i, enableUndistortion=True) align = p.create(dai.node.ImageAlign) stereo.depth.link(align.input) out.link(align.inputAlignTo) diff --git a/examples/python/RGBD/rgbd_o3d.py b/examples/python/RGBD/rgbd_o3d.py index 5ba5140155..749f99bb51 100644 --- a/examples/python/RGBD/rgbd_o3d.py +++ b/examples/python/RGBD/rgbd_o3d.py @@ -80,7 +80,7 @@ def key_callback(vis, action, mods): right.requestOutput((640, 400)).link(stereo.right) platform = p.getDefaultDevice().getPlatform() if platform == dai.Platform.RVC4: - out = color.requestOutput((640, 400), dai.ImgFrame.Type.RGB888i) + out = color.requestOutput((640, 400), dai.ImgFrame.Type.RGB888i, enableUndistortion=True) align = p.create(dai.node.ImageAlign) stereo.depth.link(align.input) out.link(align.inputAlignTo) diff --git a/examples/python/RVC2/Thermal/thermal_align.py b/examples/python/RVC2/Thermal/thermal_align.py index d49304fde2..d16800ad74 100644 --- a/examples/python/RVC2/Thermal/thermal_align.py +++ b/examples/python/RVC2/Thermal/thermal_align.py @@ -65,7 +65,7 @@ def getFps(self): sync = pipeline.create(dai.node.Sync) align = pipeline.create(dai.node.ImageAlign) - camRgbOut = camRgb.requestOutput(COLOR_RESOLUTION, fps=FPS) + camRgbOut = camRgb.requestOutput(COLOR_RESOLUTION, fps=FPS, enableUndistortion=True) sync.setSyncThreshold(timedelta(seconds=3 / FPS)) @@ -134,12 +134,6 @@ def updateDepthPlane(depth): rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(frameRgbCv.shape[1]), int(frameRgbCv.shape[0])) - cvFrameUndistorted = cv2.undistort( - frameRgbCv, - np.array(rgbIntrinsics), - np.array(rgbDistortion), - ) - # Colorize the aligned depth thermalFrame = thermalAligned.getData().view(np.float16).reshape((thermalAligned.getHeight(), thermalAligned.getWidth())).astype(np.float32) # Create a mask for nan values @@ -151,7 +145,7 @@ def updateDepthPlane(depth): # Apply the mask back with black pixels (0) colormappedFrame[mask] = 0 - blended = cv2.addWeighted(cvFrameUndistorted, rgbWeight, colormappedFrame, thermalWeight, 0) + blended = cv2.addWeighted(frameRgbCv, rgbWeight, colormappedFrame, thermalWeight, 0) cv2.putText( blended, diff --git a/tests/src/ondevice_tests/img_transformation_test.cpp b/tests/src/ondevice_tests/img_transformation_test.cpp index 84b0aad1b5..3aee47fdb5 100644 --- a/tests/src/ondevice_tests/img_transformation_test.cpp +++ b/tests/src/ondevice_tests/img_transformation_test.cpp @@ -261,3 +261,22 @@ TEST_CASE("ImgTransformation matrix inverse consistency (ImgFrame)") { REQUIRE(approxIdentity(I3)); REQUIRE(approxIdentity(I4)); } + +// ----------------------------------------------------------------------------- +// ImgTransformation isAlignedTo distortion coefficients handling +// Purpose: +// Ensures isAlignedTo treats missing distortion coefficients as zeros while +// still detecting real mismatches. +// ----------------------------------------------------------------------------- +TEST_CASE("ImgTransformation isAlignedTo distortion coefficients handling") { + dai::ImgTransformation base(640, 480); + dai::ImgTransformation zeros(640, 480); + dai::ImgTransformation nonZero(640, 480); + + base.setDistortionCoefficients({}); + zeros.setDistortionCoefficients({0.0f, 0.0f, 0.0f, 0.0f, 0.0f}); + nonZero.setDistortionCoefficients({0.0f, 0.0f, 0.0f, 0.0f, 0.01f}); + + REQUIRE(base.isAlignedTo(zeros)); + REQUIRE_FALSE(base.isAlignedTo(nonZero)); +} diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp index 57752b97b9..1161638e6a 100644 --- a/tests/src/ondevice_tests/rgbd_test.cpp +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -24,7 +24,7 @@ TEST_CASE("basic rgbd") { right->build(dai::CameraBoardSocket::CAM_C); stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); - auto* out = color->requestOutput(std::pair(1280, 800), dai::ImgFrame::Type::RGB888i); + auto* out = color->requestOutput(std::pair(1280, 800), dai::ImgFrame::Type::RGB888i, dai::ImgResizeMode::CROP, std::nullopt, true); left->requestOutput(std::pair(1280, 800))->link(stereo->left); right->requestOutput(std::pair(1280, 800))->link(stereo->right); From 87c303b08427964c2cb8af336c5dbff7d6060ff7 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 16 Jan 2026 14:32:33 +0100 Subject: [PATCH 044/180] Improve `isAlignedTo` in ImgTransformations --- src/pipeline/datatype/ImgTransformations.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/pipeline/datatype/ImgTransformations.cpp b/src/pipeline/datatype/ImgTransformations.cpp index 69e9fcaeb4..8a2b4200ca 100644 --- a/src/pipeline/datatype/ImgTransformations.cpp +++ b/src/pipeline/datatype/ImgTransformations.cpp @@ -2,6 +2,8 @@ #include +#include +#include #include #include "depthai/utility/ImageManipImpl.hpp" @@ -466,8 +468,11 @@ bool ImgTransformation::isAlignedTo(const ImgTransformation& to) const { return std::abs(a - b) <= (absTol + relTol * std::max(std::abs(a), std::abs(b))); }; - for(size_t i = 0; i < distortionCoefficients.size(); ++i) { - if(!approxEqual(distortionCoefficients[i], to.distortionCoefficients[i])) { + const size_t maxCoeffCount = std::max(distortionCoefficients.size(), to.distortionCoefficients.size()); + for(size_t i = 0; i < maxCoeffCount; ++i) { + const float lhs = (i < distortionCoefficients.size()) ? distortionCoefficients[i] : 0.0f; + const float rhs = (i < to.distortionCoefficients.size()) ? to.distortionCoefficients[i] : 0.0f; + if(!approxEqual(lhs, rhs)) { return false; } } From 76d44733335863cfdf55203cd1a9b6e9ed625b2c Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 16 Jan 2026 14:33:16 +0100 Subject: [PATCH 045/180] Undistort input stream if necessary --- src/pipeline/node/ImageAlign.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index 78295def47..4b6425c83d 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -200,6 +200,7 @@ void ImageAlign::run() { std::array, 3> depthSourceIntrinsics; std::array, 3> alignSourceIntrinsics; std::array, 4> depthToAlignExtrinsics; + std::vector depthDistortionCoefficients; dai::CameraBoardSocket alignFrom; dai::CameraBoardSocket alignTo; @@ -243,7 +244,9 @@ void ImageAlign::run() { auto extractCalibrationData = [&](int depthWidth, int depthHeight, int alignWidth, int alignHeight) { if(calibrationSet) return; - auto depthDistortionCoefficients = std::vector(14, 0.0f); + if(depthDistortionCoefficients.empty()) { + depthDistortionCoefficients.assign(14, 0.0f); + } auto alignDistortionCoefficients = calibHandler.getDistortionCoefficients(alignTo); auto depthToAlignRotation = calibHandler.getCameraRotationMatrix(alignFrom, alignTo); @@ -256,22 +259,21 @@ void ImageAlign::run() { auto cv_M1 = arrayToCvMat(3, 3, CV_32FC1, depthSourceIntrinsics); auto cv_M2 = arrayToCvMat(3, 3, CV_32FC1, alignSourceIntrinsics); - auto cv_dNone = vecToCvMat(1, depthDistortionCoefficients.size(), CV_32FC1, depthDistortionCoefficients); - auto cv_d2 = vecToCvMat(1, alignDistortionCoefficients.size(), CV_32FC1, alignDistortionCoefficients); - + auto cv_d1 = vecToCvMat(1, depthDistortionCoefficients.size(), CV_32FC1, depthDistortionCoefficients); + auto cv_dNone = vecToCvMat(1, alignDistortionCoefficients.size(), CV_32FC1, std::vector{0.0f}); // No distortion for aligned frame auto cv_R = vecToCvMat(3, 3, CV_32FC1, depthToAlignRotation); auto cv_T = vecToCvMat(1, 3, CV_32FC1, depthToAlignTranslation); cv::Mat cv_R1, cv_R2; cv::Size imageSize = cv::Size(depthWidth, depthHeight); - std::tie(cv_R1, cv_R2) = computeRectificationMatrices(cv_M1, cv_dNone, cv_M2, cv_d2, imageSize, cv_R, cv_T); + std::tie(cv_R1, cv_R2) = computeRectificationMatrices(cv_M1, cv_d1, cv_M2, cv_dNone, imageSize, cv_R, cv_T); // dai::CameraModel cameraModel = dai::CameraModel::Perspective; // todo auto cv_targetCamMatrix = cv_M1.clone(); auto cv_meshSize = cv::Size(depthWidth, depthHeight); - cv::initUndistortRectifyMap(cv_M1, cv_dNone, cv_R1, cv_targetCamMatrix, cv_meshSize, CV_32FC1, map_x_1, map_y_1); + cv::initUndistortRectifyMap(cv_M1, cv_d1, cv_R1, cv_targetCamMatrix, cv_meshSize, CV_32FC1, map_x_1, map_y_1); cv::Mat cv_newR = cv_R2 * (cv_R * cv_R1.t()); cv::Mat cv_newT = cv_R2 * cv_T.t(); @@ -414,6 +416,7 @@ void ImageAlign::run() { inputFrameType = inputImg->getType(); depthSourceIntrinsics = inputImg->transformation.getIntrinsicMatrix(); + depthDistortionCoefficients = inputImg->transformation.getDistortionCoefficients(); if(alignFrom == alignTo) { logger->error("Cannot align image to itself (camera socket {}), possible misconfiguration, skipping frame!", (int)alignFrom); From b601c85943a68d679a44ab593bde17963d65230d Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 16 Jan 2026 15:44:25 +0100 Subject: [PATCH 046/180] Correctly track intrinsics when custom size is specified --- src/pipeline/node/ImageAlign.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index 4b6425c83d..fca7dc060b 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -382,13 +382,23 @@ void ImageAlign::run() { "aligned."); } - alignSourceIntrinsics = inputAlignToImg->transformation.getIntrinsicMatrix(); - alignTo = static_cast(inputAlignToImg->getInstanceNum()); if(alignWidth == 0 || alignHeight == 0) { alignWidth = inputAlignToImg->getWidth(); alignHeight = inputAlignToImg->getHeight(); } + + auto alignTransformForIntrinsics = inputAlignToTransform; + auto [alignTransformWidth, alignTransformHeight] = alignTransformForIntrinsics.getSize(); + if(static_cast(alignTransformWidth) != alignWidth || static_cast(alignTransformHeight) != alignHeight) { + float scaleX = static_cast(alignWidth) / static_cast(alignTransformWidth); + float scaleY = static_cast(alignHeight) / static_cast(alignTransformHeight); + alignTransformForIntrinsics.addScale(scaleX, scaleY); + alignTransformForIntrinsics.setSize(alignWidth, alignHeight); + } + + alignSourceIntrinsics = alignTransformForIntrinsics.getIntrinsicMatrix(); + inputAlignToTransform = alignTransformForIntrinsics; } if(inputConfig.getWaitForMessage()) { From 3c445004da7756631756768ddd0bc20d2cbcb391 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 16 Jan 2026 16:09:38 +0100 Subject: [PATCH 047/180] Bump FWs to latest --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 1a8828d70a..972dc29a62 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+da43540f9ace113917adc8b1b3559bd8bca724f4") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+02f99e415cbed3da470e4af4f847b79635b1fde5") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 7c5e3601ec..4dda41d9a4 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "ecdc7175a8932be3fd2c64be3f30a75afc616459") +set(DEPTHAI_DEVICE_SIDE_COMMIT "780ecf50d3f757a9cd2b0f42c7e063ecde54d6d2") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 588990ae894c4b6fd25f08c1c06a9a3c19aba633 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 16 Jan 2026 18:08:08 +0100 Subject: [PATCH 048/180] Address PR comments --- bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp | 1 + src/pipeline/node/ImageAlign.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp b/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp index ff98ff37f8..232dec2e71 100644 --- a/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp @@ -175,6 +175,7 @@ void bind_imgframe(pybind11::module& m, void* pCallstack) { .def("remapPointFrom", &ImgTransformation::remapPointFrom, py::arg("to"), py::arg("point"), DOC(dai, ImgTransformation, remapPointFrom)) .def("remapRectTo", &ImgTransformation::remapRectTo, py::arg("to"), py::arg("rect"), DOC(dai, ImgTransformation, remapRectTo)) .def("remapRectFrom", &ImgTransformation::remapRectFrom, py::arg("to"), py::arg("rect"), DOC(dai, ImgTransformation, remapRectFrom)) + .def("isAlignedTo", &ImgTransformation::isAlignedTo, py::arg("to"), DOC(dai, ImgTransformation, isAlignedTo)) .def("isValid", &ImgTransformation::isValid, DOC(dai, ImgTransformation, isValid)); // TODO add RawImgFrame::CameraSettings diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index fca7dc060b..94413e09bf 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -260,7 +260,8 @@ void ImageAlign::run() { auto cv_M2 = arrayToCvMat(3, 3, CV_32FC1, alignSourceIntrinsics); auto cv_d1 = vecToCvMat(1, depthDistortionCoefficients.size(), CV_32FC1, depthDistortionCoefficients); - auto cv_dNone = vecToCvMat(1, alignDistortionCoefficients.size(), CV_32FC1, std::vector{0.0f}); // No distortion for aligned frame + auto cv_dNone = vecToCvMat( + 1, alignDistortionCoefficients.size(), CV_32FC1, std::vector(alignDistortionCoefficients.size(), 0.0f)); // No distortion for aligned frame auto cv_R = vecToCvMat(3, 3, CV_32FC1, depthToAlignRotation); auto cv_T = vecToCvMat(1, 3, CV_32FC1, depthToAlignTranslation); From 3f3c60d64b7492e16e98b78e8cd1b62da90f03a5 Mon Sep 17 00:00:00 2001 From: aljazkonec1 <53098513+aljazkonec1@users.noreply.github.com> Date: Mon, 19 Jan 2026 10:18:17 +0100 Subject: [PATCH 049/180] Feat: Improve docstrings and pybindings (#1630) * Update docstrings and bindings * update RotatedRect, Rect, Points3fRGBA, Point3d and Quarterniond --- .../python/src/pipeline/CommonBindings.cpp | 38 +++++++++++++------ .../pipeline/node/DetectionParserBindings.cpp | 2 +- include/depthai/common/Point3fRGBA.hpp | 2 +- include/depthai/common/Rect.hpp | 38 +++++++++++++++++++ include/depthai/common/RotatedRect.hpp | 10 ++++- .../pipeline/datatype/ImgDetectionsT.hpp | 12 +++--- .../depthai/pipeline/node/DetectionParser.hpp | 7 +++- include/depthai/pipeline/node/IMU.hpp | 2 +- .../properties/ColorCameraProperties.hpp | 4 +- include/depthai/properties/IMUProperties.hpp | 2 +- .../properties/MonoCameraProperties.hpp | 2 +- .../properties/StereoDepthProperties.hpp | 2 +- .../DetectionParser/DetectionParserUtils.hpp | 8 ++-- 13 files changed, 96 insertions(+), 33 deletions(-) diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 0e9c9d7e0d..4bfcc4971d 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -169,8 +169,8 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def("getPoints2f", &KeypointsList::getPoints2f, DOC(dai, KeypointsListT, getPoints2f)); rotatedRect.def(py::init<>()) - .def(py::init()) - .def(py::init()) + .def(py::init(), py::arg("center"), py::arg("size"), py::arg("angle"), DOC(dai, RotatedRect, RotatedRect)) + .def(py::init(), py::arg("rect"), py::arg("angle") = 0.f, DOC(dai, RotatedRect, RotatedRect)) .def_readwrite("center", &RotatedRect::center) .def_readwrite("size", &RotatedRect::size) .def_readwrite("angle", &RotatedRect::angle) @@ -183,12 +183,18 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def("getOuterCXCYWH", &RotatedRect::getOuterCXCYWH, DOC(dai, RotatedRect, getOuterCXCYWH)); rect.def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init()) + .def(py::init(), py::arg("x"), py::arg("y"), py::arg("width"), py::arg("height"), DOC(dai, Rect, Rect)) + .def(py::init(), + py::arg("x"), + py::arg("y"), + py::arg("width"), + py::arg("height"), + py::arg("normalized"), + DOC(dai, Rect, Rect, 2)) + .def(py::init(), py::arg("top_left"), py::arg("size"), DOC(dai, Rect, Rect, 4)) + .def(py::init(), py::arg("top_left"), py::arg("size"), py::arg("normalized"), DOC(dai, Rect, Rect, 5)) + .def(py::init(), py::arg("point_1"), py::arg("point_2"), DOC(dai, Rect, Rect, 6)) + .def(py::init(), py::arg("point_1"), py::arg("point_2"), py::arg("normalized"), DOC(dai, Rect, Rect, 7)) .def("topLeft", &Rect::topLeft, DOC(dai, Rect, topLeft)) .def("bottomRight", &Rect::bottomRight, DOC(dai, Rect, bottomRight)) @@ -225,12 +231,20 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def("isNormalized", &Point2f::isNormalized); point3f.def(py::init<>()) - .def(py::init()) + .def(py::init(), py::arg("x"), py::arg("y"), py::arg("z"), DOC(dai, Point3f, Point3f)) .def_readwrite("x", &Point3f::x) .def_readwrite("y", &Point3f::y) .def_readwrite("z", &Point3f::z); point3fRGBA.def(py::init<>()) - .def(py::init()) + .def(py::init(), + py::arg("x"), + py::arg("y"), + py::arg("z"), + py::arg("r"), + py::arg("g"), + py::arg("b"), + py::arg("a") = 255, + DOC(dai, Point3fRGBA, Point3fRGBA)) .def_readwrite("x", &Point3fRGBA::x) .def_readwrite("y", &Point3fRGBA::y) .def_readwrite("z", &Point3fRGBA::z) @@ -239,12 +253,12 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def_readwrite("b", &Point3fRGBA::b) .def_readwrite("a", &Point3fRGBA::a); point3d.def(py::init<>()) - .def(py::init()) + .def(py::init(), py::arg("x"), py::arg("y"), py::arg("z"), DOC(dai, Point3d, Point3d)) .def_readwrite("x", &Point3d::x) .def_readwrite("y", &Point3d::y) .def_readwrite("z", &Point3d::z); quaterniond.def(py::init<>()) - .def(py::init()) + .def(py::init(), py::arg("qx"), py::arg("qy"), py::arg("qz"), py::arg("qw"), DOC(dai, Quaterniond, Quaterniond)) .def_readwrite("qx", &Quaterniond::qx) .def_readwrite("qy", &Quaterniond::qy) .def_readwrite("qz", &Quaterniond::qz) diff --git a/bindings/python/src/pipeline/node/DetectionParserBindings.cpp b/bindings/python/src/pipeline/node/DetectionParserBindings.cpp index 05fb9d2f69..78a697c3e0 100644 --- a/bindings/python/src/pipeline/node/DetectionParserBindings.cpp +++ b/bindings/python/src/pipeline/node/DetectionParserBindings.cpp @@ -85,6 +85,6 @@ void bind_detectionparser(pybind11::module& m, void* pCallstack) { .def("getDecodeKeypoints", &DetectionParser::getDecodeKeypoints, DOC(dai, node, DetectionParser, getDecodeKeypoints)) .def("getDecodeSegmentation", &DetectionParser::getDecodeSegmentation, DOC(dai, node, DetectionParser, getDecodeSegmentation)) .def("getStrides", &DetectionParser::getStrides, DOC(dai, node, DetectionParser, getStrides)) - .def("build", &DetectionParser::build, DOC(dai, node, DetectionParser, build)); + .def("build", &DetectionParser::build, py::arg("input"), py::arg("nnArchive"), DOC(dai, node, DetectionParser, build)); daiNodeModule.attr("DetectionParser").attr("Properties") = detectionParserProperties; } diff --git a/include/depthai/common/Point3fRGBA.hpp b/include/depthai/common/Point3fRGBA.hpp index d539bf3e20..9e66951ed8 100644 --- a/include/depthai/common/Point3fRGBA.hpp +++ b/include/depthai/common/Point3fRGBA.hpp @@ -8,7 +8,7 @@ namespace dai { /** * Point3fRGBA structure * - * x,y,z coordinates and RGB color values that define a 3D point with color. + * x,y,z coordinates and RGB color values that define a 3D point with color and alpha. */ struct Point3fRGBA { Point3fRGBA() = default; diff --git a/include/depthai/common/Rect.hpp b/include/depthai/common/Rect.hpp index 23045047b7..80e09189ea 100644 --- a/include/depthai/common/Rect.hpp +++ b/include/depthai/common/Rect.hpp @@ -18,15 +18,53 @@ namespace dai { struct Rect { // default constructor Rect() = default; + /** + * Construct Rectangle from (x, y) coordinates of the top-left corner, width and height. + */ Rect(float x, float y, float width, float height) : x(x), y(y), width(width), height(height) {} + + /** + * Construct Rectangle from (x, y) coordinates of the top-left corner, width, height and normalization flag. + * @param x Top-left x coordinate + * @param y Top-left y coordinate + * @param width Width of the rectangle + * @param height Height of the rectangle + * @param normalized Whether the rectangle is normalized (coordinates in [0,1] range) or not + */ Rect(float x, float y, float width, float height, bool normalized) : x(x), y(y), width(width), height(height), normalized(normalized), hasNormalized(true) {} + + /** + * Copy constructor + */ Rect(const Rect& r) : x(r.x), y(r.y), width(r.width), height(r.height), normalized(r.normalized), hasNormalized(r.hasNormalized) {} + + /** + * Construct Rectangle from top left point and size + */ Rect(const Point2f& org, const Size2f& sz) : x(org.x), y(org.y), width(sz.width), height(sz.height) {} + + /** + * Construct Rectangle from top left point, size and normalization flag. + * @note: if normalized is true, the coordinates are in [0,1] range + */ Rect(const Point2f& org, const Size2f& sz, bool normalized) : x(org.x), y(org.y), width(sz.width), height(sz.height), normalized(normalized), hasNormalized(true) {} + + /** + * Construct Rectangle between any two given points. Constructor will determine top-left and bottom-right points. + * @param pt1 First point + * @param pt2 Second point + */ Rect(const Point2f& pt1, const Point2f& pt2) : x(std::min(pt1.x, pt2.x)), y(std::min(pt1.y, pt2.y)), width(std::max(pt1.x, pt2.x) - x), height(std::max(pt1.y, pt2.y) - y) {} + + /** + * Construct Rectangle between any two given points with normalization flag. Constructor will determine top-left and bottom-right points. + * @param pt1 First point + * @param pt2 Second point + * @param normalized Whether the rectangle is normalized (coordinates in [0,1] range) or not + */ Rect(const Point2f& pt1, const Point2f& pt2, bool normalized) : x(std::min(pt1.x, pt2.x)), y(std::min(pt1.y, pt2.y)), diff --git a/include/depthai/common/RotatedRect.hpp b/include/depthai/common/RotatedRect.hpp index 7e2ea36b68..55586f1897 100644 --- a/include/depthai/common/RotatedRect.hpp +++ b/include/depthai/common/RotatedRect.hpp @@ -19,11 +19,19 @@ struct RotatedRect { float angle = 0.f; RotatedRect() = default; + /** + * A rotated rectangle is specified by the center point, size, and rotation angle in degrees. + */ RotatedRect(const Point2f& center, const Size2f& size, float angle) : center(center), size(size), angle(angle) { if(size.isNormalized() != center.isNormalized()) { throw std::runtime_error("Cannot create RotatedRect with mixed normalization"); } } + /** + * Construct RotatedRect from provided dai::Rect rect and angle + * @param rect dai::Rect rectangle + * @param angle Rotation angle in degrees + */ RotatedRect(const Rect& rect, float angle = 0.f) : center(rect.x + rect.width / 2.0f, rect.y + rect.height / 2.0f, rect.isNormalized()), size(rect.width, rect.height, rect.isNormalized()), @@ -36,7 +44,7 @@ struct RotatedRect { bool isNormalized() const { if(size.isNormalized() != center.isNormalized()) { - throw std::runtime_error("Cannot denormalize RotatedRect with mixed normalization"); + throw std::runtime_error("Cannot determine normalization as size and center have mixed normalization."); } return size.isNormalized(); } diff --git a/include/depthai/pipeline/datatype/ImgDetectionsT.hpp b/include/depthai/pipeline/datatype/ImgDetectionsT.hpp index 99eb12cf75..885c516fb6 100644 --- a/include/depthai/pipeline/datatype/ImgDetectionsT.hpp +++ b/include/depthai/pipeline/datatype/ImgDetectionsT.hpp @@ -34,34 +34,34 @@ class ImgDetectionsT : public Buffer { * Common API */ - /* + /** * Returns the width of the segmentation mask. */ std::size_t getSegmentationMaskWidth() const; - /* + /** * Returns the height of the segmentation mask. */ std::size_t getSegmentationMaskHeight() const; - /* + /** * Sets the segmentation mask from a vector of bytes. * The size of the vector must be equal to width * height. */ void setSegmentationMask(const std::vector& mask, size_t width, size_t height); - /* + /** * Sets the segmentation mask from an ImgFrame. * @param frame Frame must be of type GRAY8 */ void setSegmentationMask(dai::ImgFrame& frame); - /* + /** * Returns a copy of the segmentation mask data as a vector of bytes. If mask data is not set, returns std::nullopt. */ std::optional> getMaskData() const; - /* + /** * Returns the segmentation mask as an ImgFrame. If mask data is not set, returns std::nullopt. */ std::optional getSegmentationMask() const; diff --git a/include/depthai/pipeline/node/DetectionParser.hpp b/include/depthai/pipeline/node/DetectionParser.hpp index 2a1af060a4..3bd8dbba0d 100644 --- a/include/depthai/pipeline/node/DetectionParser.hpp +++ b/include/depthai/pipeline/node/DetectionParser.hpp @@ -101,7 +101,7 @@ class DetectionParser : public DeviceNodeCRTP */ void setInputImageSize(std::tuple size); @@ -145,7 +145,7 @@ class DetectionParser : public DeviceNodeCRTP& classes); - /* + /** * Sets the number of coordinates per bounding box. * @param coordinates Number of coordinates. Default is 4 */ @@ -283,6 +283,9 @@ class DetectionParser : public DeviceNodeCRTP, public Source */ std::int32_t getMaxBatchReports() const; - /* + /** * Whether to perform firmware update or not. * Default value: false. */ diff --git a/include/depthai/properties/ColorCameraProperties.hpp b/include/depthai/properties/ColorCameraProperties.hpp index 2f8e52c678..2c7851bf63 100644 --- a/include/depthai/properties/ColorCameraProperties.hpp +++ b/include/depthai/properties/ColorCameraProperties.hpp @@ -77,7 +77,7 @@ struct ColorCameraProperties : PropertiesSerializable eventFilter = {dai::FrameEvent::READOUT_START}; diff --git a/include/depthai/properties/IMUProperties.hpp b/include/depthai/properties/IMUProperties.hpp index 92fc1a5256..9287526580 100644 --- a/include/depthai/properties/IMUProperties.hpp +++ b/include/depthai/properties/IMUProperties.hpp @@ -171,7 +171,7 @@ struct IMUProperties : PropertiesSerializable { std::int32_t batchReportThreshold = 1; /* Maximum number of IMU packets in a batch. Maximum 5. */ std::int32_t maxBatchReports = 5; - /* + /** * Whether to perform firmware update or not. * Default value: false. */ diff --git a/include/depthai/properties/MonoCameraProperties.hpp b/include/depthai/properties/MonoCameraProperties.hpp index c8f0ad6500..ce4ca9d80c 100644 --- a/include/depthai/properties/MonoCameraProperties.hpp +++ b/include/depthai/properties/MonoCameraProperties.hpp @@ -22,7 +22,7 @@ struct MonoCameraProperties : PropertiesSerializable labelName; }; -/* +/** Decode anchor free yolo v6r1 with sigmoid assisted center detection */ void decodeR1AF(const dai::NNData& nnData, @@ -25,7 +25,7 @@ void decodeR1AF(const dai::NNData& nnData, DetectionParserProperties& properties, std::shared_ptr& logger); -/* +/** Decode anchor based yolo v3 and v3-Tiny */ void decodeV3AB(const dai::NNData& nnData, @@ -33,7 +33,7 @@ void decodeV3AB(const dai::NNData& nnData, DetectionParserProperties& properties, std::shared_ptr& logger); -/* +/** Decode anchor based networks, e.g., yolo v5, v7, P */ void decodeV5AB(const dai::NNData& nnData, @@ -41,7 +41,7 @@ void decodeV5AB(const dai::NNData& nnData, DetectionParserProperties& properties, std::shared_ptr& logger); -/* +/** Decode anchor free top-left-bottom-right (TLBR) style networks, e.g., yolo v6r2, v8, v10, v11 */ void decodeTLBR(const dai::NNData& nnData, From a489804fa299e2ab0b5adfdaf5de960fa10ae91f Mon Sep 17 00:00:00 2001 From: Matic Tonin <77620080+MaticTonin@users.noreply.github.com> Date: Mon, 19 Jan 2026 10:36:47 +0100 Subject: [PATCH 050/180] Matrix getInverse to matrixOps (#1633) * Adding inverse calculation to matrixOps * Remove unnecesarry links and inverse in ImageManipImpl --- include/depthai/common/ImgTransformations.hpp | 7 ++- include/depthai/utility/ImageManipImpl.hpp | 11 ++-- include/depthai/utility/matrixOps.hpp | 5 +- src/pipeline/datatype/ImgTransformations.cpp | 44 +--------------- src/utility/ImageManipImpl.cpp | 46 +---------------- src/utility/matrixOps.cpp | 51 +++++++++++++++++++ .../image_transformations_test.cpp | 4 +- 7 files changed, 68 insertions(+), 100 deletions(-) diff --git a/include/depthai/common/ImgTransformations.hpp b/include/depthai/common/ImgTransformations.hpp index ffa3befa6f..50be0a8b67 100644 --- a/include/depthai/common/ImgTransformations.hpp +++ b/include/depthai/common/ImgTransformations.hpp @@ -4,11 +4,10 @@ #include "depthai/common/Point2f.hpp" #include "depthai/common/RotatedRect.hpp" #include "depthai/utility/Serialization.hpp" +#include "depthai/utility/matrixOps.hpp" namespace dai { -std::array, 3> getMatrixInverse(const std::array, 3>& matrix); - /** * ImgTransformation struct. Holds information of how a ImgFrame or related message was transformed from their source. Useful for remapping from one ImgFrame to * another. @@ -41,7 +40,7 @@ struct ImgTransformation { : srcWidth(srcWidth), srcHeight(srcHeight), width(width), height(height) {} ImgTransformation(size_t width, size_t height, std::array, 3> sourceIntrinsicMatrix) : sourceIntrinsicMatrix(sourceIntrinsicMatrix), srcWidth(width), srcHeight(height), width(width), height(height) { - sourceIntrinsicMatrixInv = getMatrixInverse(sourceIntrinsicMatrix); + sourceIntrinsicMatrixInv = matrix::getMatrixInverse(sourceIntrinsicMatrix); } ImgTransformation(size_t width, size_t height, @@ -55,7 +54,7 @@ struct ImgTransformation { srcHeight(height), width(width), height(height) { - sourceIntrinsicMatrixInv = getMatrixInverse(sourceIntrinsicMatrix); + sourceIntrinsicMatrixInv = matrix::getMatrixInverse(sourceIntrinsicMatrix); } /** diff --git a/include/depthai/utility/ImageManipImpl.hpp b/include/depthai/utility/ImageManipImpl.hpp index 2fb0f745a7..6045d28097 100644 --- a/include/depthai/utility/ImageManipImpl.hpp +++ b/include/depthai/utility/ImageManipImpl.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include "depthai/common/RotatedRect.hpp" @@ -2596,10 +2597,6 @@ std::tuple getOuterRect(const std::vector> getHull(const std::vector> points); -std::array, 2> getInverse(const std::array, 2> mat); - -std::array, 3> getInverse(const std::array, 3>& matrix); - dai::RotatedRect getOuterRotatedRect(const std::vector>& points); std::array, 3> getResizeMat(Resize o, float width, float height, uint32_t outputWidth, uint32_t outputHeight); @@ -2643,7 +2640,7 @@ std::tuple, 3>, std::array, auto [matrix, imageCorners, srcCorners] = getTransform(operations, inputWidth, inputHeight, base.outputWidth, base.outputHeight); - getOutputSizeFromCorners(imageCorners, base.center, getInverse(matrix), inputWidth, inputHeight, base.outputWidth, base.outputHeight); + getOutputSizeFromCorners(imageCorners, base.center, matrix::getMatrixInverse(matrix), inputWidth, inputHeight, base.outputWidth, base.outputHeight); if(base.resizeMode != ImageManipOpsBase::ResizeMode::NONE) { Resize res; @@ -2681,7 +2678,7 @@ std::tuple, 3>, std::array, outputOps.emplace_back(Translate(tx, ty)); } - auto matrixInv = getInverse(matrix); + auto matrixInv = matrix::getMatrixInverse(matrix); if(type == ImgFrame::Type::NV12 || type == ImgFrame::Type::YUV420p || outputFrameType == ImgFrame::Type::NV12 || outputFrameType == ImgFrame::Type::YUV420p) { @@ -2769,7 +2766,7 @@ ImageManipOperations& ImageManipO auto [matrix, imageCorners, _srcCorners] = getFullTransform(base, inputWidth, inputHeight, type, outputFrameType, outputOps); this->matrix = matrix; - this->matrixInv = getInverse(matrix); + this->matrixInv = matrix::getMatrixInverse(matrix); this->srcCorners = _srcCorners; if(logger) { diff --git a/include/depthai/utility/matrixOps.hpp b/include/depthai/utility/matrixOps.hpp index 324bcc6be0..705d52d41b 100644 --- a/include/depthai/utility/matrixOps.hpp +++ b/include/depthai/utility/matrixOps.hpp @@ -1,6 +1,7 @@ #pragma once #define _USE_MATH_DEFINES +#include #include #include #include @@ -10,6 +11,8 @@ namespace matrix { std::vector> matMul(std::vector>& firstMatrix, std::vector>& secondMatrix); bool matInv(std::vector>& A, std::vector>& inverse); +std::array, 2> getMatrixInverse(const std::array, 2>& matrix); +std::array, 3> getMatrixInverse(const std::array, 3>& matrix); std::vector> createRotationMatrix(float theta); std::vector> createScalingMatrix(float scaleX, float scaleY); std::vector> createTranslationMatrix(float dx, float dy); @@ -18,4 +21,4 @@ std::vector> rvecToRotationMatrix(const double rvec[3]); void printMatrix(std::vector>& matrix); } // namespace matrix -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/src/pipeline/datatype/ImgTransformations.cpp b/src/pipeline/datatype/ImgTransformations.cpp index 8a2b4200ca..b5a502dfb0 100644 --- a/src/pipeline/datatype/ImgTransformations.cpp +++ b/src/pipeline/datatype/ImgTransformations.cpp @@ -68,46 +68,6 @@ inline bool mateq(const std::array, 3>& A, const std::array return true; } -std::array, 3> getMatrixInverse(const std::array, 3>& matrixFloat) { - // Step 1: Convert to double - std::array, 3> matrix; - for(int i = 0; i < 3; ++i) - for(int j = 0; j < 3; ++j) matrix[i][j] = static_cast(matrixFloat[i][j]); - - std::array, 3> inv; - double det = matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]) - - matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]) - + matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]); - - if(det == 0) { - throw std::runtime_error("Matrix is singular and cannot be inverted."); - } - - std::array, 3> adj; - - adj[0][0] = (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]); - adj[0][1] = -(matrix[0][1] * matrix[2][2] - matrix[0][2] * matrix[2][1]); - adj[0][2] = (matrix[0][1] * matrix[1][2] - matrix[0][2] * matrix[1][1]); - - adj[1][0] = -(matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]); - adj[1][1] = (matrix[0][0] * matrix[2][2] - matrix[0][2] * matrix[2][0]); - adj[1][2] = -(matrix[0][0] * matrix[1][2] - matrix[0][2] * matrix[1][0]); - - adj[2][0] = (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]); - adj[2][1] = -(matrix[0][0] * matrix[2][1] - matrix[0][1] * matrix[2][0]); - adj[2][2] = (matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]); - - double invDet = 1.0 / det; - - for(int i = 0; i < 3; ++i) { - for(int j = 0; j < 3; ++j) { - inv[i][j] = static_cast(adj[i][j] * invDet); - } - } - - return inv; -} - dai::Point2f interSourceFrameTransform(dai::Point2f sourcePt, const ImgTransformation& from, const ImgTransformation& to) { auto fromSource = from.getSourceIntrinsicMatrix(); auto fromSourceInv = from.getSourceIntrinsicMatrixInv(); @@ -286,7 +246,7 @@ bool ImgTransformation::getDstMaskPt(size_t x, size_t y) { ImgTransformation& ImgTransformation::addTransformation(std::array, 3> matrix) { transformationMatrix = matmul(matrix, transformationMatrix); - transformationMatrixInv = getMatrixInverse(transformationMatrix); + transformationMatrixInv = matrix::getMatrixInverse(transformationMatrix); cropsValid = false; return *this; } @@ -376,7 +336,7 @@ ImgTransformation& ImgTransformation::setSourceSize(size_t width, size_t height) } ImgTransformation& ImgTransformation::setIntrinsicMatrix(std::array, 3> intrinsicMatrix) { sourceIntrinsicMatrix = intrinsicMatrix; - sourceIntrinsicMatrixInv = getMatrixInverse(intrinsicMatrix); + sourceIntrinsicMatrixInv = matrix::getMatrixInverse(intrinsicMatrix); return *this; } ImgTransformation& ImgTransformation::setDistortionModel(CameraModel model) { diff --git a/src/utility/ImageManipImpl.cpp b/src/utility/ImageManipImpl.cpp index 5e197e41dc..2d8b7f5265 100644 --- a/src/utility/ImageManipImpl.cpp +++ b/src/utility/ImageManipImpl.cpp @@ -4,6 +4,7 @@ #include "OCVPorts.hpp" #include "depthai/pipeline/datatype/ImageManipConfig.hpp" +#include "depthai/utility/matrixOps.hpp" #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT #include @@ -495,49 +496,6 @@ std::vector> dai::impl::getHull(const std::vector, 2> dai::impl::getInverse(const std::array, 2> mat) { - auto det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; - if(det == 0) { - throw std::runtime_error("Determinant is zero"); - } - return {{{mat[1][1] / det, -mat[0][1] / det}, {-mat[1][0] / det, mat[0][0] / det}}}; -} - -std::array, 3> dai::impl::getInverse(const std::array, 3>& matrix) { - std::array, 3> inv; - float det = matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]) - - matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]) - + matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]); - - if(det == 0) { - throw std::runtime_error("Matrix is singular and cannot be inverted."); - } - - std::array, 3> adj; - - adj[0][0] = (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]); - adj[0][1] = -(matrix[0][1] * matrix[2][2] - matrix[0][2] * matrix[2][1]); - adj[0][2] = (matrix[0][1] * matrix[1][2] - matrix[0][2] * matrix[1][1]); - - adj[1][0] = -(matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]); - adj[1][1] = (matrix[0][0] * matrix[2][2] - matrix[0][2] * matrix[2][0]); - adj[1][2] = -(matrix[0][0] * matrix[1][2] - matrix[0][2] * matrix[1][0]); - - adj[2][0] = (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]); - adj[2][1] = -(matrix[0][0] * matrix[2][1] - matrix[0][1] * matrix[2][0]); - adj[2][2] = (matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]); - - float invDet = 1.0f / det; - - for(int i = 0; i < 3; ++i) { - for(int j = 0; j < 3; ++j) { - inv[i][j] = adj[i][j] * invDet; - } - } - - return inv; -} - dai::RotatedRect dai::impl::getOuterRotatedRect(const std::vector>& points) { return utility::getOuterRotatedRect(points); } @@ -760,7 +718,7 @@ void dai::impl::getTransformImpl(const ManipOp& op, } imageCorners = {{{0, 0}, {(float)outputWidth, 0}, {(float)outputWidth, (float)outputHeight}, {0, (float)outputHeight}}}; - auto transformInv = getInverse(transform); + auto transformInv = matrix::getMatrixInverse(transform); srcCorners.push_back({matvecmul(transformInv, imageCorners[0]), matvecmul(transformInv, imageCorners[1]), matvecmul(transformInv, imageCorners[2]), diff --git a/src/utility/matrixOps.cpp b/src/utility/matrixOps.cpp index 7863485c74..714520bb1d 100644 --- a/src/utility/matrixOps.cpp +++ b/src/utility/matrixOps.cpp @@ -1,5 +1,8 @@ #include "depthai/utility/matrixOps.hpp" +#include +#include + namespace dai { namespace matrix { @@ -224,5 +227,53 @@ std::vector> rvecToRotationMatrix(const double rvec[3]) { return R; } +std::array, 2> getMatrixInverse(const std::array, 2>& matrix) { + auto det = matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]; + if(det == 0) { + throw std::runtime_error("Determinant is zero"); + } + return {{{matrix[1][1] / det, -matrix[0][1] / det}, {-matrix[1][0] / det, matrix[0][0] / det}}}; +} + +std::array, 3> getMatrixInverse(const std::array, 3>& matrix_float) { + // Step 1: Convert to double + std::array, 3> matrix; + for(int i = 0; i < 3; ++i) + for(int j = 0; j < 3; ++j) matrix[i][j] = static_cast(matrix_float[i][j]); + + std::array, 3> inv; + double det = matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]) + - matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]) + + matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]); + + if(det == 0) { + throw std::runtime_error("Matrix is singular and cannot be inverted."); + } + + std::array, 3> adj; + + adj[0][0] = (matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1]); + adj[0][1] = -(matrix[0][1] * matrix[2][2] - matrix[0][2] * matrix[2][1]); + adj[0][2] = (matrix[0][1] * matrix[1][2] - matrix[0][2] * matrix[1][1]); + + adj[1][0] = -(matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0]); + adj[1][1] = (matrix[0][0] * matrix[2][2] - matrix[0][2] * matrix[2][0]); + adj[1][2] = -(matrix[0][0] * matrix[1][2] - matrix[0][2] * matrix[1][0]); + + adj[2][0] = (matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0]); + adj[2][1] = -(matrix[0][0] * matrix[2][1] - matrix[0][1] * matrix[2][0]); + adj[2][2] = (matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]); + + double invDet = 1.0 / det; + + for(int i = 0; i < 3; ++i) { + for(int j = 0; j < 3; ++j) { + inv[i][j] = static_cast(adj[i][j] * invDet); + } + } + + return inv; +} + } // namespace matrix } // namespace dai diff --git a/tests/src/onhost_tests/image_transformations_test.cpp b/tests/src/onhost_tests/image_transformations_test.cpp index ff3a4d034f..162ebf9830 100644 --- a/tests/src/onhost_tests/image_transformations_test.cpp +++ b/tests/src/onhost_tests/image_transformations_test.cpp @@ -355,9 +355,9 @@ TEST_CASE("testRotation") { // ----------------------------------------------------------------------------- TEST_CASE("interFrameRemap") { std::array, 3> intr1 = {{{784.8082885742188, 0.0, 652.2084350585938}, {0.0, 786.7345581054688, 406.1820373535156}, {0.0, 0.0, 1.0}}}; - auto intr1inv = dai::impl::getInverse(intr1); + auto intr1inv = dai::matrix::getMatrixInverse(intr1); std::array, 3> intr2 = {{{3105.2021484375, 0.0, 1877.4822998046875}, {0.0, 3113.031494140625, 1128.080078125}, {0.0, 0.0, 1.0}}}; - auto intr2inv = dai::impl::getInverse(intr2); + auto intr2inv = dai::matrix::getMatrixInverse(intr2); auto tr1 = dai::ImgTransformation(1920, 1080, intr1); auto tr2 = dai::ImgTransformation(1280, 720, intr2); From 03c359a3fcea07e76fd99db9ffe612c809e2a6b0 Mon Sep 17 00:00:00 2001 From: Jakob Socan Date: Mon, 19 Jan 2026 12:51:07 +0100 Subject: [PATCH 051/180] Fix camera stream start/stop test on RVC2 --- tests/src/ondevice_tests/pipeline/node/camera_test.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tests/src/ondevice_tests/pipeline/node/camera_test.cpp b/tests/src/ondevice_tests/pipeline/node/camera_test.cpp index 8416b22349..5c2cbdb865 100644 --- a/tests/src/ondevice_tests/pipeline/node/camera_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/camera_test.cpp @@ -152,9 +152,12 @@ TEST_CASE("Camera start/stop stream") { const auto kReportTimeout = std::chrono::seconds(4); dai::Pipeline p; + auto isRvc4 = p.getDefaultDevice()->getPlatform() == dai::Platform::RVC4; + auto camera = p.create()->build(); camera->initialControl.setStopStreaming(); - auto* output = camera->requestFullResolutionOutput(dai::ImgFrame::Type::NV12, K_FPS); + auto* output = isRvc4 ? camera->requestFullResolutionOutput(dai::ImgFrame::Type::NV12, K_FPS) + : camera->requestOutput({1280, 800}, dai::ImgFrame::Type::NV12, dai::ImgResizeMode::CROP, K_FPS); // rvc2 start/stop fails on 4K REQUIRE(output != nullptr); auto benchmarkIn = p.create(); From 8123cadf77c5543cc094115fe4328953d7b85fc8 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Mon, 19 Jan 2026 13:46:13 +0100 Subject: [PATCH 052/180] Rename to small case --- bindings/python/src/pipeline/CommonBindings.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 4abb07aa86..dfdfedca40 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -75,8 +75,8 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::enum_ usbSpeed(m, "UsbSpeed", DOC(dai, UsbSpeed)); py::enum_ processorType(m, "ProcessorType"); py::enum_ detectionNetworkType(m, "DetectionNetworkType"); - py::enum_ LengthUnitEnum(m, "LengthUnit", DOC(dai, LengthUnit)); - LengthUnitEnum.value("METER", LengthUnit::METER) + py::enum_ lengthUnitEnum(m, "lengthUnit", DOC(dai, LengthUnit)); + lengthUnitEnum.value("METER", LengthUnit::METER) .value("CENTIMETER", LengthUnit::CENTIMETER) .value("MILLIMETER", LengthUnit::MILLIMETER) .value("INCH", LengthUnit::INCH) From 53287caa8ee02b2757cbf0974c1379b9ccc9f36b Mon Sep 17 00:00:00 2001 From: AljazD Date: Mon, 19 Jan 2026 15:09:10 +0100 Subject: [PATCH 053/180] Updated EventsMAnagerBindings --- bindings/python/src/utility/EventsManagerBindings.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index 14139a62e3..292d7b4fd9 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -83,7 +83,7 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { // py::arg("nnData"), // DOC(dai, utility, FileGroup, addImageNNDataPair)); - py::enum_ sendSnapCallbackStatus(m, "SendSnapCallbackStatus", DOC(dai, SendSnapCallbackStatus)); + py::enum_ sendSnapCallbackStatus(m, "SendSnapCallbackStatus", DOC(dai, utility, SendSnapCallbackStatus)); sendSnapCallbackStatus.value("SUCCESS", SendSnapCallbackStatus::SUCCESS) .value("FILE_BATCH_PREPARATION_FAILED", SendSnapCallbackStatus::FILE_BATCH_PREPARATION_FAILED) From ade77f188cae085c03fad2fa54c6f490262e722f Mon Sep 17 00:00:00 2001 From: asahtik <38485424+asahtik@users.noreply.github.com> Date: Mon, 19 Jan 2026 16:08:14 +0100 Subject: [PATCH 054/180] FPS calculation fixes for pipeline debugging (#1631) * RVC2 FW: Fix tryGet & blocking state * Run node_pipeline_events test for RVC4 only * Remove rvc4rgb tag from pipeline debugging examples * Add default value to member to silence warning * RVC2 FW: Fix non blocking queue state * RVC2 FW: Update core * Update node stats regularly, use window for fps * RVC2 FW: Update node states consistently, use time window for fps calculation * RVC4 FW: Update core * RVC2 FW: Merge develop * RVC2 FW: Use mutex per lambda * RVC2 FW: Update core * PR fixes * RVC2 FW: Merge develop * RVC4 FW: Merge develop * Clangformat --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/MessageQueue.hpp | 9 ++- src/pipeline/MessageQueue.cpp | 16 ++-- .../internal/PipelineEventAggregation.cpp | 75 +++++++++---------- tests/src/onhost_tests/message_queue_test.cpp | 14 ++++ .../pipeline_debugging_host_test.cpp | 56 +++++++++++++- 7 files changed, 120 insertions(+), 54 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 972dc29a62..8ee8f180ae 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+02f99e415cbed3da470e4af4f847b79635b1fde5") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+0cb92f48801b27b9493724e014bce9d3b5043752") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 4dda41d9a4..11141ac26b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "780ecf50d3f757a9cd2b0f42c7e063ecde54d6d2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "5c3cf9a7866498123cfec1db97b7fd3a097b036c") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/MessageQueue.hpp b/include/depthai/pipeline/MessageQueue.hpp index e3e37994bb..61cc2eed48 100644 --- a/include/depthai/pipeline/MessageQueue.hpp +++ b/include/depthai/pipeline/MessageQueue.hpp @@ -3,6 +3,7 @@ // std #include #include +#include #include // project @@ -87,10 +88,12 @@ class MessageQueue : public std::enable_shared_from_this { return *this; } - static std::unordered_map> getAny(std::unordered_map queues); + static std::unordered_map> getAny(std::unordered_map queues, + std::optional timeout = std::nullopt); template - static std::unordered_map> getAny(std::unordered_map queues) { - auto resultADatatype = getAny(queues); + static std::unordered_map> getAny(std::unordered_map queues, + std::optional timeout = std::nullopt) { + auto resultADatatype = getAny(std::move(queues), timeout); std::unordered_map> result; for(auto& [k, v] : resultADatatype) { auto casted = std::dynamic_pointer_cast(v); diff --git a/src/pipeline/MessageQueue.cpp b/src/pipeline/MessageQueue.cpp index 2a8c45f0e6..aa4f1d0758 100644 --- a/src/pipeline/MessageQueue.cpp +++ b/src/pipeline/MessageQueue.cpp @@ -215,7 +215,8 @@ void MessageQueue::notifyCondVars() { MessageQueue::QueueException::~QueueException() noexcept = default; -std::unordered_map> MessageQueue::getAny(std::unordered_map queues) { +std::unordered_map> MessageQueue::getAny(std::unordered_map queues, + std::optional timeout) { std::vector> condVarIds; condVarIds.reserve(queues.size()); std::unordered_map> inputs; @@ -228,7 +229,7 @@ std::unordered_map> MessageQueue::getAny for(auto& kv : queues) { auto& input = kv.second; auto condVarId = input.addCondVar(inputsWaitCv); - condVarIds.push_back({input, condVarId}); + condVarIds.emplace_back(input, condVarId); } // Check if any messages already present @@ -242,8 +243,7 @@ std::unordered_map> MessageQueue::getAny if(!hasAnyMessages) { // Wait for any message to arrive - std::unique_lock lock(inputsWaitMutex); - inputsWaitCv->wait(lock, [&]() { + auto pred = [&]() { for(auto& kv : queues) { if(kv.second.isClosed()) { return true; @@ -255,7 +255,13 @@ std::unordered_map> MessageQueue::getAny } } return false; - }); + }; + std::unique_lock lock(inputsWaitMutex); + if(timeout.has_value()) { + inputsWaitCv->wait_for(lock, timeout.value(), pred); + } else { + inputsWaitCv->wait(lock, pred); + } } // Remove condition variables diff --git a/src/pipeline/node/internal/PipelineEventAggregation.cpp b/src/pipeline/node/internal/PipelineEventAggregation.cpp index d90d775419..b9dce3fd82 100644 --- a/src/pipeline/node/internal/PipelineEventAggregation.cpp +++ b/src/pipeline/node/internal/PipelineEventAggregation.cpp @@ -288,45 +288,32 @@ class NodeEventAggregation { // Calculate and update FPS statistics from the given buffer inline void updateFpsStats(NodeState::Timing& timing, utility::CircularBuffer& buffer) { using namespace std::chrono; - const size_t delta = 9; - const float alpha = 0.1f; - const float expectedTimeFactor = 1.5f; - const float decayA = 2; - const float decayB = 10; - - if(buffer.size() < delta) return; - // If fps is zero, initialize it with the average fps over the buffer - if(timing.fps == 0.0f) { + const uint32_t windowSizeSeconds = 5; + const float alpha = 0.8; + + if(buffer.size() < 2) return; + auto offsetNow = steady_clock::now() - eventOffsetBuffer.getAverage(); + auto windowStart = offsetNow - seconds(windowSizeSeconds); + auto newFps = 0.0f; + if(buffer.first().time > windowStart) { + // All entries are within the window const auto& first = buffer.first(); const auto& last = buffer.last(); auto timeDiff = duration_cast(last.time - first.time).count() / (buffer.size() - 1); - timing.fps = 1e6f / (float)timeDiff; - } - // Consume fps entries, update state with exponential smoothing with a gaussian-ish decay - for(auto i = 0U; i < buffer.size() - delta; --i) { - auto& last1 = buffer.at(i + delta); - auto& last2 = buffer.at(i); - auto timeDiff = duration_cast(last1.time - last2.time).count() / delta; - auto newFps = 1e6f / (float)timeDiff; - timing.fps = timing.fps + alpha * (newFps - timing.fps); - } - auto nextExpectedTime = buffer.last().time + microseconds((long)(1e6 * (double)expectedTimeFactor / (double)timing.fps)); - auto offsetNow = steady_clock::now() - eventOffsetBuffer.getAverage(); - if(nextExpectedTime < offsetNow) { - // Decay fps towards 0 if we're late - float delay = (float)duration_cast(offsetNow - nextExpectedTime).count() / 1000.0f; - float decayFactor = exp(-powf(delay / decayA, 2) / decayB); - timing.fps *= decayFactor; - } - // Keep only the last delta-1 entries in the buffer - std::vector tail(delta - 1); - for(auto i = 0U; i < delta - 1; ++i) { - tail[i] = buffer.at(buffer.size() - delta + 1 + i); - } - buffer.clear(); - for(auto& v : tail) { - buffer.add(v); + newFps = 1e6f / (float)timeDiff; + } else { + // Count how many entries are within the window + size_t delta = 0; + for(auto rit = buffer.rbegin(); rit != buffer.rend(); ++rit) { + if(rit->time >= windowStart) { + delta++; + } else { + break; + } + } + newFps = (float)delta / (float)windowSizeSeconds; } + timing.fps = timing.fps + alpha * (newFps - timing.fps); } // Calculate and update queue size statistics inline void updateQueueStats(NodeState::QueueStats& queueStats, const utility::CircularBuffer& buffer) { @@ -389,14 +376,16 @@ class NodeEventAggregation { state.state = NodeState::State::PROCESSING; break; } - bool addedEvent = false; if(event.interval == PipelineEvent::Interval::NONE && event.type != PipelineEvent::Type::INPUT && event.type != PipelineEvent::Type::OUTPUT && event.status == PipelineEvent::Status::SUCCESS) { - addedEvent = updatePingBuffers(event); + updatePingBuffers(event); } else if(event.interval != PipelineEvent::Interval::NONE && event.status == PipelineEvent::Status::SUCCESS) { - addedEvent = updateIntervalBuffers(event); + updateIntervalBuffers(event); } - if(addedEvent && std::chrono::steady_clock::now() - lastUpdated >= std::chrono::milliseconds(statsUpdateIntervalMs)) { + } + + bool updateStats() { + if(std::chrono::steady_clock::now() - lastUpdated >= std::chrono::milliseconds(statsUpdateIntervalMs)) { lastUpdated = std::chrono::steady_clock::now(); // Update stats for all event types and sources for(int i = (int)PipelineEvent::Type::CUSTOM; i <= (int)PipelineEvent::Type::OUTPUT_BLOCK; ++i) { @@ -439,7 +428,9 @@ class NodeEventAggregation { } } updated = true; + return true; } + return false; } }; @@ -478,13 +469,14 @@ class PipelineEventHandler { std::unordered_map> events; try { // Wait for any events - auto msgs = MessageQueue::getAny(inputMap); + auto msgs = MessageQueue::getAny(inputMap, std::chrono::milliseconds(statsUpdateIntervalMs)); for(auto& [k, v] : msgs) { if(v != nullptr) { events[k] = v; } } } catch(const dai::MessageQueue::QueueException&) { + break; } // Process events for(auto& [k, event] : events) { @@ -494,6 +486,9 @@ class PipelineEventHandler { nodeStates.at(event->nodeId).add(*event); } } + for(auto& [_, nodeState] : nodeStates) { + nodeState.updateStats(); + } } } void run() { diff --git a/tests/src/onhost_tests/message_queue_test.cpp b/tests/src/onhost_tests/message_queue_test.cpp index 6616df0871..0a53074842 100644 --- a/tests/src/onhost_tests/message_queue_test.cpp +++ b/tests/src/onhost_tests/message_queue_test.cpp @@ -441,6 +441,20 @@ TEST_CASE("Get any async", "[MessageQueue]") { thread.join(); } +TEST_CASE("Get any timeout", "[MessageQueue]") { + MessageQueue queue1(10); + MessageQueue queue2(10); + MessageQueue queue3(10); + + std::unordered_map queues; + queues.insert_or_assign("queue1", queue1); + queues.insert_or_assign("queue2", queue2); + queues.insert_or_assign("queue3", queue3); + + auto out = MessageQueue::getAny(queues, std::chrono::milliseconds(1000)); + REQUIRE(out.size() == 0); +} + TEST_CASE("Pipeline event dispatcher tests", "[MessageQueue]") { class TestNode : public dai::node::CustomThreadedNode { public: diff --git a/tests/src/onhost_tests/pipeline_debugging_host_test.cpp b/tests/src/onhost_tests/pipeline_debugging_host_test.cpp index b4ab45279a..1877e9f6a0 100644 --- a/tests/src/onhost_tests/pipeline_debugging_host_test.cpp +++ b/tests/src/onhost_tests/pipeline_debugging_host_test.cpp @@ -25,7 +25,7 @@ class GeneratorNode : public node::CustomThreadedNode { public: Output output{*this, {"output", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; - Input ping{*this, {"_ping", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + Input ping{*this, {"_ping", DEFAULT_GROUP, true, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; Output ack{*this, {"_ack", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; void run() override { @@ -65,7 +65,7 @@ class ConsumerNode : public node::CustomThreadedNode { public: Input input{*this, {"input", DEFAULT_GROUP, true, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; - Input ping{*this, {"_ping", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + Input ping{*this, {"_ping", DEFAULT_GROUP, true, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; Output ack{*this, {"_ack", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; void run() override { @@ -107,7 +107,7 @@ class MapNode : public node::CustomThreadedNode { InputMap inputs{*this, "inputs", {DEFAULT_NAME, DEFAULT_GROUP, false, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; OutputMap outputs{*this, "outputs", {DEFAULT_NAME, DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; - Input ping{*this, {"_ping", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + Input ping{*this, {"_ping", DEFAULT_GROUP, true, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; Output ack{*this, {"_ack", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; void run() override { @@ -155,7 +155,7 @@ class BridgeNode : public node::CustomThreadedNode { Input input{*this, {"input", DEFAULT_GROUP, true, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; Output output{*this, {"output", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; - Input ping{*this, {"_ping", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + Input ping{*this, {"_ping", DEFAULT_GROUP, true, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; Output ack{*this, {"_ack", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; void run() override { @@ -475,6 +475,54 @@ TEST_CASE("Node timings test") { ph.stop(); } +TEST_CASE("FPS should go towards zero when pipeline is stopped") { + PipelineHandler ph; + ph.start(); + + // Let nodes run for a while + auto start = std::chrono::steady_clock::now(); + while(std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + for(const auto& nodeName : ph.getNodeNames()) { + ph.pingNoAck(nodeName, 0); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + auto state1 = ph.pipeline.getPipelineState().nodes().detailed(); + + std::this_thread::sleep_for(std::chrono::seconds(7)); + + auto state2 = ph.pipeline.getPipelineState().nodes().detailed(); + + for(const auto& nodeName : ph.getNodeNames()) { + auto nodeState1 = state1.nodeStates.at(ph.getNodeId(nodeName)); + auto nodeState2 = state2.nodeStates.at(ph.getNodeId(nodeName)); + + REQUIRE(nodeState1.mainLoopTiming.isValid()); + REQUIRE(nodeState2.mainLoopTiming.isValid()); + REQUIRE(nodeState1.mainLoopTiming.fps == Catch::Approx(10.f).margin(5.f)); + REQUIRE(nodeState2.mainLoopTiming.fps == Catch::Approx(0.f).margin(2.f)); + + for(const auto& [inputName, inputState1] : nodeState1.inputStates) { + if(inputName.rfind("_ping") != std::string::npos) continue; + auto inputState2 = nodeState2.inputStates.at(inputName); + REQUIRE(inputState1.timing.isValid()); + REQUIRE(inputState2.timing.isValid()); + REQUIRE(inputState1.timing.fps == Catch::Approx(10.f).margin(5.f)); + REQUIRE(inputState2.timing.fps == Catch::Approx(0.f).margin(2.f)); + } + for(const auto& [outputName, outputState1] : nodeState1.outputStates) { + if(outputName.rfind("_ack") != std::string::npos) continue; + auto outputState2 = nodeState2.outputStates.at(outputName); + REQUIRE(outputState1.timing.isValid()); + REQUIRE(outputState2.timing.isValid()); + REQUIRE(outputState1.timing.fps == Catch::Approx(10.f).margin(5.f)); + REQUIRE(outputState2.timing.fps == Catch::Approx(0.f).margin(2.f)); + } + } + ph.stop(); +} + TEST_CASE("Input duration test") { PipelineHandler ph(1); ph.start(); From af023b32f1cced52139b510c505de4d51fc7af28 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Mon, 19 Jan 2026 16:22:15 +0100 Subject: [PATCH 055/180] Typo fix --- src/device/CalibrationHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 6c754f883f..77bb4d6757 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -695,7 +695,7 @@ std::vector> CalibrationHandler::getHousingCalibration(Camera // cam_src → housing_origin → housing // Which gives us: cam_src → housing // ------------------------------------------------------------ - camToHousing = matMul(HousingToHousingOrigin, camToHousingOrigin); + camToHousing = matMul(housingToHousingOrigin, camToHousingOrigin); scaleTranslationInPlace(camToHousing, unit); return camToHousing; From 6ceda87a6e410dbc2d851fc543924c4e0d1c017e Mon Sep 17 00:00:00 2001 From: aljazkonec1 <53098513+aljazkonec1@users.noreply.github.com> Date: Mon, 19 Jan 2026 16:54:20 +0100 Subject: [PATCH 056/180] Feat: Update spatial location calculator (#1570) * Update CalculatorConfig message. * add spatial keypoint * add proto, imgDetectionsT * fw bump * change context of config attributes * add bindings * bindings * Change naming of useKeypoints * update bindings * Update bindings * Update spatial location calculator test * rvc2 fixes * move parser * fix bug with stepsize * Copy seg mask to spatial detections output * remove static caching * Add example and code cleanup * Formatting * Formatting * update logger * bump fw * move py example and add cpp example * Binding update * bump fw * bump fw * Add spatial keypoints example * Fix failing test bug * Fix spelling mistakes Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Default parameter in header * Implement suggested changes * Bump fw * Implement suggested changes * Fix test * Make unaligned optional * Update test and use transformation * more improvements * fix transformations * update to transformations * Update test and use transformation * Fix example and UB for running on host * Fix test and bump fw * Fix serialization, python example and formatting * bump fw * fix style * bump fw * Update examples * bump fw * Fix Windows build error * Fix test * wsin fixes * camelCase * CamelCase * move stepSize check to runtime * add labelName to spatialKeypoints * Simplify compute function * add skips and better nullptr handling * addTopPanel function * Update clang-tidy to work with modern versions * simplify compute function * Small clang-yidy implementations * extend isAlignedTo * Add const to arguments * Make calculation config private, update to pipeline graph support * Fix call headers * Function signature missmatch * Add ImgFramCapability arguument to build * clang-tidy improvements * Update examples with new build function definitions * Fix bindings * remove boost algo * update example and segmentationPassthrough fix * improve test * add neural depth option to example * fix fps * bump fw * bump fw * Bump fw and fix clang format --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Matevz Morato --- CMakeLists.txt | 1 + .../python/src/pipeline/CommonBindings.cpp | 67 ++ .../datatype/SpatialImgDetectionsBindings.cpp | 140 ++- ...patialLocationCalculatorConfigBindings.cpp | 34 +- .../SpatialLocationCalculatorBindings.cpp | 5 +- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- .../SpatialLocationCalculator/CMakeLists.txt | 3 + .../spatial_segmentation.cpp | 167 ++++ .../spatial_keypoints.py | 166 ++++ .../spatial_segmentation.py | 108 +++ include/depthai/common/SpatialKeypoint.hpp | 92 ++ .../pipeline/datatype/ImgDetections.hpp | 2 +- .../datatype/SpatialImgDetections.hpp | 168 +++- .../SpatialLocationCalculatorConfig.hpp | 147 ++- .../SpatialLocationCalculatorData.hpp | 1 + .../node/SpatialLocationCalculator.hpp | 45 +- protos/ImgDetections.proto | 2 +- protos/SpatialImgDetections.proto | 19 +- protos/common.proto | 14 + src/pipeline/datatype/ImgDetectionsT.cpp | 2 + src/pipeline/datatype/ImgTransformations.cpp | 1 + .../datatype/SpatialImgDetections.cpp | 158 ++++ .../SpatialLocationCalculatorConfig.cpp | 66 ++ src/pipeline/node/DetectionParser.cpp | 2 - src/pipeline/node/ObjectTracker.cpp | 2 +- .../node/SpatialLocationCalculator.cpp | 119 ++- .../SpatialUtils.cpp | 462 ++++++++++ .../SpatialUtils.hpp | 43 + src/utility/ProtoSerialize.cpp | 95 +- src/utility/ProtoSerialize.hpp | 2 +- .../img_transformation_test.cpp | 28 + .../node/spatial_location_calculator_test.cpp | 866 +++++++++++++++++- 33 files changed, 2866 insertions(+), 165 deletions(-) create mode 100644 examples/cpp/SpatialLocationCalculator/spatial_segmentation.cpp create mode 100644 examples/python/SpatialLocationCalculator/spatial_keypoints.py create mode 100644 examples/python/SpatialLocationCalculator/spatial_segmentation.py create mode 100644 include/depthai/common/SpatialKeypoint.hpp create mode 100644 src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp create mode 100644 src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index cfc47e1ee2..9780a73ff2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -315,6 +315,7 @@ set(TARGET_CORE_SOURCES src/pipeline/node/ToF.cpp src/pipeline/node/DetectionParser.cpp src/pipeline/utilities/DetectionParser/DetectionParserUtils.cpp + src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp src/pipeline/node/test/MyProducer.cpp src/pipeline/node/test/MyConsumer.cpp src/pipeline/node/UVC.cpp diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 4bfcc4971d..892754140f 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -35,6 +35,7 @@ #include "depthai/common/Rect.hpp" #include "depthai/common/RotatedRect.hpp" #include "depthai/common/Size2f.hpp" +#include "depthai/common/SpatialKeypoint.hpp" #include "depthai/common/StereoPair.hpp" #include "depthai/common/Timestamp.hpp" #include "depthai/common/UsbSpeed.hpp" @@ -97,6 +98,8 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::enum_ deviceModelZoo(m, "DeviceModelZoo", DOC(dai, DeviceModelZoo)); py::class_ keypoint(m, "Keypoint", DOC(dai, Keypoint)); py::class_ keypointsList(m, "KeypointsList", DOC(dai, KeypointsList)); + py::class_ spatialKeypoint(m, "SpatialKeypoint", DOC(dai, SpatialKeypoint)); + py::class_ spatialKeypointsList(m, "SpatialKeypointsList", DOC(dai, SpatialKeypointsList)); /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -168,6 +171,70 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def("getPoints3f", &KeypointsList::getPoints3f, DOC(dai, KeypointsListT, getPoints3f)) .def("getPoints2f", &KeypointsList::getPoints2f, DOC(dai, KeypointsListT, getPoints2f)); + spatialKeypoint.def(py::init<>()) + .def(py::init(), + py::arg("imageCoordinates"), + py::arg("spatialCoordinates") = Point3f{}, + py::arg("confidence") = -1.f, + py::arg("label") = 0, + py::arg("labelName") = "", + DOC(dai, SpatialKeypoint, SpatialKeypoint)) + .def(py::init(), + py::arg("imageCoordinates"), + py::arg("spatialCoordinates") = Point3f{}, + py::arg("confidence") = 0.f, + py::arg("label") = 0, + py::arg("labelName") = "", + DOC(dai, SpatialKeypoint, SpatialKeypoint)) + .def(py::init(), + py::arg("x"), + py::arg("y"), + py::arg("z"), + py::arg("spatialX"), + py::arg("spatialY"), + py::arg("spatialZ"), + py::arg("confidence") = 0.f, + py::arg("label") = 0, + py::arg("labelName") = "", + DOC(dai, SpatialKeypoint, SpatialKeypoint)) + .def_readwrite("imageCoordinates", &SpatialKeypoint::imageCoordinates, DOC(dai, SpatialKeypoint, imageCoordinates)) + .def_readwrite("spatialCoordinates", &SpatialKeypoint::spatialCoordinates, DOC(dai, SpatialKeypoint, spatialCoordinates)) + .def_readwrite("confidence", &SpatialKeypoint::confidence, DOC(dai, SpatialKeypoint, confidence)) + .def_readwrite("label", &SpatialKeypoint::label, DOC(dai, SpatialKeypoint, label)) + .def_readwrite("labelName", &SpatialKeypoint::labelName, DOC(dai, SpatialKeypoint, labelName)); + + spatialKeypointsList.def(py::init<>()) + .def(py::init, std::vector>(), py::arg("keypoints"), py::arg("edges"), DOC(dai, KeypointsListT, KeypointsListT)) + .def(py::init>(), py::arg("keypoints"), DOC(dai, KeypointsListT, KeypointsListT)) + .def( + "setKeypoints", + [](SpatialKeypointsList& self, const std::vector& kps) { self.Base::setKeypoints(kps); }, + py::arg("keypoints"), + DOC(dai, KeypointsListT, setKeypoints)) + .def("setKeypoints", + py::overload_cast&>(&SpatialKeypointsList::setKeypoints), + py::arg("keypoints"), + DOC(dai, KeypointsListT, setKeypoints)) + .def( + "setKeypoints", + [](SpatialKeypointsList& self, std::vector keypoints, std::vector edges) { + self.Base::setKeypoints(std::move(keypoints), std::move(edges)); + }, + py::arg("keypoints"), + py::arg("edges"), + DOC(dai, KeypointsListT, setKeypoints)) + .def( + "setEdges", [](SpatialKeypointsList& self, const std::vector& edges) { self.Base::setEdges(edges); }, py::arg("edges")) + .def("getKeypoints", &SpatialKeypointsList::getKeypoints, DOC(dai, KeypointsListT, getKeypoints)) + .def("getEdges", &SpatialKeypointsList::getEdges, DOC(dai, KeypointsListT, getEdges)) + .def("getPoints3f", &SpatialKeypointsList::getPoints3f, DOC(dai, KeypointsListT, getPoints3f)) + .def("getPoints2f", &SpatialKeypointsList::getPoints2f, DOC(dai, KeypointsListT, getPoints2f)) + .def("setSpatialCoordinates", + &SpatialKeypointsList::setSpatialCoordinates, + py::arg("spatialCoordinates"), + DOC(dai, SpatialKeypointsList, setSpatialCoordinates)) + .def("getSpatialCoordinates", &SpatialKeypointsList::getSpatialCoordinates, DOC(dai, SpatialKeypointsList, getSpatialCoordinates)); + rotatedRect.def(py::init<>()) .def(py::init(), py::arg("center"), py::arg("size"), py::arg("angle"), DOC(dai, RotatedRect, RotatedRect)) .def(py::init(), py::arg("rect"), py::arg("angle") = 0.f, DOC(dai, RotatedRect, RotatedRect)) diff --git a/bindings/python/src/pipeline/datatype/SpatialImgDetectionsBindings.cpp b/bindings/python/src/pipeline/datatype/SpatialImgDetectionsBindings.cpp index d89ec8cf25..0b63126f30 100644 --- a/bindings/python/src/pipeline/datatype/SpatialImgDetectionsBindings.cpp +++ b/bindings/python/src/pipeline/datatype/SpatialImgDetectionsBindings.cpp @@ -6,6 +6,7 @@ // depthai #include "depthai/pipeline/datatype/SpatialImgDetections.hpp" +#include "ndarray_converter.h" // pybind #include @@ -18,7 +19,7 @@ void bind_spatialimgdetections(pybind11::module& m, void* pCallstack) { // py::class_> rawSpatialImgDetections(m, "RawSpatialImgDetections", DOC(dai, // RawSpatialImgDetections)); - py::class_ spatialImgDetection(m, "SpatialImgDetection", DOC(dai, SpatialImgDetection)); + py::class_ spatialImgDetection(m, "SpatialImgDetection", DOC(dai, SpatialImgDetection)); py::class_, Buffer, std::shared_ptr> spatialImgDetections( m, "SpatialImgDetections", DOC(dai, SpatialImgDetections)); @@ -37,48 +38,127 @@ void bind_spatialimgdetections(pybind11::module& m, void* pCallstack) { // Metadata / raw spatialImgDetection.def(py::init<>()) + .def(py::init(), + py::arg("boundingBox"), + py::arg("spatialCoordinates"), + py::arg("confidence"), + py::arg("label"), + DOC(dai, SpatialImgDetection, SpatialImgDetection)) + .def(py::init(), + py::arg("boundingBox"), + py::arg("spatialCoordinates"), + py::arg("labelName"), + py::arg("confidence"), + py::arg("label"), + DOC(dai, SpatialImgDetection, SpatialImgDetection)) + .def(py::init(), + py::arg("boundingBox"), + py::arg("spatialCoordinates"), + py::arg("keypoints"), + py::arg("confidence"), + py::arg("label"), + DOC(dai, SpatialImgDetection, SpatialImgDetection)) + .def(py::init(), + py::arg("boundingBox"), + py::arg("spatialCoordinates"), + py::arg("keypoints"), + py::arg("labelName"), + py::arg("confidence"), + py::arg("label"), + DOC(dai, SpatialImgDetection, SpatialImgDetection)) + .def("__repr__", &SpatialImgDetection::str) + .def_readwrite("label", &SpatialImgDetection::label) + .def_readwrite("labelName", &SpatialImgDetection::labelName) + .def_readwrite("confidence", &SpatialImgDetection::confidence) + .def_readwrite("xmin", &SpatialImgDetection::xmin, "Deprecation warning: use boundingBox instead") + .def_readwrite("ymin", &SpatialImgDetection::ymin, "Deprecation warning: use boundingBox instead") + .def_readwrite("xmax", &SpatialImgDetection::xmax, "Deprecation warning: use boundingBox instead") + .def_readwrite("ymax", &SpatialImgDetection::ymax, "Deprecation warning: use boundingBox instead") .def_readwrite("spatialCoordinates", &SpatialImgDetection::spatialCoordinates) - .def_readwrite("boundingBoxMapping", &SpatialImgDetection::boundingBoxMapping); - - // rawSpatialImgDetections - // .def(py::init<>()) - // .def_readwrite("detections", &RawSpatialImgDetections::detections) - // .def_property("ts", - // [](const RawSpatialImgDetections& o){ - // double ts = o.ts.sec + o.ts.nsec / 1000000000.0; - // return ts; - // }, - // [](RawSpatialImgDetections& o, double ts){ - // o.ts.sec = ts; - // o.ts.nsec = (ts - o.ts.sec) * 1000000000.0; - // } - // ) - // .def_property("tsDevice", - // [](const RawSpatialImgDetections& o){ - // double ts = o.tsDevice.sec + o.tsDevice.nsec / 1000000000.0; - // return ts; - // }, - // [](RawSpatialImgDetections& o, double ts){ - // o.tsDevice.sec = ts; - // o.tsDevice.nsec = (ts - o.tsDevice.sec) * 1000000000.0; - // } - // ) - // .def_readwrite("sequenceNum", &RawSpatialImgDetections::sequenceNum) - // ; + .def_readwrite("boundingBoxMapping", &SpatialImgDetection::boundingBoxMapping) + .def_readwrite("boundingBox", &SpatialImgDetection::boundingBox) + .def("setBoundingBox", &SpatialImgDetection::setBoundingBox, py::arg("boundingBox"), DOC(dai, SpatialImgDetection, setBoundingBox)) + .def("getBoundingBox", &SpatialImgDetection::getBoundingBox, DOC(dai, SpatialImgDetection, getBoundingBox)) + .def("setOuterBoundingBox", + &SpatialImgDetection::setOuterBoundingBox, + py::arg("xmin"), + py::arg("ymin"), + py::arg("xmax"), + py::arg("ymax"), + DOC(dai, SpatialImgDetection, setOuterBoundingBox)) + .def("setKeypoints", + py::overload_cast(&SpatialImgDetection::setKeypoints), + py::arg("keypoints"), + DOC(dai, SpatialImgDetection, setKeypoints)) + .def("setKeypoints", + py::overload_cast>(&SpatialImgDetection::setKeypoints), + py::arg("keypoints"), + DOC(dai, SpatialImgDetection, setKeypoints)) + .def("setKeypoints", + py::overload_cast, const std::vector>(&SpatialImgDetection::setKeypoints), + py::arg("keypoints"), + py::arg("edges"), + DOC(dai, SpatialImgDetection, setKeypoints)) + .def("setKeypoints", + py::overload_cast>(&SpatialImgDetection::setKeypoints), + py::arg("keypoints"), + DOC(dai, SpatialImgDetection, setKeypoints)) + .def("setSpatialCoordinate", + &SpatialImgDetection::setSpatialCoordinate, + py::arg("spatialCoordinates"), + DOC(dai, SpatialImgDetection, setSpatialCoordinate)) + .def("setEdges", &SpatialImgDetection::setEdges, py::arg("edges"), DOC(dai, SpatialImgDetection, setEdges)) + .def("getImgDetection", &SpatialImgDetection::getImgDetection, DOC(dai, SpatialImgDetection, getImgDetection)) + .def("getKeypoints", &SpatialImgDetection::getKeypoints, DOC(dai, SpatialImgDetection, getKeypoints)) + .def("getKeypointSpatialCoordinates", &SpatialImgDetection::getKeypointSpatialCoordinates, DOC(dai, SpatialImgDetection, getKeypointSpatialCoordinates)) + .def("getEdges", &SpatialImgDetection::getEdges, DOC(dai, SpatialImgDetection, getEdges)) + .def("getCenterX", &SpatialImgDetection::getCenterX, DOC(dai, SpatialImgDetection, getCenterX)) + .def("getCenterY", &SpatialImgDetection::getCenterY, DOC(dai, SpatialImgDetection, getCenterY)) + .def("getWidth", &SpatialImgDetection::getWidth, DOC(dai, SpatialImgDetection, getWidth)) + .def("getHeight", &SpatialImgDetection::getHeight, DOC(dai, SpatialImgDetection, getHeight)) + .def("getAngle", &SpatialImgDetection::getAngle, DOC(dai, SpatialImgDetection, getAngle)); // Message - spatialImgDetections.def(py::init<>()) + spatialImgDetections.def(py::init<>(), DOC(dai, ImgDetectionsT, ImgDetectionsT)) .def("__repr__", &SpatialImgDetections::str) .def_property( "detections", [](SpatialImgDetections& det) { return &det.detections; }, - [](SpatialImgDetections& det, std::vector val) { det.detections = val; }) + [](SpatialImgDetections& det, std::vector val) { det.detections = val; }, + DOC(dai, ImgDetectionsT, detections), + py::return_value_policy::reference_internal) .def("getTimestamp", &SpatialImgDetections::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &SpatialImgDetections::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &SpatialImgDetections::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) .def("getTransformation", [](SpatialImgDetections& msg) { return msg.transformation; }) .def("setTransformation", [](SpatialImgDetections& msg, const std::optional& transformation) { msg.transformation = transformation; }) + .def("getSegmentationMaskWidth", &SpatialImgDetections::getSegmentationMaskWidth, DOC(dai, ImgDetectionsT, getSegmentationMaskWidth)) + .def("getSegmentationMaskHeight", &SpatialImgDetections::getSegmentationMaskHeight, DOC(dai, ImgDetectionsT, getSegmentationMaskHeight)) + .def("setSegmentationMask", + py::overload_cast(&SpatialImgDetections::setSegmentationMask), + py::arg("frame"), + DOC(dai, ImgDetectionsT, setSegmentationMask), + py::return_value_policy::reference_internal) + .def("getMaskData", &SpatialImgDetections::getMaskData, DOC(dai, ImgDetectionsT, getMaskData)) + .def("getSegmentationMask", &SpatialImgDetections::getSegmentationMask, DOC(dai, ImgDetectionsT, getSegmentationMask)) +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + .def("setCvSegmentationMask", &SpatialImgDetections::setCvSegmentationMask, py::arg("mask"), DOC(dai, ImgDetectionsT, setCvSegmentationMask)) + .def( + "getCvSegmentationMask", + [](SpatialImgDetections& self) { return self.getCvSegmentationMask(&g_numpyAllocator); }, + DOC(dai, ImgDetectionsT, getCvSegmentationMask)) + .def( + "getCvSegmentationMaskByIndex", + [](SpatialImgDetections& self, uint8_t index) { return self.getCvSegmentationMaskByIndex(index, &g_numpyAllocator); }, + py::arg("index"), + DOC(dai, ImgDetectionsT, getCvSegmentationMaskByIndex)) + .def( + "getCvSegmentationMaskByClass", + [](SpatialImgDetections& self, uint8_t semanticClass) { return self.getCvSegmentationMaskByClass(semanticClass, &g_numpyAllocator); }, + py::arg("semantic_class"), + DOC(dai, ImgDetectionsT, getCvSegmentationMaskByClass)) +#endif // .def("setTimestamp", &SpatialImgDetections::setTimestamp, DOC(dai, SpatialImgDetections, setTimestamp)) // .def("setTimestampDevice", &SpatialImgDetections::setTimestampDevice, DOC(dai, SpatialImgDetections, setTimestampDevice)) // .def("setSequenceNum", &SpatialImgDetections::setSequenceNum, DOC(dai, SpatialImgDetections, setSequenceNum)) diff --git a/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp b/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp index 3cc29af05f..e16542d015 100644 --- a/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp +++ b/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp @@ -60,11 +60,39 @@ void bind_spatiallocationcalculatorconfig(pybind11::module& m, void* pCallstack) // Message spatialLocationCalculatorConfig.def(py::init<>()) .def("__repr__", &SpatialLocationCalculatorConfig::str) + .def_readwrite("globalStepSize", &SpatialLocationCalculatorConfig::globalStepSize) + .def_readwrite("globalLowerThreshold", &SpatialLocationCalculatorConfig::globalLowerThreshold) + .def_readwrite("globalUpperThreshold", &SpatialLocationCalculatorConfig::globalUpperThreshold) + .def_readwrite("calculateSpatialKeypoints", &SpatialLocationCalculatorConfig::calculateSpatialKeypoints) + .def_readwrite("useSegmentation", &SpatialLocationCalculatorConfig::useSegmentation) // setters .def("setROIs", &SpatialLocationCalculatorConfig::setROIs, py::arg("ROIs"), DOC(dai, SpatialLocationCalculatorConfig, setROIs)) .def("addROI", &SpatialLocationCalculatorConfig::addROI, py::arg("ROI"), DOC(dai, SpatialLocationCalculatorConfig, addROI)) .def("getConfigData", &SpatialLocationCalculatorConfig::getConfigData, DOC(dai, SpatialLocationCalculatorConfig, getConfigData)) - // .def("set", &SpatialLocationCalculatorConfig::set, py::arg("config"), DOC(dai, SpatialLocationCalculatorConfig, set)) - // .def("get", &SpatialLocationCalculatorConfig::get, DOC(dai, SpatialLocationCalculatorConfig, get)) - ; + .def("setDepthThresholds", &SpatialLocationCalculatorConfig::setDepthThresholds, DOC(dai, SpatialLocationCalculatorConfig, setDepthThresholds)) + .def("setCalculationAlgorithm", + &SpatialLocationCalculatorConfig::setCalculationAlgorithm, + DOC(dai, SpatialLocationCalculatorConfig, setCalculationAlgorithm)) + .def("setStepSize", &SpatialLocationCalculatorConfig::setStepSize, DOC(dai, SpatialLocationCalculatorConfig, setStepSize)) + .def("setKeypointRadius", &SpatialLocationCalculatorConfig::setKeypointRadius, DOC(dai, SpatialLocationCalculatorConfig, setKeypointRadius)) + .def("setCalculateSpatialKeypoints", + &SpatialLocationCalculatorConfig::setCalculateSpatialKeypoints, + DOC(dai, SpatialLocationCalculatorConfig, setCalculateSpatialKeypoints)) + .def("setUseSegmentation", &SpatialLocationCalculatorConfig::setUseSegmentation, DOC(dai, SpatialLocationCalculatorConfig, setUseSegmentation)) + .def("setSegmentationPassthrough", + &SpatialLocationCalculatorConfig::setSegmentationPassthrough, + DOC(dai, SpatialLocationCalculatorConfig, setSegmentationPassthrough)) + .def("getDepthThresholds", &SpatialLocationCalculatorConfig::getDepthThresholds, DOC(dai, SpatialLocationCalculatorConfig, getDepthThresholds)) + .def("getCalculationAlgorithm", + &SpatialLocationCalculatorConfig::getCalculationAlgorithm, + DOC(dai, SpatialLocationCalculatorConfig, getCalculationAlgorithm)) + .def("getStepSize", &SpatialLocationCalculatorConfig::getStepSize, DOC(dai, SpatialLocationCalculatorConfig, getStepSize)) + .def("getKeypointRadius", &SpatialLocationCalculatorConfig::getKeypointRadius, DOC(dai, SpatialLocationCalculatorConfig, getKeypointRadius)) + .def("getCalculateSpatialKeypoints", + &SpatialLocationCalculatorConfig::getCalculateSpatialKeypoints, + DOC(dai, SpatialLocationCalculatorConfig, getCalculateSpatialKeypoints)) + .def("getUseSegmentation", &SpatialLocationCalculatorConfig::getUseSegmentation, DOC(dai, SpatialLocationCalculatorConfig, getUseSegmentation)) + .def("getSegmentationPassthrough", + &SpatialLocationCalculatorConfig::getSegmentationPassthrough, + DOC(dai, SpatialLocationCalculatorConfig, getSegmentationPassthrough)); } diff --git a/bindings/python/src/pipeline/node/SpatialLocationCalculatorBindings.cpp b/bindings/python/src/pipeline/node/SpatialLocationCalculatorBindings.cpp index c3e7d4d998..6b8b5c0976 100644 --- a/bindings/python/src/pipeline/node/SpatialLocationCalculatorBindings.cpp +++ b/bindings/python/src/pipeline/node/SpatialLocationCalculatorBindings.cpp @@ -1,7 +1,4 @@ #include "Common.hpp" -#include "NodeBindings.hpp" -#include "depthai/pipeline/Node.hpp" -#include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/SpatialLocationCalculator.hpp" void bind_spatiallocationcalculator(pybind11::module& m, void* pCallstack) { @@ -31,8 +28,10 @@ void bind_spatiallocationcalculator(pybind11::module& m, void* pCallstack) { // Node spatialLocationCalculator.def_readonly("inputConfig", &SpatialLocationCalculator::inputConfig, DOC(dai, node, SpatialLocationCalculator, inputConfig)) + .def_readonly("inputDetections", &SpatialLocationCalculator::inputDetections, DOC(dai, node, SpatialLocationCalculator, inputDetections)) .def_readonly("inputDepth", &SpatialLocationCalculator::inputDepth, DOC(dai, node, SpatialLocationCalculator, inputDepth)) .def_readonly("out", &SpatialLocationCalculator::out, DOC(dai, node, SpatialLocationCalculator, out)) + .def_readonly("outputDetections", &SpatialLocationCalculator::outputDetections, DOC(dai, node, SpatialLocationCalculator, outputDetections)) .def_readonly("passthroughDepth", &SpatialLocationCalculator::passthroughDepth, DOC(dai, node, SpatialLocationCalculator, passthroughDepth)) .def_readonly("initialConfig", &SpatialLocationCalculator::initialConfig, DOC(dai, node, SpatialLocationCalculator, initialConfig)); // ALIAS diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 8ee8f180ae..b0b4b368ef 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+0cb92f48801b27b9493724e014bce9d3b5043752") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+8f5f686773382428d08026d678eff7bb255e1275") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 11141ac26b..36821a6a24 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "5c3cf9a7866498123cfec1db97b7fd3a097b036c") +set(DEPTHAI_DEVICE_SIDE_COMMIT "fdad0853e6993e63f35fa20f41f43a1b7058ba83") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/cpp/SpatialLocationCalculator/CMakeLists.txt b/examples/cpp/SpatialLocationCalculator/CMakeLists.txt index b37edccef5..f8dc01bba2 100644 --- a/examples/cpp/SpatialLocationCalculator/CMakeLists.txt +++ b/examples/cpp/SpatialLocationCalculator/CMakeLists.txt @@ -6,3 +6,6 @@ cmake_minimum_required(VERSION 3.10) dai_add_example(spatial_location_calculator spatial_location_calculator.cpp ON OFF) dai_set_example_test_labels(spatial_location_calculator ondevice rvc2_all rvc4 ci) + +dai_add_example(spatial_segmentation spatial_segmentation.cpp ON OFF) +dai_set_example_test_labels(spatial_segmentation ondevice rvc2_all rvc4 ci) diff --git a/examples/cpp/SpatialLocationCalculator/spatial_segmentation.cpp b/examples/cpp/SpatialLocationCalculator/spatial_segmentation.cpp new file mode 100644 index 0000000000..837b3b434d --- /dev/null +++ b/examples/cpp/SpatialLocationCalculator/spatial_segmentation.cpp @@ -0,0 +1,167 @@ +#include +#include +#include +#include +#include + +#include "depthai/capabilities/ImgFrameCapability.hpp" +#include "depthai/depthai.hpp" +#include "depthai/pipeline/datatype/SpatialImgDetections.hpp" + +namespace { + +cv::Mat colorizeDepth(const cv::Mat& depthFrame) { + cv::Mat depthAbs; + cv::Mat colorizedDepth; + cv::convertScaleAbs(depthFrame, depthAbs, 0.03); + cv::applyColorMap(depthAbs, colorizedDepth, cv::COLORMAP_JET); + return colorizedDepth; +} + +void applySegmentationOverlay(cv::Mat& image, const cv::Mat& segmentationMask) { + cv::Mat lut(1, 256, CV_8U); + for(int i = 0; i < 256; ++i) { + lut.at(i) = (i == 255) ? 255 : cv::saturate_cast(i * 25); + } + cv::Mat scaledMask; + cv::LUT(segmentationMask, lut, scaledMask); + + cv::Mat coloredMask; + cv::applyColorMap(scaledMask, coloredMask, cv::COLORMAP_JET); + image.copyTo(coloredMask, (segmentationMask == 255)); + cv::addWeighted(image, 0.7, coloredMask, 0.3, 0, image); +} + +void drawSpatialDetections(cv::Mat& image, const dai::SpatialImgDetections& detections) { + for(const auto& det : detections.detections) { + auto bbox = det.getBoundingBox().denormalize(image.cols, image.rows); + auto points = bbox.getPoints(); + std::vector contour; + contour.resize(points.size()); + std::transform(points.begin(), points.end(), contour.begin(), [](const auto& pt) { return cv::Point(static_cast(pt.x), static_cast(pt.y)); }); + + if(contour.empty()) { + continue; + } + + cv::polylines(image, contour, true, cv::Scalar(0, 255, 0), 2); + + auto depthCoord = det.spatialCoordinates; + std::string text = "X: " + std::to_string(static_cast(depthCoord.x / 10)) + " cm, Y: " + std::to_string(static_cast(depthCoord.y / 10)) + + " cm, Z: " + std::to_string(static_cast(depthCoord.z / 10)) + " cm"; + cv::putText(image, text, contour.front(), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(232, 36, 87), 2); + } +} + +cv::Mat makeTopPanel(int width, bool useSegmentation) { + cv::Mat topPanel(100, width, CV_8UC3, cv::Scalar(255, 255, 255)); + cv::putText(topPanel, "Press 's' to toggle setUseSegmentation", cv::Point(10, 30), cv::FONT_HERSHEY_TRIPLEX, 0.7, cv::Scalar(0, 0, 0), 1); + cv::putText(topPanel, + "Current setUseSegmentation: " + std::to_string(static_cast(useSegmentation)), + cv::Point(10, 60), + cv::FONT_HERSHEY_TRIPLEX, + 0.7, + cv::Scalar(0, 0, 0), + 1); + return topPanel; +} + +} // namespace + +int main() { + try { + auto device = std::make_shared(); + std::string modelName = "luxonis/yolov8-instance-segmentation-large:coco-640x480"; + bool setRunOnHost = false; + float fps = 30.0f; + if(device->getPlatform() == dai::Platform::RVC2) { + modelName = "luxonis/yolov8-instance-segmentation-nano:coco-512x288"; + setRunOnHost = true; + fps = 10.0f; + } + dai::ImgFrameCapability cap{}; + cap.fps.value = fps; + cap.enableUndistortion = true; + + dai::Pipeline pipeline{device}; + + auto cameraNode = pipeline.create(); + cameraNode->build(dai::CameraBoardSocket::CAM_A); + + auto detectionNetwork = pipeline.create()->build(cameraNode, modelName, cap); + detectionNetwork->detectionParser->setRunOnHost(setRunOnHost); + + auto monoLeft = pipeline.create(); + monoLeft->build(dai::CameraBoardSocket::CAM_B, std::nullopt, fps); + auto monoRight = pipeline.create(); + monoRight->build(dai::CameraBoardSocket::CAM_C, std::nullopt, fps); + + auto stereo = pipeline.create(); + + monoLeft->requestFullResolutionOutput()->link(stereo->left); + monoRight->requestFullResolutionOutput()->link(stereo->right); + + auto align = pipeline.create(); + stereo->depth.link(align->input); + detectionNetwork->passthrough.link(align->inputAlignTo); + + auto spatialCalculator = pipeline.create(); + spatialCalculator->initialConfig->setUseSegmentation(true); + align->outputAligned.link(spatialCalculator->inputDepth); + detectionNetwork->out.link(spatialCalculator->inputDetections); + + auto camQueue = detectionNetwork->passthrough.createOutputQueue(); + auto outputDetectionsQueue = spatialCalculator->outputDetections.createOutputQueue(); + auto depthQueue = spatialCalculator->passthroughDepth.createOutputQueue(); + auto inputConfigQueue = spatialCalculator->inputConfig.createInputQueue(); + auto calculatorConfig = spatialCalculator->initialConfig; + + pipeline.start(); + + while(pipeline.isRunning()) { + auto inSpatialDet = outputDetectionsQueue->get(); + auto rgbFrame = camQueue->get(); + auto depthFrame = depthQueue->get(); + + if(!inSpatialDet || !rgbFrame || !depthFrame) { + continue; + } + cv::Mat depthCv = depthFrame->getCvFrame(); + cv::Mat colorizedDepth = colorizeDepth(depthCv); + + cv::Mat image = rgbFrame->getCvFrame(); + + auto segmentationMask = inSpatialDet->getCvSegmentationMask(); + if(segmentationMask && calculatorConfig->getUseSegmentation()) { + applySegmentationOverlay(image, *segmentationMask); + } + + drawSpatialDetections(image, *inSpatialDet); + + cv::Mat topPanel = makeTopPanel(image.cols, calculatorConfig->getUseSegmentation()); + cv::Mat concatenated; + cv::vconcat(topPanel, image, concatenated); + + cv::imshow("Depth", colorizedDepth); + cv::imshow("Spatial detections", concatenated); + + auto key = cv::waitKey(1); + if(key == 'q') { + break; + } + if(key == 's') { + calculatorConfig->setUseSegmentation(!calculatorConfig->getUseSegmentation()); + inputConfigQueue->send(calculatorConfig); + } + } + + pipeline.stop(); + pipeline.wait(); + + } catch(const std::exception& e) { + std::cerr << "Error: " << e.what() << '\n'; + return 1; + } + + return 0; +} diff --git a/examples/python/SpatialLocationCalculator/spatial_keypoints.py b/examples/python/SpatialLocationCalculator/spatial_keypoints.py new file mode 100644 index 0000000000..4044e68fc9 --- /dev/null +++ b/examples/python/SpatialLocationCalculator/spatial_keypoints.py @@ -0,0 +1,166 @@ +import depthai as dai +import sys +import numpy as np +import cv2 +import argparse + +try: + import open3d as o3d +except ImportError: + sys.exit( + "Critical dependency missing: Open3D. Please install it using the command: '{} -m pip install open3d' and then rerun the script.".format( + sys.executable + ) + ) + +parser = argparse.ArgumentParser() +parser.add_argument( + "--depthSource", type=str, default="stereo", choices=["stereo", "neural"] +) +args = parser.parse_args() + +device = dai.Device() +fps = 24 +modelName = "luxonis/yolov8-large-pose-estimation:coco-640x352" +if device.getPlatform() == dai.Platform.RVC2: + modelName = "luxonis/yolov8-nano-pose-estimation:coco-512x288" + fps = 10 + +requiredCamCapabilities = dai.ImgFrameCapability() +requiredCamCapabilities.fps.fixed(fps) +requiredCamCapabilities.enableUndistortion = True + +with dai.Pipeline(device) as pipeline: + print("Creating pipeline...") + + cameraNode = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A) + monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B, sensorFps=fps) + monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C, sensorFps=fps) + monoLeftOut = monoLeft.requestOutput((640, 400), fps=fps) + monoRightOut = monoRight.requestOutput((640, 400), fps=fps) + + detNN = pipeline.create(dai.node.DetectionNetwork).build(cameraNode, modelName, requiredCamCapabilities) + + spatialCalculator = pipeline.create(dai.node.SpatialLocationCalculator) + spatialCalculator.initialConfig.setCalculateSpatialKeypoints(True) + detNN.out.link(spatialCalculator.inputDetections) + + if args.depthSource == "stereo": + depth = pipeline.create(dai.node.StereoDepth).build(monoLeftOut, monoRightOut, presetMode=dai.node.StereoDepth.PresetMode.FAST_DENSITY) + if device.getPlatform() == dai.Platform.RVC2: + detNN.passthrough.link(depth.inputAlignTo) + depth.depth.link(spatialCalculator.inputDepth) + + elif args.depthSource == "neural": + depth = pipeline.create(dai.node.NeuralDepth).build(monoLeftOut, monoRightOut, dai.DeviceModelZoo.NEURAL_DEPTH_MEDIUM) + + else: + raise ValueError(f"Invalid depth source: {args.depthSource}") + + if device.getPlatform() == dai.Platform.RVC4: + align = pipeline.create(dai.node.ImageAlign) + depth.depth.link(align.input) + detNN.passthrough.link(align.inputAlignTo) + align.outputAligned.link(spatialCalculator.inputDepth) + + camQueue = detNN.passthrough.createOutputQueue() + spatialOutputQueue = spatialCalculator.outputDetections.createOutputQueue() + depthQueue = spatialCalculator.passthroughDepth.createOutputQueue() + pipeline.start() + print("Pipeline created.") + + # Prepare Open3D visualization for spatial keypoints (converted to centimeters) + vis = o3d.visualization.Visualizer() + vis.create_window(window_name="Spatial Keypoints 3D", width=1280, height=720) + opt = vis.get_render_option() + opt.background_color = np.asarray([200, 200, 200]) + opt.point_size = 15.0 + opt.line_width = 5.0 + axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1, origin=[-1,-1,0]) + vis.add_geometry(axes) + + pc = o3d.geometry.PointCloud() + lineSet = o3d.geometry.LineSet() + + vis.add_geometry(pc) + vis.add_geometry(lineSet) + vis.poll_events() + vis.update_renderer() + + while pipeline.isRunning(): + spatialPointsCm = [] + lines = [] + pointCountOffset = 0 + spatialData = spatialOutputQueue.get() + passthrough = camQueue.get() + depthFrame = depthQueue.get() + + assert isinstance(spatialData, dai.SpatialImgDetections) + assert isinstance(passthrough, dai.ImgFrame) + assert isinstance(depthFrame, dai.ImgFrame) + + depthImg = depthFrame.getCvFrame() + colorizedDepth = cv2.applyColorMap(cv2.convertScaleAbs(depthImg, alpha=0.03), cv2.COLORMAP_JET) + image = passthrough.getCvFrame() + + filterKeypoints = [0, 3, 4, 7, 8, 13, 14, 15, 16] # filter out nose, ears, elbows, knees, ankles + connectedKeypoints = [[0, 1], [2, 3], [2, 4], [3, 5], [2, 6], [3, 7]] # indices of keypoints that are connected with lines. + + for (i, det) in enumerate(spatialData.detections): + bbox = det.getBoundingBox().denormalize(image.shape[1], image.shape[0]) + outerPoints = bbox.getPoints() + + outerPoints = [[int(p.x), int(p.y)] for p in outerPoints] + outerPoints = np.array(outerPoints, dtype=np.int32) + image = cv2.polylines(image, [outerPoints], isClosed=True, color=(0, 255, 0), thickness=2) + depthCoordinate = det.spatialCoordinates + depth = depthCoordinate.z + text = f"X: {int(depthCoordinate.x / 10 )} cm, Y: {int(depthCoordinate.y / 10)} cm, Z: {int(depth / 10)} cm" + cv2.putText(image, text, outerPoints[0], cv2.FONT_HERSHEY_PLAIN, 1, (232,36,87), 2) + keypoints = det.getKeypoints() + detIndices = [] + for (j, kp) in enumerate(keypoints): + x = kp.imageCoordinates.x + y = kp.imageCoordinates.y + if (j in filterKeypoints): + continue + + image = cv2.circle(image, (int(x * image.shape[1]), int(y * image.shape[0])), 10, (255, 0, 0), -1) + cv2.circle(colorizedDepth, (int(x * depthImg.shape[1]), int(y * depthImg.shape[0])), 10, (0, 0, 255), -1) + cv2.putText(image,f"Z: {int(kp.spatialCoordinates.z / 10)} cm", (int(x * image.shape[1]), int(y * image.shape[0]) - 15), cv2.FONT_HERSHEY_PLAIN, 1, (0,255,255), 2) + spatialPointsCm.append([-kp.spatialCoordinates.x / 1000.0, + -kp.spatialCoordinates.y / 1000.0, + kp.spatialCoordinates.z / 1000.0]) + detIndices.append(pointCountOffset) + pointCountOffset += 1 + + if (len(keypoints) > 0): + for kp_indices in connectedKeypoints: + try: + idx1 = detIndices[kp_indices[0]] + idx2 = detIndices[kp_indices[1]] + lines.append([idx1, idx2]) + except IndexError: + continue + + # origin=[0,0,0]. X=Red, Y=Green, Z=Blue + npPoints = np.array(spatialPointsCm, dtype=np.float32) + lineSet.points = o3d.utility.Vector3dVector(npPoints) + lineSet.lines = o3d.utility.Vector2iVector(np.array(lines)) + lineSet.colors = o3d.utility.Vector3dVector(np.tile([0,1 ,0], (len(lines),1))) + + pc.points = o3d.utility.Vector3dVector(npPoints) + # Color points bright magenta in 3D view + colors = np.tile([1.0, 0.0, 1.0], (len(npPoints), 1)) + pc.colors = o3d.utility.Vector3dVector(colors) + vis.update_geometry(pc) + vis.update_geometry(lineSet) + vis.poll_events() + vis.update_renderer() + + cv2.imshow("Depth", colorizedDepth) + cv2.imshow("Detections", image) + key = cv2.waitKey(1) + if key == ord('q'): + vis.destroy_window() + break diff --git a/examples/python/SpatialLocationCalculator/spatial_segmentation.py b/examples/python/SpatialLocationCalculator/spatial_segmentation.py new file mode 100644 index 0000000000..348baaf544 --- /dev/null +++ b/examples/python/SpatialLocationCalculator/spatial_segmentation.py @@ -0,0 +1,108 @@ +import depthai as dai +import numpy as np +import cv2 + +modelName = "luxonis/yolov8-instance-segmentation-large:coco-640x480" +setRunOnHost = False +device = dai.Device() +fps = 30 +if device.getPlatform() == dai.Platform.RVC2: + modelName = "luxonis/yolov8-instance-segmentation-nano:coco-512x288" + setRunOnHost = True + fps = 10 + +requiredCamCapabilities = dai.ImgFrameCapability() +requiredCamCapabilities.fps.fixed(fps) +requiredCamCapabilities.enableUndistortion = True + +def addTopPanel(image: np.ndarray, useSegmentation: bool) -> np.ndarray: + topPanel = np.ones((100, image.shape[1], 3), dtype=np.uint8) * 255 + cv2.putText(topPanel, "Press 's' to toggle setUseSegmentation", (10, 30), cv2.FONT_HERSHEY_TRIPLEX, 0.7, (0, 0, 0), 1) + cv2.putText(topPanel, f"Current setUseSegmentation: {useSegmentation}", (10, 60), cv2.FONT_HERSHEY_TRIPLEX, 0.7, (0, 0, 0), 1) + concatenatedFrame = cv2.vconcat([topPanel, image]) + return concatenatedFrame + +with dai.Pipeline(device) as pipeline: + print("Creating pipeline...") + + cameraNode = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A) + + detNN = pipeline.create(dai.node.DetectionNetwork).build(cameraNode, modelName, requiredCamCapabilities) + detNN.detectionParser.setRunOnHost(setRunOnHost) + + monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B, sensorFps=fps) + monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C, sensorFps=fps) + stereo = pipeline.create(dai.node.StereoDepth) + + monoLeftOut = monoLeft.requestFullResolutionOutput() + monoRightOut = monoRight.requestFullResolutionOutput() + monoLeftOut.link(stereo.left) + monoRightOut.link(stereo.right) + + align = pipeline.create(dai.node.ImageAlign) + stereo.depth.link(align.input) + detNN.passthrough.link(align.inputAlignTo) + + spatialCalculator = pipeline.create(dai.node.SpatialLocationCalculator) + spatialCalculator.initialConfig.setUseSegmentation(True) + align.outputAligned.link(spatialCalculator.inputDepth) + detNN.out.link(spatialCalculator.inputDetections) + + cameraQueue = detNN.passthrough.createOutputQueue() + spatialOutputQueue = spatialCalculator.outputDetections.createOutputQueue() + depthQueue = spatialCalculator.passthroughDepth.createOutputQueue() + inputConfigQueue = spatialCalculator.inputConfig.createInputQueue() + configUpdate = spatialCalculator.initialConfig + useSegmentation = spatialCalculator.initialConfig.getUseSegmentation() + + pipeline.start() + print("Pipeline created.") + while pipeline.isRunning(): + spatialDetections = spatialOutputQueue.get() + rgbFrame = cameraQueue.get() + depthFrame = depthQueue.get() + + assert isinstance(spatialDetections, dai.SpatialImgDetections) + assert isinstance(rgbFrame, dai.ImgFrame) + assert isinstance(depthFrame, dai.ImgFrame) + + depthImg = depthFrame.getCvFrame() + colorizedDepth = cv2.applyColorMap(cv2.convertScaleAbs(depthImg, alpha=0.03), cv2.COLORMAP_JET) + image = rgbFrame.getCvFrame() + segmentationMask = cv2.Mat(np.zeros((spatialDetections.getSegmentationMaskHeight(), spatialDetections.getSegmentationMaskWidth()), dtype=np.uint8)) + segmentationMask = spatialDetections.getCvSegmentationMask() + + if segmentationMask is not None and useSegmentation: + scaledMask = segmentationMask.copy() + scaledMask[segmentationMask != 255] = segmentationMask[segmentationMask != 255] * 25 # scale for better visualization + coloredMask = cv2.applyColorMap(scaledMask, cv2.COLORMAP_JET) + coloredMask[segmentationMask == 255] = image[segmentationMask == 255] + image = cv2.addWeighted(image, 0.7, coloredMask, 0.3, 0) + + for i, det in enumerate(spatialDetections.detections): + outerPoints = det.getBoundingBox() + if outerPoints is not None: + outerPoints = outerPoints.denormalize(image.shape[1], image.shape[0]) + outerPoints = outerPoints.getPoints() + outerPoints = [[int(p.x), int(p.y)] for p in outerPoints] + outerPoints = np.array(outerPoints, dtype=np.int32) + cv2.polylines(image, [outerPoints], isClosed=True, color=(0, 255, 0), thickness=2) + + # depth + depth_coordinate = det.spatialCoordinates + depth = depth_coordinate.z + text = f"X: {int(depth_coordinate.x / 10 )} cm, Y: {int(depth_coordinate.y / 10)} cm, Z: {int(depth / 10)} cm" + cv2.putText(image, text, outerPoints[0], cv2.FONT_HERSHEY_PLAIN, 1, (232,36,87), 2) + + concatenatedFrame = addTopPanel(image, useSegmentation) + + cv2.imshow("Depth", colorizedDepth) + cv2.imshow("Spatial detections", concatenatedFrame) + key = cv2.waitKey(1) + if key == ord('q'): + break + + elif key == ord('s'): + useSegmentation = not useSegmentation + configUpdate.setUseSegmentation(useSegmentation) + inputConfigQueue.send(configUpdate) diff --git a/include/depthai/common/SpatialKeypoint.hpp b/include/depthai/common/SpatialKeypoint.hpp new file mode 100644 index 0000000000..804f418112 --- /dev/null +++ b/include/depthai/common/SpatialKeypoint.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include + +#include "depthai/common/Point2f.hpp" +#include "depthai/common/Point3f.hpp" + +// project +#include "depthai/common/KeypointsListT.hpp" +#include "depthai/utility/Serialization.hpp" + +namespace dai { + +struct SpatialKeypoint { + Point3f imageCoordinates{}; + float confidence = -1.f; + uint32_t label = 0; + std::string labelName = ""; + Point3f spatialCoordinates{}; + + SpatialKeypoint() = default; + explicit SpatialKeypoint(Point3f imageCoordinates, Point3f spatialCoordinates = Point3f{}, float conf = 0.f, uint32_t label = 0, std::string labelName = "") + : imageCoordinates(imageCoordinates), confidence(conf), label(label), labelName(labelName), spatialCoordinates(spatialCoordinates) {} + + explicit SpatialKeypoint(Point2f imageCoordinates, Point3f spatialCoordinates = Point3f{}, float conf = 0.f, uint32_t label = 0, std::string labelName = "") + : SpatialKeypoint(Point3f(imageCoordinates.x, imageCoordinates.y, 0.f), spatialCoordinates, conf, label, labelName) {} + explicit SpatialKeypoint(float x, float y, float z, float sx, float sy, float sz, float conf = 0.f, uint32_t label = 0, std::string labelName = "") + : SpatialKeypoint(Point3f(x, y, z), Point3f{sx, sy, sz}, conf, label, labelName) {} + + DEPTHAI_SERIALIZE(dai::SpatialKeypoint, imageCoordinates, confidence, label, labelName, spatialCoordinates); +}; + +struct SpatialKeypointsList : KeypointsListT { + public: + using Base = KeypointsListT; + using Base::Base; + using Base::edges; + using Base::getEdges; + using Base::getKeypoints; + using Base::getPoints2f; + using Base::getPoints3f; + using Base::keypoints; + using Base::setEdges; + using Base::setKeypoints; + + /** + * Sets the keypoints list. + * @param keypoints list of Point3f objects to set. + * @note This will clear any existing keypoints and edges. + */ + void setKeypoints(const std::vector& kps3) { + edges.clear(); + keypoints.clear(); + keypoints.reserve(kps3.size()); + for(const auto& kp : kps3) { + keypoints.emplace_back(SpatialKeypoint(kp)); + } + } + + /** + * Sets the keypoints from a vector of 3D spatial points. + * @param spatialCoordinates vector of Point3f objects to set as spatial coordinates. + * @note The size of spatialCoordinates must match the number of keypoints. + */ + void setSpatialCoordinates(const std::vector& spatialCoordinates) { + if(spatialCoordinates.size() != keypoints.size()) { + throw std::invalid_argument("Size of spatialCoordinates must match the number of keypoints."); + } + for(size_t i = 0; i < keypoints.size(); ++i) { + keypoints[i].spatialCoordinates = spatialCoordinates[i]; + } + } + + /** + * Get spatial coordinates of the keypoints. + * @return Vector of Point3f spatial coordinates. + */ + std::vector getSpatialCoordinates() const { + std::vector spatialCoordinates; + spatialCoordinates.reserve(keypoints.size()); + for(const auto& kp : keypoints) { + spatialCoordinates.emplace_back(kp.spatialCoordinates); + } + return spatialCoordinates; + } + + DEPTHAI_SERIALIZE(SpatialKeypointsList, keypoints, edges); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatype/ImgDetections.hpp b/include/depthai/pipeline/datatype/ImgDetections.hpp index e1129065fb..c6d139066c 100644 --- a/include/depthai/pipeline/datatype/ImgDetections.hpp +++ b/include/depthai/pipeline/datatype/ImgDetections.hpp @@ -155,7 +155,7 @@ struct ImgDetection { */ class ImgDetections : public ImgDetectionsT, public ProtoSerializable { public: - virtual ~ImgDetections() override; + ~ImgDetections() override; using Base = ImgDetectionsT; using Base::Base; using Base::detections; diff --git a/include/depthai/pipeline/datatype/SpatialImgDetections.hpp b/include/depthai/pipeline/datatype/SpatialImgDetections.hpp index 7e70f69d49..57887fd3aa 100644 --- a/include/depthai/pipeline/datatype/SpatialImgDetections.hpp +++ b/include/depthai/pipeline/datatype/SpatialImgDetections.hpp @@ -6,9 +6,11 @@ #include "depthai/common/ImgTransformations.hpp" #include "depthai/common/Point3f.hpp" +#include "depthai/common/SpatialKeypoint.hpp" #include "depthai/common/optional.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" +#include "depthai/pipeline/datatype/ImgDetectionsT.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" #include "depthai/utility/ProtoSerializable.hpp" #include "depthai/utility/Serialization.hpp" @@ -20,39 +22,148 @@ namespace dai { * * Contains image detection results together with spatial location data. */ -struct SpatialImgDetection : public ImgDetection { +struct SpatialImgDetection { + uint32_t label = 0; + std::string labelName; + float confidence = 0.f; + float xmin = 0.f; + float ymin = 0.f; + float xmax = 0.f; + float ymax = 0.f; + std::optional boundingBox; + std::optional keypoints; Point3f spatialCoordinates; SpatialLocationCalculatorConfigData boundingBoxMapping; - DEPTHAI_SERIALIZE(SpatialImgDetection, - ImgDetection::xmax, - ImgDetection::xmin, - ImgDetection::ymax, - ImgDetection::ymin, - ImgDetection::label, - ImgDetection::labelName, - ImgDetection::confidence, - ImgDetection::boundingBox, - ImgDetection::keypoints, - spatialCoordinates, - boundingBoxMapping); + SpatialImgDetection() = default; + SpatialImgDetection(const RotatedRect& boundingBox, Point3f spatialCoordinates, float confidence = 1.f, uint32_t label = 0); + SpatialImgDetection(const RotatedRect& boundingBox, Point3f spatialCoordinates, std::string labelName, float confidence, uint32_t label); + SpatialImgDetection( + const RotatedRect& boundingBox, Point3f spatialCoordinates, const SpatialKeypointsList& keypoints, float confidence = 1.f, uint32_t label = 0); + SpatialImgDetection(const RotatedRect& boundingBox, + Point3f spatialCoordinates, + const SpatialKeypointsList& keypoints, + std::string labelName, + float confidence, + uint32_t label); + + /** + * Sets the bounding box and the legacy coordinates of the detection. + */ + void setBoundingBox(RotatedRect boundingBox); + + /** + * Returns bounding box if it was set, else it constructs a new one from the legacy xmin, ymin, xmax, ymax values. + */ + RotatedRect getBoundingBox() const; + + /** + * Sets the bounding box and the legacy coordinates of the detection from the top-left and bottom-right points. + */ + void setOuterBoundingBox(const float xmin, const float ymin, const float xmax, const float ymax); + + /** + * Sets the keypoints of the detection. + * @param keypoints list of Keypoint objects to set. + * @note This will clear any existing keypoints and edges. + */ + void setKeypoints(const SpatialKeypointsList keypoints); + + /** + * Sets the keypoints of the detection. + * @param keypoints list of Keypoint objects to set. + * @note This will clear any existing keypoints and edges. + */ + void setKeypoints(const std::vector keypoints); + + /** + * Sets the keypoints of the detection. + * @param keypoints list of Point2f objects to set. + * @note This will clear any existing keypoints and edges. + */ + void setKeypoints(const std::vector keypoints, const std::vector edges); + + /** + * Sets the keypoints of the detection. + * @param keypoints list of Point3f objects to set. + * @note This will clear any existing keypoints and edges. + */ + void setKeypoints(const std::vector keypoints); + + /** + * Sets spatial coordinates for the detection. + * @param spatialCoordinates list of Point3f objects to set. + * @note The size of spatialCoordinates. + */ + void setSpatialCoordinate(const Point3f spatialCoordinates); + + /** + * Sets edges for the keypoints, throws if no keypoints were set beforehand. + */ + void setEdges(const std::vector edges); + + /** + * Converts SpatialImgDetection to ImgDetection by dropping spatial data. + * @return dai::ImgDetection object. + */ + dai::ImgDetection getImgDetection() const; + + /** + * Returns a list of Keypoint objects, or empty list if no keypoints were set. + */ + std::vector getKeypoints() const; + + /** + * Returns a list of spatial coordinates for each keypoint, or empty list if no keypoints were set. + */ + std::vector getKeypointSpatialCoordinates() const; + + /** + * Returns a list of edges, each edge is a pair of indices, or empty list if no keypoints were set. + */ + std::vector getEdges() const; + + /** + * Returns the X coordinate of the center of the bounding box. + */ + float getCenterX() const; + + /** + * Returns the Y coordinate of the center of the bounding box. + */ + float getCenterY() const; + + /** + * Returns the width of the (rotated) bounding box. + */ + float getWidth() const; + + /** + * Returns the height of the (rotated) bounding box. + */ + float getHeight() const; + + /** + * Returns the angle of the bounding box. + */ + float getAngle() const; + DEPTHAI_SERIALIZE( + SpatialImgDetection, label, labelName, confidence, xmin, ymin, xmax, ymax, boundingBox, keypoints, spatialCoordinates, boundingBoxMapping); }; + /** * SpatialImgDetections message. Carries detection results together with spatial location data */ -class SpatialImgDetections : public Buffer, public ProtoSerializable { +class SpatialImgDetections : public ImgDetectionsT, public ProtoSerializable { public: - /** - * Construct SpatialImgDetections message. - */ - SpatialImgDetections() = default; - virtual ~SpatialImgDetections(); + ~SpatialImgDetections() override; + using Base = ImgDetectionsT; + using Base::Base; + using Base::detections; + using Base::segmentationMaskHeight; + using Base::segmentationMaskWidth; + using Base::transformation; - /** - * Detection results. - */ - std::vector detections; - std::optional transformation; void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; DatatypeEnum getDatatype() const override { @@ -75,7 +186,14 @@ class SpatialImgDetections : public Buffer, public ProtoSerializable { ProtoSerializable::SchemaPair serializeSchema() const override; #endif - DEPTHAI_SERIALIZE(SpatialImgDetections, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, detections, transformation); + DEPTHAI_SERIALIZE(SpatialImgDetections, + Base::Buffer::sequenceNum, + Base::Buffer::ts, + Base::Buffer::tsDevice, + detections, + transformation, + segmentationMaskWidth, + segmentationMaskHeight); }; } // namespace dai diff --git a/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp b/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp index 3932c70408..f6c032f092 100644 --- a/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp +++ b/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp @@ -3,7 +3,6 @@ #include #include -#include "depthai/common/Point3f.hpp" #include "depthai/common/Rect.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" namespace dai { @@ -66,10 +65,39 @@ struct SpatialLocationCalculatorConfigData { DEPTHAI_SERIALIZE_EXT(SpatialLocationCalculatorConfigData, roi, depthThresholds, calculationAlgorithm, stepSize); /** - * SpatialLocationCalculatorConfig message. Carries ROI (region of interest) and threshold for depth calculation + * Configuration for SpatialLocationCalculator. + * + * Holds global parameters and optional per-ROI entries used to compute 3D + * spatial locations from a depth map. + * + * Global parameters (defaults): + * - Lower depth threshold [mm]: 0 + * - Upper depth threshold [mm]: 65535 + * - Calculation algorithm: MEDIAN + * - Step size: AUTO + * - Keypoint radius [px]: 10 + * - Calculate spatial keypoints: true + * - Use segmentation for ImgDetections: true + * - Segmentation passthrough: true + * + * An optional list of per-ROI configurations is available via `config`. ROI + * settings override the corresponding global values where specified. + * */ class SpatialLocationCalculatorConfig : public Buffer { + static constexpr std::int32_t AUTO = -1; + public: + int32_t globalStepSize = AUTO; + uint32_t globalLowerThreshold = 100; // less than 100mm is considered too close + uint32_t globalUpperThreshold = 65535; + SpatialLocationCalculatorAlgorithm globalCalculationAlgorithm = SpatialLocationCalculatorAlgorithm::MEDIAN; + int32_t globalKeypointRadius = 10; + bool calculateSpatialKeypoints = true; + bool useSegmentation = true; + bool segmentationPassthrough = true; + std::vector config; + /** * Construct SpatialLocationCalculatorConfig message. */ @@ -77,27 +105,132 @@ class SpatialLocationCalculatorConfig : public Buffer { virtual ~SpatialLocationCalculatorConfig(); /** - * Set a vector of ROIs as configuration data. + * Specify additional regions of interest (ROI) to calculate their spatial coordinates. Results of ROI coordinates are available on + SpatialLocationCalculatorData output. * @param ROIs Vector of configuration parameters for ROIs (region of interests) */ void setROIs(std::vector ROIs); + /** - * Add a new ROI to configuration data. - * @param roi Configuration parameters for ROI (region of interest) + * Add a new region of interest (ROI) to configuration data. + * @param roi Configuration parameters for ROI */ void addROI(SpatialLocationCalculatorConfigData& ROI); + /** + * Set the lower and upper depth value thresholds to be used in the spatial calculations. + * @param lowerThreshold Lower threshold in depth units (millimeter by default). + * @param upperThreshold Upper threshold in depth units (millimeter by default). + */ + void setDepthThresholds(uint32_t lowerThreshold = 0, uint32_t upperThreshold = 30000); + + /** + * Set spatial location calculation algorithm. Possible values: + * + * - MEDIAN: Median of all depth values in the ROI + * - AVERAGE: Average of all depth values in the ROI + * - MIN: Minimum depth value in the ROI + * - MAX: Maximum depth value in the ROI + * - MODE: Most frequent depth value in the ROI + */ + void setCalculationAlgorithm(SpatialLocationCalculatorAlgorithm calculationAlgorithm); + + /** + * Set step size for spatial location calculation. + * Step size 1 means that every pixel is taken into calculation, size 2 means every second etc. + * for AVERAGE, MIN, MAX step size is 1; for MODE/MEDIAN it's 2. + */ + void setStepSize(int32_t stepSize); + + /** + * Set radius around keypoints to calculate spatial coordinates. + * @param radius Radius in pixels. + * @warning Only applicable to Keypoints or ImgDetections with keypoints. + */ + void setKeypointRadius(int32_t radius); + + /** + * If false, spatial coordinates of keypoints will not be calculated. + * @param calculateSpatialKeypoints + * @warning Only applicable to ImgDetections with keypoints. + */ + void setCalculateSpatialKeypoints(bool calculateSpatialKeypoints); + + /** + * Specify whether to consider only segmented pixels within a detection bounding box for spatial calculations. + * @param useSegmentation + * @warning Only applicable to ImgDetections with segmentation masks. + */ + void setUseSegmentation(bool useSegmentation); + + /** + * Specify whether to passthrough segmentation mask along with spatial detections. + * @param passthroughSegmentation + * @warning Only applicable to ImgDetections with segmentation masks. + */ + void setSegmentationPassthrough(bool passthroughSegmentation); + /** * Retrieve configuration data for SpatialLocationCalculator * @returns Vector of configuration parameters for ROIs (region of interests) */ std::vector getConfigData() const; - std::vector config; + + /* + * Retrieve the lower and upper depth value thresholds used in the spatial calculations. + * @returns Pair of lower and upper thresholds in depth units (millimeter by default). + */ + std::pair getDepthThresholds() const; + + /* + * Retrieve spatial location calculation algorithm. + */ + SpatialLocationCalculatorAlgorithm getCalculationAlgorithm() const; + + /* + * Retrieve step size for spatial location calculation. + */ + int32_t getStepSize() const; + + /** + * Retrieve radius around keypoints used to calculate spatial coordinates. + */ + int32_t getKeypointRadius() const; + + /** + * Retrieve whether keypoints are used for spatial location calculation. + * @warning Only applicable to ImgDetections with keypoints. + */ + bool getCalculateSpatialKeypoints() const; + + /** + * Retrieve whether segmentation is used for spatial location calculation. + * @warning Only applicable to ImgDetections with segmentation masks. + */ + bool getUseSegmentation() const; + + /** + * Retrieve whether segmentation is passed through along with spatial detections. + * @warning Only applicable to ImgDetections with segmentation masks. + */ + bool getSegmentationPassthrough() const; + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; + DatatypeEnum getDatatype() const override { return DatatypeEnum::SpatialLocationCalculatorConfig; } - DEPTHAI_SERIALIZE(SpatialLocationCalculatorConfig, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, config); + + DEPTHAI_SERIALIZE(SpatialLocationCalculatorConfig, + globalStepSize, + globalLowerThreshold, + globalUpperThreshold, + globalCalculationAlgorithm, + globalKeypointRadius, + calculateSpatialKeypoints, + useSegmentation, + segmentationPassthrough, + config); }; } // namespace dai diff --git a/include/depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp b/include/depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp index 0b25b6f337..95f60cf641 100644 --- a/include/depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp +++ b/include/depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp @@ -3,6 +3,7 @@ #include #include +#include "depthai/common/Point3f.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" diff --git a/include/depthai/pipeline/node/SpatialLocationCalculator.hpp b/include/depthai/pipeline/node/SpatialLocationCalculator.hpp index 19cff2281d..7bb04f1a27 100644 --- a/include/depthai/pipeline/node/SpatialLocationCalculator.hpp +++ b/include/depthai/pipeline/node/SpatialLocationCalculator.hpp @@ -11,19 +11,24 @@ namespace dai { namespace node { /** - * @brief SpatialLocationCalculator node. Calculates spatial location data on a set of ROIs on depth map. + * @brief SpatialLocationCalculator node. Calculates the spatial locations of detected objects based on the input depth map. Spatial location calculations can + * be additionally refined by using a segmentation mask. If keypoints are provided, the spatial location is calculated around each keypoint. */ -class SpatialLocationCalculator : public DeviceNodeCRTP { - public: - constexpr static const char* NAME = "SpatialLocationCalculator"; - using DeviceNodeCRTP::DeviceNodeCRTP; +class SpatialLocationCalculator : public DeviceNodeCRTP, public HostRunnable { + private: + bool runOnHostVar = false; + std::shared_ptr calculationConfig; protected: - Properties& getProperties(); + Properties& getProperties() override; public: + constexpr static const char* NAME = "SpatialLocationCalculator"; + using DeviceNodeCRTP::DeviceNodeCRTP; + SpatialLocationCalculator() = default; - SpatialLocationCalculator(std::unique_ptr props); + SpatialLocationCalculator(std::unique_ptr props) + : DeviceNodeCRTP(std::move(props)), calculationConfig(std::make_shared(properties.roiConfig)) {} /** * Initial config to use when calculating spatial location data. @@ -36,6 +41,12 @@ class SpatialLocationCalculator : public DeviceNodeCRTP ImgDetectionsT::getCvSegmentationMaskByClass( #endif template class ImgDetectionsT; +template class ImgDetectionsT; } // namespace dai diff --git a/src/pipeline/datatype/ImgTransformations.cpp b/src/pipeline/datatype/ImgTransformations.cpp index b5a502dfb0..93ff513079 100644 --- a/src/pipeline/datatype/ImgTransformations.cpp +++ b/src/pipeline/datatype/ImgTransformations.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include diff --git a/src/pipeline/datatype/SpatialImgDetections.cpp b/src/pipeline/datatype/SpatialImgDetections.cpp index 66d1916470..b3d1525c6f 100644 --- a/src/pipeline/datatype/SpatialImgDetections.cpp +++ b/src/pipeline/datatype/SpatialImgDetections.cpp @@ -1,4 +1,9 @@ #include "depthai/pipeline/datatype/SpatialImgDetections.hpp" + +#include + +#include "depthai/common/Keypoint.hpp" +#include "depthai/common/Point3f.hpp" #ifdef DEPTHAI_ENABLE_PROTOBUF #include "depthai/schemas/SpatialImgDetections.pb.h" #include "utility/ProtoSerialize.hpp" @@ -6,6 +11,159 @@ namespace dai { +// SpatialImgDetection functions + +SpatialImgDetection::SpatialImgDetection(const dai::RotatedRect& boundingBox, Point3f spatialCoordinates, float conf, uint32_t label) { + setBoundingBox(boundingBox); + this->spatialCoordinates = spatialCoordinates; + this->confidence = conf; + this->label = label; +} +SpatialImgDetection::SpatialImgDetection(const dai::RotatedRect& boundingBox, Point3f spatialCoordinates, std::string labelName, float conf, uint32_t label) + : SpatialImgDetection(boundingBox, spatialCoordinates, conf, label) { + this->labelName = std::move(labelName); +} + +SpatialImgDetection::SpatialImgDetection( + const dai::RotatedRect& boundingBox, Point3f spatialCoordinates, const dai::SpatialKeypointsList& keypoints, float conf, uint32_t label) + : SpatialImgDetection(boundingBox, spatialCoordinates, conf, label) { + this->keypoints = keypoints; +} + +SpatialImgDetection::SpatialImgDetection(const dai::RotatedRect& boundingBox, + Point3f spatialCoordinates, + const dai::SpatialKeypointsList& keypoints, + std::string labelName, + float conf, + uint32_t label) + : SpatialImgDetection(boundingBox, spatialCoordinates, conf, label) { + this->keypoints = keypoints; + this->labelName = std::move(labelName); +} + +void SpatialImgDetection::setBoundingBox(const RotatedRect boundingBox) { + this->boundingBox = boundingBox; + + std::array outerPoints = this->boundingBox->getOuterRect(); + xmin = outerPoints[0]; + ymin = outerPoints[1]; + xmax = outerPoints[2]; + ymax = outerPoints[3]; +} + +RotatedRect SpatialImgDetection::getBoundingBox() const { + if(boundingBox.has_value()) { + return boundingBox.value(); + } else if(xmin == 0.f && xmax == 0.f && ymin == 0.f && ymax == 0.f) { + throw std::runtime_error("All bounding box values are zero, no bounding box can be built."); + } + + // construct from legacy values + RotatedRect rect = RotatedRect{dai::Rect{Point2f{xmin, ymin}, Point2f{xmax, ymax}}}; + return rect; +} + +void SpatialImgDetection::setOuterBoundingBox(const float xmin, const float ymin, const float xmax, const float ymax) { + this->xmin = xmin; + this->ymin = ymin; + this->xmax = xmax; + this->ymax = ymax; + + Point2f topLeft{xmin, ymin}; + Point2f bottomRight{xmax, ymax}; + + this->boundingBox = RotatedRect{Rect{topLeft, bottomRight}}; +} + +void SpatialImgDetection::setKeypoints(const SpatialKeypointsList kp) { + this->keypoints = kp; +} + +void SpatialImgDetection::setKeypoints(const std::vector kp) { + this->keypoints = SpatialKeypointsList(kp); +} + +void SpatialImgDetection::setKeypoints(const std::vector kps, const std::vector> edges) { + this->keypoints = SpatialKeypointsList(kps, edges); +} + +void SpatialImgDetection::setKeypoints(const std::vector kps3) { + this->keypoints = SpatialKeypointsList(); + this->keypoints->setKeypoints(kps3); +} + +void SpatialImgDetection::setSpatialCoordinate(Point3f spatialCoordinates) { + this->spatialCoordinates = spatialCoordinates; +} + +dai::ImgDetection SpatialImgDetection::getImgDetection() const { + dai::ImgDetection imgDetection(getBoundingBox(), labelName, confidence, label); + + if(keypoints.has_value()) { + dai::KeypointsList converted; + std::vector convertedKeypoints; + convertedKeypoints.reserve(keypoints->size()); + for(const auto& spatialKeypoint : keypoints->getKeypoints()) { + convertedKeypoints.emplace_back(spatialKeypoint.imageCoordinates, spatialKeypoint.confidence, spatialKeypoint.label); + } + converted.setKeypoints(std::move(convertedKeypoints), keypoints->getEdges()); + imgDetection.keypoints = std::move(converted); + } + + return imgDetection; +} + +std::vector SpatialImgDetection::getKeypoints() const { + if(keypoints.has_value()) { + return keypoints->getKeypoints(); + } else { + return {}; + } +} + +std::vector SpatialImgDetection::getKeypointSpatialCoordinates() const { + if(keypoints.has_value()) { + return keypoints->getSpatialCoordinates(); + } else { + return {}; + } +} + +std::vector SpatialImgDetection::getEdges() const { + if(keypoints.has_value()) { + return keypoints->getEdges(); + } else { + return {}; + } +} + +void SpatialImgDetection::setEdges(const std::vector edges) { + if(!keypoints.has_value()) { + throw std::runtime_error("No keypoints set, cannot set edges."); + } + keypoints->setEdges(edges); +} + +float SpatialImgDetection::getCenterX() const { + return getBoundingBox().center.x; +} + +float SpatialImgDetection::getCenterY() const { + return getBoundingBox().center.y; +} + +float SpatialImgDetection::getWidth() const { + return getBoundingBox().size.width; +} + +float SpatialImgDetection::getHeight() const { + return getBoundingBox().size.height; +} + +float SpatialImgDetection::getAngle() const { + return getBoundingBox().angle; +} + SpatialImgDetections::~SpatialImgDetections() = default; void SpatialImgDetections::serialize(std::vector& metadata, DatatypeEnum& datatype) const { diff --git a/src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp b/src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp index e5ffbee4a2..63afca553d 100644 --- a/src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp +++ b/src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp @@ -21,4 +21,70 @@ std::vector SpatialLocationCalculatorConfig return config; } +void SpatialLocationCalculatorConfig::setDepthThresholds(uint32_t lowerThreshold, uint32_t upperThreshold) { + if(lowerThreshold >= upperThreshold) { + throw std::invalid_argument("Lower threshold must be less than upper threshold"); + } + globalLowerThreshold = lowerThreshold; + globalUpperThreshold = upperThreshold; +} + +void SpatialLocationCalculatorConfig::setCalculationAlgorithm(SpatialLocationCalculatorAlgorithm calculationAlgorithm) { + globalCalculationAlgorithm = calculationAlgorithm; +} + +void SpatialLocationCalculatorConfig::setStepSize(int32_t stepSize) { + if(stepSize < -1 || stepSize == 0) { + throw std::invalid_argument("Step size must be -1 (AUTO) or a positive integer."); + } + globalStepSize = stepSize; +} + +void SpatialLocationCalculatorConfig::setKeypointRadius(int32_t radius) { + if(radius <= 0) { + throw std::invalid_argument("Keypoint radius must be positive"); + } + globalKeypointRadius = radius; +} + +void SpatialLocationCalculatorConfig::setCalculateSpatialKeypoints(bool calculateSpatialKeypoints) { + this->calculateSpatialKeypoints = calculateSpatialKeypoints; +} + +void SpatialLocationCalculatorConfig::setUseSegmentation(bool useSegmentation) { + this->useSegmentation = useSegmentation; +} + +void SpatialLocationCalculatorConfig::setSegmentationPassthrough(bool passthroughSegmentation) { + this->segmentationPassthrough = passthroughSegmentation; +} + +std::pair SpatialLocationCalculatorConfig::getDepthThresholds() const { + return {globalLowerThreshold, globalUpperThreshold}; +} + +SpatialLocationCalculatorAlgorithm SpatialLocationCalculatorConfig::getCalculationAlgorithm() const { + return globalCalculationAlgorithm; +} + +int32_t SpatialLocationCalculatorConfig::getStepSize() const { + return globalStepSize; +} + +int32_t SpatialLocationCalculatorConfig::getKeypointRadius() const { + return globalKeypointRadius; +} + +bool SpatialLocationCalculatorConfig::getCalculateSpatialKeypoints() const { + return calculateSpatialKeypoints; +} + +bool SpatialLocationCalculatorConfig::getUseSegmentation() const { + return useSegmentation; +} + +bool SpatialLocationCalculatorConfig::getSegmentationPassthrough() const { + return segmentationPassthrough; +} + } // namespace dai diff --git a/src/pipeline/node/DetectionParser.cpp b/src/pipeline/node/DetectionParser.cpp index 36eab876f0..b21fd45a3e 100644 --- a/src/pipeline/node/DetectionParser.cpp +++ b/src/pipeline/node/DetectionParser.cpp @@ -4,14 +4,12 @@ #include #include #include -#include #include #include #include "common/DetectionNetworkType.hpp" #include "common/ModelType.hpp" #include "common/YoloDecodingFamily.hpp" -#include "depthai/modelzoo/Zoo.hpp" #include "nn_archive/NNArchive.hpp" #include "nn_archive/v1/Head.hpp" #include "pipeline/ThreadedNodeImpl.hpp" diff --git a/src/pipeline/node/ObjectTracker.cpp b/src/pipeline/node/ObjectTracker.cpp index e791b3291c..30095cce04 100644 --- a/src/pipeline/node/ObjectTracker.cpp +++ b/src/pipeline/node/ObjectTracker.cpp @@ -156,7 +156,7 @@ void ObjectTracker::run() { } for(size_t i = 0; i < (inputImgDetections ? inputImgDetections->detections.size() : inputSpatialImgDetections->detections.size()); ++i) { - const auto& detection = inputImgDetections ? inputImgDetections->detections[i] : (ImgDetection)inputSpatialImgDetections->detections[i]; + const auto& detection = inputImgDetections ? inputImgDetections->detections[i] : inputSpatialImgDetections->detections[i].getImgDetection(); if(detection.confidence >= trackerThreshold && (detectionLabelsToTrack.empty() || contains(detectionLabelsToTrack, detection.label))) { // Denormalize and remap to inputTrackerImg uint32_t width = detectionsTransformation.getSize().first; diff --git a/src/pipeline/node/SpatialLocationCalculator.cpp b/src/pipeline/node/SpatialLocationCalculator.cpp index 6f0a19d57e..230a2febb1 100644 --- a/src/pipeline/node/SpatialLocationCalculator.cpp +++ b/src/pipeline/node/SpatialLocationCalculator.cpp @@ -1,17 +1,126 @@ #include "depthai/pipeline/node/SpatialLocationCalculator.hpp" -#include "spdlog/fmt/fmt.h" +#include "depthai/depthai.hpp" +#include "pipeline/ThreadedNodeImpl.hpp" +#include "pipeline/utilities/SpatialLocationCalculator/SpatialUtils.hpp" namespace dai { namespace node { -SpatialLocationCalculator::SpatialLocationCalculator(std::unique_ptr props) - : DeviceNodeCRTP(std::move(props)) {} - SpatialLocationCalculator::Properties& SpatialLocationCalculator::getProperties() { properties.roiConfig = *initialConfig; return properties; } +void SpatialLocationCalculator::setRunOnHost(bool runOnHost) { + runOnHostVar = runOnHost; +} + +bool SpatialLocationCalculator::runOnHost() const { + return runOnHostVar; +} + +void SpatialLocationCalculator::run() { + auto& logger = ThreadedNode::pimpl->logger; + logger->debug("Start SpatialLocationCalculator"); + + bool inputConfigSync = inputConfig.getWaitForMessage(); + if(!calculationConfig) { + calculationConfig = initialConfig; + } + + using std::chrono::duration_cast; + using std::chrono::high_resolution_clock; + using std::chrono::microseconds; + using std::chrono::steady_clock; + while(mainLoop()) { + std::shared_ptr imgFrame; + auto outputSpatialImgDetections = std::make_shared(); + std::shared_ptr imgDetections = nullptr; + + auto tAbsoluteBeginning = steady_clock::now(); + { + bool hasConfig = false; + std::shared_ptr config; + auto blockEvent = this->inputBlockEvent(); + if(inputConfigSync) { + config = inputConfig.get(); + if(config == nullptr) { + logger->critical("Invalid input config."); + } else { + hasConfig = true; + } + } else { + auto tmpConfig = inputConfig.tryGet(); + if(tmpConfig != nullptr) { + config = tmpConfig; + hasConfig = true; + } + } + if(hasConfig) { + calculationConfig = config; + } + + imgFrame = inputDepth.get(); + if(imgFrame == nullptr) { + logger->warn("Invalid input depth. Skipping frame."); + continue; + } + if(imgFrame->getType() != dai::ImgFrame::Type::RAW16) { + logger->warn("Invalid frame type for depth image. Depth image must be RAW16 type, got {}", static_cast(imgFrame->getType())); + continue; + } + if(inputDetections.isConnected()) { // detections are connected and need to be processed + imgDetections = inputDetections.get(); + } + } + + auto tAfterMessageBeginning = steady_clock::now(); + + std::vector spatialLocations; + + auto start = high_resolution_clock::now(); + auto outputSpatial = std::make_shared(); + outputSpatial->setSequenceNum(imgFrame->getSequenceNum()); + outputSpatial->setTimestampDevice(imgFrame->getTimestampDevice()); + outputSpatial->setTimestamp(imgFrame->getTimestamp()); + if(calculationConfig->getConfigData().size() > 0) { + utilities::SpatialUtils::computeSpatialData(imgFrame, calculationConfig->getConfigData(), spatialLocations, logger); + auto stop = high_resolution_clock::now(); + auto timeToComputeSpatialData = duration_cast(stop - start); + + logger->trace("Time to compute spatial data: {} us", timeToComputeSpatialData.count()); + outputSpatial->spatialLocations = std::move(spatialLocations); + } + + if(imgDetections != nullptr) { // process imgDetections + start = high_resolution_clock::now(); + utilities::SpatialUtils::computeSpatialDetections(*imgFrame, *calculationConfig, *imgDetections, *outputSpatialImgDetections, logger); + + auto stop = high_resolution_clock::now(); + auto timeToComputeSpatialDetections = duration_cast(stop - start); + logger->trace("Time to compute spatial detections: {} us", timeToComputeSpatialDetections.count()); + outputSpatialImgDetections->setSequenceNum(imgDetections->getSequenceNum()); + outputSpatialImgDetections->setTimestampDevice(imgDetections->getTimestampDevice()); + outputSpatialImgDetections->setTimestamp(imgDetections->getTimestamp()); + outputSpatialImgDetections->transformation = imgDetections->transformation; + } + + auto tBeforeSend = steady_clock::now(); + { + auto blockEvent = this->outputBlockEvent(); + + outputDetections.send(outputSpatialImgDetections); + out.send(outputSpatial); + passthroughDepth.send(imgFrame); + } + auto tAbsoluteEnd = steady_clock::now(); + logger->debug("SpatialLocationCalculator total took {} ms, processing {} ms, getting_frames {} ms, sending_frames {} ms", + duration_cast(tAbsoluteEnd - tAbsoluteBeginning).count() / 1000, + duration_cast(tBeforeSend - tAfterMessageBeginning).count() / 1000, + duration_cast(tAfterMessageBeginning - tAbsoluteBeginning).count() / 1000, + duration_cast(tAbsoluteEnd - tBeforeSend).count() / 1000); + } +} } // namespace node -} // namespace dai +} // namespace dai \ No newline at end of file diff --git a/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp new file mode 100644 index 0000000000..64c226487f --- /dev/null +++ b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp @@ -0,0 +1,462 @@ +#include "SpatialUtils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/ImgTransformations.hpp" +#include "common/Keypoint.hpp" +#include "common/Point2f.hpp" +#include "common/RotatedRect.hpp" +#include "depthai/common/Point3f.hpp" +#include "depthai/common/SpatialKeypoint.hpp" +#include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" +#include "pipeline/datatype/SpatialImgDetections.hpp" + +namespace dai { +namespace utilities { +namespace SpatialUtils { + +DepthStats::DepthStats(std::uint32_t lowerThreshold, std::uint32_t upperThreshold, std::uint32_t maxNumPixels) + : lowerThreshold(lowerThreshold), upperThreshold(upperThreshold) { + validPixels.reserve(maxNumPixels); +} + +void DepthStats::addPixel(uint16_t px) { + if(px > lowerThreshold && px < upperThreshold) { + validPixels.push_back(px); + sum += static_cast(px); + ++counter; + if(px < min) min = static_cast(px); + if(px > max) max = static_cast(px); + } +} + +float DepthStats::calculateDepth(SpatialLocationCalculatorAlgorithm algo) { + if(counter == 0 || validPixels.empty()) { + return 0; + } + + float z = 0; + size_t midIndex = validPixels.size() / 2; + switch(algo) { + default: + case SpatialLocationCalculatorAlgorithm::MEDIAN: + std::nth_element(validPixels.begin(), validPixels.begin() + midIndex, validPixels.end()); + z = validPixels[midIndex]; + break; + case SpatialLocationCalculatorAlgorithm::AVERAGE: + z = (float)sum / counter; + break; + case SpatialLocationCalculatorAlgorithm::MIN: + z = min; + break; + case SpatialLocationCalculatorAlgorithm::MAX: + z = max; + break; + case SpatialLocationCalculatorAlgorithm::MODE: { + constexpr std::size_t MAX_HISTOGRAM_RANGE = 4096; + uint16_t minVal = static_cast(min); + uint16_t maxVal = static_cast(max); + if(maxVal < minVal) { + z = 0; + break; + } + std::size_t valueRange = static_cast(maxVal - minVal) + 1; + + if(valueRange <= MAX_HISTOGRAM_RANGE) { + // Small range: build a dense histogram for cache-friendly mode lookup + std::vector histogram(valueRange, 0); + for(const auto& px : validPixels) { + if(px < minVal || px > maxVal) { + continue; + } + histogram[px - minVal]++; + } + + uint32_t maxCount = 0; + uint16_t modeValue = minVal; + for(std::size_t i = 0; i < histogram.size(); ++i) { + if(histogram[i] > maxCount) { + maxCount = histogram[i]; + modeValue = static_cast(minVal + i); + } + } + z = maxCount ? modeValue : 0; + } else { + std::sort(validPixels.begin(), validPixels.end()); + + uint16_t modeValue = validPixels[0]; + uint32_t maxCount = 1; + uint32_t currentCount = 1; + + for(std::size_t i = 1; i < validPixels.size(); ++i) { + if(validPixels[i] == validPixels[i - 1]) { + currentCount++; + } else { + if(currentCount > maxCount) { + maxCount = currentCount; + modeValue = validPixels[i - 1]; + } + currentCount = 1; + } + } + + if(currentCount > maxCount) { + modeValue = validPixels.back(); + } + + z = modeValue; + } + break; + } + } + return z; +} + +dai::Point3f calculateSpatialCoordinates(float z, const std::array, 3>& intrinsicMatrix, dai::Point2f center) { + float fx = intrinsicMatrix[0][0]; + float fy = intrinsicMatrix[1][1]; + float cx = intrinsicMatrix[0][2]; + float cy = intrinsicMatrix[1][2]; + + float x = (center.x - cx) * z / fx; + float y = (center.y - cy) * z / fy; + + return dai::Point3f(x, y, z); +} + +static inline dai::Point2f clampPoint(dai::Point2f point, int width, int height) { + float x = std::min(std::max(0.0f, point.x), static_cast(width)); + float y = std::min(std::max(0.0f, point.y), static_cast(height)); + return dai::Point2f(x, y); +} + +static inline std::array clampRectCorners(dai::Point2f topLeft, dai::Point2f bottomRight, int width, int height) { + dai::Point2f clampedTopLeft = clampPoint(topLeft, width - 1, height - 1); // -1 to ensure start is within bounds + dai::Point2f clampedBottomRight = clampPoint(bottomRight, width, height); + + int xstart = std::min(std::max(0, static_cast(clampedTopLeft.x)), width); + int ystart = std::min(std::max(0, static_cast(clampedTopLeft.y)), height); + int xend = std::min(std::max(0, static_cast(clampedBottomRight.x)), width); + int yend = std::min(std::max(0, static_cast(clampedBottomRight.y)), height); + + return {xstart, ystart, xend, yend}; +} + +static inline unsigned int getStepSize(int stepSize, SpatialLocationCalculatorAlgorithm algo) { + if(stepSize == SpatialLocationCalculatorConfigData::AUTO) { + if(algo == SpatialLocationCalculatorAlgorithm::MODE || algo == SpatialLocationCalculatorAlgorithm::MEDIAN) { + return 2; + } else { + return 1; + } + } + + return static_cast(stepSize); +} + +dai::SpatialKeypoint createSpatialKeypoint(const dai::Keypoint& keypoint, const dai::Point3f& spatialCoordinates) { + dai::SpatialKeypoint spatialKeypoint; + spatialKeypoint.imageCoordinates = keypoint.imageCoordinates; + spatialKeypoint.confidence = keypoint.confidence; + spatialKeypoint.label = keypoint.label; + spatialKeypoint.labelName = keypoint.labelName; + spatialKeypoint.spatialCoordinates = spatialCoordinates; + return spatialKeypoint; +} + +dai::SpatialImgDetection createSpatialDetection(const dai::ImgDetection& detection, const dai::Point3f& spatialCoordinates) { + dai::SpatialImgDetection spatialDetection; + spatialDetection.setBoundingBox(detection.getBoundingBox()); + spatialDetection.label = detection.label; + spatialDetection.labelName = detection.labelName; + spatialDetection.confidence = detection.confidence; + spatialDetection.spatialCoordinates = spatialCoordinates; + return spatialDetection; +} + +void computeSpatialData(const std::shared_ptr& depthFrame, + const std::vector& configDataVec, + std::vector& spatialLocations, + const std::shared_ptr& logger) { + if(configDataVec.empty()) { + logger->debug("Empty config vector!"); + return; + } + + if(!depthFrame->transformation.isValid()) { + throw std::runtime_error("DepthFrame has invalid Image Transformations. Cannot get intrinsic matrix."); + } + + spatialLocations.resize(configDataVec.size()); + std::int32_t depthWidth = depthFrame->getWidth(); + std::int32_t depthHeight = depthFrame->getHeight(); + for(size_t i = 0; i < configDataVec.size(); i++) { + const auto& cfg = configDataVec[i]; + const auto& roi = cfg.roi; + float maxWidthSize = 1; + float maxHeightSize = 1; + const SpatialLocationCalculatorAlgorithm calcAlgo = cfg.calculationAlgorithm; + const int stepSize = getStepSize(cfg.stepSize, calcAlgo); + if(stepSize >= depthWidth / 2 || stepSize >= depthHeight / 2) { + auto message = fmt::format("Step size {} is too large for depth frame size {}x{}. It must be less than half of the depth frame dimensions.", + stepSize, + depthWidth, + depthHeight); + throw std::invalid_argument(message); + } + if(!roi.isNormalized()) { + maxWidthSize = depthFrame->getWidth(); + maxHeightSize = depthFrame->getHeight(); + } + if((roi.x + roi.width > maxWidthSize) || (roi.y + roi.height > maxHeightSize)) { + logger->error("ROI x:{} y:{} width:{} height:{} is not a valid rectangle. Out of depth map range: {}x{}", + roi.x, + roi.y, + roi.width, + roi.height, + maxWidthSize, + maxHeightSize); + spatialLocations[i] = SpatialLocations(); + continue; + } + + dai::Rect denormRoi = roi.denormalize(depthWidth, depthHeight); + auto [xstart, ystart, xend, yend] = clampRectCorners(denormRoi.topLeft(), denormRoi.bottomRight(), depthWidth, depthHeight); + + std::uint32_t maxNumPixels = ((xend - xstart + stepSize - 1) / stepSize) * ((yend - ystart + stepSize - 1) / stepSize); + DepthStats depthStats(cfg.depthThresholds.lowerThreshold, cfg.depthThresholds.upperThreshold, maxNumPixels); + + const uint16_t* plane = (uint16_t*)depthFrame->data->getData().data(); + for(int y = ystart; y < yend; y += stepSize) { + for(int x = xstart; x < xend; x += stepSize) { + uint16_t px = plane[y * depthWidth + x]; + depthStats.addPixel(px); + } + } + + SpatialLocations spatialData = {}; + spatialData.depthAverage = depthStats.calculateDepth(SpatialLocationCalculatorAlgorithm::AVERAGE); + spatialData.depthMin = depthStats.min; + spatialData.depthMax = depthStats.max; + spatialData.config = cfg; + spatialData.depthAveragePixelCount = depthStats.counter; + + float z = depthStats.calculateDepth(calcAlgo); + + float roiCx = denormRoi.x + denormRoi.width / 2.0f; + float roiCy = denormRoi.y + denormRoi.height / 2.0f; + + dai::Point3f spatialCoordinates = calculateSpatialCoordinates(z, depthFrame->transformation.getIntrinsicMatrix(), dai::Point2f(roiCx, roiCy)); + logger->trace("Calculated spatial coordinates: {} {} {}", spatialCoordinates.x, spatialCoordinates.y, spatialCoordinates.z); + + spatialData.spatialCoordinates = spatialCoordinates; + spatialLocations[i] = spatialData; + } +} +template +void populateDepthStats(DepthStats& depthStats, + dai::RotatedRect rectCorners, + unsigned int stepSize, + unsigned int widthMultiplier, + const dai::ImgFrame& depthFrame, + PixelValidatorFn&& validatePixelFn) { + const auto depthWidth = static_cast(depthFrame.getWidth()); + const auto depthHeight = static_cast(depthFrame.getHeight()); + + const auto& outerPoints = rectCorners.getOuterRect(); + auto [xstart, ystart, xend, yend] = + clampRectCorners(dai::Point2f{outerPoints[0], outerPoints[1]}, dai::Point2f{outerPoints[2], outerPoints[3]}, depthWidth, depthHeight); + + const auto& depthData = depthFrame.data->getData(); + span plane(reinterpret_cast(depthData.data()), depthData.size() / sizeof(uint16_t)); + for(int y = ystart; y < yend; y += static_cast(stepSize)) { + for(int x = xstart; x < xend; x += static_cast(stepSize)) { + int index = (y * widthMultiplier) + x; + uint16_t px = plane[index]; + + if(std::forward(validatePixelFn)(x, y)) { + depthStats.addPixel(px); + } + } + } +} + +dai::SpatialImgDetection computeSpatialDetection(const dai::ImgFrame& depthFrame, + const dai::ImgDetection& detection, + const span& maskSpan, + const SpatialLocationCalculatorConfig& config, + const int instanceIndex, + const dai::ImgTransformation& detectionsTransformation, + const int segMaskWidth, + const int segMaskHeight, + const std::shared_ptr& logger) { + const uint32_t lowerThreshold = config.globalLowerThreshold; + const uint32_t upperThreshold = config.globalUpperThreshold; + const SpatialLocationCalculatorAlgorithm calculationAlgorithm = config.globalCalculationAlgorithm; + const unsigned int stepSize = getStepSize(config.globalStepSize, calculationAlgorithm); + const int32_t keypointRadius = config.globalKeypointRadius; + const unsigned int depthWidth = depthFrame.getWidth(); + const unsigned int depthHeight = depthFrame.getHeight(); + + const dai::ImgTransformation* depthTransformation = &depthFrame.transformation; + const bool areAligned = detectionsTransformation.isAlignedTo(*depthTransformation); + + const dai::RotatedRect detectionBBox = detection.getBoundingBox(); + dai::RotatedRect denormalizedRect = detectionBBox; + if(!areAligned) denormalizedRect = detectionsTransformation.remapRectTo(*depthTransformation, denormalizedRect); + denormalizedRect = denormalizedRect.denormalize(depthWidth, depthHeight); + + if(!maskSpan.empty()) { + assert(maskSpan.size() == static_cast(segMaskWidth * segMaskHeight)); // should never happen + } + + auto usePixelSeg = [&](int x, int y) -> bool { + if(maskSpan.empty()) return true; + dai::Point2f pt{static_cast(x), static_cast(y)}; + if(!areAligned) { + pt = detectionsTransformation.remapPointTo(*depthTransformation, pt); + pt = clampPoint(pt, segMaskWidth - 1, segMaskHeight - 1); + } + int maskIndex = static_cast(pt.y) * segMaskWidth + static_cast(pt.x); + return maskSpan[maskIndex] == instanceIndex; + }; + + // object depth + std::uint32_t maxNumPixels = ((denormalizedRect.size.width * denormalizedRect.size.height) / (stepSize * stepSize)); + DepthStats depthStats(lowerThreshold, upperThreshold, maxNumPixels); + populateDepthStats(depthStats, denormalizedRect, stepSize, depthWidth, depthFrame, usePixelSeg); + + dai::Point2f centerPoint = denormalizedRect.center; + if(!areAligned) centerPoint = depthTransformation->remapPointTo(detectionsTransformation, centerPoint); + dai::Point3f spatialCoordinates = + calculateSpatialCoordinates(depthStats.calculateDepth(calculationAlgorithm), detectionsTransformation.getIntrinsicMatrix(), centerPoint); + logger->trace("Calculated spatial coordinates: {} {} {}", spatialCoordinates.x, spatialCoordinates.y, spatialCoordinates.z); + + dai::SpatialImgDetection spatialDetection = createSpatialDetection(detection, spatialCoordinates); + + if(config.calculateSpatialKeypoints) { // keypoint depths + std::vector spatialKeypoints; + for(auto kp : detection.getKeypoints()) { + dai::Point2f mappedKp = dai::Point2f{kp.imageCoordinates.x, kp.imageCoordinates.y}; + + float kpx = mappedKp.x * depthWidth; + float kpy = mappedKp.y * depthHeight; + dai::RotatedRect keypointRect{ + dai::Point2f{kpx, kpy, false}, + dai::Size2f{static_cast(keypointRadius * 2), static_cast(keypointRadius * 2), false}, + 0.0f, + }; + + auto useKpPixel = [kpx, kpy, keypointRadius, usePixelSeg](int x, int y) { + bool usePixel = (std::hypot(kpx - x, kpy - y) <= keypointRadius); + return usePixel && usePixelSeg(x, y); + }; + + DepthStats kpDepthStats(lowerThreshold, upperThreshold, static_cast(keypointRadius * keypointRadius)); + populateDepthStats(kpDepthStats, keypointRect, 1, depthWidth, depthFrame, useKpPixel); + + float kpz = kpDepthStats.calculateDepth(calculationAlgorithm); + dai::Point2f kpCenter{kpx, kpy}; + if(!areAligned) kpCenter = depthTransformation->remapPointTo(detectionsTransformation, kpCenter); + dai::Point3f spatialCoordinates = calculateSpatialCoordinates(kpz, detectionsTransformation.getIntrinsicMatrix(), kpCenter); + + spatialKeypoints.push_back(createSpatialKeypoint(kp, spatialCoordinates)); + } + spatialDetection.setKeypoints(spatialKeypoints); + } + + SpatialLocationCalculatorConfigData boundingBoxMapping; + boundingBoxMapping.depthThresholds.lowerThreshold = lowerThreshold; + boundingBoxMapping.depthThresholds.upperThreshold = upperThreshold; + boundingBoxMapping.calculationAlgorithm = calculationAlgorithm; + spatialDetection.boundingBoxMapping = boundingBoxMapping; + + return spatialDetection; +} + +void computeSpatialDetections(const dai::ImgFrame& depthFrame, + const SpatialLocationCalculatorConfig& config, + const dai::ImgDetections& imgDetections, + dai::SpatialImgDetections& spatialDetections, + const std::shared_ptr& logger) { + if(!imgDetections.transformation.has_value()) { + throw std::runtime_error("No transformation set on ImgDetections. Cannot compute spatial coordinates."); + } + + if(!imgDetections.transformation.value().isValid()) { + throw std::runtime_error("ImgDetections has invalid Image Transformations. Cannot map segmentation mask to depth frame."); + } + + if(!depthFrame.transformation.isValid()) { + throw std::runtime_error("DepthFrame has invalid Image Transformations. Cannot map segmentation mask to depth frame."); + } + + const dai::ImgTransformation* depthTransformation = &depthFrame.transformation; + const dai::ImgTransformation* detectionsTransformation = &imgDetections.transformation.value(); + const bool areAligned = detectionsTransformation->isAlignedTo(*depthTransformation); + if(!areAligned) { + logger->warn( + "DepthFrame and ImgDetections transformations are not aligned and processing may be slowed down due to need for remapping points. Consider " + "using " + "ImageAlign node beforehand."); + } + + std::vector imgDetectionsVector = imgDetections.detections; + if(imgDetectionsVector.size() == 0) { + return; + } + spatialDetections.detections.resize(imgDetectionsVector.size()); + + const SpatialLocationCalculatorAlgorithm calculationAlgorithm = config.globalCalculationAlgorithm; + const unsigned int stepSize = getStepSize(config.globalStepSize, calculationAlgorithm); + + const unsigned int depthWidth = depthFrame.getWidth(); + const unsigned int depthHeight = depthFrame.getHeight(); + const std::size_t segmentationMaskWidth = imgDetections.getSegmentationMaskWidth(); + const std::size_t segmentationMaskHeight = imgDetections.getSegmentationMaskHeight(); + + span maskSpan = imgDetections.getData(); + const bool passthroughSegmentation = config.segmentationPassthrough && !maskSpan.empty(); + if(!config.useSegmentation) { // ignore segmentation mask even if provided + maskSpan = span{}; + } + + if(stepSize >= depthWidth / 2 || stepSize >= depthHeight / 2) { + auto message = fmt::format("Step size {} is too large for depth frame size {}x{}. It must be less than half of the depth frame dimensions.", + stepSize, + depthWidth, + depthHeight); + throw std::invalid_argument(message); + } + + for(int i = 0; i < static_cast(imgDetectionsVector.size()); i++) { + const dai::ImgDetection& detection = imgDetectionsVector[i]; + dai::SpatialImgDetection spatialImgDetection = computeSpatialDetection(depthFrame, + detection, + maskSpan, + config, + i, + *detectionsTransformation, + static_cast(segmentationMaskWidth), + static_cast(segmentationMaskHeight), + logger); + spatialDetections.detections[i] = spatialImgDetection; + } + + if(passthroughSegmentation) { + spatialDetections.data = imgDetections.data; + spatialDetections.segmentationMaskWidth = imgDetections.getSegmentationMaskWidth(); + spatialDetections.segmentationMaskHeight = imgDetections.getSegmentationMaskHeight(); + } +} + +} // namespace SpatialUtils +} // namespace utilities +} // namespace dai diff --git a/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.hpp b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.hpp new file mode 100644 index 0000000000..0c6ed0fc35 --- /dev/null +++ b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.hpp @@ -0,0 +1,43 @@ +#pragma once +#include + +#include +#include +#include +#include + +#include "depthai/pipeline/datatype/SpatialImgDetections.hpp" + +namespace dai { +namespace utilities { +namespace SpatialUtils { + +struct DepthStats { + std::double_t sum = 0; + std::uint32_t counter = 0; + std::uint32_t max = std::numeric_limits::min(); + std::uint32_t min = std::numeric_limits::max(); + std::uint32_t lowerThreshold = 100; + std::uint32_t upperThreshold = std::numeric_limits::max(); + std::vector validPixels; + + DepthStats(std::uint32_t lowerThreshold, std::uint32_t upperThreshold, std::uint32_t maxNumPixels); + + void addPixel(uint16_t px); + + float calculateDepth(SpatialLocationCalculatorAlgorithm algo); +}; + +void computeSpatialData(const std::shared_ptr& depthFrame, + const std::vector& configDataVec, + std::vector& spatialLocations, + const std::shared_ptr& logger); + +void computeSpatialDetections(const dai::ImgFrame& depthFrame, + const SpatialLocationCalculatorConfig& config, + const dai::ImgDetections& imgDetections, + dai::SpatialImgDetections& spatialDetections, + const std::shared_ptr& logger); +} // namespace SpatialUtils +} // namespace utilities +} // namespace dai \ No newline at end of file diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index bce13b9226..6fed5ff19d 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -259,7 +259,7 @@ std::unique_ptr getProtoMessage(const ImgAnnotations* return imageAnnotations; } template <> -std::unique_ptr getProtoMessage(const SpatialImgDetections* message, bool) { +std::unique_ptr getProtoMessage(const SpatialImgDetections* message, bool metadataOnly) { // create and populate SpatialImgDetections protobuf message auto spatialImgDetections = std::make_unique(); spatialImgDetections->set_sequencenum(message->sequenceNum); @@ -273,51 +273,102 @@ std::unique_ptr getProtoMessage(const SpatialImgDetec tsDevice->set_nsec(message->tsDevice.nsec); for(const auto& detection : message->detections) { - proto::spatial_img_detections::SpatialImgDetection* spatialImgDetection = spatialImgDetections->add_detections(); + auto* spatialImgDetection = spatialImgDetections->add_detections(); + + // Populate embedded ImgDetection message. + auto* protoDetection = spatialImgDetection->mutable_detection(); + protoDetection->set_label(detection.label); + protoDetection->set_labelname(detection.labelName); + protoDetection->set_confidence(detection.confidence); + protoDetection->set_xmin(detection.xmin); + protoDetection->set_ymin(detection.ymin); + protoDetection->set_xmax(detection.xmax); + protoDetection->set_ymax(detection.ymax); - // populate SpatialImgDetection.ImgDetection from struct inheritance - proto::img_detections::ImgDetection* imgDetection = spatialImgDetection->mutable_detection(); - imgDetection->set_label(detection.label); - imgDetection->set_labelname(detection.labelName); - imgDetection->set_confidence(detection.confidence); - imgDetection->set_xmin(detection.xmin); - imgDetection->set_ymin(detection.ymin); - imgDetection->set_xmax(detection.xmax); - imgDetection->set_ymax(detection.ymax); + if(detection.boundingBox.has_value() || !(detection.xmin == 0.f && detection.xmax == 0.f && detection.ymin == 0.f && detection.ymax == 0.f)) { + const auto bbox = detection.boundingBox.has_value() ? detection.boundingBox.value() : detection.getBoundingBox(); + auto* bboxProto = protoDetection->mutable_boundingbox(); + auto* center = bboxProto->mutable_center(); + center->set_x(bbox.center.x); + center->set_y(bbox.center.y); + auto* size = bboxProto->mutable_size(); + size->set_width(bbox.size.width); + size->set_height(bbox.size.height); + bboxProto->set_angle(bbox.angle); + } - // populate SpatialImgDetection.Point3f - proto::spatial_img_detections::Point3f* spatialCoordinates = spatialImgDetection->mutable_spatialcoordinates(); + // Populate SpatialImgDetection.Point3f + auto* spatialCoordinates = spatialImgDetection->mutable_spatialcoordinates(); spatialCoordinates->set_x(detection.spatialCoordinates.x); spatialCoordinates->set_y(detection.spatialCoordinates.y); spatialCoordinates->set_z(detection.spatialCoordinates.z); - // populate SpatialImgDetection.SpatialLocationCalculatorConfigData - proto::spatial_img_detections::SpatialLocationCalculatorConfigData* boundingBoxMapping = spatialImgDetection->mutable_boundingboxmapping(); + // Populate SpatialImgDetection.SpatialLocationCalculatorConfigData + auto* boundingBoxMapping = spatialImgDetection->mutable_boundingboxmapping(); - // populate SpatialImgDetection.SpatialLocationCalculatorConfigData.Rect - proto::spatial_img_detections::Rect* roi = boundingBoxMapping->mutable_roi(); + // Populate SpatialImgDetection.SpatialLocationCalculatorConfigData.Rect + auto* roi = boundingBoxMapping->mutable_roi(); roi->set_x(detection.boundingBoxMapping.roi.x); roi->set_y(detection.boundingBoxMapping.roi.y); roi->set_width(detection.boundingBoxMapping.roi.width); roi->set_height(detection.boundingBoxMapping.roi.height); - // populate SpatialImgDetection.SpatialLocationCalculatorConfigData.SpatialLocationCalculatorConfigThresholds - proto::spatial_img_detections::SpatialLocationCalculatorConfigThresholds* depthTresholds = boundingBoxMapping->mutable_depththresholds(); + // Populate SpatialImgDetection.SpatialLocationCalculatorConfigData.SpatialLocationCalculatorConfigThresholds + auto* depthTresholds = boundingBoxMapping->mutable_depththresholds(); depthTresholds->set_lowerthreshold(detection.boundingBoxMapping.depthThresholds.lowerThreshold); depthTresholds->set_upperthreshold(detection.boundingBoxMapping.depthThresholds.upperThreshold); - // populate SpatialImgDetection.SpatialLocationCalculatorConfigData.SpatialLocationCalculatorAlgorithm + // Populate SpatialImgDetection.SpatialLocationCalculatorConfigData.SpatialLocationCalculatorAlgorithm boundingBoxMapping->set_calculationalgorithm( static_cast(detection.boundingBoxMapping.calculationAlgorithm)); - // populate SpatialImgDetection.SpatialLocationCalculatorConfigData.stepSize + // Populate SpatialImgDetection.SpatialLocationCalculatorConfigData.stepSize boundingBoxMapping->set_stepsize(detection.boundingBoxMapping.stepSize); + + if(detection.keypoints.has_value()) { + const auto& keypointsList = detection.keypoints.value(); + const auto keypointsVec = keypointsList.getKeypoints(); + const auto edgesVec = keypointsList.getEdges(); + + // Spatial keypoints with spatial coordinates. + auto* protoSpatialKeypoints = spatialImgDetection->mutable_keypoints(); + for(const auto& keypoint : keypointsVec) { + auto* protoKeypoint = protoSpatialKeypoints->add_keypoints(); + auto* coords = protoKeypoint->mutable_imagecoordinates(); + coords->set_x(keypoint.imageCoordinates.x); + coords->set_y(keypoint.imageCoordinates.y); + coords->set_z(keypoint.imageCoordinates.z); + protoKeypoint->set_confidence(keypoint.confidence); + protoKeypoint->set_label(keypoint.label); + protoKeypoint->set_labelname(keypoint.labelName); + + auto* spatialCoords = protoKeypoint->mutable_spatialcoordinates(); + spatialCoords->set_x(keypoint.spatialCoordinates.x); + spatialCoords->set_y(keypoint.spatialCoordinates.y); + spatialCoords->set_z(keypoint.spatialCoordinates.z); + } + for(const auto& edge : edgesVec) { + auto* protoEdge = protoSpatialKeypoints->add_edges(); + protoEdge->set_src(edge[0]); + protoEdge->set_dst(edge[1]); + } + } } + proto::common::ImgTransformation* imgTransformation = spatialImgDetections->mutable_transformation(); if(message->transformation.has_value()) { utility::serializeImgTransformation(imgTransformation, message->transformation.value()); } + spatialImgDetections->set_segmentationmaskwidth(static_cast(message->getSegmentationMaskWidth())); + spatialImgDetections->set_segmentationmaskheight(static_cast(message->getSegmentationMaskHeight())); + + if(!metadataOnly) { + std::optional> segMaskData = message->getMaskData(); + if(segMaskData) { + spatialImgDetections->set_maskdata((*segMaskData).data(), (*segMaskData).size()); + } + } return spatialImgDetections; } template <> @@ -453,7 +504,7 @@ std::unique_ptr getProtoMessage(const ImgDetections* if(!metadataOnly) { std::optional> segMaskData = message->getMaskData(); if(segMaskData) { - imgDetections->set_data((*segMaskData).data(), (*segMaskData).size()); + imgDetections->set_maskdata((*segMaskData).data(), (*segMaskData).size()); } } return imgDetections; diff --git a/src/utility/ProtoSerialize.hpp b/src/utility/ProtoSerialize.hpp index d67cae0437..db96bd1018 100644 --- a/src/utility/ProtoSerialize.hpp +++ b/src/utility/ProtoSerialize.hpp @@ -48,7 +48,7 @@ std::unique_ptr getProtoMessage(const T*, bool = fals template <> std::unique_ptr getProtoMessage(const ImgAnnotations* message, bool); template <> -std::unique_ptr getProtoMessage(const SpatialImgDetections* message, bool); +std::unique_ptr getProtoMessage(const SpatialImgDetections* message, bool metadataOnly); template <> std::unique_ptr getProtoMessage(const IMUData* message, bool); template <> diff --git a/tests/src/ondevice_tests/img_transformation_test.cpp b/tests/src/ondevice_tests/img_transformation_test.cpp index 3aee47fdb5..b6edbf876b 100644 --- a/tests/src/ondevice_tests/img_transformation_test.cpp +++ b/tests/src/ondevice_tests/img_transformation_test.cpp @@ -227,6 +227,34 @@ TEST_CASE("ImgTransformation remap vertical") { pipeline.stop(); } +// ----------------------------------------------------------------------------- +// ImgTransformation isAlignedTo +// Purpose: +// Validates alignment checks between transformations. Ensures that +// transformations from the same stream are aligned, while transformations +// from differently-sized outputs are not. +// ----------------------------------------------------------------------------- +TEST_CASE("ImgTransformation isAlignedTo") { + dai::Pipeline pipeline; + auto camera = pipeline.create()->build(); + auto alignedOut = camera->requestOutput({640, 480}); + auto misalignedOut = camera->requestOutput({320, 240}); + auto alignedQueue = alignedOut->createOutputQueue(); + auto misalignedQueue = misalignedOut->createOutputQueue(); + pipeline.start(); + auto alignedFrame = alignedQueue->get(); + auto misalignedFrame = misalignedQueue->get(); + REQUIRE(alignedFrame != nullptr); + REQUIRE(misalignedFrame != nullptr); + pipeline.stop(); + + REQUIRE(alignedFrame->transformation.isValid()); + REQUIRE(misalignedFrame->transformation.isValid()); + REQUIRE(alignedFrame->transformation.isAlignedTo(alignedFrame->transformation)); + REQUIRE_FALSE(alignedFrame->transformation.isAlignedTo(misalignedFrame->transformation)); + REQUIRE_FALSE(misalignedFrame->transformation.isAlignedTo(alignedFrame->transformation)); +} + // ----------------------------------------------------------------------------- // ImgTransformation matrix inverse consistency (ImgFrame) // Purpose: diff --git a/tests/src/ondevice_tests/pipeline/node/spatial_location_calculator_test.cpp b/tests/src/ondevice_tests/pipeline/node/spatial_location_calculator_test.cpp index 0a8ca09b78..b1c5c7632b 100644 --- a/tests/src/ondevice_tests/pipeline/node/spatial_location_calculator_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/spatial_location_calculator_test.cpp @@ -1,13 +1,149 @@ +#include #include #include +#include +#include +#include #include #include +#include +#include +#include +#include +#include +#include #include +#include "depthai/common/RotatedRect.hpp" #include "depthai/depthai.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/datatype/SpatialImgDetections.hpp" +#include "depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp" +#include "depthai/pipeline/node/ImageManip.hpp" +#include "depthai/xlink/XLinkConnection.hpp" using Catch::Approx; +namespace { + +void setDepthValue(cv::Mat& depth, int xStart, int yStart, int xEnd, int yEnd, std::uint16_t value) { + for(int y = yStart; y < yEnd; ++y) { + for(int x = xStart; x < xEnd; ++x) { + depth.at(y, x) = value; + } + } +} + +std::shared_ptr createDepthFrame(const cv::Mat& depthMat, const std::array, 3>& intrinsics) { + const unsigned width = depthMat.cols; + const unsigned height = depthMat.rows; + auto depthFrame = std::make_shared(); + depthFrame->setCvFrame(depthMat, dai::ImgFrame::Type::RAW16); + depthFrame->setSourceSize(width, height); + depthFrame->setTimestamp(std::chrono::steady_clock::now()); + depthFrame->setSequenceNum(0); + depthFrame->transformation.setSourceSize(width, height); + depthFrame->transformation.setSize(width, height); + depthFrame->transformation.setIntrinsicMatrix(intrinsics); + REQUIRE(depthFrame->validateTransformations()); + return depthFrame; +} + +std::shared_ptr createDetectionFrameWithManipulation(const std::shared_ptr& depthFrame, + unsigned width, + unsigned height, + std::optional deviceInfo = std::nullopt) { + constexpr float rotationDegrees = 180.0F; + constexpr std::array shearMatrix = {1.0F, 0.08F, 0.12F, 1.0F}; + + dai::Pipeline pipeline = deviceInfo.has_value() ? dai::Pipeline{std::make_shared(*deviceInfo)} : dai::Pipeline{}; + auto manip = pipeline.create(); + manip->initialConfig->setFrameType(depthFrame->getType()); + manip->initialConfig->setOutputSize(width, height); + manip->initialConfig->addRotateDeg(rotationDegrees); + manip->initialConfig->addTransformAffine(shearMatrix); + + auto inputQueue = manip->inputImage.createInputQueue(); + auto outputQueue = manip->out.createOutputQueue(); + + pipeline.start(); + inputQueue->send(depthFrame); + auto transformedFrame = outputQueue->get(); + REQUIRE(transformedFrame != nullptr); + pipeline.stop(); + pipeline.wait(); + + return transformedFrame; +} + +std::tuple processDepthFrame( + dai::SpatialLocationCalculatorConfig initialConfig, + std::shared_ptr depthMat, + std::optional deviceInfo = std::nullopt, + std::shared_ptr detectionMsg = nullptr, + std::optional> configData = std::nullopt) { + dai::Pipeline pipeline = deviceInfo.has_value() ? dai::Pipeline{std::make_shared(*deviceInfo)} : dai::Pipeline{}; + + auto spatial = pipeline.create(); + + spatial->initialConfig = std::make_shared(initialConfig); + if(configData.has_value()) { + spatial->initialConfig->setROIs(*configData); + } + + auto depthQueue = spatial->inputDepth.createInputQueue(); + std::shared_ptr detectionsQueue = nullptr; + if(detectionMsg != nullptr) { + detectionsQueue = spatial->inputDetections.createInputQueue(); + } + + std::shared_ptr outputQueue = nullptr; + if(detectionMsg != nullptr) { + outputQueue = spatial->outputDetections.createOutputQueue(); + } + auto passthroughQueue = spatial->passthroughDepth.createOutputQueue(); + std::shared_ptr legacyOutputQueue = nullptr; + if(configData.has_value()) { + legacyOutputQueue = spatial->out.createOutputQueue(); + } + + pipeline.start(); + std::shared_ptr legacySpatialData = nullptr; + std::shared_ptr passthroughFrame = nullptr; + std::shared_ptr detectionOutput = nullptr; + + depthQueue->send(depthMat); + if(detectionsQueue != nullptr) { + detectionsQueue->send(detectionMsg); + } + std::shared_ptr spatialDetections = nullptr; + + passthroughFrame = passthroughQueue->get(); + REQUIRE(passthroughFrame != nullptr); + if(outputQueue != nullptr) { + spatialDetections = outputQueue->get(); + REQUIRE(spatialDetections != nullptr); + } + + if(legacyOutputQueue != nullptr) { + legacySpatialData = legacyOutputQueue->get(); + REQUIRE(legacySpatialData != nullptr); + } + + if(spatialDetections != nullptr) { + detectionOutput = spatialDetections; + } + + pipeline.stop(); + pipeline.wait(); + + return std::make_tuple(configData.has_value() ? *legacySpatialData : dai::SpatialLocationCalculatorData(), + *passthroughFrame, + detectionOutput ? *detectionOutput : dai::SpatialImgDetections()); +} + +} // namespace + TEST_CASE("SpatialLocationCalculatorConfig tracks ROI updates") { dai::SpatialLocationCalculatorConfig config; REQUIRE(config.getConfigData().empty()); @@ -85,9 +221,6 @@ TEST_CASE("SpatialLocationCalculator synthetic depth data test") { {dai::Rect(420.0F, 80.0F, 96.0F, 96.0F, false), static_cast(800), 96, 96}, }; - dai::Pipeline pipeline; - auto spatial = pipeline.create(); - std::vector configData; configData.reserve(roiSpecs.size()); for(const auto& spec : roiSpecs) { @@ -99,57 +232,29 @@ TEST_CASE("SpatialLocationCalculator synthetic depth data test") { cfg.stepSize = 1; configData.push_back(cfg); } - spatial->initialConfig->setROIs(configData); - - auto depthQueue = spatial->inputDepth.createInputQueue(); - auto outputQueue = spatial->out.createOutputQueue(); - auto passthroughQueue = spatial->passthroughDepth.createOutputQueue(); - - std::vector depthPixels(width * height, 1000); - auto setRegionDepth = [&](const RoiSpec& spec) { - const int x0 = static_cast(spec.roi.x); - const int y0 = static_cast(spec.roi.y); - for(int y = 0; y < static_cast(spec.heightPx); ++y) { - for(int x = 0; x < static_cast(spec.widthPx); ++x) { - depthPixels[(y0 + y) * width + (x0 + x)] = spec.depth; - } - } - }; - for(const auto& spec : roiSpecs) { - setRegionDepth(spec); - } - - // Prepare synthetic depth frame - auto depthFrame = std::make_shared(); - depthFrame->setType(dai::ImgFrame::Type::RAW16); - depthFrame->setSize(width, height); - depthFrame->setSourceSize(width, height); - std::vector rawDepth(depthPixels.size() * sizeof(std::uint16_t)); - std::memcpy(rawDepth.data(), depthPixels.data(), rawDepth.size()); - depthFrame->setData(rawDepth); const std::array, 3> intrinsics = {{ {{7.5F, 0.0F, 3.2F}}, {{0.0F, 4.8F, 2.4F}}, {{0.0F, 0.0F, 1.0F}}, }}; - depthFrame->transformation.setSourceSize(width, height); - depthFrame->transformation.setSize(width, height); - depthFrame->transformation.setIntrinsicMatrix(intrinsics); - REQUIRE(depthFrame->validateTransformations()); + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(1000)); + for(const auto& spec : roiSpecs) { + const int x0 = static_cast(spec.roi.x); + const int y0 = static_cast(spec.roi.y); + setDepthValue(depthMat, x0, y0, x0 + static_cast(spec.widthPx), y0 + static_cast(spec.heightPx), spec.depth); + } + auto depthFrame = createDepthFrame(depthMat, intrinsics); - pipeline.start(); - depthQueue->send(depthFrame); + dai::SpatialLocationCalculatorConfig config; - auto spatialData = outputQueue->get(); - REQUIRE(spatialData != nullptr); - const auto results = spatialData->getSpatialLocations(); + auto [spatialData, passthroughFrame, spatialDetections] = processDepthFrame(config, depthFrame, std::nullopt, nullptr, configData); + REQUIRE(spatialDetections.detections.empty()); + const auto results = spatialData.getSpatialLocations(); REQUIRE(results.size() == roiSpecs.size()); - auto passthroughFrame = passthroughQueue->get(); - REQUIRE(passthroughFrame != nullptr); - CHECK(passthroughFrame->getWidth() == width); - CHECK(passthroughFrame->getHeight() == height); + CHECK(passthroughFrame.getWidth() == width); + CHECK(passthroughFrame.getHeight() == height); const float fx = intrinsics[0][0]; const float fy = intrinsics[1][1]; @@ -175,7 +280,676 @@ TEST_CASE("SpatialLocationCalculator synthetic depth data test") { CHECK(spatialLoc.spatialCoordinates.y == Approx(expectedY).margin(5.0F)); CHECK(spatialLoc.spatialCoordinates.z == Approx(expectedZ).margin(1.0F)); } +} - pipeline.stop(); - pipeline.wait(); +TEST_CASE("Spatial keypoints support") { + constexpr unsigned width = 800; + constexpr unsigned height = 600; + + const std::array, 3> intrinsics = {{ + {{7.5F, 0.0F, 3.2F}}, + {{0.0F, 4.8F, 2.4F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + constexpr int kpRadius = 10; + const int kp0x = 120; + const int kp0y = 200; + const int kp1x = 500; + const int kp1y = 300; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(3000)); + setDepthValue(depthMat, kp0x - kpRadius, kp0y - kpRadius, kp0x + kpRadius, kp0y + kpRadius, 1800); + setDepthValue(depthMat, kp1x - kpRadius, kp1y - kpRadius, kp1x + kpRadius, kp1y + kpRadius, 11000); + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + constexpr std::uint16_t maxDepthThreshold = 10000; + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, maxDepthThreshold); + initialConfig.setKeypointRadius(kpRadius); + initialConfig.setCalculateSpatialKeypoints(true); + initialConfig.setUseSegmentation(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(0.5F, 0.5F, true), dai::Size2f(0.8F, 0.8F, true), 0.0F)); + detection.confidence = 0.95F; + detection.label = 11; + std::vector keypoints; + keypoints.emplace_back(dai::Point2f(static_cast(kp0x) / width, static_cast(kp0y) / height, true)); + keypoints.emplace_back(dai::Point2f(static_cast(kp1x) / width, static_cast(kp1y) / height, true)); + detection.setKeypoints(keypoints); + + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + + const auto& spatialDetection = spatialDetections.detections.front(); + const auto spatialKeypoints = spatialDetection.getKeypoints(); + REQUIRE(spatialKeypoints.size() == keypoints.size()); + + const float fx = intrinsics[0][0]; + const float fy = intrinsics[1][1]; + const float cx = intrinsics[0][2]; + const float cy = intrinsics[1][2]; + + auto computeExpectedDepth = [&](int pxCenter, int pyCenter) { + const int startX = std::max(0, pxCenter - kpRadius); + const int startY = std::max(0, pyCenter - kpRadius); + const int endX = std::min(static_cast(width) - 1, pxCenter + kpRadius); + const int endY = std::min(static_cast(height) - 1, pyCenter + kpRadius); + + std::uint32_t sum = 0; + std::uint32_t count = 0; + for(int y = startY; y < endY; ++y) { + for(int x = startX; x < endX; ++x) { + const float dist = std::hypot(static_cast(pxCenter - x), static_cast(pyCenter - y)); + if(dist <= kpRadius) { + const std::uint16_t px = depthMat.at(y, x); + if(px > 0 && px < maxDepthThreshold) { + sum += px; + ++count; + } + } + } + } + REQUIRE(count > 0); + return static_cast(sum) / static_cast(count); + }; + + const float kp0DepthAvg = computeExpectedDepth(kp0x, kp0y); + auto expectCoord = [&](int px, int py, float depthMm) { + const float expectedX = (static_cast(px) - cx) * depthMm / fx; + const float expectedY = (static_cast(py) - cy) * depthMm / fy; + return std::array{expectedX, expectedY, depthMm}; + }; + + const auto kp0Expected = expectCoord(kp0x, kp0y, kp0DepthAvg); + CHECK(spatialKeypoints[0].spatialCoordinates.x == Approx(kp0Expected[0]).margin(1.0F)); + CHECK(spatialKeypoints[0].spatialCoordinates.y == Approx(kp0Expected[1]).margin(1.0F)); + CHECK(spatialKeypoints[0].spatialCoordinates.z == Approx(kp0Expected[2]).margin(1.0F)); + + CHECK(spatialKeypoints[1].spatialCoordinates.z == 0); +} + +TEST_CASE("Spatial detections respect segmentation mask pixels") { + constexpr unsigned width = 800; + constexpr unsigned height = 600; + const std::array, 3> intrinsics = {{ + {{7.5F, 0.0F, 3.2F}}, + {{0.0F, 4.8F, 2.4F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(3100)); + const int bboxStartX = 150; + const int bboxStartY = 120; + const int bboxWidth = 420; + const int bboxHeight = 300; + setDepthValue(depthMat, bboxStartX, bboxStartY, bboxStartX + bboxWidth, bboxStartY + bboxHeight, static_cast(2600)); + + const int segStartX = bboxStartX + 80; + const int segStartY = bboxStartY + 60; + const int segWidth = 160; + const int segHeight = 140; + std::vector> segmentationPixels; + segmentationPixels.reserve(segWidth * segHeight); + for(int y = segStartY; y < segStartY + segHeight; ++y) { + for(int x = segStartX; x < segStartX + segWidth; ++x) { + segmentationPixels.emplace_back(x, y); + depthMat.at(y, x) = static_cast(1400); + } + } + + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 10000); + initialConfig.setUseSegmentation(true); + initialConfig.setCalculateSpatialKeypoints(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + const float normWidth = static_cast(bboxWidth) / static_cast(width); + const float normHeight = static_cast(bboxHeight) / static_cast(height); + const float normCenterX = static_cast(bboxStartX) / width + normWidth / 2.0F; + const float normCenterY = static_cast(bboxStartY) / height + normHeight / 2.0F; + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(normCenterX, normCenterY, true), dai::Size2f(normWidth, normHeight, true), 0.0F)); + detection.confidence = 0.7F; + detection.label = 0; + + std::vector mask(width * height, 255); + for(const auto& [x, y] : segmentationPixels) { + mask.at(y * static_cast(width) + x) = 0; + } + detectionMsg->setSegmentationMask(mask, width, height); + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + + REQUIRE(spatialDetections.detections.size() == 1); + + const auto& spatialDetection = spatialDetections.detections.front(); + const float expectedDepth = 1400.0F; + CHECK(spatialDetection.spatialCoordinates.z == Approx(expectedDepth).margin(1.0F)); + + const float fx = intrinsics[0][0]; + const float fy = intrinsics[1][1]; + const float cx = intrinsics[0][2]; + const float cy = intrinsics[1][2]; + const float centerXpx = static_cast(bboxStartX) + static_cast(bboxWidth) / 2.0F; + const float centerYpx = static_cast(bboxStartY) + static_cast(bboxHeight) / 2.0F; + const float expectedX = (centerXpx - cx) * expectedDepth / fx; + const float expectedY = (centerYpx - cy) * expectedDepth / fy; + CHECK(spatialDetection.spatialCoordinates.x == Approx(expectedX).margin(1.0F)); + CHECK(spatialDetection.spatialCoordinates.y == Approx(expectedY).margin(1.0F)); + + REQUIRE(spatialDetection.boundingBoxMapping.depthThresholds.lowerThreshold == initialConfig.globalLowerThreshold); + REQUIRE(spatialDetection.boundingBoxMapping.depthThresholds.upperThreshold == initialConfig.globalUpperThreshold); + REQUIRE(spatialDetection.boundingBoxMapping.calculationAlgorithm == initialConfig.globalCalculationAlgorithm); +} + +TEST_CASE("Segmentation usage can be toggled") { + constexpr unsigned width = 640; + constexpr unsigned height = 480; + const std::array, 3> intrinsics = {{ + {{7.5F, 0.0F, 3.2F}}, + {{0.0F, 4.8F, 2.4F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(3000)); + const int bboxStartX = 100; + const int bboxStartY = 80; + const int bboxWidth = 120; + const int bboxHeight = 80; + setDepthValue(depthMat, bboxStartX, bboxStartY, bboxStartX + bboxWidth, bboxStartY + bboxHeight, 2600); + + const int segStartX = bboxStartX + 20; + const int segStartY = bboxStartY + 20; + const int segWidth = 40; + const int segHeight = 30; + setDepthValue(depthMat, segStartX, segStartY, segStartX + segWidth, segStartY + segHeight, 1400); + + auto runSpatial = [&](bool useSegmentation) { + auto depthFrame = createDepthFrame(depthMat, intrinsics); + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 10000); + initialConfig.setUseSegmentation(useSegmentation); + initialConfig.setCalculateSpatialKeypoints(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + dai::ImgDetection detection; + const float normWidth = static_cast(bboxWidth) / static_cast(width); + const float normHeight = static_cast(bboxHeight) / static_cast(height); + const float normCenterX = static_cast(bboxStartX) / width + normWidth / 2.0F; + const float normCenterY = static_cast(bboxStartY) / height + normHeight / 2.0F; + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(normCenterX, normCenterY, true), dai::Size2f(normWidth, normHeight, true), 0.0F)); + detection.confidence = 0.6F; + detection.label = 1; + detectionMsg->detections[0] = detection; + + std::vector mask(width * height, 255); + for(int y = segStartY; y < segStartY + segHeight; ++y) { + for(int x = segStartX; x < segStartX + segWidth; ++x) { + mask.at(y * static_cast(width) + x) = 0; + } + } + detectionMsg->setSegmentationMask(mask, width, height); + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + return spatialDetections.detections.front(); + }; + + SECTION("Segmentation enabled uses mask pixels") { + const auto detection = runSpatial(true); + CHECK(detection.spatialCoordinates.z == Approx(1400.0F).margin(1.0F)); + } + SECTION("Segmentation disabled falls back to bounding box averaging") { + const auto detection = runSpatial(false); + const float segArea = static_cast(segWidth * segHeight); + const float bboxArea = static_cast(bboxWidth * bboxHeight); + const float expectedDepth = ((segArea * 1400.0F) + ((bboxArea - segArea) * 2600.0F)) / bboxArea; // mask ignored, so full bbox average applies + CHECK(detection.spatialCoordinates.z == Approx(expectedDepth).margin(1.0F)); + } +} + +TEST_CASE("Segmentation passthrough can be toggled") { + constexpr unsigned width = 320; + constexpr unsigned height = 240; + const std::array, 3> intrinsics = {{ + {{7.5F, 0.0F, 3.2F}}, + {{0.0F, 4.8F, 2.4F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(2000)); + + std::vector mask(width * height, 255); + const int segStartX = 40; + const int segStartY = 30; + const int segWidth = 60; + const int segHeight = 50; + for(int y = segStartY; y < segStartY + segHeight; ++y) { + for(int x = segStartX; x < segStartX + segWidth; ++x) { + mask.at(y * static_cast(width) + x) = 0; + } + } + + auto runSpatial = [&](bool passthrough) { + auto depthFrame = createDepthFrame(depthMat, intrinsics); + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 10000); + initialConfig.setUseSegmentation(true); + initialConfig.setSegmentationPassthrough(passthrough); + initialConfig.setCalculateSpatialKeypoints(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(0.5F, 0.5F, true), dai::Size2f(0.4F, 0.4F, true), 0.0F)); + detection.confidence = 0.5F; + detection.label = 2; + detectionMsg->setSegmentationMask(mask, width, height); + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + return spatialDetections; + }; + + SECTION("Passthrough enabled includes mask data") { + const auto spatialDetections = runSpatial(true); + const auto outMask = spatialDetections.getMaskData(); + REQUIRE(outMask.has_value()); + CHECK(outMask->size() == mask.size()); + CHECK(*outMask == mask); + CHECK(spatialDetections.getSegmentationMaskWidth() == width); + CHECK(spatialDetections.getSegmentationMaskHeight() == height); + } + + SECTION("Passthrough disabled omits mask data") { + const auto spatialDetections = runSpatial(false); + const auto outMask = spatialDetections.getMaskData(); + CHECK_FALSE(outMask.has_value()); + CHECK(spatialDetections.getSegmentationMaskWidth() == 0); + CHECK(spatialDetections.getSegmentationMaskHeight() == 0); + } +} + +TEST_CASE("Spatial detections remap depth to detection transformations") { + using std::chrono::steady_clock; + const auto RESET_REMOTE_TIMEOUT_MS = std::chrono::milliseconds(2000); + auto waitForDevice = [RESET_REMOTE_TIMEOUT_MS](dai::DeviceInfo& connectedDeviceInfo) { + auto disappearStart = steady_clock::now(); + while(steady_clock::now() - disappearStart < RESET_REMOTE_TIMEOUT_MS) { + std::vector devices = dai::XLinkConnection::getAllConnectedDevices(X_LINK_ANY_STATE, true, 50); + bool found = false; + for(const auto& dev : devices) { + if(dev.deviceId == connectedDeviceInfo.deviceId) { + found = true; + } + } + if(!found) { + break; // Device disappeared and went into reset + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + }; + + constexpr unsigned width = 320; + constexpr unsigned height = 240; + const std::array, 3> intrinsics = {{ + {{5.5F, 0.0F, 2.5F}}, + {{0.0F, 5.5F, 2.0F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + std::shared_ptr device = std::make_shared(); + + if(device->getPlatform() == dai::Platform::RVC2) { // skipping test on RVC2 + device->close(); + return; + } + + dai::DeviceInfo connectedDeviceInfo; + connectedDeviceInfo.deviceId = device->getDeviceId(); + connectedDeviceInfo.platform = device->getDeviceInfo().platform; + device->close(); + + waitForDevice(connectedDeviceInfo); + + constexpr std::uint16_t farDepth = 4200; + constexpr std::uint16_t nearDepth = 1400; + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(farDepth)); + setDepthValue(depthMat, 0, 0, static_cast(width / 2), static_cast(height), nearDepth); + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 10000); + initialConfig.setUseSegmentation(false); + initialConfig.setCalculateSpatialKeypoints(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + const float bboxWidthNorm = 0.2F; + const float bboxHeightNorm = 0.5F; + const float bboxCenterXNorm = 0.75F; + const float bboxCenterYNorm = 0.5F; + const dai::RotatedRect gtBbox{dai::Point2f(bboxCenterXNorm, bboxCenterYNorm, true), dai::Size2f(bboxWidthNorm, bboxHeightNorm, true), 0.0F}; + const dai::RotatedRect denormGtBbox = gtBbox.denormalize(width, height); + detection.setBoundingBox(gtBbox); + detection.confidence = 0.9F; + detection.label = 7; + auto manipulatedDetectionFrame = createDetectionFrameWithManipulation(depthFrame, width, height, connectedDeviceInfo); + waitForDevice(connectedDeviceInfo); + detectionMsg->transformation = manipulatedDetectionFrame->transformation; + detectionMsg->setTimestamp(manipulatedDetectionFrame->getTimestamp()); + detectionMsg->setSequenceNum(manipulatedDetectionFrame->getSequenceNum()); + + const auto detectionBoundingBox = detection.getBoundingBox(); + REQUIRE(detectionMsg->transformation.has_value()); + const auto& detectionTransformation = detectionMsg->transformation.value(); + auto remappedRect = detectionTransformation.remapRectTo(depthFrame->transformation, detectionBoundingBox); + auto remappedRectPx = remappedRect.denormalize(width, height); + const float remappedCenterX = remappedRectPx.center.x; + const float remappedCenterY = remappedRectPx.center.y; + REQUIRE(remappedCenterX < static_cast(width) / 2.0F); // should land on the near-depth side + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, connectedDeviceInfo, detectionMsg, std::nullopt); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + + const auto formatMatrix = [](const std::array, 3>& matrix) { + std::ostringstream oss; + oss << '['; + for(size_t row = 0; row < matrix.size(); ++row) { + if(row > 0) { + oss << ", "; + } + oss << '[' << matrix[row][0] << ", " << matrix[row][1] << ", " << matrix[row][2] << ']'; + } + oss << ']'; + return oss.str(); + }; + INFO("Depth transformation matrix: " << formatMatrix(depthFrame->transformation.getMatrix())); + INFO("RGB transformation matrix: " << formatMatrix(detectionTransformation.getMatrix())); + + const auto& spatialDetection = spatialDetections.detections.front(); + const auto intrinsicMatrix = detectionTransformation.getIntrinsicMatrix(); + const float fx = intrinsicMatrix[0][0]; + const float fy = intrinsicMatrix[1][1]; + const float cx = intrinsicMatrix[0][2]; + const float cy = intrinsicMatrix[1][2]; + const dai::RotatedRect denormalizedBBox = detection.getBoundingBox().denormalize(width, height); + + const float expectedDepth = static_cast(nearDepth); + const float expectedX = (denormGtBbox.center.x - cx) * expectedDepth / fx; + const float expectedY = (denormGtBbox.center.y - cy) * expectedDepth / fy; + + // Check bounding box mapping + const auto outputBox = spatialDetection.getBoundingBox(); + CHECK(outputBox.center.x == Approx(bboxCenterXNorm).margin(1e-6F)); + CHECK(outputBox.center.y == Approx(bboxCenterYNorm).margin(1e-6F)); + + // Check spatial coordinates + CHECK(spatialDetection.spatialCoordinates.z == Approx(expectedDepth).margin(1.0F)); + CHECK(spatialDetection.spatialCoordinates.x == Approx(expectedX).margin(1.0F)); + CHECK(spatialDetection.spatialCoordinates.y == Approx(expectedY).margin(1.0F)); +} + +TEST_CASE("Spatial detections return zero depth when depth frame is empty") { + constexpr unsigned width = 160; + constexpr unsigned height = 120; + const std::array, 3> intrinsics = {{ + {{4.0F, 0.0F, 1.0F}}, + {{0.0F, 4.0F, 1.0F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(0)); + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 5000); + initialConfig.setUseSegmentation(false); + initialConfig.setCalculateSpatialKeypoints(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(0.5F, 0.5F, true), dai::Size2f(0.6F, 0.6F, true), 0.0F)); + detection.confidence = 0.5F; + detection.label = 5; + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + + const auto& spatialDetection = spatialDetections.detections.front(); + CHECK(spatialDetection.spatialCoordinates.z == Approx(0.0F).margin(1.0F)); +} + +TEST_CASE("Spatial keypoints can be disabled") { + constexpr unsigned width = 320; + constexpr unsigned height = 240; + const std::array, 3> intrinsics = {{ + {{5.0F, 0.0F, 1.0F}}, + {{0.0F, 5.0F, 1.0F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(2200)); + const int kpX = 100; + const int kpY = 80; + setDepthValue(depthMat, kpX - 5, kpY - 5, kpX + 5, kpY + 5, 1200); + + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 10000); + initialConfig.setKeypointRadius(6); + initialConfig.setCalculateSpatialKeypoints(false); + initialConfig.setUseSegmentation(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(0.5F, 0.5F, true), dai::Size2f(0.5F, 0.5F, true), 0.0F)); + detection.confidence = 0.9F; + detection.label = 2; + detection.setKeypoints({dai::Keypoint(dai::Point2f(static_cast(kpX) / width, static_cast(kpY) / height, true))}); + + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + + const auto& spatialDetection = spatialDetections.detections.front(); + CHECK(spatialDetection.getKeypoints().empty()); + + const int bboxStartX = static_cast(width * 0.25F); + const int bboxStartY = static_cast(height * 0.25F); + const int bboxWidth = static_cast(width * 0.5F); + const int bboxHeight = static_cast(height * 0.5F); + auto computeBboxAverage = [&]() { + std::uint64_t sum = 0; + std::uint64_t count = 0; + for(int y = bboxStartY; y < bboxStartY + bboxHeight; ++y) { + for(int x = bboxStartX; x < bboxStartX + bboxWidth; ++x) { + sum += depthMat.at(y, x); + ++count; + } + } + return static_cast(sum) / static_cast(count); + }; + const float expectedZ = computeBboxAverage(); + const float fx = intrinsics[0][0]; + const float fy = intrinsics[1][1]; + const float cx = intrinsics[0][2]; + const float cy = intrinsics[1][2]; + const float centerXpx = static_cast(bboxStartX + bboxWidth / 2.0F); + const float centerYpx = static_cast(bboxStartY + bboxHeight / 2.0F); + const float expectedX = (centerXpx - cx) * expectedZ / fx; + const float expectedY = (centerYpx - cy) * expectedZ / fy; + REQUIRE(spatialDetection.spatialCoordinates.x == Approx(expectedX).margin(1.0F)); + REQUIRE(spatialDetection.spatialCoordinates.y == Approx(expectedY).margin(1.0F)); + REQUIRE(spatialDetection.spatialCoordinates.z == Approx(expectedZ).margin(1.0F)); +} + +TEST_CASE("Keypoint spatial calculation skips invalid or thresholded depth") { + constexpr unsigned width = 200; + constexpr unsigned height = 150; + const std::array, 3> intrinsics = {{ + {{6.0F, 0.0F, 2.0F}}, + {{0.0F, 6.0F, 2.0F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(0)); + const int kpX = 80; + const int kpY = 60; + setDepthValue(depthMat, kpX - 8, kpY - 8, kpX, kpY, 4500); // outside upper threshold + setDepthValue(depthMat, kpX, kpY, kpX + 8, kpY + 8, 0); // outside lower threshold + setDepthValue(depthMat, kpX - 3, kpY - 3, kpX + 3, kpY + 3, 1500); + + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(100, 2000); + initialConfig.setKeypointRadius(8); + initialConfig.setCalculateSpatialKeypoints(true); + initialConfig.setUseSegmentation(false); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(0.4F, 0.4F, true), dai::Size2f(0.4F, 0.4F, true), 0.0F)); + detection.confidence = 0.8F; + detection.label = 3; + detection.setKeypoints({dai::Keypoint(dai::Point2f(static_cast(kpX) / width, static_cast(kpY) / height, true))}); + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + + const auto& det = spatialDetections.detections.front(); + const auto kp = det.getKeypoints().at(0); + REQUIRE(kp.spatialCoordinates.z == Approx(1500.0F).margin(1.0F)); +} + +TEST_CASE("Spatial detections handle segmentation and keypoints together") { + constexpr unsigned width = 800; + constexpr unsigned height = 600; + const std::array, 3> intrinsics = {{ + {{7.0F, 0.0F, 3.0F}}, + {{0.0F, 7.0F, 2.0F}}, + {{0.0F, 0.0F, 1.0F}}, + }}; + + cv::Mat depthMat(height, width, CV_16UC1, cv::Scalar(3200)); + const int bboxStartX = 60; + const int bboxStartY = 40; + const int bboxWidth = 200; + const int bboxHeight = 160; + setDepthValue(depthMat, bboxStartX, bboxStartY, bboxStartX + bboxWidth, bboxStartY + bboxHeight, 2800); + + // Segmentation mask region with closer depth + const int segStartX = bboxStartX + 40; + const int segStartY = bboxStartY + 50; + const int segWidth = 60; + const int segHeight = 50; + setDepthValue(depthMat, segStartX, segStartY, segStartX + segWidth, segStartY + segHeight, 1200); + + // Keypoint located outside the segmentation blob, with different depth + const int kpX = bboxStartX + 20; + const int kpY = bboxStartY + 20; + setDepthValue(depthMat, kpX - 5, kpY - 5, kpX + 5, kpY + 5, 2200); + + auto depthFrame = createDepthFrame(depthMat, intrinsics); + + dai::SpatialLocationCalculatorConfig initialConfig; + initialConfig.setCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm::AVERAGE); + initialConfig.setDepthThresholds(0, 10000); + initialConfig.setKeypointRadius(6); + initialConfig.setCalculateSpatialKeypoints(true); + initialConfig.setUseSegmentation(true); + + auto detectionMsg = std::make_shared(); + detectionMsg->detections.resize(1); + auto& detection = detectionMsg->detections.front(); + const float normWidth = static_cast(bboxWidth) / static_cast(width); + const float normHeight = static_cast(bboxHeight) / static_cast(height); + const float normCenterX = static_cast(bboxStartX) / width + normWidth / 2.0F; + const float normCenterY = static_cast(bboxStartY) / height + normHeight / 2.0F; + detection.setBoundingBox(dai::RotatedRect(dai::Point2f(normCenterX, normCenterY, true), dai::Size2f(normWidth, normHeight, true), 0.0F)); + detection.confidence = 0.85F; + detection.label = 4; + + detection.setKeypoints({dai::Keypoint(dai::Point2f(static_cast(kpX) / width, static_cast(kpY) / height, true))}); + + std::vector mask(width * height, 255); + for(int y = segStartY; y < segStartY + segHeight; ++y) { + for(int x = segStartX; x < segStartX + segWidth; ++x) { + mask.at(y * static_cast(width) + x) = 0; + } + } + detectionMsg->setSegmentationMask(mask, width, height); + detectionMsg->transformation = depthFrame->transformation; + detectionMsg->setTimestamp(depthFrame->getTimestamp()); + detectionMsg->setSequenceNum(depthFrame->getSequenceNum()); + + auto [unusedLegacy, unusedPassthrough, spatialDetections] = processDepthFrame(initialConfig, depthFrame, std::nullopt, detectionMsg); + static_cast(unusedLegacy); + static_cast(unusedPassthrough); + REQUIRE(spatialDetections.detections.size() == 1); + + const auto& spatialDetection = spatialDetections.detections.front(); + CHECK(spatialDetection.spatialCoordinates.z == Approx(1200.0F).margin(1.0F)); + + const auto keypoints = spatialDetection.getKeypoints(); + const auto& kp = keypoints.at(0); + REQUIRE(kp.spatialCoordinates.z == Approx(0).margin(1.0F)); } From f8c106a24aad27aeecbce3d02ffd436d33bf1477 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Mon, 19 Jan 2026 17:27:27 +0100 Subject: [PATCH 057/180] Fix typo --- bindings/python/src/pipeline/CommonBindings.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index dfdfedca40..0fb2ec58f2 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -75,7 +75,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::enum_ usbSpeed(m, "UsbSpeed", DOC(dai, UsbSpeed)); py::enum_ processorType(m, "ProcessorType"); py::enum_ detectionNetworkType(m, "DetectionNetworkType"); - py::enum_ lengthUnitEnum(m, "lengthUnit", DOC(dai, LengthUnit)); + py::enum_ lengthUnitEnum(m, "LengthUnit", DOC(dai, LengthUnit)); lengthUnitEnum.value("METER", LengthUnit::METER) .value("CENTIMETER", LengthUnit::CENTIMETER) .value("MILLIMETER", LengthUnit::MILLIMETER) From e9ca3814e0488aebc465277324a5649f0393aa33 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 21 Jan 2026 00:06:16 +0100 Subject: [PATCH 058/180] Remove numpy 2.4.0 warnings --- examples/python/ImageAlign/depth_align.py | 5 +++-- examples/python/ObjectTracker/object_tracker_remap.py | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/python/ImageAlign/depth_align.py b/examples/python/ImageAlign/depth_align.py index 6d59d8352e..00fa3d73a3 100755 --- a/examples/python/ImageAlign/depth_align.py +++ b/examples/python/ImageAlign/depth_align.py @@ -65,7 +65,8 @@ def colorizeDepth(frameDepth): try: minDepth = np.percentile(frameDepth[frameDepth != 0], 3) maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) + logDepth = np.zeros_like(frameDepth, dtype=np.float32) + np.log(frameDepth, where=frameDepth != 0, out=logDepth) logMinDepth = np.log(minDepth) logMaxDepth = np.log(maxDepth) np.nan_to_num(logDepth, copy=False, nan=logMinDepth) @@ -156,4 +157,4 @@ def updateBlendWeights(percentRgb): key = cv2.waitKey(1) if key == ord("q"): - break \ No newline at end of file + break diff --git a/examples/python/ObjectTracker/object_tracker_remap.py b/examples/python/ObjectTracker/object_tracker_remap.py index 7ea55579a8..d172e86741 100644 --- a/examples/python/ObjectTracker/object_tracker_remap.py +++ b/examples/python/ObjectTracker/object_tracker_remap.py @@ -10,7 +10,8 @@ def colorizeDepth(frameDepth): try: minDepth = np.percentile(frameDepth[frameDepth != 0], 3) maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) - logDepth = np.log(frameDepth, where=frameDepth != 0) + logDepth = np.zeros_like(frameDepth, dtype=np.float32) + np.log(frameDepth, where=frameDepth != 0, out=logDepth) logMinDepth = np.log(minDepth) logMaxDepth = np.log(maxDepth) np.nan_to_num(logDepth, copy=False, nan=logMinDepth) @@ -114,4 +115,3 @@ def displayFrame(name: str, frame: dai.ImgFrame, tracklets: dai.Tracklets): if cv2.waitKey(1) == ord("q"): pipeline.stop() break - From 3b91c1f127b639a49ce6812db10a2418955a8132 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Wed, 21 Jan 2026 12:40:28 +0100 Subject: [PATCH 059/180] Bump FW to latest --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index b0b4b368ef..248ba7299f 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+8f5f686773382428d08026d678eff7bb255e1275") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+fd0b94cac3b662d94da861c24bbd4bd247dad213") From ae1660ddb70c4d7e2649b17d72291ed8955cb79a Mon Sep 17 00:00:00 2001 From: aljazkonec1 <53098513+aljazkonec1@users.noreply.github.com> Date: Wed, 21 Jan 2026 13:56:14 +0100 Subject: [PATCH 060/180] Update: SpatialDetectionNetwork is now a DeviceNodeGroup (#1641) * Update SpatialDetectionNetwork to NodeGroup * bump fw * remove default setting * Bump Fw * update example and clangformat --- ...patialLocationCalculatorConfigBindings.cpp | 8 +- .../node/SpatialDetectionNetworkBindings.cpp | 32 +++-- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- .../spatial_detection.cpp | 31 +++-- .../spatial_detection.py | 25 ++-- include/depthai/common/Rect.hpp | 1 + .../SpatialLocationCalculatorConfig.hpp | 27 +++- .../pipeline/node/SpatialDetectionNetwork.hpp | 126 ++++-------------- .../SpatialDetectionNetworkProperties.hpp | 10 +- .../SpatialLocationCalculatorConfig.cpp | 11 ++ src/pipeline/node/SpatialDetectionNetwork.cpp | 126 +++++++++++++++--- .../SpatialUtils.cpp | 22 ++- .../node/spatial_detection_network_test.cpp | 2 - .../onhost_tests/calibration_handler_test.cpp | 2 +- 15 files changed, 247 insertions(+), 180 deletions(-) diff --git a/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp b/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp index e16542d015..ff84fd5cf9 100644 --- a/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp +++ b/bindings/python/src/pipeline/datatype/SpatialLocationCalculatorConfigBindings.cpp @@ -82,6 +82,9 @@ void bind_spatiallocationcalculatorconfig(pybind11::module& m, void* pCallstack) .def("setSegmentationPassthrough", &SpatialLocationCalculatorConfig::setSegmentationPassthrough, DOC(dai, SpatialLocationCalculatorConfig, setSegmentationPassthrough)) + .def("setBoundingBoxScaleFactor", + &SpatialLocationCalculatorConfig::setBoundingBoxScaleFactor, + DOC(dai, SpatialLocationCalculatorConfig, setBoundingBoxScaleFactor)) .def("getDepthThresholds", &SpatialLocationCalculatorConfig::getDepthThresholds, DOC(dai, SpatialLocationCalculatorConfig, getDepthThresholds)) .def("getCalculationAlgorithm", &SpatialLocationCalculatorConfig::getCalculationAlgorithm, @@ -94,5 +97,8 @@ void bind_spatiallocationcalculatorconfig(pybind11::module& m, void* pCallstack) .def("getUseSegmentation", &SpatialLocationCalculatorConfig::getUseSegmentation, DOC(dai, SpatialLocationCalculatorConfig, getUseSegmentation)) .def("getSegmentationPassthrough", &SpatialLocationCalculatorConfig::getSegmentationPassthrough, - DOC(dai, SpatialLocationCalculatorConfig, getSegmentationPassthrough)); + DOC(dai, SpatialLocationCalculatorConfig, getSegmentationPassthrough)) + .def("getBoundingBoxScaleFactor", + &SpatialLocationCalculatorConfig::getBoundingBoxScaleFactor, + DOC(dai, SpatialLocationCalculatorConfig, getBoundingBoxScaleFactor)); } diff --git a/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp b/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp index f6d5f19d88..743c11c354 100644 --- a/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp +++ b/bindings/python/src/pipeline/node/SpatialDetectionNetworkBindings.cpp @@ -1,8 +1,6 @@ #include "Common.hpp" -#include "NodeBindings.hpp" -#include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/pipeline/DeviceNodeGroup.hpp" #include "depthai/pipeline/Node.hpp" -#include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/node/SpatialDetectionNetwork.hpp" void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack) { @@ -30,7 +28,9 @@ void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack) { // Properties spatialDetectionNetworkProperties.def_readwrite("detectedBBScaleFactor", &SpatialDetectionNetworkProperties::detectedBBScaleFactor) - .def_readwrite("depthThresholds", &SpatialDetectionNetworkProperties::depthThresholds); + .def_readwrite("depthThresholds", &SpatialDetectionNetworkProperties::depthThresholds) + .def_readwrite("calculationAlgorithm", &SpatialDetectionNetworkProperties::calculationAlgorithm) + .def_readwrite("stepSize", &SpatialDetectionNetworkProperties::stepSize); // Node spatialDetectionNetwork @@ -151,7 +151,7 @@ void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack) { DOC(dai, node, NeuralNetwork, input)) .def_property_readonly( "out", - [](const SpatialDetectionNetwork& n) { return &n.detectionParser->out; }, + [](const SpatialDetectionNetwork& n) { return &n.spatialLocationCalculator->outputDetections; }, py::return_value_policy::reference_internal, DOC(dai, node, SpatialDetectionNetwork, out)) .def_property_readonly( @@ -169,19 +169,27 @@ void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack) { [](SpatialDetectionNetwork& n) { return &(*n.detectionParser); }, py::return_value_policy::reference_internal, DOC(dai, node, SpatialDetectionNetwork, detectionParser)) + .def_property_readonly( + "spatialLocationCalculator", + [](SpatialDetectionNetwork& n) { return &(*n.spatialLocationCalculator); }, + py::return_value_policy::reference_internal, + DOC(dai, node, SpatialDetectionNetwork, spatialLocationCalculator)) .def_property_readonly( "neuralNetwork", [](SpatialDetectionNetwork& n) { return &(*n.neuralNetwork); }, py::return_value_policy::reference_internal, DOC(dai, node, SpatialDetectionNetwork, neuralNetwork)) - .def_readonly("inputDepth", &SpatialDetectionNetwork::inputDepth, DOC(dai, node, SpatialDetectionNetwork, inputDepth)) - .def_readonly("out", &SpatialDetectionNetwork::out, DOC(dai, node, SpatialDetectionNetwork, out)) - .def_readonly("boundingBoxMapping", &SpatialDetectionNetwork::boundingBoxMapping, DOC(dai, node, SpatialDetectionNetwork, boundingBoxMapping)) - .def_readonly("passthroughDepth", &SpatialDetectionNetwork::passthroughDepth, DOC(dai, node, SpatialDetectionNetwork, passthroughDepth)) - .def_readonly("spatialLocationCalculatorOutput", - &SpatialDetectionNetwork::spatialLocationCalculatorOutput, - DOC(dai, node, SpatialDetectionNetwork, spatialLocationCalculatorOutput)) + .def_property_readonly( + "inputDepth", + [](SpatialDetectionNetwork& n) { return &n.spatialLocationCalculator->inputDepth; }, + py::return_value_policy::reference_internal, + DOC(dai, node, SpatialDetectionNetwork, inputDepth)) + .def_property_readonly( + "passthroughDepth", + [](SpatialDetectionNetwork& n) { return &n.spatialLocationCalculator->passthroughDepth; }, + py::return_value_policy::reference_internal, + DOC(dai, node, SpatialDetectionNetwork, passthroughDepth)) .def("setDepthLowerThreshold", &SpatialDetectionNetwork::setDepthLowerThreshold, diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 248ba7299f..fbe20e4bb5 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+fd0b94cac3b662d94da861c24bbd4bd247dad213") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+2a5145f0430b2cd922c0ab15e958ea1c0a66be0b") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 36821a6a24..647857830d 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "fdad0853e6993e63f35fa20f41f43a1b7058ba83") +set(DEPTHAI_DEVICE_SIDE_COMMIT "276d50325cb0954fc5afd51dae933c8474907e13") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp b/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp index a19bf283c5..b1131a8f0a 100644 --- a/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp +++ b/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp @@ -7,7 +7,7 @@ #include "depthai/depthai.hpp" constexpr float NEURAL_FPS = 8.0f; -constexpr float STEREO_DEFAULT_FPS = 30.0f; +constexpr float STEREO_DEFAULT_FPS = 20.0f; // Custom host node for spatial visualization class SpatialVisualizer : public dai::NodeCRTP { @@ -41,19 +41,19 @@ class SpatialVisualizer : public dai::NodeCRTP 0) { - cv::minMaxLoc(depth_downscaled, &min_depth, &max_depth, nullptr, nullptr, mask); + cv::minMaxLoc(depthDownscaled, &minDepth, &maxDepth, nullptr, nullptr, mask); } // Normalize depth frame cv::Mat depthFrameColor; - depthFrame.convertTo(depthFrameColor, CV_8UC1, 255.0 / (max_depth - min_depth), -min_depth * 255.0 / (max_depth - min_depth)); + depthFrame.convertTo(depthFrameColor, CV_8UC1, 255.0 / (maxDepth - minDepth), -minDepth * 255.0 / (maxDepth - minDepth)); // Apply color map cv::Mat colorized; @@ -138,7 +138,7 @@ int main(int argc, char** argv) { try { program.parse_args(argc, argv); } catch(const std::runtime_error& err) { - std::cerr << err.what() << std::endl; + std::cerr << err.what() << '\n'; std::cerr << program; return EXIT_FAILURE; } @@ -148,8 +148,8 @@ int main(int argc, char** argv) { // Validate depth source argument if(depthSourceArg != "stereo" && depthSourceArg != "neural" && depthSourceArg != "tof") { - std::cerr << "Invalid depth source: " << depthSourceArg << std::endl; - std::cerr << "Valid options are: stereo, neural, tof" << std::endl; + std::cerr << "Invalid depth source: " << depthSourceArg << '\n'; + std::cerr << "Valid options are: stereo, neural, tof" << '\n'; return EXIT_FAILURE; } @@ -182,10 +182,6 @@ int main(int argc, char** argv) { monoRight->build(dai::CameraBoardSocket::CAM_C, std::nullopt, fps); stereo->setExtendedDisparity(true); - if(platform == dai::Platform::RVC2) { - stereo->setOutputSize(640, 400); - } - monoLeft->requestOutput(size, std::nullopt, dai::ImgResizeMode::CROP)->link(stereo->left); monoRight->requestOutput(size, std::nullopt, dai::ImgResizeMode::CROP)->link(stereo->right); @@ -218,23 +214,26 @@ int main(int argc, char** argv) { // Set up model and build with DepthSource variant dai::NNModelDescription modelDesc; + // For better results on OAK4, use a segmentation model like "luxonis/yolov8-instance-segmentation-large:coco-640x480" + // for depth estimation over the objects mask instead of the full bounding box. modelDesc.model = "yolov6-nano"; spatialDetectionNetwork->build(camRgb, depthSource, modelDesc); // Set label map visualizer->labelMap = spatialDetectionNetwork->getClasses().value(); + spatialDetectionNetwork->spatialLocationCalculator->initialConfig->setSegmentationPassthrough(false); // Linking visualizer->build(spatialDetectionNetwork->passthroughDepth, spatialDetectionNetwork->out, spatialDetectionNetwork->passthrough); - std::cout << "Pipeline starting with depth source: " << depthSourceArg << std::endl; + std::cout << "Pipeline starting with depth source: " << depthSourceArg << '\n'; // Start pipeline pipeline.start(); pipeline.wait(); } catch(const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; + std::cerr << "Error: " << e.what() << '\n'; return EXIT_FAILURE; } diff --git a/examples/python/SpatialDetectionNetwork/spatial_detection.py b/examples/python/SpatialDetectionNetwork/spatial_detection.py index 0fd4a57007..a0439c5e1d 100644 --- a/examples/python/SpatialDetectionNetwork/spatial_detection.py +++ b/examples/python/SpatialDetectionNetwork/spatial_detection.py @@ -7,14 +7,15 @@ import numpy as np NEURAL_FPS = 8 -STEREO_DEFAULT_FPS = 30 +STEREO_DEFAULT_FPS = 20 parser = argparse.ArgumentParser() parser.add_argument( "--depthSource", type=str, default="stereo", choices=["stereo", "neural"] ) args = parser.parse_args() - +# For better results on OAK4, use a segmentation model like "luxonis/yolov8-instance-segmentation-large:coco-640x480" +# for depth estimation over the objects mask instead of the full bounding box. modelDescription = dai.NNModelDescription("yolov6-nano") size = (640, 400) @@ -37,13 +38,13 @@ def process(self, depthPreview, detections, rgbPreview): self.displayResults(rgbPreview, depthFrameColor, detections.detections) def processDepthFrame(self, depthFrame): - depth_downscaled = depthFrame[::4] - if np.all(depth_downscaled == 0): - min_depth = 0 + depthDownscaled = depthFrame[::4] + if np.all(depthDownscaled == 0): + minDepth = 0 else: - min_depth = np.percentile(depth_downscaled[depth_downscaled != 0], 1) - max_depth = np.percentile(depth_downscaled, 99) - depthFrameColor = np.interp(depthFrame, (min_depth, max_depth), (0, 255)).astype(np.uint8) + minDepth = np.percentile(depthDownscaled[depthDownscaled != 0], 1) + maxDepth = np.percentile(depthDownscaled, 99) + depthFrameColor = np.interp(depthFrame, (minDepth, maxDepth), (0, 255)).astype(np.uint8) return cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_HOT) def displayResults(self, rgbFrame, depthFrameColor, detections): @@ -52,8 +53,8 @@ def displayResults(self, rgbFrame, depthFrameColor, detections): self.drawBoundingBoxes(depthFrameColor, detection) self.drawDetections(rgbFrame, detection, width, height) - cv2.imshow("depth", depthFrameColor) - cv2.imshow("rgb", rgbFrame) + cv2.imshow("Depth frame", depthFrameColor) + cv2.imshow("Color frame", rgbFrame) if cv2.waitKey(1) == ord('q'): self.stopPipeline() @@ -90,8 +91,6 @@ def drawDetections(self, frame, detection, frameWidth, frameHeight): if args.depthSource == "stereo": depthSource = p.create(dai.node.StereoDepth) depthSource.setExtendedDisparity(True) - if platform == dai.Platform.RVC2: - depthSource.setOutputSize(640, 400) monoLeft.requestOutput(size).link(depthSource.left) monoRight.requestOutput(size).link(depthSource.right) elif args.depthSource == "neural": @@ -108,8 +107,8 @@ def drawDetections(self, frame, detection, frameWidth, frameHeight): ) visualizer = p.create(SpatialVisualizer) + spatialDetectionNetwork.spatialLocationCalculator.initialConfig.setSegmentationPassthrough(False) spatialDetectionNetwork.input.setBlocking(False) - spatialDetectionNetwork.setBoundingBoxScaleFactor(0.5) spatialDetectionNetwork.setDepthLowerThreshold(100) spatialDetectionNetwork.setDepthUpperThreshold(5000) diff --git a/include/depthai/common/Rect.hpp b/include/depthai/common/Rect.hpp index 80e09189ea..d499f1e70a 100644 --- a/include/depthai/common/Rect.hpp +++ b/include/depthai/common/Rect.hpp @@ -2,6 +2,7 @@ // libraries #include +#include #include "depthai/common/Point2f.hpp" #include "depthai/common/Size2f.hpp" diff --git a/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp b/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp index f6c032f092..f8fa4f4066 100644 --- a/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp +++ b/include/depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp @@ -1,12 +1,13 @@ #pragma once -#include #include #include "depthai/common/Rect.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" namespace dai { +constexpr int MAX_UPPER_THRESHOLD = std::numeric_limits::max(); +constexpr int MIN_LOWER_THRESHOLD = 100; /** * SpatialLocation configuration thresholds structure * @@ -17,11 +18,11 @@ struct SpatialLocationCalculatorConfigThresholds { /** * Values less or equal than this threshold are not taken into calculation. */ - uint32_t lowerThreshold = 0; + uint32_t lowerThreshold = MIN_LOWER_THRESHOLD; /** * Values greater or equal than this threshold are not taken into calculation. */ - uint32_t upperThreshold = 65535; + uint32_t upperThreshold = MAX_UPPER_THRESHOLD; }; DEPTHAI_SERIALIZE_EXT(SpatialLocationCalculatorConfigThresholds, lowerThreshold, upperThreshold); @@ -79,6 +80,7 @@ DEPTHAI_SERIALIZE_EXT(SpatialLocationCalculatorConfigData, roi, depthThresholds, * - Calculate spatial keypoints: true * - Use segmentation for ImgDetections: true * - Segmentation passthrough: true + * - Bounding box scale factor: 1.0 * * An optional list of per-ROI configurations is available via `config`. ROI * settings override the corresponding global values where specified. @@ -89,13 +91,14 @@ class SpatialLocationCalculatorConfig : public Buffer { public: int32_t globalStepSize = AUTO; - uint32_t globalLowerThreshold = 100; // less than 100mm is considered too close - uint32_t globalUpperThreshold = 65535; + uint32_t globalLowerThreshold = MIN_LOWER_THRESHOLD; + uint32_t globalUpperThreshold = MAX_UPPER_THRESHOLD; SpatialLocationCalculatorAlgorithm globalCalculationAlgorithm = SpatialLocationCalculatorAlgorithm::MEDIAN; int32_t globalKeypointRadius = 10; bool calculateSpatialKeypoints = true; bool useSegmentation = true; bool segmentationPassthrough = true; + float bBoxScaleFactor = 1.0; std::vector config; /** @@ -122,7 +125,7 @@ class SpatialLocationCalculatorConfig : public Buffer { * @param lowerThreshold Lower threshold in depth units (millimeter by default). * @param upperThreshold Upper threshold in depth units (millimeter by default). */ - void setDepthThresholds(uint32_t lowerThreshold = 0, uint32_t upperThreshold = 30000); + void setDepthThresholds(uint32_t lowerThreshold = MIN_LOWER_THRESHOLD, uint32_t upperThreshold = MAX_UPPER_THRESHOLD); /** * Set spatial location calculation algorithm. Possible values: @@ -170,6 +173,12 @@ class SpatialLocationCalculatorConfig : public Buffer { */ void setSegmentationPassthrough(bool passthroughSegmentation); + /** + * Set scale factor for bounding boxes used in spatial calculations. + * @param scaleFactor Scale factor must be in the interval (0,1]. + */ + void setBoundingBoxScaleFactor(float scaleFactor); + /** * Retrieve configuration data for SpatialLocationCalculator * @returns Vector of configuration parameters for ROIs (region of interests) @@ -215,6 +224,11 @@ class SpatialLocationCalculatorConfig : public Buffer { */ bool getSegmentationPassthrough() const; + /** + * Retrieve scale factor for bounding boxes used in spatial calculations. + */ + float getBoundingBoxScaleFactor() const; + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; DatatypeEnum getDatatype() const override { @@ -230,6 +244,7 @@ class SpatialLocationCalculatorConfig : public Buffer { calculateSpatialKeypoints, useSegmentation, segmentationPassthrough, + bBoxScaleFactor, config); }; diff --git a/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp b/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp index a189977328..454998f81c 100644 --- a/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp +++ b/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp @@ -1,10 +1,11 @@ #pragma once -#include +#include #include #include #include #include +#include #include // depth map source nodes @@ -29,80 +30,29 @@ using DepthSource = std::variant, std::shared_ptr { +class SpatialDetectionNetwork : public DeviceNodeGroup { public: + using Properties = SpatialDetectionNetworkProperties; using Model = NeuralNetwork::Model; - explicit SpatialDetectionNetwork(const std::shared_ptr& device) - : DeviceNodeCRTP(device) -#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 - , - input{neuralNetwork->input}, - outNetwork{neuralNetwork->out}, - passthrough{neuralNetwork->passthrough} -#endif - { - if(device) { - auto platform = device->getPlatform(); - if(platform == Platform::RVC4) { - if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); - } - } - }; - SpatialDetectionNetwork(std::unique_ptr props) - : DeviceNodeCRTP(std::move(props)) -#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 - , - input{neuralNetwork->input}, - outNetwork{neuralNetwork->out}, - passthrough{neuralNetwork->passthrough} -#endif - { - auto device = getDevice(); - if(device) { - auto platform = device->getPlatform(); - if(platform == Platform::RVC4) { - if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); - } - } - }; - SpatialDetectionNetwork(std::unique_ptr props, bool confMode) - : DeviceNodeCRTP(std::move(props), confMode) -#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 - , - input{neuralNetwork->input}, - outNetwork{neuralNetwork->out}, - passthrough{neuralNetwork->passthrough} -#endif - { - auto device = getDevice(); - if(device) { - auto platform = device->getPlatform(); - if(platform == Platform::RVC4) { - if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); - } - } - }; - SpatialDetectionNetwork(const std::shared_ptr& device, std::unique_ptr props, bool confMode) - : DeviceNodeCRTP(device, std::move(props), confMode) -#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 - , + explicit SpatialDetectionNetwork(const std::shared_ptr& device); - input{neuralNetwork->input}, - outNetwork{neuralNetwork->out}, - passthrough{neuralNetwork->passthrough} -#endif - { - if(device) { - auto platform = device->getPlatform(); - if(platform == Platform::RVC4) { - if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); - } - } - }; + static std::shared_ptr create(const std::shared_ptr& device) { + auto networkPtr = std::make_shared(device); + networkPtr->buildInternal(); + return networkPtr; + } + + explicit SpatialDetectionNetwork(std::unique_ptr props); + + SpatialDetectionNetwork(std::unique_ptr props, bool confMode); + + SpatialDetectionNetwork(const std::shared_ptr& device, std::unique_ptr props, bool confMode); constexpr static const char* NAME = "SpatialDetectionNetwork"; + Properties& properties; + /** * @brief Build SpatialDetectionNetwork node with specified depth source. Connect Camera and depth source outputs to this node's inputs and configure the * inference model @@ -135,6 +85,8 @@ class SpatialDetectionNetwork : public DeviceNodeCRTP neuralNetwork{*this, "neuralNetwork"}; Subnode detectionParser{*this, "detectionParser"}; + Subnode spatialLocationCalculator{*this, "spatialLocationCalculator"}; + std::unique_ptr> depthAlign; #ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 @@ -155,48 +107,31 @@ class SpatialDetectionNetwork : public DeviceNodeCRTP& stereo, const std::shared_ptr& camera); void alignDepthImpl(const std::shared_ptr& neuralDepth, const std::shared_ptr& camera); void alignDepthImpl(const std::shared_ptr& tof, const std::shared_ptr& camera); - - protected: - using DeviceNodeCRTP::DeviceNodeCRTP; }; } // namespace node diff --git a/include/depthai/properties/SpatialDetectionNetworkProperties.hpp b/include/depthai/properties/SpatialDetectionNetworkProperties.hpp index 651f21c572..1ebc018e9d 100644 --- a/include/depthai/properties/SpatialDetectionNetworkProperties.hpp +++ b/include/depthai/properties/SpatialDetectionNetworkProperties.hpp @@ -1,12 +1,4 @@ #pragma once - -// std -#include - -// libraries - -// project -#include "depthai/common/DetectionNetworkType.hpp" #include "depthai/common/optional.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" #include "depthai/properties/Properties.hpp" @@ -25,6 +17,6 @@ struct SpatialDetectionNetworkProperties : PropertiesSerializablesegmentationPassthrough = passthroughSegmentation; } +void SpatialLocationCalculatorConfig::setBoundingBoxScaleFactor(float scaleFactor) { + if(scaleFactor <= 0.0f) { + throw std::invalid_argument("Scale factor must be larger than 0."); + } + bBoxScaleFactor = scaleFactor; +} + std::pair SpatialLocationCalculatorConfig::getDepthThresholds() const { return {globalLowerThreshold, globalUpperThreshold}; } @@ -87,4 +94,8 @@ bool SpatialLocationCalculatorConfig::getSegmentationPassthrough() const { return segmentationPassthrough; } +float SpatialLocationCalculatorConfig::getBoundingBoxScaleFactor() const { + return bBoxScaleFactor; +} + } // namespace dai diff --git a/src/pipeline/node/SpatialDetectionNetwork.cpp b/src/pipeline/node/SpatialDetectionNetwork.cpp index dcf8039a8f..4771cbbdff 100644 --- a/src/pipeline/node/SpatialDetectionNetwork.cpp +++ b/src/pipeline/node/SpatialDetectionNetwork.cpp @@ -1,14 +1,11 @@ #include "depthai/pipeline/node/SpatialDetectionNetwork.hpp" -#include - #include "../../utility/ErrorMacros.hpp" -#include "depthai/common/DetectionNetworkType.hpp" #include "depthai/modelzoo/Zoo.hpp" #include "depthai/pipeline/node/NeuralDepth.hpp" #include "nn_archive/NNArchive.hpp" -#include "openvino/BlobReader.hpp" #include "openvino/OpenVINO.hpp" +#include "pipeline/DeviceNodeGroup.hpp" namespace dai { namespace node { @@ -17,18 +14,110 @@ namespace node { // Base Detection Network Class //-------------------------------------------------------------------- +SpatialDetectionNetwork::SpatialDetectionNetwork(const std::shared_ptr& device) + : DeviceNodeGroup(device, std::make_unique(), false), + properties(static_cast(*propertiesHolder)) +#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 + , + input{neuralNetwork->input}, + outNetwork{neuralNetwork->out}, + passthrough{neuralNetwork->passthrough}, + inputDepth{spatialLocationCalculator->inputDepth}, + out{spatialLocationCalculator->outputDetections}, + passthroughDepth{spatialLocationCalculator->passthroughDepth} +#endif +{ + if(device) { + auto platform = device->getPlatform(); + if(platform == Platform::RVC4) { + if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); + } + } +}; + +SpatialDetectionNetwork::SpatialDetectionNetwork(std::unique_ptr props) + : DeviceNodeGroup(std::move(props), true), + properties(static_cast(*propertiesHolder)) +#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 + , + input{neuralNetwork->input}, + outNetwork{neuralNetwork->out}, + passthrough{neuralNetwork->passthrough}, + inputDepth{spatialLocationCalculator->inputDepth}, + out{spatialLocationCalculator->outputDetections}, + passthroughDepth{spatialLocationCalculator->passthroughDepth} +#endif +{ + auto device = getDevice(); + if(device) { + auto platform = device->getPlatform(); + if(platform == Platform::RVC4) { + if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); + } + } +}; + +SpatialDetectionNetwork::SpatialDetectionNetwork(std::unique_ptr props, bool confMode) + : DeviceNodeGroup(std::move(props), confMode), + properties(static_cast(*propertiesHolder)) +#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 + , + input{neuralNetwork->input}, + outNetwork{neuralNetwork->out}, + passthrough{neuralNetwork->passthrough}, + inputDepth{spatialLocationCalculator->inputDepth}, + out{spatialLocationCalculator->outputDetections}, + passthroughDepth{spatialLocationCalculator->passthroughDepth} +#endif +{ + auto device = getDevice(); + if(device) { + auto platform = device->getPlatform(); + if(platform == Platform::RVC4) { + if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); + } + } +}; + +SpatialDetectionNetwork::SpatialDetectionNetwork(const std::shared_ptr& device, std::unique_ptr props, bool confMode) + : DeviceNodeGroup(device, std::move(props), confMode), + properties(static_cast(*propertiesHolder)) +#ifndef DEPTHAI_INTERNAL_DEVICE_BUILD_RVC4 + , + input{neuralNetwork->input}, + outNetwork{neuralNetwork->out}, + passthrough{neuralNetwork->passthrough}, + inputDepth{spatialLocationCalculator->inputDepth}, + out{spatialLocationCalculator->outputDetections}, + passthroughDepth{spatialLocationCalculator->passthroughDepth} +#endif +{ + if(device) { + auto platform = device->getPlatform(); + if(platform == Platform::RVC4) { + if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); + } + } +}; + void SpatialDetectionNetwork::buildInternal() { // Default confidence threshold detectionParser->properties.parser.confidenceThreshold = 0.5; + + // Mirror SpatialDetectionNetwork properties onto the SpatialLocationCalculator initial config + auto& initialConfig = *spatialLocationCalculator->initialConfig; + initialConfig.setDepthThresholds(properties.depthThresholds.lowerThreshold, properties.depthThresholds.upperThreshold); + initialConfig.setCalculationAlgorithm(properties.calculationAlgorithm); + initialConfig.setStepSize(properties.stepSize); + neuralNetwork->out.link(detectionParser->input); - neuralNetwork->passthrough.link(inputImg); - detectionParser->out.link(inputDetections); + detectionParser->out.link(spatialLocationCalculator->inputDetections); // No "internal" buffering to keep interface similar to monolithic nodes detectionParser->input.setBlocking(true); detectionParser->input.setMaxSize(1); - inputDetections.setMaxSize(1); - inputDetections.setBlocking(true); + spatialLocationCalculator->inputDetections.setBlocking(true); + spatialLocationCalculator->inputDetections.setMaxSize(1); } std::shared_ptr SpatialDetectionNetwork::build(const std::shared_ptr& inputRgb, @@ -81,23 +170,23 @@ void SpatialDetectionNetwork::alignDepthImpl(const std::shared_ptr& auto platform = device->getPlatform(); switch(platform) { case Platform::RVC4: { + if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); Subnode& align = *depthAlign; stereo->depth.link(align->input); neuralNetwork->passthrough.link(align->inputAlignTo); - align->outputAligned.link(inputDepth); + align->outputAligned.link(spatialLocationCalculator->inputDepth); } break; - case Platform::RVC2: - stereo->depth.link(inputDepth); + case Platform::RVC2: { neuralNetwork->passthrough.link(stereo->inputAlignTo); - break; + stereo->depth.link(spatialLocationCalculator->inputDepth); + } break; case Platform::RVC3: default: throw std::runtime_error("Unsupported platform"); break; } - } else { - stereo->depth.link(inputDepth); + stereo->depth.link(spatialLocationCalculator->inputDepth); stereo->setDepthAlign(camera->getBoardSocket()); } } @@ -107,10 +196,11 @@ void SpatialDetectionNetwork::alignDepthImpl(const std::shared_ptr& auto device = getDevice(); DAI_CHECK_V(device, "Device is not set."); DAI_CHECK_V(device->getPlatform() == Platform::RVC4, "NeuralDepth with SpatialDetectionNetwork is only supported on RVC4 platforms"); + if(!depthAlign) depthAlign = std::make_unique>(*this, "depthAlign"); Subnode& align = *depthAlign; neuralDepth->depth.link(align->input); neuralNetwork->passthrough.link(align->inputAlignTo); - align->outputAligned.link(inputDepth); + align->outputAligned.link(spatialLocationCalculator->inputDepth); } void SpatialDetectionNetwork::alignDepthImpl(const std::shared_ptr& tof, const std::shared_ptr& camera) { @@ -244,23 +334,29 @@ float SpatialDetectionNetwork::getConfidenceThreshold() const { } void SpatialDetectionNetwork::setBoundingBoxScaleFactor(float scaleFactor) { + spatialLocationCalculator->initialConfig->setBoundingBoxScaleFactor(scaleFactor); properties.detectedBBScaleFactor = scaleFactor; } void SpatialDetectionNetwork::setDepthLowerThreshold(uint32_t lowerThreshold) { properties.depthThresholds.lowerThreshold = lowerThreshold; + spatialLocationCalculator->initialConfig->setDepthThresholds(lowerThreshold); } void SpatialDetectionNetwork::setDepthUpperThreshold(uint32_t upperThreshold) { properties.depthThresholds.upperThreshold = upperThreshold; + uint32_t currentLower = spatialLocationCalculator->initialConfig->getDepthThresholds().first; + spatialLocationCalculator->initialConfig->setDepthThresholds(currentLower, upperThreshold); } void SpatialDetectionNetwork::setSpatialCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm calculationAlgorithm) { properties.calculationAlgorithm = calculationAlgorithm; + spatialLocationCalculator->initialConfig->setCalculationAlgorithm(calculationAlgorithm); } void SpatialDetectionNetwork::setSpatialCalculationStepSize(int stepSize) { properties.stepSize = stepSize; + spatialLocationCalculator->initialConfig->setStepSize(stepSize); } std::optional> SpatialDetectionNetwork::getClasses() const { diff --git a/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp index 64c226487f..1cb646ac00 100644 --- a/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp +++ b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp @@ -13,6 +13,7 @@ #include "common/Keypoint.hpp" #include "common/Point2f.hpp" #include "common/RotatedRect.hpp" +#include "common/Size2f.hpp" #include "depthai/common/Point3f.hpp" #include "depthai/common/SpatialKeypoint.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" @@ -181,6 +182,13 @@ dai::SpatialImgDetection createSpatialDetection(const dai::ImgDetection& detecti return spatialDetection; } +dai::RotatedRect scaleRect(const dai::RotatedRect& rect, float scaleFactor) { + dai::RotatedRect scaledRect = rect; + scaledRect.size.width *= scaleFactor; + scaledRect.size.height *= scaleFactor; + return scaledRect; +} + void computeSpatialData(const std::shared_ptr& depthFrame, const std::vector& configDataVec, std::vector& spatialLocations, @@ -233,7 +241,7 @@ void computeSpatialData(const std::shared_ptr& depthFrame, std::uint32_t maxNumPixels = ((xend - xstart + stepSize - 1) / stepSize) * ((yend - ystart + stepSize - 1) / stepSize); DepthStats depthStats(cfg.depthThresholds.lowerThreshold, cfg.depthThresholds.upperThreshold, maxNumPixels); - const uint16_t* plane = (uint16_t*)depthFrame->data->getData().data(); + const uint16_t* plane = reinterpret_cast(depthFrame->data->getData().data()); for(int y = ystart; y < yend; y += stepSize) { for(int x = xstart; x < xend; x += stepSize) { uint16_t px = plane[y * depthWidth + x]; @@ -308,8 +316,14 @@ dai::SpatialImgDetection computeSpatialDetection(const dai::ImgFrame& depthFrame const dai::ImgTransformation* depthTransformation = &depthFrame.transformation; const bool areAligned = detectionsTransformation.isAlignedTo(*depthTransformation); + SpatialLocationCalculatorConfigData boundingBoxMapping; + boundingBoxMapping.depthThresholds.lowerThreshold = lowerThreshold; + boundingBoxMapping.depthThresholds.upperThreshold = upperThreshold; + boundingBoxMapping.calculationAlgorithm = calculationAlgorithm; + const dai::RotatedRect detectionBBox = detection.getBoundingBox(); - dai::RotatedRect denormalizedRect = detectionBBox; + dai::RotatedRect denormalizedRect = scaleRect(detectionBBox, config.bBoxScaleFactor); + boundingBoxMapping.roi = dai::Rect(denormalizedRect); if(!areAligned) denormalizedRect = detectionsTransformation.remapRectTo(*depthTransformation, denormalizedRect); denormalizedRect = denormalizedRect.denormalize(depthWidth, depthHeight); @@ -372,10 +386,6 @@ dai::SpatialImgDetection computeSpatialDetection(const dai::ImgFrame& depthFrame spatialDetection.setKeypoints(spatialKeypoints); } - SpatialLocationCalculatorConfigData boundingBoxMapping; - boundingBoxMapping.depthThresholds.lowerThreshold = lowerThreshold; - boundingBoxMapping.depthThresholds.upperThreshold = upperThreshold; - boundingBoxMapping.calculationAlgorithm = calculationAlgorithm; spatialDetection.boundingBoxMapping = boundingBoxMapping; return spatialDetection; diff --git a/tests/src/ondevice_tests/pipeline/node/spatial_detection_network_test.cpp b/tests/src/ondevice_tests/pipeline/node/spatial_detection_network_test.cpp index 2e968b1025..3cbb55dbf3 100644 --- a/tests/src/ondevice_tests/pipeline/node/spatial_detection_network_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/spatial_detection_network_test.cpp @@ -60,7 +60,6 @@ TEST_CASE("SpatialDetectionNetwork model description API") { auto right = p.create()->build(dai::CameraBoardSocket::CAM_C); auto stereo = p.create()->build(*left->requestOutput(std::make_pair(640, 400)), *right->requestOutput(std::make_pair(640, 400))); stereo->setSubpixel(false); - if(p.getDefaultDevice()->getPlatform() == dai::Platform::RVC2) stereo->setOutputSize(640, 400); dai::NNModelDescription modelDesc{"yolov6-nano"}; // Load NNArchive @@ -83,7 +82,6 @@ TEST_CASE("SpatialDetectionNetwork NNArchive API") { auto right = p.create()->build(dai::CameraBoardSocket::CAM_C); auto stereo = p.create()->build(*left->requestOutput(std::make_pair(640, 400)), *right->requestOutput(std::make_pair(640, 400))); stereo->setSubpixel(false); - if(p.getDefaultDevice()->getPlatform() == dai::Platform::RVC2) stereo->setOutputSize(640, 400); // Load NNArchive auto platform = p.getDefaultDevice()->getPlatformAsString(); dai::NNModelDescription modelDesc{"yolov6-nano", platform}; diff --git a/tests/src/onhost_tests/calibration_handler_test.cpp b/tests/src/onhost_tests/calibration_handler_test.cpp index 40574eb0ce..9ababb9a2e 100644 --- a/tests/src/onhost_tests/calibration_handler_test.cpp +++ b/tests/src/onhost_tests/calibration_handler_test.cpp @@ -1,7 +1,7 @@ #include +#include #include #include -#include #include #include From e8393d0ed6b140cdff1e49141b1de585b154372f Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 22 Jan 2026 11:04:39 +0100 Subject: [PATCH 061/180] Updated events examples --- examples/cpp/Events/events.cpp | 6 ++++-- examples/cpp/Events/events_file_group.cpp | 2 +- examples/python/Events/events.py | 6 ++++-- examples/python/Events/events_file_group.py | 3 ++- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/examples/cpp/Events/events.cpp b/examples/cpp/Events/events.cpp index 65b0bbc249..415e1a33c3 100644 --- a/examples/cpp/Events/events.cpp +++ b/examples/cpp/Events/events.cpp @@ -59,8 +59,10 @@ int main() { pipeline.start(); + std::cout << "Press 's' to send a snap, and 'q' to quit" << std::endl; while(pipeline.isRunning()) { - if(cv::waitKey(1) != -1) { + auto key = cv::waitKey(1); + if(key == 'q') { break; } @@ -98,7 +100,7 @@ int main() { } // Trigger sendSnap() - if(cv::waitKey(1) == 's') { + if(key == 's') { auto localSnapID = eventsManager->sendSnap("ImageDetection", std::nullopt, inRgb, diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index 55c4a3c2c0..6ca3b36feb 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -61,7 +61,7 @@ int main() { int counter = 0; while(pipeline.isRunning()) { - if(cv::waitKey(1) != -1) { + if(cv::waitKey(1) == 'q') { break; } diff --git a/examples/python/Events/events.py b/examples/python/Events/events.py index 132fd98b46..943c084b55 100644 --- a/examples/python/Events/events.py +++ b/examples/python/Events/events.py @@ -48,8 +48,10 @@ def frameNorm(frame, bbox): return (np.clip(np.array(bbox), 0, 1) * normVals).astype(int) + print("Press 's' to send a snap, and 'q' to quit") while pipeline.isRunning(): - if cv2.waitKey(1) != -1: + key = cv2.waitKey(1) + if key == ord("q"): pipeline.stop() break @@ -88,7 +90,7 @@ def frameNorm(frame, bbox): cv2.imshow("rgb", frame) # Trigger sendSnap() - if cv2.waitKey(1) == ord("s"): + if key == ord("s"): localSnapID = eventMan.sendSnap("ImageDetection", None, inRgb, inDet, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, uploadSuccessCallback, uploadFailureCallback) if localSnapID != None: diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index da942f9757..b7bb407b76 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -47,9 +47,10 @@ def frameNorm(frame, bbox): return (np.clip(np.array(bbox), 0, 1) * normVals).astype(int) + print("Press 'q' to quit") counter = 0 while pipeline.isRunning(): - if cv2.waitKey(1) != -1: + if cv2.waitKey(1) == ord("q"): pipeline.stop() break From 7b8bdefb476474837af727ee43ef3435414e7102 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 22 Jan 2026 15:53:20 +0100 Subject: [PATCH 062/180] Bump FW --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index fbe20e4bb5..32eabb33f5 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+2a5145f0430b2cd922c0ab15e958ea1c0a66be0b") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+45960e1e9bde60e465457eb1fb4825fd0afc457b") From 32660bf121f3bb2fedf54d43c415579b221d489a Mon Sep 17 00:00:00 2001 From: asahtik <38485424+asahtik@users.noreply.github.com> Date: Fri, 23 Jan 2026 06:38:30 +0100 Subject: [PATCH 063/180] Add get- and setTransformation to C++ (#1647) --- .../src/pipeline/datatype/ImgFrameBindings.cpp | 4 ++-- include/depthai/pipeline/datatype/ImgFrame.hpp | 12 ++++++++++++ src/pipeline/datatype/ImgFrame.cpp | 9 +++++++++ 3 files changed, 23 insertions(+), 2 deletions(-) diff --git a/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp b/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp index 232dec2e71..c56d99443e 100644 --- a/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp @@ -212,7 +212,7 @@ void bind_imgframe(pybind11::module& m, void* pCallstack) { .def("getSourceDFov", &ImgFrame::getSourceDFov, DOC(dai, ImgFrame, getSourceDFov)) .def("getSourceWidth", &ImgFrame::getSourceWidth, DOC(dai, ImgFrame, getSourceWidth)) .def("getSourceHeight", &ImgFrame::getSourceHeight, DOC(dai, ImgFrame, getSourceHeight)) - .def("getTransformation", [](ImgFrame& msg) { return msg.transformation; }) + .def("getTransformation", &ImgFrame::getTransformation, DOC(dai, ImgFrame, getTransformation)) .def("validateTransformations", &ImgFrame::validateTransformations, DOC(dai, ImgFrame, validateTransformations)) #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT @@ -265,7 +265,7 @@ void bind_imgframe(pybind11::module& m, void* pCallstack) { py::arg("sizer"), DOC(dai, ImgFrame, setSize, 2)) .def("setType", &ImgFrame::setType, py::arg("type"), DOC(dai, ImgFrame, setType)) - .def("setTransformation", [](ImgFrame& msg, const ImgTransformation& transformation) { msg.transformation = transformation; }) + .def("setTransformation", &ImgFrame::setTransformation, py::arg("transformation"), DOC(dai, ImgFrame, setTransformation)) // .def("set", &ImgFrame::set, py::arg("type"), DOC(dai, ImgFrame, set)) ; // add aliases dai.ImgFrame.Type and dai.ImgFrame.Specs diff --git a/include/depthai/pipeline/datatype/ImgFrame.hpp b/include/depthai/pipeline/datatype/ImgFrame.hpp index caa51ca960..ed0928d95a 100644 --- a/include/depthai/pipeline/datatype/ImgFrame.hpp +++ b/include/depthai/pipeline/datatype/ImgFrame.hpp @@ -193,6 +193,11 @@ class ImgFrame : public Buffer, public ProtoSerializable { */ float getLensPositionRaw() const; + /** + * Retrieves image transformation data + */ + ImgTransformation& getTransformation(); + /** * Instance number relates to the origin of the frame (which camera) * @@ -263,6 +268,13 @@ class ImgFrame : public Buffer, public ProtoSerializable { */ ImgFrame& setType(Type type); + /** + * Specifies image transformation data + * + * @param transformation transformation data + */ + ImgFrame& setTransformation(const ImgTransformation& transformation); + /** * Remap a point from the current frame to the source frame * @param point point to remap diff --git a/src/pipeline/datatype/ImgFrame.cpp b/src/pipeline/datatype/ImgFrame.cpp index fb73c7d229..3b80e5d2bf 100644 --- a/src/pipeline/datatype/ImgFrame.cpp +++ b/src/pipeline/datatype/ImgFrame.cpp @@ -131,6 +131,10 @@ unsigned int ImgFrame::getSourceHeight() const { return sourceFb.height; } +ImgTransformation& ImgFrame::getTransformation() { + return transformation; +} + ImgFrame& ImgFrame::setInstanceNum(unsigned int instanceNum) { this->instanceNum = instanceNum; return *this; @@ -180,6 +184,11 @@ ImgFrame& ImgFrame::setType(Type type) { return *this; } +ImgFrame& ImgFrame::setTransformation(const ImgTransformation& transformation) { + this->transformation = transformation; + return *this; +} + ImgFrame& ImgFrame::setMetadata(const ImgFrame& sourceFrame) { auto tmpData = this->data; *this = sourceFrame; From f60a9d5cf5391fe3537bec18ca0e15ea1cf5345a Mon Sep 17 00:00:00 2001 From: asahtik <38485424+asahtik@users.noreply.github.com> Date: Fri, 23 Jan 2026 06:38:56 +0100 Subject: [PATCH 064/180] Throw if there is no script set on Script node (#1645) * Throw if there is no script set on Script node * Add test for missing script in script node --- include/depthai/pipeline/node/Script.hpp | 2 ++ src/pipeline/node/Script.cpp | 6 ++++++ tests/src/ondevice_tests/script_node_test.cpp | 11 +++++++++++ 3 files changed, 19 insertions(+) diff --git a/include/depthai/pipeline/node/Script.hpp b/include/depthai/pipeline/node/Script.hpp index bd2b695e77..bb1c707f08 100644 --- a/include/depthai/pipeline/node/Script.hpp +++ b/include/depthai/pipeline/node/Script.hpp @@ -85,6 +85,8 @@ class Script : public DeviceNodeCRTP { ProcessorType getProcessor() const; void buildInternal() override; + + void buildStage1() override; }; } // namespace node diff --git a/src/pipeline/node/Script.cpp b/src/pipeline/node/Script.cpp index 7ef3351c3f..38b6705b06 100644 --- a/src/pipeline/node/Script.cpp +++ b/src/pipeline/node/Script.cpp @@ -8,6 +8,12 @@ namespace node { void Script::buildInternal() {} +void Script::buildStage1() { + if(properties.scriptUri.empty()) { + throw std::runtime_error("No script set. Please set a script using setScriptPath or setScript."); + } +} + void Script::setScriptPath(const std::filesystem::path& path, const std::string& name) { properties.scriptUri = assetManager.set("__script", path)->getRelativeUri(); scriptPath = path; diff --git a/tests/src/ondevice_tests/script_node_test.cpp b/tests/src/ondevice_tests/script_node_test.cpp index 6f35089b15..77f6ce6c03 100644 --- a/tests/src/ondevice_tests/script_node_test.cpp +++ b/tests/src/ondevice_tests/script_node_test.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -56,3 +57,13 @@ TEST_CASE("New API Test") { REQUIRE(output != nullptr); } + +TEST_CASE("Missing script exception test") { + // Create pipeline + dai::Pipeline pipeline; + + // Setup script node + auto script = pipeline.create(); + + REQUIRE_THROWS_AS(pipeline.build(), std::runtime_error); +} From bb43ec4f25d8b330bbdae34440088480cb992d7d Mon Sep 17 00:00:00 2001 From: lnotspotl Date: Fri, 23 Jan 2026 10:58:25 +0100 Subject: [PATCH 065/180] add missing stereo depth filter bindings --- bindings/python/src/pipeline/FilterParamsBindings.cpp | 1 + .../python/src/pipeline/datatype/StereoDepthConfigBindings.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/bindings/python/src/pipeline/FilterParamsBindings.cpp b/bindings/python/src/pipeline/FilterParamsBindings.cpp index 4e3882af20..bf91f5e8ea 100644 --- a/bindings/python/src/pipeline/FilterParamsBindings.cpp +++ b/bindings/python/src/pipeline/FilterParamsBindings.cpp @@ -87,6 +87,7 @@ void FilterParamsBindings::bind(pybind11::module& m, void* pCallstack) { // Aliases for backward compatibility m.attr("MedianFilter") = medianFilter; m.attr("StereoDepthConfig").attr("MedianFilter") = medianFilter; + m.attr("StereoDepthConfig").attr("PostProcessing").attr("MedianFilter") = medianFilter; m.attr("StereoDepthConfig").attr("PostProcessing").attr("SpatialFilter") = spatialFilter; m.attr("StereoDepthConfig").attr("PostProcessing").attr("TemporalFilter") = temporalFilter; m.attr("StereoDepthConfig").attr("PostProcessing").attr("SpeckleFilter") = speckleFilter; diff --git a/bindings/python/src/pipeline/datatype/StereoDepthConfigBindings.cpp b/bindings/python/src/pipeline/datatype/StereoDepthConfigBindings.cpp index 170792b54f..68c01159c6 100644 --- a/bindings/python/src/pipeline/datatype/StereoDepthConfigBindings.cpp +++ b/bindings/python/src/pipeline/datatype/StereoDepthConfigBindings.cpp @@ -344,6 +344,7 @@ void bind_stereodepthconfig(pybind11::module& m, void* pCallstack) { .def("getFiltersComputeBackend", &StereoDepthConfig::getFiltersComputeBackend, DOC(dai, StereoDepthConfig, getFiltersComputeBackend)); m.attr("StereoDepthConfig").attr("AlgorithmControl") = algorithmControl; m.attr("StereoDepthConfig").attr("PostProcessing") = postProcessing; + m.attr("StereoDepthConfig").attr("PostProcessing").attr("Filter") = filterEnum; m.attr("StereoDepthConfig").attr("CensusTransform") = censusTransform; m.attr("StereoDepthConfig").attr("CostMatching") = costMatching; m.attr("StereoDepthConfig").attr("CostAggregation") = costAggregation; From 17ed4694c4a2d64d6c24f9ce597a0eaf512c0dc6 Mon Sep 17 00:00:00 2001 From: lnotspotl Date: Fri, 23 Jan 2026 13:11:42 +0100 Subject: [PATCH 066/180] add camera image orientation setter and getter --- include/depthai/pipeline/node/Camera.hpp | 13 +++++++++++++ src/pipeline/node/Camera.cpp | 9 +++++++++ 2 files changed, 22 insertions(+) diff --git a/include/depthai/pipeline/node/Camera.hpp b/include/depthai/pipeline/node/Camera.hpp index 061b2542fe..c2cfb32613 100644 --- a/include/depthai/pipeline/node/Camera.hpp +++ b/include/depthai/pipeline/node/Camera.hpp @@ -72,6 +72,19 @@ class Camera : public DeviceNodeCRTP, publ return getProperties().sensorType; } + /** + * Set camera image orientation + * @param imageOrientation Image orientation to set + * @return Shared pointer to the camera node + */ + std::shared_ptr setImageOrientation(CameraImageOrientation imageOrientation); + + /** + * Get camera image orientation + * @return Image orientation + */ + CameraImageOrientation getImageOrientation() const; + #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT /** * Build with a specific board socket and mock input diff --git a/src/pipeline/node/Camera.cpp b/src/pipeline/node/Camera.cpp index 2d0706469c..3cb6ef070f 100644 --- a/src/pipeline/node/Camera.cpp +++ b/src/pipeline/node/Camera.cpp @@ -413,5 +413,14 @@ std::optional Camera::getOutputsMaxSizePool() const { return properties.maxSizePoolOutputs; } +std::shared_ptr Camera::setImageOrientation(CameraImageOrientation imageOrientation) { + properties.imageOrientation = imageOrientation; + return std::dynamic_pointer_cast(shared_from_this()); +} + +CameraImageOrientation Camera::getImageOrientation() const { + return properties.imageOrientation; +} + } // namespace node } // namespace dai From 0073e6cfae2dc585be2c8c62715527d2028b9c4a Mon Sep 17 00:00:00 2001 From: lnotspotl Date: Fri, 23 Jan 2026 13:15:18 +0100 Subject: [PATCH 067/180] add python bindings for camera orientation setter and getter --- bindings/python/src/pipeline/node/CameraBindings.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bindings/python/src/pipeline/node/CameraBindings.cpp b/bindings/python/src/pipeline/node/CameraBindings.cpp index 269fb09d0f..5d00722d4d 100644 --- a/bindings/python/src/pipeline/node/CameraBindings.cpp +++ b/bindings/python/src/pipeline/node/CameraBindings.cpp @@ -40,6 +40,8 @@ void bind_camera(pybind11::module& m, void* pCallstack) { #endif .def("setSensorType", py::overload_cast(&Camera::setSensorType), py::arg("sensorType"), DOC(dai, node, Camera, setSensorType)) .def("getSensorType", &Camera::getSensorType, DOC(dai, node, Camera, getSensorType)) + .def("setImageOrientation", &Camera::setImageOrientation, py::arg("imageOrientation"), DOC(dai, node, Camera, setImageOrientation)) + .def("getImageOrientation", &Camera::getImageOrientation, DOC(dai, node, Camera, getImageOrientation)) .def("setRawNumFramesPool", &Camera::setRawNumFramesPool, "num"_a, DOC(dai, node, Camera, setRawNumFramesPool)) .def("setMaxSizePoolRaw", &Camera::setMaxSizePoolRaw, "size"_a, DOC(dai, node, Camera, setMaxSizePoolRaw)) .def("setIspNumFramesPool", &Camera::setIspNumFramesPool, "num"_a, DOC(dai, node, Camera, setIspNumFramesPool)) From 7389a033fbb46cd097f717ae08d8b4a24e9c649d Mon Sep 17 00:00:00 2001 From: AljazD Date: Fri, 23 Jan 2026 13:24:23 +0100 Subject: [PATCH 068/180] Disabled container-overflow detection inside neural_depth_node_test --- tests/src/ondevice_tests/neural_depth_node_test.cpp | 5 +++++ tests/src/onhost_tests/replay_test.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/src/ondevice_tests/neural_depth_node_test.cpp b/tests/src/ondevice_tests/neural_depth_node_test.cpp index a5800155fb..918a555260 100644 --- a/tests/src/ondevice_tests/neural_depth_node_test.cpp +++ b/tests/src/ondevice_tests/neural_depth_node_test.cpp @@ -3,6 +3,11 @@ #include "depthai/depthai.hpp" +// Disable container overflow detection for this test binary (false positive from protobuf) +extern "C" const char* __asan_default_options() { + return "detect_container_overflow=0"; +} + namespace { void testNeuralDepthModelBasic(dai::DeviceModelZoo model, float minFps) { dai::Pipeline pipeline; diff --git a/tests/src/onhost_tests/replay_test.cpp b/tests/src/onhost_tests/replay_test.cpp index fe6e6e3eef..5558f7c8f8 100644 --- a/tests/src/onhost_tests/replay_test.cpp +++ b/tests/src/onhost_tests/replay_test.cpp @@ -14,7 +14,7 @@ constexpr unsigned int NUM_MSGS = 100; -// Disable container overflow detection for this test binary +// Disable container overflow detection for this test binary (false positive from protobuf) extern "C" const char* __asan_default_options() { return "detect_container_overflow=0"; } From 2d21e6c116a998bbae3d715955fd9ee5047da07a Mon Sep 17 00:00:00 2001 From: asahtik <38485424+asahtik@users.noreply.github.com> Date: Fri, 23 Jan 2026 16:09:11 +0100 Subject: [PATCH 069/180] Fix failing test (#1650) --- .../src/ondevice_tests/pipeline_debugging_rvc4_test.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/tests/src/ondevice_tests/pipeline_debugging_rvc4_test.cpp b/tests/src/ondevice_tests/pipeline_debugging_rvc4_test.cpp index 90d7270633..fdb7b05e64 100644 --- a/tests/src/ondevice_tests/pipeline_debugging_rvc4_test.cpp +++ b/tests/src/ondevice_tests/pipeline_debugging_rvc4_test.cpp @@ -179,7 +179,7 @@ TEST_CASE("FPS check") { REQUIRE(nodeState.outputStates.at("outputAligned").isValid()); REQUIRE(nodeState.outputStates.at("outputAligned").timing.fps == Catch::Approx(30.0).margin(5.0)); } - if(std::string(node->getName()) == "SpatialDetectionNetwork") { + if(std::string(node->getName()) == "SpatialLocationCalculator") { ++gotNodes; REQUIRE(nodeState.mainLoopTiming.fps == Catch::Approx(30.0).margin(5.0)); REQUIRE(nodeState.inputsGetTiming.fps == Catch::Approx(30.0).margin(5.0)); @@ -188,16 +188,15 @@ TEST_CASE("FPS check") { REQUIRE(nodeState.inputStates.at("inputDepth").timing.fps == Catch::Approx(30.0).margin(5.0)); REQUIRE(nodeState.inputStates.at("inputDetections").isValid()); REQUIRE(nodeState.inputStates.at("inputDetections").timing.fps == Catch::Approx(30.0).margin(5.0)); - REQUIRE(nodeState.inputStates.at("inputImg").isValid()); - REQUIRE(nodeState.inputStates.at("inputImg").timing.fps == Catch::Approx(30.0).margin(5.0)); REQUIRE(nodeState.outputStates.at("passthroughDepth").isValid()); REQUIRE(nodeState.outputStates.at("passthroughDepth").timing.fps == Catch::Approx(30.0).margin(5.0)); + REQUIRE(nodeState.outputStates.at("outputDetections").isValid()); + REQUIRE(nodeState.outputStates.at("outputDetections").timing.fps == Catch::Approx(30.0).margin(5.0)); REQUIRE(nodeState.outputStates.at("out").isValid()); REQUIRE(nodeState.outputStates.at("out").timing.fps == Catch::Approx(30.0).margin(5.0)); } } - REQUIRE(gotNodes == 8); // 3 cameras, stereo, neural network, detection parser, image align, spatial detection network - + REQUIRE(gotNodes == 8); // 3 cameras, stereo, neural network, detection parser, image align, spatial location calculator pipeline.stop(); } From 9f72399ec2ecaf8fabf0ede85d0a2bbaedcbaf01 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Fri, 23 Jan 2026 23:35:01 +0100 Subject: [PATCH 070/180] Refactored wrong usage of parant ref from PipelineImpl class --- include/depthai/pipeline/Pipeline.hpp | 10 +++++----- src/opencv/HolisticRecordReplay.cpp | 4 ++-- src/pipeline/Pipeline.cpp | 12 ++++++------ src/utility/HolisticRecordReplay.hpp | 4 ++-- src/utility/PipelineImplHelper.cpp | 18 ++++++++++++------ src/utility/PipelineImplHelper.hpp | 4 ++-- 6 files changed, 29 insertions(+), 23 deletions(-) diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index ff91c75ff8..5432a0b8e8 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -38,12 +38,15 @@ class PipelineImpl : public std::enable_shared_from_this { friend class utility::PipelineImplHelper; public: - PipelineImpl(Pipeline& pipeline, bool createImplicitDevice = true) : assetManager("/pipeline/"), parent(pipeline) { + PipelineImpl(Pipeline& pipeline, bool createImplicitDevice = true) : assetManager("/pipeline/") { + (void)pipeline; if(createImplicitDevice) { defaultDevice = std::make_shared(); } } - PipelineImpl(Pipeline& pipeline, std::shared_ptr device) : assetManager("/pipeline/"), parent(pipeline), defaultDevice{std::move(device)} {} + PipelineImpl(Pipeline& pipeline, std::shared_ptr device) : assetManager("/pipeline/"), defaultDevice{std::move(device)} { + (void)pipeline; + } PipelineImpl(const PipelineImpl&) = delete; PipelineImpl& operator=(const PipelineImpl&) = delete; PipelineImpl(PipelineImpl&&) = delete; @@ -138,9 +141,6 @@ class PipelineImpl : public std::enable_shared_from_this { // Output queues std::vector> outputQueues; - // parent - Pipeline& parent; - // is pipeline running AtomicBool running{false}; diff --git a/src/opencv/HolisticRecordReplay.cpp b/src/opencv/HolisticRecordReplay.cpp index 0a8b3068e9..f2f1681a09 100644 --- a/src/opencv/HolisticRecordReplay.cpp +++ b/src/opencv/HolisticRecordReplay.cpp @@ -92,7 +92,7 @@ Node::Output* setupHolistiRecordCamera( return cam->requestOutput(std::make_pair(width, height), dai::ImgFrame::Type::NV12, dai::ImgResizeMode::CROP); } -bool setupHolisticRecord(Pipeline& pipeline, +bool setupHolisticRecord(Pipeline pipeline, const std::string& deviceId, RecordConfig& recordConfig, std::unordered_map& outFilenames, @@ -178,7 +178,7 @@ bool setupHolisticRecord(Pipeline& pipeline, return true; } -bool setupHolisticReplay(Pipeline& pipeline, +bool setupHolisticReplay(Pipeline pipeline, std::filesystem::path replayPath, const std::string& deviceId, RecordConfig& recordConfig, diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 009b767002..05fb66a910 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -436,7 +436,7 @@ BoardConfig PipelineImpl::getBoardConfig() const { void PipelineImpl::remove(std::shared_ptr toRemove) { DAI_CHECK_V(!isBuilt(), "Cannot remove node from pipeline once it is built."); DAI_CHECK_V(toRemove->parent.lock() != nullptr, "Cannot remove a node that is not a part of any pipeline"); - DAI_CHECK_V(toRemove->parent.lock() == parent.pimpl, "Cannot remove a node that is not a part of this pipeline"); + DAI_CHECK_V(toRemove->parent.lock() == shared_from_this(), "Cannot remove a node that is not a part of this pipeline"); // First remove the node from the pipeline directly auto it = std::remove(nodes.begin(), nodes.end(), toRemove); @@ -582,8 +582,8 @@ void PipelineImpl::add(std::shared_ptr node) { } if(curNode->parent.lock() == nullptr) { - curNode->parent = parent.pimpl; - } else if(curNode->parent.lock() != parent.pimpl) { + curNode->parent = shared_from_this(); + } else if(curNode->parent.lock() != shared_from_this()) { throw std::invalid_argument("Cannot add a node that is already part of another pipeline"); } @@ -615,7 +615,7 @@ void PipelineImpl::build() { if(isBuild) return; - utility::PipelineImplHelper(this).setupHolisticRecordAndReplay(); + utility::PipelineImplHelper(shared_from_this()).setupHolisticRecordAndReplay(); // Run first build stage for all nodes for(const auto& node : getAllNodes()) { @@ -631,7 +631,7 @@ void PipelineImpl::build() { node->buildStage3(); } - utility::PipelineImplHelper(this).setupPipelineDebuggingPre(); + utility::PipelineImplHelper(shared_from_this()).setupPipelineDebuggingPre(); // Go through all the connections and handle any // Host -> Device connections @@ -774,7 +774,7 @@ void PipelineImpl::build() { } } - utility::PipelineImplHelper(this).setupPipelineDebuggingPost(bridgesOut, bridgesIn); + utility::PipelineImplHelper(shared_from_this()).setupPipelineDebuggingPost(bridgesOut, bridgesIn); isBuild = true; } diff --git a/src/utility/HolisticRecordReplay.hpp b/src/utility/HolisticRecordReplay.hpp index ae1203cd5c..f87cb9f487 100644 --- a/src/utility/HolisticRecordReplay.hpp +++ b/src/utility/HolisticRecordReplay.hpp @@ -12,12 +12,12 @@ namespace dai { namespace utility { #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT -bool setupHolisticRecord(Pipeline& pipeline, +bool setupHolisticRecord(Pipeline pipeline, const std::string& deviceId, RecordConfig& recordConfig, std::unordered_map& outFilenames, bool legacy = false); -bool setupHolisticReplay(Pipeline& pipeline, +bool setupHolisticReplay(Pipeline pipeline, std::filesystem::path replayPath, const std::string& deviceId, RecordConfig& recordConfig, diff --git a/src/utility/PipelineImplHelper.cpp b/src/utility/PipelineImplHelper.cpp index a56a28132d..5de9123ba7 100644 --- a/src/utility/PipelineImplHelper.cpp +++ b/src/utility/PipelineImplHelper.cpp @@ -15,6 +15,8 @@ namespace dai { namespace utility { void PipelineImplHelper::setupHolisticRecordAndReplay() { + auto pipeline = pipelineWeak.lock(); + // TODO: Refactor this function to reduce complexity if(pipeline->buildingOnHost) { if(pipeline->defaultDevice) { @@ -49,7 +51,7 @@ void PipelineImplHelper::setupHolisticRecordAndReplay() { } else if(!recordPath.empty()) { if(pipeline->enableHolisticRecordReplay || checkRecordConfig(recordPath, pipeline->recordConfig)) { if(platform::checkWritePermissions(recordPath)) { - if(utility::setupHolisticRecord(pipeline->parent, + if(utility::setupHolisticRecord(dai::Pipeline(pipeline), pipeline->defaultDeviceId, pipeline->recordConfig, pipeline->recordReplayFilenames, @@ -69,7 +71,7 @@ void PipelineImplHelper::setupHolisticRecordAndReplay() { } else if(!replayPath.empty()) { if(platform::checkPathExists(replayPath)) { if(platform::checkWritePermissions(replayPath)) { - if(utility::setupHolisticReplay(pipeline->parent, + if(utility::setupHolisticReplay(dai::Pipeline(pipeline), replayPath, pipeline->defaultDeviceId, pipeline->recordConfig, @@ -107,6 +109,8 @@ void PipelineImplHelper::setupHolisticRecordAndReplay() { } } void PipelineImplHelper::setupPipelineDebuggingPre() { + auto pipeline = pipelineWeak.lock(); + // Create pipeline event aggregator node and link bool envPipelineDebugging = utility::getEnvAs("DEPTHAI_PIPELINE_DEBUGGING", false); pipeline->enablePipelineDebugging = pipeline->enablePipelineDebugging || envPipelineDebugging; @@ -121,11 +125,11 @@ void PipelineImplHelper::setupPipelineDebuggingPre() { } std::shared_ptr hostEventAgg = nullptr; std::shared_ptr deviceEventAgg = nullptr; - hostEventAgg = pipeline->parent.create(); + hostEventAgg = pipeline->create(pipeline); hostEventAgg->setRunOnHost(true); hostEventAgg->setTraceOutput(envPipelineDebugging); if(hasDeviceNodes) { - deviceEventAgg = pipeline->parent.create(); + deviceEventAgg = pipeline->create(pipeline); deviceEventAgg->setRunOnHost(false); deviceEventAgg->setTraceOutput(envPipelineDebugging); } @@ -141,10 +145,10 @@ void PipelineImplHelper::setupPipelineDebuggingPre() { } } } - auto stateMerge = pipeline->parent.create()->build(hasDeviceNodes, true); + auto stateMerge = pipeline->create(pipeline)->build(hasDeviceNodes, true); std::shared_ptr traceStateMerge; if(envPipelineDebugging) { - traceStateMerge = pipeline->parent.create()->build(hasDeviceNodes, true); + traceStateMerge = pipeline->create(pipeline)->build(hasDeviceNodes, true); traceStateMerge->setAllowConfiguration(false); } if(deviceEventAgg) { @@ -173,6 +177,8 @@ void PipelineImplHelper::setupPipelineDebuggingPre() { } void PipelineImplHelper::setupPipelineDebuggingPost(std::unordered_map& bridgesOut, std::unordered_map& bridgesIn) { + auto pipeline = pipelineWeak.lock(); + // Finish setting up pipeline debugging if(pipeline->buildingOnHost && pipeline->enablePipelineDebugging) { // Enable events on xlink bridges diff --git a/src/utility/PipelineImplHelper.hpp b/src/utility/PipelineImplHelper.hpp index 35260e775e..204c45fd05 100644 --- a/src/utility/PipelineImplHelper.hpp +++ b/src/utility/PipelineImplHelper.hpp @@ -6,10 +6,10 @@ namespace dai { namespace utility { class PipelineImplHelper { - PipelineImpl* pipeline; + std::weak_ptr pipelineWeak; public: - PipelineImplHelper(PipelineImpl* pipeline) : pipeline(pipeline){}; + PipelineImplHelper(std::weak_ptr pipeline) : pipelineWeak(pipeline){}; void setupHolisticRecordAndReplay(); void setupPipelineDebuggingPre(); void setupPipelineDebuggingPost(std::unordered_map&, From 74d1f7ad8e261db934e48031642b083a83e77934 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Sat, 24 Jan 2026 18:47:17 +0100 Subject: [PATCH 071/180] Add ImgTransformations to ImgAnnotations --- .../src/pipeline/datatype/ImgAnnotationsBindings.cpp | 4 +++- include/depthai/pipeline/datatype/ImgAnnotations.hpp | 7 ++++++- protos/ImageAnnotations.proto | 1 + src/utility/ProtoSerialize.cpp | 5 +++++ 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/bindings/python/src/pipeline/datatype/ImgAnnotationsBindings.cpp b/bindings/python/src/pipeline/datatype/ImgAnnotationsBindings.cpp index 18c1de452b..e955b70ab2 100644 --- a/bindings/python/src/pipeline/datatype/ImgAnnotationsBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ImgAnnotationsBindings.cpp @@ -97,5 +97,7 @@ void bind_imageannotations(pybind11::module& m, void* pCallstack) { .def_readwrite("annotations", &ImgAnnotations::annotations) .def("getTimestamp", &ImgAnnotations::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &ImgAnnotations::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) - .def("getSequenceNum", &ImgAnnotations::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)); + .def("getSequenceNum", &ImgAnnotations::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) + .def("getTransformation", [](ImgAnnotations& msg) { return msg.transformation; }) + .def("setTransformation", [](ImgAnnotations& msg, const std::optional& transformation) { msg.transformation = transformation; }); } diff --git a/include/depthai/pipeline/datatype/ImgAnnotations.hpp b/include/depthai/pipeline/datatype/ImgAnnotations.hpp index ac7cc1694a..d2ecb08a7f 100644 --- a/include/depthai/pipeline/datatype/ImgAnnotations.hpp +++ b/include/depthai/pipeline/datatype/ImgAnnotations.hpp @@ -1,6 +1,10 @@ #pragma once +#include + #include "depthai/common/Color.hpp" +#include "depthai/common/ImgTransformations.hpp" #include "depthai/common/Point2f.hpp" +#include "depthai/common/optional.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/utility/ProtoSerializable.hpp" @@ -59,6 +63,7 @@ class ImgAnnotations : public Buffer, public ProtoSerializable { /// Transform std::vector annotations; + std::optional transformation; void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; @@ -82,7 +87,7 @@ class ImgAnnotations : public Buffer, public ProtoSerializable { ProtoSerializable::SchemaPair serializeSchema() const override; #endif - DEPTHAI_SERIALIZE(ImgAnnotations, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, annotations); + DEPTHAI_SERIALIZE(ImgAnnotations, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, annotations, transformation); }; } // namespace dai diff --git a/protos/ImageAnnotations.proto b/protos/ImageAnnotations.proto index 409cc8a6e3..72404c7a33 100644 --- a/protos/ImageAnnotations.proto +++ b/protos/ImageAnnotations.proto @@ -9,6 +9,7 @@ message ImageAnnotations { common.Timestamp ts = 2; common.Timestamp tsDevice = 3; repeated ImageAnnotation annotations = 4; + common.ImgTransformation transformation = 5; } message ImageAnnotation { diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index 6fed5ff19d..c7ebafa643 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -197,6 +197,11 @@ std::unique_ptr getProtoMessage(const ImgAnnotations* tsDevice->set_sec(message->tsDevice.sec); tsDevice->set_nsec(message->tsDevice.nsec); + if(message->transformation.has_value()) { + proto::common::ImgTransformation* imgTransformation = imageAnnotations->mutable_transformation(); + utility::serializeImgTransformation(imgTransformation, message->transformation.value()); + } + for(const auto& annotation : message->annotations) { proto::image_annotations::ImageAnnotation* imageAnnotation = imageAnnotations->add_annotations(); for(const auto& circle : annotation.circles) { From 91eff55900c4cb9ed9354e66dc2aef63a008a833 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Sat, 24 Jan 2026 18:54:46 +0100 Subject: [PATCH 072/180] Update examples --- examples/cpp/Visualizer/visualizer_encoded.cpp | 3 ++- examples/python/Visualizer/visualizer_encoded.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/cpp/Visualizer/visualizer_encoded.cpp b/examples/cpp/Visualizer/visualizer_encoded.cpp index 5b85f43cd7..ddcc4a4201 100644 --- a/examples/cpp/Visualizer/visualizer_encoded.cpp +++ b/examples/cpp/Visualizer/visualizer_encoded.cpp @@ -36,6 +36,7 @@ class ImgAnnotationsGenerator : public dai::NodeCRTP(); imgAnnt->setTimestamp(nnData->getTimestamp()); + imgAnnt->transformation = nnData->transformation; dai::ImgAnnotation annotation; for(const auto& detection : detections) { @@ -141,4 +142,4 @@ int main(int argc, char** argv) { } return 0; -} \ No newline at end of file +} diff --git a/examples/python/Visualizer/visualizer_encoded.py b/examples/python/Visualizer/visualizer_encoded.py index f60b6c89ec..b0d43879d5 100644 --- a/examples/python/Visualizer/visualizer_encoded.py +++ b/examples/python/Visualizer/visualizer_encoded.py @@ -25,6 +25,7 @@ def run(self): detections = nnData.detections imgAnnt = dai.ImgAnnotations() imgAnnt.setTimestamp(nnData.getTimestamp()) + imgAnnt.setTransformation(nnData.getTransformation()) annotation = dai.ImgAnnotation() for detection in detections: pointsAnnotation = dai.PointsAnnotation() @@ -85,4 +86,3 @@ def run(self): if remoteConnector.waitKey(1) == ord("q"): pipeline.stop() break - From 47d88a11ba20902e51c34f1c4eed9c2e0dc7caae Mon Sep 17 00:00:00 2001 From: Adam Serafin Date: Tue, 27 Jan 2026 12:13:13 +0100 Subject: [PATCH 073/180] Vio and VSLAM updates (#1530) --- CMakeLists.txt | 33 +- bindings/python/CMakeLists.txt | 1 + bindings/python/src/DatatypeBindings.cpp | 3 + .../src/pipeline/datatype/MapDataBindings.cpp | 38 ++ .../src/pipeline/node/BasaltVIOBindings.cpp | 140 ++++--- bindings/python/src/pybind11_common.hpp | 2 +- cmake/depthaiConfig.cmake.in | 10 - cmake/depthaiDependencies.cmake | 26 +- examples/cpp/RVC2/VSLAM/CMakeLists.txt | 2 +- .../RVC2/VSLAM/basalt_vio_rtabmap_slam.cpp | 8 +- examples/cpp/RVC2/VSLAM/rerun_node.hpp | 10 +- examples/cpp/RVC2/VSLAM/rtabmap_vio.cpp | 4 +- examples/cpp/RVC2/VSLAM/rtabmap_vio_slam.cpp | 8 +- examples/python/RVC2/VSLAM/rerun_node.py | 2 +- include/depthai/basalt/BasaltVIO.hpp | 111 ++++- include/depthai/device/CalibrationHandler.hpp | 38 -- .../pipeline/datatype/DatatypeEnum.hpp | 1 + include/depthai/pipeline/datatype/MapData.hpp | 28 ++ .../pipeline/datatype/TransformData.hpp | 7 - include/depthai/pipeline/datatypes.hpp | 1 + include/depthai/rtabmap/RTABMapSLAM.hpp | 55 +-- include/depthai/rtabmap/RTABMapVIO.hpp | 24 +- src/basalt/BasaltVIO.cpp | 383 ++++++++++++------ src/pipeline/Node.cpp | 2 +- src/pipeline/datatype/DatatypeEnum.cpp | 3 + src/pipeline/datatype/MapData.cpp | 10 + src/pipeline/datatype/StreamMessageParser.cpp | 3 + src/pipeline/node/DetectionNetwork.cpp | 1 - src/pipeline/node/host/Replay.cpp | 2 + src/rtabmap/CalibrationHandler.cpp | 41 -- src/rtabmap/RTABMapConversions.cpp | 68 ++++ src/rtabmap/RTABMapConversions.hpp | 24 ++ src/rtabmap/RTABMapSLAM.cpp | 259 ++++++++---- src/rtabmap/RTABMapVIO.cpp | 102 +++-- src/rtabmap/TransformData.cpp | 23 -- src/utility/ProtoSerialize.cpp | 1 + tests/src/ondevice_tests/filesystem_test.cpp | 2 +- 37 files changed, 932 insertions(+), 544 deletions(-) create mode 100644 bindings/python/src/pipeline/datatype/MapDataBindings.cpp create mode 100644 include/depthai/pipeline/datatype/MapData.hpp create mode 100644 src/pipeline/datatype/MapData.cpp delete mode 100644 src/rtabmap/CalibrationHandler.cpp create mode 100644 src/rtabmap/RTABMapConversions.cpp create mode 100644 src/rtabmap/RTABMapConversions.hpp delete mode 100644 src/rtabmap/TransformData.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9780a73ff2..1da4264e8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -233,15 +233,11 @@ set(THIRDPARTY_RTABMAP_LIBRARIES "rtabmap::utilite" CACHE STRING "Optional libra set(TARGET_RTABMAP_NAME ${PROJECT_NAME}-rtabmap) set(TARGET_RTABMAP_ALIAS rtabmap) if(DEPTHAI_RTABMAP_SUPPORT) - if(${RTABMap_FOUND} AND DEPTHAI_HAVE_PCL_SUPPORT) + if(${RTABMap_FOUND}) set(DEPTHAI_HAVE_RTABMAP_SUPPORT ON) - message(STATUC "RTABMAP found. RTABMAP Support enabled") + message(STATUS "RTABMAP found. RTABMAP Support enabled") else() - if(${RTABMap_FOUND}) - message(WARNING "RTABMAP found but depthai does not have PCL support. RTABMAP Support disabled") - else() - message(WARNING "RTABMAP not found. RTABMAP Support disabled") - endif() + message(WARNING "RTABMAP not found. RTABMAP Support disabled") endif() endif() @@ -254,7 +250,7 @@ set(TARGET_BASALT_ALIAS basalt) if(DEPTHAI_BASALT_SUPPORT) if(${basalt_sdk_FOUND}) set(DEPTHAI_HAVE_BASALT_SUPPORT ON) - message(STATUC "Basalt found. Basalt Support enabled") + message(STATUS "Basalt found. Basalt Support enabled") else() message(WARNING "Basalt not found. Basalt Support disabled") endif() @@ -366,6 +362,7 @@ set(TARGET_CORE_SOURCES src/pipeline/datatype/PipelineState.cpp src/pipeline/datatype/PipelineEventAggregationConfig.cpp src/pipeline/datatype/RGBDData.cpp + src/pipeline/datatype/MapData.cpp src/pipeline/datatype/MessageGroup.cpp src/pipeline/datatype/ThermalConfig.cpp src/pipeline/datatype/TransformData.cpp @@ -452,9 +449,9 @@ set(TARGET_PCL_SOURCES src/pcl/PointCloudData.cpp) set(TARGET_BASALT_SOURCES src/basalt/BasaltVIO.cpp) set(TARGET_RTABMAP_SOURCES - src/rtabmap/CalibrationHandler.cpp - src/rtabmap/TransformData.cpp - src/rtabmap/RTABMapVIO.cpp src/rtabmap/RTABMapSLAM.cpp + src/rtabmap/RTABMapConversions.cpp + src/rtabmap/RTABMapVIO.cpp + src/rtabmap/RTABMapSLAM.cpp ) if(DEPTHAI_HAVE_OPENCV_SUPPORT AND DEPTHAI_MERGED_TARGET) @@ -599,7 +596,7 @@ endif() if(DEPTHAI_HAVE_RTABMAP_SUPPORT AND DEPTHAI_MERGED_TARGET) # Link to rtabmap - target_link_libraries(${TARGET_CORE_NAME} PUBLIC rtabmap::core ${THIRDPARTY_RTABMAP_LIBRARIES}) + target_link_libraries(${TARGET_CORE_NAME} PRIVATE rtabmap::core ${THIRDPARTY_RTABMAP_LIBRARIES}) # add compile defs target_compile_definitions(${TARGET_CORE_NAME} PUBLIC DEPTHAI_TARGET_RTABMAP) @@ -609,7 +606,7 @@ endif() if(DEPTHAI_HAVE_BASALT_SUPPORT AND DEPTHAI_MERGED_TARGET) # Link to Basalt - target_link_libraries(${TARGET_CORE_NAME} PUBLIC ${THIRDPARTY_BASALT_LIBRARIES}) + target_link_libraries(${TARGET_CORE_NAME} PRIVATE ${THIRDPARTY_BASALT_LIBRARIES}) # add compile defs target_compile_definitions(${TARGET_CORE_NAME} PUBLIC DEPTHAI_TARGET_BASALT) @@ -1043,9 +1040,9 @@ if(DEPTHAI_HAVE_RTABMAP_SUPPORT AND NOT DEPTHAI_MERGED_TARGET) endif() # Add depthai-rtabmap library and depthai::rtabmap alias add_library(${TARGET_RTABMAP_NAME} - src/rtabmap/CalibrationHandler.cpp - src/rtabmap/TransformData.cpp - src/rtabmap/RTABMapVIO.cpp src/rtabmap/RTABMapSLAM.cpp + src/rtabmap/RTABMapConversions.cpp + src/rtabmap/RTABMapVIO.cpp + src/rtabmap/RTABMapSLAM.cpp ) add_library("${PROJECT_NAME}::${TARGET_RTABMAP_ALIAS}" ALIAS ${TARGET_RTABMAP_NAME}) # Specifies name of generated IMPORTED target (set to alias) @@ -1055,7 +1052,7 @@ if(DEPTHAI_HAVE_RTABMAP_SUPPORT AND NOT DEPTHAI_MERGED_TARGET) add_default_flags(${TARGET_RTABMAP_NAME}) add_flag(${TARGET_RTABMAP_NAME} -Wno-switch-enum) # Link to RTABMap (publically) - target_link_libraries(${TARGET_RTABMAP_NAME} PUBLIC rtabmap::core ${TARGET_OPENCV_NAME} ${TARGET_PCL_NAME} ${THIRDPARTY_RTABMAP_LIBRARIES} PRIVATE spdlog::spdlog) + target_link_libraries(${TARGET_RTABMAP_NAME} PRIVATE rtabmap::core ${TARGET_OPENCV_NAME} ${TARGET_PCL_NAME} ${THIRDPARTY_RTABMAP_LIBRARIES} spdlog::spdlog) target_include_directories(${TARGET_RTABMAP_NAME} PRIVATE "$") # Add public compile definition indicating that RTABMap support is available @@ -1096,7 +1093,7 @@ if(DEPTHAI_HAVE_BASALT_SUPPORT AND NOT DEPTHAI_MERGED_TARGET) add_flag(${TARGET_BASALT_NAME} -Wno-switch-enum) # Link to Basalt (publically) - target_link_libraries(${TARGET_BASALT_NAME} PUBLIC ${THIRDPARTY_BASALT_LIBRARIES} ${TARGET_OPENCV_NAME} PRIVATE spdlog::spdlog) + target_link_libraries(${TARGET_BASALT_NAME} PRIVATE ${THIRDPARTY_BASALT_LIBRARIES} ${TARGET_OPENCV_NAME} spdlog::spdlog) # Add public compile definition indicating that Basalt support is available set(DEPTHAI_HAVE_BASALT_SUPPORT ON) diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index e46a50b3d0..d93ce960be 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -124,6 +124,7 @@ set(SOURCE_LIST src/pipeline/datatype/ImgFrameBindings.cpp src/pipeline/datatype/EncodedFrameBindings.cpp src/pipeline/datatype/IMUDataBindings.cpp + src/pipeline/datatype/MapDataBindings.cpp src/pipeline/datatype/MessageGroupBindings.cpp src/pipeline/datatype/NNDataBindings.cpp src/pipeline/datatype/NeuralDepthConfigBindings.cpp diff --git a/bindings/python/src/DatatypeBindings.cpp b/bindings/python/src/DatatypeBindings.cpp index 30a309e7cb..410f9b36db 100644 --- a/bindings/python/src/DatatypeBindings.cpp +++ b/bindings/python/src/DatatypeBindings.cpp @@ -37,6 +37,7 @@ void bind_transformdata(pybind11::module& m, void* pCallstack); void bind_rgbddata(pybind11::module& m, void* pCallstack); void bind_imagealignconfig(pybind11::module& m, void* pCallstack); void bind_imageannotations(pybind11::module& m, void* pCallstack); +void bind_mapdata(pybind11::module& m, void* pCallstack); #ifdef DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT void bind_dynamic_calibration_results(pybind11::module& m, void* pCallstack); void bind_dynamic_calibration_control(pybind11::module& m, void* pCallstack); @@ -82,6 +83,7 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_imagealignconfig); callstack.push_front(bind_imageannotations); callstack.push_front(bind_rgbddata); + callstack.push_front(bind_mapdata); callstack.push_front(bind_vppconfig); #ifdef DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT callstack.push_front(bind_dynamic_calibration_results); @@ -140,6 +142,7 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack) { .value("PointCloudData", DatatypeEnum::PointCloudData) .value("ImageAlignConfig", DatatypeEnum::ImageAlignConfig) .value("ImgAnnotations", DatatypeEnum::ImgAnnotations) + .value("MapData", DatatypeEnum::MapData) .value("RGBDData", DatatypeEnum::RGBDData) .value("PipelineEvent", DatatypeEnum::PipelineEvent) .value("PipelineState", DatatypeEnum::PipelineState) diff --git a/bindings/python/src/pipeline/datatype/MapDataBindings.cpp b/bindings/python/src/pipeline/datatype/MapDataBindings.cpp new file mode 100644 index 0000000000..b2e4015314 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/MapDataBindings.cpp @@ -0,0 +1,38 @@ +#include +#include + +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" + +// depthai +#include "depthai/pipeline/datatype/MapData.hpp" + +// pybind +#include +#include + +void bind_mapdata(pybind11::module& m, void* pCallstack) { + using namespace dai; + + py::class_, Buffer, std::shared_ptr> mapData(m, "MapData", DOC(dai, MapData)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + // Actual bindings + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + // Metadata / raw + mapData.def(py::init<>()) + .def("__repr__", &MapData::str) + .def_readwrite("map", &MapData::map) + .def_readwrite("minX", &MapData::minX) + .def_readwrite("minY", &MapData::minY); +} diff --git a/bindings/python/src/pipeline/node/BasaltVIOBindings.cpp b/bindings/python/src/pipeline/node/BasaltVIOBindings.cpp index 86f8e9e49d..4a4d7ea652 100644 --- a/bindings/python/src/pipeline/node/BasaltVIOBindings.cpp +++ b/bindings/python/src/pipeline/node/BasaltVIOBindings.cpp @@ -13,7 +13,7 @@ void bind_basaltnode(pybind11::module& m, void* pCallstack) { // declare upfront auto basaltNode = ADD_NODE_DERIVED(BasaltVIO, ThreadedHostNode); - py::class_ vioConfig(m, "VioConfig"); + py::class_ vioConfig(m, "VioConfig"); /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// @@ -36,73 +36,79 @@ void bind_basaltnode(pybind11::module& m, void* pCallstack) { .def_readonly("passthrough", &BasaltVIO::passthrough, DOC(dai, node, BasaltVIO, passthrough)) .def("setImuUpdateRate", &BasaltVIO::setImuUpdateRate, py::arg("rate"), DOC(dai, node, BasaltVIO, setImuUpdateRate)) .def("setConfigPath", &BasaltVIO::setConfigPath, py::arg("path"), DOC(dai, node, BasaltVIO, setConfigPath)) - .def_readwrite("vioConfig", &BasaltVIO::vioConfig) + .def("setConfig", &BasaltVIO::setConfig, py::arg("config"), DOC(dai, node, BasaltVIO, setConfig)) + .def("setAccelBias", &BasaltVIO::setAccelBias, py::arg("bias")) + .def("setGyroBias", &BasaltVIO::setGyroBias, py::arg("bias")) + .def("setAccelNoiseStd", &BasaltVIO::setAccelNoiseStd, py::arg("noise")) + .def("setGyroNoiseStd", &BasaltVIO::setGyroNoiseStd, py::arg("noise")) + .def("runSyncOnHost", &BasaltVIO::runSyncOnHost, py::arg("runOnHost"), DOC(dai, node, BasaltVIO, runSyncOnHost)) + .def("setImuExtrinsics", &BasaltVIO::setImuExtrinsics, py::arg("imuExtr"), DOC(dai, node, BasaltVIO, setImuExtrinsics)) .def("setLocalTransform", &BasaltVIO::setLocalTransform, py::arg("transform"), DOC(dai, node, BasaltVIO, setLocalTransform)); // VioConfig vioConfig.def(py::init<>()) - .def_readwrite("optical_flow_type", &basalt::VioConfig::optical_flow_type) - .def_readwrite("optical_flow_detection_grid_size", &basalt::VioConfig::optical_flow_detection_grid_size) - .def_readwrite("optical_flow_detection_num_points_cell", &basalt::VioConfig::optical_flow_detection_num_points_cell) - .def_readwrite("optical_flow_detection_min_threshold", &basalt::VioConfig::optical_flow_detection_min_threshold) - .def_readwrite("optical_flow_detection_max_threshold", &basalt::VioConfig::optical_flow_detection_max_threshold) - .def_readwrite("optical_flow_detection_nonoverlap", &basalt::VioConfig::optical_flow_detection_nonoverlap) - .def_readwrite("optical_flow_max_recovered_dist2", &basalt::VioConfig::optical_flow_max_recovered_dist2) - .def_readwrite("optical_flow_pattern", &basalt::VioConfig::optical_flow_pattern) - .def_readwrite("optical_flow_max_iterations", &basalt::VioConfig::optical_flow_max_iterations) - .def_readwrite("optical_flow_epipolar_error", &basalt::VioConfig::optical_flow_epipolar_error) - .def_readwrite("optical_flow_levels", &basalt::VioConfig::optical_flow_levels) - .def_readwrite("optical_flow_skip_frames", &basalt::VioConfig::optical_flow_skip_frames) - .def_readwrite("optical_flow_matching_guess_type", &basalt::VioConfig::optical_flow_matching_guess_type) - .def_readwrite("optical_flow_matching_default_depth", &basalt::VioConfig::optical_flow_matching_default_depth) - .def_readwrite("optical_flow_image_safe_radius", &basalt::VioConfig::optical_flow_image_safe_radius) - .def_readwrite("optical_flow_recall_enable", &basalt::VioConfig::optical_flow_recall_enable) - .def_readwrite("optical_flow_recall_all_cams", &basalt::VioConfig::optical_flow_recall_all_cams) - .def_readwrite("optical_flow_recall_num_points_cell", &basalt::VioConfig::optical_flow_recall_num_points_cell) - .def_readwrite("optical_flow_recall_over_tracking", &basalt::VioConfig::optical_flow_recall_over_tracking) - .def_readwrite("optical_flow_recall_update_patch_viewpoint", &basalt::VioConfig::optical_flow_recall_update_patch_viewpoint) - .def_readwrite("optical_flow_recall_max_patch_dist", &basalt::VioConfig::optical_flow_recall_max_patch_dist) - .def_readwrite("optical_flow_recall_max_patch_norms", &basalt::VioConfig::optical_flow_recall_max_patch_norms) - .def_readwrite("vio_linearization_type", &basalt::VioConfig::vio_linearization_type) - .def_readwrite("vio_sqrt_marg", &basalt::VioConfig::vio_sqrt_marg) - .def_readwrite("vio_max_states", &basalt::VioConfig::vio_max_states) - .def_readwrite("vio_max_kfs", &basalt::VioConfig::vio_max_kfs) - .def_readwrite("vio_min_frames_after_kf", &basalt::VioConfig::vio_min_frames_after_kf) - .def_readwrite("vio_new_kf_keypoints_thresh", &basalt::VioConfig::vio_new_kf_keypoints_thresh) - .def_readwrite("vio_debug", &basalt::VioConfig::vio_debug) - .def_readwrite("vio_extended_logging", &basalt::VioConfig::vio_extended_logging) - .def_readwrite("vio_obs_std_dev", &basalt::VioConfig::vio_obs_std_dev) - .def_readwrite("vio_obs_huber_thresh", &basalt::VioConfig::vio_obs_huber_thresh) - .def_readwrite("vio_min_triangulation_dist", &basalt::VioConfig::vio_min_triangulation_dist) - .def_readwrite("vio_max_iterations", &basalt::VioConfig::vio_max_iterations) - .def_readwrite("vio_enforce_realtime", &basalt::VioConfig::vio_enforce_realtime) - .def_readwrite("vio_use_lm", &basalt::VioConfig::vio_use_lm) - .def_readwrite("vio_lm_lambda_initial", &basalt::VioConfig::vio_lm_lambda_initial) - .def_readwrite("vio_lm_lambda_min", &basalt::VioConfig::vio_lm_lambda_min) - .def_readwrite("vio_lm_lambda_max", &basalt::VioConfig::vio_lm_lambda_max) - .def_readwrite("vio_scale_jacobian", &basalt::VioConfig::vio_scale_jacobian) - .def_readwrite("vio_init_pose_weight", &basalt::VioConfig::vio_init_pose_weight) - .def_readwrite("vio_init_ba_weight", &basalt::VioConfig::vio_init_ba_weight) - .def_readwrite("vio_init_bg_weight", &basalt::VioConfig::vio_init_bg_weight) - .def_readwrite("vio_marg_lost_landmarks", &basalt::VioConfig::vio_marg_lost_landmarks) - .def_readwrite("vio_fix_long_term_keyframes", &basalt::VioConfig::vio_fix_long_term_keyframes) - .def_readwrite("vio_kf_marg_feature_ratio", &basalt::VioConfig::vio_kf_marg_feature_ratio) - .def_readwrite("vio_kf_marg_criteria", &basalt::VioConfig::vio_kf_marg_criteria) - .def_readwrite("mapper_obs_std_dev", &basalt::VioConfig::mapper_obs_std_dev) - .def_readwrite("mapper_obs_huber_thresh", &basalt::VioConfig::mapper_obs_huber_thresh) - .def_readwrite("mapper_detection_num_points", &basalt::VioConfig::mapper_detection_num_points) - .def_readwrite("mapper_num_frames_to_match", &basalt::VioConfig::mapper_num_frames_to_match) - .def_readwrite("mapper_frames_to_match_threshold", &basalt::VioConfig::mapper_frames_to_match_threshold) - .def_readwrite("mapper_min_matches", &basalt::VioConfig::mapper_min_matches) - .def_readwrite("mapper_ransac_threshold", &basalt::VioConfig::mapper_ransac_threshold) - .def_readwrite("mapper_min_track_length", &basalt::VioConfig::mapper_min_track_length) - .def_readwrite("mapper_max_hamming_distance", &basalt::VioConfig::mapper_max_hamming_distance) - .def_readwrite("mapper_second_best_test_ratio", &basalt::VioConfig::mapper_second_best_test_ratio) - .def_readwrite("mapper_bow_num_bits", &basalt::VioConfig::mapper_bow_num_bits) - .def_readwrite("mapper_min_triangulation_dist", &basalt::VioConfig::mapper_min_triangulation_dist) - .def_readwrite("mapper_no_factor_weights", &basalt::VioConfig::mapper_no_factor_weights) - .def_readwrite("mapper_use_factors", &basalt::VioConfig::mapper_use_factors) - .def_readwrite("mapper_use_lm", &basalt::VioConfig::mapper_use_lm) - .def_readwrite("mapper_lm_lambda_min", &basalt::VioConfig::mapper_lm_lambda_min) - .def_readwrite("mapper_lm_lambda_max", &basalt::VioConfig::mapper_lm_lambda_max); + .def_readwrite("optical_flow_type", &BasaltVIO::VioConfig::optical_flow_type) + .def_readwrite("optical_flow_detection_grid_size", &BasaltVIO::VioConfig::optical_flow_detection_grid_size) + .def_readwrite("optical_flow_detection_num_points_cell", &BasaltVIO::VioConfig::optical_flow_detection_num_points_cell) + .def_readwrite("optical_flow_detection_min_threshold", &BasaltVIO::VioConfig::optical_flow_detection_min_threshold) + .def_readwrite("optical_flow_detection_max_threshold", &BasaltVIO::VioConfig::optical_flow_detection_max_threshold) + .def_readwrite("optical_flow_detection_nonoverlap", &BasaltVIO::VioConfig::optical_flow_detection_nonoverlap) + .def_readwrite("optical_flow_max_recovered_dist2", &BasaltVIO::VioConfig::optical_flow_max_recovered_dist2) + .def_readwrite("optical_flow_pattern", &BasaltVIO::VioConfig::optical_flow_pattern) + .def_readwrite("optical_flow_max_iterations", &BasaltVIO::VioConfig::optical_flow_max_iterations) + .def_readwrite("optical_flow_epipolar_error", &BasaltVIO::VioConfig::optical_flow_epipolar_error) + .def_readwrite("optical_flow_levels", &BasaltVIO::VioConfig::optical_flow_levels) + .def_readwrite("optical_flow_skip_frames", &BasaltVIO::VioConfig::optical_flow_skip_frames) + .def_readwrite("optical_flow_matching_guess_type", &BasaltVIO::VioConfig::optical_flow_matching_guess_type) + .def_readwrite("optical_flow_matching_default_depth", &BasaltVIO::VioConfig::optical_flow_matching_default_depth) + .def_readwrite("optical_flow_image_safe_radius", &BasaltVIO::VioConfig::optical_flow_image_safe_radius) + .def_readwrite("optical_flow_recall_enable", &BasaltVIO::VioConfig::optical_flow_recall_enable) + .def_readwrite("optical_flow_recall_all_cams", &BasaltVIO::VioConfig::optical_flow_recall_all_cams) + .def_readwrite("optical_flow_recall_num_points_cell", &BasaltVIO::VioConfig::optical_flow_recall_num_points_cell) + .def_readwrite("optical_flow_recall_over_tracking", &BasaltVIO::VioConfig::optical_flow_recall_over_tracking) + .def_readwrite("optical_flow_recall_update_patch_viewpoint", &BasaltVIO::VioConfig::optical_flow_recall_update_patch_viewpoint) + .def_readwrite("optical_flow_recall_max_patch_dist", &BasaltVIO::VioConfig::optical_flow_recall_max_patch_dist) + .def_readwrite("optical_flow_recall_max_patch_norms", &BasaltVIO::VioConfig::optical_flow_recall_max_patch_norms) + .def_readwrite("vio_linearization_type", &BasaltVIO::VioConfig::vio_linearization_type) + .def_readwrite("vio_sqrt_marg", &BasaltVIO::VioConfig::vio_sqrt_marg) + .def_readwrite("vio_max_states", &BasaltVIO::VioConfig::vio_max_states) + .def_readwrite("vio_max_kfs", &BasaltVIO::VioConfig::vio_max_kfs) + .def_readwrite("vio_min_frames_after_kf", &BasaltVIO::VioConfig::vio_min_frames_after_kf) + .def_readwrite("vio_new_kf_keypoints_thresh", &BasaltVIO::VioConfig::vio_new_kf_keypoints_thresh) + .def_readwrite("vio_debug", &BasaltVIO::VioConfig::vio_debug) + .def_readwrite("vio_extended_logging", &BasaltVIO::VioConfig::vio_extended_logging) + .def_readwrite("vio_obs_std_dev", &BasaltVIO::VioConfig::vio_obs_std_dev) + .def_readwrite("vio_obs_huber_thresh", &BasaltVIO::VioConfig::vio_obs_huber_thresh) + .def_readwrite("vio_min_triangulation_dist", &BasaltVIO::VioConfig::vio_min_triangulation_dist) + .def_readwrite("vio_max_iterations", &BasaltVIO::VioConfig::vio_max_iterations) + .def_readwrite("vio_enforce_realtime", &BasaltVIO::VioConfig::vio_enforce_realtime) + .def_readwrite("vio_use_lm", &BasaltVIO::VioConfig::vio_use_lm) + .def_readwrite("vio_lm_lambda_initial", &BasaltVIO::VioConfig::vio_lm_lambda_initial) + .def_readwrite("vio_lm_lambda_min", &BasaltVIO::VioConfig::vio_lm_lambda_min) + .def_readwrite("vio_lm_lambda_max", &BasaltVIO::VioConfig::vio_lm_lambda_max) + .def_readwrite("vio_scale_jacobian", &BasaltVIO::VioConfig::vio_scale_jacobian) + .def_readwrite("vio_init_pose_weight", &BasaltVIO::VioConfig::vio_init_pose_weight) + .def_readwrite("vio_init_ba_weight", &BasaltVIO::VioConfig::vio_init_ba_weight) + .def_readwrite("vio_init_bg_weight", &BasaltVIO::VioConfig::vio_init_bg_weight) + .def_readwrite("vio_marg_lost_landmarks", &BasaltVIO::VioConfig::vio_marg_lost_landmarks) + .def_readwrite("vio_fix_long_term_keyframes", &BasaltVIO::VioConfig::vio_fix_long_term_keyframes) + .def_readwrite("vio_kf_marg_feature_ratio", &BasaltVIO::VioConfig::vio_kf_marg_feature_ratio) + .def_readwrite("vio_kf_marg_criteria", &BasaltVIO::VioConfig::vio_kf_marg_criteria) + .def_readwrite("mapper_obs_std_dev", &BasaltVIO::VioConfig::mapper_obs_std_dev) + .def_readwrite("mapper_obs_huber_thresh", &BasaltVIO::VioConfig::mapper_obs_huber_thresh) + .def_readwrite("mapper_detection_num_points", &BasaltVIO::VioConfig::mapper_detection_num_points) + .def_readwrite("mapper_num_frames_to_match", &BasaltVIO::VioConfig::mapper_num_frames_to_match) + .def_readwrite("mapper_frames_to_match_threshold", &BasaltVIO::VioConfig::mapper_frames_to_match_threshold) + .def_readwrite("mapper_min_matches", &BasaltVIO::VioConfig::mapper_min_matches) + .def_readwrite("mapper_ransac_threshold", &BasaltVIO::VioConfig::mapper_ransac_threshold) + .def_readwrite("mapper_min_track_length", &BasaltVIO::VioConfig::mapper_min_track_length) + .def_readwrite("mapper_max_hamming_distance", &BasaltVIO::VioConfig::mapper_max_hamming_distance) + .def_readwrite("mapper_second_best_test_ratio", &BasaltVIO::VioConfig::mapper_second_best_test_ratio) + .def_readwrite("mapper_bow_num_bits", &BasaltVIO::VioConfig::mapper_bow_num_bits) + .def_readwrite("mapper_min_triangulation_dist", &BasaltVIO::VioConfig::mapper_min_triangulation_dist) + .def_readwrite("mapper_no_factor_weights", &BasaltVIO::VioConfig::mapper_no_factor_weights) + .def_readwrite("mapper_use_factors", &BasaltVIO::VioConfig::mapper_use_factors) + .def_readwrite("mapper_use_lm", &BasaltVIO::VioConfig::mapper_use_lm) + .def_readwrite("mapper_lm_lambda_min", &BasaltVIO::VioConfig::mapper_lm_lambda_min) + .def_readwrite("mapper_lm_lambda_max", &BasaltVIO::VioConfig::mapper_lm_lambda_max); /////////////////////////////////////////////////////////////////////// -} \ No newline at end of file +} diff --git a/bindings/python/src/pybind11_common.hpp b/bindings/python/src/pybind11_common.hpp index ed09b4879c..e0f3db10f4 100644 --- a/bindings/python/src/pybind11_common.hpp +++ b/bindings/python/src/pybind11_common.hpp @@ -1,6 +1,6 @@ #pragma once -#if(_MSC_VER >= 1910) || !defined(_MSC_VER) +#if (_MSC_VER >= 1910) || !defined(_MSC_VER) #ifndef HAVE_SNPRINTF #define HAVE_SNPRINTF #endif diff --git a/cmake/depthaiConfig.cmake.in b/cmake/depthaiConfig.cmake.in index edce71fe93..96ecd1fd01 100644 --- a/cmake/depthaiConfig.cmake.in +++ b/cmake/depthaiConfig.cmake.in @@ -5,8 +5,6 @@ find_dependency(libnop CONFIG REQUIRED) find_dependency(XLink CONFIG REQUIRED COMPONENTS XLinkPublic) set(DEPTHAI_OPENCV_SUPPORT @DEPTHAI_OPENCV_SUPPORT@) -set(DEPTHAI_BASALT_SUPPORT @DEPTHAI_BASALT_SUPPORT@) -set(DEPTHAI_RTABMAP_SUPPORT @DEPTHAI_RTABMAP_SUPPORT@) set(DEPTHAI_PCL_SUPPORT @DEPTHAI_PCL_SUPPORT@) set(DEPTHAI_XTENSOR_SUPPORT @DEPTHAI_XTENSOR_SUPPORT@) set(DEPTHAI_DYNAMIC_CALIBRATION_SUPPORT @DEPTHAI_DYNAMIC_CALIBRATION_SUPPORT@) @@ -15,14 +13,6 @@ if(DEPTHAI_OPENCV_SUPPORT) find_dependency(OpenCV 4 CONFIG REQUIRED) endif() -if(DEPTHAI_RTABMAP_SUPPORT) - find_dependency(rtabmap CONFIG REQUIRED) -endif() - -if(DEPTHAI_BASALT_SUPPORT) - find_dependency(Basalt CONFIG REQUIRED) -endif() - if(DEPTHAI_PCL_SUPPORT) find_dependency(PCL CONFIG REQUIRED) endif() diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index f608293730..f250f1c053 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -63,6 +63,16 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT DEPTHAI_SHARED_LIBS)) find_package(CURL ${_QUIET} CONFIG REQUIRED) find_package(cpr ${_QUIET} CONFIG REQUIRED) endif() + if(DEPTHAI_BASALT_SUPPORT) + find_package(basalt-headers ${_QUIET} CONFIG REQUIRED) + find_package(basalt_sdk ${_QUIET} CONFIG REQUIRED) + endif() + if(DEPTHAI_RTABMAP_SUPPORT) + find_package(g2o ${_QUIET} CONFIG REQUIRED) + find_package(Ceres ${_QUIET} CONFIG REQUIRED) + find_package(PCL CONFIG COMPONENTS common) + find_package(RTABMap ${_QUIET} CONFIG REQUIRED COMPONENTS core utilite) + endif() # Backward if(DEPTHAI_ENABLE_BACKWARD) @@ -202,16 +212,6 @@ endif() if(DEPTHAI_PCL_SUPPORT) find_package(PCL CONFIG COMPONENTS common) endif() -if(DEPTHAI_RTABMAP_SUPPORT) - find_package(RTABMap ${_QUIET} CONFIG REQUIRED COMPONENTS core utilite) - find_package(g2o ${_QUIET} CONFIG REQUIRED) - find_package(Ceres ${_QUIET} CONFIG REQUIRED) -endif() - -if(DEPTHAI_BASALT_SUPPORT) - find_package(basalt-headers ${_QUIET} CONFIG REQUIRED) - find_package(basalt_sdk ${_QUIET} CONFIG REQUIRED) -endif() # include optional dependency cmake if(DEPTHAI_DEPENDENCY_INCLUDE) @@ -289,6 +289,8 @@ if(DEPTHAI_DYNAMIC_CALIBRATION_SUPPORT) set(DYNAMIC_CALIBRATION_LIB $<$:${DYNAMIC_CALIBRATION_DEBUG_LIB}> $<$:${DYNAMIC_CALIBRATION_RELEASE_LIB}> + $<$::${DYNAMIC_CALIBRATION_RELEASE_LIB}> + $<$:${DYNAMIC_CALIBRATION_RELEASE_LIB}> ) else() set(DYNAMIC_CALIBRATION_LIB ${DYNAMIC_CALIBRATION_RELEASE}) @@ -310,7 +312,9 @@ if(DEPTHAI_DYNAMIC_CALIBRATION_SUPPORT) set_target_properties(dynamic_calibration_imported PROPERTIES IMPORTED_LOCATION_DEBUG "${DYNAMIC_CALIBRATION_RELEASE}" IMPORTED_LOCATION_RELEASE "${DYNAMIC_CALIBRATION_RELEASE}" - IMPORTED_CONFIGURATIONS "DEBUG;RELEASE" + IMPORTED_LOCATION_RELWITHDEBINFO "${DYNAMIC_CALIBRATION_RELEASE}" + IMPORTED_LOCATION_MINSIZEREL "${DYNAMIC_CALIBRATION_RELEASE}" + IMPORTED_CONFIGURATIONS "DEBUG;RELEASE;RELWITHDEBINFO;MINSIZEREL" INTERFACE_INCLUDE_DIRECTORIES "${DYNAMIC_CALIBRATION_DIR}/include" ) endif() diff --git a/examples/cpp/RVC2/VSLAM/CMakeLists.txt b/examples/cpp/RVC2/VSLAM/CMakeLists.txt index b3f5b903e1..14820deae5 100644 --- a/examples/cpp/RVC2/VSLAM/CMakeLists.txt +++ b/examples/cpp/RVC2/VSLAM/CMakeLists.txt @@ -40,4 +40,4 @@ if(DEPTHAI_BASALT_SUPPORT) else() target_link_libraries(basalt_vio PRIVATE depthai::basalt rerun_sdk) endif() -endif() \ No newline at end of file +endif() diff --git a/examples/cpp/RVC2/VSLAM/basalt_vio_rtabmap_slam.cpp b/examples/cpp/RVC2/VSLAM/basalt_vio_rtabmap_slam.cpp index d377395a03..94d5c5bb62 100644 --- a/examples/cpp/RVC2/VSLAM/basalt_vio_rtabmap_slam.cpp +++ b/examples/cpp/RVC2/VSLAM/basalt_vio_rtabmap_slam.cpp @@ -17,10 +17,10 @@ int main() { auto imu = pipeline.create(); auto odom = pipeline.create(); auto slam = pipeline.create(); - auto params = rtabmap::ParametersMap(); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGrid3D(), "true")); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapSaveWMState(), "true")); + std::map params{}; + params.insert(std::make_pair("RGBD/CreateOccupancyGrid", "true")); + params.insert(std::make_pair("Grid/3D", "true")); + params.insert(std::make_pair("Rtabmap/SaveWMState", "true")); slam->setParams(params); auto rerun = pipeline.create(); diff --git a/examples/cpp/RVC2/VSLAM/rerun_node.hpp b/examples/cpp/RVC2/VSLAM/rerun_node.hpp index 0c7fd1a41e..98eeab5a08 100644 --- a/examples/cpp/RVC2/VSLAM/rerun_node.hpp +++ b/examples/cpp/RVC2/VSLAM/rerun_node.hpp @@ -3,6 +3,7 @@ #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/datatype/MapData.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/datatype/TransformData.hpp" #include "depthai/common/CameraBoardSocket.hpp" @@ -24,7 +25,7 @@ class RerunNode : public dai::NodeCRTP { Input inputImg{*this, {.name = "inImg", .types = {{dai::DatatypeEnum::ImgFrame, true}}}}; Input inputObstaclePCL{*this, {.name = "inObstaclePCL", .types = {{dai::DatatypeEnum::PointCloudData, true}}}}; Input inputGroundPCL{*this, {.name = "inGroundPCL", .types = {{dai::DatatypeEnum::PointCloudData, true}}}}; - Input inputMap{*this, {.name = "inMap", .types = {{dai::DatatypeEnum::ImgFrame, true}}}}; + Input inputMap{*this, {.name = "inMap", .types = {{dai::DatatypeEnum::MapData, true}}}}; void getFocalLengthFromImage(std::shared_ptr imgFrame) { auto p = getParentPipeline(); @@ -49,7 +50,7 @@ class RerunNode : public dai::NodeCRTP { } auto pclObstData = inputObstaclePCL.tryGet(); auto pclGrndData = inputGroundPCL.tryGet(); - auto mapData = inputMap.tryGet(); + auto mapData = inputMap.tryGet(); if(transData != nullptr) { auto trans = transData->getTranslation(); auto quat = transData->getQuaternion(); @@ -96,9 +97,10 @@ class RerunNode : public dai::NodeCRTP { } #endif if(mapData != nullptr) { - auto image = mapData->getCvFrame(); + auto &map = mapData->map; + auto image = map.getCvFrame(); rec.log("map", rerun::Image(reinterpret_cast(image.data), - {static_cast(mapData->getWidth()), static_cast(mapData->getHeight())}, rerun::datatypes::ColorModel::L)); + {static_cast(map.getWidth()), static_cast(map.getHeight())}, rerun::datatypes::ColorModel::L)); } } } diff --git a/examples/cpp/RVC2/VSLAM/rtabmap_vio.cpp b/examples/cpp/RVC2/VSLAM/rtabmap_vio.cpp index 1413b6b813..8a932b5f67 100644 --- a/examples/cpp/RVC2/VSLAM/rtabmap_vio.cpp +++ b/examples/cpp/RVC2/VSLAM/rtabmap_vio.cpp @@ -17,8 +17,8 @@ int main() { auto featureTracker = pipeline.create(); auto odom = pipeline.create(); auto rerun = pipeline.create(); - auto params = rtabmap::ParametersMap(); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), "30")); + std::map params{}; + params.insert(std::make_pair("Odom/ResetCountDown", "30")); odom->setParams(params); imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100); diff --git a/examples/cpp/RVC2/VSLAM/rtabmap_vio_slam.cpp b/examples/cpp/RVC2/VSLAM/rtabmap_vio_slam.cpp index 46bc2f6d30..0d6e9f6391 100644 --- a/examples/cpp/RVC2/VSLAM/rtabmap_vio_slam.cpp +++ b/examples/cpp/RVC2/VSLAM/rtabmap_vio_slam.cpp @@ -18,13 +18,13 @@ int main() { auto odom = pipeline.create(); auto slam = pipeline.create(); auto rerun = pipeline.create(); - auto params = rtabmap::ParametersMap(); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), "30")); + std::map params{}; + params.insert(std::make_pair("Odom/ResetCountDown", "30")); odom->setParams(params); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); - params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGrid3D(), "true")); + params.insert(std::make_pair("RGBD/CreateOccupancyGrid", "true")); + params.insert(std::make_pair("Grid/3D", "true")); slam->setParams(params); imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100); imu->setBatchReportThreshold(1); diff --git a/examples/python/RVC2/VSLAM/rerun_node.py b/examples/python/RVC2/VSLAM/rerun_node.py index caefda601a..7b71896170 100644 --- a/examples/python/RVC2/VSLAM/rerun_node.py +++ b/examples/python/RVC2/VSLAM/rerun_node.py @@ -63,4 +63,4 @@ def run(self): points, colors = pclGrndData.getPointsRGB() rr.log("world/ground_pcl", rr.Points3D(points, colors=colors, radii=[0.01])) if mapData is not None: - rr.log("map", rr.Image(mapData.getCvFrame())) + rr.log("map", rr.Image(mapData.map.getCvFrame())) diff --git a/include/depthai/basalt/BasaltVIO.hpp b/include/depthai/basalt/BasaltVIO.hpp index 05bd62198b..d4c512f213 100644 --- a/include/depthai/basalt/BasaltVIO.hpp +++ b/include/depthai/basalt/BasaltVIO.hpp @@ -1,11 +1,6 @@ #pragma once #define SOPHUS_USE_BASIC_LOGGING -#include "basalt/calibration/calibration.hpp" -#include "basalt/serialization/headers_serialization.h" -#include "basalt/spline/se3_spline.h" -#include "basalt/utils/vio_config.h" -#include "basalt/vi_estimator/vio_estimator.h" #include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" @@ -22,6 +17,87 @@ namespace node { */ class BasaltVIO : public NodeCRTP { public: + enum class LinearizationType { ABS_QR, ABS_SC, REL_SC }; + enum class MatchingGuessType { SAME_PIXEL, REPROJ_FIX_DEPTH, REPROJ_AVG_DEPTH }; + enum class KeyframeMargCriteria { KF_MARG_DEFAULT, KF_MARG_FORWARD_VECTOR }; + + struct VioConfig { + std::string optical_flow_type; + int optical_flow_detection_grid_size; + int optical_flow_detection_num_points_cell; + int optical_flow_detection_min_threshold; + int optical_flow_detection_max_threshold; + bool optical_flow_detection_nonoverlap; + float optical_flow_max_recovered_dist2; + int optical_flow_pattern; + int optical_flow_max_iterations; + int optical_flow_levels; + float optical_flow_epipolar_error; + int optical_flow_skip_frames; + MatchingGuessType optical_flow_matching_guess_type; + float optical_flow_matching_default_depth; + float optical_flow_image_safe_radius; // Use to mask black corners in cameras + bool optical_flow_recall_enable; // Enable feature/landmark recall + bool optical_flow_recall_all_cams; // Recall in all cameras, not just cam0 + bool optical_flow_recall_num_points_cell; // Respect gridcell feature limit + bool optical_flow_recall_over_tracking; // Always perform recall, even on already tracked features + bool optical_flow_recall_update_patch_viewpoint; // Update feature patch when succesfully recalled + float optical_flow_recall_max_patch_dist; // Maximum distance in % of width to accept a recall, or 0 + std::vector optical_flow_recall_max_patch_norms; // Maximum patch residual norm to accept a recall + + LinearizationType vio_linearization_type; + bool vio_sqrt_marg; + + int vio_max_states; + int vio_max_kfs; + int vio_min_frames_after_kf; + float vio_new_kf_keypoints_thresh; + bool vio_debug; + bool vio_extended_logging; + + int vio_max_iterations; + + double vio_obs_std_dev; + double vio_obs_huber_thresh; + double vio_min_triangulation_dist; + + bool vio_enforce_realtime; + + bool vio_use_lm; + double vio_lm_lambda_initial; + double vio_lm_lambda_min; + double vio_lm_lambda_max; + + bool vio_scale_jacobian; + + double vio_init_pose_weight; + double vio_init_ba_weight; + double vio_init_bg_weight; + + bool vio_marg_lost_landmarks; + bool vio_fix_long_term_keyframes; + double vio_kf_marg_feature_ratio; + KeyframeMargCriteria vio_kf_marg_criteria; + + double mapper_obs_std_dev; + double mapper_obs_huber_thresh; + int mapper_detection_num_points; + double mapper_num_frames_to_match; + double mapper_frames_to_match_threshold; + double mapper_min_matches; + double mapper_ransac_threshold; + double mapper_min_track_length; + double mapper_max_hamming_distance; + double mapper_second_best_test_ratio; + int mapper_bow_num_bits; + double mapper_min_triangulation_dist; + bool mapper_no_factor_weights; + bool mapper_use_factors; + + bool mapper_use_lm; + double mapper_lm_lambda_min; + double mapper_lm_lambda_max; + }; constexpr static const char* NAME = "BasaltVIO"; BasaltVIO(); ~BasaltVIO(); @@ -55,21 +131,24 @@ class BasaltVIO : public NodeCRTP { */ Output passthrough{*this, {"imgPassthrough", DEFAULT_GROUP, {{DatatypeEnum::ImgFrame, true}}}}; - /** - * VIO configuration file. - */ - basalt::VioConfig vioConfig; void setImuUpdateRate(int rate) { imuUpdateRate = rate; } void setConfigPath(const std::string& path) { configPath = path; } + void setConfig(const VioConfig& config); void setUseSpecTranslation(bool use) { useSpecTranslation = use; } void setLocalTransform(const std::shared_ptr& transform); + void setImuExtrinsics(const std::shared_ptr& imuExtr); + void setAccelBias(const std::vector& accelBias); + void setAccelNoiseStd(const std::vector& accelNoiseStd); + void setGyroBias(const std::vector& gyroBias); + void setGyroNoiseStd(const std::vector& gyroNoiseStd); void setDefaultVIOConfig(); + void runSyncOnHost(bool runOnHost); private: // pimpl @@ -82,20 +161,18 @@ class BasaltVIO : public NodeCRTP { void imuCB(std::shared_ptr imuData); void stop() override; Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; - std::shared_ptr> calib; - - basalt::OpticalFlowBase::Ptr optFlowPtr; - basalt::VioEstimatorBase::Ptr vio; - basalt::OpticalFlowInput::Ptr lastImgData; - - std::vector vioTNSec; - std::shared_ptr::SE3> localTransform; std::shared_ptr leftImg; + std::mutex imgMtx; bool initialized = false; std::string configPath = ""; int imuUpdateRate = 200; int threadNum = 1; bool useSpecTranslation = true; + std::optional> imuExtrinsics; + std::optional> accelBias; + std::optional> accelNoiseStd; + std::optional> gyroBias; + std::optional> gyroNoiseStd; }; } // namespace node } // namespace dai diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index 0ef940517c..52a58aebcd 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -12,13 +12,6 @@ #include "depthai/common/Point2f.hpp" #include "depthai/common/Size2f.hpp" -#ifdef DEPTHAI_HAVE_RTABMAP_SUPPORT - #pragma push_macro("_res") - #undef _res - #define _res resfixed_ - #include "rtabmap/core/StereoCameraModel.h" - #pragma pop_macro("_res") -#endif namespace dai { /** * CalibrationHandler is an interface to read/load/write structured calibration and device data. @@ -631,37 +624,6 @@ class CalibrationHandler { */ void validateCalibrationHandler(bool throwOnError = true) const; -// Optional - RTABMap support -#ifdef DEPTHAI_HAVE_RTABMAP_SUPPORT - /** - * @note This API only available if RTABMap support is enabled - * - * Provide the rtabmap::StereoCameraModel object - * - * @param model StereoCameraModel object to be filled with the intrinsics and extrinsics of the camera - * @param socketID CameraBoardSocket of the camera - * @param width Width of the image for which intrinsics is requested - * @param height Height of the image for which intrinsics is requested - */ - rtabmap::StereoCameraModel getRTABMapCameraModel(CameraBoardSocket cameraId, - int width, - int height, - const rtabmap::Transform& localTransform = rtabmap::Transform::getIdentity(), - float alphaScaling = -1.0, - dai::CameraBoardSocket left = CameraBoardSocket::CAM_B, - dai::CameraBoardSocket right = CameraBoardSocket::CAM_C); - -#else - template - struct dependent_false { - static constexpr bool value = false; - }; - template - void getRTABMapCameraModel(T...) { - static_assert(dependent_false::value, "Library not configured with RTABMap support"); - } -#endif - private: /** when the user is writing extrinsics do we validate if * the connection between all the cameras exists ? diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index 0c9dcdb05a..05f8d8667c 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -31,6 +31,7 @@ enum class DatatypeEnum : std::int32_t { TrackedFeatures, BenchmarkReport, MessageGroup, + MapData, TransformData, PointCloudConfig, PointCloudData, diff --git a/include/depthai/pipeline/datatype/MapData.hpp b/include/depthai/pipeline/datatype/MapData.hpp new file mode 100644 index 0000000000..a6f561a30e --- /dev/null +++ b/include/depthai/pipeline/datatype/MapData.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include "depthai/common/ADatatypeSharedPtrSerialization.hpp" +#include "depthai/pipeline/datatype/ADatatype.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/utility/Serialization.hpp" + +namespace dai { + +/** + * MapData message. Carries grid map data and minX/minY messages to help place it in 3D space. + */ +class MapData : public Buffer { + public: + MapData() = default; + + virtual ~MapData(); + + dai::ImgFrame map; + float minX = 0.0; + float minY = 0.0; + + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; + DEPTHAI_SERIALIZE(MapData, Buffer::ts, Buffer::tsDevice, Buffer::sequenceNum, map, minX, minY); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatype/TransformData.hpp b/include/depthai/pipeline/datatype/TransformData.hpp index f2219499b2..fbb2293b27 100644 --- a/include/depthai/pipeline/datatype/TransformData.hpp +++ b/include/depthai/pipeline/datatype/TransformData.hpp @@ -3,9 +3,6 @@ #include "depthai/common/Quaterniond.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" -#ifdef DEPTHAI_HAVE_RTABMAP_SUPPORT - #include "rtabmap/core/Transform.h" -#endif namespace dai { struct Transform { std::array, 4> matrix; @@ -27,10 +24,6 @@ class TransformData : public Buffer { TransformData(double x, double y, double z, double qx, double qy, double qz, double qw); TransformData(double x, double y, double z, double roll, double pitch, double yaw); -#ifdef DEPTHAI_HAVE_RTABMAP_SUPPORT - TransformData(const rtabmap::Transform& transformRTABMap); - rtabmap::Transform getRTABMapTransform() const; -#endif virtual ~TransformData(); /// Transform diff --git a/include/depthai/pipeline/datatypes.hpp b/include/depthai/pipeline/datatypes.hpp index fb4a67d1e8..e9a51052ad 100644 --- a/include/depthai/pipeline/datatypes.hpp +++ b/include/depthai/pipeline/datatypes.hpp @@ -16,6 +16,7 @@ #include "datatype/ImgDetections.hpp" #include "datatype/ImgDetectionsT.hpp" #include "datatype/ImgFrame.hpp" +#include "datatype/MapData.hpp" #include "datatype/MessageGroup.hpp" #include "datatype/NNData.hpp" #include "datatype/NeuralDepthConfig.hpp" diff --git a/include/depthai/rtabmap/RTABMapSLAM.hpp b/include/depthai/rtabmap/RTABMapSLAM.hpp index 50319ca025..363d6c10b4 100644 --- a/include/depthai/rtabmap/RTABMapSLAM.hpp +++ b/include/depthai/rtabmap/RTABMapSLAM.hpp @@ -1,24 +1,18 @@ #pragma once -#include - #include "depthai/pipeline/DeviceNode.hpp" +#include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/datatype/MapData.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/datatype/TrackedFeatures.hpp" #include "depthai/pipeline/datatype/TransformData.hpp" #include "depthai/pipeline/node/Sync.hpp" -#include "rtabmap/core/CameraModel.h" -#include "rtabmap/core/LocalGrid.h" -#include "rtabmap/core/Rtabmap.h" -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/Transform.h" -#include "rtabmap/core/global_map/CloudMap.h" -#include "rtabmap/core/global_map/OccupancyGrid.h" +#include "depthai/utility/Pimpl.hpp" namespace dai { namespace node { @@ -31,6 +25,7 @@ class RTABMapSLAM : public dai::NodeCRTP sync{*this, "sync"}; InputMap& inputs = sync->inputs; @@ -74,7 +69,7 @@ class RTABMapSLAM : public dai::NodeCRTP& params); @@ -135,21 +133,15 @@ class RTABMapSLAM : public dai::NodeCRTP transform) { - localTransform = transform->getRTABMapTransform(); - } - std::shared_ptr getLocalTransform() { - return std::make_shared(localTransform); - } + void setLocalTransform(std::shared_ptr transform); + std::shared_ptr getLocalTransform(); /** * Trigger a new map. */ @@ -180,26 +168,15 @@ class RTABMapSLAM : public dai::NodeCRTP pimplRtabmap; void run() override; Input inSync{*this, {"inSync", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{dai::DatatypeEnum::MessageGroup, true}}}}}; void syncCB(std::shared_ptr data); void odomPoseCB(std::shared_ptr data); void imuCB(std::shared_ptr msg); void initialize(dai::Pipeline& pipeline, int instanceNum, int width, int height); - void publishGridMap(const std::map& optimizedPoses); - void publishPointClouds(const std::map& optimizedPoses); - - rtabmap::StereoCameraModel model; - rtabmap::Rtabmap rtabmap; - rtabmap::Transform currPose, odomCorr; - std::chrono::steady_clock::time_point lastProcessTime; - std::chrono::steady_clock::time_point startTime; - rtabmap::Transform imuLocalTransform; - rtabmap::Transform localTransform; - std::shared_ptr localMaps; - std::unique_ptr occupancyGrid; - std::unique_ptr cloudMap; - rtabmap::SensorData sensorData; float alphaScaling = -1.0; bool useFeatures = false; bool initialized = false; diff --git a/include/depthai/rtabmap/RTABMapVIO.hpp b/include/depthai/rtabmap/RTABMapVIO.hpp index b2f1758d2a..c7be3c8175 100644 --- a/include/depthai/rtabmap/RTABMapVIO.hpp +++ b/include/depthai/rtabmap/RTABMapVIO.hpp @@ -1,8 +1,7 @@ #pragma once -#include - #include "depthai/pipeline/DeviceNode.hpp" +#include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" @@ -12,11 +11,6 @@ #include "depthai/pipeline/datatype/TrackedFeatures.hpp" #include "depthai/pipeline/datatype/TransformData.hpp" #include "depthai/pipeline/node/Sync.hpp" -#include "rtabmap/core/CameraModel.h" -#include "rtabmap/core/Odometry.h" -#include "rtabmap/core/OdometryInfo.h" -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/Transform.h" namespace dai { namespace node { @@ -27,6 +21,8 @@ namespace node { class RTABMapVIO : public NodeCRTP { public: constexpr static const char* NAME = "RTABMapVIO"; + RTABMapVIO(); + ~RTABMapVIO() override; std::string rectInputName = "rect"; std::string depthInputName = "depth"; @@ -77,9 +73,7 @@ class RTABMapVIO : public NodeCRTP { */ void setUseFeatures(bool use); - void setLocalTransform(std::shared_ptr transform) { - localTransform = transform->getRTABMapTransform(); - } + void setLocalTransform(std::shared_ptr transform); /** * Reset Odometry. @@ -89,18 +83,14 @@ class RTABMapVIO : public NodeCRTP { void buildInternal() override; private: + // pimpl + class Impl; + Pimpl pimplRtabmap; void run() override; void syncCB(std::shared_ptr data); Input inSync{*this, {"inSync", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::MessageGroup, true}}}}}; void imuCB(std::shared_ptr msg); void initialize(Pipeline& pipeline, int instanceNum, int width, int height); - rtabmap::StereoCameraModel model; - std::unique_ptr odom; - rtabmap::Transform localTransform; - rtabmap::Transform imuLocalTransform; - std::map rtabParams; - std::map accBuffer; - std::map gyroBuffer; std::mutex imuMtx; float alphaScaling = -1.0; bool initialized = false; diff --git a/src/basalt/BasaltVIO.cpp b/src/basalt/BasaltVIO.cpp index 8d0d2bdfb3..1544a28c09 100644 --- a/src/basalt/BasaltVIO.cpp +++ b/src/basalt/BasaltVIO.cpp @@ -1,10 +1,16 @@ #include "depthai/basalt/BasaltVIO.hpp" #include "../utility/PimplImpl.hpp" +#include "basalt/calibration/calibration.hpp" +#include "basalt/serialization/headers_serialization.h" +#include "basalt/spline/se3_spline.h" +#include "basalt/utils/common_types.h" +#include "basalt/utils/vio_config.h" #include "basalt/vi_estimator/vio_estimator.h" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" +#include "pipeline/datatype/TransformData.hpp" #include "tbb/concurrent_queue.h" #include "tbb/global_control.h" namespace dai { @@ -18,6 +24,18 @@ class BasaltVIO::Impl { std::shared_ptr::Ptr>> imuDataQueue; std::shared_ptr::Ptr>> outStateQueue; std::shared_ptr tbbGlobalControl; + std::shared_ptr> calib; + + basalt::OpticalFlowBase::Ptr optFlowPtr; + basalt::VioEstimatorBase::Ptr vio; + basalt::OpticalFlowInput::Ptr lastImgData; + /** + * VIO configuration file. + */ + basalt::VioConfig vioConfig; + + std::vector vioTNSec; + std::shared_ptr::SE3> localTransform; }; BasaltVIO::BasaltVIO() {} @@ -26,7 +44,6 @@ BasaltVIO::~BasaltVIO() = default; void BasaltVIO::buildInternal() { sync->out.link(inSync); - sync->setRunOnHost(false); inSync.addCallback(std::bind(&BasaltVIO::stereoCB, this, std::placeholders::_1)); imu.addCallback(std::bind(&BasaltVIO::imuCB, this, std::placeholders::_1)); @@ -36,14 +53,14 @@ void BasaltVIO::buildInternal() { Eigen::Quaterniond q(R); basalt::PoseState::SE3 initialRotation(q, Eigen::Vector3d(0, 0, 0)); // to output pose in FLU world coordinates - localTransform = std::make_shared::SE3>(initTrans * initialRotation.inverse()); + pimpl->localTransform = std::make_shared::SE3>(initTrans * initialRotation.inverse()); setDefaultVIOConfig(); } void BasaltVIO::setLocalTransform(const std::shared_ptr& transform) { auto trans = transform->getTranslation(); auto quat = transform->getQuaternion(); - localTransform = + pimpl->localTransform = std::make_shared::SE3>(Eigen::Quaterniond(quat.qw, quat.qx, quat.qy, quat.qz), Eigen::Vector3d(trans.x, trans.y, trans.z)); } void BasaltVIO::run() { @@ -58,7 +75,7 @@ void BasaltVIO::run() { pimpl->outStateQueue->pop(data); if(!data.get()) continue; - basalt::PoseState::SE3 pose = (*localTransform * data->T_w_i * calib->T_i_c[0]); + basalt::PoseState::SE3 pose = (*pimpl->localTransform * data->T_w_i * pimpl->calib->T_i_c[0]); // pose is in RDF orientation, convert to FLU auto finalPose = pose * opticalTransform.inverse(); @@ -66,6 +83,7 @@ void BasaltVIO::run() { auto rot = finalPose.unit_quaternion(); auto out = std::make_shared(trans.x(), trans.y(), trans.z(), rot.x(), rot.y(), rot.z(), rot.w()); transform.send(out); + std::lock_guard lck(imgMtx); passthrough.send(leftImg); } } @@ -86,6 +104,7 @@ void BasaltVIO::stereoCB(std::shared_ptr in) { for(auto& msg : *group) { std::shared_ptr imgFrame = std::dynamic_pointer_cast(msg.second); if(i == 0) { + std::lock_guard lck(imgMtx); leftImg = imgFrame; }; auto t = imgFrame->getTimestamp(); @@ -106,7 +125,7 @@ void BasaltVIO::stereoCB(std::shared_ptr in) { } i++; } - lastImgData = data; + pimpl->lastImgData = data; if(pimpl->imageDataQueue) { pimpl->imageDataQueue->push(data); } @@ -137,6 +156,38 @@ void BasaltVIO::stop() { ThreadedHostNode::stop(); } +void BasaltVIO::setImuExtrinsics(const std::shared_ptr& imuExtr) { + imuExtrinsics = imuExtr; +} + +void BasaltVIO::setAccelBias(const std::vector& accelBias) { + if(accelBias.size() != 9) { + throw std::invalid_argument("Accelerometer bias vector must have 9 elements."); + } + this->accelBias = accelBias; +} + +void BasaltVIO::setAccelNoiseStd(const std::vector& accelNoiseStd) { + if(accelNoiseStd.size() != 3) { + throw std::invalid_argument("Accelerometer noise vector must have 3 elements."); + } + this->accelNoiseStd = accelNoiseStd; +} + +void BasaltVIO::setGyroNoiseStd(const std::vector& gyroNoiseStd) { + if(gyroNoiseStd.size() != 3) { + throw std::invalid_argument("Gyroscope noise vector must have 3 elements."); + } + this->gyroNoiseStd = gyroNoiseStd; +} + +void BasaltVIO::setGyroBias(const std::vector& gyroBias) { + if(gyroBias.size() != 12) { + throw std::invalid_argument("Gyroscope bias vector must have 12 elements."); + } + this->gyroBias = gyroBias; +} + void BasaltVIO::initialize(std::vector> frames) { if(threadNum > 0) { pimpl->tbbGlobalControl = std::make_shared(tbb::global_control::max_allowed_parallelism, threadNum); @@ -144,26 +195,62 @@ void BasaltVIO::initialize(std::vector> frames) { auto pipeline = getParentPipeline(); using Scalar = double; - calib = std::make_shared>(); - calib->imu_update_rate = imuUpdateRate; + pimpl->calib = std::make_shared>(); + pimpl->calib->imu_update_rate = imuUpdateRate; auto calibHandler = pipeline.getDefaultDevice()->readCalibration(); for(const auto& frame : frames) { Eigen::Vector2i resolution; resolution << frame->getWidth(), frame->getHeight(); - calib->resolution.push_back(resolution); + pimpl->calib->resolution.push_back(resolution); auto camID = static_cast(frame->getInstanceNum()); // imu extrinsics - std::vector> imuExtr = calibHandler.getCameraToImuExtrinsics(camID, useSpecTranslation); + if(imuExtrinsics.has_value()) { + Eigen::Vector3d trans( + imuExtrinsics.value()->getTranslation().x, imuExtrinsics.value()->getTranslation().y, imuExtrinsics.value()->getTranslation().z); + Eigen::Quaterniond q(imuExtrinsics.value()->getQuaternion().qw, + imuExtrinsics.value()->getQuaternion().qx, + imuExtrinsics.value()->getQuaternion().qy, + imuExtrinsics.value()->getQuaternion().qz); + basalt::Calibration::SE3 T_i_c(q, trans); + pimpl->calib->T_i_c.push_back(T_i_c); + } else { + std::vector> imuExtr = calibHandler.getCameraToImuExtrinsics(camID, useSpecTranslation); - Eigen::Matrix R; - R << imuExtr[0][0], imuExtr[0][1], imuExtr[0][2], imuExtr[1][0], imuExtr[1][1], imuExtr[1][2], imuExtr[2][0], imuExtr[2][1], imuExtr[2][2]; - Eigen::Quaterniond q(R); + Eigen::Matrix R; + R << double(imuExtr[0][0]), double(imuExtr[0][1]), double(imuExtr[0][2]), double(imuExtr[1][0]), double(imuExtr[1][1]), double(imuExtr[1][2]), + double(imuExtr[2][0]), double(imuExtr[2][1]), double(imuExtr[2][2]); + Eigen::Quaterniond q(R); - Eigen::Vector3d trans(double(imuExtr[0][3]) * 0.01, double(imuExtr[1][3]) * 0.01, double(imuExtr[2][3]) * 0.01); - basalt::Calibration::SE3 T_i_c(q, trans); - calib->T_i_c.push_back(T_i_c); + Eigen::Vector3d trans(double(imuExtr[0][3]) * 0.01, double(imuExtr[1][3]) * 0.01, double(imuExtr[2][3]) * 0.01); + basalt::Calibration::SE3 T_i_c(q, trans); + pimpl->calib->T_i_c.push_back(T_i_c); + } + if(accelBias.has_value()) { + Eigen::Matrix accelBiasFull; + + accelBiasFull << accelBias.value()[3] + 1, 0, 0, accelBias.value()[4], accelBias.value()[6] + 1, 0, accelBias.value()[5], accelBias.value()[7], + accelBias.value()[8] + 1; + basalt::CalibAccelBias accel_bias; + accel_bias.getParam() = accelBiasFull; + pimpl->calib->calib_accel_bias = accel_bias; + } + if(gyroBias.has_value()) { + Eigen::Matrix gyroBiasFull; + + gyroBiasFull << gyroBias.value()[3] + 1, gyroBias.value()[4], gyroBias.value()[5], gyroBias.value()[6], gyroBias.value()[7] + 1, + gyroBias.value()[8], gyroBias.value()[9], gyroBias.value()[10], gyroBias.value()[11] + 1; + basalt::CalibGyroBias gyro_bias; + gyro_bias.getParam() = gyroBiasFull; + pimpl->calib->calib_gyro_bias = gyro_bias; + } + if(accelNoiseStd.has_value()) { + pimpl->calib->accel_noise_std = Eigen::Vector3d(accelNoiseStd.value()[0], accelNoiseStd.value()[1], accelNoiseStd.value()[2]).cwiseSqrt(); + } + if(gyroNoiseStd.has_value()) { + pimpl->calib->gyro_noise_std = Eigen::Vector3d(gyroNoiseStd.value()[0], gyroNoiseStd.value()[1], gyroNoiseStd.value()[2]).cwiseSqrt(); + } // camera intrinsics auto intrinsics = calibHandler.getCameraIntrinsics(camID, frame->getWidth(), frame->getHeight()); @@ -173,31 +260,31 @@ void BasaltVIO::initialize(std::vector> frames) { if(model == CameraModel::Perspective) { basalt::PinholeRadtan8Camera::VecN params; // fx, fy, cx, cy - double fx = intrinsics[0][0]; - double fy = intrinsics[1][1]; - double cx = intrinsics[0][2]; - double cy = intrinsics[1][2]; - double k1 = distCoeffs[0]; - double k2 = distCoeffs[1]; - double p1 = distCoeffs[2]; - double p2 = distCoeffs[3]; - double k3 = distCoeffs[4]; - double k4 = distCoeffs[5]; - double k5 = distCoeffs[6]; - double k6 = distCoeffs[7]; + double fx = double(intrinsics[0][0]); + double fy = double(intrinsics[1][1]); + double cx = double(intrinsics[0][2]); + double cy = double(intrinsics[1][2]); + double k1 = double(distCoeffs[0]); + double k2 = double(distCoeffs[1]); + double p1 = double(distCoeffs[2]); + double p2 = double(distCoeffs[3]); + double k3 = double(distCoeffs[4]); + double k4 = double(distCoeffs[5]); + double k5 = double(distCoeffs[6]); + double k6 = double(distCoeffs[7]); params << fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6; basalt::PinholeRadtan8Camera pinhole(params); camera.variant = pinhole; } else if(model == CameraModel::Fisheye) { // fx, fy, cx, cy - double fx = intrinsics[0][0]; - double fy = intrinsics[1][1]; - double cx = intrinsics[0][2]; - double cy = intrinsics[1][2]; - double k1 = distCoeffs[0]; - double k2 = distCoeffs[1]; - double k3 = distCoeffs[2]; - double k4 = distCoeffs[3]; + double fx = double(intrinsics[0][0]); + double fy = double(intrinsics[1][1]); + double cx = double(intrinsics[0][2]); + double cy = double(intrinsics[1][2]); + double k1 = double(distCoeffs[0]); + double k2 = double(distCoeffs[1]); + double k3 = double(distCoeffs[2]); + double k4 = double(distCoeffs[3]); basalt::KannalaBrandtCamera4::VecN params; params << fx, fy, cx, cy, k1, k2, k3, k4; basalt::KannalaBrandtCamera4 kannala(params); @@ -205,93 +292,161 @@ void BasaltVIO::initialize(std::vector> frames) { } else { throw std::runtime_error("Unknown distortion model"); } - calib->intrinsics.push_back(camera); + pimpl->calib->intrinsics.push_back(camera); } if(!configPath.empty()) { - vioConfig.load(configPath); + pimpl->vioConfig.load(configPath); } - optFlowPtr = basalt::OpticalFlowFactory::getOpticalFlow(vioConfig, *calib); - optFlowPtr->show_gui = false; - optFlowPtr->start(); - pimpl->imageDataQueue = optFlowPtr->input_img_queue; - vio = basalt::VioEstimatorFactory::getVioEstimator(vioConfig, *calib, basalt::constants::g, true, true); - vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); - pimpl->imuDataQueue = vio->imu_data_queue; - optFlowPtr->output_queue = vio->vision_data_queue; + pimpl->optFlowPtr = basalt::OpticalFlowFactory::getOpticalFlow(pimpl->vioConfig, *pimpl->calib); + pimpl->optFlowPtr->show_gui = false; + pimpl->optFlowPtr->start(); + pimpl->imageDataQueue = pimpl->optFlowPtr->input_img_queue; + pimpl->vio = basalt::VioEstimatorFactory::getVioEstimator(pimpl->vioConfig, *pimpl->calib, basalt::constants::g, true, true); + pimpl->vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + pimpl->imuDataQueue = pimpl->vio->imu_data_queue; + pimpl->optFlowPtr->output_queue = pimpl->vio->vision_data_queue; pimpl->outStateQueue = std::make_shared::Ptr>>(); - vio->out_state_queue = pimpl->outStateQueue; - vio->opt_flow_depth_guess_queue = optFlowPtr->input_depth_queue; - vio->opt_flow_state_queue = optFlowPtr->input_state_queue; - vio->opt_flow_lm_bundle_queue = optFlowPtr->input_lm_bundle_queue; + pimpl->vio->out_state_queue = pimpl->outStateQueue; + pimpl->vio->opt_flow_depth_guess_queue = pimpl->optFlowPtr->input_depth_queue; + pimpl->vio->opt_flow_state_queue = pimpl->optFlowPtr->input_state_queue; + pimpl->vio->opt_flow_lm_bundle_queue = pimpl->optFlowPtr->input_lm_bundle_queue; initialized = true; } - +void BasaltVIO::runSyncOnHost(bool runOnHost) { + sync->setRunOnHost(runOnHost); +} +void BasaltVIO::setConfig(const BasaltVIO::VioConfig& config) { + pimpl->vioConfig.optical_flow_type = config.optical_flow_type; + pimpl->vioConfig.optical_flow_detection_grid_size = config.optical_flow_detection_grid_size; + pimpl->vioConfig.optical_flow_detection_num_points_cell = config.optical_flow_detection_num_points_cell; + pimpl->vioConfig.optical_flow_detection_min_threshold = config.optical_flow_detection_min_threshold; + pimpl->vioConfig.optical_flow_detection_max_threshold = config.optical_flow_detection_max_threshold; + pimpl->vioConfig.optical_flow_detection_nonoverlap = config.optical_flow_detection_nonoverlap; + pimpl->vioConfig.optical_flow_max_recovered_dist2 = config.optical_flow_max_recovered_dist2; + pimpl->vioConfig.optical_flow_pattern = config.optical_flow_pattern; + pimpl->vioConfig.optical_flow_max_iterations = config.optical_flow_max_iterations; + pimpl->vioConfig.optical_flow_epipolar_error = config.optical_flow_epipolar_error; + pimpl->vioConfig.optical_flow_levels = config.optical_flow_levels; + pimpl->vioConfig.optical_flow_skip_frames = config.optical_flow_skip_frames; + pimpl->vioConfig.optical_flow_matching_guess_type = static_cast(config.optical_flow_matching_guess_type); + pimpl->vioConfig.optical_flow_matching_default_depth = config.optical_flow_matching_default_depth; + pimpl->vioConfig.optical_flow_image_safe_radius = config.optical_flow_image_safe_radius; + pimpl->vioConfig.optical_flow_recall_enable = config.optical_flow_recall_enable; + pimpl->vioConfig.optical_flow_recall_all_cams = config.optical_flow_recall_all_cams; + pimpl->vioConfig.optical_flow_recall_num_points_cell = config.optical_flow_recall_num_points_cell; + pimpl->vioConfig.optical_flow_recall_over_tracking = config.optical_flow_recall_over_tracking; + pimpl->vioConfig.optical_flow_recall_update_patch_viewpoint = config.optical_flow_recall_update_patch_viewpoint; + pimpl->vioConfig.optical_flow_recall_max_patch_dist = config.optical_flow_recall_max_patch_dist; + pimpl->vioConfig.optical_flow_recall_max_patch_norms = config.optical_flow_recall_max_patch_norms; + pimpl->vioConfig.vio_linearization_type = static_cast(config.vio_linearization_type); + pimpl->vioConfig.vio_sqrt_marg = config.vio_sqrt_marg; + pimpl->vioConfig.vio_max_states = config.vio_max_states; + pimpl->vioConfig.vio_max_kfs = config.vio_max_kfs; + pimpl->vioConfig.vio_min_frames_after_kf = config.vio_min_frames_after_kf; + pimpl->vioConfig.vio_new_kf_keypoints_thresh = config.vio_new_kf_keypoints_thresh; + pimpl->vioConfig.vio_debug = config.vio_debug; + pimpl->vioConfig.vio_extended_logging = config.vio_extended_logging; + pimpl->vioConfig.vio_obs_std_dev = config.vio_obs_std_dev; + pimpl->vioConfig.vio_obs_huber_thresh = config.vio_obs_huber_thresh; + pimpl->vioConfig.vio_min_triangulation_dist = config.vio_min_triangulation_dist; + pimpl->vioConfig.vio_max_iterations = config.vio_max_iterations; + pimpl->vioConfig.vio_enforce_realtime = config.vio_enforce_realtime; + pimpl->vioConfig.vio_use_lm = config.vio_use_lm; + pimpl->vioConfig.vio_lm_lambda_initial = config.vio_lm_lambda_initial; + pimpl->vioConfig.vio_lm_lambda_min = config.vio_lm_lambda_min; + pimpl->vioConfig.vio_lm_lambda_max = config.vio_lm_lambda_max; + pimpl->vioConfig.vio_scale_jacobian = config.vio_scale_jacobian; + pimpl->vioConfig.vio_init_pose_weight = config.vio_init_pose_weight; + pimpl->vioConfig.vio_init_ba_weight = config.vio_init_ba_weight; + pimpl->vioConfig.vio_init_bg_weight = config.vio_init_bg_weight; + pimpl->vioConfig.vio_marg_lost_landmarks = config.vio_marg_lost_landmarks; + pimpl->vioConfig.vio_fix_long_term_keyframes = config.vio_fix_long_term_keyframes; + pimpl->vioConfig.vio_kf_marg_feature_ratio = config.vio_kf_marg_feature_ratio; + pimpl->vioConfig.vio_kf_marg_criteria = static_cast(config.vio_kf_marg_criteria); + pimpl->vioConfig.mapper_obs_std_dev = config.mapper_obs_std_dev; + pimpl->vioConfig.mapper_obs_huber_thresh = config.mapper_obs_huber_thresh; + pimpl->vioConfig.mapper_detection_num_points = config.mapper_detection_num_points; + pimpl->vioConfig.mapper_num_frames_to_match = config.mapper_num_frames_to_match; + pimpl->vioConfig.mapper_frames_to_match_threshold = config.mapper_frames_to_match_threshold; + pimpl->vioConfig.mapper_min_matches = config.mapper_min_matches; + pimpl->vioConfig.mapper_ransac_threshold = config.mapper_ransac_threshold; + pimpl->vioConfig.mapper_min_track_length = config.mapper_min_track_length; + pimpl->vioConfig.mapper_max_hamming_distance = config.mapper_max_hamming_distance; + pimpl->vioConfig.mapper_second_best_test_ratio = config.mapper_second_best_test_ratio; + pimpl->vioConfig.mapper_bow_num_bits = config.mapper_bow_num_bits; + pimpl->vioConfig.mapper_min_triangulation_dist = config.mapper_min_triangulation_dist; + pimpl->vioConfig.mapper_no_factor_weights = config.mapper_no_factor_weights; + pimpl->vioConfig.mapper_use_factors = config.mapper_use_factors; + pimpl->vioConfig.mapper_use_lm = config.mapper_use_lm; + pimpl->vioConfig.mapper_lm_lambda_min = config.mapper_lm_lambda_min; + pimpl->vioConfig.mapper_lm_lambda_max = config.mapper_lm_lambda_max; +} void BasaltVIO::setDefaultVIOConfig() { - vioConfig.optical_flow_type = "frame_to_frame"; - vioConfig.optical_flow_detection_grid_size = 50; - vioConfig.optical_flow_detection_num_points_cell = 1; - vioConfig.optical_flow_detection_min_threshold = 5; - vioConfig.optical_flow_detection_max_threshold = 40; - vioConfig.optical_flow_detection_nonoverlap = true; - vioConfig.optical_flow_max_recovered_dist2 = 0.04; - vioConfig.optical_flow_pattern = 51; - vioConfig.optical_flow_max_iterations = 5; - vioConfig.optical_flow_epipolar_error = 0.005; - vioConfig.optical_flow_levels = 3; - vioConfig.optical_flow_skip_frames = 1; - vioConfig.optical_flow_matching_guess_type = basalt::MatchingGuessType::REPROJ_AVG_DEPTH; - vioConfig.optical_flow_matching_default_depth = 2.0; - vioConfig.optical_flow_image_safe_radius = 472.0; - vioConfig.optical_flow_recall_enable = false; - vioConfig.optical_flow_recall_all_cams = false; - vioConfig.optical_flow_recall_num_points_cell = true; - vioConfig.optical_flow_recall_over_tracking = false; - vioConfig.optical_flow_recall_update_patch_viewpoint = false; - vioConfig.optical_flow_recall_max_patch_dist = 3; - vioConfig.optical_flow_recall_max_patch_norms = {1.74, 0.96, 0.99, 0.44}; - vioConfig.vio_linearization_type = basalt::LinearizationType::ABS_QR; - vioConfig.vio_sqrt_marg = true; - vioConfig.vio_max_states = 3; - vioConfig.vio_max_kfs = 7; - vioConfig.vio_min_frames_after_kf = 5; - vioConfig.vio_new_kf_keypoints_thresh = 0.7; - vioConfig.vio_debug = false; - vioConfig.vio_extended_logging = false; - vioConfig.vio_obs_std_dev = 0.5; - vioConfig.vio_obs_huber_thresh = 1.0; - vioConfig.vio_min_triangulation_dist = 0.05; - vioConfig.vio_max_iterations = 7; - vioConfig.vio_enforce_realtime = false; - vioConfig.vio_use_lm = true; - vioConfig.vio_lm_lambda_initial = 1e-4; - vioConfig.vio_lm_lambda_min = 1e-6; - vioConfig.vio_lm_lambda_max = 1e2; - vioConfig.vio_scale_jacobian = false; - vioConfig.vio_init_pose_weight = 1e8; - vioConfig.vio_init_ba_weight = 1e1; - vioConfig.vio_init_bg_weight = 1e2; - vioConfig.vio_marg_lost_landmarks = true; - vioConfig.vio_fix_long_term_keyframes = false; - vioConfig.vio_kf_marg_feature_ratio = 0.1; - vioConfig.vio_kf_marg_criteria = basalt::KeyframeMargCriteria::KF_MARG_DEFAULT; - vioConfig.mapper_obs_std_dev = 0.25; - vioConfig.mapper_obs_huber_thresh = 1.5; - vioConfig.mapper_detection_num_points = 800; - vioConfig.mapper_num_frames_to_match = 30; - vioConfig.mapper_frames_to_match_threshold = 0.04; - vioConfig.mapper_min_matches = 20; - vioConfig.mapper_ransac_threshold = 5e-5; - vioConfig.mapper_min_track_length = 5; - vioConfig.mapper_max_hamming_distance = 70; - vioConfig.mapper_second_best_test_ratio = 1.2; - vioConfig.mapper_bow_num_bits = 16; - vioConfig.mapper_min_triangulation_dist = 0.07; - vioConfig.mapper_no_factor_weights = false; - vioConfig.mapper_use_factors = true; - vioConfig.mapper_use_lm = true; - vioConfig.mapper_lm_lambda_min = 1e-32; - vioConfig.mapper_lm_lambda_max = 1e3; + pimpl->vioConfig.optical_flow_type = "frame_to_frame"; + pimpl->vioConfig.optical_flow_detection_grid_size = 50; + pimpl->vioConfig.optical_flow_detection_num_points_cell = 1; + pimpl->vioConfig.optical_flow_detection_min_threshold = 5; + pimpl->vioConfig.optical_flow_detection_max_threshold = 40; + pimpl->vioConfig.optical_flow_detection_nonoverlap = true; + pimpl->vioConfig.optical_flow_max_recovered_dist2 = 0.04; + pimpl->vioConfig.optical_flow_pattern = 51; + pimpl->vioConfig.optical_flow_max_iterations = 5; + pimpl->vioConfig.optical_flow_epipolar_error = 0.005; + pimpl->vioConfig.optical_flow_levels = 3; + pimpl->vioConfig.optical_flow_skip_frames = 1; + pimpl->vioConfig.optical_flow_matching_guess_type = basalt::MatchingGuessType::REPROJ_AVG_DEPTH; + pimpl->vioConfig.optical_flow_matching_default_depth = 2.0; + pimpl->vioConfig.optical_flow_image_safe_radius = 472.0; + pimpl->vioConfig.optical_flow_recall_enable = false; + pimpl->vioConfig.optical_flow_recall_all_cams = false; + pimpl->vioConfig.optical_flow_recall_num_points_cell = true; + pimpl->vioConfig.optical_flow_recall_over_tracking = false; + pimpl->vioConfig.optical_flow_recall_update_patch_viewpoint = false; + pimpl->vioConfig.optical_flow_recall_max_patch_dist = 3; + pimpl->vioConfig.optical_flow_recall_max_patch_norms = {1.74, 0.96, 0.99, 0.44}; + pimpl->vioConfig.vio_linearization_type = basalt::LinearizationType::ABS_QR; + pimpl->vioConfig.vio_sqrt_marg = true; + pimpl->vioConfig.vio_max_states = 3; + pimpl->vioConfig.vio_max_kfs = 7; + pimpl->vioConfig.vio_min_frames_after_kf = 5; + pimpl->vioConfig.vio_new_kf_keypoints_thresh = 0.7; + pimpl->vioConfig.vio_debug = false; + pimpl->vioConfig.vio_extended_logging = false; + pimpl->vioConfig.vio_obs_std_dev = 0.5; + pimpl->vioConfig.vio_obs_huber_thresh = 1.0; + pimpl->vioConfig.vio_min_triangulation_dist = 0.05; + pimpl->vioConfig.vio_max_iterations = 7; + pimpl->vioConfig.vio_enforce_realtime = false; + pimpl->vioConfig.vio_use_lm = true; + pimpl->vioConfig.vio_lm_lambda_initial = 1e-4; + pimpl->vioConfig.vio_lm_lambda_min = 1e-6; + pimpl->vioConfig.vio_lm_lambda_max = 1e2; + pimpl->vioConfig.vio_scale_jacobian = false; + pimpl->vioConfig.vio_init_pose_weight = 1e8; + pimpl->vioConfig.vio_init_ba_weight = 1e1; + pimpl->vioConfig.vio_init_bg_weight = 1e2; + pimpl->vioConfig.vio_marg_lost_landmarks = true; + pimpl->vioConfig.vio_fix_long_term_keyframes = false; + pimpl->vioConfig.vio_kf_marg_feature_ratio = 0.1; + pimpl->vioConfig.vio_kf_marg_criteria = basalt::KeyframeMargCriteria::KF_MARG_DEFAULT; + pimpl->vioConfig.mapper_obs_std_dev = 0.25; + pimpl->vioConfig.mapper_obs_huber_thresh = 1.5; + pimpl->vioConfig.mapper_detection_num_points = 800; + pimpl->vioConfig.mapper_num_frames_to_match = 30; + pimpl->vioConfig.mapper_frames_to_match_threshold = 0.04; + pimpl->vioConfig.mapper_min_matches = 20; + pimpl->vioConfig.mapper_ransac_threshold = 5e-5; + pimpl->vioConfig.mapper_min_track_length = 5; + pimpl->vioConfig.mapper_max_hamming_distance = 70; + pimpl->vioConfig.mapper_second_best_test_ratio = 1.2; + pimpl->vioConfig.mapper_bow_num_bits = 16; + pimpl->vioConfig.mapper_min_triangulation_dist = 0.07; + pimpl->vioConfig.mapper_no_factor_weights = false; + pimpl->vioConfig.mapper_use_factors = true; + pimpl->vioConfig.mapper_use_lm = true; + pimpl->vioConfig.mapper_lm_lambda_min = 1e-32; + pimpl->vioConfig.mapper_lm_lambda_max = 1e3; } } // namespace node } // namespace dai diff --git a/src/pipeline/Node.cpp b/src/pipeline/Node.cpp index 057462461b..3a4850c174 100644 --- a/src/pipeline/Node.cpp +++ b/src/pipeline/Node.cpp @@ -330,7 +330,7 @@ Node::OutputMap::OutputMap(Node& parent, std::string name, Node::OutputDescripti } } -Node::OutputMap::OutputMap(Node& parent, Node::OutputDescription defaultOutput, bool ref) : OutputMap(parent, "", std::move(defaultOutput), ref){}; +Node::OutputMap::OutputMap(Node& parent, Node::OutputDescription defaultOutput, bool ref) : OutputMap(parent, "", std::move(defaultOutput), ref) {}; Node::Output& Node::OutputMap::operator[](const std::string& key) { if(count({name, key}) == 0) { diff --git a/src/pipeline/datatype/DatatypeEnum.cpp b/src/pipeline/datatype/DatatypeEnum.cpp index a7401ac65f..ee27b07191 100644 --- a/src/pipeline/datatype/DatatypeEnum.cpp +++ b/src/pipeline/datatype/DatatypeEnum.cpp @@ -48,6 +48,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::VppConfig, DatatypeEnum::ImageFiltersConfig, DatatypeEnum::ToFDepthConfidenceFilterConfig, + DatatypeEnum::MapData, DatatypeEnum::ObjectTrackerConfig, DatatypeEnum::DynamicCalibrationControl, DatatypeEnum::DynamicCalibrationResult, @@ -93,6 +94,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::ImageFiltersConfig, DatatypeEnum::VppConfig, DatatypeEnum::ToFDepthConfidenceFilterConfig, + DatatypeEnum::MapData, DatatypeEnum::ObjectTrackerConfig, DatatypeEnum::DynamicCalibrationControl, DatatypeEnum::DynamicCalibrationResult, @@ -135,6 +137,7 @@ const std::unordered_map> hierarchy = { {DatatypeEnum::VppConfig, {}}, {DatatypeEnum::ImageFiltersConfig, {}}, {DatatypeEnum::ToFDepthConfidenceFilterConfig, {}}, + {DatatypeEnum::MapData, {}}, {DatatypeEnum::ObjectTrackerConfig, {}}, {DatatypeEnum::DynamicCalibrationControl, {}}, {DatatypeEnum::DynamicCalibrationResult, {}}, diff --git a/src/pipeline/datatype/MapData.cpp b/src/pipeline/datatype/MapData.cpp new file mode 100644 index 0000000000..3c9c97f69a --- /dev/null +++ b/src/pipeline/datatype/MapData.cpp @@ -0,0 +1,10 @@ +#include "depthai/pipeline/datatype/MapData.hpp" + +namespace dai { +MapData::~MapData() = default; +void MapData::serialize(std::vector& metadata, DatatypeEnum& datatype) const { + metadata = utility::serialize(*this); + datatype = DatatypeEnum::MapData; +}; +// No implementation needed +} // namespace dai diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 439889143d..94c20ef28b 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -32,6 +32,7 @@ #include "depthai/pipeline/datatype/ImgAnnotations.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/datatype/MapData.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" #include "depthai/pipeline/datatype/NNData.hpp" #include "depthai/pipeline/datatype/NeuralDepthConfig.hpp" @@ -257,6 +258,8 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* case DatatypeEnum::PointCloudData: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; + case DatatypeEnum::MapData: + return parseDatatype(metadataStart, serializedObjectSize, data, fd); case DatatypeEnum::PipelineEvent: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; diff --git a/src/pipeline/node/DetectionNetwork.cpp b/src/pipeline/node/DetectionNetwork.cpp index 71a0fc1374..4882ed9c3c 100644 --- a/src/pipeline/node/DetectionNetwork.cpp +++ b/src/pipeline/node/DetectionNetwork.cpp @@ -33,7 +33,6 @@ DetectionNetwork::DetectionNetwork(const std::shared_ptr& device) outNetwork{neuralNetwork->out}, input{neuralNetwork->input}, passthrough{neuralNetwork->passthrough} {}; - // ------------------------------------------------------------------- // Neural Network API // ------------------------------------------------------------------- diff --git a/src/pipeline/node/host/Replay.cpp b/src/pipeline/node/host/Replay.cpp index 70fcb3a1ed..b6db715456 100644 --- a/src/pipeline/node/host/Replay.cpp +++ b/src/pipeline/node/host/Replay.cpp @@ -85,6 +85,7 @@ inline std::shared_ptr getMessage(const std::shared_ptr getProtoMessage(utility::ByteP case DatatypeEnum::TrackedFeatures: case DatatypeEnum::BenchmarkReport: case DatatypeEnum::MessageGroup: + case DatatypeEnum::MapData: case DatatypeEnum::TransformData: case DatatypeEnum::PointCloudConfig: case DatatypeEnum::ImageAlignConfig: diff --git a/src/rtabmap/CalibrationHandler.cpp b/src/rtabmap/CalibrationHandler.cpp deleted file mode 100644 index 4404dc0ade..0000000000 --- a/src/rtabmap/CalibrationHandler.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "depthai/device/CalibrationHandler.hpp" - -#include "rtabmap/core/StereoCameraModel.h" - -namespace dai { -rtabmap::StereoCameraModel CalibrationHandler::getRTABMapCameraModel(CameraBoardSocket cameraId, - int width, - int height, - const rtabmap::Transform& localTransform, - float alphaScaling, - dai::CameraBoardSocket left, - dai::CameraBoardSocket right) { - cv::Mat cameraMatrix, distCoeffs, newCameraMatrix; - std::vector > matrix = getCameraIntrinsics(cameraId, width, height); - cameraMatrix = (cv::Mat_(3, 3) << matrix[0][0], - matrix[0][1], - matrix[0][2], - matrix[1][0], - matrix[1][1], - matrix[1][2], - matrix[2][0], - matrix[2][1], - matrix[2][2]); - - std::vector coeffs = getDistortionCoefficients(cameraId); - if(getDistortionModel(cameraId) == dai::CameraModel::Perspective) - distCoeffs = (cv::Mat_(1, 8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]); - - if(alphaScaling > -1.0f) - newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, cv::Size(width, height), alphaScaling); - else - newCameraMatrix = cameraMatrix; - double fx = newCameraMatrix.at(0, 0); - double fy = newCameraMatrix.at(1, 1); - double cx = newCameraMatrix.at(0, 2); - double cy = newCameraMatrix.at(1, 2); - double baseline = double(getBaselineDistance(right, left, true, LengthUnit::METER)); - - return rtabmap::StereoCameraModel(eepromData.boardName, fx, fy, cx, cy, baseline, localTransform, cv::Size(width, height)); -} -} // namespace dai \ No newline at end of file diff --git a/src/rtabmap/RTABMapConversions.cpp b/src/rtabmap/RTABMapConversions.cpp new file mode 100644 index 0000000000..08e2525d8b --- /dev/null +++ b/src/rtabmap/RTABMapConversions.cpp @@ -0,0 +1,68 @@ +#include "RTABMapConversions.hpp" + +#include "common/CameraBoardSocket.hpp" +#include "depthai/pipeline/datatype/TransformData.hpp" +#include "rtabmap/core/StereoCameraModel.h" +#include "rtabmap/core/Transform.h" +namespace dai { + +std::shared_ptr rtabmapToTransformData(rtabmap::Transform transformRTABMap) { + auto transform = std::make_shared(); + + transform->transform.matrix = {{{transformRTABMap.r11(), transformRTABMap.r12(), transformRTABMap.r13(), transformRTABMap.o14()}, + {transformRTABMap.r21(), transformRTABMap.r22(), transformRTABMap.r23(), transformRTABMap.o24()}, + {transformRTABMap.r31(), transformRTABMap.r32(), transformRTABMap.r33(), transformRTABMap.o34()}}}; + return transform; +} +rtabmap::Transform getRTABMapTransform(const Transform& transform) { + return rtabmap::Transform(transform.matrix[0][0], + transform.matrix[0][1], + transform.matrix[0][2], + transform.matrix[0][3], + transform.matrix[1][0], + transform.matrix[1][1], + transform.matrix[1][2], + transform.matrix[1][3], + transform.matrix[2][0], + transform.matrix[2][1], + transform.matrix[2][2], + transform.matrix[2][3]); +} +rtabmap::StereoCameraModel getRTABMapCameraModel(CameraBoardSocket cameraId, + int width, + int height, + const rtabmap::Transform& localTransform, + float alphaScaling, + CalibrationHandler calHandler, + CameraBoardSocket left, + CameraBoardSocket right) { + cv::Mat cameraMatrix, distCoeffs, newCameraMatrix; + std::vector > matrix = calHandler.getCameraIntrinsics(cameraId, width, height); + cameraMatrix = (cv::Mat_(3, 3) << matrix[0][0], + matrix[0][1], + matrix[0][2], + matrix[1][0], + matrix[1][1], + matrix[1][2], + matrix[2][0], + matrix[2][1], + matrix[2][2]); + + std::vector coeffs = calHandler.getDistortionCoefficients(cameraId); + if(calHandler.getDistortionModel(cameraId) == dai::CameraModel::Perspective) + distCoeffs = (cv::Mat_(1, 8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]); + + if(alphaScaling > -1.0f) + newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, cv::Size(width, height), alphaScaling); + else + newCameraMatrix = cameraMatrix; + double fx = newCameraMatrix.at(0, 0); + double fy = newCameraMatrix.at(1, 1); + double cx = newCameraMatrix.at(0, 2); + double cy = newCameraMatrix.at(1, 2); + double baseline = double(calHandler.getBaselineDistance(right, left)) / 100.0; + + return rtabmap::StereoCameraModel(calHandler.getEepromData().boardName, fx, fy, cx, cy, baseline, localTransform, cv::Size(width, height)); +} + +} // namespace dai diff --git a/src/rtabmap/RTABMapConversions.hpp b/src/rtabmap/RTABMapConversions.hpp new file mode 100644 index 0000000000..11a6094d73 --- /dev/null +++ b/src/rtabmap/RTABMapConversions.hpp @@ -0,0 +1,24 @@ +#include + +#include "common/CameraBoardSocket.hpp" +#include "depthai/device/CalibrationHandler.hpp" +#include "depthai/pipeline/datatype/TransformData.hpp" + +namespace rtabmap { +class Transform; +class StereoCameraModel; +} // namespace rtabmap + +namespace dai { + +std::shared_ptr rtabmapToTransformData(rtabmap::Transform transformRTABMap); +rtabmap::Transform getRTABMapTransform(const Transform& transform); +rtabmap::StereoCameraModel getRTABMapCameraModel(CameraBoardSocket cameraId, + int width, + int height, + const rtabmap::Transform& localTransform, + float alphaScaling, + CalibrationHandler calHandler, + CameraBoardSocket left = CameraBoardSocket::CAM_B, + CameraBoardSocket right = CameraBoardSocket::CAM_C); +} // namespace dai diff --git a/src/rtabmap/RTABMapSLAM.cpp b/src/rtabmap/RTABMapSLAM.cpp index d7a5ed0d87..daa36dbb71 100644 --- a/src/rtabmap/RTABMapSLAM.cpp +++ b/src/rtabmap/RTABMapSLAM.cpp @@ -1,24 +1,132 @@ #include "depthai/rtabmap/RTABMapSLAM.hpp" -#include -#include -#include - +#include "../utility/PimplImpl.hpp" #include "depthai/pipeline/Pipeline.hpp" +#include "RTABMapConversions.hpp" +#include "pcl/filters/filter.h" +#include "pcl/point_cloud.h" #include "pipeline/ThreadedNodeImpl.hpp" +#include "pipeline/datatype/ImgFrame.hpp" +#include "pipeline/datatype/MapData.hpp" +#include "pipeline/datatype/TransformData.hpp" +#include "rtabmap/core/CameraModel.h" +#include "rtabmap/core/LocalGrid.h" +#include "rtabmap/core/Rtabmap.h" +#include "rtabmap/core/SensorData.h" +#include "rtabmap/core/Transform.h" +#include "rtabmap/core/global_map/CloudMap.h" +#include "rtabmap/core/global_map/OccupancyGrid.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_mapping.h" +#include "spdlog/spdlog.h" namespace dai { namespace node { +class RTABMapSLAM::Impl { + public: + Impl() = default; + void setLocalTransform(std::shared_ptr transform) { + localTransform = getRTABMapTransform(transform->transform); + } + std::shared_ptr getLocalTransform() { + return rtabmapToTransformData(localTransform); + } + void setPublishObstacleCloud(bool publish) { + publishObstacleCloud = publish; + } + void setPublishGroundCloud(bool publish) { + publishGroundCloud = publish; + } + void setPublishGrid(bool publish) { + publishGrid = publish; + } + void publishGridMap(const std::map& optimizedPoses, dai::Node::Output& occupancyGridMap) { + if(occupancyGrid->addedNodes().size() || localMaps->size() > 0) { + occupancyGrid->update(optimizedPoses); + } + float xMin, yMin; + cv::Mat map = occupancyGrid->getMap(xMin, yMin); + if(!map.empty()) { + cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map); + cv::flip(map8U, map8U, 0); + + auto mapMsg = std::make_shared(); + dai::ImgFrame mapImg; + mapImg.setTimestamp(std::chrono::steady_clock::now()); + mapImg.setCvFrame(map8U, ImgFrame::Type::GRAY8); + mapMsg->ts = mapImg.ts; + mapMsg->minX = xMin; + mapMsg->minY = yMin; + mapMsg->map = mapImg; + + occupancyGridMap.send(mapMsg); + } + } + + void publishPointClouds(const std::map& optimizedPoses, dai::Node::Output& obstaclePCL, dai::Node::Output& groundPCL) { + if(cloudMap->addedNodes().size() || localMaps->size() > 0) { + cloudMap->update(optimizedPoses); + } + + if(publishObstacleCloud) { + auto obstaclesMap = cloudMap->getMapObstacles(); + // convert point to point3frgba + + auto pclData = setPclDataRGB(obstaclesMap); + obstaclePCL.send(pclData); + } + if(publishGroundCloud) { + auto groundMap = cloudMap->getMapGround(); + auto pclData = setPclDataRGB(groundMap); + groundPCL.send(pclData); + } + } + std::shared_ptr setPclDataRGB(const pcl::PointCloud::Ptr& cloud) { + // Ensure cloud is valid + if(!cloud) { + throw std::invalid_argument("Input cloud is null"); + } + auto pcl = std::make_shared(); + + auto size = cloud->points.size(); + std::vector data(size * sizeof(Point3fRGBA)); + auto* dataPtr = reinterpret_cast(data.data()); + pcl->setWidth(cloud->width); + pcl->setHeight(cloud->height); + pcl->setSparse(!cloud->is_dense); + + std::for_each(cloud->points.begin(), cloud->points.end(), [dataPtr, &cloud](const pcl::PointXYZRGB& point) mutable { + size_t i = &point - &cloud->points[0]; + dataPtr[i] = Point3fRGBA{point.x, point.y, point.z, point.r, point.g, point.b}; + }); + pcl->setColor(true); + pcl->setData(data); + return pcl; + } + + rtabmap::StereoCameraModel model; + rtabmap::Rtabmap rtabmap; + rtabmap::Transform currPose, odomCorr; + std::chrono::steady_clock::time_point lastProcessTime; + std::chrono::steady_clock::time_point startTime; + rtabmap::Transform imuLocalTransform; + rtabmap::Transform localTransform; + std::shared_ptr localMaps; + std::unique_ptr occupancyGrid; + std::unique_ptr cloudMap; + rtabmap::SensorData sensorData; + bool publishObstacleCloud = true; + bool publishGroundCloud = true; + bool publishGrid = true; +}; void RTABMapSLAM::buildInternal() { sync->out.link(inSync); - sync->setRunOnHost(false); + sync->setRunOnHost(true); alphaScaling = -1.0; - localTransform = rtabmap::Transform::getIdentity(); + pimplRtabmap->localTransform = rtabmap::Transform::getIdentity(); rtabmap::Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); - localTransform = localTransform * opticalTransform; + pimplRtabmap->localTransform = pimplRtabmap->localTransform * opticalTransform; rect.setBlocking(false); rect.setMaxSize(1); depth.setBlocking(false); @@ -29,9 +137,9 @@ void RTABMapSLAM::buildInternal() { odom.setMaxSize(1); odom.setBlocking(false); odom.addCallback(std::bind(&RTABMapSLAM::odomPoseCB, this, std::placeholders::_1)); - localMaps = std::make_shared(); + pimplRtabmap->localMaps = std::make_shared(); } - +RTABMapSLAM::RTABMapSLAM() = default; RTABMapSLAM::~RTABMapSLAM() { auto& logger = pimpl->logger; @@ -40,22 +148,28 @@ RTABMapSLAM::~RTABMapSLAM() { databasePath = "/tmp/rtabmap.db"; } logger->info("Saving database at {}", databasePath); - rtabmap.close(true, databasePath); + pimplRtabmap->rtabmap.close(true, databasePath); } } -void RTABMapSLAM::setParams(const rtabmap::ParametersMap& params) { +void RTABMapSLAM::setParams(const std::map& params) { rtabParams = params; } void RTABMapSLAM::triggerNewMap() { - rtabmap.triggerNewMap(); + pimplRtabmap->rtabmap.triggerNewMap(); } void RTABMapSLAM::saveDatabase() { - rtabmap.close(); - rtabmap.init(rtabParams, databasePath); + pimplRtabmap->rtabmap.close(); + pimplRtabmap->rtabmap.init(rtabParams, databasePath); } +std::shared_ptr RTABMapSLAM::getLocalTransform() { + return pimplRtabmap->getLocalTransform(); +} +void RTABMapSLAM::setLocalTransform(std::shared_ptr transform) { + pimplRtabmap->setLocalTransform(transform); +} void RTABMapSLAM::setUseFeatures(bool use) { useFeatures = use; if(useFeatures) { @@ -65,6 +179,18 @@ void RTABMapSLAM::setUseFeatures(bool use) { } } +void RTABMapSLAM::setPublishGrid(bool publish) { + pimplRtabmap->setPublishGrid(publish); +} + +void RTABMapSLAM::setPublishGroundCloud(bool publish) { + pimplRtabmap->setPublishGroundCloud(publish); +} + +void RTABMapSLAM::setPublishObstacleCloud(bool publish) { + pimplRtabmap->setPublishObstacleCloud(publish); +} + void RTABMapSLAM::syncCB(std::shared_ptr data) { auto group = std::dynamic_pointer_cast(data); if(group == nullptr) return; @@ -83,13 +209,14 @@ void RTABMapSLAM::syncCB(std::shared_ptr data) { } else { double stamp = std::chrono::duration(imgFrame->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); - sensorData = rtabmap::SensorData(imgFrame->getCvFrame(), depthFrame->getCvFrame(), model.left(), imgFrame->getSequenceNum(), stamp); + pimplRtabmap->sensorData = + rtabmap::SensorData(imgFrame->getCvFrame(), depthFrame->getCvFrame(), pimplRtabmap->model.left(), imgFrame->getSequenceNum(), stamp); std::vector keypoints; if(featuresFrame != nullptr) { for(auto& feature : featuresFrame->trackedFeatures) { keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3)); } - sensorData.setFeatures(keypoints, std::vector(), cv::Mat()); + pimplRtabmap->sensorData.setFeatures(keypoints, std::vector(), cv::Mat()); } } passthroughRect.send(imgFrame); @@ -103,11 +230,12 @@ void RTABMapSLAM::syncCB(std::shared_ptr data) { void RTABMapSLAM::odomPoseCB(std::shared_ptr data) { auto odomPose = std::dynamic_pointer_cast(data); // convert odom pose to rtabmap pose - rtabmap::Transform p = odomPose->getRTABMapTransform(); - currPose = p; + rtabmap::Transform p = getRTABMapTransform(odomPose->transform); + pimplRtabmap->currPose = p; - auto outTransform = std::make_shared(odomCorr * currPose); - auto outCorrection = std::make_shared(odomCorr); + auto outTransform = rtabmapToTransformData(pimplRtabmap->odomCorr * pimplRtabmap->currPose); + auto outCorrection = rtabmapToTransformData(pimplRtabmap->odomCorr); + // set unit rotation for outCorrection transform.send(outTransform); odomCorrection.send(outCorrection); passthroughOdom.send(odomPose); @@ -121,97 +249,62 @@ void RTABMapSLAM::run() { } else { rtabmap::Statistics stats; std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - if(now - lastProcessTime > std::chrono::milliseconds(int(1000.0f / freq))) { - lastProcessTime = now; - bool success = rtabmap.process(sensorData, currPose); + if(now - pimplRtabmap->lastProcessTime > std::chrono::milliseconds(int(1000.0f / freq))) { + pimplRtabmap->lastProcessTime = now; + bool success = pimplRtabmap->rtabmap.process(pimplRtabmap->sensorData, pimplRtabmap->currPose); if(success) { - stats = rtabmap.getStatistics(); - if(rtabmap.getLoopClosureId() > 0) { - logger->debug("Loop closure detected! last loop closure id = {}", rtabmap.getLoopClosureId()); + stats = pimplRtabmap->rtabmap.getStatistics(); + if(pimplRtabmap->rtabmap.getLoopClosureId() > 0) { + logger->debug("Loop closure detected! last loop closure id = {}", pimplRtabmap->rtabmap.getLoopClosureId()); } - odomCorr = stats.mapCorrection(); + pimplRtabmap->odomCorr = stats.mapCorrection(); - const std::map& optimizedPoses = rtabmap.getLocalOptimizedPoses(); + const std::map& optimizedPoses = pimplRtabmap->rtabmap.getLocalOptimizedPoses(); if(optimizedPoses.find(stats.getLastSignatureData().id()) != optimizedPoses.end()) { const rtabmap::Signature& node = stats.getLastSignatureData(); - localMaps->add(node.id(), - node.sensorData().gridGroundCellsRaw(), - node.sensorData().gridObstacleCellsRaw(), - node.sensorData().gridEmptyCellsRaw(), - node.sensorData().gridCellSize(), - node.sensorData().gridViewPoint()); + pimplRtabmap->localMaps->add(node.id(), + node.sensorData().gridGroundCellsRaw(), + node.sensorData().gridObstacleCellsRaw(), + node.sensorData().gridEmptyCellsRaw(), + node.sensorData().gridCellSize(), + node.sensorData().gridViewPoint()); } if(publishGrid) { - publishGridMap(optimizedPoses); + pimplRtabmap->publishGridMap(optimizedPoses, occupancyGridMap); } if(publishObstacleCloud || publishGroundCloud) { - publishPointClouds(optimizedPoses); + pimplRtabmap->publishPointClouds(optimizedPoses, obstaclePCL, groundPCL); } } } } // save database periodically if set - if(saveDatabasePeriodically && std::chrono::duration(std::chrono::steady_clock::now() - startTime).count() > databaseSaveInterval) { - rtabmap.close(true, databasePath); - rtabmap.init(rtabParams, databasePath); + if(saveDatabasePeriodically + && std::chrono::duration(std::chrono::steady_clock::now() - pimplRtabmap->startTime).count() > databaseSaveInterval) { + pimplRtabmap->rtabmap.close(true, databasePath); + pimplRtabmap->rtabmap.init(rtabParams, databasePath); logger->info("Database saved at {}", databasePath); - startTime = std::chrono::steady_clock::now(); + pimplRtabmap->startTime = std::chrono::steady_clock::now(); } } } -void RTABMapSLAM::publishGridMap(const std::map& optimizedPoses) { - if(occupancyGrid->addedNodes().size() || localMaps->size() > 0) { - occupancyGrid->update(optimizedPoses); - } - float xMin, yMin; - cv::Mat map = occupancyGrid->getMap(xMin, yMin); - if(!map.empty()) { - cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map); - cv::flip(map8U, map8U, 0); - - auto mapMsg = std::make_shared(); - mapMsg->setTimestamp(std::chrono::steady_clock::now()); - mapMsg->setCvFrame(map8U, ImgFrame::Type::GRAY8); - occupancyGridMap.send(mapMsg); - } -} - -void RTABMapSLAM::publishPointClouds(const std::map& optimizedPoses) { - if(cloudMap->addedNodes().size() || localMaps->size() > 0) { - cloudMap->update(optimizedPoses); - } - - if(publishObstacleCloud) { - auto obstaclesMap = cloudMap->getMapObstacles(); - auto pclData = std::make_shared(); - pclData->setPclDataRGB(obstaclesMap); - obstaclePCL.send(pclData); - } - if(publishGroundCloud) { - auto groundMap = cloudMap->getMapGround(); - auto pclData = std::make_shared(); - pclData->setPclDataRGB(groundMap); - groundPCL.send(pclData); - } -} - void RTABMapSLAM::initialize(dai::Pipeline& pipeline, int instanceNum, int width, int height) { auto calibHandler = pipeline.getDefaultDevice()->readCalibration(); auto cameraId = static_cast(instanceNum); - model = calibHandler.getRTABMapCameraModel(cameraId, width, height, localTransform, alphaScaling); + pimplRtabmap->model = getRTABMapCameraModel(cameraId, width, height, pimplRtabmap->localTransform, alphaScaling, calibHandler); if(!databasePath.empty()) { - rtabmap.init(rtabParams, databasePath); + pimplRtabmap->rtabmap.init(rtabParams, databasePath); } else { - rtabmap.init(rtabParams); + pimplRtabmap->rtabmap.init(rtabParams); } - lastProcessTime = std::chrono::steady_clock::now(); - startTime = std::chrono::steady_clock::now(); - occupancyGrid = std::make_unique(localMaps.get(), rtabParams); - cloudMap = std::make_unique(localMaps.get(), rtabParams); + pimplRtabmap->lastProcessTime = std::chrono::steady_clock::now(); + pimplRtabmap->startTime = std::chrono::steady_clock::now(); + pimplRtabmap->occupancyGrid = std::make_unique(pimplRtabmap->localMaps.get(), rtabParams); + pimplRtabmap->cloudMap = std::make_unique(pimplRtabmap->localMaps.get(), rtabParams); initialized = true; } } // namespace node diff --git a/src/rtabmap/RTABMapVIO.cpp b/src/rtabmap/RTABMapVIO.cpp index 55f1ddae5e..e6674793cc 100644 --- a/src/rtabmap/RTABMapVIO.cpp +++ b/src/rtabmap/RTABMapVIO.cpp @@ -1,17 +1,37 @@ #include "depthai/rtabmap/RTABMapVIO.hpp" +#include "../utility/PimplImpl.hpp" #include "depthai/pipeline/Pipeline.hpp" +#include "RTABMapConversions.hpp" +#include "rtabmap/core/CameraModel.h" +#include "rtabmap/core/Odometry.h" +#include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/core/SensorData.h" +#include "rtabmap/core/Transform.h" namespace dai { namespace node { - +class RTABMapVIO::Impl { + public: + Impl() = default; + rtabmap::StereoCameraModel model; + std::unique_ptr odom; + rtabmap::Transform localTransform; + rtabmap::Transform imuLocalTransform; + std::map rtabParams; + std::map accBuffer; + std::map gyroBuffer; +}; + +RTABMapVIO::RTABMapVIO() = default; +RTABMapVIO::~RTABMapVIO() = default; void RTABMapVIO::buildInternal() { sync->out.link(inSync); sync->setRunOnHost(false); - localTransform = rtabmap::Transform::getIdentity(); + pimplRtabmap->localTransform = rtabmap::Transform::getIdentity(); rtabmap::Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); - localTransform = localTransform * opticalTransform.inverse(); + pimplRtabmap->localTransform = pimplRtabmap->localTransform * opticalTransform.inverse(); inSync.setBlocking(false); inSync.setMaxSize(0); @@ -49,8 +69,8 @@ void RTABMapVIO::imuCB(std::shared_ptr msg) { auto& gyroValues = imuPacket.gyroscope; double accStamp = std::chrono::duration(acceleroValues.getTimestampDevice().time_since_epoch()).count(); double gyroStamp = std::chrono::duration(gyroValues.getTimestampDevice().time_since_epoch()).count(); - accBuffer.emplace_hint(accBuffer.end(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)); - gyroBuffer.emplace_hint(gyroBuffer.end(), gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)); + pimplRtabmap->accBuffer.emplace_hint(pimplRtabmap->accBuffer.end(), accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)); + pimplRtabmap->gyroBuffer.emplace_hint(pimplRtabmap->gyroBuffer.end(), gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)); } } @@ -71,10 +91,10 @@ void RTABMapVIO::syncCB(std::shared_ptr data) { } else { double stamp = std::chrono::duration(imgFrame->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); - sensorData = rtabmap::SensorData(imgFrame->getCvFrame(), depthFrame->getCvFrame(), model.left(), imgFrame->getSequenceNum(), stamp); + sensorData = rtabmap::SensorData(imgFrame->getCvFrame(), depthFrame->getCvFrame(), pimplRtabmap->model.left(), imgFrame->getSequenceNum(), stamp); - std::vector keypoints; if(featuresFrame != nullptr) { + std::vector keypoints; for(auto& feature : featuresFrame->trackedFeatures) { keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3)); } @@ -84,40 +104,41 @@ void RTABMapVIO::syncCB(std::shared_ptr data) { cv::Vec3d acc, gyro; std::map::const_iterator iterA, iterB; - if(!accBuffer.empty() && !gyroBuffer.empty() && accBuffer.rbegin()->first >= stamp && gyroBuffer.rbegin()->first >= stamp) { + if(!pimplRtabmap->accBuffer.empty() && !pimplRtabmap->gyroBuffer.empty() && pimplRtabmap->accBuffer.rbegin()->first >= stamp + && pimplRtabmap->gyroBuffer.rbegin()->first >= stamp) { // acc - iterB = accBuffer.lower_bound(stamp); - if(iterB != accBuffer.end()) { + iterB = pimplRtabmap->accBuffer.lower_bound(stamp); + if(iterB != pimplRtabmap->accBuffer.end()) { iterA = iterB; - if(iterA != accBuffer.begin()) --iterA; + if(iterA != pimplRtabmap->accBuffer.begin()) --iterA; if(iterA == iterB || stamp == iterB->first) { acc = iterB->second; } else if(stamp > iterA->first && stamp < iterB->first) { float t = (stamp - iterA->first) / (iterB->first - iterA->first); acc = iterA->second + t * (iterB->second - iterA->second); } - accBuffer.erase(accBuffer.begin(), iterB); + pimplRtabmap->accBuffer.erase(pimplRtabmap->accBuffer.begin(), iterB); } // gyro - iterB = gyroBuffer.lower_bound(stamp); - if(iterB != gyroBuffer.end()) { + iterB = pimplRtabmap->gyroBuffer.lower_bound(stamp); + if(iterB != pimplRtabmap->gyroBuffer.end()) { iterA = iterB; - if(iterA != gyroBuffer.begin()) --iterA; + if(iterA != pimplRtabmap->gyroBuffer.begin()) --iterA; if(iterA == iterB || stamp == iterB->first) { gyro = iterB->second; } else if(stamp > iterA->first && stamp < iterB->first) { float t = (stamp - iterA->first) / (iterB->first - iterA->first); gyro = iterA->second + t * (iterB->second - iterA->second); } - gyroBuffer.erase(gyroBuffer.begin(), iterB); + pimplRtabmap->gyroBuffer.erase(pimplRtabmap->gyroBuffer.begin(), iterB); } - sensorData.setIMU(rtabmap::IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform)); + sensorData.setIMU(rtabmap::IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), pimplRtabmap->imuLocalTransform)); } rtabmap::OdometryInfo info; - auto pose = odom->process(sensorData, &info); - pose = localTransform * pose * localTransform.inverse(); - auto out = std::make_shared(pose); + auto pose = pimplRtabmap->odom->process(sensorData, &info); + pose = pimplRtabmap->localTransform * pose * pimplRtabmap->localTransform.inverse(); + auto out = rtabmapToTransformData(pose); transform.send(out); passthroughRect.send(imgFrame); passthroughDepth.send(depthFrame); @@ -139,39 +160,42 @@ void RTABMapVIO::run() { void RTABMapVIO::reset(std::shared_ptr transform) { if(transform != nullptr) { - odom->reset(transform->getRTABMapTransform()); + pimplRtabmap->odom->reset(getRTABMapTransform(transform->transform)); } else { - odom->reset(); + pimplRtabmap->odom->reset(); } } void RTABMapVIO::setParams(const rtabmap::ParametersMap& params) { - rtabParams = params; + pimplRtabmap->rtabParams = params; +} +void RTABMapVIO::setLocalTransform(std::shared_ptr transform) { + pimplRtabmap->localTransform = getRTABMapTransform(transform->transform); } void RTABMapVIO::initialize(dai::Pipeline& pipeline, int instanceNum, int width, int height) { auto calibHandler = pipeline.getDefaultDevice()->readCalibration(); auto cameraId = static_cast(instanceNum); - model = calibHandler.getRTABMapCameraModel(cameraId, width, height, localTransform, alphaScaling); + pimplRtabmap->model = getRTABMapCameraModel(cameraId, width, height, pimplRtabmap->localTransform, alphaScaling, calibHandler); std::vector> imuExtr = calibHandler.getImuToCameraExtrinsics(cameraId, true); - imuLocalTransform = rtabmap::Transform(double(imuExtr[0][0]), - double(imuExtr[0][1]), - double(imuExtr[0][2]), - double(imuExtr[0][3]) * 0.01, - double(imuExtr[1][0]), - double(imuExtr[1][1]), - double(imuExtr[1][2]), - double(imuExtr[1][3]) * 0.01, - double(imuExtr[2][0]), - double(imuExtr[2][1]), - double(imuExtr[2][2]), - double(imuExtr[2][3]) * 0.01); - - imuLocalTransform = localTransform * imuLocalTransform; - odom.reset(rtabmap::Odometry::create(rtabParams)); + pimplRtabmap->imuLocalTransform = rtabmap::Transform(double(imuExtr[0][0]), + double(imuExtr[0][1]), + double(imuExtr[0][2]), + double(imuExtr[0][3]) * 0.01, + double(imuExtr[1][0]), + double(imuExtr[1][1]), + double(imuExtr[1][2]), + double(imuExtr[1][3]) * 0.01, + double(imuExtr[2][0]), + double(imuExtr[2][1]), + double(imuExtr[2][2]), + double(imuExtr[2][3]) * 0.01); + + pimplRtabmap->imuLocalTransform = pimplRtabmap->localTransform * pimplRtabmap->imuLocalTransform; + pimplRtabmap->odom.reset(rtabmap::Odometry::create(pimplRtabmap->rtabParams)); initialized = true; } } // namespace node diff --git a/src/rtabmap/TransformData.cpp b/src/rtabmap/TransformData.cpp deleted file mode 100644 index 0f66c9680c..0000000000 --- a/src/rtabmap/TransformData.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "depthai/pipeline/datatype/TransformData.hpp" - -namespace dai { -TransformData::TransformData(const rtabmap::Transform& transformRTABMap) { - transform.matrix = {{{transformRTABMap.r11(), transformRTABMap.r12(), transformRTABMap.r13(), transformRTABMap.o14()}, - {transformRTABMap.r21(), transformRTABMap.r22(), transformRTABMap.r23(), transformRTABMap.o24()}, - {transformRTABMap.r31(), transformRTABMap.r32(), transformRTABMap.r33(), transformRTABMap.o34()}}}; -} -rtabmap::Transform TransformData::getRTABMapTransform() const { - return rtabmap::Transform(transform.matrix[0][0], - transform.matrix[0][1], - transform.matrix[0][2], - transform.matrix[0][3], - transform.matrix[1][0], - transform.matrix[1][1], - transform.matrix[1][2], - transform.matrix[1][3], - transform.matrix[2][0], - transform.matrix[2][1], - transform.matrix[2][2], - transform.matrix[2][3]); -} -} // namespace dai \ No newline at end of file diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index c7ebafa643..89278b588b 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -163,6 +163,7 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::ToFConfig: case DatatypeEnum::TrackedFeatures: case DatatypeEnum::BenchmarkReport: + case DatatypeEnum::MapData: case DatatypeEnum::MessageGroup: case DatatypeEnum::TransformData: case DatatypeEnum::PointCloudConfig: diff --git a/tests/src/ondevice_tests/filesystem_test.cpp b/tests/src/ondevice_tests/filesystem_test.cpp index a2d3601290..36d2dc5054 100644 --- a/tests/src/ondevice_tests/filesystem_test.cpp +++ b/tests/src/ondevice_tests/filesystem_test.cpp @@ -5,7 +5,7 @@ using namespace Catch::Matchers; // Include depthai library #include -#if(__cplusplus >= 201703L) || (_MSVC_LANG >= 201703L) +#if (__cplusplus >= 201703L) || (_MSVC_LANG >= 201703L) #include #endif #include From e41e1a2de1861dad93ddd1fe1ff1e8c344416049 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 27 Jan 2026 18:53:41 +0100 Subject: [PATCH 074/180] Formatting --- bindings/python/src/pybind11_common.hpp | 2 +- src/pipeline/Node.cpp | 2 +- src/rtabmap/RTABMapSLAM.cpp | 2 +- src/rtabmap/RTABMapVIO.cpp | 2 +- tests/src/ondevice_tests/filesystem_test.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/bindings/python/src/pybind11_common.hpp b/bindings/python/src/pybind11_common.hpp index e0f3db10f4..ed09b4879c 100644 --- a/bindings/python/src/pybind11_common.hpp +++ b/bindings/python/src/pybind11_common.hpp @@ -1,6 +1,6 @@ #pragma once -#if (_MSC_VER >= 1910) || !defined(_MSC_VER) +#if(_MSC_VER >= 1910) || !defined(_MSC_VER) #ifndef HAVE_SNPRINTF #define HAVE_SNPRINTF #endif diff --git a/src/pipeline/Node.cpp b/src/pipeline/Node.cpp index 3a4850c174..057462461b 100644 --- a/src/pipeline/Node.cpp +++ b/src/pipeline/Node.cpp @@ -330,7 +330,7 @@ Node::OutputMap::OutputMap(Node& parent, std::string name, Node::OutputDescripti } } -Node::OutputMap::OutputMap(Node& parent, Node::OutputDescription defaultOutput, bool ref) : OutputMap(parent, "", std::move(defaultOutput), ref) {}; +Node::OutputMap::OutputMap(Node& parent, Node::OutputDescription defaultOutput, bool ref) : OutputMap(parent, "", std::move(defaultOutput), ref){}; Node::Output& Node::OutputMap::operator[](const std::string& key) { if(count({name, key}) == 0) { diff --git a/src/rtabmap/RTABMapSLAM.cpp b/src/rtabmap/RTABMapSLAM.cpp index daa36dbb71..bbb406c1d4 100644 --- a/src/rtabmap/RTABMapSLAM.cpp +++ b/src/rtabmap/RTABMapSLAM.cpp @@ -1,8 +1,8 @@ #include "depthai/rtabmap/RTABMapSLAM.hpp" #include "../utility/PimplImpl.hpp" -#include "depthai/pipeline/Pipeline.hpp" #include "RTABMapConversions.hpp" +#include "depthai/pipeline/Pipeline.hpp" #include "pcl/filters/filter.h" #include "pcl/point_cloud.h" #include "pipeline/ThreadedNodeImpl.hpp" diff --git a/src/rtabmap/RTABMapVIO.cpp b/src/rtabmap/RTABMapVIO.cpp index e6674793cc..da20f14b68 100644 --- a/src/rtabmap/RTABMapVIO.cpp +++ b/src/rtabmap/RTABMapVIO.cpp @@ -1,8 +1,8 @@ #include "depthai/rtabmap/RTABMapVIO.hpp" #include "../utility/PimplImpl.hpp" -#include "depthai/pipeline/Pipeline.hpp" #include "RTABMapConversions.hpp" +#include "depthai/pipeline/Pipeline.hpp" #include "rtabmap/core/CameraModel.h" #include "rtabmap/core/Odometry.h" #include "rtabmap/core/OdometryInfo.h" diff --git a/tests/src/ondevice_tests/filesystem_test.cpp b/tests/src/ondevice_tests/filesystem_test.cpp index 36d2dc5054..a2d3601290 100644 --- a/tests/src/ondevice_tests/filesystem_test.cpp +++ b/tests/src/ondevice_tests/filesystem_test.cpp @@ -5,7 +5,7 @@ using namespace Catch::Matchers; // Include depthai library #include -#if (__cplusplus >= 201703L) || (_MSVC_LANG >= 201703L) +#if(__cplusplus >= 201703L) || (_MSVC_LANG >= 201703L) #include #endif #include From 88515514e112d7d61ee7ea629581324c0832aa7c Mon Sep 17 00:00:00 2001 From: sb-lxn Date: Wed, 28 Jan 2026 07:07:35 +0000 Subject: [PATCH 075/180] fsync slave mode examples (#1488) * Add RPC wait for device Signed-off-by: stas.bucik * Add example script for fsync slave mode Signed-off-by: stas.bucik * Add M8 Fsync role switching Signed-off-by: stas.bucik * Update examples Signed-off-by: stas.bucik * Rename examples Signed-off-by: stas.bucik * Documentation Signed-off-by: stas.bucik * Remove MASTER_WITHOUT_OUTPUT Signed-off-by: stas.bucik * Improve error message Signed-off-by: stas.bucik * Add M8 strobe configuration Signed-off-by: stas.bucik * Add multi device fsync test Signed-off-by: stas.bucik * Update examples Signed-off-by: stas.bucik * Review Signed-off-by: stas.bucik * Add multi device fsync back to CI Signed-off-by: stas.bucik * Review: snake_case to camelCase Signed-off-by: stas.bucik * Review: remove new line Signed-off-by: stas.bucik * Review: remove comment Signed-off-by: stas.bucik * Review: remove unneded set strobe limits Signed-off-by: stas.bucik * Review: fix typo Signed-off-by: stas.bucik * Review: Add explanation of strobe limits Signed-off-by: stas.bucik * Review: clang-format Signed-off-by: stas.bucik * Review: Remove mentions of M8 connector Signed-off-by: stas.bucik * Review: multidevice fsync test: decrease sync threshold to 1ms Signed-off-by: stas.bucik * Review: Fsync rename to frame sync Signed-off-by: stas.bucik * Review: strobe limits renamed to relative strobe limits. Signed-off-by: stas.bucik * Review: add socket selection option Signed-off-by: stas.bucik * Review: Change name to relative limits Signed-off-by: stas.bucik * Review: Fix IR detection Signed-off-by: stas.bucik * Review: make examples more readable Signed-off-by: stas.bucik * Review: Check if rvc4 on examples Signed-off-by: stas.bucik * Bump depthai-device Signed-off-by: stas.bucik --------- Signed-off-by: stas.bucik --- .github/workflows/test_child.yml | 16 + bindings/python/src/DeviceBindings.cpp | 36 ++ .../python/src/pipeline/CommonBindings.cpp | 7 + cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/cpp/Misc/CMakeLists.txt | 3 +- examples/cpp/Misc/MultiDevice/CMakeLists.txt | 7 + .../multi_device_external_frame_sync.cpp | 399 ++++++++++++++++++ .../multi_device_external_frame_sync.py | 341 +++++++++++++++ .../depthai/common/ExternalFrameSyncRoles.hpp | 45 ++ include/depthai/device/DeviceBase.hpp | 33 ++ src/device/DeviceBase.cpp | 32 +- tests/CMakeLists.txt | 4 + tests/run_tests.py | 17 +- .../pipeline/fsync_multi_device.cpp | 308 ++++++++++++++ 15 files changed, 1243 insertions(+), 9 deletions(-) create mode 100644 examples/cpp/Misc/MultiDevice/CMakeLists.txt create mode 100644 examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp create mode 100644 examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py create mode 100644 include/depthai/common/ExternalFrameSyncRoles.hpp create mode 100644 tests/src/onhost_tests/pipeline/fsync_multi_device.cpp diff --git a/.github/workflows/test_child.yml b/.github/workflows/test_child.yml index bb473e7c94..358987cfef 100644 --- a/.github/workflows/test_child.yml +++ b/.github/workflows/test_child.yml @@ -89,3 +89,19 @@ jobs: source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}-rgb" exec hil_runner --models "oak4_s" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc4rgb" + + linux_rvc4_fsync_test: + needs: [build_docker_container] + strategy: + matrix: + rvc4os: ${{ fromJson(inputs.luxonis_os_versions_to_test) }} + fail-fast: false + runs-on: ['self-hosted', 'testbed-runner'] + steps: + - uses: actions/checkout@v3 + + - name: Run RVC4 Fsync tests + run: | + source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} + export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}-fsync" + exec hil_runner --platforms=rvc4 --scope fsync --basic-sanity --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh fsync" diff --git a/bindings/python/src/DeviceBindings.cpp b/bindings/python/src/DeviceBindings.cpp index 8fe086fc7d..c48aa50235 100644 --- a/bindings/python/src/DeviceBindings.cpp +++ b/bindings/python/src/DeviceBindings.cpp @@ -845,6 +845,42 @@ void DeviceBindings::bind(pybind11::module& m, void* pCallstack) { }, py::arg("enable"), DOC(dai, DeviceBase, setTimesync, 2)) + .def( + "setExternalFrameSyncRole", + [](DeviceBase& d, ExternalFrameSyncRole role) { + py::gil_scoped_release release; + return d.setExternalFrameSyncRole(role); + }, + py::arg("role"), + DOC(dai, DeviceBase, setExternalFrameSyncRole) + ) + .def( + "getExternalFrameSyncRole", + [](DeviceBase& d) { + py::gil_scoped_release release; + return d.getExternalFrameSyncRole(); + }, + DOC(dai, DeviceBase, getExternalFrameSyncRole) + ) + .def( + "setExternalStrobeRelativeLimits", + [](DeviceBase& d, float min, float max) { + py::gil_scoped_release release; + return d.setExternalStrobeRelativeLimits(min, max); + }, + py::arg("min"), + py::arg("max"), + DOC(dai, DeviceBase, setExternalStrobeRelativeLimits) + ) + .def( + "setExternalStrobeEnable", + [](DeviceBase& d, bool enable) { + py::gil_scoped_release release; + d.setExternalStrobeEnable(enable); + }, + py::arg("enable"), + DOC(dai, DeviceBase, setExternalStrobeEnable) + ) .def( "getDeviceName", [](DeviceBase& d) { diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 54c9183145..19470bf3cd 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -7,6 +7,7 @@ // depthai/ #include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/ExternalFrameSyncRoles.hpp" #include "depthai/common/CameraFeatures.hpp" #include "depthai/common/CameraImageOrientation.hpp" #include "depthai/common/CameraSensorType.hpp" @@ -59,6 +60,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::class_ size2f(m, "Size2f", DOC(dai, Size2f)); py::enum_ cameraBoardSocket(m, "CameraBoardSocket", DOC(dai, CameraBoardSocket)); py::enum_ housingCoordinateSystem(m, "HousingCoordinateSystem", DOC(dai, HousingCoordinateSystem)); + py::enum_ externalFrameSyncRole(m, "ExternalFrameSyncRole", DOC(dai, ExternalFrameSyncRole)); py::enum_ connectionInterface(m, "connectionInterface", DOC(dai, ConnectionInterface)); py::enum_ cameraSensorType(m, "CameraSensorType", DOC(dai, CameraSensorType)); py::enum_ cameraImageOrientation(m, "CameraImageOrientation", DOC(dai, CameraImageOrientation)); @@ -413,6 +415,11 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .value("VESA_I", HousingCoordinateSystem::VESA_I) .value("VESA_J", HousingCoordinateSystem::VESA_J) .value("IMU", HousingCoordinateSystem::IMU); + + // ExternalFrameSyncRole enum bindings + externalFrameSyncRole.value("AUTO_DETECT", ExternalFrameSyncRole::AUTO_DETECT) + .value("MASTER", ExternalFrameSyncRole::MASTER) + .value("SLAVE", ExternalFrameSyncRole::SLAVE); // CameraSensorType enum bindings cameraSensorType.value("COLOR", CameraSensorType::COLOR) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index fbe20e4bb5..0a3dde0ce9 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+2a5145f0430b2cd922c0ab15e958ea1c0a66be0b") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+ad61a2047fcf728becea15dc9431f43941a2a934") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 647857830d..33746df1ce 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "276d50325cb0954fc5afd51dae933c8474907e13") +set(DEPTHAI_DEVICE_SIDE_COMMIT "34ce5bbb381e5836bc2712bc63d0647c43b080f6") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/cpp/Misc/CMakeLists.txt b/examples/cpp/Misc/CMakeLists.txt index c60886595f..dd9c9c68a3 100644 --- a/examples/cpp/Misc/CMakeLists.txt +++ b/examples/cpp/Misc/CMakeLists.txt @@ -5,4 +5,5 @@ add_subdirectory(AutoReconnect) add_subdirectory(Projectors) add_subdirectory(PipelineDebugging) add_subdirectory(SystemLogger) -add_subdirectory(XLinkBridge) \ No newline at end of file +add_subdirectory(XLinkBridge) +add_subdirectory(MultiDevice) \ No newline at end of file diff --git a/examples/cpp/Misc/MultiDevice/CMakeLists.txt b/examples/cpp/Misc/MultiDevice/CMakeLists.txt new file mode 100644 index 0000000000..1d1b277b8a --- /dev/null +++ b/examples/cpp/Misc/MultiDevice/CMakeLists.txt @@ -0,0 +1,7 @@ +project(projectors_examples) +cmake_minimum_required(VERSION 3.10) + +## function: dai_add_example(example_name example_src enable_test use_pcl) +## function: dai_set_example_test_labels(example_name ...) + +dai_add_example(multi_device_external_frame_sync multi_device_external_frame_sync.cpp OFF OFF) \ No newline at end of file diff --git a/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp b/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp new file mode 100644 index 0000000000..b858846f93 --- /dev/null +++ b/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp @@ -0,0 +1,399 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/capabilities/ImgFrameCapability.hpp" +#include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/CameraExposureOffset.hpp" +#include "depthai/common/ExternalFrameSyncRoles.hpp" +#include "depthai/depthai.hpp" +#include "depthai/pipeline/MessageQueue.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/node/Camera.hpp" +#include "depthai/pipeline/node/Sync.hpp" +#include "depthai/xlink/XLinkConnection.hpp" + +class FPSCounter { + public: + std::vector> frameTimes; + + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push_back(now); + if(frameTimes.size() > 100) { + frameTimes.erase(frameTimes.begin(), frameTimes.begin() + (frameTimes.size() - 100)); + } + } + + float getFps() { + if(frameTimes.size() <= 1) { + return 0.0f; + } + // Calculate the FPS + return float(frameTimes.size() - 1) * 1e6 / std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + } +}; + +dai::Node::Output* createPipeline(std::shared_ptr pipeline, dai::CameraBoardSocket socket, float sensorFps, dai::ExternalFrameSyncRole role) { + std::shared_ptr cam; + if(role == dai::ExternalFrameSyncRole::MASTER) { + cam = pipeline->create()->build(socket, std::nullopt, sensorFps); + } else { + cam = pipeline->create()->build(socket, std::nullopt); + } + + auto output = cam->requestOutput(std::make_pair(640, 480), dai::ImgFrame::Type::NV12, dai::ImgResizeMode::STRETCH); + return output; +} + +std::shared_ptr createSyncNode(std::shared_ptr& masterPipeline, + std::map& masterNode, + std::chrono::nanoseconds syncThreshold, + std::vector& outputNames, + std::map>>& slaveQueues, + std::map>& inputQueues) { + auto sync = masterPipeline->create(); + sync->setRunOnHost(true); + sync->setSyncThreshold(syncThreshold); + for(auto p : masterNode) { + auto name = std::string("master_") + p.first; + p.second->link(sync->inputs[name]); + outputNames.push_back(name); + } + + for(auto& p : slaveQueues) { + for(auto& q : p.second) { + auto name = std::string("slave_") + p.first + "_" + q.first; + outputNames.push_back(name); + auto input_queue = sync->inputs[name].createInputQueue(); + inputQueues.emplace(name, input_queue); + } + } + + return sync; +} + +void setUpCameraSocket(std::shared_ptr& pipeline, + dai::CameraBoardSocket socket, + std::string& name, + float targetFps, + dai::ExternalFrameSyncRole role, + std::optional>& masterNode, + std::map>>& slaveQueues, + std::vector& camSockets) { + auto outNode = createPipeline(pipeline, socket, targetFps, role); + + if(role == dai::ExternalFrameSyncRole::MASTER) { + if(!masterNode.has_value()) { + masterNode.emplace(); + } + masterNode.value().emplace(dai::toString(socket), outNode); + + } else if(role == dai::ExternalFrameSyncRole::SLAVE) { + if(slaveQueues.find(name) == slaveQueues.end()) { + slaveQueues.emplace(name, std::map>()); + } + slaveQueues[name].emplace(dai::toString(socket), outNode->createOutputQueue()); + + } else { + throw std::runtime_error("Don't know how to handle role " + dai::toString(role)); + } + + if(std::find(camSockets.begin(), camSockets.end(), dai::toString(socket)) == camSockets.end()) { + camSockets.emplace_back(dai::toString(socket)); + } +} + +void setupDevice(dai::DeviceInfo& deviceInfo, + std::shared_ptr& masterPipeline, + std::optional>& masterNode, + std::map>& slavePipelines, + std::map>>& slaveQueues, + std::vector& camSockets, + std::vector& sockets, + float targetFps) { + auto pipeline = std::make_shared(std::make_shared(deviceInfo)); + auto device = pipeline->getDefaultDevice(); + + if(device->getPlatform() != dai::Platform::RVC4) { + throw std::runtime_error("This example supports only RVC4 platform!"); + } + + std::string name = deviceInfo.getXLinkDeviceDesc().name; + auto role = device->getExternalFrameSyncRole(); + + std::cout << "=== Connected to " << deviceInfo.getDeviceId() << std::endl; + std::cout << " Device ID: " << device->getDeviceId() << std::endl; + std::cout << " Num of cameras: " << device->getConnectedCameras().size() << std::endl; + + for(auto socket : device->getConnectedCameras()) { + if(std::find(sockets.begin(), sockets.end(), dai::toString(socket)) == sockets.end()) { + continue; + } + setUpCameraSocket(pipeline, socket, name, targetFps, role, masterNode, slaveQueues, camSockets); + } + + if(role == dai::ExternalFrameSyncRole::MASTER) { + device->setExternalStrobeEnable(true); + std::cout << device->getDeviceId() << " is master" << std::endl; + + if(masterPipeline != nullptr) { + throw std::runtime_error("Only one master pipeline is supported"); + } + masterPipeline = pipeline; + } else if(role == dai::ExternalFrameSyncRole::SLAVE) { + slavePipelines[name] = pipeline; + std::cout << device->getDeviceId() << " is slave" << std::endl; + } else { + throw std::runtime_error("Don't know how to handle role " + dai::toString(role)); + } +} + +namespace { +bool running = true; +} + +void interruptHandler(int sig) { + if(running) { + std::cout << "Interrupted! Exiting..." << std::endl; + running = false; + } else { + std::cout << "Exiting now!" << std::endl; + exit(0); + } +} + +int main(int argc, char** argv) { + if(argc < 6) { + std::cout + << "Usage: " << argv[0] + << " [ [device_ip_3]] ..." + << std::endl; + std::exit(0); + } + + signal(SIGINT, interruptHandler); + + float targetFps = std::stof(argv[1]); + + int recvAllTimeoutSec = std::stoi(argv[2]); + float syncThresholdSec = std::stof(argv[3]); + int initialSyncTimeoutSec = std::stoi(argv[4]); + + std::vector sockets; + std::stringstream ss(argv[5]); + std::string item; + while(std::getline(ss, item, ',')) { + sockets.push_back(item); + } + + std::vector deviceInfos; + + if(argc > 6) { + for(int i = 6; i < argc; i++) { + deviceInfos.emplace_back(std::string(argv[i])); + } + } else { + deviceInfos = dai::Device::getAllAvailableDevices(); + } + + if(deviceInfos.size() < 2) { + std::cout << "At least two devices are required for this example." << std::endl; + std::exit(0); + } + + std::shared_ptr masterPipeline; + std::optional> masterNode; + + std::map> slavePipelines; + std::map>> slaveQueues; + + std::map> inputQueues; + std::vector outputNames; + std::vector camSockets; + + for(auto deviceInfo : deviceInfos) { + setupDevice(deviceInfo, masterPipeline, masterNode, slavePipelines, slaveQueues, camSockets, sockets, targetFps); + } + + if(masterPipeline == nullptr || !masterNode.has_value()) { + throw std::runtime_error("No master detected!"); + } + if(slavePipelines.size() < 1) { + throw std::runtime_error("No slaves detected!"); + } + + auto sync = + createSyncNode(masterPipeline, *masterNode, std::chrono::nanoseconds(long(round(1e9 * 0.5f / targetFps))), outputNames, slaveQueues, inputQueues); + auto queue = sync->out.createOutputQueue(); + + masterPipeline->start(); + for(auto p : slavePipelines) { + p.second->start(); + } + + FPSCounter fpsCounter; + + std::optional> latestFrameGroup; + bool firstReceived = false; + auto startTime = std::chrono::steady_clock::now(); + auto prevReceived = std::chrono::steady_clock::now(); + + std::optional> initialSyncTime; + + bool waitingForInitialSync = true; + + while(running) { + for(auto p : slaveQueues) { + for(auto q : p.second) { + while(q.second->has()) { + inputQueues[std::string("slave_") + p.first + "_" + q.first]->send(q.second->get()); + } + } + } + + while(queue->has()) { + auto syncData = queue->get(); + latestFrameGroup = std::dynamic_pointer_cast(syncData); + if(!firstReceived) { + firstReceived = true; + initialSyncTime = std::chrono::steady_clock::now(); + } + prevReceived = std::chrono::steady_clock::now(); + fpsCounter.tick(); + } + + if(!firstReceived) { + auto endTime = std::chrono::steady_clock::now(); + auto elapsedSec = std::chrono::duration_cast(endTime - startTime).count(); + if(elapsedSec >= recvAllTimeoutSec) { + std::cout << "Timeout: Didn't receive all frames in time: " << elapsedSec << std::endl; + running = false; + } + } + + if(latestFrameGroup.has_value() && size_t(latestFrameGroup.value()->getNumMessages()) == outputNames.size()) { + using ts_type = std::chrono::time_point; + std::map tsValues; + for(auto name : outputNames) { + auto frame = latestFrameGroup.value()->get(name); + tsValues.emplace(name, frame->getTimestamp(dai::CameraExposureOffset::END)); + } + + std::vector imgs; + std::vector firstFrameDone; + for(auto name : camSockets) { + imgs.emplace_back(); + firstFrameDone.emplace_back(false); + } + + int32_t idx = -1; + for(auto outputName : outputNames) { + for(uint32_t i = 0; i < camSockets.size(); i++) { + if(outputName.find(camSockets[i]) != std::string::npos) { + idx = i; + break; + } + } + if(idx == -1) { + throw std::runtime_error(std::string("Could not find camera socket for ") + outputName); + } + + auto msg = latestFrameGroup.value()->get(outputName); + auto fps = fpsCounter.getFps(); + auto frame = msg->getCvFrame(); + + cv::putText(frame, outputName, {20, 40}, cv::FONT_HERSHEY_SIMPLEX, 0.6, {0, 127, 255}, 2, cv::LINE_AA); + + cv::putText(frame, + "Timestamp: " + + std::to_string(1e-6 * std::chrono::duration_cast(tsValues[outputName].time_since_epoch()).count()) + + " | FPS: " + std::to_string(fps).substr(0, 5), + {20, 80}, + cv::FONT_HERSHEY_SIMPLEX, + 0.6, + {255, 0, 50}, + 2, + cv::LINE_AA); + + if(!firstFrameDone[idx]) { + imgs[idx] = frame; + firstFrameDone[idx] = true; + } else { + cv::hconcat(frame, imgs[idx], imgs[idx]); + } + } + + auto compFunct = [](const std::pair& p1, const std::pair& p2) -> bool { return p1.second < p2.second; }; + + auto maxElement = std::max_element(tsValues.begin(), tsValues.end(), compFunct); + auto minElement = std::min_element(tsValues.begin(), tsValues.end(), compFunct); + + auto delta = maxElement->second - minElement->second; + + bool syncStatus = std::chrono::duration_cast(delta).count() < syncThresholdSec; + std::string syncStatusStr = (syncStatus) ? "in sync" : "out of sync"; + + cv::Scalar color = (syncStatus) ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255); + + if(!syncStatus && waitingForInitialSync) { + auto endTime = std::chrono::steady_clock::now(); + auto elapsedSec = std::chrono::duration_cast(endTime - initialSyncTime.value()).count(); + if(elapsedSec >= initialSyncTimeoutSec) { + std::cout << "Timeout: Didn't sync frames in time" << std::endl; + running = false; + } + } + + if(syncStatus && waitingForInitialSync) { + std::cout << "Sync status: " << syncStatusStr << std::endl; + waitingForInitialSync = false; + } + + if(!syncStatus && !waitingForInitialSync) { + std::cout << "Sync error: Sync lost, threshold exceeded " << std::chrono::duration_cast(delta).count() << " us" + << std::endl; + running = false; + } + + for(auto i = 0; i < imgs.size(); i++) { + cv::putText(imgs[i], + syncStatusStr + " | delta = " + + std::to_string(1e-3 * float(std::chrono::duration_cast(delta).count())).substr(0, 5) + " ms", + {20, 120}, + cv::FONT_HERSHEY_SIMPLEX, + 0.7, + color, + 2, + cv::LINE_AA); + } + + for(auto i = 0; i < imgs.size(); i++) { + cv::imshow("synced_view_" + camSockets[i], imgs[i]); + } + + latestFrameGroup.reset(); + } + + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py new file mode 100644 index 0000000000..c17565f8e4 --- /dev/null +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -0,0 +1,341 @@ +#!/usr/bin/env python3 + +import contextlib +import datetime + +import cv2 +import depthai as dai +import time +import math + +import argparse +import signal +import numpy as np + +from typing import Optional, Dict, List +# --------------------------------------------------------------------------- +# Configuration +# --------------------------------------------------------------------------- +SET_MANUAL_EXPOSURE = False # Set to True to use manual exposure settings +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + + +# --------------------------------------------------------------------------- +# Pipeline creation (unchanged API – only uses TARGET_FPS constant) +# --------------------------------------------------------------------------- +def createPipeline(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, sensorFps: float, role: dai.ExternalFrameSyncRole): + cam = None + if role == dai.ExternalFrameSyncRole.MASTER: + cam = ( + pipeline.create(dai.node.Camera) + .build(socket, sensorFps=sensorFps) + ) + else: + cam = ( + pipeline.create(dai.node.Camera) + .build(socket) + ) + + output = ( + cam.requestOutput( + (640, 480), dai.ImgFrame.Type.NV12, dai.ImgResizeMode.STRETCH + ) + ) + return pipeline, output + +def createSyncNode(syncThreshold: datetime.timedelta): + global masterPipeline, masterNode, slaveQueues, inputQueues, outputNames + sync = masterPipeline.create(dai.node.Sync) + sync.setRunOnHost(True) + sync.setSyncThreshold(syncThreshold) + for k, v in masterNode.items(): + name = f"master_{k}" + v.link(sync.inputs[name]) + outputNames.append(name) + + for k, v in slaveQueues.items(): + for k2, v2 in v.items(): + name = f"slave_{k}_{k2}" + outputNames.append(name) + input_queue = sync.inputs[name].createInputQueue() + inputQueues[name] = input_queue + + return sync + +def setUpCameraSocket( + pipeline: dai.Pipeline, + socket: dai.CameraBoardSocket, + name: str, + targetFps: float, + role: dai.ExternalFrameSyncRole): + global masterNode, slaveQueues, camSockets + pipeline, outNode = createPipeline(pipeline, socket, targetFps, role) + + if role == dai.ExternalFrameSyncRole.MASTER: + if masterNode is None: + masterNode = {} + + masterNode[socket.name] = outNode + elif role == dai.ExternalFrameSyncRole.SLAVE: + if slaveQueues.get(name) is None: + slaveQueues[name] = {} + + slaveQueues[name][socket.name] = outNode.createOutputQueue() + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + if socket.name not in camSockets: + camSockets.append(socket.name) + + return pipeline + +def setupDevice( + stack: contextlib.ExitStack, + deviceInfo: dai.DeviceInfo, + sockets: Optional[List[str]], + targetFps: float): + global masterPipeline, masterNode, slavePipelines, slaveQueues, camSockets + pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) + device = pipeline.getDefaultDevice() + + if device.getPlatform() != dai.Platform.RVC4: + raise RuntimeError("This example supports only RVC4 platform!") + + name = deviceInfo.getXLinkDeviceDesc().name + role = device.getExternalFrameSyncRole() + + print("=== Connected to", deviceInfo.getDeviceId()) + print(" Device ID:", device.getDeviceId()) + print(" Num of cameras:", len(device.getConnectedCameras())) + + for socket in device.getConnectedCameras(): + if sockets is not None and socket.name not in sockets: + continue + pipeline = setUpCameraSocket(pipeline, socket, name, targetFps, role) + + if role == dai.ExternalFrameSyncRole.MASTER: + device.setExternalStrobeEnable(True) + print(f"{device.getDeviceId()} is master") + + if masterPipeline is not None: + raise RuntimeError("Only one master pipeline is supported") + + masterPipeline = pipeline + elif role == dai.ExternalFrameSyncRole.SLAVE: + slavePipelines[name] = pipeline + print(f"{device.getDeviceId()} is slave") + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + +running = True + +def interruptHandler(sig, frame): + global running + if running: + print("Interrupted! Exiting...") + running = False + else: + print("Exiting now!") + exit(0) + +signal.signal(signal.SIGINT, interruptHandler) + +parser = argparse.ArgumentParser(add_help=False) +parser.add_argument("-f", "--fps", type=float, default=30.0, help="Target FPS", required=False) +parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs", required=False) +parser.add_argument("--sockets", type=str, help="Socket ids to sync, comma separated", required=False) +parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) +parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=3e-3, help="Sync threshold in seconds", required=False) +parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) +args = parser.parse_args() + +if len(args.devices) == 0: + deviceInfos = dai.Device.getAllAvailableDevices() +else: + deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] #The master camera needs to be first here + +assert len(deviceInfos) > 1, "At least two devices are required for this example." + +targetFps = args.fps # Must match sensorFps in createPipeline() + +if args.sockets: + sockets = args.sockets.split(",") +else: + sockets = None + +recvAllTimeoutSec = args.recv_all_timeout_sec + +syncThresholdSec = args.sync_threshold_sec +initialSyncTimeoutSec = args.initial_sync_timeout_sec +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- +with contextlib.ExitStack() as stack: + + masterPipeline: Optional[dai.Pipeline] = None + masterNode: Optional[Dict[str, dai.Node.Output]] = None + + slavePipelines: Dict[str, dai.Pipeline] = {} + slaveQueues: Dict[str, Dict[str, dai.MessageQueue]] = {} + + inputQueues = {} + + outputNames = [] + + camSockets = [] + + for idx, deviceInfo in enumerate(deviceInfos): + setupDevice(stack, deviceInfo, sockets, targetFps) + + if masterPipeline is None or masterNode is None: + raise RuntimeError("No master detected!") + + if len(slavePipelines) < 1: + raise RuntimeError("No slaves detected!") + + sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) + queue = sync.out.createOutputQueue() + + masterPipeline.start() + for k, v in slavePipelines.items(): + v.start() + + fpsCounter = FPSCounter() + + latestFrameGroup = None + firstReceived = False + startTime = datetime.datetime.now() + prevReceived = datetime.datetime.now() + + initialSyncTime = None + waitingForSync = True + + while running: + + for k, v in slaveQueues.items(): + for k2, v2 in v.items(): + while v2.has(): + inputQueues[f"slave_{k}_{k2}"].send(v2.get()) + + while queue.has(): + latestFrameGroup = queue.get() + if not firstReceived: + firstReceived = True + initialSyncTime = datetime.datetime.now() + prevReceived = datetime.datetime.now() + fpsCounter.tick() + + if not firstReceived: + endTime = datetime.datetime.now() + elapsedSec = (endTime - startTime).total_seconds() + if elapsedSec >= recvAllTimeoutSec: + print(f"Timeout: Didn't receive all frames in time: {elapsedSec:.2f} sec") + running = False + + # ------------------------------------------------------------------- + # Synchronise: we need at least one frame from every camera and their + # timestamps must align within SYNC_THRESHOLD_SEC. + # ------------------------------------------------------------------- + if latestFrameGroup is not None and latestFrameGroup.getNumMessages() == len(outputNames): + tsValues = {} + for name in outputNames: + tsValues[name] = latestFrameGroup[name].getTimestamp(dai.CameraExposureOffset.END).total_seconds() + # Build composite image side‑by‑side + imgs = [] + for name in camSockets: + imgs.append([]) + fps = fpsCounter.getFps() + + for outputName in outputNames: + idx = -1 + for i, name in enumerate(camSockets): + if name in outputName: + idx = i + break + if idx == -1: + raise RuntimeError(f"Could not find camera socket for {outputName}") + + msg = latestFrameGroup[outputName] + frame = msg.getCvFrame() + cv2.putText( + frame, + f"{outputName}", + (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (0, 127, 255), + 2, + cv2.LINE_AA, + ) + cv2.putText( + frame, + f"Timestamp: {tsValues[outputName]} | FPS:{fps:.2f}", + (20, 80), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 0, 50), + 2, + cv2.LINE_AA, + ) + imgs[idx].append(frame) + + delta = max(tsValues.values()) - min(tsValues.values()) + + syncStatus = abs(delta) < syncThresholdSec + syncStatusStr = "in sync" if syncStatus else "out of sync" + + if not syncStatus and waitingForSync: + endTime = datetime.datetime.now() + elapsedSec = (endTime - initialSyncTime).total_seconds() + if elapsedSec >= initialSyncTimeoutSec: + print("Timeout: Didn't sync frames in time") + running = False + + if syncStatus and waitingForSync: + print(f"Sync status: {syncStatusStr}") + waitingForSync = False + + if not syncStatus and not waitingForSync: + print(f"Sync error: Sync lost, threshold exceeded {delta * 1e6} us") + running = False + + color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + + for i, img in enumerate(imgs): + cv2.putText( + imgs[i][0], + f"{syncStatusStr} | delta = {delta*1e3:.3f} ms", + (20, 120), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + color, + 2, + cv2.LINE_AA, + ) + + for i, img in enumerate(imgs): + cv2.imshow(f"synced_view_{camSockets[i]}", cv2.hconcat(imgs[i])) + + latestFrameGroup = None # Wait for next batch + + if cv2.waitKey(1) & 0xFF == ord("q"): + break + +cv2.destroyAllWindows() diff --git a/include/depthai/common/ExternalFrameSyncRoles.hpp b/include/depthai/common/ExternalFrameSyncRoles.hpp new file mode 100644 index 0000000000..07cdfb0b49 --- /dev/null +++ b/include/depthai/common/ExternalFrameSyncRoles.hpp @@ -0,0 +1,45 @@ +#pragma once +#include +#include +#include + +namespace dai { +/** + * Which external frame sync role the device should have. + * + * AUTO_DETECT denotes that the decision will be made by device. + * It will choose between MASTER and SLAVE. + */ +enum class ExternalFrameSyncRole : int32_t { + // This role is used in setExternalFrameSyncRole() function. + // It signals to the device to automatically detect External frame sync configuration. + AUTO_DETECT = -1, + + // External frame sync master. + // Device generates frame sync signal and outputs it to other devices. + MASTER, + + // External frame sync slave. + // Device must lock onto an external frame sync signal. + SLAVE, +}; + +inline std::string toString(ExternalFrameSyncRole role) { + switch(role) { + case ExternalFrameSyncRole::AUTO_DETECT: + return "AUTO DETECT"; + case ExternalFrameSyncRole::MASTER: + return "MASTER"; + case ExternalFrameSyncRole::SLAVE: + return "SLAVE"; + default: + return "UNKNOWN"; + } +} + +} // namespace dai + +// Global namespace +inline std::ostream& operator<<(std::ostream& out, const dai::ExternalFrameSyncRole& socket) { + return out << dai::toString(socket); +} diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index 84a82522b1..ddc212a3ef 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -17,6 +17,7 @@ // project #include "depthai/common/CameraBoardSocket.hpp" #include "depthai/common/CameraFeatures.hpp" +#include "depthai/common/ExternalFrameSyncRoles.hpp" #include "depthai/common/UsbSpeed.hpp" #include "depthai/device/CalibrationHandler.hpp" #include "depthai/device/DeviceGate.hpp" @@ -804,6 +805,38 @@ class DeviceBase { */ void setMaxReconnectionAttempts(int maxAttempts, std::function callBack = nullptr); + /** + * Sets external frame sync role for the device + * @param role External frame sync role to be set, AUTO_DETECT by default + * @returns Tuple of bool and string. Bool specifies if role was set without failures. String is the error message describing the failure reason. + */ + std::tuple setExternalFrameSyncRole(ExternalFrameSyncRole role = ExternalFrameSyncRole::AUTO_DETECT); + + /** + * Gets external frame sync role for the device + * @returns Gets external frame sync role + */ + ExternalFrameSyncRole getExternalFrameSyncRole(); + + /** + * Sets the relative external strobe limits. + * Limits the strobe duty cycle, between 0 and 1, as a fraction of the whole period. 0 means always off, 1 means always on. + * The rising edge of the strobe signal is always synced to end of exposure. + * The falling edge of the strobe signal is then limited according to the min and max values. + * Default values are 0.005 and 0.995 + * @param min Minimum strobe value + * @param max Maximum strobe value + * @returns Tuple of bool and string. Bool specifies if role was set without failures. String is the error message describing the failure reason. + */ + std::tuple setExternalStrobeRelativeLimits(float min, float max); + + /** + * Set whether the external strobe should be enabled. + * External strobe signal is low for the duration of exposure, and high for the rest of the frame. + * @param enable Enables or disables strobe + */ + void setExternalStrobeEnable(bool enable); + protected: std::shared_ptr connection; diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index d3971a3e9c..a386dd9b20 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -296,8 +296,8 @@ class DeviceBase::Impl { * RPC call with custom timeout. Set timeout to 0 to enable endless wait. */ template - auto rpcCall(std::chrono::milliseconds timeout, std::string name, Args&&... args) -> decltype(rpcClient->call(std::string(name), - std::forward(args)...)) { + auto rpcCall(std::chrono::milliseconds timeout, std::string name, Args&&... args) + -> decltype(rpcClient->call(std::string(name), std::forward(args)...)) { ScopedRpcTimeout guard(timeout); return rpcClient->call(name, std::forward(args)...); } @@ -1246,6 +1246,22 @@ void DeviceBase::crashDevice() { } } +std::tuple DeviceBase::setExternalFrameSyncRole(ExternalFrameSyncRole role) { + return pimpl->rpcClient->call("setExternalFrameSyncRole", role); +} + +ExternalFrameSyncRole DeviceBase::getExternalFrameSyncRole() { + return pimpl->rpcClient->call("getExternalFrameSyncRole"); +} + +std::tuple DeviceBase::setExternalStrobeRelativeLimits(float min, float max) { + return pimpl->rpcClient->call("setExternalStrobeRelativeLimits", min, max); +} + +void DeviceBase::setExternalStrobeEnable(bool enable) { + pimpl->rpcCall("setExternalStrobeEnable", enable); +} + dai::Version DeviceBase::getIMUFirmwareVersion() { isClosed(); std::string versionStr = pimpl->rpcCall("getIMUFirmwareVersion").as(); @@ -1719,14 +1735,22 @@ bool DeviceBase::startPipelineImpl(const Pipeline& pipeline) { logCollection::logPipeline(schema, deviceInfo); this->pipelineSchema = schema; // Save the schema so it can be saved alongside the crashdump - // Build and start the pipeline bool success = false; std::string errorMsg; + + // Initialize the device (External frame sync slaves need to lock onto the signal first) + std::tie(success, errorMsg) = pimpl->rpcCall(std::chrono::seconds(60), "waitForDeviceReady").as>(); + + if(!success) { + throw std::runtime_error("Device " + getDeviceId() + " not ready: " + errorMsg); + } + + // Build and start the pipeline std::tie(success, errorMsg) = pimpl->rpcCall("buildPipeline").as>(); if(success) { pimpl->rpcCall("startPipeline"); } else { - throw std::runtime_error(errorMsg); + throw std::runtime_error("Device " + getDeviceId() + " error: " + errorMsg); return false; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e67af2211e..730ccb2fa2 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -638,6 +638,10 @@ dai_set_test_labels(sync_test ondevice rvc2_all rvc4 ci) dai_add_test(fsync_test src/ondevice_tests/pipeline/node/fsync_test.cpp) dai_set_test_labels(fsync_test ondevice rvc2 usb rvc4 ci) +# Fsync Multi-device test +dai_add_test(fsync_multi_device_test src/onhost_tests/pipeline/fsync_multi_device.cpp) +dai_set_test_labels(fsync_multi_device_test ondevice rvc4fsync ci) + ## Large messages tests dai_add_test(bridge_large_messages src/ondevice_tests/bridge_large_messages.cpp) # Only test on RVC4, as RVC2 is not yet supported to auto-increase the max stream capacity diff --git a/tests/run_tests.py b/tests/run_tests.py index dbb1d00902..7e7b80f79e 100644 --- a/tests/run_tests.py +++ b/tests/run_tests.py @@ -87,7 +87,13 @@ def compute_or_result(results): action="store_true", required=False, ) - + + parser.add_argument( + "--fsync", + action="store_true", + required=False, + ) + parser.add_argument( "--rvc4rgb", action="store_true", @@ -117,6 +123,11 @@ def compute_or_result(results): "env": {"DEPTHAI_PLATFORM": "rvc4"}, "labels": ["rvc4"], }, + { + "name": "RVC4 - Fsync", + "env": {"DEPTHAI_PLATFORM": "rvc4"}, + "labels": ["rvc4fsync"], + }, { "name": "RVC4 - RGB", "env": {"DEPTHAI_PLATFORM": "rvc4"}, @@ -138,7 +149,7 @@ def compute_or_result(results): resultThreads = [] # Filter configurations based on command-line arguments - if args.rvc4==args.rvc2==args.rvc4rgb: + if args.rvc4==args.rvc2==args.rvc4rgb==args.fsync: test_configs = [config for config in all_configs if "rvc2" in config.get("labels", []) or "rvc4" in config.get("labels", []) or "onhost" in config.get("labels", [])] elif args.rvc4: test_configs = [config for config in all_configs if "rvc4" in config.get("labels", [])] @@ -146,6 +157,8 @@ def compute_or_result(results): test_configs = [config for config in all_configs if "rvc2" in config.get("labels", []) or "onhost" in config.get("labels", [])] elif args.rvc4rgb: test_configs = [config for config in all_configs if "rvc4rgb" in config.get("labels", [])] + elif args.fsync: + test_configs = [config for config in all_configs if "rvc4fsync" in config.get("labels", [])] for config in test_configs: diff --git a/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp b/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp new file mode 100644 index 0000000000..d9367f1972 --- /dev/null +++ b/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp @@ -0,0 +1,308 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/ExternalFrameSyncRoles.hpp" +#include "depthai/depthai.hpp" +#include "depthai/pipeline/InputQueue.hpp" +#include "depthai/pipeline/MessageQueue.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/datatype/MessageGroup.hpp" +#include "depthai/pipeline/node/Sync.hpp" +#include "depthai/xlink/XLinkConnection.hpp" + +#define REQUIRE_MSG(x, msg) \ + if(!(x)) { \ + std::cout << "\x1B[1;31m" << msg << "\x1B[0m" << std::endl; \ + REQUIRE((x)); \ + } + +dai::Node::Output* createPipeline(std::shared_ptr pipeline, dai::CameraBoardSocket socket, float sensorFps, dai::ExternalFrameSyncRole role) { + std::shared_ptr cam; + if(role == dai::ExternalFrameSyncRole::MASTER) { + cam = pipeline->create()->build(socket, std::nullopt, sensorFps); + } else { + cam = pipeline->create()->build(socket, std::nullopt); + } + + auto output = cam->requestOutput(std::make_pair(640, 480), dai::ImgFrame::Type::NV12, dai::ImgResizeMode::STRETCH); + return output; +} + +std::shared_ptr createSyncNode(std::shared_ptr& masterPipeline, + std::map& masterNode, + std::chrono::nanoseconds syncThreshold, + std::vector& outputNames, + std::map>>& slaveQueues, + std::map>& inputQueues) { + auto sync = masterPipeline->create(); + sync->setRunOnHost(true); + sync->setSyncThreshold(syncThreshold); + for(auto p : masterNode) { + auto name = std::string("master_") + p.first; + p.second->link(sync->inputs[name]); + outputNames.push_back(name); + } + + for(auto& p : slaveQueues) { + for(auto& q : p.second) { + auto name = std::string("slave_") + p.first + "_" + q.first; + outputNames.push_back(name); + auto input_queue = sync->inputs[name].createInputQueue(); + inputQueues.emplace(name, input_queue); + } + } + + return sync; +} + +void setUpCameraSocket(std::shared_ptr& pipeline, + dai::CameraBoardSocket socket, + std::string& name, + float targetFps, + dai::ExternalFrameSyncRole role, + std::optional>& masterNode, + std::map>>& slaveQueues, + std::vector& camSockets) { + auto outNode = createPipeline(pipeline, socket, targetFps, role); + + if(role == dai::ExternalFrameSyncRole::MASTER) { + if(!masterNode.has_value()) { + masterNode.emplace(); + } + masterNode.value().emplace(dai::toString(socket), outNode); + + } else if(role == dai::ExternalFrameSyncRole::SLAVE) { + if(slaveQueues.find(name) == slaveQueues.end()) { + slaveQueues.emplace(name, std::map>()); + } + slaveQueues[name].emplace(dai::toString(socket), outNode->createOutputQueue()); + + } else { + throw std::runtime_error("Don't know how to handle role " + dai::toString(role)); + } + + if(std::find(camSockets.begin(), camSockets.end(), dai::toString(socket)) == camSockets.end()) { + camSockets.emplace_back(dai::toString(socket)); + } +} + +void setUpIrLeds(std::shared_ptr device) { + auto drivers = device->getIrDrivers(); + bool found = false; + + for(auto& driver : drivers) { + std::string name; + int bus; + int addr; + std::tie(name, bus, addr) = driver; + + if(name == "stm-dot") { + device->setIrLaserDotProjectorIntensity(0.1); + found = true; + } else if(name == "stm-flood") { + device->setIrFloodLightIntensity(0.1); + found = true; + } + } + + if(!found) { + std::cout << "No IR drivers found, skipping IR intensity setting" << std::endl; + } else { + std::cout << "IR intensity set" << std::endl; + } +} + +void setupDevice(dai::DeviceInfo& deviceInfo, + std::shared_ptr& masterPipeline, + std::optional>& masterNode, + std::map>& slavePipelines, + std::map>>& slaveQueues, + std::vector& camSockets, + std::vector& sockets, + float targetFps) { + auto pipeline = std::make_shared(std::make_shared(deviceInfo)); + auto device = pipeline->getDefaultDevice(); + + if(device->getPlatform() != dai::Platform::RVC4) { + throw std::runtime_error("This test supports only RVC4 platform!"); + } + + std::string name = deviceInfo.getXLinkDeviceDesc().name; + auto role = device->getExternalFrameSyncRole(); + + std::cout << "=== Connected to " << deviceInfo.getDeviceId() << std::endl; + std::cout << " Device ID: " << device->getDeviceId() << std::endl; + std::cout << " Num of cameras: " << device->getConnectedCameras().size() << std::endl; + + for(auto socket : device->getConnectedCameras()) { + if(std::find(sockets.begin(), sockets.end(), dai::toString(socket)) == sockets.end()) { + continue; + } + setUpCameraSocket(pipeline, socket, name, targetFps, role, masterNode, slaveQueues, camSockets); + } + + setUpIrLeds(device); + + if(role == dai::ExternalFrameSyncRole::MASTER) { + device->setExternalStrobeEnable(true); + std::cout << device->getDeviceId() << " is master" << std::endl; + + if(masterPipeline != nullptr) { + throw std::runtime_error("Only one master pipeline is supported"); + } + masterPipeline = pipeline; + } else if(role == dai::ExternalFrameSyncRole::SLAVE) { + slavePipelines[name] = pipeline; + std::cout << device->getDeviceId() << " is slave" << std::endl; + } else { + throw std::runtime_error("Don't know how to handle role " + dai::toString(role)); + } +} + +int testFsync( + float targetFps, double syncThresholdSec, uint64_t testDurationSec, int recvAllTimeoutSec, int initialSyncTimeoutSec, std::vector sockets) { + std::cout << "=================================\x1B[1;32mTest started\x1B[0m================================" << std::endl; + std::cout << "FPS: " << targetFps << std::endl; + std::cout << "SYNC_THRESHOLD_SEC: " << syncThresholdSec << std::endl; + std::cout << "RECV_ALL_TIMEOUT_SEC: " << recvAllTimeoutSec << std::endl; + std::cout << "INITIAL_SYNC_TIMEOUT_SEC: " << initialSyncTimeoutSec << std::endl; + std::cout << "SOCKETS: "; + for(auto s : sockets) { + std::cout << s << " "; + } + std::cout << std::endl; + + std::vector deviceInfos = dai::Device::getAllAvailableDevices(); + + REQUIRE_MSG(deviceInfos.size() >= 2, "At least two devices are required for this test."); + + std::shared_ptr masterPipeline; + std::optional> masterNode; + + std::map> slavePipelines; + std::map>> slaveQueues; + + std::map> inputQueues; + std::vector outputNames; + std::vector camSockets; + + for(auto deviceInfo : deviceInfos) { + setupDevice(deviceInfo, masterPipeline, masterNode, slavePipelines, slaveQueues, camSockets, sockets, targetFps); + } + + if(masterPipeline == nullptr || !masterNode.has_value()) { + throw std::runtime_error("No master detected!"); + } + if(slavePipelines.size() < 1) { + throw std::runtime_error("No slaves detected!"); + } + + auto sync = + createSyncNode(masterPipeline, *masterNode, std::chrono::nanoseconds(long(round(1e9 * 0.5f / targetFps))), outputNames, slaveQueues, inputQueues); + auto queue = sync->out.createOutputQueue(); + + masterPipeline->start(); + for(auto p : slavePipelines) { + p.second->start(); + } + + std::optional> latestFrameGroup; + bool firstReceived = false; + auto startTime = std::chrono::steady_clock::now(); + auto prevReceived = std::chrono::steady_clock::now(); + + std::optional> initialSyncTime; + + bool waitingForInitialSync = true; + + while(true) { + for(auto p : slaveQueues) { + for(auto q : p.second) { + while(q.second->has()) { + inputQueues[std::string("slave_") + p.first + "_" + q.first]->send(q.second->get()); + } + } + } + + while(queue->has()) { + auto syncData = queue->get(); + REQUIRE_MSG(syncData != nullptr, "Sync node failed to receive message"); + latestFrameGroup = std::dynamic_pointer_cast(syncData); + if(!firstReceived) { + firstReceived = true; + initialSyncTime = std::chrono::steady_clock::now(); + } + prevReceived = std::chrono::steady_clock::now(); + } + + if(!firstReceived) { + auto endTime = std::chrono::steady_clock::now(); + auto elapsedSec = std::chrono::duration_cast(endTime - startTime).count(); + REQUIRE_MSG(elapsedSec < recvAllTimeoutSec, "Timeout: Didn't receive all frames in time"); + } + + auto totalElapsedSec = std::chrono::duration_cast(std::chrono::steady_clock::now() - startTime).count(); + + if(totalElapsedSec >= testDurationSec) { + std::cout << "Timeout: Test finished after " << totalElapsedSec << " sec" << std::endl; + break; + } + + if(latestFrameGroup.has_value()) { + REQUIRE_MSG(size_t(latestFrameGroup.value()->getNumMessages()) == outputNames.size(), + "Number of messages received doesn't match number of outputs"); + + using ts_type = std::chrono::time_point; + std::map tsValues; + for(auto name : outputNames) { + auto frame = latestFrameGroup.value()->get(name); + REQUIRE_MSG(frame != nullptr, "Frame pointer is null"); + tsValues.emplace(name, frame->getTimestamp(dai::CameraExposureOffset::END)); + } + + auto compFunct = [](const std::pair& p1, const std::pair& p2) -> bool { return p1.second < p2.second; }; + + auto maxElement = std::max_element(tsValues.begin(), tsValues.end(), compFunct); + auto minElement = std::min_element(tsValues.begin(), tsValues.end(), compFunct); + + auto delta = maxElement->second - minElement->second; + auto deltaUs = std::chrono::duration_cast(delta).count(); + + bool syncStatus = abs(deltaUs) < syncThresholdSec * 1e6; + + if(!syncStatus && waitingForInitialSync) { + auto endTime = std::chrono::steady_clock::now(); + auto elapsedSec = std::chrono::duration_cast(endTime - initialSyncTime.value()).count(); + REQUIRE_MSG(elapsedSec < initialSyncTimeoutSec, "Timeout: Didn't sync frames in time"); + } + + if(syncStatus && waitingForInitialSync) { + std::cout << "Sync status: in sync" << std::endl; + waitingForInitialSync = false; + } + + REQUIRE_MSG(waitingForInitialSync || syncStatus, "Sync error: Sync lost, threshold exceeded: " << deltaUs << " us"); + + latestFrameGroup.reset(); + } + } + + return 0; +} + +TEST_CASE("Test Multi-device Fsync with different FPS values", "[fsync]") { + // Specify a list of FPS values to test with. + // auto fps = GENERATE(10.0f, 13.0f, 18.5f, 30.0f, 60.0f, 120.0f, 240.0f, 300.0f, 600.0f); + auto fps = GENERATE(10.0f, 13.0f, 18.5f, 30.0f, 60.0f); + CAPTURE(fps); + testFsync(fps, 1e-3, 60, 10, 4, {"CAM_B"}); +} From 24efd6bc98edf09a9ce1f53cf06777342e773bbd Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 28 Jan 2026 09:58:02 +0100 Subject: [PATCH 076/180] Formatting --- bindings/python/src/DeviceBindings.cpp | 12 ++++-------- bindings/python/src/pipeline/CommonBindings.cpp | 4 ++-- src/device/DeviceBase.cpp | 4 ++-- .../src/onhost_tests/pipeline/fsync_multi_device.cpp | 2 +- 4 files changed, 9 insertions(+), 13 deletions(-) diff --git a/bindings/python/src/DeviceBindings.cpp b/bindings/python/src/DeviceBindings.cpp index c48aa50235..3d095b7cc1 100644 --- a/bindings/python/src/DeviceBindings.cpp +++ b/bindings/python/src/DeviceBindings.cpp @@ -852,16 +852,14 @@ void DeviceBindings::bind(pybind11::module& m, void* pCallstack) { return d.setExternalFrameSyncRole(role); }, py::arg("role"), - DOC(dai, DeviceBase, setExternalFrameSyncRole) - ) + DOC(dai, DeviceBase, setExternalFrameSyncRole)) .def( "getExternalFrameSyncRole", [](DeviceBase& d) { py::gil_scoped_release release; return d.getExternalFrameSyncRole(); }, - DOC(dai, DeviceBase, getExternalFrameSyncRole) - ) + DOC(dai, DeviceBase, getExternalFrameSyncRole)) .def( "setExternalStrobeRelativeLimits", [](DeviceBase& d, float min, float max) { @@ -870,8 +868,7 @@ void DeviceBindings::bind(pybind11::module& m, void* pCallstack) { }, py::arg("min"), py::arg("max"), - DOC(dai, DeviceBase, setExternalStrobeRelativeLimits) - ) + DOC(dai, DeviceBase, setExternalStrobeRelativeLimits)) .def( "setExternalStrobeEnable", [](DeviceBase& d, bool enable) { @@ -879,8 +876,7 @@ void DeviceBindings::bind(pybind11::module& m, void* pCallstack) { d.setExternalStrobeEnable(enable); }, py::arg("enable"), - DOC(dai, DeviceBase, setExternalStrobeEnable) - ) + DOC(dai, DeviceBase, setExternalStrobeEnable)) .def( "getDeviceName", [](DeviceBase& d) { diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 19470bf3cd..31fe3c31ec 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -7,7 +7,6 @@ // depthai/ #include "depthai/common/CameraBoardSocket.hpp" -#include "depthai/common/ExternalFrameSyncRoles.hpp" #include "depthai/common/CameraFeatures.hpp" #include "depthai/common/CameraImageOrientation.hpp" #include "depthai/common/CameraSensorType.hpp" @@ -22,6 +21,7 @@ #include "depthai/common/DetectionParserOptions.hpp" #include "depthai/common/DeviceModelZoo.hpp" #include "depthai/common/EepromData.hpp" +#include "depthai/common/ExternalFrameSyncRoles.hpp" #include "depthai/common/FrameEvent.hpp" #include "depthai/common/HousingCoordinateSystem.hpp" #include "depthai/common/Interpolation.hpp" @@ -415,7 +415,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .value("VESA_I", HousingCoordinateSystem::VESA_I) .value("VESA_J", HousingCoordinateSystem::VESA_J) .value("IMU", HousingCoordinateSystem::IMU); - + // ExternalFrameSyncRole enum bindings externalFrameSyncRole.value("AUTO_DETECT", ExternalFrameSyncRole::AUTO_DETECT) .value("MASTER", ExternalFrameSyncRole::MASTER) diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index a386dd9b20..67e23b077f 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -296,8 +296,8 @@ class DeviceBase::Impl { * RPC call with custom timeout. Set timeout to 0 to enable endless wait. */ template - auto rpcCall(std::chrono::milliseconds timeout, std::string name, Args&&... args) - -> decltype(rpcClient->call(std::string(name), std::forward(args)...)) { + auto rpcCall(std::chrono::milliseconds timeout, std::string name, Args&&... args) -> decltype(rpcClient->call(std::string(name), + std::forward(args)...)) { ScopedRpcTimeout guard(timeout); return rpcClient->call(name, std::forward(args)...); } diff --git a/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp b/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp index d9367f1972..b4a67fe009 100644 --- a/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp +++ b/tests/src/onhost_tests/pipeline/fsync_multi_device.cpp @@ -135,7 +135,7 @@ void setupDevice(dai::DeviceInfo& deviceInfo, if(device->getPlatform() != dai::Platform::RVC4) { throw std::runtime_error("This test supports only RVC4 platform!"); } - + std::string name = deviceInfo.getXLinkDeviceDesc().name; auto role = device->getExternalFrameSyncRole(); From cddce5cd94c574c8fef3392cd76624cc67ae95aa Mon Sep 17 00:00:00 2001 From: lnotspotl Date: Wed, 28 Jan 2026 11:03:58 +0100 Subject: [PATCH 077/180] add RGBDData protobuf message definition and serialization functions (#1566) * add RGBDData protobuf message definition and serialization functions * use populateImgFrameProto when serializing ImgFrame and RGBDData messages * include EncodedFrame alongside ImgFrame in the RGBDData message * address copilot's comments * remove redundancies --- examples/cpp/RGBD/rgbd.cpp | 4 +- .../depthai/pipeline/datatype/RGBDData.hpp | 51 +++- protos/RGBDData.proto | 25 ++ src/pipeline/datatype/RGBDData.cpp | 63 ++++- src/utility/ProtoSerialize.cpp | 256 +++++++++++++++--- src/utility/ProtoSerialize.hpp | 5 + tests/src/ondevice_tests/rgbd_test.cpp | 16 +- 7 files changed, 355 insertions(+), 65 deletions(-) create mode 100644 protos/RGBDData.proto diff --git a/examples/cpp/RGBD/rgbd.cpp b/examples/cpp/RGBD/rgbd.cpp index f8879ebfa3..d21f5cb75a 100644 --- a/examples/cpp/RGBD/rgbd.cpp +++ b/examples/cpp/RGBD/rgbd.cpp @@ -30,7 +30,9 @@ class RerunNode : public dai::NodeCRTP { colors.emplace_back(pclData[i].r, pclData[i].g, pclData[i].b); } rec.log("world/obstacle_pcl", rerun::Points3D(points).with_colors(colors).with_radii({0.01f})); - auto colorFrame = rgbdIn->getRGBFrame()->getCvFrame(); + auto rgbFrame = rgbdIn->getRGBFrame(); + if(!rgbFrame.has_value()) continue; + auto colorFrame = std::get>(rgbFrame.value())->getCvFrame(); cv::cvtColor(colorFrame, colorFrame, cv::COLOR_BGR2RGB); rec.log("rgb", rerun::Image(reinterpret_cast(colorFrame.data), diff --git a/include/depthai/pipeline/datatype/RGBDData.hpp b/include/depthai/pipeline/datatype/RGBDData.hpp index 2e426f632c..bc17ae3a35 100644 --- a/include/depthai/pipeline/datatype/RGBDData.hpp +++ b/include/depthai/pipeline/datatype/RGBDData.hpp @@ -1,36 +1,69 @@ #pragma once +#include +#include + #include "depthai/common/ADatatypeSharedPtrSerialization.hpp" -#include "depthai/pipeline/datatype/ADatatype.hpp" +#include "depthai/common/optional.hpp" +#include "depthai/common/variant.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" +#include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/utility/ProtoSerializable.hpp" #include "depthai/utility/Serialization.hpp" namespace dai { /** * RGBD message. Carries RGB and Depth frames. + * Frames can be either of type ImgFrame or EncodedFrame. */ -class RGBDData : public Buffer { +class RGBDData : public Buffer, public ProtoSerializable { public: + using FrameVariant = std::variant, std::shared_ptr>; + /** * Construct RGBD message. */ RGBDData() = default; - virtual ~RGBDData(); + ~RGBDData() override; + + // Setters + void setRGBFrame(const FrameVariant& frame); + void setDepthFrame(const FrameVariant& frame); - std::map> frames; - void setRGBFrame(const std::shared_ptr& frame); - void setDepthFrame(const std::shared_ptr& frame); - std::shared_ptr getRGBFrame(); - std::shared_ptr getDepthFrame(); + // Getters: return std::nullopt if no frame is set, otherwise a valid FrameVariant is returned (it's not a nullptr) + std::optional getRGBFrame() const; + std::optional getDepthFrame() const; + private: + std::shared_ptr colorFrame; // nullptr if not set; ImgFrame or EncodedFrame are both ADatatype + std::shared_ptr depthFrame; // nullptr if not set; ImgFrame or EncodedFrame are both ADatatype + + public: void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; DatatypeEnum getDatatype() const override { return DatatypeEnum::RGBDData; } - DEPTHAI_SERIALIZE(RGBDData, frames, Buffer::ts, Buffer::tsDevice, Buffer::sequenceNum); + +#ifdef DEPTHAI_ENABLE_PROTOBUF + /** + * Serialize message to proto buffer + * + * @returns serialized message + */ + std::vector serializeProto(bool metadataOnly = false) const override; + + /** + * Serialize schema to proto buffer + * + * @returns serialized schema + */ + ProtoSerializable::SchemaPair serializeSchema() const override; +#endif + + DEPTHAI_SERIALIZE(RGBDData, colorFrame, depthFrame, Buffer::ts, Buffer::tsDevice, Buffer::sequenceNum); }; } // namespace dai diff --git a/protos/RGBDData.proto b/protos/RGBDData.proto new file mode 100644 index 0000000000..640c3aff0f --- /dev/null +++ b/protos/RGBDData.proto @@ -0,0 +1,25 @@ +syntax = "proto3"; + +package dai.proto.rgbd_data; + +import "ImgFrame.proto"; +import "EncodedFrame.proto"; +import "common.proto"; + +message RGBDData { + common.Timestamp ts = 1; + common.Timestamp tsDevice = 2; + int64 sequenceNum = 3; + + // Color frame can be either ImgFrame or EncodedFrame + oneof color_frame { + img_frame.ImgFrame colorImgFrame = 4; + encoded_frame.EncodedFrame colorEncodedFrame = 5; + } + + // Depth frame can be either ImgFrame or EncodedFrame + oneof depth_frame { + img_frame.ImgFrame depthImgFrame = 6; + encoded_frame.EncodedFrame depthEncodedFrame = 7; + } +} diff --git a/src/pipeline/datatype/RGBDData.cpp b/src/pipeline/datatype/RGBDData.cpp index c9e5e0f4e7..85a0069cf2 100644 --- a/src/pipeline/datatype/RGBDData.cpp +++ b/src/pipeline/datatype/RGBDData.cpp @@ -1,5 +1,11 @@ #include "depthai/pipeline/datatype/RGBDData.hpp" +#ifdef DEPTHAI_ENABLE_PROTOBUF + #include "depthai/schemas/RGBDData.pb.h" + #include "utility/ProtoSerialize.hpp" +#endif + +#include namespace dai { RGBDData::~RGBDData() = default; @@ -9,19 +15,60 @@ void RGBDData::serialize(std::vector& metadata, DatatypeEnum& data datatype = DatatypeEnum::RGBDData; } -void RGBDData::setRGBFrame(const std::shared_ptr& frame) { - frames["rgb"] = frame; +// Setters +void RGBDData::setRGBFrame(const FrameVariant& frame) { + // Unpack the variant and assign the contained pointer to colorFrame + std::visit([&](auto&& arg) { this->colorFrame = arg; }, frame); +} + +void RGBDData::setDepthFrame(const FrameVariant& frame) { + // Unpack the variant and assign the contained pointer to depthFrame + std::visit([&](auto&& arg) { this->depthFrame = arg; }, frame); +} + +// Getters +std::optional RGBDData::getRGBFrame() const { + if(!colorFrame) { + // color frame is not set and we don't know what type it is, so we return std::nullopt + // (we don't want to commit to a specific pointer type) + return std::nullopt; + } + + auto type = colorFrame->getDatatype(); + if(type == DatatypeEnum::ImgFrame) { + return std::dynamic_pointer_cast(colorFrame); + } + if(type == DatatypeEnum::EncodedFrame) { + return std::dynamic_pointer_cast(colorFrame); + } + throw std::runtime_error("Unhandled frame type in RGBDData color frame variant"); } -void RGBDData::setDepthFrame(const std::shared_ptr& frame) { - frames["depth"] = frame; +std::optional RGBDData::getDepthFrame() const { + if(!depthFrame) { + // depth frame is not set and we don't know what type it is, so we return std::nullopt + // (we don't want to commit to a specific pointer type) + return std::nullopt; + } + + auto type = depthFrame->getDatatype(); + if(type == DatatypeEnum::ImgFrame) { + return std::dynamic_pointer_cast(depthFrame); + } + if(type == DatatypeEnum::EncodedFrame) { + return std::dynamic_pointer_cast(depthFrame); + } + throw std::runtime_error("Unhandled frame type in RGBDData depth frame variant"); } -std::shared_ptr RGBDData::getRGBFrame() { - return std::dynamic_pointer_cast(frames["rgb"]); +#ifdef DEPTHAI_ENABLE_PROTOBUF +std::vector RGBDData::serializeProto(bool metadataOnly) const { + return utility::serializeProto(utility::getProtoMessage(this, metadataOnly)); } -std::shared_ptr RGBDData::getDepthFrame() { - return std::dynamic_pointer_cast(frames["depth"]); +ProtoSerializable::SchemaPair RGBDData::serializeSchema() const { + return utility::serializeSchema(utility::getProtoMessage(this)); } +#endif + } // namespace dai diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index 89278b588b..b38d1da85c 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -8,11 +8,14 @@ #include #include #include +#include #include "depthai/schemas/PointCloudData.pb.h" +#include "depthai/schemas/RGBDData.pb.h" #include "depthai/schemas/common.pb.h" #include "pipeline/datatype/DatatypeEnum.hpp" #include "pipeline/datatype/ImgDetections.hpp" +#include "pipeline/datatype/RGBDData.hpp" namespace dai { namespace utility { @@ -129,6 +132,8 @@ DatatypeEnum schemaNameToDatatype(const std::string& schemaName) { return DatatypeEnum::PointCloudData; } else if(schemaName == proto::spatial_img_detections::SpatialImgDetections::descriptor()->full_name()) { return DatatypeEnum::SpatialImgDetections; + } else if(schemaName == proto::rgbd_data::RGBDData::descriptor()->full_name()) { + return DatatypeEnum::RGBDData; } else { throw std::runtime_error("Unknown schema name: " + schemaName); } @@ -140,6 +145,7 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::EncodedFrame: case DatatypeEnum::IMUData: case DatatypeEnum::PointCloudData: + case DatatypeEnum::RGBDData: return true; case DatatypeEnum::ADatatype: case DatatypeEnum::Buffer: @@ -171,7 +177,6 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::ImgAnnotations: case DatatypeEnum::ImageFiltersConfig: case DatatypeEnum::ToFDepthConfidenceFilterConfig: - case DatatypeEnum::RGBDData: case DatatypeEnum::ObjectTrackerConfig: case DatatypeEnum::VppConfig: case DatatypeEnum::DynamicCalibrationControl: @@ -516,11 +521,8 @@ std::unique_ptr getProtoMessage(const ImgDetections* return imgDetections; } -template <> -std::unique_ptr getProtoMessage(const EncodedFrame* message, bool metadataOnly) { - // Create a unique pointer to the protobuf EncodedFrame message - auto encodedFrame = std::make_unique(); - +// Helper function to populate an EncodedFrame proto from an EncodedFrame object +static void populateEncodedFrameToProto(proto::encoded_frame::EncodedFrame* encodedFrame, const EncodedFrame* message, bool metadataOnly) { // Populate the protobuf message fields with the EncodedFrame data encodedFrame->set_instancenum(message->instanceNum); // instanceNum -> instancenum encodedFrame->set_width(message->width); @@ -558,14 +560,17 @@ std::unique_ptr getProtoMessage(const EncodedFrame* m proto::common::ImgTransformation* imgTransformation = encodedFrame->mutable_transformation(); utility::serializeImgTransformation(imgTransformation, message->transformation); +} - // Return the populated protobuf message +template <> +std::unique_ptr getProtoMessage(const EncodedFrame* message, bool metadataOnly) { + auto encodedFrame = std::make_unique(); + populateEncodedFrameToProto(encodedFrame.get(), message, metadataOnly); return encodedFrame; } -template <> -std::unique_ptr getProtoMessage(const ImgFrame* message, bool metadataOnly) { - // create and populate ImgFrame protobuf message - auto imgFrame = std::make_unique(); + +// Helper function to populate an ImgFrame proto from an ImgFrame object +static void populateImgFrameToProto(proto::img_frame::ImgFrame* imgFrame, const ImgFrame* message, bool metadataOnly) { proto::common::Timestamp* ts = imgFrame->mutable_ts(); ts->set_sec(message->ts.sec); ts->set_nsec(message->ts.nsec); @@ -575,6 +580,7 @@ std::unique_ptr getProtoMessage(const ImgFrame* messa imgFrame->set_sequencenum(message->sequenceNum); + // frame buffer info proto::img_frame::Specs* fb = imgFrame->mutable_fb(); fb->set_type(static_cast(message->fb.type)); fb->set_width(message->fb.width); @@ -585,6 +591,7 @@ std::unique_ptr getProtoMessage(const ImgFrame* messa fb->set_p2offset(message->fb.p2Offset); fb->set_p3offset(message->fb.p3Offset); + // source frame buffer info proto::img_frame::Specs* sourceFb = imgFrame->mutable_sourcefb(); sourceFb->set_type(static_cast(message->sourceFb.type)); sourceFb->set_width(message->sourceFb.width); @@ -595,6 +602,7 @@ std::unique_ptr getProtoMessage(const ImgFrame* messa sourceFb->set_p2offset(message->sourceFb.p2Offset); sourceFb->set_p3offset(message->sourceFb.p3Offset); + // camera settings proto::common::CameraSettings* cam = imgFrame->mutable_cam(); cam->set_exposuretimeus(message->cam.exposureTimeUs); cam->set_sensitivityiso(message->cam.sensitivityIso); @@ -602,8 +610,8 @@ std::unique_ptr getProtoMessage(const ImgFrame* messa cam->set_wbcolortemp(message->cam.wbColorTemp); cam->set_lenspositionraw(message->cam.lensPositionRaw); + // instance number and category imgFrame->set_instancenum(message->instanceNum); - imgFrame->set_category(message->category); proto::common::ImgTransformation* imgTransformation = imgFrame->mutable_transformation(); @@ -612,7 +620,12 @@ std::unique_ptr getProtoMessage(const ImgFrame* messa if(!metadataOnly) { imgFrame->set_data(message->data->getData().data(), message->data->getData().size()); } +} +template <> +std::unique_ptr getProtoMessage(const ImgFrame* message, bool metadataOnly) { + auto imgFrame = std::make_unique(); + populateImgFrameToProto(imgFrame.get(), message, metadataOnly); return imgFrame; } template <> @@ -647,6 +660,67 @@ std::unique_ptr getProtoMessage(const PointCloudData* return pointCloudData; } +template <> +std::unique_ptr getProtoMessage(const RGBDData* message, bool metadataOnly) { + auto rgbdData = std::make_unique(); + + // Set timestamps + auto timestamp = rgbdData->mutable_ts(); + timestamp->set_sec(message->ts.sec); + timestamp->set_nsec(message->ts.nsec); + + auto timestampDevice = rgbdData->mutable_tsdevice(); + timestampDevice->set_sec(message->tsDevice.sec); + timestampDevice->set_nsec(message->tsDevice.nsec); + + // Set sequence number + rgbdData->set_sequencenum(message->sequenceNum); + + // Serialize color frame if present (can be ImgFrame or EncodedFrame) + auto colorFrame = message->getRGBFrame(); + if(colorFrame.has_value()) { + auto handler = [&](auto&& framePtr) { + using T = std::decay_t; + if constexpr(std::is_same_v>) { + if(framePtr) { + populateImgFrameToProto(rgbdData->mutable_colorimgframe(), framePtr.get(), metadataOnly); + } + } else if constexpr(std::is_same_v>) { + if(framePtr) { + populateEncodedFrameToProto(rgbdData->mutable_colorencodedframe(), framePtr.get(), metadataOnly); + } + } else { + static_assert(sizeof(T*) == 0, "Unhandled frame type in RGBDData color frame variant"); + } + }; + + std::visit(handler, colorFrame.value()); + } + + // Serialize depth frame if present (can be ImgFrame or EncodedFrame) + auto depthFrame = message->getDepthFrame(); + if(depthFrame.has_value()) { + auto handler = [&](auto&& framePtr) { + using T = std::decay_t; + if constexpr(std::is_same_v>) { + if(framePtr) { + populateImgFrameToProto(rgbdData->mutable_depthimgframe(), framePtr.get(), metadataOnly); + } + } else if constexpr(std::is_same_v>) { + if(framePtr) { + populateEncodedFrameToProto(rgbdData->mutable_depthencodedframe(), framePtr.get(), metadataOnly); + } + } else { + static_assert(sizeof(T*) == 0, "Unhandled frame type in RGBDData depth frame variant"); + } + }; + + std::visit(handler, depthFrame.value()); + } + + return rgbdData; +} + // template <> // void setProtoMessage(ImgAnnotations* obj, std::shared_ptr msg, bool) { // } @@ -659,6 +733,9 @@ std::unique_ptr getProtoMessage(const PointCloudData* template <> void setProtoMessage(IMUData& obj, const google::protobuf::Message* msg, bool) { auto imuData = dynamic_cast(msg); + if(imuData == nullptr) { + throw std::runtime_error("Failed to cast protobuf message to IMUData"); + } obj.packets.clear(); obj.packets.reserve(imuData->packets().size()); for(auto packet : imuData->packets()) { @@ -770,50 +847,56 @@ void setProtoMessage(ImgFrame& obj, const google::protobuf::Message* msg, bool m obj.setData(data); } } -template <> -void setProtoMessage(EncodedFrame& obj, const google::protobuf::Message* msg, bool metadataOnly) { - auto encFrame = dynamic_cast(msg); - if(encFrame == nullptr) { - throw std::runtime_error("Failed to cast protobuf message to EncodedFrame"); - } + +// Helper function to populate an EncodedFrame object from an EncodedFrame proto +static void populateEncodedFrameFromProto(EncodedFrame& obj, const proto::encoded_frame::EncodedFrame& encFrame, bool metadataOnly) { const auto safeTimestamp = [](const auto& protoTs, bool hasField) { using steady_tp = std::chrono::time_point; return hasField ? utility::fromProtoTimestamp(protoTs) : steady_tp{}; }; - // create and populate ImgFrame protobuf message - obj.setTimestamp(safeTimestamp(encFrame->ts(), encFrame->has_ts())); - obj.setTimestampDevice(safeTimestamp(encFrame->tsdevice(), encFrame->has_tsdevice())); - obj.setSequenceNum(encFrame->sequencenum()); + obj.setTimestamp(safeTimestamp(encFrame.ts(), encFrame.has_ts())); + obj.setTimestampDevice(safeTimestamp(encFrame.tsdevice(), encFrame.has_tsdevice())); - obj.width = encFrame->width(); - obj.height = encFrame->height(); + obj.setSequenceNum(encFrame.sequencenum()); - obj.instanceNum = encFrame->instancenum(); + obj.width = encFrame.width(); + obj.height = encFrame.height(); - obj.quality = encFrame->quality(); - obj.bitrate = encFrame->bitrate(); - obj.profile = static_cast(encFrame->profile()); + obj.instanceNum = encFrame.instancenum(); - obj.lossless = encFrame->lossless(); - obj.type = static_cast(encFrame->type()); + obj.quality = encFrame.quality(); + obj.bitrate = encFrame.bitrate(); + obj.profile = static_cast(encFrame.profile()); - obj.frameOffset = encFrame->frameoffset(); - obj.frameSize = encFrame->framesize(); + obj.lossless = encFrame.lossless(); + obj.type = static_cast(encFrame.type()); - obj.cam.exposureTimeUs = encFrame->cam().exposuretimeus(); - obj.cam.sensitivityIso = encFrame->cam().sensitivityiso(); - obj.cam.lensPosition = encFrame->cam().lensposition(); - obj.cam.wbColorTemp = encFrame->cam().wbcolortemp(); - obj.cam.lensPositionRaw = encFrame->cam().lenspositionraw(); + obj.frameOffset = encFrame.frameoffset(); + obj.frameSize = encFrame.framesize(); - obj.transformation = deserializeImgTransformation(encFrame->transformation()); + obj.cam.exposureTimeUs = encFrame.cam().exposuretimeus(); + obj.cam.sensitivityIso = encFrame.cam().sensitivityiso(); + obj.cam.lensPosition = encFrame.cam().lensposition(); + obj.cam.wbColorTemp = encFrame.cam().wbcolortemp(); + obj.cam.lensPositionRaw = encFrame.cam().lenspositionraw(); + + obj.transformation = deserializeImgTransformation(encFrame.transformation()); if(!metadataOnly) { - std::vector data(encFrame->data().begin(), encFrame->data().end()); + std::vector data(encFrame.data().begin(), encFrame.data().end()); obj.setData(data); } } + +template <> +void setProtoMessage(EncodedFrame& obj, const google::protobuf::Message* msg, bool metadataOnly) { + auto encFrame = dynamic_cast(msg); + if(encFrame == nullptr) { + throw std::runtime_error("Failed to cast protobuf message to EncodedFrame"); + } + populateEncodedFrameFromProto(obj, *encFrame, metadataOnly); +} template <> void setProtoMessage(PointCloudData& obj, const google::protobuf::Message* msg, bool metadataOnly) { auto pcl = dynamic_cast(msg); @@ -850,5 +933,100 @@ void setProtoMessage(PointCloudData& obj, const google::protobuf::Message* msg, } } +// Helper function to populate an ImgFrame object from an ImgFrame proto +static void populateImgFrameFromProto(ImgFrame& obj, const proto::img_frame::ImgFrame& imgFrame, bool metadataOnly) { + obj.setTimestamp(utility::fromProtoTimestamp(imgFrame.ts())); + obj.setTimestampDevice(utility::fromProtoTimestamp(imgFrame.tsdevice())); + + obj.setSequenceNum(imgFrame.sequencenum()); + + // frame buffer info + obj.fb.type = static_cast(imgFrame.fb().type()); + obj.fb.width = imgFrame.fb().width(); + obj.fb.height = imgFrame.fb().height(); + obj.fb.stride = imgFrame.fb().stride(); + obj.fb.bytesPP = imgFrame.fb().bytespp(); + obj.fb.p1Offset = imgFrame.fb().p1offset(); + obj.fb.p2Offset = imgFrame.fb().p2offset(); + obj.fb.p3Offset = imgFrame.fb().p3offset(); + + // source frame buffer info + obj.sourceFb.type = static_cast(imgFrame.sourcefb().type()); + obj.sourceFb.width = imgFrame.sourcefb().width(); + obj.sourceFb.height = imgFrame.sourcefb().height(); + obj.sourceFb.stride = imgFrame.sourcefb().stride(); + obj.sourceFb.bytesPP = imgFrame.sourcefb().bytespp(); + obj.sourceFb.p1Offset = imgFrame.sourcefb().p1offset(); + obj.sourceFb.p2Offset = imgFrame.sourcefb().p2offset(); + obj.sourceFb.p3Offset = imgFrame.sourcefb().p3offset(); + + // camera settings + obj.cam.exposureTimeUs = imgFrame.cam().exposuretimeus(); + obj.cam.sensitivityIso = imgFrame.cam().sensitivityiso(); + obj.cam.lensPosition = imgFrame.cam().lensposition(); + obj.cam.wbColorTemp = imgFrame.cam().wbcolortemp(); + obj.cam.lensPositionRaw = imgFrame.cam().lenspositionraw(); + + // instance number and category + obj.instanceNum = imgFrame.instancenum(); + obj.category = imgFrame.category(); + + // transformation + obj.transformation = deserializeImgTransformation(imgFrame.transformation()); + + if(!metadataOnly) { + std::vector data(imgFrame.data().begin(), imgFrame.data().end()); + obj.setData(data); + } +} + +template <> +void setProtoMessage(RGBDData& obj, const google::protobuf::Message* msg, bool metadataOnly) { + auto rgbdData = dynamic_cast(msg); + if(rgbdData == nullptr) { + throw std::runtime_error("Failed to cast protobuf message to RGBDData"); + } + + obj.setTimestamp(utility::fromProtoTimestamp(rgbdData->ts())); + obj.setTimestampDevice(utility::fromProtoTimestamp(rgbdData->tsdevice())); + obj.setSequenceNum(rgbdData->sequencenum()); + + // Deserialize color frame if present (can be ImgFrame or EncodedFrame) + switch(rgbdData->color_frame_case()) { + case proto::rgbd_data::RGBDData::kColorImgFrame: { + auto colorFrame = std::make_shared(); + populateImgFrameFromProto(*colorFrame, rgbdData->colorimgframe(), metadataOnly); + obj.setRGBFrame(colorFrame); + break; + } + case proto::rgbd_data::RGBDData::kColorEncodedFrame: { + auto colorFrame = std::make_shared(); + populateEncodedFrameFromProto(*colorFrame, rgbdData->colorencodedframe(), metadataOnly); + obj.setRGBFrame(colorFrame); + break; + } + case proto::rgbd_data::RGBDData::COLOR_FRAME_NOT_SET: + break; + } + + // Deserialize depth frame if present (can be ImgFrame or EncodedFrame) + switch(rgbdData->depth_frame_case()) { + case proto::rgbd_data::RGBDData::kDepthImgFrame: { + auto depthFrame = std::make_shared(); + populateImgFrameFromProto(*depthFrame, rgbdData->depthimgframe(), metadataOnly); + obj.setDepthFrame(depthFrame); + break; + } + case proto::rgbd_data::RGBDData::kDepthEncodedFrame: { + auto depthFrame = std::make_shared(); + populateEncodedFrameFromProto(*depthFrame, rgbdData->depthencodedframe(), metadataOnly); + obj.setDepthFrame(depthFrame); + break; + } + case proto::rgbd_data::RGBDData::DEPTH_FRAME_NOT_SET: + break; + } +} + }; // namespace utility }; // namespace dai diff --git a/src/utility/ProtoSerialize.hpp b/src/utility/ProtoSerialize.hpp index db96bd1018..be89e28425 100644 --- a/src/utility/ProtoSerialize.hpp +++ b/src/utility/ProtoSerialize.hpp @@ -16,6 +16,7 @@ #include "depthai/schemas/ImgDetections.pb.h" #include "depthai/schemas/ImgFrame.pb.h" #include "depthai/schemas/PointCloudData.pb.h" +#include "depthai/schemas/RGBDData.pb.h" #include "depthai/schemas/SpatialImgDetections.pb.h" #include "depthai/schemas/common.pb.h" #include "utility/ProtoSerializable.hpp" @@ -59,6 +60,8 @@ template <> std::unique_ptr getProtoMessage(const ImgFrame* message, bool metadataOnly); template <> std::unique_ptr getProtoMessage(const PointCloudData* message, bool metadataOnly); +template <> +std::unique_ptr getProtoMessage(const RGBDData* message, bool metadataOnly); // Helpers to deserialize messages from protobuf template @@ -77,6 +80,8 @@ template <> void setProtoMessage(EncodedFrame& obj, const google::protobuf::Message* msg, bool metadataOnly); template <> void setProtoMessage(PointCloudData& obj, const google::protobuf::Message* msg, bool metadataOnly); +template <> +void setProtoMessage(RGBDData& obj, const google::protobuf::Message* msg, bool metadataOnly); }; // namespace utility }; // namespace dai diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp index 1161638e6a..dbef26b153 100644 --- a/tests/src/ondevice_tests/rgbd_test.cpp +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -53,11 +53,11 @@ TEST_CASE("basic rgbd") { REQUIRE(pcl->getMinZ() <= pcl->getMaxZ()); auto rgbdData = rgbdQ->get(); auto rgbFrame = rgbdData->getRGBFrame(); - REQUIRE(rgbFrame != nullptr); - REQUIRE(rgbFrame->getData().size() == 1280UL * 800UL * 3UL); + REQUIRE(rgbFrame.has_value()); + REQUIRE(std::get>(rgbFrame.value())->getData().size() == 1280UL * 800UL * 3UL); auto depthFrame = rgbdData->getDepthFrame(); - REQUIRE(depthFrame != nullptr); - REQUIRE(depthFrame->getData().size() == 1280UL * 800UL * 2UL); + REQUIRE(depthFrame.has_value()); + REQUIRE(std::get>(depthFrame.value())->getData().size() == 1280UL * 800UL * 2UL); } } TEST_CASE("rgbd autocreate") { @@ -80,10 +80,10 @@ TEST_CASE("rgbd autocreate") { REQUIRE(pcl->getMinZ() <= pcl->getMaxZ()); auto rgbdData = rgbdQ->get(); auto rgbFrame = rgbdData->getRGBFrame(); - REQUIRE(rgbFrame != nullptr); - REQUIRE(rgbFrame->getData().size() == 640UL * 400UL * 3UL); + REQUIRE(rgbFrame.has_value()); + REQUIRE(std::get>(rgbFrame.value())->getData().size() == 640UL * 400UL * 3UL); auto depthFrame = rgbdData->getDepthFrame(); - REQUIRE(depthFrame != nullptr); - REQUIRE(depthFrame->getData().size() == 640UL * 400UL * 2UL); + REQUIRE(depthFrame.has_value()); + REQUIRE(std::get>(depthFrame.value())->getData().size() == 640UL * 400UL * 2UL); } } From 65e0838d4c28329c7ad2fd18e15a804e39f4ca2d Mon Sep 17 00:00:00 2001 From: AljazD Date: Wed, 28 Jan 2026 13:13:59 +0100 Subject: [PATCH 078/180] Cached snaps and events are now gradually uploaded during runtime, minor fixes --- include/depthai/utility/EventsManager.hpp | 13 ++- src/utility/EventsManager.cpp | 101 ++++++++++++++++------ 2 files changed, 82 insertions(+), 32 deletions(-) diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 186cf6c054..75b072b861 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -193,9 +193,10 @@ class EventsManager { /** * Fetch the configuration limits and quotas for snaps & events + * @param retryOnFail Retry fetching on failure * @return bool */ - bool fetchConfigurationLimits(); + bool fetchConfigurationLimits(const bool retryOnFail); /** * Prepare a batch of file groups from inputSnapBatch */ @@ -227,10 +228,13 @@ class EventsManager { */ void cacheSnapData(std::deque>& inputSnapBatch); /** - * Upload cached data to the events service - * @return void + * Upload cached events to the events service + */ + void uploadCachedEvents(); + /** + * Upload cached snaps to the events service */ - void uploadCachedData(); + void uploadCachedSnaps(); /** * Check if there's any cached data in the filesystem */ @@ -259,6 +263,7 @@ class EventsManager { std::mutex stopThreadConditionMutex; std::atomic stopUploadThread; std::atomic configurationLimitsFetched; + std::atomic connectionEstablished; std::condition_variable eventBufferCondition; uint64_t maxFileSizeBytes; diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index c867919e04..3c093cf29f 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -119,7 +119,8 @@ FileData::FileData(std::filesystem::path filePath, std::string fileName) : fileN {".gif", "image/gif"}, {".svg", "image/svg+xml"}, {".json", "application/json"}, - {".txt", "text/plain"}}; + {".txt", "text/plain"}, + {".annotation", "application/x-protobuf; proto=SnapAnnotation"}}; // Read the data std::ifstream fileStream(filePath, std::ios::binary | std::ios::ate); if(!fileStream) { @@ -141,6 +142,8 @@ FileData::FileData(std::filesystem::path filePath, std::string fileName) : fileN static const std::unordered_set imageMimeTypes = {"image/jpeg", "image/png", "image/webp", "image/bmp", "image/tiff"}; if(imageMimeTypes.find(mimeType) != imageMimeTypes.end()) { classification = proto::event::PrepareFileUploadClass::IMAGE_COLOR; + } else if(mimeType == "application/x-protobuf; proto=SnapAnnotation") { + classification = proto::event::PrepareFileUploadClass::ANNOTATION; } else { classification = proto::event::PrepareFileUploadClass::UNKNOWN_FILE; } @@ -217,7 +220,12 @@ bool FileData::toFile(const std::filesystem::path& inputPath) { logger::error("Filename is empty"); return false; } - std::string extension = mimeType == "image/jpeg" ? ".jpg" : ".txt"; + std::string extension; + if(mimeType == "image/jpeg") { + extension = ".jpg"; + } else if(mimeType == "application/x-protobuf; proto=SnapAnnotation") { + extension = ".annotation"; + } // Choose a unique filename std::filesystem::path target = inputPath / (fileName + extension); for(int i = 1; std::filesystem::exists(target); ++i) { @@ -262,18 +270,15 @@ EventsManager::EventsManager(bool uploadCachedOnStart) // Thread handling preparation and uploads uploadThread = std::make_unique([this]() { // Fetch configuration limits when starting the new thread - configurationLimitsFetched = fetchConfigurationLimits(); - auto currentTime = std::chrono::steady_clock::now(); - auto nextTime = currentTime + std::chrono::hours(1); + configurationLimitsFetched = fetchConfigurationLimits(true); while(!stopUploadThread) { - // Hourly check for fetching configuration and limits - currentTime = std::chrono::steady_clock::now(); - if(currentTime >= nextTime) { - fetchConfigurationLimits(); - nextTime += std::chrono::hours(1); - if(remainingStorageBytes <= warningStorageBytes) { - logger::warn("Current remaining storage is running low: {} MB", remainingStorageBytes / (1024 * 1024)); - } + connectionEstablished = fetchConfigurationLimits(false); + if(remainingStorageBytes <= warningStorageBytes) { + logger::warn("Current remaining storage is running low: {} MB", remainingStorageBytes / (1024 * 1024)); + } + // Add cached snaps (if any) to the snapBuffer + if(connectionEstablished) { + uploadCachedSnaps(); } // Prepare the batch first to reduce contention std::deque> snapBatch; @@ -301,10 +306,8 @@ EventsManager::EventsManager(bool uploadCachedOnStart) eventBufferCondition.wait_for(lock, std::chrono::seconds(static_cast(this->publishInterval)), [this]() { return stopUploadThread.load(); }); } }); - // Upload or clear previously cached data on start - if(uploadCachedOnStart) { - uploadCachedData(); - } else { + // If upload on start is not set, clear the previously cached data when starting + if(!uploadCachedOnStart) { clearCachedData(cacheDir); } } @@ -317,7 +320,7 @@ EventsManager::~EventsManager() { } } -bool EventsManager::fetchConfigurationLimits() { +bool EventsManager::fetchConfigurationLimits(const bool retryOnFail) { logger::info("Fetching configuration limits"); if(token.empty()) { logger::warn("Missing token, please set DEPTHAI_HUB_API_KEY environment variable or use the setToken method"); @@ -346,6 +349,10 @@ bool EventsManager::fetchConfigurationLimits() { })); if(response.status_code != cpr::status::HTTP_OK) { logger::error("Failed to fetch configuration limits, status code: {}", response.status_code); + // Fetching should be retried indefinetly only on startup + if(!retryOnFail) { + return false; + } // Apply exponential backoff auto factor = std::pow(uploadRetryPolicy.factor, ++retryAttempt); @@ -606,6 +613,11 @@ bool EventsManager::uploadFile(std::shared_ptr fileData, std::string u } void EventsManager::uploadEventBatch() { + // Add cached events, if any + if(connectionEstablished) { + uploadCachedEvents(); + } + auto eventBatch = std::make_unique(); { std::lock_guard lock(eventBufferMutex); @@ -893,8 +905,11 @@ void EventsManager::cacheEvents() { logger::info("Caching events"); std::lock_guard lock(eventBufferMutex); for(const auto& eventData : eventBuffer) { - std::filesystem::path path(cacheDir); - path = path / ("event_" + eventData->event->name() + "_" + std::to_string(eventData->event->created_at())); + std::filesystem::path inputPath(cacheDir); + std::filesystem::path path = inputPath / ("event_" + eventData->event->name() + "_" + std::to_string(eventData->event->created_at())); + for(int i = 1; std::filesystem::exists(path); ++i) { + path = inputPath / ("event_" + eventData->event->name() + "_" + std::to_string(eventData->event->created_at()) + "_" + std::to_string(i)); + } std::string eventDir = path.string(); logger::info("Caching event to {}", eventDir); if(!std::filesystem::exists(cacheDir)) { @@ -911,8 +926,12 @@ void EventsManager::cacheSnapData(std::deque>& inputSn // Create a unique directory and save the snapData logger::info("Caching snapData"); for(const auto& snap : inputSnapBatch) { - std::filesystem::path path(cacheDir); - path = path / ("snap_" + snap->eventData->event->name() + "_" + std::to_string(snap->eventData->event->created_at())); + std::filesystem::path inputPath(cacheDir); + std::filesystem::path path = inputPath / ("snap_" + snap->eventData->event->name() + "_" + std::to_string(snap->eventData->event->created_at())); + for(int i = 1; std::filesystem::exists(path); ++i) { + path = + inputPath / ("snap_" + snap->eventData->event->name() + "_" + std::to_string(snap->eventData->event->created_at()) + "_" + std::to_string(i)); + } std::string snapDir = path.string(); logger::info("Caching snap to {}", snapDir); if(!std::filesystem::exists(cacheDir)) { @@ -927,13 +946,12 @@ void EventsManager::cacheSnapData(std::deque>& inputSn } } -void EventsManager::uploadCachedData() { - // Iterate over the directories in cacheDir, add events and snapsData to buffers +void EventsManager::uploadCachedEvents() { + // Iterate over the directories in cacheDir, add events to eventBuffer if(!checkForCachedData()) { - logger::warn("Cache directory is empty, no cached data will be uploaded"); return; } - logger::info("Uploading cached data"); + logger::info("Uploading cached events"); for(const auto& entry : std::filesystem::directory_iterator(cacheDir)) { if(!entry.is_directory()) { @@ -945,13 +963,34 @@ void EventsManager::uploadCachedData() { auto eventData = std::make_unique(); auto event = std::make_shared(); event->ParseFromIstream(&eventFile); + + // Cached events should be added only if there is enough space in the upcoming request std::lock_guard lock(eventBufferMutex); + if(eventBuffer.size() >= eventsPerRequest) { + return; + } + eventData->event = event; eventBuffer.push_back(std::move(eventData)); // Clear entries added to the eventBuffer clearCachedData(entry.path()); + } + } +} + +void EventsManager::uploadCachedSnaps() { + // Iterate over the directories in cacheDir, add snaps to snapBuffer + if(!checkForCachedData()) { + return; + } + logger::info("Uploading cached snaps"); + + for(const auto& entry : std::filesystem::directory_iterator(cacheDir)) { + if(!entry.is_directory()) { + continue; + } - } else if(entry.path().filename().string().rfind("snap", 0) == 0) { + if(entry.path().filename().string().rfind("snap", 0) == 0) { std::ifstream snapFile(entry.path() / "snap.pb", std::ios::binary); auto snapData = std::make_unique(); auto eventData = std::make_shared(); @@ -960,14 +999,20 @@ void EventsManager::uploadCachedData() { event->ParseFromIstream(&snapFile); for(const auto& fileEntry : std::filesystem::directory_iterator(entry.path())) { if(fileEntry.is_regular_file() && fileEntry.path() != entry.path() / "snap.pb") { - auto fileData = std::make_shared(fileEntry.path(), fileEntry.path().filename().string()); + auto fileData = std::make_shared(fileEntry.path(), fileEntry.path().stem().string()); fileGroup->fileData.push_back(fileData); } } snapData->eventData = eventData; snapData->eventData->event = event; snapData->fileGroup = fileGroup; + + // Cached snaps should be added only if there is enough space in the upcoming request std::lock_guard lock(snapBufferMutex); + if(snapBuffer.size() >= maxGroupsPerBatch) { + return; + } + snapBuffer.push_back(std::move(snapData)); // Clear entries added to the snapBuffer clearCachedData(entry.path()); From ece37ac0e4e987a9246009f96ffbe81911373e9d Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Wed, 28 Jan 2026 15:14:57 +0100 Subject: [PATCH 079/180] Mask tests Signed-off-by: stas.bucik --- .github/workflows/test_child.yml | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/.github/workflows/test_child.yml b/.github/workflows/test_child.yml index 358987cfef..3ac81c73ff 100644 --- a/.github/workflows/test_child.yml +++ b/.github/workflows/test_child.yml @@ -90,18 +90,18 @@ jobs: export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}-rgb" exec hil_runner --models "oak4_s" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc4rgb" - linux_rvc4_fsync_test: - needs: [build_docker_container] - strategy: - matrix: - rvc4os: ${{ fromJson(inputs.luxonis_os_versions_to_test) }} - fail-fast: false - runs-on: ['self-hosted', 'testbed-runner'] - steps: - - uses: actions/checkout@v3 +# linux_rvc4_fsync_test: +# needs: [build_docker_container] +# strategy: +# matrix: +# rvc4os: ${{ fromJson(inputs.luxonis_os_versions_to_test) }} +# fail-fast: false +# runs-on: ['self-hosted', 'testbed-runner'] +# steps: +# - uses: actions/checkout@v3 - - name: Run RVC4 Fsync tests - run: | - source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} - export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}-fsync" - exec hil_runner --platforms=rvc4 --scope fsync --basic-sanity --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh fsync" +# - name: Run RVC4 Fsync tests +# run: | +# source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} +# export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}-fsync" +# exec hil_runner --platforms=rvc4 --scope fsync --basic-sanity --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh fsync" From 4d714df6662686ebed361d2c757c584c9d5bb45e Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 13 Jan 2026 15:12:03 +0100 Subject: [PATCH 080/180] Add Gate node --- CMakeLists.txt | 2 + bindings/python/CMakeLists.txt | 2 + bindings/python/src/DatatypeBindings.cpp | 2 + .../src/pipeline/datatype/GateControl.cpp | 28 +++++++++ .../pipeline/datatype/GateControlBindings.cpp | 28 +++++++++ .../python/src/pipeline/node/GateBindings.cpp | 34 +++++++++++ .../python/src/pipeline/node/NodeBindings.cpp | 2 + examples/python/Gate/gate_example.py | 59 +++++++++++++++++++ .../pipeline/datatype/DatatypeEnum.hpp | 1 + .../depthai/pipeline/datatype/GateControl.hpp | 35 +++++++++++ include/depthai/pipeline/node/Gate.hpp | 32 ++++++++++ include/depthai/properties/GateProperties.hpp | 19 ++++++ src/pipeline/datatype/DatatypeEnum.cpp | 2 + src/pipeline/datatype/GateControl.cpp | 5 ++ src/pipeline/datatype/StreamMessageParser.cpp | 5 ++ src/pipeline/node/Gate.cpp | 24 ++++++++ src/pipeline/node/host/Replay.cpp | 2 + src/properties/Properties.cpp | 2 + 18 files changed, 284 insertions(+) create mode 100644 bindings/python/src/pipeline/datatype/GateControl.cpp create mode 100644 bindings/python/src/pipeline/datatype/GateControlBindings.cpp create mode 100644 bindings/python/src/pipeline/node/GateBindings.cpp create mode 100644 examples/python/Gate/gate_example.py create mode 100644 include/depthai/pipeline/datatype/GateControl.hpp create mode 100644 include/depthai/pipeline/node/Gate.hpp create mode 100644 include/depthai/properties/GateProperties.hpp create mode 100644 src/pipeline/datatype/GateControl.cpp create mode 100644 src/pipeline/node/Gate.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1da4264e8d..ed877d2b33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -320,6 +320,7 @@ set(TARGET_CORE_SOURCES src/pipeline/node/host/HostNode.cpp src/pipeline/node/host/RGBD.cpp src/pipeline/node/Vpp.cpp + src/pipeline/node/Gate.cpp src/pipeline/datatype/DatatypeEnum.cpp src/pipeline/datatype/ADataType.cpp src/pipeline/node/PointCloud.cpp @@ -369,6 +370,7 @@ set(TARGET_CORE_SOURCES src/properties/Properties.cpp src/capabilities/Capabilities.cpp src/pipeline/datatype/VppConfig.cpp + src/pipeline/datatype/GateControl.cpp src/utility/H26xParsers.cpp src/utility/ImageManipImpl.cpp src/utility/ObjectTrackerImpl.cpp diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index d93ce960be..a336d72c39 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -75,6 +75,7 @@ set(SOURCE_LIST src/pipeline/node/MonoCameraBindings.cpp src/pipeline/node/StereoDepthBindings.cpp src/pipeline/node/NeuralNetworkBindings.cpp + src/pipeline/node/GateBindings.cpp src/pipeline/node/VideoEncoderBindings.cpp src/pipeline/node/ImageManipBindings.cpp src/pipeline/node/SPIOutBindings.cpp @@ -117,6 +118,7 @@ set(SOURCE_LIST src/pipeline/datatype/CameraControlBindings.cpp src/pipeline/datatype/EdgeDetectorConfigBindings.cpp src/pipeline/datatype/FeatureTrackerConfigBindings.cpp + src/pipeline/datatype/GateControlBindings.cpp src/pipeline/datatype/ThermalConfigBindings.cpp src/pipeline/datatype/ToFConfigBindings.cpp src/pipeline/datatype/ImageManipConfigBindings.cpp diff --git a/bindings/python/src/DatatypeBindings.cpp b/bindings/python/src/DatatypeBindings.cpp index 410f9b36db..7822fc5f36 100644 --- a/bindings/python/src/DatatypeBindings.cpp +++ b/bindings/python/src/DatatypeBindings.cpp @@ -43,6 +43,7 @@ void bind_dynamic_calibration_results(pybind11::module& m, void* pCallstack); void bind_dynamic_calibration_control(pybind11::module& m, void* pCallstack); #endif // DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT void bind_vppconfig(pybind11::module& m, void* pCallstack); +void bind_gate_control(pybind11::module& m, void* pCallstack); void DatatypeBindings::addToCallstack(std::deque& callstack) { // Bind common datatypebindings @@ -85,6 +86,7 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_rgbddata); callstack.push_front(bind_mapdata); callstack.push_front(bind_vppconfig); + callstack.push_front(bind_gate_control); #ifdef DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT callstack.push_front(bind_dynamic_calibration_results); callstack.push_front(bind_dynamic_calibration_control); diff --git a/bindings/python/src/pipeline/datatype/GateControl.cpp b/bindings/python/src/pipeline/datatype/GateControl.cpp new file mode 100644 index 0000000000..6be1d61907 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/GateControl.cpp @@ -0,0 +1,28 @@ +#include +#include + +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" + +// depthai +#include "depthai/pipeline/datatype/GateControl.hpp" + +void bind_gate_control(pybind11::module& m, void* pCallstack) { + using namespace dai; + using namespace pybind11::literals; + + py::class_> gateControl(m, "GateControl", DOC(dai, GateControl)); + + /////////////////////////////////////////////////////////////////////// + // Callstack handling + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + /////////////////////////////////////////////////////////////////////// + + gateControl.def(py::init<>()) + .def_readwrite("value", &GateControl::value, DOC(dai, GateControl, value)) + .def("start", &GateControl::start, DOC(dai, GateControl, start)) + .def("stop", &GateControl::stop, DOC(dai, GateControl, stop)) +} diff --git a/bindings/python/src/pipeline/datatype/GateControlBindings.cpp b/bindings/python/src/pipeline/datatype/GateControlBindings.cpp new file mode 100644 index 0000000000..f8aa328fc0 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/GateControlBindings.cpp @@ -0,0 +1,28 @@ +#include +#include + +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" + +// depthai +#include "depthai/pipeline/datatype/GateControl.hpp" + +void bind_gate_control(pybind11::module& m, void* pCallstack) { + using namespace dai; + using namespace pybind11::literals; + + py::class_> gateControl(m, "GateControl", DOC(dai, GateControl)); + + /////////////////////////////////////////////////////////////////////// + // Callstack handling + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + /////////////////////////////////////////////////////////////////////// + + gateControl.def(py::init<>()) + .def_readwrite("value", &GateControl::value, DOC(dai, GateControl, value)) + .def("start", &GateControl::start, DOC(dai, GateControl, start)) + .def("stop", &GateControl::stop, DOC(dai, GateControl, stop)); +} diff --git a/bindings/python/src/pipeline/node/GateBindings.cpp b/bindings/python/src/pipeline/node/GateBindings.cpp new file mode 100644 index 0000000000..13aa80d00a --- /dev/null +++ b/bindings/python/src/pipeline/node/GateBindings.cpp @@ -0,0 +1,34 @@ +#include "Common.hpp" +#include "NodeBindings.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/Gate.hpp" + +void bind_gate(pybind11::module& m, void* pCallstack) { + using namespace dai; + using namespace dai::node; + using namespace pybind11::literals; + + py::class_ gateProperties(m, "GateProperties", DOC(dai, GateProperties)); + auto gate = ADD_NODE(Gate); + + /////////////////////////////////////////////////////////////////////// + // Callstack handling + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + /////////////////////////////////////////////////////////////////////// + + gate.def_readonly("input", &Gate::input, DOC(dai, node, Gate, input)) + + .def_readonly("inputControl", &Gate::inputControl, DOC(dai, node, Gate, inputControl)) + + .def("runOnHost", &Gate::runOnHost, DOC(dai, node, Gate, runOnHost)) + + .def("setRunOnHost", &Gate::setRunOnHost, py::arg("runOnHost"), DOC(dai, node, Gate, setRunOnHost)) + + .def_readonly("output", &Gate::output, DOC(dai, node, Gate, output)); + + daiNodeModule.attr("Gate").attr("Properties") = gateProperties; +} diff --git a/bindings/python/src/pipeline/node/NodeBindings.cpp b/bindings/python/src/pipeline/node/NodeBindings.cpp index b47629ad4e..aea5925144 100644 --- a/bindings/python/src/pipeline/node/NodeBindings.cpp +++ b/bindings/python/src/pipeline/node/NodeBindings.cpp @@ -169,6 +169,7 @@ void bind_rectification(pybind11::module& m, void* pCallstack); void bind_neuraldepth(pybind11::module& m, void* pCallstack); void bind_neuralassistedstereo(pybind11::module& m, void* pCallstack); void bind_vpp(pybind11::module& m, void* pCallstack); +void bind_gate(pybind11::module& m, void* pCallstack); #ifdef DEPTHAI_HAVE_BASALT_SUPPORT void bind_basaltnode(pybind11::module& m, void* pCallstack); #endif @@ -222,6 +223,7 @@ void NodeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_neuraldepth); callstack.push_front(bind_neuralassistedstereo); callstack.push_front(bind_vpp); + callstack.push_front(bind_gate); #ifdef DEPTHAI_HAVE_BASALT_SUPPORT callstack.push_front(bind_basaltnode); #endif diff --git a/examples/python/Gate/gate_example.py b/examples/python/Gate/gate_example.py new file mode 100644 index 0000000000..6fce5ba8a9 --- /dev/null +++ b/examples/python/Gate/gate_example.py @@ -0,0 +1,59 @@ +import os +import sys +import time +import depthai as dai +import cv2 + +deviceInfo = dai.DeviceInfo(ip) +device = dai.Device(deviceInfo) + +withGate = True + +with dai.Pipeline(device) as pipeline: + camera = pipeline.create(dai.node.Camera).build() + cameraOut = camera.requestFullResolutionOutput(fps=30) + + gate = pipeline.create(dai.node.Gate) + cameraOut.link(gate.input) + cameraQueue = gate.output.createOutputQueue() + # gateControlQueue = gate.input.createInputQueue() + gateControlQueue = gate.inputControl.createInputQueue() + + gateControl = dai.GateControl() + gateControl.stop() + + + pipeline.start() + + ellapsed = 0 + start_time = time.monotonic() + gate_state = True + + while pipeline.isRunning(): + frame = cameraQueue.tryGet() + if frame is not None: + cv2.imshow("camera", frame.getCvFrame()) + + current_time = time.monotonic() + elapsed = current_time - start_time + + if elapsed > 5.0: # 5 seconds + # Toggle the value + gate_state = not gate_state + print(gate_state) + + # Create and send the control message + ctrl = dai.GateControl() + ctrl.value = gate_state + gateControlQueue.send(ctrl) + + print(f"Gate toggled to: {gate_state}") + + # Reset the timer + start_time = current_time + + key = cv2.waitKey(1) + + if key == ord('q'): + pipeline.stop() + break diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index 05f8d8667c..3e50ad348a 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -9,6 +9,7 @@ enum class DatatypeEnum : std::int32_t { Buffer, ImgFrame, EncodedFrame, + GateControl, NNData, ImageManipConfig, CameraControl, diff --git a/include/depthai/pipeline/datatype/GateControl.hpp b/include/depthai/pipeline/datatype/GateControl.hpp new file mode 100644 index 0000000000..4c045c903f --- /dev/null +++ b/include/depthai/pipeline/datatype/GateControl.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +class GateControl : public Buffer { + public: + GateControl() = default; + + ~GateControl() override; + + bool value = true; + + void start() { + value = true; + } + + void stop() { + value = false; + } + + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override { + metadata = utility::serialize(*this); + datatype = this->getDatatype(); + } + + DatatypeEnum getDatatype() const override { + return DatatypeEnum::GateControl; + } + + DEPTHAI_SERIALIZE(GateControl, value); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp new file mode 100644 index 0000000000..8406e23c36 --- /dev/null +++ b/include/depthai/pipeline/node/Gate.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include + +#include "depthai/pipeline/datatype/GateControl.hpp" +#include "depthai/properties/GateProperties.hpp" + +namespace dai { +namespace node { + +class Gate : public DeviceNodeCRTP { + protected: + Properties& getProperties() override { + return properties; + } + + public: + constexpr static const char* NAME = "Gate"; + + using DeviceNodeCRTP::DeviceNodeCRTP; + + Input input{*this, {"input", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::ImgFrame, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + + Output output{*this, {"output", DEFAULT_GROUP, {{{DatatypeEnum::ImgFrame, false}}}}}; + + Input inputControl{*this, {"inputControl", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::GateControl, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + + void run() override; +}; + +} // namespace node +} // namespace dai diff --git a/include/depthai/properties/GateProperties.hpp b/include/depthai/properties/GateProperties.hpp new file mode 100644 index 0000000000..355816a003 --- /dev/null +++ b/include/depthai/properties/GateProperties.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "depthai/properties/Properties.hpp" + +namespace dai { + +/** + * Specify properties for Gate. + */ + +struct GateProperties : PropertiesSerializable { + int sleepingTimeMs = 50; // sleep 50 ms between each inputControl reading + + ~GateProperties() override; +}; + +DEPTHAI_SERIALIZE_EXT(GateProperties, sleepingTimeMs); + +} // namespace dai diff --git a/src/pipeline/datatype/DatatypeEnum.cpp b/src/pipeline/datatype/DatatypeEnum.cpp index ee27b07191..d518816bc6 100644 --- a/src/pipeline/datatype/DatatypeEnum.cpp +++ b/src/pipeline/datatype/DatatypeEnum.cpp @@ -93,6 +93,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::ImgAnnotations, DatatypeEnum::ImageFiltersConfig, DatatypeEnum::VppConfig, + DatatypeEnum::GateControl, DatatypeEnum::ToFDepthConfidenceFilterConfig, DatatypeEnum::MapData, DatatypeEnum::ObjectTrackerConfig, @@ -135,6 +136,7 @@ const std::unordered_map> hierarchy = { {DatatypeEnum::TransformData, {}}, {DatatypeEnum::ImgAnnotations, {}}, {DatatypeEnum::VppConfig, {}}, + {DatatypeEnum::GateControl, {}}, {DatatypeEnum::ImageFiltersConfig, {}}, {DatatypeEnum::ToFDepthConfidenceFilterConfig, {}}, {DatatypeEnum::MapData, {}}, diff --git a/src/pipeline/datatype/GateControl.cpp b/src/pipeline/datatype/GateControl.cpp new file mode 100644 index 0000000000..13bfa968b7 --- /dev/null +++ b/src/pipeline/datatype/GateControl.cpp @@ -0,0 +1,5 @@ +#include "depthai/pipeline/datatype/GateControl.hpp" + +namespace dai { +GateControl::~GateControl() = default; // Avoid weak vtable +} // namespace dai diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 94c20ef28b..4fceef8d79 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -25,6 +25,7 @@ #include "depthai/pipeline/datatype/EdgeDetectorConfig.hpp" #include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" +#include "depthai/pipeline/datatype/GateControl.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" #include "depthai/pipeline/datatype/ImageAlignConfig.hpp" #include "depthai/pipeline/datatype/ImageFiltersConfig.hpp" @@ -184,6 +185,10 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; + case DatatypeEnum::GateControl: + return parseDatatype(metadataStart, serializedObjectSize, data, fd); + break; + case DatatypeEnum::ImgDetections: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; diff --git a/src/pipeline/node/Gate.cpp b/src/pipeline/node/Gate.cpp new file mode 100644 index 0000000000..bf83820b54 --- /dev/null +++ b/src/pipeline/node/Gate.cpp @@ -0,0 +1,24 @@ +#include + +namespace dai { +namespace node { + +void Gate::run() { + bool startStopSwitch = true; // true start, false stop + while(mainLoop()) { + auto control = inputControl.tryGet(); + if(control) { + startStopSwitch = control->value; + } + + if(startStopSwitch) { + auto inData = input.get(); + output.send(inData); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(properties.sleepingTimeMs)); + } + } +} + +} // namespace node +} // namespace dai diff --git a/src/pipeline/node/host/Replay.cpp b/src/pipeline/node/host/Replay.cpp index b6db715456..6f5bc84a22 100644 --- a/src/pipeline/node/host/Replay.cpp +++ b/src/pipeline/node/host/Replay.cpp @@ -80,6 +80,7 @@ inline std::shared_ptr getMessage(const std::shared_ptr getProtoMessage(utility::ByteP case DatatypeEnum::SpatialLocationCalculatorConfig: case DatatypeEnum::SpatialLocationCalculatorData: case DatatypeEnum::EdgeDetectorConfig: + case DatatypeEnum::GateControl: case DatatypeEnum::AprilTagConfig: case DatatypeEnum::AprilTags: case DatatypeEnum::Tracklets: diff --git a/src/properties/Properties.cpp b/src/properties/Properties.cpp index c541e8ddae..6686782d09 100644 --- a/src/properties/Properties.cpp +++ b/src/properties/Properties.cpp @@ -10,6 +10,7 @@ #include "depthai/properties/DeviceNodeGroupProperties.hpp" #include "depthai/properties/EdgeDetectorProperties.hpp" #include "depthai/properties/FeatureTrackerProperties.hpp" +#include "depthai/properties/GateProperties.hpp" #include "depthai/properties/GlobalProperties.hpp" #include "depthai/properties/IMUProperties.hpp" #include "depthai/properties/ImageAlignProperties.hpp" @@ -64,6 +65,7 @@ CameraProperties::~CameraProperties() = default; ColorCameraProperties::~ColorCameraProperties() = default; DetectionParserProperties::~DetectionParserProperties() = default; EdgeDetectorProperties::~EdgeDetectorProperties() = default; +GateProperties::~GateProperties() = default; RectificationProperties::~RectificationProperties() = default; NeuralDepthProperties::~NeuralDepthProperties() = default; FeatureTrackerProperties::~FeatureTrackerProperties() = default; From e7369057107806d940541aec1609ffb65131b4e3 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Wed, 14 Jan 2026 16:37:44 +0100 Subject: [PATCH 081/180] Add runOnHost function --- include/depthai/pipeline/node/Gate.hpp | 11 +++++++++-- src/pipeline/node/Gate.cpp | 23 +++++++++++++++++------ 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 8406e23c36..1bf49beee2 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -19,13 +19,20 @@ class Gate : public DeviceNodeCRTP { using DeviceNodeCRTP::DeviceNodeCRTP; - Input input{*this, {"input", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::ImgFrame, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + Input input{*this, {"input", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; - Output output{*this, {"output", DEFAULT_GROUP, {{{DatatypeEnum::ImgFrame, false}}}}}; + Output output{*this, {"output", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; Input inputControl{*this, {"inputControl", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::GateControl, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + void setRunOnHost(bool runOnHost); + + bool runOnHost() const override; + void run() override; + + private: + bool runOnHostVar = false; }; } // namespace node diff --git a/src/pipeline/node/Gate.cpp b/src/pipeline/node/Gate.cpp index bf83820b54..707e499c08 100644 --- a/src/pipeline/node/Gate.cpp +++ b/src/pipeline/node/Gate.cpp @@ -3,19 +3,30 @@ namespace dai { namespace node { +void Gate::setRunOnHost(bool runOnHost) { + runOnHostVar = runOnHost; +} + +bool Gate::runOnHost() const { + return runOnHostVar; +} + void Gate::run() { bool startStopSwitch = true; // true start, false stop while(mainLoop()) { - auto control = inputControl.tryGet(); - if(control) { - startStopSwitch = control->value; - } - if(startStopSwitch) { + auto control = inputControl.tryGet(); + if(control) { + startStopSwitch = control->value; + } + auto inData = input.get(); output.send(inData); } else { - std::this_thread::sleep_for(std::chrono::milliseconds(properties.sleepingTimeMs)); + auto control = inputControl.get(); + if(control) { + startStopSwitch = control->value; + } } } } From 1d4ecd0357d13fb1cb1a9c7c2af07c89fd3934a5 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 15 Jan 2026 16:23:50 +0100 Subject: [PATCH 082/180] Add c++ example --- examples/cpp/CMakeLists.txt | 1 + examples/cpp/Gate/CMakeLists.txt | 5 +++ examples/cpp/Gate/gate_example.cpp | 60 ++++++++++++++++++++++++++++ examples/python/Gate/gate_example.py | 6 +-- 4 files changed, 67 insertions(+), 5 deletions(-) create mode 100644 examples/cpp/Gate/CMakeLists.txt create mode 100644 examples/cpp/Gate/gate_example.cpp diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index ca849b223b..3b872f4efd 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -160,3 +160,4 @@ add_subdirectory(ImageAlign) add_subdirectory(NeuralDepth) add_subdirectory(Vpp) add_subdirectory(NeuralAssistedStereo) +add_subdirectory(Gate) diff --git a/examples/cpp/Gate/CMakeLists.txt b/examples/cpp/Gate/CMakeLists.txt new file mode 100644 index 0000000000..2c15bcba75 --- /dev/null +++ b/examples/cpp/Gate/CMakeLists.txt @@ -0,0 +1,5 @@ +project(gate_examples) +cmake_minimum_required(VERSION 3.10) + +dai_add_example(gate_node_example gate_example.cpp ON OFF) +dai_set_example_test_labels(gate_node_example ondevice rvc2_all rvc4 rvc4rgb ci) diff --git a/examples/cpp/Gate/gate_example.cpp b/examples/cpp/Gate/gate_example.cpp new file mode 100644 index 0000000000..94838f643f --- /dev/null +++ b/examples/cpp/Gate/gate_example.cpp @@ -0,0 +1,60 @@ +#include +#include + +// DepthAI and OpenCV headers +#include + +#include "depthai/depthai.hpp" +#include "depthai/pipeline/node/Gate.hpp" + +int main() { + dai::Pipeline pipeline; + + auto camera = pipeline.create()->build(); + auto cameraOut = camera->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, 30); + + auto gate = pipeline.create(); + + cameraOut->link(gate->input); + + auto cameraQueue = gate->output.createOutputQueue(); + + auto gateControlQueue = gate->inputControl.createInputQueue(); + + auto gateControl = std::make_shared(); + gateControl->stop(); + + pipeline.start(); + + auto start_time = std::chrono::steady_clock::now(); + bool gate_state = true; + + while(pipeline.isRunning()) { + auto frame = cameraQueue->tryGet(); + if(frame != nullptr) { + cv::imshow("camera", frame->getCvFrame()); + } + + auto current_time = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = current_time - start_time; + + if(elapsed.count() > 5.0) { + gate_state = !gate_state; + + gateControl->value = gate_state; + + gateControlQueue->send(gateControl); + + std::cout << "Gate toggled to: " << (gate_state ? "True" : "False") << std::endl; + + start_time = current_time; + } + + int key = cv::waitKey(1); + if(key == 'q') { + break; + } + } + + return 0; +} diff --git a/examples/python/Gate/gate_example.py b/examples/python/Gate/gate_example.py index 6fce5ba8a9..e8bb5e23ac 100644 --- a/examples/python/Gate/gate_example.py +++ b/examples/python/Gate/gate_example.py @@ -4,7 +4,7 @@ import depthai as dai import cv2 -deviceInfo = dai.DeviceInfo(ip) +deviceInfo = dai.DeviceInfo() device = dai.Device(deviceInfo) withGate = True @@ -16,12 +16,10 @@ gate = pipeline.create(dai.node.Gate) cameraOut.link(gate.input) cameraQueue = gate.output.createOutputQueue() - # gateControlQueue = gate.input.createInputQueue() gateControlQueue = gate.inputControl.createInputQueue() gateControl = dai.GateControl() gateControl.stop() - pipeline.start() @@ -40,8 +38,6 @@ if elapsed > 5.0: # 5 seconds # Toggle the value gate_state = not gate_state - print(gate_state) - # Create and send the control message ctrl = dai.GateControl() ctrl.value = gate_state From b8bcdc7176cd6d99163057cf4ecd54c0db87239c Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 16 Jan 2026 08:52:57 +0100 Subject: [PATCH 083/180] Add tests --- tests/CMakeLists.txt | 4 + .../pipeline/node/gate_note_tests.cpp | 83 +++++++++++++++++++ 2 files changed, 87 insertions(+) create mode 100644 tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 730ccb2fa2..254edb4dbe 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -438,6 +438,10 @@ dai_set_test_labels(nndata_test onhost ci) dai_add_test(imgdetections_test src/onhost_tests/pipeline/datatype/imgdetections_test.cpp) dai_set_test_labels(imgdetections_test onhost ci) +#Gate tests +dai_add_test(gate_node_test src/ondevice_tests/pipeline/node/gate_note_tests.cpp) +dai_set_test_labels(gate_node_test onhost ci) + #ImgFrame tests dai_add_test(imgframe_test src/onhost_tests/pipeline/datatype/imgframe_test.cpp) dai_set_test_labels(imgframe_test onhost ci) diff --git a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp new file mode 100644 index 0000000000..aa76dd5c37 --- /dev/null +++ b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp @@ -0,0 +1,83 @@ +#include +#include +#include + +#include "depthai/depthai.hpp" +#include "depthai/pipeline/node/Gate.hpp" + +TEST_CASE("Test Gate Timing and Data Flow") { + dai::Pipeline pipeline; + + // 1. Setup Nodes + auto camera = pipeline.create()->build(); + // We set 30fps: 2 seconds should yield ~60 frames + auto cameraOut = camera->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, 30); + + auto gate = pipeline.create(); + cameraOut->link(gate->input); + + // 2. Setup Queues + auto cameraQueue = gate->output.createOutputQueue(8, false); // Non-blocking + auto gateControlQueue = gate->inputControl.createInputQueue(); + + pipeline.start(); + + // Test Parameters + const int num_cycles = 4; // Number of times to toggle + const double period_duration = 2.0; // 2 seconds per state + const double wait_delay = 0.05; // 0.05s stabilization wait + bool current_gate_state = true; // Start with Gate ON as requested + + auto start_time = std::chrono::steady_clock::now(); + int cycle_count = 0; + int frames_in_period = 0; + bool is_measuring = false; + + // Initial Gate Control (On) + auto ctrl = std::make_shared(); + ctrl->value = current_gate_state; + gateControlQueue->send(ctrl); + + std::cout << "--- Starting Test: Gate is ON ---" << std::endl; + + while(pipeline.isRunning() && cycle_count < num_cycles) { + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - start_time; + + if(elapsed.count() >= period_duration) { + std::cout << "\n--- Cycle " << cycle_count << ": Toggling Gate to " << (current_gate_state ? "ON" : "OFF") << " ---" << std::endl; + if(current_gate_state) { + CHECK(frames_in_period > 30); + std::cout << "[VERIFY] Gate ON period finished. Frames received: " << frames_in_period << std::endl; + } else { + CHECK(frames_in_period == 0); + std::cout << "[VERIFY] Gate OFF period finished. Frames received: " << frames_in_period << std::endl; + } + + // Toggle State + current_gate_state = !current_gate_state; + ctrl->value = current_gate_state; + gateControlQueue->send(ctrl); + + // Reset Period + start_time = now; + frames_in_period = 0; + is_measuring = false; + cycle_count++; + continue; + } + auto frame = cameraQueue->tryGet(); + + if(elapsed.count() >= wait_delay) { + is_measuring = true; + } + + if(is_measuring) { + if(frame) { + if(is_measuring) { + frames_in_period++; + } + } + } + } +} From 14fb067a7fde8de5defb42f4a3ad04c34de69cf3 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 16 Jan 2026 13:38:22 +0100 Subject: [PATCH 084/180] Add possiblity to send N messages --- .../pipeline/datatype/GateControlBindings.cpp | 14 ++++-- examples/cpp/Gate/gate_example.cpp | 17 ++++--- examples/python/Gate/gate_example.py | 16 +++---- .../depthai/pipeline/datatype/GateControl.hpp | 24 ++++++---- include/depthai/pipeline/node/Gate.hpp | 6 +++ src/pipeline/node/Gate.cpp | 44 ++++++++++++++----- .../pipeline/node/gate_note_tests.cpp | 5 +-- 7 files changed, 83 insertions(+), 43 deletions(-) diff --git a/bindings/python/src/pipeline/datatype/GateControlBindings.cpp b/bindings/python/src/pipeline/datatype/GateControlBindings.cpp index f8aa328fc0..7ec07c8fe3 100644 --- a/bindings/python/src/pipeline/datatype/GateControlBindings.cpp +++ b/bindings/python/src/pipeline/datatype/GateControlBindings.cpp @@ -1,3 +1,4 @@ + #include #include @@ -22,7 +23,14 @@ void bind_gate_control(pybind11::module& m, void* pCallstack) { /////////////////////////////////////////////////////////////////////// gateControl.def(py::init<>()) - .def_readwrite("value", &GateControl::value, DOC(dai, GateControl, value)) - .def("start", &GateControl::start, DOC(dai, GateControl, start)) - .def("stop", &GateControl::stop, DOC(dai, GateControl, stop)); + .def(py::init(), "open"_a, "numMessages"_a) + // Member variables + .def_readwrite("open", &GateControl::open, DOC(dai, GateControl, open)) + .def_readwrite("numMessages", &GateControl::numMessages, DOC(dai, GateControl, numMessages)) + // Static factory methods + .def_static("openGate", py::overload_cast(&GateControl::openGate), "numMessages"_a, DOC(dai, GateControl, openGate)) + .def_static("openGate", py::overload_cast<>(&GateControl::openGate), DOC(dai, GateControl, openGate, 2)) + .def_static("closeGate", &GateControl::closeGate, DOC(dai, GateControl, closeGate)) + // Metadata helper + .def("getDatatype", &GateControl::getDatatype, DOC(dai, GateControl, getDatatype)); } diff --git a/examples/cpp/Gate/gate_example.cpp b/examples/cpp/Gate/gate_example.cpp index 94838f643f..1a633d200c 100644 --- a/examples/cpp/Gate/gate_example.cpp +++ b/examples/cpp/Gate/gate_example.cpp @@ -21,13 +21,10 @@ int main() { auto gateControlQueue = gate->inputControl.createInputQueue(); - auto gateControl = std::make_shared(); - gateControl->stop(); - pipeline.start(); auto start_time = std::chrono::steady_clock::now(); - bool gate_state = true; + bool openGate = true; while(pipeline.isRunning()) { auto frame = cameraQueue->tryGet(); @@ -39,13 +36,15 @@ int main() { std::chrono::duration elapsed = current_time - start_time; if(elapsed.count() > 5.0) { - gate_state = !gate_state; - - gateControl->value = gate_state; + openGate = !openGate; - gateControlQueue->send(gateControl); + if(openGate) { + gateControlQueue->send(dai::GateControl::openGate()); + } else { + gateControlQueue->send(dai::GateControl::closeGate()); + } - std::cout << "Gate toggled to: " << (gate_state ? "True" : "False") << std::endl; + std::cout << "Gate toggled to: " << (openGate ? "True" : "False") << std::endl; start_time = current_time; } diff --git a/examples/python/Gate/gate_example.py b/examples/python/Gate/gate_example.py index e8bb5e23ac..13b505b557 100644 --- a/examples/python/Gate/gate_example.py +++ b/examples/python/Gate/gate_example.py @@ -11,7 +11,7 @@ with dai.Pipeline(device) as pipeline: camera = pipeline.create(dai.node.Camera).build() - cameraOut = camera.requestFullResolutionOutput(fps=30) + cameraOut = camera.requestOutput((640, 400), fps=30) gate = pipeline.create(dai.node.Gate) cameraOut.link(gate.input) @@ -19,13 +19,12 @@ gateControlQueue = gate.inputControl.createInputQueue() gateControl = dai.GateControl() - gateControl.stop() pipeline.start() ellapsed = 0 start_time = time.monotonic() - gate_state = True + gateOpen = True while pipeline.isRunning(): frame = cameraQueue.tryGet() @@ -37,13 +36,14 @@ if elapsed > 5.0: # 5 seconds # Toggle the value - gate_state = not gate_state + gateOpen = not gateOpen # Create and send the control message - ctrl = dai.GateControl() - ctrl.value = gate_state - gateControlQueue.send(ctrl) + if (gateOpen): + gateControlQueue.send(dai.GateControl.openGate()) + else: + gateControlQueue.send(dai.GateControl.closeGate()) - print(f"Gate toggled to: {gate_state}") + print(f"Gate toggled to: {gateOpen}") # Reset the timer start_time = current_time diff --git a/include/depthai/pipeline/datatype/GateControl.hpp b/include/depthai/pipeline/datatype/GateControl.hpp index 4c045c903f..f203254eeb 100644 --- a/include/depthai/pipeline/datatype/GateControl.hpp +++ b/include/depthai/pipeline/datatype/GateControl.hpp @@ -6,20 +6,28 @@ namespace dai { class GateControl : public Buffer { public: - GateControl() = default; + bool open = true; - ~GateControl() override; + int numMessages = -1; - bool value = true; + static std::shared_ptr openGate(int numMessages) { + return std::make_shared(true, numMessages); + } - void start() { - value = true; + static std::shared_ptr openGate() { + return std::make_shared(true, -1); } - void stop() { - value = false; + static std::shared_ptr closeGate() { + return std::make_shared(false, -1); } + GateControl() = default; + + GateControl(bool open, int numMessages) : open(open), numMessages(numMessages) {}; + + ~GateControl() override; + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override { metadata = utility::serialize(*this); datatype = this->getDatatype(); @@ -29,7 +37,7 @@ class GateControl : public Buffer { return DatatypeEnum::GateControl; } - DEPTHAI_SERIALIZE(GateControl, value); + DEPTHAI_SERIALIZE(GateControl, open, numMessages); }; } // namespace dai diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 1bf49beee2..01033d3efc 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -33,6 +33,12 @@ class Gate : public DeviceNodeCRTP { private: bool runOnHostVar = false; + + std::shared_ptr sendMessages(); + + std::shared_ptr sendMessages(int numMessages); + + std::shared_ptr waitFotCommand(); }; } // namespace node diff --git a/src/pipeline/node/Gate.cpp b/src/pipeline/node/Gate.cpp index 707e499c08..03eee320fa 100644 --- a/src/pipeline/node/Gate.cpp +++ b/src/pipeline/node/Gate.cpp @@ -11,22 +11,42 @@ bool Gate::runOnHost() const { return runOnHostVar; } +std::shared_ptr Gate::sendMessages() { + while(true) { + auto control = inputControl.tryGet(); + if(control) { + return control; + } + auto inData = input.get(); + output.send(inData); + } +} + +std::shared_ptr Gate::sendMessages(int numMessages) { + for(int i = 0; i < numMessages; i++) { + auto inData = input.get(); + output.send(inData); + } + return std::make_shared(false, -1); +} + +std::shared_ptr Gate::waitFotCommand() { + auto control = inputControl.get(); + return control; +} + void Gate::run() { - bool startStopSwitch = true; // true start, false stop + auto currentCommand = std::make_shared(true, -1); + while(mainLoop()) { - if(startStopSwitch) { - auto control = inputControl.tryGet(); - if(control) { - startStopSwitch = control->value; + if(currentCommand->open) { + if(currentCommand->numMessages >= 0) { + currentCommand = sendMessages(currentCommand->numMessages); + } else { + currentCommand = sendMessages(); } - - auto inData = input.get(); - output.send(inData); } else { - auto control = inputControl.get(); - if(control) { - startStopSwitch = control->value; - } + currentCommand = waitFotCommand(); } } } diff --git a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp index aa76dd5c37..6046413772 100644 --- a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp +++ b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp @@ -34,8 +34,7 @@ TEST_CASE("Test Gate Timing and Data Flow") { bool is_measuring = false; // Initial Gate Control (On) - auto ctrl = std::make_shared(); - ctrl->value = current_gate_state; + auto ctrl = std::make_shared(current_gate_state, -1); gateControlQueue->send(ctrl); std::cout << "--- Starting Test: Gate is ON ---" << std::endl; @@ -56,7 +55,7 @@ TEST_CASE("Test Gate Timing and Data Flow") { // Toggle State current_gate_state = !current_gate_state; - ctrl->value = current_gate_state; + ctrl->open = current_gate_state; gateControlQueue->send(ctrl); // Reset Period From 930a3b0ffc8ce2507b5439a29108e474801a11f5 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 16 Jan 2026 16:00:03 +0100 Subject: [PATCH 085/180] Add test for N messages --- .../pipeline/node/gate_note_tests.cpp | 60 +++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp index 6046413772..ac1b8bd209 100644 --- a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp +++ b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp @@ -80,3 +80,63 @@ TEST_CASE("Test Gate Timing and Data Flow") { } } } + +TEST_CASE("Test Gate N Messages") { + dai::Pipeline pipeline; + + // 1. Setup Nodes + auto camera = pipeline.create()->build(); + // We set 30fps: 2 seconds should yield ~60 frames + auto cameraOut = camera->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, 30); + + auto gate = pipeline.create(); + cameraOut->link(gate->input); + + // 2. Setup Queues + auto cameraQueue = gate->output.createOutputQueue(8, false); // Non-blocking + auto gateControlQueue = gate->inputControl.createInputQueue(); + + pipeline.start(); + + // Test Parameters + const int num_messages = 8; + const int num_cycles = 4; // Number of times to toggle + const double period_duration = 2.0; // 2 seconds per state + const double wait_delay = 0.05; // 0.05s stabilization wait + + auto start_time = std::chrono::steady_clock::now(); + int cycle_count = 0; + int frames_in_period = 0; + + // Initial Gate Control (On) + auto ctrl = std::make_shared(true, num_messages); + gateControlQueue->send(ctrl); + + std::cout << "--- Starting Test: Gate is ON ---" << std::endl; + + while(pipeline.isRunning() && cycle_count < num_cycles) { + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - start_time; + + if(elapsed.count() >= period_duration) { + if(cycle_count == 0) { + CHECK(frames_in_period >= num_messages); + } else { + CHECK(frames_in_period == num_messages); + } + gateControlQueue->send(ctrl); + std::cout << "[VERIFY] Gate ON for N messages period finished. Frames received: " << frames_in_period << std::endl; + + // Reset Period + start_time = now; + frames_in_period = 0; + cycle_count++; + continue; + } + auto frame = cameraQueue->tryGet(); + + if(frame) { + frames_in_period++; + } + } +} From 5710196a5d33ff6ab489380109a03098a7b79609 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 16 Jan 2026 17:12:09 +0100 Subject: [PATCH 086/180] Add initialConfig --- bindings/python/src/pipeline/node/GateBindings.cpp | 7 +++++++ include/depthai/pipeline/node/Gate.hpp | 8 +++++--- include/depthai/properties/GateProperties.hpp | 5 +++-- src/pipeline/node/Gate.cpp | 11 ++++++++++- .../pipeline/node/gate_note_tests.cpp | 13 +++++-------- 5 files changed, 30 insertions(+), 14 deletions(-) diff --git a/bindings/python/src/pipeline/node/GateBindings.cpp b/bindings/python/src/pipeline/node/GateBindings.cpp index 13aa80d00a..6ee524e93e 100644 --- a/bindings/python/src/pipeline/node/GateBindings.cpp +++ b/bindings/python/src/pipeline/node/GateBindings.cpp @@ -22,6 +22,13 @@ void bind_gate(pybind11::module& m, void* pCallstack) { gate.def_readonly("input", &Gate::input, DOC(dai, node, Gate, input)) + .def_property( + "initialConfig", + [](Gate& self) { return self.initialConfig; }, + [](Gate& self, std::shared_ptr cfg) { self.initialConfig = cfg; }, + DOC(dai, node, Gate, initialConfig), + "Initial config of the node.") + .def_readonly("inputControl", &Gate::inputControl, DOC(dai, node, Gate, inputControl)) .def("runOnHost", &Gate::runOnHost, DOC(dai, node, Gate, runOnHost)) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 01033d3efc..8398fbcd60 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -10,11 +10,13 @@ namespace node { class Gate : public DeviceNodeCRTP { protected: - Properties& getProperties() override { - return properties; - } + Properties& getProperties() override; public: + Gate(std::unique_ptr props); + + std::shared_ptr initialConfig = std::make_shared(); + constexpr static const char* NAME = "Gate"; using DeviceNodeCRTP::DeviceNodeCRTP; diff --git a/include/depthai/properties/GateProperties.hpp b/include/depthai/properties/GateProperties.hpp index 355816a003..f9dad55ddc 100644 --- a/include/depthai/properties/GateProperties.hpp +++ b/include/depthai/properties/GateProperties.hpp @@ -1,5 +1,6 @@ #pragma once +#include "depthai/pipeline/datatype/GateControl.hpp" #include "depthai/properties/Properties.hpp" namespace dai { @@ -9,11 +10,11 @@ namespace dai { */ struct GateProperties : PropertiesSerializable { - int sleepingTimeMs = 50; // sleep 50 ms between each inputControl reading + GateControl initialConfig; ~GateProperties() override; }; -DEPTHAI_SERIALIZE_EXT(GateProperties, sleepingTimeMs); +DEPTHAI_SERIALIZE_EXT(GateProperties, initialConfig); } // namespace dai diff --git a/src/pipeline/node/Gate.cpp b/src/pipeline/node/Gate.cpp index 03eee320fa..4923e48907 100644 --- a/src/pipeline/node/Gate.cpp +++ b/src/pipeline/node/Gate.cpp @@ -3,6 +3,11 @@ namespace dai { namespace node { +Gate::Properties& Gate::getProperties() { + properties.initialConfig = *initialConfig; + return properties; +} + void Gate::setRunOnHost(bool runOnHost) { runOnHostVar = runOnHost; } @@ -11,6 +16,10 @@ bool Gate::runOnHost() const { return runOnHostVar; } +Gate::Gate(std::unique_ptr props) + : DeviceNodeCRTP(std::move(props)), + initialConfig(std::make_shared(properties.initialConfig)) {} + std::shared_ptr Gate::sendMessages() { while(true) { auto control = inputControl.tryGet(); @@ -36,7 +45,7 @@ std::shared_ptr Gate::waitFotCommand() { } void Gate::run() { - auto currentCommand = std::make_shared(true, -1); + auto currentCommand = std::make_shared(*initialConfig); while(mainLoop()) { if(currentCommand->open) { diff --git a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp index ac1b8bd209..aef790827d 100644 --- a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp +++ b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp @@ -96,8 +96,6 @@ TEST_CASE("Test Gate N Messages") { auto cameraQueue = gate->output.createOutputQueue(8, false); // Non-blocking auto gateControlQueue = gate->inputControl.createInputQueue(); - pipeline.start(); - // Test Parameters const int num_messages = 8; const int num_cycles = 4; // Number of times to toggle @@ -110,7 +108,10 @@ TEST_CASE("Test Gate N Messages") { // Initial Gate Control (On) auto ctrl = std::make_shared(true, num_messages); - gateControlQueue->send(ctrl); + gate->initialConfig->open = true; + gate->initialConfig->numMessages = num_messages; + + pipeline.start(); std::cout << "--- Starting Test: Gate is ON ---" << std::endl; @@ -119,11 +120,7 @@ TEST_CASE("Test Gate N Messages") { std::chrono::duration elapsed = now - start_time; if(elapsed.count() >= period_duration) { - if(cycle_count == 0) { - CHECK(frames_in_period >= num_messages); - } else { - CHECK(frames_in_period == num_messages); - } + CHECK(frames_in_period == num_messages); gateControlQueue->send(ctrl); std::cout << "[VERIFY] Gate ON for N messages period finished. Frames received: " << frames_in_period << std::endl; From cba1256edd03542132996cb8529a560913c8f5b2 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Mon, 19 Jan 2026 08:59:57 +0100 Subject: [PATCH 087/180] Remove redundant deviceInfo --- examples/python/Gate/gate_example.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/examples/python/Gate/gate_example.py b/examples/python/Gate/gate_example.py index 13b505b557..0842c5035f 100644 --- a/examples/python/Gate/gate_example.py +++ b/examples/python/Gate/gate_example.py @@ -1,11 +1,8 @@ -import os -import sys import time -import depthai as dai import cv2 +import depthai as dai -deviceInfo = dai.DeviceInfo() -device = dai.Device(deviceInfo) +device = dai.Device() withGate = True From f4fe49f01ad82754604f8c85f73f8055806a5928 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Mon, 19 Jan 2026 09:00:42 +0100 Subject: [PATCH 088/180] Fix camelCase --- examples/cpp/Gate/gate_example.cpp | 8 +- examples/python/Gate/gate_example.py | 8 +- .../pipeline/node/gate_note_tests.cpp | 94 +++++++++---------- 3 files changed, 54 insertions(+), 56 deletions(-) diff --git a/examples/cpp/Gate/gate_example.cpp b/examples/cpp/Gate/gate_example.cpp index 1a633d200c..dbebf8b7e8 100644 --- a/examples/cpp/Gate/gate_example.cpp +++ b/examples/cpp/Gate/gate_example.cpp @@ -23,7 +23,7 @@ int main() { pipeline.start(); - auto start_time = std::chrono::steady_clock::now(); + auto startTime = std::chrono::steady_clock::now(); bool openGate = true; while(pipeline.isRunning()) { @@ -32,8 +32,8 @@ int main() { cv::imshow("camera", frame->getCvFrame()); } - auto current_time = std::chrono::steady_clock::now(); - std::chrono::duration elapsed = current_time - start_time; + auto currentTime = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = currentTime - startTime; if(elapsed.count() > 5.0) { openGate = !openGate; @@ -46,7 +46,7 @@ int main() { std::cout << "Gate toggled to: " << (openGate ? "True" : "False") << std::endl; - start_time = current_time; + startTime = currentTime; } int key = cv::waitKey(1); diff --git a/examples/python/Gate/gate_example.py b/examples/python/Gate/gate_example.py index 0842c5035f..cd86747aed 100644 --- a/examples/python/Gate/gate_example.py +++ b/examples/python/Gate/gate_example.py @@ -20,7 +20,7 @@ pipeline.start() ellapsed = 0 - start_time = time.monotonic() + startTime = time.monotonic() gateOpen = True while pipeline.isRunning(): @@ -28,8 +28,8 @@ if frame is not None: cv2.imshow("camera", frame.getCvFrame()) - current_time = time.monotonic() - elapsed = current_time - start_time + currentTime = time.monotonic() + elapsed = currentTime - startTime if elapsed > 5.0: # 5 seconds # Toggle the value @@ -43,7 +43,7 @@ print(f"Gate toggled to: {gateOpen}") # Reset the timer - start_time = current_time + startTime = currentTime key = cv2.waitKey(1) diff --git a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp index aef790827d..c3315dd019 100644 --- a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp +++ b/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp @@ -23,59 +23,57 @@ TEST_CASE("Test Gate Timing and Data Flow") { pipeline.start(); // Test Parameters - const int num_cycles = 4; // Number of times to toggle - const double period_duration = 2.0; // 2 seconds per state - const double wait_delay = 0.05; // 0.05s stabilization wait - bool current_gate_state = true; // Start with Gate ON as requested + const int numCycles = 4; // Number of times to toggle + const double periodDuration = 2.0; // 2 seconds per state + const double waitDelay = 0.05; // 0.05s stabilization wait + bool currentGateState = true; // Start with Gate ON as requested - auto start_time = std::chrono::steady_clock::now(); - int cycle_count = 0; - int frames_in_period = 0; - bool is_measuring = false; + auto startTime = std::chrono::steady_clock::now(); + int cycleCount = 0; + int framesInPeriod = 0; + bool isMeasuring = false; // Initial Gate Control (On) - auto ctrl = std::make_shared(current_gate_state, -1); + auto ctrl = std::make_shared(currentGateState, -1); gateControlQueue->send(ctrl); std::cout << "--- Starting Test: Gate is ON ---" << std::endl; - while(pipeline.isRunning() && cycle_count < num_cycles) { + while(pipeline.isRunning() && cycleCount < numCycles) { auto now = std::chrono::steady_clock::now(); - std::chrono::duration elapsed = now - start_time; + std::chrono::duration elapsed = now - startTime; - if(elapsed.count() >= period_duration) { - std::cout << "\n--- Cycle " << cycle_count << ": Toggling Gate to " << (current_gate_state ? "ON" : "OFF") << " ---" << std::endl; - if(current_gate_state) { - CHECK(frames_in_period > 30); - std::cout << "[VERIFY] Gate ON period finished. Frames received: " << frames_in_period << std::endl; + if(elapsed.count() >= periodDuration) { + std::cout << "\n--- Cycle " << cycleCount << ": Toggling Gate to " << (currentGateState ? "ON" : "OFF") << " ---" << std::endl; + if(currentGateState) { + CHECK(framesInPeriod > 30); + std::cout << "[VERIFY] Gate ON period finished. Frames received: " << framesInPeriod << std::endl; } else { - CHECK(frames_in_period == 0); - std::cout << "[VERIFY] Gate OFF period finished. Frames received: " << frames_in_period << std::endl; + CHECK(framesInPeriod == 0); + std::cout << "[VERIFY] Gate OFF period finished. Frames received: " << framesInPeriod << std::endl; } // Toggle State - current_gate_state = !current_gate_state; - ctrl->open = current_gate_state; + currentGateState = !currentGateState; + ctrl->open = currentGateState; gateControlQueue->send(ctrl); // Reset Period - start_time = now; - frames_in_period = 0; - is_measuring = false; - cycle_count++; + startTime = now; + framesInPeriod = 0; + isMeasuring = false; + cycleCount++; continue; } auto frame = cameraQueue->tryGet(); - if(elapsed.count() >= wait_delay) { - is_measuring = true; + if(elapsed.count() >= waitDelay) { + isMeasuring = true; } - if(is_measuring) { + if(isMeasuring) { if(frame) { - if(is_measuring) { - frames_in_period++; - } + framesInPeriod++; } } } @@ -97,43 +95,43 @@ TEST_CASE("Test Gate N Messages") { auto gateControlQueue = gate->inputControl.createInputQueue(); // Test Parameters - const int num_messages = 8; - const int num_cycles = 4; // Number of times to toggle - const double period_duration = 2.0; // 2 seconds per state - const double wait_delay = 0.05; // 0.05s stabilization wait + const int numMessages = 8; + const int numCycles = 4; // Number of times to toggle + const double periodDuration = 2.0; // 2 seconds per state + const double waitDelay = 0.05; // 0.05s stabilization wait - auto start_time = std::chrono::steady_clock::now(); - int cycle_count = 0; - int frames_in_period = 0; + auto startTime = std::chrono::steady_clock::now(); + int cycleCount = 0; + int framesInPeriod = 0; // Initial Gate Control (On) - auto ctrl = std::make_shared(true, num_messages); + auto ctrl = std::make_shared(true, numMessages); gate->initialConfig->open = true; - gate->initialConfig->numMessages = num_messages; + gate->initialConfig->numMessages = numMessages; pipeline.start(); std::cout << "--- Starting Test: Gate is ON ---" << std::endl; - while(pipeline.isRunning() && cycle_count < num_cycles) { + while(pipeline.isRunning() && cycleCount < numCycles) { auto now = std::chrono::steady_clock::now(); - std::chrono::duration elapsed = now - start_time; + std::chrono::duration elapsed = now - startTime; - if(elapsed.count() >= period_duration) { - CHECK(frames_in_period == num_messages); + if(elapsed.count() >= periodDuration) { + CHECK(framesInPeriod == numMessages); gateControlQueue->send(ctrl); - std::cout << "[VERIFY] Gate ON for N messages period finished. Frames received: " << frames_in_period << std::endl; + std::cout << "[VERIFY] Gate ON for N messages period finished. Frames received: " << framesInPeriod << std::endl; // Reset Period - start_time = now; - frames_in_period = 0; - cycle_count++; + startTime = now; + framesInPeriod = 0; + cycleCount++; continue; } auto frame = cameraQueue->tryGet(); if(frame) { - frames_in_period++; + framesInPeriod++; } } } From cdc5cdee6a832fcfe52003b25ee04c6f83806e81 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Mon, 19 Jan 2026 09:07:25 +0100 Subject: [PATCH 089/180] Remove diplicit bindings --- .../src/pipeline/datatype/GateControl.cpp | 28 ------------------- 1 file changed, 28 deletions(-) delete mode 100644 bindings/python/src/pipeline/datatype/GateControl.cpp diff --git a/bindings/python/src/pipeline/datatype/GateControl.cpp b/bindings/python/src/pipeline/datatype/GateControl.cpp deleted file mode 100644 index 6be1d61907..0000000000 --- a/bindings/python/src/pipeline/datatype/GateControl.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include - -#include "DatatypeBindings.hpp" -#include "pipeline/CommonBindings.hpp" - -// depthai -#include "depthai/pipeline/datatype/GateControl.hpp" - -void bind_gate_control(pybind11::module& m, void* pCallstack) { - using namespace dai; - using namespace pybind11::literals; - - py::class_> gateControl(m, "GateControl", DOC(dai, GateControl)); - - /////////////////////////////////////////////////////////////////////// - // Callstack handling - Callstack* callstack = (Callstack*)pCallstack; - auto cb = callstack->top(); - callstack->pop(); - cb(m, pCallstack); - /////////////////////////////////////////////////////////////////////// - - gateControl.def(py::init<>()) - .def_readwrite("value", &GateControl::value, DOC(dai, GateControl, value)) - .def("start", &GateControl::start, DOC(dai, GateControl, start)) - .def("stop", &GateControl::stop, DOC(dai, GateControl, stop)) -} From c1a211370cadb8d5ecc26f30f9459263fe6ef806 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 28 Jan 2026 23:25:37 +0100 Subject: [PATCH 090/180] Add reference to unordered map in getAny --- include/depthai/pipeline/MessageQueue.hpp | 8 ++++---- src/pipeline/MessageQueue.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/depthai/pipeline/MessageQueue.hpp b/include/depthai/pipeline/MessageQueue.hpp index 61cc2eed48..9c05c65ab7 100644 --- a/include/depthai/pipeline/MessageQueue.hpp +++ b/include/depthai/pipeline/MessageQueue.hpp @@ -61,14 +61,14 @@ class MessageQueue : public std::enable_shared_from_this { name(c.name), callbacks(c.callbacks), uniqueCallbackId(c.uniqueCallbackId), - pipelineEventDispatcher(c.pipelineEventDispatcher){}; + pipelineEventDispatcher(c.pipelineEventDispatcher) {}; MessageQueue(MessageQueue&& m) noexcept : enable_shared_from_this(m), queue(std::move(m.queue)), name(std::move(m.name)), callbacks(std::move(m.callbacks)), uniqueCallbackId(m.uniqueCallbackId), - pipelineEventDispatcher(m.pipelineEventDispatcher){}; + pipelineEventDispatcher(m.pipelineEventDispatcher) {}; MessageQueue& operator=(const MessageQueue& c) { queue = c.queue; @@ -88,10 +88,10 @@ class MessageQueue : public std::enable_shared_from_this { return *this; } - static std::unordered_map> getAny(std::unordered_map queues, + static std::unordered_map> getAny(const std::unordered_map& queues, std::optional timeout = std::nullopt); template - static std::unordered_map> getAny(std::unordered_map queues, + static std::unordered_map> getAny(const std::unordered_map& queues, std::optional timeout = std::nullopt) { auto resultADatatype = getAny(std::move(queues), timeout); std::unordered_map> result; diff --git a/src/pipeline/MessageQueue.cpp b/src/pipeline/MessageQueue.cpp index aa4f1d0758..7fbfc11e8c 100644 --- a/src/pipeline/MessageQueue.cpp +++ b/src/pipeline/MessageQueue.cpp @@ -215,7 +215,7 @@ void MessageQueue::notifyCondVars() { MessageQueue::QueueException::~QueueException() noexcept = default; -std::unordered_map> MessageQueue::getAny(std::unordered_map queues, +std::unordered_map> MessageQueue::getAny(const std::unordered_map& queues, std::optional timeout) { std::vector> condVarIds; condVarIds.reserve(queues.size()); From a9792eea77ed27da0b26e71c0a87d9525b9e0ffb Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 20 Jan 2026 09:50:35 +0100 Subject: [PATCH 091/180] Use getAny --- include/depthai/pipeline/node/Gate.hpp | 4 +++- src/pipeline/node/Gate.cpp | 31 +++++++++++++++++--------- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 8398fbcd60..4103422d70 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -34,13 +34,15 @@ class Gate : public DeviceNodeCRTP { void run() override; private: + const std::unordered_map inputs = {{"inputControl", inputControl}, {"input", input}}; + bool runOnHostVar = false; std::shared_ptr sendMessages(); std::shared_ptr sendMessages(int numMessages); - std::shared_ptr waitFotCommand(); + std::shared_ptr waitForCommand(); }; } // namespace node diff --git a/src/pipeline/node/Gate.cpp b/src/pipeline/node/Gate.cpp index 4923e48907..8d203209b3 100644 --- a/src/pipeline/node/Gate.cpp +++ b/src/pipeline/node/Gate.cpp @@ -21,25 +21,36 @@ Gate::Gate(std::unique_ptr props) initialConfig(std::make_shared(properties.initialConfig)) {} std::shared_ptr Gate::sendMessages() { - while(true) { - auto control = inputControl.tryGet(); - if(control) { - return control; + while(mainLoop()) { + auto inputMap = MessageQueue::getAny(inputs); + if(inputMap["input"]) { + output.send(inputMap["input"]); + } + if(inputMap["inputControl"]) { + if(auto control = std::dynamic_pointer_cast(inputMap["inputControl"])) { + return control; + } } - auto inData = input.get(); - output.send(inData); } + return nullptr; } std::shared_ptr Gate::sendMessages(int numMessages) { for(int i = 0; i < numMessages; i++) { - auto inData = input.get(); - output.send(inData); + auto inputMap = MessageQueue::getAny(inputs); + if(inputMap["input"]) { + output.send(inputMap["input"]); + } + if(inputMap["inputControl"]) { + if(auto control = std::dynamic_pointer_cast(inputMap["inputControl"])) { + return control; + } + } } return std::make_shared(false, -1); } -std::shared_ptr Gate::waitFotCommand() { +std::shared_ptr Gate::waitForCommand() { auto control = inputControl.get(); return control; } @@ -55,7 +66,7 @@ void Gate::run() { currentCommand = sendMessages(); } } else { - currentCommand = waitFotCommand(); + currentCommand = waitForCommand(); } } } From 12d6c2d2cef09913d17b31744c4c8647c4b79972 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 28 Jan 2026 23:27:31 +0100 Subject: [PATCH 092/180] Add waitAny --- include/depthai/pipeline/MessageQueue.hpp | 2 + src/pipeline/MessageQueue.cpp | 33 +++++++++++++ tests/src/onhost_tests/message_queue_test.cpp | 47 +++++++++++++++++++ 3 files changed, 82 insertions(+) diff --git a/include/depthai/pipeline/MessageQueue.hpp b/include/depthai/pipeline/MessageQueue.hpp index 9c05c65ab7..59ea909540 100644 --- a/include/depthai/pipeline/MessageQueue.hpp +++ b/include/depthai/pipeline/MessageQueue.hpp @@ -88,6 +88,8 @@ class MessageQueue : public std::enable_shared_from_this { return *this; } + static bool waitAny(const std::vector>& queues); + static std::unordered_map> getAny(const std::unordered_map& queues, std::optional timeout = std::nullopt); template diff --git a/src/pipeline/MessageQueue.cpp b/src/pipeline/MessageQueue.cpp index 7fbfc11e8c..f02c7c8906 100644 --- a/src/pipeline/MessageQueue.cpp +++ b/src/pipeline/MessageQueue.cpp @@ -215,6 +215,39 @@ void MessageQueue::notifyCondVars() { MessageQueue::QueueException::~QueueException() noexcept = default; +bool MessageQueue::waitAny(const std::vector>& queues) { + auto inputsWaitCv = std::make_shared(); + std::mutex waitMutex; + + // Store IDs so we can unsubscribe later + std::vector> ids; + + // 1. Subscribe to all queues in the vector + for(MessageQueue& queue : queues) { + ids.push_back({queue, queue.addCondVar(inputsWaitCv)}); + } + + // 2. Wait until any queue has data or closes + std::unique_lock lock(waitMutex); + inputsWaitCv->wait(lock, [&]() { + for(MessageQueue& queue : queues) { + if(queue.has() || queue.isClosed()) return true; + } + return false; + }); + + // 3. Unsubscribe + for(auto& pair : ids) { + pair.first.removeCondVar(pair.second); + } + + // 4. Return true if data is available + for(MessageQueue& queue : queues) { + if(queue.has()) return true; + } + return false; +} + std::unordered_map> MessageQueue::getAny(const std::unordered_map& queues, std::optional timeout) { std::vector> condVarIds; diff --git a/tests/src/onhost_tests/message_queue_test.cpp b/tests/src/onhost_tests/message_queue_test.cpp index 0a53074842..b0d1fcdced 100644 --- a/tests/src/onhost_tests/message_queue_test.cpp +++ b/tests/src/onhost_tests/message_queue_test.cpp @@ -565,3 +565,50 @@ TEST_CASE("Pipeline event dispatcher tests", "[MessageQueue]") { REQUIRE(event2->interval == dai::PipelineEvent::Interval::END); } } + +TEST_CASE("MessageQueue::waitAny Tests", "[MessageQueue]") { + auto msg = std::make_shared(); + + SECTION("One queue has nothing, one has a message") { + MessageQueue q1("empty_q"), q2("full_q"); + q2.send(msg); // Populate q2 + + std::vector> queues{q1, q2}; + + // Act: Should detect the message in q2 and return immediately + bool result = MessageQueue::waitAny(queues); + + REQUIRE(result == true); + REQUIRE(q2.getSize() == 1); + } + + SECTION("One queue is closed") { + MessageQueue q1("empty_1"), q2("empty_2"); + + // To prevent waitAny from blocking forever in a test, + // we close one queue which triggers the CV logic. + q1.close(); + + std::vector> queues{q1, q2}; + + REQUIRE_THROWS_AS(MessageQueue::waitAny(queues), MessageQueue::QueueException); + } + + SECTION("Message arrives after 1 second") { + MessageQueue q1("delayed_q"); + std::vector> queues{q1}; + + std::thread producer([&]() { + std::this_thread::sleep_for(std::chrono::seconds(1)); + q1.send(msg); + }); + + // Act: This will block until the producer thread sends the message + bool result = MessageQueue::waitAny(queues); + + REQUIRE(result == true); + REQUIRE(q1.getSize() == 1); + + if(producer.joinable()) producer.join(); + } +} From c9fff890e80c2cdae7cc9b11570c8173bc85b392 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 20 Jan 2026 13:00:44 +0100 Subject: [PATCH 093/180] Change getAny to waitAny --- include/depthai/pipeline/node/Gate.hpp | 3 +-- src/pipeline/node/Gate.cpp | 28 +++++++++++++------------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 4103422d70..6d72e73f40 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -34,8 +34,7 @@ class Gate : public DeviceNodeCRTP { void run() override; private: - const std::unordered_map inputs = {{"inputControl", inputControl}, {"input", input}}; - + const std::vector> inputs = {inputControl, input}; bool runOnHostVar = false; std::shared_ptr sendMessages(); diff --git a/src/pipeline/node/Gate.cpp b/src/pipeline/node/Gate.cpp index 8d203209b3..55dc9132f7 100644 --- a/src/pipeline/node/Gate.cpp +++ b/src/pipeline/node/Gate.cpp @@ -22,13 +22,12 @@ Gate::Gate(std::unique_ptr props) std::shared_ptr Gate::sendMessages() { while(mainLoop()) { - auto inputMap = MessageQueue::getAny(inputs); - if(inputMap["input"]) { - output.send(inputMap["input"]); - } - if(inputMap["inputControl"]) { - if(auto control = std::dynamic_pointer_cast(inputMap["inputControl"])) { - return control; + if(MessageQueue::waitAny(inputs)) { + if(auto inControl = inputControl.tryGet()) { + return inControl; + } + if(auto in = input.tryGet()) { + output.send(in); } } } @@ -37,13 +36,14 @@ std::shared_ptr Gate::sendMessages() { std::shared_ptr Gate::sendMessages(int numMessages) { for(int i = 0; i < numMessages; i++) { - auto inputMap = MessageQueue::getAny(inputs); - if(inputMap["input"]) { - output.send(inputMap["input"]); - } - if(inputMap["inputControl"]) { - if(auto control = std::dynamic_pointer_cast(inputMap["inputControl"])) { - return control; + if(MessageQueue::waitAny(inputs)) { + if(!mainLoop()) break; + + if(auto inControl = inputControl.tryGet()) { + return inControl; + } + if(auto in = input.tryGet()) { + output.send(in); } } } From 51455419f2c919500d5749c01d8ff10b9a67007d Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 20 Jan 2026 13:26:49 +0100 Subject: [PATCH 094/180] Add GateControl to Protoresialize.cpp --- src/utility/ProtoSerialize.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index b38d1da85c..557ef291f3 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -152,6 +152,7 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::NNData: case DatatypeEnum::ImageManipConfig: case DatatypeEnum::CameraControl: + case DatatypeEnum::GateControl: case DatatypeEnum::ImgDetections: case DatatypeEnum::SpatialImgDetections: case DatatypeEnum::SystemInformation: From 3b9bffbbeb08d73bd3909c98ed409eeaed1bbf1d Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 20 Jan 2026 14:08:36 +0100 Subject: [PATCH 095/180] Fix waitAny: do not throw if one queue is closed --- src/pipeline/MessageQueue.cpp | 4 +- tests/src/onhost_tests/message_queue_test.cpp | 39 +++++++++++++++---- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/src/pipeline/MessageQueue.cpp b/src/pipeline/MessageQueue.cpp index f02c7c8906..5658752c6b 100644 --- a/src/pipeline/MessageQueue.cpp +++ b/src/pipeline/MessageQueue.cpp @@ -231,7 +231,8 @@ bool MessageQueue::waitAny(const std::vector lock(waitMutex); inputsWaitCv->wait(lock, [&]() { for(MessageQueue& queue : queues) { - if(queue.has() || queue.isClosed()) return true; + if(queue.isClosed()) continue; + if(queue.has()) return true; } return false; }); @@ -243,6 +244,7 @@ bool MessageQueue::waitAny(const std::vector> queues{q1, q2}; + + // Act: Should detect the message in q2 and return immediately + bool result = MessageQueue::waitAny(queues); - // To prevent waitAny from blocking forever in a test, - // we close one queue which triggers the CV logic. - q1.close(); + REQUIRE(result == true); + REQUIRE(q1.getSize() == 1); + REQUIRE(q2.getSize() == 1); + } + + SECTION("Zero queues have messages one is closed") { + MessageQueue q1("empty_q1"); + MessageQueue q2("closed_q2"); std::vector> queues{q1, q2}; - REQUIRE_THROWS_AS(MessageQueue::waitAny(queues), MessageQueue::QueueException); + q2.close(); + std::thread producer([&]() { + std::this_thread::sleep_for(std::chrono::seconds(1)); + q1.send(msg); + }); + + // Act: This will block until the producer thread sends the message + bool result = MessageQueue::waitAny(queues); + + REQUIRE(result == true); + REQUIRE(q1.getSize() == 1); + + if(producer.joinable()) producer.join(); } SECTION("Message arrives after 1 second") { MessageQueue q1("delayed_q"); - std::vector> queues{q1}; + MessageQueue q2("delayed_q"); + std::vector> queues{q1, q2}; std::thread producer([&]() { std::this_thread::sleep_for(std::chrono::seconds(1)); From 7a5b3bc323ddb4b70415601a2cafb82719ca7aef Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Wed, 21 Jan 2026 14:17:11 +0100 Subject: [PATCH 096/180] Simplify waitAny --- src/pipeline/MessageQueue.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/pipeline/MessageQueue.cpp b/src/pipeline/MessageQueue.cpp index 5658752c6b..bea9de4a09 100644 --- a/src/pipeline/MessageQueue.cpp +++ b/src/pipeline/MessageQueue.cpp @@ -232,7 +232,9 @@ bool MessageQueue::waitAny(const std::vectorwait(lock, [&]() { for(MessageQueue& queue : queues) { if(queue.isClosed()) continue; - if(queue.has()) return true; + if(queue.has()) { + return true; + } } return false; }); @@ -241,13 +243,7 @@ bool MessageQueue::waitAny(const std::vector> MessageQueue::getAny(const std::unordered_map& queues, From ef4c0da8700913aee3afa1524261b7a0d029188f Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 22 Jan 2026 09:46:53 +0100 Subject: [PATCH 097/180] Add docstrings --- include/depthai/pipeline/node/Gate.hpp | 36 ++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 6d72e73f40..1a7e671f90 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -8,6 +8,14 @@ namespace dai { namespace node { +/** + * @brief Gate Node. + * + * This node acts as a valve for data pipelines. It controls the flow of messages + * from the 'input' to the 'output' based on the state configured via 'inputControl'. + * It can be configured to stay open indefinitely, stay closed, or open for a + * specific number of messages. + */ class Gate : public DeviceNodeCRTP { protected: Properties& getProperties() override; @@ -21,14 +29,42 @@ class Gate : public DeviceNodeCRTP { using DeviceNodeCRTP::DeviceNodeCRTP; + /** + * @brief Main data input. + * * Accepts arbitrary Buffer messages (e.g., ImgFrame, NNData). + * If the Gate is Open, messages received here are forwarded to 'output'. + * If the Gate is Closed, messages received here are discarded/dropped. + * * Default queue size: 4 + * Blocking: False + */ Input input{*this, {"input", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + /** + * @brief Main data output. + * * Forwards messages that were allowed through the Gate. + * The data type matches the input message. + */ Output output{*this, {"output", DEFAULT_GROUP, {{{DatatypeEnum::Buffer, true}}}}}; + /** + * @brief Control input. + * * Accepts 'GateControl' messages to dynamically change the Gate's state. + * Use this to Open/Close the gate or set it to pass a specific number of frames + * at runtime. + * * Default queue size: 4 + */ Input inputControl{*this, {"inputControl", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::GateControl, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + /** + * Specify whether to run on host or device + * By default, the node will run on device. + */ void setRunOnHost(bool runOnHost); + /** + * @brief Check if the node is configured to run on the host. + * @return true if running on host, false otherwise. + */ bool runOnHost() const override; void run() override; From 4366b7f64b439074c2a467dd86c6c9b220b90da9 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 28 Jan 2026 00:18:26 +0100 Subject: [PATCH 098/180] Bump FW to latest to run tests --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 0a3dde0ce9..5f6e34ec2b 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+ad61a2047fcf728becea15dc9431f43941a2a934") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+34cb4209782c688865f277975838a169f7bbaa4d") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 33746df1ce..d765523e1e 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "34ce5bbb381e5836bc2712bc63d0647c43b080f6") +set(DEPTHAI_DEVICE_SIDE_COMMIT "9ea9c8e4ae5fb24e02d3e1cf69e4b2294f365462") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 08e676c44ee08833ea3e0dcabbba1d591eeed43e Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Wed, 28 Jan 2026 00:20:18 +0100 Subject: [PATCH 099/180] Clang format --- include/depthai/pipeline/MessageQueue.hpp | 4 ++-- include/depthai/pipeline/datatype/GateControl.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/depthai/pipeline/MessageQueue.hpp b/include/depthai/pipeline/MessageQueue.hpp index 59ea909540..d5e8f26b1d 100644 --- a/include/depthai/pipeline/MessageQueue.hpp +++ b/include/depthai/pipeline/MessageQueue.hpp @@ -61,14 +61,14 @@ class MessageQueue : public std::enable_shared_from_this { name(c.name), callbacks(c.callbacks), uniqueCallbackId(c.uniqueCallbackId), - pipelineEventDispatcher(c.pipelineEventDispatcher) {}; + pipelineEventDispatcher(c.pipelineEventDispatcher){}; MessageQueue(MessageQueue&& m) noexcept : enable_shared_from_this(m), queue(std::move(m.queue)), name(std::move(m.name)), callbacks(std::move(m.callbacks)), uniqueCallbackId(m.uniqueCallbackId), - pipelineEventDispatcher(m.pipelineEventDispatcher) {}; + pipelineEventDispatcher(m.pipelineEventDispatcher){}; MessageQueue& operator=(const MessageQueue& c) { queue = c.queue; diff --git a/include/depthai/pipeline/datatype/GateControl.hpp b/include/depthai/pipeline/datatype/GateControl.hpp index f203254eeb..5b8ca40bf4 100644 --- a/include/depthai/pipeline/datatype/GateControl.hpp +++ b/include/depthai/pipeline/datatype/GateControl.hpp @@ -24,7 +24,7 @@ class GateControl : public Buffer { GateControl() = default; - GateControl(bool open, int numMessages) : open(open), numMessages(numMessages) {}; + GateControl(bool open, int numMessages) : open(open), numMessages(numMessages){}; ~GateControl() override; From 34cf93127a2dca64938f6a6331be27e01ccd2309 Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 29 Jan 2026 10:02:49 +0100 Subject: [PATCH 100/180] Send an error to pipelineState request if PD is not enabled --- include/depthai/pipeline/Pipeline.hpp | 1 + src/pipeline/Pipeline.cpp | 4 ++ .../RemoteConnectionImpl.cpp | 42 +++++++++++-------- 3 files changed, 30 insertions(+), 17 deletions(-) diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index ff91c75ff8..2601502b7a 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -538,6 +538,7 @@ class Pipeline { /// Pipeline debugging void enablePipelineDebugging(bool enable = true); + bool isPipelineDebuggingEnabled() const; // Access to pipeline state queues std::shared_ptr getPipelineStateOut() const; diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 009b767002..186a52685e 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -1069,6 +1069,10 @@ void Pipeline::enablePipelineDebugging(bool enable) { impl()->enablePipelineDebugging = enable; } +bool Pipeline::isPipelineDebuggingEnabled() const { + return impl()->enablePipelineDebugging; +} + std::shared_ptr Pipeline::getPipelineStateOut() const { return impl()->pipelineStateOut; } diff --git a/src/remote_connection/RemoteConnectionImpl.cpp b/src/remote_connection/RemoteConnectionImpl.cpp index 6eb8d13725..94cfad2858 100644 --- a/src/remote_connection/RemoteConnectionImpl.cpp +++ b/src/remote_connection/RemoteConnectionImpl.cpp @@ -503,23 +503,31 @@ void RemoteConnectionImpl::exposePipelineService(const Pipeline& pipeline) { auto id = ids[2]; - PipelineStateApi pipelineStateApi(pipeline.getPipelineStateOut(), pipeline.getPipelineStateRequest(), pipeline.getAllNodes()); - - serviceMap[id] = [pipelineStateApi](foxglove::ServiceResponse request) mutable { - (void)request; - std::string stateStr; - try { - auto state = pipelineStateApi.nodes().detailed(); - stateStr = state.toJson().dump(); - } catch(const dai::MessageQueue::QueueException& ex) { - stateStr = R"({"error": "Message queue closed."})"; - } catch(const std::runtime_error& e) { - stateStr = R"({"error": "Pipeline debugging disabled. Cannot get pipeline state."})"; - } - auto response = foxglove::ServiceResponse(); - response.data = std::vector(stateStr.begin(), stateStr.end()); - return response; - }; + if(pipeline.isPipelineDebuggingEnabled()) { + PipelineStateApi pipelineStateApi(pipeline.getPipelineStateOut(), pipeline.getPipelineStateRequest(), pipeline.getAllNodes()); + + serviceMap[id] = [pipelineStateApi](foxglove::ServiceResponse request) mutable { + (void)request; + std::string stateStr; + try { + auto state = pipelineStateApi.nodes().detailed(); + stateStr = state.toJson().dump(); + } catch(const dai::MessageQueue::QueueException& ex) { + stateStr = R"({"error": "Message queue closed."})"; + } + auto response = foxglove::ServiceResponse(); + response.data = std::vector(stateStr.begin(), stateStr.end()); + return response; + }; + } else { + serviceMap[id] = [](foxglove::ServiceResponse request) mutable { + (void)request; + std::string stateStr = R"({"error": "Pipeline debugging disabled. Cannot get pipeline state."})"; + auto response = foxglove::ServiceResponse(); + response.data = std::vector(stateStr.begin(), stateStr.end()); + return response; + }; + } } } From ed23aa2275958fc07daee286eb75b198f2f637fe Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 29 Jan 2026 10:14:39 +0100 Subject: [PATCH 101/180] Remove mutable keyword --- src/remote_connection/RemoteConnectionImpl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/remote_connection/RemoteConnectionImpl.cpp b/src/remote_connection/RemoteConnectionImpl.cpp index 94cfad2858..6a7ed1552e 100644 --- a/src/remote_connection/RemoteConnectionImpl.cpp +++ b/src/remote_connection/RemoteConnectionImpl.cpp @@ -520,7 +520,7 @@ void RemoteConnectionImpl::exposePipelineService(const Pipeline& pipeline) { return response; }; } else { - serviceMap[id] = [](foxglove::ServiceResponse request) mutable { + serviceMap[id] = [](foxglove::ServiceResponse request) { (void)request; std::string stateStr = R"({"error": "Pipeline debugging disabled. Cannot get pipeline state."})"; auto response = foxglove::ServiceResponse(); From a693622a0c5e1db3a08f35f7318085925e42776f Mon Sep 17 00:00:00 2001 From: aljazdu <74094620+aljazdu@users.noreply.github.com> Date: Thu, 29 Jan 2026 13:39:25 +0100 Subject: [PATCH 102/180] Remove redundant debug STATUS messages Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- cmake/sanitizers/FindASan.cmake | 3 +-- cmake/toolchain/tsan.cmake | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/cmake/sanitizers/FindASan.cmake b/cmake/sanitizers/FindASan.cmake index 6aad8295c5..1d3fc0395d 100644 --- a/cmake/sanitizers/FindASan.cmake +++ b/cmake/sanitizers/FindASan.cmake @@ -35,8 +35,7 @@ set(FLAG_CANDIDATES if (SANITIZE_ADDRESS AND (SANITIZE_THREAD OR SANITIZE_MEMORY)) - message(STATUS "SANITIZE_THREAD = ${SANITIZE_THREAD}") - message(STATUS "SANITIZE_ADDRESS = ${SANITIZE_ADDRESS}") + message(FATAL_ERROR "AddressSanitizer is not compatible with " "ThreadSanitizer or MemorySanitizer.") endif () diff --git a/cmake/toolchain/tsan.cmake b/cmake/toolchain/tsan.cmake index 5847d43dbd..3d23e51946 100644 --- a/cmake/toolchain/tsan.cmake +++ b/cmake/toolchain/tsan.cmake @@ -1,4 +1,4 @@ -message(STATUS ">>> Toolchain loaded: ${CMAKE_CURRENT_LIST_FILE}") + set(_internal_flags_sanitizer "-fno-omit-frame-pointer -fsanitize=thread") set(CMAKE_C_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_CXX_FLAGS ${_internal_flags_sanitizer}) From 8fc7750d0ad906b425095f2ae49285cfed661059 Mon Sep 17 00:00:00 2001 From: AljazD Date: Thu, 29 Jan 2026 14:02:36 +0100 Subject: [PATCH 103/180] Removed duplicated CMAKE_TOOLCHAIN_FILE include, updated bspatch.c --- CMakeLists.txt | 10 +++++++--- src/bspatch/bspatch.c | 4 ++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3785a9aa02..c1ab490d65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,10 +2,9 @@ cmake_minimum_required(VERSION 3.20) include(cmake/depthaiOptions.cmake) if(CMAKE_TOOLCHAIN_FILE) - message(STATUS "Including toolchain file: ${CMAKE_TOOLCHAIN_FILE}") - include("${CMAKE_TOOLCHAIN_FILE}") + message(STATUS "Toolchain file specified: ${CMAKE_TOOLCHAIN_FILE}") else() - message(STATUS "No toolchain file specified, skipping include.") + message(STATUS "No toolchain file specified.") endif() if(WIN32) @@ -36,6 +35,11 @@ if(POLICY CMP0028) endif() if(DEPTHAI_BOOTSTRAP_VCPKG) + if(CMAKE_TOOLCHAIN_FILE AND NOT CMAKE_TOOLCHAIN_FILE MATCHES "generated/toolchain.cmake$") + get_filename_component(_abs_toolchain "${CMAKE_TOOLCHAIN_FILE}" ABSOLUTE) + set(VCPKG_CHAINLOAD_TOOLCHAIN_FILE "${_abs_toolchain}" CACHE STRING "Toolchain to chainload" FORCE) + endif() + message(STATUS "Including vcpkg.cmake") include(cmake/vcpkg.cmake) include(cmake/depthaiVcpkgFeatures.cmake) diff --git a/src/bspatch/bspatch.c b/src/bspatch/bspatch.c index 63024870f9..a89c5587a8 100644 --- a/src/bspatch/bspatch.c +++ b/src/bspatch/bspatch.c @@ -213,10 +213,10 @@ int bspatch_mem(const uint8_t* oldfile_bin, const int64_t oldfile_size, const ui } return -1; } - if(ctrl[1] > 0 && p_decompressed_block[EXTRA_BLOCK] != NULL) { + if(p_decompressed_block[EXTRA_BLOCK] != NULL) { memcpy(new + newpos, p_decompressed_block[EXTRA_BLOCK], ctrl[1]); - p_decompressed_block[EXTRA_BLOCK] += ctrl[1]; } + p_decompressed_block[EXTRA_BLOCK] += ctrl[1]; /* Adjust pointers */ newpos += ctrl[1]; From 7c35dfbeac7ff558c9eb4481b57d9f7e21fcb873 Mon Sep 17 00:00:00 2001 From: aljazkonec1 <53098513+aljazkonec1@users.noreply.github.com> Date: Thu, 29 Jan 2026 14:15:35 +0100 Subject: [PATCH 104/180] Fix: img_transformation_test regression (#1662) * Bump fw --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 33746df1ce..464451df11 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "34ce5bbb381e5836bc2712bc63d0647c43b080f6") +set(DEPTHAI_DEVICE_SIDE_COMMIT "174e63c577683a214e4b165acbea236a92952239") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 1429cab75ab712de40d04184bf202da62470049f Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Thu, 29 Jan 2026 14:22:53 +0100 Subject: [PATCH 105/180] Bump both FW --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 5f6e34ec2b..9633a4dbe8 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+34cb4209782c688865f277975838a169f7bbaa4d") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+5d5691e443c183b351018b05e4fcff2c7d2f51e9") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index d765523e1e..4d97a061a9 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "9ea9c8e4ae5fb24e02d3e1cf69e4b2294f365462") +set(DEPTHAI_DEVICE_SIDE_COMMIT "85e8b75ec523014c38906a4e00db2a7080c0d466") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 900e3be1730ac4ffa1e4a8701d1132bbfe2d6408 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Thu, 29 Jan 2026 14:53:12 +0100 Subject: [PATCH 106/180] Clang format --- .../cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp b/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp index b858846f93..0ba0a3a5d7 100644 --- a/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp +++ b/examples/cpp/Misc/MultiDevice/multi_device_external_frame_sync.cpp @@ -133,7 +133,7 @@ void setupDevice(dai::DeviceInfo& deviceInfo, if(device->getPlatform() != dai::Platform::RVC4) { throw std::runtime_error("This example supports only RVC4 platform!"); } - + std::string name = deviceInfo.getXLinkDeviceDesc().name; auto role = device->getExternalFrameSyncRole(); From 92327cd3343243f2af22659f52f26817262f947c Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 9 Dec 2025 11:33:31 +0100 Subject: [PATCH 107/180] Add missing consts to XLinkStream --- include/depthai/xlink/XLinkStream.hpp | 46 +++++++++++++-------------- src/xlink/XLinkStream.cpp | 46 +++++++++++++-------------- 2 files changed, 46 insertions(+), 46 deletions(-) diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index d53ef21e0b..5d8cfff3fe 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -68,37 +68,37 @@ class XLinkStream { ~XLinkStream(); // Blocking - void write(span data, span data2); - void write(span data); - void write(long fd); - void write(long fd, span data); - void write(const void* data, std::size_t size); - std::vector read(); - std::vector read(std::chrono::milliseconds timeout); - std::vector read(XLinkTimespec& timestampReceived); - void read(std::vector& data); - void read(std::vector& data, long& fd); - void read(std::vector& data, XLinkTimespec& timestampReceived); - void read(std::vector& data, long& fd, XLinkTimespec& timestampReceived); + void write(span data, span data2) const; + void write(span data) const; + void write(long fd) const; + void write(long fd, span data) const; + void write(const void* data, std::size_t size) const; + std::vector read() const; + std::vector read(std::chrono::milliseconds timeout) const; + std::vector read(XLinkTimespec& timestampReceived) const; + void read(std::vector& data) const; + void read(std::vector& data, long& fd) const; + void read(std::vector& data, XLinkTimespec& timestampReceived) const; + void read(std::vector& data, long& fd, XLinkTimespec& timestampReceived) const; // split write helper - void writeSplit(const void* data, std::size_t size, std::size_t split); - void writeSplit(const std::vector& data, std::size_t split); - StreamPacketDesc readMove(); + void writeSplit(const void* data, std::size_t size, std::size_t split) const; + void writeSplit(const std::vector& data, std::size_t split) const; + StreamPacketDesc readMove() const; // Timeout - bool write(const void* data, std::size_t size, std::chrono::milliseconds timeout); - bool write(const std::uint8_t* data, std::size_t size, std::chrono::milliseconds timeout); - bool write(const std::vector& data, std::chrono::milliseconds timeout); - bool read(std::vector& data, std::chrono::milliseconds timeout); - bool readMove(StreamPacketDesc& packet, const std::chrono::milliseconds timeout); + bool write(const void* data, std::size_t size, std::chrono::milliseconds timeout) const; + bool write(const std::uint8_t* data, std::size_t size, std::chrono::milliseconds timeout) const; + bool write(const std::vector& data, std::chrono::milliseconds timeout) const; + bool read(std::vector& data, std::chrono::milliseconds timeout) const; + bool readMove(StreamPacketDesc& packet, const std::chrono::milliseconds timeout) const; // TODO optional readMove(timeout) -or- tuple readMove(timeout) // deprecated use readMove() instead; readRaw leads to memory violations and/or memory leaks - [[deprecated("use readMove()")]] streamPacketDesc_t* readRaw(); + [[deprecated("use readMove()")]] streamPacketDesc_t* readRaw() const; // deprecated use readMove(packet, timeout) instead; readRaw leads to memory violations and/or memory leaks - [[deprecated("use readMove(packet, timeout)")]] bool readRaw(streamPacketDesc_t*& pPacket, std::chrono::milliseconds timeout); + [[deprecated("use readMove(packet, timeout)")]] bool readRaw(streamPacketDesc_t*& pPacket, std::chrono::milliseconds timeout) const; // deprecated; unsafe leads to memory violations and/or memory leaks - [[deprecated]] void readRawRelease(); + [[deprecated]] void readRawRelease() const; streamId_t getStreamId() const; std::string getStreamName() const; diff --git a/src/xlink/XLinkStream.cpp b/src/xlink/XLinkStream.cpp index 7461c39107..2ac42de089 100644 --- a/src/xlink/XLinkStream.cpp +++ b/src/xlink/XLinkStream.cpp @@ -103,24 +103,24 @@ void StreamPacketMemory::setSize(size_t size) { // BLOCKING VERSIONS //////////////////// -void XLinkStream::write(span data, span data2) { +void XLinkStream::write(span data, span data2) const { auto status = XLinkWriteData2(streamId, data.data(), static_cast(data.size()), data2.data(), data2.size()); if(status != X_LINK_SUCCESS) { throw XLinkWriteError(status, streamName); } } -void XLinkStream::write(span data) { +void XLinkStream::write(span data) const { auto status = XLinkWriteData(streamId, data.data(), static_cast(data.size())); if(status != X_LINK_SUCCESS) { throw XLinkWriteError(status, streamName); } } -void XLinkStream::write(const void* data, std::size_t size) { +void XLinkStream::write(const void* data, std::size_t size) const { write(span(reinterpret_cast(data), size)); } -void XLinkStream::write(long fd) { +void XLinkStream::write(long fd) const { auto status = XLinkWriteFd(streamId, fd); if(status != X_LINK_SUCCESS) { @@ -128,7 +128,7 @@ void XLinkStream::write(long fd) { } } -void XLinkStream::write(long fd, span data) { +void XLinkStream::write(long fd, span data) const { auto status = XLinkWriteFdData(streamId, fd, data.data(), data.size()); if(status != X_LINK_SUCCESS) { @@ -136,12 +136,12 @@ void XLinkStream::write(long fd, span data) { } } -void XLinkStream::read(std::vector& data) { +void XLinkStream::read(std::vector& data) const { XLinkTimespec timestampReceived; read(data, timestampReceived); } -void XLinkStream::read(std::vector& data, XLinkTimespec& timestampReceived) { +void XLinkStream::read(std::vector& data, XLinkTimespec& timestampReceived) const { long fd; read(data, fd, timestampReceived); @@ -153,12 +153,12 @@ void XLinkStream::read(std::vector& data, XLinkTimespec& timestamp } } -void XLinkStream::read(std::vector& data, long& fd) { +void XLinkStream::read(std::vector& data, long& fd) const { XLinkTimespec timestampReceived; read(data, fd, timestampReceived); } -void XLinkStream::read(std::vector& data, long& fd, XLinkTimespec& timestampReceived) { +void XLinkStream::read(std::vector& data, long& fd, XLinkTimespec& timestampReceived) const { StreamPacketDesc packet; const auto status = XLinkReadMoveData(streamId, &packet); if(status != X_LINK_SUCCESS) { @@ -169,19 +169,19 @@ void XLinkStream::read(std::vector& data, long& fd, XLinkTimespec& timestampReceived = packet.tReceived; } -std::vector XLinkStream::read() { +std::vector XLinkStream::read() const { std::vector data; read(data); return data; } -std::vector XLinkStream::read(XLinkTimespec& timestampReceived) { +std::vector XLinkStream::read(XLinkTimespec& timestampReceived) const { std::vector data; read(data, timestampReceived); return data; } -StreamPacketDesc XLinkStream::readMove() { +StreamPacketDesc XLinkStream::readMove() const { StreamPacketDesc packet; const auto status = XLinkReadMoveData(streamId, &packet); if(status != X_LINK_SUCCESS) { @@ -191,7 +191,7 @@ StreamPacketDesc XLinkStream::readMove() { } // USE ONLY WHEN COPYING DATA AT LATER STAGES -streamPacketDesc_t* XLinkStream::readRaw() { +streamPacketDesc_t* XLinkStream::readRaw() const { streamPacketDesc_t* pPacket = nullptr; auto status = XLinkReadData(streamId, &pPacket); if(status != X_LINK_SUCCESS) { @@ -201,13 +201,13 @@ streamPacketDesc_t* XLinkStream::readRaw() { } // USE ONLY WHEN COPYING DATA AT LATER STAGES -void XLinkStream::readRawRelease() { +void XLinkStream::readRawRelease() const { XLinkError_t status; if((status = XLinkReleaseData(streamId)) != X_LINK_SUCCESS) throw XLinkReadError(status, streamName); } // SPLIT HELPER -void XLinkStream::writeSplit(const void* d, std::size_t size, std::size_t split) { +void XLinkStream::writeSplit(const void* d, std::size_t size, std::size_t split) const { const uint8_t* data = (const uint8_t*)d; std::size_t currentOffset = 0; std::size_t remaining = size; @@ -224,7 +224,7 @@ void XLinkStream::writeSplit(const void* d, std::size_t size, std::size_t split) } } -void XLinkStream::writeSplit(const std::vector& data, std::size_t split) { +void XLinkStream::writeSplit(const std::vector& data, std::size_t split) const { writeSplit(data.data(), data.size(), split); } @@ -232,7 +232,7 @@ void XLinkStream::writeSplit(const std::vector& data, std::size_t split // Timeout versions // ////////////////////// -bool XLinkStream::write(const std::uint8_t* data, std::size_t size, std::chrono::milliseconds timeout) { +bool XLinkStream::write(const std::uint8_t* data, std::size_t size, std::chrono::milliseconds timeout) const { auto status = XLinkWriteDataWithTimeout(streamId, data, static_cast(size), static_cast(timeout.count())); if(status == X_LINK_SUCCESS) { return true; @@ -243,15 +243,15 @@ bool XLinkStream::write(const std::uint8_t* data, std::size_t size, std::chrono: } } -bool XLinkStream::write(const void* data, std::size_t size, std::chrono::milliseconds timeout) { +bool XLinkStream::write(const void* data, std::size_t size, std::chrono::milliseconds timeout) const { return write(reinterpret_cast(data), size, timeout); } -bool XLinkStream::write(const std::vector& data, std::chrono::milliseconds timeout) { +bool XLinkStream::write(const std::vector& data, std::chrono::milliseconds timeout) const { return write(data.data(), data.size(), timeout); } -std::vector XLinkStream::read(std::chrono::milliseconds timeout) { +std::vector XLinkStream::read(std::chrono::milliseconds timeout) const { std::vector data; if(!read(data, timeout)) { throw XLinkReadError(X_LINK_TIMEOUT, streamName); @@ -259,7 +259,7 @@ std::vector XLinkStream::read(std::chrono::milliseconds timeout) { return data; } -bool XLinkStream::read(std::vector& data, std::chrono::milliseconds timeout) { +bool XLinkStream::read(std::vector& data, std::chrono::milliseconds timeout) const { StreamPacketDesc packet; const auto status = XLinkReadMoveDataWithTimeout(streamId, &packet, static_cast(timeout.count())); if(status == X_LINK_SUCCESS) { @@ -272,7 +272,7 @@ bool XLinkStream::read(std::vector& data, std::chrono::millisecond } } -bool XLinkStream::readMove(StreamPacketDesc& packet, const std::chrono::milliseconds timeout) { +bool XLinkStream::readMove(StreamPacketDesc& packet, const std::chrono::milliseconds timeout) const { const auto status = XLinkReadMoveDataWithTimeout(streamId, &packet, static_cast(timeout.count())); if(status == X_LINK_SUCCESS) { return true; @@ -283,7 +283,7 @@ bool XLinkStream::readMove(StreamPacketDesc& packet, const std::chrono::millisec } } -bool XLinkStream::readRaw(streamPacketDesc_t*& pPacket, std::chrono::milliseconds timeout) { +bool XLinkStream::readRaw(streamPacketDesc_t*& pPacket, std::chrono::milliseconds timeout) const { auto status = XLinkReadDataWithTimeout(streamId, &pPacket, static_cast(timeout.count())); if(status == X_LINK_SUCCESS) { return true; From a0b4849d682d8e87c5210b001df479bb9c14ab74 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 28 Nov 2025 16:45:35 +0100 Subject: [PATCH 108/180] Add option to send XLink data in packets --- CMakeLists.txt | 1 + .../python/src/pipeline/node/NodeBindings.cpp | 7 + .../pipeline/datatype/DatatypeEnum.hpp | 3 +- .../pipeline/datatype/PacketizedData.hpp | 26 ++++ .../pipeline/node/internal/XLinkInHost.hpp | 9 +- .../pipeline/node/internal/XLinkOut.hpp | 8 + .../internal/XLinkOutProperties.hpp | 13 +- src/pipeline/datatype/DatatypeEnum.cpp | 4 +- src/pipeline/datatype/PacketizedData.cpp | 5 + src/pipeline/datatype/StreamMessageParser.cpp | 4 + src/pipeline/node/internal/XLinkInHost.cpp | 57 +++++-- src/pipeline/node/internal/XLinkOut.cpp | 28 +++- tests/src/ondevice_tests/xlink_test.cpp | 141 +++++++++++++++++- 13 files changed, 286 insertions(+), 20 deletions(-) create mode 100644 include/depthai/pipeline/datatype/PacketizedData.hpp create mode 100644 src/pipeline/datatype/PacketizedData.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1da4264e8d..41429a0be8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -366,6 +366,7 @@ set(TARGET_CORE_SOURCES src/pipeline/datatype/MessageGroup.cpp src/pipeline/datatype/ThermalConfig.cpp src/pipeline/datatype/TransformData.cpp + src/pipeline/datatype/PacketizedData.cpp src/properties/Properties.cpp src/capabilities/Capabilities.cpp src/pipeline/datatype/VppConfig.cpp diff --git a/bindings/python/src/pipeline/node/NodeBindings.cpp b/bindings/python/src/pipeline/node/NodeBindings.cpp index b47629ad4e..c75aa5c143 100644 --- a/bindings/python/src/pipeline/node/NodeBindings.cpp +++ b/bindings/python/src/pipeline/node/NodeBindings.cpp @@ -340,6 +340,13 @@ void NodeBindings::bind(pybind11::module& m, void* pCallstack) { .def("getStreamName", &node::internal::XLinkOut::getStreamName, DOC(dai, node, internal, XLinkOut, getStreamName)) .def("setFpsLimit", &node::internal::XLinkOut::setFpsLimit, py::arg("fps"), DOC(dai, node, internal, XLinkOut, setFpsLimit)) .def("getFpsLimit", &node::internal::XLinkOut::getFpsLimit, DOC(dai, node, internal, XLinkOut, getFpsLimit)) + .def("setPacketSize", &node::internal::XLinkOut::setPacketSize, py::arg("packetSize"), DOC(dai, node, internal, XLinkOut, setPacketSize)) + .def("getPacketSize", &node::internal::XLinkOut::getPacketSize, DOC(dai, node, internal, XLinkOut, getPacketSize)) + .def("setPacketFrequency", + &node::internal::XLinkOut::setPacketFrequency, + py::arg("packetFrequency"), + DOC(dai, node, internal, XLinkOut, setPacketFrequency)) + .def("getPacketFrequency", &node::internal::XLinkOut::getPacketFrequency, DOC(dai, node, internal, XLinkOut, getPacketFrequency)) .def("setMetadataOnly", &node::internal::XLinkOut::setMetadataOnly, py::arg("metadataOnly"), DOC(dai, node, internal, XLinkOut, setMetadataOnly)) .def("getMetadataOnly", &node::internal::XLinkOut::getMetadataOnly, DOC(dai, node, internal, XLinkOut, getMetadataOnly)) .def_readonly("input", &node::internal::XLinkOut::input, DOC(dai, node, internal, XLinkOut, input)); diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index 05f8d8667c..592ac7bdb7 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -48,7 +48,8 @@ enum class DatatypeEnum : std::int32_t { PipelineEvent, PipelineState, PipelineEventAggregationConfig, - VppConfig + VppConfig, + PacketizedData }; bool isDatatypeSubclassOf(DatatypeEnum parent, DatatypeEnum children); diff --git a/include/depthai/pipeline/datatype/PacketizedData.hpp b/include/depthai/pipeline/datatype/PacketizedData.hpp new file mode 100644 index 0000000000..31d8cde700 --- /dev/null +++ b/include/depthai/pipeline/datatype/PacketizedData.hpp @@ -0,0 +1,26 @@ +#pragma once +#include +#include +#include +#include + +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +/** + * Represents a frame split into smaller packets, sent incrementally. + */ +struct PacketizedData : public Buffer { + public: + PacketizedData() = default; + PacketizedData(std::uint32_t numPackets, std::uint32_t totalSize) : numPackets(numPackets), totalSize(totalSize) {} + virtual ~PacketizedData(); + + std::uint32_t numPackets; + std::uint32_t totalSize; + + DEPTHAI_SERIALIZE(PacketizedData, numPackets, totalSize); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/node/internal/XLinkInHost.hpp b/include/depthai/pipeline/node/internal/XLinkInHost.hpp index 4447e55e68..5f8c86ccf9 100644 --- a/include/depthai/pipeline/node/internal/XLinkInHost.hpp +++ b/include/depthai/pipeline/node/internal/XLinkInHost.hpp @@ -2,6 +2,9 @@ #include "depthai/pipeline/Node.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" +#include "depthai/pipeline/datatype/MessageGroup.hpp" +#include "depthai/pipeline/datatype/PacketizedData.hpp" +#include "depthai/pipeline/datatype/StreamMessageParser.hpp" #include "depthai/xlink/XLinkConnection.hpp" namespace dai { @@ -16,6 +19,10 @@ class XLinkInHost : public NodeCRTP { std::mutex mtx; bool isDisconnected = false; + std::shared_ptr readData(const XLinkStream& stream) const; + std::shared_ptr parsePacketizedData(const std::shared_ptr& packetizedData, const XLinkStream& stream) const; + void parseMessageGroup(const std::shared_ptr& messageGroup, const XLinkStream& stream) const; + public: constexpr static const char* NAME = "XLinkInHost"; // Output out{*this, "out", Output::Type::MSender, {{DatatypeEnum::Buffer, true}}}; @@ -29,4 +36,4 @@ class XLinkInHost : public NodeCRTP { } // namespace internal } // namespace node -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/include/depthai/pipeline/node/internal/XLinkOut.hpp b/include/depthai/pipeline/node/internal/XLinkOut.hpp index bafbe2d7c7..3647f6e38b 100644 --- a/include/depthai/pipeline/node/internal/XLinkOut.hpp +++ b/include/depthai/pipeline/node/internal/XLinkOut.hpp @@ -49,6 +49,10 @@ class XLinkOut : public DeviceNodeCRTP */ void setMetadataOnly(bool metadataOnly); + void setPacketSize(int packetSize); + + void setPacketFrequency(int packetFrequency); + /// Get stream name std::string getStreamName() const; /// Get rate limit in messages per second @@ -56,6 +60,10 @@ class XLinkOut : public DeviceNodeCRTP /// Get whether to transfer only messages attributes and not buffer data bool getMetadataOnly() const; + int getPacketSize() const; + + int getPacketFrequency() const; + void buildInternal() override; }; diff --git a/include/depthai/properties/internal/XLinkOutProperties.hpp b/include/depthai/properties/internal/XLinkOutProperties.hpp index b4940f4b26..6ef352264c 100644 --- a/include/depthai/properties/internal/XLinkOutProperties.hpp +++ b/include/depthai/properties/internal/XLinkOutProperties.hpp @@ -18,16 +18,25 @@ struct XLinkOutProperties : PropertiesSerializable> hierarchy = { DatatypeEnum::DynamicCalibrationResult, DatatypeEnum::CoverageData, DatatypeEnum::CalibrationQuality, - + DatatypeEnum::PacketizedData, }}, {DatatypeEnum::Buffer, { @@ -100,6 +100,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::DynamicCalibrationResult, DatatypeEnum::CoverageData, DatatypeEnum::CalibrationQuality, + DatatypeEnum::PacketizedData, }}, {DatatypeEnum::ImgFrame, {}}, {DatatypeEnum::EncodedFrame, {}}, @@ -143,6 +144,7 @@ const std::unordered_map> hierarchy = { {DatatypeEnum::DynamicCalibrationResult, {}}, {DatatypeEnum::CoverageData, {}}, {DatatypeEnum::CalibrationQuality, {}}, + {DatatypeEnum::PacketizedData, {}}, }; bool isDatatypeSubclassOf(DatatypeEnum parent, DatatypeEnum children) { diff --git a/src/pipeline/datatype/PacketizedData.cpp b/src/pipeline/datatype/PacketizedData.cpp new file mode 100644 index 0000000000..fc6c541f28 --- /dev/null +++ b/src/pipeline/datatype/PacketizedData.cpp @@ -0,0 +1,5 @@ +#include "depthai/pipeline/datatype/PacketizedData.hpp" + +namespace dai { +PacketizedData::~PacketizedData() = default; +} // namespace dai diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 94c20ef28b..a82100efa9 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -37,6 +37,7 @@ #include "depthai/pipeline/datatype/NNData.hpp" #include "depthai/pipeline/datatype/NeuralDepthConfig.hpp" #include "depthai/pipeline/datatype/ObjectTrackerConfig.hpp" +#include "depthai/pipeline/datatype/PacketizedData.hpp" #include "depthai/pipeline/datatype/PointCloudConfig.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/datatype/RGBDData.hpp" @@ -294,6 +295,9 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; } + case DatatypeEnum::PacketizedData: { + return parseDatatype(metadataStart, serializedObjectSize, data, fd); + } break; #ifdef DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT case DatatypeEnum::DynamicCalibrationControl: return parseDatatype(metadataStart, serializedObjectSize, data, fd); diff --git a/src/pipeline/node/internal/XLinkInHost.cpp b/src/pipeline/node/internal/XLinkInHost.cpp index b684071faf..f183ffda9c 100644 --- a/src/pipeline/node/internal/XLinkInHost.cpp +++ b/src/pipeline/node/internal/XLinkInHost.cpp @@ -1,5 +1,6 @@ #include "depthai/pipeline/node/internal/XLinkInHost.hpp" +#include "depthai/pipeline/datatype/PacketizedData.hpp" #include "depthai/pipeline/datatype/StreamMessageParser.hpp" #include "depthai/xlink/XLinkConnection.hpp" #include "depthai/xlink/XLinkConstants.hpp" @@ -32,6 +33,52 @@ void XLinkInHost::disconnect() { isWaitingForReconnect.notify_all(); } +std::shared_ptr XLinkInHost::readData(const XLinkStream& stream) const { + auto packet = stream.readMove(); + const auto msg = StreamMessageParser::parseMessage(std::move(packet)); + if(auto messageGroup = std::dynamic_pointer_cast(msg)) { + parseMessageGroup(messageGroup, stream); + } + if(auto packetizedData = std::dynamic_pointer_cast(msg)) { + return parsePacketizedData(packetizedData, stream); + } + return msg; +} + +std::shared_ptr XLinkInHost::parsePacketizedData(const std::shared_ptr& packetizedData, const XLinkStream& stream) const { + std::vector payload; + payload.reserve(packetizedData->totalSize); + std::uint32_t currentSize = 0; + + for(std::uint32_t i = 0; i < packetizedData->numPackets; ++i) { + auto packet = stream.readMove(); + if(packet.length == 0) { + continue; + } + currentSize += packet.length; + payload.insert(payload.end(), packet.data, packet.data + packet.length); + } + + if(currentSize != packetizedData->totalSize) { + throw std::runtime_error( + "XLinkInHost: Data size mismatch. " + "Expected size: " + + std::to_string(packetizedData->totalSize) + ", but received size: " + std::to_string(currentSize) + "."); + } + + streamPacketDesc_t packet; + packet.data = payload.data(); + packet.length = static_cast(payload.size()); + + return StreamMessageParser::parseMessage(std::move(&packet)); +} + +void XLinkInHost::parseMessageGroup(const std::shared_ptr& messageGroup, const XLinkStream& stream) const { + for(auto& msg : messageGroup->group) { + msg.second = readData(stream); + } +} + void XLinkInHost::run() { // Create a stream for the connection bool reconnect = true; @@ -41,16 +88,8 @@ void XLinkInHost::run() { while(mainLoop()) { try { // Blocking -- parse packet and gather timing information - auto packet = stream.readMove(); const auto t1Parse = std::chrono::steady_clock::now(); - const auto msg = StreamMessageParser::parseMessage(std::move(packet)); - if(std::dynamic_pointer_cast(msg) != nullptr) { - auto msgGrp = std::static_pointer_cast(msg); - for(auto& msg : msgGrp->group) { - auto dpacket = stream.readMove(); - msg.second = StreamMessageParser::parseMessage(&dpacket); - } - } + auto msg = readData(stream); const auto t2Parse = std::chrono::steady_clock::now(); // Trace level debugging diff --git a/src/pipeline/node/internal/XLinkOut.cpp b/src/pipeline/node/internal/XLinkOut.cpp index 7976df02ed..c5e7a6ced9 100644 --- a/src/pipeline/node/internal/XLinkOut.cpp +++ b/src/pipeline/node/internal/XLinkOut.cpp @@ -3,33 +3,51 @@ namespace dai { namespace node { namespace internal { + void XLinkOut::buildInternal() { // set some default properties properties.maxFpsLimit = -1; } - +// Setters void XLinkOut::setStreamName(const std::string& name) { properties.streamName = name; } +void XLinkOut::setMetadataOnly(bool metadataOnly) { + properties.metadataOnly = metadataOnly; +} + void XLinkOut::setFpsLimit(float fps) { properties.maxFpsLimit = fps; } -void XLinkOut::setMetadataOnly(bool metadataOnly) { - properties.metadataOnly = metadataOnly; +void XLinkOut::setPacketSize(int packetSize) { + properties.packetSize = packetSize; +} + +void XLinkOut::setPacketFrequency(int packetFrequency) { + properties.packetFrequency = packetFrequency; } +// Getters std::string XLinkOut::getStreamName() const { return properties.streamName; } +bool XLinkOut::getMetadataOnly() const { + return properties.metadataOnly; +} + float XLinkOut::getFpsLimit() const { return properties.maxFpsLimit; } -bool XLinkOut::getMetadataOnly() const { - return properties.metadataOnly; +int XLinkOut::getPacketSize() const { + return properties.packetSize; +} + +int XLinkOut::getPacketFrequency() const { + return properties.packetFrequency; } } // namespace internal diff --git a/tests/src/ondevice_tests/xlink_test.cpp b/tests/src/ondevice_tests/xlink_test.cpp index 0eaf00e815..369857af0b 100644 --- a/tests/src/ondevice_tests/xlink_test.cpp +++ b/tests/src/ondevice_tests/xlink_test.cpp @@ -65,4 +65,143 @@ TEST_CASE("XLinkBridge fps limit test") { } } REQUIRE(numReceived == Catch::Approx(XLINK_FPS_LIMIT * TEST_DURATION.count()).margin(1.01)); // +- 1 frame -} \ No newline at end of file +} + +TEST_CASE("Sync node packet transfer timing and data integrity with varying delays", "[sync][xlink][timing][generate]") { + // Use GENERATE to run the entire test case once for each value + const int sendingDistanceMs = GENERATE(1, 10, 50, 100); + + // --- Core Test Parameters --- + constexpr int width = 1280; + constexpr int height = 800; + const int totalSize = width * height; + + constexpr int numPackets = 20; + // Calculate packet size to ensure the whole image fits perfectly + const int packetSize = static_cast((totalSize + numPackets - 1) / numPackets); + + // --- 1. Pipeline and Node Setup --- + dai::Pipeline pipeline; + auto syncNode = pipeline.create(); + + // Create input and output queues + auto input = syncNode->inputs["input"]; + auto inputQueue = input.createInputQueue(); + auto output = syncNode->out.createOutputQueue(); + + // --- 2. Build, Configure XLink, and Start Pipeline --- + pipeline.build(); + auto xlinkBridge = syncNode->out.getXLinkBridge(); + xlinkBridge->xLinkOut->setPacketSize(packetSize); + xlinkBridge->xLinkOut->setPacketFrequency(1000. / sendingDistanceMs); + pipeline.start(); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // --- 3. Create Test Frame and Timestamps --- + auto desired_duration = std::chrono::seconds(5); + std::chrono::time_point custom_timestamp(desired_duration); + cv::Mat mat = cv::Mat::zeros(height, width, CV_8UC1); + auto imgFrame = std::make_shared(); + imgFrame->setCvFrame(mat, dai::ImgFrame::Type::GRAY8); + imgFrame->setTimestamp(custom_timestamp); + imgFrame->setTimestampDevice(custom_timestamp); + + // --- 4. Send, Receive, and Measure Time --- + auto start = std::chrono::high_resolution_clock::now(); + inputQueue->send(imgFrame); + auto returnedMessageGroup = std::static_pointer_cast(output->get()); + auto returnedFrame = returnedMessageGroup->get("input"); + auto end = std::chrono::high_resolution_clock::now(); + auto durationMs = std::chrono::duration_cast(end - start).count(); + + // Validate data integrity + cv::Mat returnedMat = returnedFrame->getCvFrame(); + bool same = std::equal(mat.begin(), mat.end(), returnedMat.begin()); + + REQUIRE(same); + + // Assert minimum forced delay (numPackets * sendingDistanceMs) + REQUIRE(durationMs >= numPackets * sendingDistanceMs); + + // Assert maximum bound allowing for a reasonable margin of 200ms overhead + REQUIRE(durationMs < numPackets * sendingDistanceMs + 200); +} + +TEST_CASE("Sync node packet transfer data integrity with more frames in MessageGroup", "[sync][xlink]") { + // Use GENERATE to run the entire test case once for each value + const int sendingDistanceMs = 10; + + // --- Core Test Parameters --- + constexpr int width = 1280; + constexpr int height = 800; + const int totalSize = width * height; + + constexpr int numPackets = 20; + // Calculate packet size to ensure the whole image fits perfectly + const int packetSize = static_cast((totalSize + numPackets - 1) / numPackets); + + // --- 1. Pipeline and Node Setup --- + dai::Pipeline pipeline; + auto syncNode = pipeline.create(); + + // Create input and output queues + auto input1 = syncNode->inputs["input1"]; + auto input2 = syncNode->inputs["input2"]; + auto inputQueue1 = input1.createInputQueue(); + auto inputQueue2 = input2.createInputQueue(); + auto output = syncNode->out.createOutputQueue(); + + // --- 2. Build, Configure XLink, and Start Pipeline --- + pipeline.build(); + auto xlinkBridge = syncNode->out.getXLinkBridge(); + xlinkBridge->xLinkOut->setPacketSize(packetSize); + xlinkBridge->xLinkOut->setPacketFrequency(1000. / sendingDistanceMs); + pipeline.start(); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // --- 3. Create Test Frame and Timestamps --- + auto desired_duration = std::chrono::seconds(5); + std::chrono::time_point custom_timestamp(desired_duration); + cv::Mat mat1 = cv::Mat::zeros(height, width, CV_8UC1); + { + uchar* dataPtr = mat1.data; + for(int i = 1; i <= totalSize; ++i) { + *dataPtr = static_cast(i % 256); + dataPtr++; + } + } + cv::Mat mat2 = cv::Mat::zeros(height, width, CV_8UC1); + { + uchar* dataPtr = mat2.data; + for(int i = 1; i <= totalSize; ++i) { + *dataPtr = static_cast(i % 128); + dataPtr++; + } + } + auto imgFrame1 = std::make_shared(); + imgFrame1->setCvFrame(mat1, dai::ImgFrame::Type::GRAY8); + imgFrame1->setTimestamp(custom_timestamp); + imgFrame1->setTimestampDevice(custom_timestamp); + + auto imgFrame2 = std::make_shared(); + imgFrame2->setCvFrame(mat2, dai::ImgFrame::Type::GRAY8); + imgFrame2->setTimestamp(custom_timestamp); + imgFrame2->setTimestampDevice(custom_timestamp); + + // --- 4. Send, Receive, and Measure Time --- + inputQueue1->send(imgFrame1); + inputQueue2->send(imgFrame2); + auto returnedMessageGroup = output->get(); + + auto returnedFrame1 = returnedMessageGroup->get("input1"); + auto returnedFrame2 = returnedMessageGroup->get("input2"); + + // Validate data integrity + cv::Mat returnedMat1 = returnedFrame1->getCvFrame(); + cv::Mat returnedMat2 = returnedFrame2->getCvFrame(); + bool same1 = std::equal(mat1.begin(), mat1.end(), returnedMat1.begin()); + bool same2 = std::equal(mat2.begin(), mat2.end(), returnedMat2.begin()); + + REQUIRE(same1); + REQUIRE(same2); +} From 7f59c04b68027998927f9d43300b930e47ccbd5d Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 16 Dec 2025 14:54:28 +0100 Subject: [PATCH 109/180] Fix warnings --- src/pipeline/node/host/Replay.cpp | 2 ++ src/utility/ProtoSerialize.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/src/pipeline/node/host/Replay.cpp b/src/pipeline/node/host/Replay.cpp index b6db715456..09fe223b3b 100644 --- a/src/pipeline/node/host/Replay.cpp +++ b/src/pipeline/node/host/Replay.cpp @@ -103,6 +103,7 @@ inline std::shared_ptr getMessage(const std::shared_ptr getProtoMessage(utility::ByteP case DatatypeEnum::PipelineEventAggregationConfig: case DatatypeEnum::NeuralDepthConfig: case DatatypeEnum::VppConfig: + case DatatypeEnum::PacketizedData: throw std::runtime_error("Cannot replay message type: " + std::to_string((int)datatype)); } return {}; diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index b38d1da85c..d84e759df6 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -186,6 +186,7 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::PipelineEvent: case DatatypeEnum::PipelineState: case DatatypeEnum::PipelineEventAggregationConfig: + case DatatypeEnum::PacketizedData: return false; } return false; From 1651239e83f3cf71d25dd08684db37d09f6c9136 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 16 Dec 2025 15:26:39 +0100 Subject: [PATCH 110/180] Make PacketizedData struct private --- include/depthai/pipeline/node/internal/XLinkInHost.hpp | 2 +- src/pipeline/datatype/PacketizedData.cpp | 2 +- {include/depthai => src}/pipeline/datatype/PacketizedData.hpp | 0 src/pipeline/datatype/StreamMessageParser.cpp | 2 +- src/pipeline/node/internal/XLinkInHost.cpp | 2 +- 5 files changed, 4 insertions(+), 4 deletions(-) rename {include/depthai => src}/pipeline/datatype/PacketizedData.hpp (100%) diff --git a/include/depthai/pipeline/node/internal/XLinkInHost.hpp b/include/depthai/pipeline/node/internal/XLinkInHost.hpp index 5f8c86ccf9..6e078979f7 100644 --- a/include/depthai/pipeline/node/internal/XLinkInHost.hpp +++ b/include/depthai/pipeline/node/internal/XLinkInHost.hpp @@ -3,11 +3,11 @@ #include "depthai/pipeline/Node.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" -#include "depthai/pipeline/datatype/PacketizedData.hpp" #include "depthai/pipeline/datatype/StreamMessageParser.hpp" #include "depthai/xlink/XLinkConnection.hpp" namespace dai { +struct PacketizedData; namespace node { namespace internal { diff --git a/src/pipeline/datatype/PacketizedData.cpp b/src/pipeline/datatype/PacketizedData.cpp index fc6c541f28..847eaeed8d 100644 --- a/src/pipeline/datatype/PacketizedData.cpp +++ b/src/pipeline/datatype/PacketizedData.cpp @@ -1,4 +1,4 @@ -#include "depthai/pipeline/datatype/PacketizedData.hpp" +#include "PacketizedData.hpp" namespace dai { PacketizedData::~PacketizedData() = default; diff --git a/include/depthai/pipeline/datatype/PacketizedData.hpp b/src/pipeline/datatype/PacketizedData.hpp similarity index 100% rename from include/depthai/pipeline/datatype/PacketizedData.hpp rename to src/pipeline/datatype/PacketizedData.hpp diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index a82100efa9..25a983fdbd 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -22,6 +22,7 @@ #include "depthai/pipeline/datatype/DynamicCalibrationControl.hpp" #include "depthai/pipeline/datatype/DynamicCalibrationResults.hpp" #endif // DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT +#include "PacketizedData.hpp" #include "depthai/pipeline/datatype/EdgeDetectorConfig.hpp" #include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" @@ -37,7 +38,6 @@ #include "depthai/pipeline/datatype/NNData.hpp" #include "depthai/pipeline/datatype/NeuralDepthConfig.hpp" #include "depthai/pipeline/datatype/ObjectTrackerConfig.hpp" -#include "depthai/pipeline/datatype/PacketizedData.hpp" #include "depthai/pipeline/datatype/PointCloudConfig.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/datatype/RGBDData.hpp" diff --git a/src/pipeline/node/internal/XLinkInHost.cpp b/src/pipeline/node/internal/XLinkInHost.cpp index f183ffda9c..29d1e7479d 100644 --- a/src/pipeline/node/internal/XLinkInHost.cpp +++ b/src/pipeline/node/internal/XLinkInHost.cpp @@ -1,6 +1,6 @@ #include "depthai/pipeline/node/internal/XLinkInHost.hpp" -#include "depthai/pipeline/datatype/PacketizedData.hpp" +#include "../../datatype/PacketizedData.hpp" #include "depthai/pipeline/datatype/StreamMessageParser.hpp" #include "depthai/xlink/XLinkConnection.hpp" #include "depthai/xlink/XLinkConstants.hpp" From 47d23ce8cf8855422e4c7e2e395a9c9e413d1c55 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 16 Dec 2025 16:22:51 +0100 Subject: [PATCH 111/180] Change frequencyLimit to BytesPerSecondLimit --- bindings/python/src/pipeline/node/NodeBindings.cpp | 10 +++++----- include/depthai/pipeline/node/internal/XLinkOut.hpp | 4 ++-- .../depthai/properties/internal/XLinkOutProperties.hpp | 6 +++--- src/pipeline/node/internal/XLinkOut.cpp | 8 ++++---- tests/src/ondevice_tests/xlink_test.cpp | 4 ++-- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/bindings/python/src/pipeline/node/NodeBindings.cpp b/bindings/python/src/pipeline/node/NodeBindings.cpp index c75aa5c143..e13e3442cf 100644 --- a/bindings/python/src/pipeline/node/NodeBindings.cpp +++ b/bindings/python/src/pipeline/node/NodeBindings.cpp @@ -342,11 +342,11 @@ void NodeBindings::bind(pybind11::module& m, void* pCallstack) { .def("getFpsLimit", &node::internal::XLinkOut::getFpsLimit, DOC(dai, node, internal, XLinkOut, getFpsLimit)) .def("setPacketSize", &node::internal::XLinkOut::setPacketSize, py::arg("packetSize"), DOC(dai, node, internal, XLinkOut, setPacketSize)) .def("getPacketSize", &node::internal::XLinkOut::getPacketSize, DOC(dai, node, internal, XLinkOut, getPacketSize)) - .def("setPacketFrequency", - &node::internal::XLinkOut::setPacketFrequency, - py::arg("packetFrequency"), - DOC(dai, node, internal, XLinkOut, setPacketFrequency)) - .def("getPacketFrequency", &node::internal::XLinkOut::getPacketFrequency, DOC(dai, node, internal, XLinkOut, getPacketFrequency)) + .def("setBytesPerSecondLimit", + &node::internal::XLinkOut::setBytesPerSecondLimit, + py::arg("bytesPerSecondLimit"), + DOC(dai, node, internal, XLinkOut, setBytesPerSecondLimit)) + .def("getBytesPerSecondLimit", &node::internal::XLinkOut::getBytesPerSecondLimit, DOC(dai, node, internal, XLinkOut, getBytesPerSecondLimit)) .def("setMetadataOnly", &node::internal::XLinkOut::setMetadataOnly, py::arg("metadataOnly"), DOC(dai, node, internal, XLinkOut, setMetadataOnly)) .def("getMetadataOnly", &node::internal::XLinkOut::getMetadataOnly, DOC(dai, node, internal, XLinkOut, getMetadataOnly)) .def_readonly("input", &node::internal::XLinkOut::input, DOC(dai, node, internal, XLinkOut, input)); diff --git a/include/depthai/pipeline/node/internal/XLinkOut.hpp b/include/depthai/pipeline/node/internal/XLinkOut.hpp index 3647f6e38b..c0398e3d8c 100644 --- a/include/depthai/pipeline/node/internal/XLinkOut.hpp +++ b/include/depthai/pipeline/node/internal/XLinkOut.hpp @@ -51,7 +51,7 @@ class XLinkOut : public DeviceNodeCRTP void setPacketSize(int packetSize); - void setPacketFrequency(int packetFrequency); + void setBytesPerSecondLimit(int bytesPerSecondLimit); /// Get stream name std::string getStreamName() const; @@ -62,7 +62,7 @@ class XLinkOut : public DeviceNodeCRTP int getPacketSize() const; - int getPacketFrequency() const; + int getBytesPerSecondLimit() const; void buildInternal() override; }; diff --git a/include/depthai/properties/internal/XLinkOutProperties.hpp b/include/depthai/properties/internal/XLinkOutProperties.hpp index 6ef352264c..d29c29e428 100644 --- a/include/depthai/properties/internal/XLinkOutProperties.hpp +++ b/include/depthai/properties/internal/XLinkOutProperties.hpp @@ -29,14 +29,14 @@ struct XLinkOutProperties : PropertiesSerializableout.getXLinkBridge(); xlinkBridge->xLinkOut->setPacketSize(packetSize); - xlinkBridge->xLinkOut->setPacketFrequency(1000. / sendingDistanceMs); + xlinkBridge->xLinkOut->setBytesPerSecondLimit(packetSize * (1000 / sendingDistanceMs)); pipeline.start(); std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -155,7 +155,7 @@ TEST_CASE("Sync node packet transfer data integrity with more frames in MessageG pipeline.build(); auto xlinkBridge = syncNode->out.getXLinkBridge(); xlinkBridge->xLinkOut->setPacketSize(packetSize); - xlinkBridge->xLinkOut->setPacketFrequency(1000. / sendingDistanceMs); + xlinkBridge->xLinkOut->setBytesPerSecondLimit(packetSize * (1000 / sendingDistanceMs)); pipeline.start(); std::this_thread::sleep_for(std::chrono::seconds(1)); From 50c4738051aeddc14d89b200d2248073ca173818 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 16 Dec 2025 16:37:57 +0100 Subject: [PATCH 112/180] Remove inappropriate // --- include/depthai/properties/internal/XLinkOutProperties.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/depthai/properties/internal/XLinkOutProperties.hpp b/include/depthai/properties/internal/XLinkOutProperties.hpp index d29c29e428..3b0c282396 100644 --- a/include/depthai/properties/internal/XLinkOutProperties.hpp +++ b/include/depthai/properties/internal/XLinkOutProperties.hpp @@ -26,12 +26,12 @@ struct XLinkOutProperties : PropertiesSerializable Date: Tue, 16 Dec 2025 16:40:04 +0100 Subject: [PATCH 113/180] Remove of inappropriate usage of std::move() --- src/pipeline/node/internal/XLinkInHost.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pipeline/node/internal/XLinkInHost.cpp b/src/pipeline/node/internal/XLinkInHost.cpp index 29d1e7479d..5809fbdd6b 100644 --- a/src/pipeline/node/internal/XLinkInHost.cpp +++ b/src/pipeline/node/internal/XLinkInHost.cpp @@ -70,7 +70,7 @@ std::shared_ptr XLinkInHost::parsePacketizedData(const std::shared_pt packet.data = payload.data(); packet.length = static_cast(payload.size()); - return StreamMessageParser::parseMessage(std::move(&packet)); + return StreamMessageParser::parseMessage(&packet); } void XLinkInHost::parseMessageGroup(const std::shared_ptr& messageGroup, const XLinkStream& stream) const { From abbcceeede98f5a08598c1327f245b0c3f42eb57 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 8 Jan 2026 16:43:17 +0100 Subject: [PATCH 114/180] Add tests for the XLinkInHost --- .../pipeline/node/internal/XLinkInHost.hpp | 12 +- src/pipeline/datatype/PacketizedData.cpp | 5 + src/pipeline/datatype/PacketizedData.hpp | 2 + src/pipeline/node/internal/XLinkInHost.cpp | 24 +- tests/CMakeLists.txt | 15 +- .../node/internal/XLinkInHostTest.cpp | 236 ++++++++++++++++++ 6 files changed, 280 insertions(+), 14 deletions(-) create mode 100644 tests/src/onhost_tests/pipeline/node/internal/XLinkInHostTest.cpp diff --git a/include/depthai/pipeline/node/internal/XLinkInHost.hpp b/include/depthai/pipeline/node/internal/XLinkInHost.hpp index 6e078979f7..3edd1c57c1 100644 --- a/include/depthai/pipeline/node/internal/XLinkInHost.hpp +++ b/include/depthai/pipeline/node/internal/XLinkInHost.hpp @@ -11,17 +11,23 @@ struct PacketizedData; namespace node { namespace internal { +class XLinkInHostTestable; + class XLinkInHost : public NodeCRTP { private: + friend XLinkInHostTestable; std::shared_ptr conn; std::string streamName; std::condition_variable isWaitingForReconnect; std::mutex mtx; bool isDisconnected = false; + std::unique_ptr stream; + + virtual StreamPacketDesc readStreamMessage() const; - std::shared_ptr readData(const XLinkStream& stream) const; - std::shared_ptr parsePacketizedData(const std::shared_ptr& packetizedData, const XLinkStream& stream) const; - void parseMessageGroup(const std::shared_ptr& messageGroup, const XLinkStream& stream) const; + std::shared_ptr readData() const; + std::shared_ptr parsePacketizedData(const std::shared_ptr& packetizedData) const; + void parseMessageGroup(const std::shared_ptr& messageGroup) const; public: constexpr static const char* NAME = "XLinkInHost"; diff --git a/src/pipeline/datatype/PacketizedData.cpp b/src/pipeline/datatype/PacketizedData.cpp index 847eaeed8d..b7cd9f3f0f 100644 --- a/src/pipeline/datatype/PacketizedData.cpp +++ b/src/pipeline/datatype/PacketizedData.cpp @@ -2,4 +2,9 @@ namespace dai { PacketizedData::~PacketizedData() = default; + +void PacketizedData::serialize(std::vector& metadata, DatatypeEnum& datatype) const { + metadata = utility::serialize(*this); + datatype = DatatypeEnum::PacketizedData; +} } // namespace dai diff --git a/src/pipeline/datatype/PacketizedData.hpp b/src/pipeline/datatype/PacketizedData.hpp index 31d8cde700..7fd113b8fc 100644 --- a/src/pipeline/datatype/PacketizedData.hpp +++ b/src/pipeline/datatype/PacketizedData.hpp @@ -20,6 +20,8 @@ struct PacketizedData : public Buffer { std::uint32_t numPackets; std::uint32_t totalSize; + void serialize(std::vector& metadata, DatatypeEnum& datatype) const; + DEPTHAI_SERIALIZE(PacketizedData, numPackets, totalSize); }; diff --git a/src/pipeline/node/internal/XLinkInHost.cpp b/src/pipeline/node/internal/XLinkInHost.cpp index 5809fbdd6b..c78b1af575 100644 --- a/src/pipeline/node/internal/XLinkInHost.cpp +++ b/src/pipeline/node/internal/XLinkInHost.cpp @@ -33,25 +33,29 @@ void XLinkInHost::disconnect() { isWaitingForReconnect.notify_all(); } -std::shared_ptr XLinkInHost::readData(const XLinkStream& stream) const { - auto packet = stream.readMove(); +StreamPacketDesc XLinkInHost::readStreamMessage() const { + return stream->readMove(); +} + +std::shared_ptr XLinkInHost::readData() const { + auto packet = readStreamMessage(); const auto msg = StreamMessageParser::parseMessage(std::move(packet)); if(auto messageGroup = std::dynamic_pointer_cast(msg)) { - parseMessageGroup(messageGroup, stream); + parseMessageGroup(messageGroup); } if(auto packetizedData = std::dynamic_pointer_cast(msg)) { - return parsePacketizedData(packetizedData, stream); + return parsePacketizedData(packetizedData); } return msg; } -std::shared_ptr XLinkInHost::parsePacketizedData(const std::shared_ptr& packetizedData, const XLinkStream& stream) const { +std::shared_ptr XLinkInHost::parsePacketizedData(const std::shared_ptr& packetizedData) const { std::vector payload; payload.reserve(packetizedData->totalSize); std::uint32_t currentSize = 0; for(std::uint32_t i = 0; i < packetizedData->numPackets; ++i) { - auto packet = stream.readMove(); + auto packet = readStreamMessage(); if(packet.length == 0) { continue; } @@ -73,9 +77,9 @@ std::shared_ptr XLinkInHost::parsePacketizedData(const std::shared_pt return StreamMessageParser::parseMessage(&packet); } -void XLinkInHost::parseMessageGroup(const std::shared_ptr& messageGroup, const XLinkStream& stream) const { +void XLinkInHost::parseMessageGroup(const std::shared_ptr& messageGroup) const { for(auto& msg : messageGroup->group) { - msg.second = readData(stream); + msg.second = readData(); } } @@ -84,12 +88,12 @@ void XLinkInHost::run() { bool reconnect = true; while(reconnect) { reconnect = false; - XLinkStream stream(std::move(conn), streamName, 1); + stream = std::make_unique(std::move(conn), streamName, 1); while(mainLoop()) { try { // Blocking -- parse packet and gather timing information const auto t1Parse = std::chrono::steady_clock::now(); - auto msg = readData(stream); + auto msg = readData(); const auto t2Parse = std::chrono::steady_clock::now(); // Trace level debugging diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 730ccb2fa2..f6f3720fee 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -5,6 +5,15 @@ enable_testing() # Add Catch2 for writing tests find_package(Catch2 CONFIG REQUIRED) +# Add Trompeloeil mocking framework +include(FetchContent) +FetchContent_Declare( + trompeloeil + GIT_REPOSITORY https://github.com/rollbear/trompeloeil.git + GIT_TAG v44 # You can update this to the latest version +) +FetchContent_MakeAvailable(trompeloeil) + # Print details of the Catch2 package message(STATUS "Catch2_FOUND: ${Catch2_FOUND}") message(STATUS "Catch2_VERSION: ${Catch2_VERSION}") @@ -56,7 +65,7 @@ function(dai_add_test test_name test_src) if(NOT DEPTHAI_MERGED_TARGET) set(DEPTHAI_TARGET depthai::opencv) endif() - target_link_libraries(${test_name} PRIVATE ${DEPTHAI_TARGET} ${OpenCV_LIBS} Catch2::Catch2WithMain Threads::Threads spdlog::spdlog) + target_link_libraries(${test_name} PRIVATE ${DEPTHAI_TARGET} ${OpenCV_LIBS} Catch2::Catch2WithMain Threads::Threads spdlog::spdlog trompeloeil) # Add sanitizers for tests as well if(COMMAND add_sanitizers) @@ -364,6 +373,10 @@ endif() dai_add_test(calibration_handler_test src/onhost_tests/calibration_handler_test.cpp) dai_set_test_labels(calibration_handler_test onhost ci) +# XLInkInHost test +dai_add_test(xlink_in_host_test src/onhost_tests/pipeline/node/internal/XLinkInHostTest.cpp) +dai_set_test_labels(xlink_in_host_test onhost ci) + # NNArchive test if(DEPTHAI_FETCH_ARTIFACTS) dai_add_test(nn_archive_test src/onhost_tests/nn_archive/nn_archive_test.cpp) diff --git a/tests/src/onhost_tests/pipeline/node/internal/XLinkInHostTest.cpp b/tests/src/onhost_tests/pipeline/node/internal/XLinkInHostTest.cpp new file mode 100644 index 0000000000..f0f7d6ddf4 --- /dev/null +++ b/tests/src/onhost_tests/pipeline/node/internal/XLinkInHostTest.cpp @@ -0,0 +1,236 @@ +#include +#include +#include + +#include "../../../../../../src/pipeline/datatype/PacketizedData.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/datatype/StreamMessageParser.hpp" +#include "depthai/pipeline/node/internal/XLinkInHost.hpp" + +namespace dai { +namespace node { +namespace internal { + +class XLinkInHostTestable : public XLinkInHost { + public: + using dai::node::internal::XLinkInHost::parseMessageGroup; + using dai::node::internal::XLinkInHost::parsePacketizedData; + using dai::node::internal::XLinkInHost::readData; + + MAKE_CONST_MOCK0(readStreamMessage, dai::StreamPacketDesc(), override); +}; +} // namespace internal +} // namespace node +} // namespace dai + +using trompeloeil::_; // for wildcard matching if needed + +dai::StreamPacketDesc getRawBuffer(std::shared_ptr frame) { + auto span = frame->getData(); + std::vector pixelData(span.begin(), span.end()); + std::vector metadata = dai::StreamMessageParser::serializeMetadata(frame); + + size_t totalSize = pixelData.size() + metadata.size(); + void* rawBuffer = std::malloc(totalSize); + std::memcpy(rawBuffer, pixelData.data(), pixelData.size()); + std::memcpy(static_cast(rawBuffer) + pixelData.size(), metadata.data(), metadata.size()); + dai::StreamPacketDesc packetFrame; + packetFrame.data = static_cast(rawBuffer); + packetFrame.length = totalSize; + packetFrame.fd = -1; + return packetFrame; +} + +dai::StreamPacketDesc getRawBuffer(std::shared_ptr messageGroup) { + std::vector metadata = dai::StreamMessageParser::serializeMetadata(messageGroup); + + size_t totalSize = metadata.size(); + void* rawBuffer = std::malloc(totalSize); + std::memcpy(static_cast(rawBuffer), metadata.data(), metadata.size()); + dai::StreamPacketDesc packetFrame; + packetFrame.data = static_cast(rawBuffer); + packetFrame.length = totalSize; + packetFrame.fd = -1; + return packetFrame; +} + +dai::StreamPacketDesc getRawBuffer(std::shared_ptr packetizedData) { + std::vector metadata = dai::StreamMessageParser::serializeMetadata(packetizedData); + size_t totalSize = metadata.size(); + void* rawBuffer = std::malloc(totalSize); + std::memcpy(static_cast(rawBuffer), metadata.data(), metadata.size()); + dai::StreamPacketDesc packetFrame; + packetFrame.data = static_cast(rawBuffer); + packetFrame.length = totalSize; + packetFrame.fd = -1; + return packetFrame; +} + +std::vector getPackets(std::shared_ptr frame, size_t maxPacketSize) { + // 1. Prepare the full buffer + auto span = frame->getData(); + std::vector pixelData(span.begin(), span.end()); + std::vector metadata = dai::StreamMessageParser::serializeMetadata(frame); + + std::vector fullMessage; + fullMessage.insert(fullMessage.end(), pixelData.begin(), pixelData.end()); + fullMessage.insert(fullMessage.end(), metadata.begin(), metadata.end()); + + std::vector result; + size_t totalSize = fullMessage.size(); + size_t offset = 0; + + while(offset < totalSize) { + size_t currentChunkSize = std::min(maxPacketSize, totalSize - offset); + + void* chunkBuffer = std::malloc(currentChunkSize); + std::memcpy(chunkBuffer, fullMessage.data() + offset, currentChunkSize); + + dai::StreamPacketDesc packet; + packet.data = static_cast(chunkBuffer); + packet.length = currentChunkSize; + packet.fd = -1; + + result.push_back(std::move(packet)); + offset += currentChunkSize; + } + return result; +} + +TEST_CASE("XLinkInHost - readData") { + using namespace dai::node::internal; + + XLinkInHostTestable xlinkIn; + REQUIRE(0 == 0); + + SECTION("ImgFrame") { + auto frame = std::make_shared(); + cv::Mat mat(2, 2, CV_8UC1, cv::Scalar(1)); + frame->setCvFrame(mat, dai::ImgFrame::Type::GRAY8); + + dai::StreamPacketDesc packet = getRawBuffer(frame); + + REQUIRE_CALL(xlinkIn, readStreamMessage()).LR_RETURN(std::move(packet)); + + auto result = xlinkIn.readData(); + REQUIRE(result != nullptr); + auto img = std::dynamic_pointer_cast(result); + REQUIRE(img != nullptr); + REQUIRE(img->getType() == dai::ImgFrame::Type::GRAY8); + auto cvFrame = img->getCvFrame(); + REQUIRE(cvFrame.rows == mat.rows); + REQUIRE(cvFrame.cols == mat.cols); + REQUIRE(cv::countNonZero(cvFrame != mat) == 0); + } + + SECTION("MessageGroup") { + auto frame1 = std::make_shared(); + cv::Mat mat1(2, 2, CV_8UC1, cv::Scalar(1)); + frame1->setCvFrame(mat1, dai::ImgFrame::Type::GRAY8); + + auto frame2 = std::make_shared(); + cv::Mat mat2(2, 2, CV_8UC1, cv::Scalar(2)); + frame2->setCvFrame(mat2, dai::ImgFrame::Type::GRAY8); + + auto messageGroup = std::make_shared(); + messageGroup->add("frame1", frame1); + messageGroup->add("frame2", frame2); + + std::vector serialized = dai::StreamMessageParser::serializeMetadata(messageGroup); + + dai::StreamPacketDesc packetMessageGroup = getRawBuffer(messageGroup); + dai::StreamPacketDesc packetFrame1 = getRawBuffer(frame1); + dai::StreamPacketDesc packetFrame2 = getRawBuffer(frame2); + + trompeloeil::sequence seq; + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetMessageGroup)); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetFrame1)); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetFrame2)); + + auto result = xlinkIn.readData(); + REQUIRE(result != nullptr); + auto messasgeGroupGot = std::dynamic_pointer_cast(result); + REQUIRE(messasgeGroupGot != nullptr); + auto imgGot1 = messasgeGroupGot->get("frame1"); + auto imgGot2 = messasgeGroupGot->get("frame2"); + REQUIRE(imgGot1 != nullptr); + REQUIRE(imgGot2 != nullptr); + auto cvFrame1 = imgGot1->getCvFrame(); + auto cvFrame2 = imgGot2->getCvFrame(); + + REQUIRE(cvFrame1.rows == mat1.rows); + REQUIRE(cvFrame1.cols == mat1.cols); + + REQUIRE(cvFrame2.rows == mat2.rows); + REQUIRE(cvFrame2.cols == mat2.cols); + + REQUIRE(cv::countNonZero(cvFrame1 != mat1) == 0); + REQUIRE(cv::countNonZero(cvFrame2 != mat2) == 0); + } + + SECTION("PacketizedFrame") { + auto frame = std::make_shared(); + cv::Mat mat(2, 2, CV_8UC1, cv::Scalar(1)); + frame->setCvFrame(mat, dai::ImgFrame::Type::GRAY8); + + // 1. Prepare the chunks + auto packetVec = getPackets(frame, 100); + auto fullPacket = getRawBuffer(frame); + + // 2. Prepare the header that tells readData how many chunks to expect + auto packetizedData = std::make_shared(packetVec.size(), fullPacket.length); + dai::StreamPacketDesc headerPacket = getRawBuffer(packetizedData); + + trompeloeil::sequence seq; + std::vector> expectations; + REQUIRE(packetVec.size() == 4); + + // EXPECTATION 1: The Header + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(headerPacket)); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[0])); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[1])); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[2])); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[3])); + + // 3. Act + auto result = xlinkIn.readData(); + + // 4. Assert + REQUIRE(result != nullptr); + // Note: readData should return the RECONSTRUCTED ImgFrame, not the PacketizedData descriptor + auto imgGot = std::dynamic_pointer_cast(result); + REQUIRE(imgGot->getType() == dai::ImgFrame::Type::GRAY8); + auto cvFrame = imgGot->getCvFrame(); + REQUIRE(cvFrame.rows == mat.rows); + REQUIRE(cvFrame.cols == mat.cols); + REQUIRE(cv::countNonZero(cvFrame != mat) == 0); + } + + SECTION("PacketizedFrame Failed") { + auto frame = std::make_shared(); + cv::Mat mat(2, 2, CV_8UC1, cv::Scalar(1)); + frame->setCvFrame(mat, dai::ImgFrame::Type::GRAY8); + + // 1. Prepare the chunks + auto packetVec = getPackets(frame, 100); + auto fullPacket = getRawBuffer(frame); + + // 2. Prepare the header that tells readData how many chunks to expect + auto packetizedData = std::make_shared(packetVec.size(), fullPacket.length + 200); + dai::StreamPacketDesc headerPacket = getRawBuffer(packetizedData); + + trompeloeil::sequence seq; + std::vector> expectations; + REQUIRE(packetVec.size() == 4); + + // EXPECTATION 1: The Header + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(headerPacket)); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[0])); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[1])); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[2])); + REQUIRE_CALL(xlinkIn, readStreamMessage()).IN_SEQUENCE(seq).LR_RETURN(std::move(packetVec[3])); + + // 3. Act + REQUIRE_THROWS_AS(xlinkIn.readData(), std::runtime_error); + } +} From 4812ef3744e843078d9274d3edf97d94f61c5a4d Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 23 Jan 2026 10:47:20 +0100 Subject: [PATCH 115/180] Update RVC4 fw --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 0a3dde0ce9..87deb4b6a7 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+ad61a2047fcf728becea15dc9431f43941a2a934") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+003c91727db29b3baf8f0da12b6df0744460cbfa") From e13192e70f86967e8330c2b4c1be5275e1973d5e Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 23 Jan 2026 14:17:43 +0100 Subject: [PATCH 116/180] Update RVC2 fw --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 464451df11..2ed857ad3f 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "174e63c577683a214e4b165acbea236a92952239") +set(DEPTHAI_DEVICE_SIDE_COMMIT "01b9e8279083d42ff8a5f0725c1942559dae9ba3") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 722e32deda8f5a5b2097995b202ab2da64114be1 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 27 Jan 2026 19:36:26 +0100 Subject: [PATCH 117/180] Bump RVC4 device side --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 87deb4b6a7..f1c243987b 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+003c91727db29b3baf8f0da12b6df0744460cbfa") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+15fe39853211a90d17ffc737604554a842b9994a") From 6060c9f58317fbb4c260eefb0378f972dc01854c Mon Sep 17 00:00:00 2001 From: Mark Dickie Date: Fri, 30 Jan 2026 05:54:56 +1100 Subject: [PATCH 118/180] fix: Add null pointer checks to FileGroup methods FileGroup::addFile and FileGroup::addImageDetectionsPair now throw std::invalid_argument when passed null shared_ptr for ImgFrame, EncodedFrame, or ImgDetections. Previously, passing null would cause a segfault when the code tried to dereference the null pointer (e.g., calling imgFrame->getCvFrame()). This is consistent with other null handling in the codebase (e.g., ImgFrame::setMetadata throws std::invalid_argument for null input). Fixes #1665 --- src/utility/EventsManager.cpp | 22 +++++ tests/CMakeLists.txt | 6 ++ .../utility/events_manager_test.cpp | 81 +++++++++++++++++++ 3 files changed, 109 insertions(+) create mode 100644 tests/src/onhost_tests/utility/events_manager_test.cpp diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 34322b1a3c..5cb595d691 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include "Environment.hpp" @@ -36,11 +37,17 @@ void FileGroup::addFile(std::string fileName, std::filesystem::path filePath) { } void FileGroup::addFile(const std::optional& fileName, const std::shared_ptr& imgFrame) { + if(!imgFrame) { + throw std::invalid_argument("FileGroup::addFile called with null ImgFrame"); + } std::string dataFileName = fileName.value_or("Image"); addToFileData(fileData, imgFrame, std::move(dataFileName)); } void FileGroup::addFile(const std::optional& fileName, const std::shared_ptr& encodedFrame) { + if(!encodedFrame) { + throw std::invalid_argument("FileGroup::addFile called with null EncodedFrame"); + } std::string dataFileName = fileName.value_or("Image"); addToFileData(fileData, encodedFrame, std::move(dataFileName)); } @@ -50,6 +57,9 @@ void FileGroup::addFile(const std::optional& fileName, const std::s // } void FileGroup::addFile(const std::optional& fileName, const std::shared_ptr& imgDetections) { + if(!imgDetections) { + throw std::invalid_argument("FileGroup::addFile called with null ImgDetections"); + } std::string dataFileName = fileName.value_or("Detections"); addToFileData(fileData, imgDetections, std::move(dataFileName)); } @@ -57,6 +67,12 @@ void FileGroup::addFile(const std::optional& fileName, const std::s void FileGroup::addImageDetectionsPair(const std::optional& fileName, const std::shared_ptr& imgFrame, const std::shared_ptr& imgDetections) { + if(!imgFrame) { + throw std::invalid_argument("FileGroup::addImageDetectionsPair called with null ImgFrame"); + } + if(!imgDetections) { + throw std::invalid_argument("FileGroup::addImageDetectionsPair called with null ImgDetections"); + } std::string dataFileName = fileName.value_or("ImageDetection"); addToFileData(fileData, imgFrame, dataFileName); addToFileData(fileData, imgDetections, std::move(dataFileName)); @@ -65,6 +81,12 @@ void FileGroup::addImageDetectionsPair(const std::optional& fileNam void FileGroup::addImageDetectionsPair(const std::optional& fileName, const std::shared_ptr& encodedFrame, const std::shared_ptr& imgDetections) { + if(!encodedFrame) { + throw std::invalid_argument("FileGroup::addImageDetectionsPair called with null EncodedFrame"); + } + if(!imgDetections) { + throw std::invalid_argument("FileGroup::addImageDetectionsPair called with null ImgDetections"); + } std::string dataFileName = fileName.value_or("ImageDetection"); addToFileData(fileData, encodedFrame, dataFileName); addToFileData(fileData, imgDetections, std::move(dataFileName)); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e67af2211e..d8ec89c442 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -426,6 +426,12 @@ add_dependencies(platform_test fslock_dummy) dai_add_test(circular_buffer_test src/onhost_tests/utility/circular_buffer_test.cpp) dai_set_test_labels(circular_buffer_test onhost ci) +# EventsManager test +if(DEPTHAI_ENABLE_EVENTS_MANAGER) + dai_add_test(events_manager_test src/onhost_tests/utility/events_manager_test.cpp) + dai_set_test_labels(events_manager_test onhost ci) +endif() + # Pipeline debugging tests dai_add_test(pipeline_debugging_host_test src/onhost_tests/pipeline_debugging_host_test.cpp) dai_set_test_labels(pipeline_debugging_host_test onhost ci) diff --git a/tests/src/onhost_tests/utility/events_manager_test.cpp b/tests/src/onhost_tests/utility/events_manager_test.cpp new file mode 100644 index 0000000000..fa465d4402 --- /dev/null +++ b/tests/src/onhost_tests/utility/events_manager_test.cpp @@ -0,0 +1,81 @@ +#include +#include +#include + +#include "depthai/pipeline/datatype/EncodedFrame.hpp" +#include "depthai/pipeline/datatype/ImgDetections.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/utility/EventsManager.hpp" + +using namespace dai; +using namespace dai::utility; + +TEST_CASE("FileGroup throws on null pointer inputs", "[FileGroup][EventsManager]") { + FileGroup fileGroup; + + SECTION("addFile with null ImgFrame throws") { + std::shared_ptr nullFrame = nullptr; + REQUIRE_THROWS_AS(fileGroup.addFile("test.jpg", nullFrame), std::invalid_argument); + } + + SECTION("addFile with null EncodedFrame throws") { + std::shared_ptr nullFrame = nullptr; + REQUIRE_THROWS_AS(fileGroup.addFile("test.jpg", nullFrame), std::invalid_argument); + } + + SECTION("addFile with null ImgDetections throws") { + std::shared_ptr nullDetections = nullptr; + REQUIRE_THROWS_AS(fileGroup.addFile("test.json", nullDetections), std::invalid_argument); + } + + SECTION("addImageDetectionsPair with null ImgFrame throws") { + std::shared_ptr nullFrame = nullptr; + auto detections = std::make_shared(); + REQUIRE_THROWS_AS(fileGroup.addImageDetectionsPair("test", nullFrame, detections), std::invalid_argument); + } + + SECTION("addImageDetectionsPair with null EncodedFrame throws") { + std::shared_ptr nullFrame = nullptr; + auto detections = std::make_shared(); + REQUIRE_THROWS_AS(fileGroup.addImageDetectionsPair("test", nullFrame, detections), std::invalid_argument); + } + + SECTION("addImageDetectionsPair with null ImgDetections throws") { + auto frame = std::make_shared(); + frame->setType(ImgFrame::Type::BGR888i).setSize(4, 4); + std::vector data(4 * 4 * 3, 128); + frame->setData(data); + std::shared_ptr nullDetections = nullptr; + REQUIRE_THROWS_AS(fileGroup.addImageDetectionsPair("test", frame, nullDetections), std::invalid_argument); + } +} + +TEST_CASE("FileGroup accepts valid inputs", "[FileGroup][EventsManager]") { + FileGroup fileGroup; + + SECTION("addFile with valid ImgFrame works") { + auto frame = std::make_shared(); + frame->setType(ImgFrame::Type::BGR888i).setSize(4, 4); + std::vector data(4 * 4 * 3, 128); // Gray image + frame->setData(data); + REQUIRE_NOTHROW(fileGroup.addFile("test.jpg", frame)); + } + + SECTION("addFile with valid ImgDetections works") { + auto detections = std::make_shared(); + REQUIRE_NOTHROW(fileGroup.addFile("test.json", detections)); + } + + SECTION("addImageDetectionsPair with valid inputs works") { + auto frame = std::make_shared(); + frame->setType(ImgFrame::Type::BGR888i).setSize(4, 4); + std::vector data(4 * 4 * 3, 128); + frame->setData(data); + auto detections = std::make_shared(); + REQUIRE_NOTHROW(fileGroup.addImageDetectionsPair("test", frame, detections)); + } + + SECTION("addFile with string data works") { + REQUIRE_NOTHROW(fileGroup.addFile("test.txt", "hello world", "text/plain")); + } +} From 8bd0f78f102121158f62d6583b38d7fbd596674a Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Thu, 29 Jan 2026 23:03:19 +0100 Subject: [PATCH 119/180] Bump FW to latest --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index f1c243987b..b52085a022 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+15fe39853211a90d17ffc737604554a842b9994a") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+a412c85954366d09defede43d775cd676d68173f") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 2ed857ad3f..8dbe0da3bf 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "01b9e8279083d42ff8a5f0725c1942559dae9ba3") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c2eb5a3661dbe60ee3a67f00a24d269a7ecce8e7") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From a847d2b5c727bf0197b8ce6b9f09411ed2339e67 Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Fri, 30 Jan 2026 00:19:53 +0100 Subject: [PATCH 120/180] Bump FW after merges --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index b52085a022..f88a558a22 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+a412c85954366d09defede43d775cd676d68173f") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+5d393dbecabf3343abfa9fd0746cbd82ba3b99ae") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 8dbe0da3bf..0ed0fb7e63 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "c2eb5a3661dbe60ee3a67f00a24d269a7ecce8e7") +set(DEPTHAI_DEVICE_SIDE_COMMIT "6d8681c877c0329ae9283946ba1e3af8d8a08214") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From aa8e4b9e4b9d203bfd59fcffb5e13037ce4758f7 Mon Sep 17 00:00:00 2001 From: AljazD Date: Fri, 30 Jan 2026 08:33:51 +0100 Subject: [PATCH 121/180] Added additional sanitizer flags --- cmake/toolchain/asan-ubsan.cmake | 12 +++++++++++- cmake/toolchain/tsan.cmake | 3 +-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/cmake/toolchain/asan-ubsan.cmake b/cmake/toolchain/asan-ubsan.cmake index 41cbad1b9b..c0cbbc5379 100644 --- a/cmake/toolchain/asan-ubsan.cmake +++ b/cmake/toolchain/asan-ubsan.cmake @@ -1,4 +1,13 @@ -set(_internal_flags_sanitizer "-fno-omit-frame-pointer -fsanitize=address,undefined -fno-sanitize=pointer-overflow") +set(_internal_flags_sanitizer_list + -fno-omit-frame-pointer + -fno-optimize-sibling-calls + -fsanitize=address,undefined,leak + -fsanitize-address-use-after-scope + -fsanitize=float-divide-by-zero + -fno-sanitize=pointer-overflow + -fno-sanitize-recover=all +) +string(JOIN " " _internal_flags_sanitizer ${_internal_flags_sanitizer_list}) set(CMAKE_C_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_CXX_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_C_FLAGS_INIT ${_internal_flags_sanitizer}) @@ -13,4 +22,5 @@ set(DEPTHAI_SANITIZE ON) set(SANITIZE_ADDRESS ON) set(SANITIZE_UNDEFINED ON) set(SANITIZE_THREAD OFF) +set(_internal_flags_sanitizer_list) set(_internal_flags_sanitizer) diff --git a/cmake/toolchain/tsan.cmake b/cmake/toolchain/tsan.cmake index 3d23e51946..13adfd9678 100644 --- a/cmake/toolchain/tsan.cmake +++ b/cmake/toolchain/tsan.cmake @@ -1,4 +1,3 @@ - set(_internal_flags_sanitizer "-fno-omit-frame-pointer -fsanitize=thread") set(CMAKE_C_FLAGS ${_internal_flags_sanitizer}) set(CMAKE_CXX_FLAGS ${_internal_flags_sanitizer}) @@ -14,4 +13,4 @@ set(DEPTHAI_SANITIZE ON) set(SANITIZE_ADDRESS OFF) set(SANITIZE_UNDEFINED OFF) set(SANITIZE_THREAD ON) -set(_internal_flags_sanitizer) \ No newline at end of file +set(_internal_flags_sanitizer) From e816980ac09dc9a795555060131461b3aa3256b0 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 30 Jan 2026 10:55:49 +0100 Subject: [PATCH 122/180] Rename and correctly tag the test --- tests/CMakeLists.txt | 8 ++++---- .../node/{gate_note_tests.cpp => gate_node_tests.cpp} | 0 2 files changed, 4 insertions(+), 4 deletions(-) rename tests/src/ondevice_tests/pipeline/node/{gate_note_tests.cpp => gate_node_tests.cpp} (100%) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b06843d701..7cc59af7aa 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -451,10 +451,6 @@ dai_set_test_labels(nndata_test onhost ci) dai_add_test(imgdetections_test src/onhost_tests/pipeline/datatype/imgdetections_test.cpp) dai_set_test_labels(imgdetections_test onhost ci) -#Gate tests -dai_add_test(gate_node_test src/ondevice_tests/pipeline/node/gate_note_tests.cpp) -dai_set_test_labels(gate_node_test onhost ci) - #ImgFrame tests dai_add_test(imgframe_test src/onhost_tests/pipeline/datatype/imgframe_test.cpp) dai_set_test_labels(imgframe_test onhost ci) @@ -754,3 +750,7 @@ endif() # NeuralAssistedStereo test dai_add_test(neural_assisted_stereo_node_test src/ondevice_tests/pipeline/node/neural_assisted_stereo_node_test.cpp) dai_set_test_labels(neural_assisted_stereo_node_test ondevice rvc4 ci) + +# Gate tests +dai_add_test(gate_node_test src/ondevice_tests/pipeline/node/gate_node_tests.cpp) +dai_set_test_labels(gate_node_test ondevice rvc4 rvc2_all ci) diff --git a/tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_node_tests.cpp similarity index 100% rename from tests/src/ondevice_tests/pipeline/node/gate_note_tests.cpp rename to tests/src/ondevice_tests/pipeline/node/gate_node_tests.cpp From a46a3844a0b24cdc570dcdcb7430e46263248c8d Mon Sep 17 00:00:00 2001 From: AljazD Date: Fri, 30 Jan 2026 11:06:48 +0100 Subject: [PATCH 123/180] Added bspatch.c fix --- src/bspatch/bspatch.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/bspatch/bspatch.c b/src/bspatch/bspatch.c index a89c5587a8..eb3c8086a9 100644 --- a/src/bspatch/bspatch.c +++ b/src/bspatch/bspatch.c @@ -28,6 +28,7 @@ #include "bspatch.h" #include +#include #include #include @@ -156,6 +157,7 @@ int bspatch_mem(const uint8_t* oldfile_bin, const int64_t oldfile_size, const ui oldpos = 0; newpos = 0; + const bool has_extra_block = p_decompressed_block[EXTRA_BLOCK] != NULL; while(newpos < newsize) { /* Read control data */ for(i = 0; i <= 2; i++) { @@ -213,10 +215,10 @@ int bspatch_mem(const uint8_t* oldfile_bin, const int64_t oldfile_size, const ui } return -1; } - if(p_decompressed_block[EXTRA_BLOCK] != NULL) { + if(has_extra_block) { memcpy(new + newpos, p_decompressed_block[EXTRA_BLOCK], ctrl[1]); + p_decompressed_block[EXTRA_BLOCK] += ctrl[1]; } - p_decompressed_block[EXTRA_BLOCK] += ctrl[1]; /* Adjust pointers */ newpos += ctrl[1]; From 4d4289c013a05fb134afc9757745b2d813828bb7 Mon Sep 17 00:00:00 2001 From: Danilo Pejovic <115164734+danilo-pejovic@users.noreply.github.com> Date: Fri, 30 Jan 2026 13:18:41 +0100 Subject: [PATCH 124/180] Modify RVC4 test command to use no-shell option (#1658) --- .github/workflows/test_child_windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test_child_windows.yml b/.github/workflows/test_child_windows.yml index 2a1900015e..e5482e1beb 100644 --- a/.github/workflows/test_child_windows.yml +++ b/.github/workflows/test_child_windows.yml @@ -176,4 +176,4 @@ jobs: run: | source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-windows-${{ matrix.rvc4os }}" - exec hil --models "oak4_pro or oak4_d" -os windows --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --commands "rmdir /S /Q depthai-core & git clone https://github.com/luxonis/depthai-core.git && cd depthai-core && git fetch origin ${{ github.sha }} && git checkout ${{ github.sha }} && scripts\hil\get_artifacts_and_run_tests.cmd \"${{ github.repository }}\" \"${{ github.token }}\" \"C:\depthai-core\" \"${{ needs.build_windows_tests.outputs.artifact_id }}\"" + exec hil --models "oak4_pro or oak4_d" -os windows --no-shell --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --commands "rmdir /S /Q depthai-core & git clone https://github.com/luxonis/depthai-core.git && cd depthai-core && git fetch origin ${{ github.sha }} && git checkout ${{ github.sha }} && scripts\hil\get_artifacts_and_run_tests.cmd \"${{ github.repository }}\" \"${{ github.token }}\" \"C:\depthai-core\" \"${{ needs.build_windows_tests.outputs.artifact_id }}\"" From e9bc679cb1defeacda36c6095e49119dd2b997a7 Mon Sep 17 00:00:00 2001 From: AljazD Date: Fri, 30 Jan 2026 13:33:42 +0100 Subject: [PATCH 125/180] Bump RVC4 FW --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 32eabb33f5..8c7b63c184 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+45960e1e9bde60e465457eb1fb4825fd0afc457b") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+77bc43dab4950b360fe3f15e1b6df454a52e35f6") From 4ec7c87f49d0abfd50f105c7ef8c41eb79673714 Mon Sep 17 00:00:00 2001 From: Jakob Socan Date: Fri, 30 Jan 2026 19:00:35 +0100 Subject: [PATCH 126/180] Added C++ HFR examples --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- examples/cpp/CMakeLists.txt | 1 + examples/cpp/HFR/CMakeLists.txt | 14 +++ examples/cpp/HFR/README.md | 31 ++++++ examples/cpp/HFR/hfr_nn.cpp | 57 ++++++++++ examples/cpp/HFR/hfr_save_encoded.cpp | 104 ++++++++++++++++++ examples/cpp/HFR/hfr_small_preview.cpp | 47 ++++++++ examples/python/{RVC4 => }/HFR/.gitignore | 0 examples/python/{RVC4 => }/HFR/README.md | 41 +------ examples/python/{RVC4 => }/HFR/hfr_nn.py | 4 + .../python/{RVC4 => }/HFR/hfr_save_encoded.py | 4 + .../{RVC4 => }/HFR/hfr_small_preview.py | 4 + 12 files changed, 268 insertions(+), 41 deletions(-) create mode 100644 examples/cpp/HFR/CMakeLists.txt create mode 100644 examples/cpp/HFR/README.md create mode 100644 examples/cpp/HFR/hfr_nn.cpp create mode 100644 examples/cpp/HFR/hfr_save_encoded.cpp create mode 100644 examples/cpp/HFR/hfr_small_preview.cpp rename examples/python/{RVC4 => }/HFR/.gitignore (100%) rename examples/python/{RVC4 => }/HFR/README.md (57%) rename examples/python/{RVC4 => }/HFR/hfr_nn.py (89%) rename examples/python/{RVC4 => }/HFR/hfr_save_encoded.py (93%) rename examples/python/{RVC4 => }/HFR/hfr_small_preview.py (85%) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index f88a558a22..488490f062 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+5d393dbecabf3343abfa9fd0746cbd82ba3b99ae") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+f341152dd9cf7f6a78582f6dec8f0271156a4dc8") diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 3b872f4efd..58ccc681a3 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -140,6 +140,7 @@ add_subdirectory(Events) add_subdirectory(FeatureTracker) add_subdirectory(ObjectTracker) add_subdirectory(HostNodes) +add_subdirectory(HFR) add_subdirectory(ImageManip) add_subdirectory(IMU) add_subdirectory(Misc) diff --git a/examples/cpp/HFR/CMakeLists.txt b/examples/cpp/HFR/CMakeLists.txt new file mode 100644 index 0000000000..b5894a415d --- /dev/null +++ b/examples/cpp/HFR/CMakeLists.txt @@ -0,0 +1,14 @@ +project(hfr_examples) +cmake_minimum_required(VERSION 3.10) + +## function: dai_add_example(example_name example_src enable_test use_pcl) +## function: dai_set_example_test_labels(example_name ...) + +dai_add_example(hfr_nn "hfr_nn.cpp" ON OFF) +dai_set_example_test_labels(hfr_nn ondevice rvc4 rvc4rgb ci) + +dai_add_example(hfr_save_encoded "hfr_save_encoded.cpp" ON OFF) +dai_set_example_test_labels(hfr_save_encoded ondevice rvc4 rvc4rgb ci) + +dai_add_example(hfr_small_preview "hfr_small_preview.cpp" ON OFF) +dai_set_example_test_labels(hfr_small_preview ondevice rvc4 rvc4rgb ci) diff --git a/examples/cpp/HFR/README.md b/examples/cpp/HFR/README.md new file mode 100644 index 0000000000..5eaf687ca2 --- /dev/null +++ b/examples/cpp/HFR/README.md @@ -0,0 +1,31 @@ +# HFR (High Frame Rate) examples on IMX586 on the RVC4 platform +This directory contains examples demonstrating high frame rate (HFR) capabilities on the IMX586 sensor using the RVC4 platform. +The examples showcase an early preview of the capabilities. + + +## HFR resolutions +The HFR mode introduces two new resolutions that can run at a high frame-rate. +At the current time, the resolutions cannot be scaled arbitrarily nor can the FPS be varied arbitrarily — it must be one of the two supported values. +We plan to add more flexibility in the future and it's possible to use `ImageManip` in the meantime. + +The current supported resolutions are: +* 1920x1080 @ 240 FPS +* 1280x720 @ 480 FPS + + +## Example descriptions +### Object Detection +The object detection [example](hfr_nn) demonstrates how to use the HFR mode with a YOLOv6 model for real-time object detection at **480 FPS**. + +### Small live preview +The small preview [example](hfr_small_preview) demonstrates how to use the HFR mode for a small live preview at **240 FPS** or **480 FPS**. + +### Video encoding +The video encoding [example](hfr_save_encoded) demonstrates how to use the HFR mode for video encoding at **240 FPS** or **480 FPS**. + +All three examples have a `BenchmarkIn` node included that prints the framerate and the latency. This is the expected output: +``` +[2025-08-14 23:31:49.487] [ThreadedNode] [warning] FPS: 474.3766 +[2025-08-14 23:31:49.487] [ThreadedNode] [warning] Messages took 1.0118543 s +[2025-08-14 23:31:49.487] [ThreadedNode] [warning] Average latency: 0.05904912 s +``` diff --git a/examples/cpp/HFR/hfr_nn.cpp b/examples/cpp/HFR/hfr_nn.cpp new file mode 100644 index 0000000000..f97c32446e --- /dev/null +++ b/examples/cpp/HFR/hfr_nn.cpp @@ -0,0 +1,57 @@ +#include + +#include "depthai/depthai.hpp" + +constexpr int FPS = 480; + +int main() { + dai::Pipeline pipeline; + + auto platform = pipeline.getDefaultDevice()->getPlatform(); + if(platform != dai::Platform::RVC4) { + throw std::runtime_error("This example is only supported on RVC4 devices"); + } + + dai::NNModelDescription modelDescription; + modelDescription.model = "yolov6-nano"; + modelDescription.platform = "RVC4"; + + auto nnArchivePath = getModelFromZoo(modelDescription); + dai::NNArchive nnArchive(nnArchivePath); + auto inputSize = nnArchive.getInputSize().value(); + + auto cameraNode = pipeline.create()->build(); + + // Configure the ImageManip as in HFR mode requesting arbitrary outputs is not yet supported + auto cameraOutput = cameraNode->requestOutput( + std::make_pair(1280, 720), + std::nullopt, + dai::ImgResizeMode::CROP, + static_cast(FPS)); + + auto imageManip = pipeline.create(); + imageManip->initialConfig->setOutputSize(std::get<0>(inputSize), std::get<1>(inputSize)); + imageManip->setMaxOutputFrameSize(static_cast(std::get<0>(inputSize) * std::get<1>(inputSize) * 3)); + imageManip->initialConfig->setFrameType(dai::ImgFrame::Type::BGR888i); + cameraOutput->link(imageManip->inputImage); + + auto detectionNetwork = pipeline.create(); + detectionNetwork->setNNArchive(nnArchive); + imageManip->out.link(detectionNetwork->input); + + auto benchmarkIn = pipeline.create(); + benchmarkIn->setRunOnHost(true); + benchmarkIn->sendReportEveryNMessages(FPS); + detectionNetwork->out.link(benchmarkIn->input); + + auto qDet = detectionNetwork->out.createOutputQueue(); + pipeline.start(); + + while(pipeline.isRunning()) { + auto inDet = qDet->get(); + if(inDet == nullptr) continue; + (void)inDet; + } + + return 0; +} diff --git a/examples/cpp/HFR/hfr_save_encoded.cpp b/examples/cpp/HFR/hfr_save_encoded.cpp new file mode 100644 index 0000000000..d8943694c5 --- /dev/null +++ b/examples/cpp/HFR/hfr_save_encoded.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/depthai.hpp" +#include "depthai/pipeline/datatype/MessageGroup.hpp" + +constexpr std::pair SIZE = {1280, 720}; +constexpr int FPS = 480; + +std::atomic quitEvent(false); + +void signalHandler(int signum) { + quitEvent = true; +} + +class VideoSaver : public dai::node::CustomNode { + public: + VideoSaver() : fileHandle("video_hfr.encoded", std::ios::binary) { + if(!fileHandle.is_open()) { + throw std::runtime_error("Could not open video_hfr.encoded for writing"); + } + } + + ~VideoSaver() { + if(fileHandle.is_open()) { + fileHandle.close(); + } + } + + std::shared_ptr processGroup(std::shared_ptr message) override { + if(!fileHandle.is_open()) return nullptr; + + auto frame = message->get("data"); + unsigned char* frameData = frame->getData().data(); + size_t frameSize = frame->getData().size(); + fileHandle.write(reinterpret_cast(frameData), frameSize); + + return nullptr; + } + + private: + std::ofstream fileHandle; +}; + +int main() { + signal(SIGTERM, signalHandler); + signal(SIGINT, signalHandler); + + dai::Pipeline pipeline; + + auto platform = pipeline.getDefaultDevice()->getPlatform(); + if(platform != dai::Platform::RVC4) { + throw std::runtime_error("This example is only supported on RVC4 devices"); + } + + auto camRgb = pipeline.create()->build(dai::CameraBoardSocket::CAM_A); + auto output = camRgb->requestOutput(SIZE, std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); + + // ImageManip is added to workaround a limitation with VideoEncoder with native resolutions + // This limitation will be lifted in the future + auto imageManip = pipeline.create(); + imageManip->initialConfig->setOutputSize(SIZE.first, SIZE.second + 10); + imageManip->setMaxOutputFrameSize(static_cast(SIZE.first * (SIZE.second + 10) * 1.6)); + output->link(imageManip->inputImage); + auto encodedInput = imageManip->out; + + auto benchmarkIn = pipeline.create(); + benchmarkIn->setRunOnHost(true); + + auto encoded = pipeline.create(); + encoded->setDefaultProfilePreset(static_cast(FPS), dai::VideoEncoderProperties::Profile::H264_MAIN); + encodedInput.link(encoded->input); + encoded->out.link(benchmarkIn->input); + + auto saver = pipeline.create(); + encoded->out.link(saver->inputs["data"]); + + pipeline.start(); + + std::cout << "Started to save video to video_hfr.encoded" << std::endl; + std::cout << "Press Ctrl+C to stop" << std::endl; + + while(pipeline.isRunning() && !quitEvent) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + pipeline.stop(); + pipeline.wait(); + + std::cout << "To view the encoded data, convert the stream file (.encoded) into a video file (.mp4) using a command below:" << std::endl; + std::cout << "ffmpeg -framerate " << FPS << " -i video_hfr.encoded -c copy video_hfr.mp4" << std::endl; + + std::cout << "If the FPS is not set correctly, you can ask ffmpeg to generate it with the command below" << std::endl; + + std::cout << "\nffmpeg -fflags +genpts -r " << FPS << " -i video_hfr.encoded \\\n -vsync cfr -fps_mode cfr \\\n -video_track_timescale " << FPS + << "00 \\\n -c:v copy \\\n video_hfr.mp4\n"; + + return 0; +} diff --git a/examples/cpp/HFR/hfr_small_preview.cpp b/examples/cpp/HFR/hfr_small_preview.cpp new file mode 100644 index 0000000000..9c022285c8 --- /dev/null +++ b/examples/cpp/HFR/hfr_small_preview.cpp @@ -0,0 +1,47 @@ +#include +#include + +#include "depthai/depthai.hpp" + +constexpr std::pair SIZE = {1280, 720}; +constexpr int FPS = 480; + +int main() { + dai::Pipeline pipeline; + + auto platform = pipeline.getDefaultDevice()->getPlatform(); + if(platform != dai::Platform::RVC4) { + throw std::runtime_error("This example is only supported on RVC4 devices"); + } + + auto cam = pipeline.create()->build(); + auto benchmarkIn = pipeline.create(); + benchmarkIn->setRunOnHost(true); + benchmarkIn->sendReportEveryNMessages(FPS); + + auto imageManip = pipeline.create(); + imageManip->initialConfig->setOutputSize(250, 250); + imageManip->setMaxOutputFrameSize(static_cast(250 * 250 * 1.6)); + + // One of the two modes can be selected + // NOTE: Generic resolutions are not yet supported through camera node when using HFR mode + auto output = cam->requestOutput(SIZE, std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); + + output->link(imageManip->inputImage); + imageManip->out.link(benchmarkIn->input); + + auto outputQueue = imageManip->out.createOutputQueue(); + + pipeline.start(); + + while(pipeline.isRunning()) { + auto imgFrame = outputQueue->get(); + if(imgFrame == nullptr) continue; + cv::imshow("frame", imgFrame->getCvFrame()); + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} diff --git a/examples/python/RVC4/HFR/.gitignore b/examples/python/HFR/.gitignore similarity index 100% rename from examples/python/RVC4/HFR/.gitignore rename to examples/python/HFR/.gitignore diff --git a/examples/python/RVC4/HFR/README.md b/examples/python/HFR/README.md similarity index 57% rename from examples/python/RVC4/HFR/README.md rename to examples/python/HFR/README.md index 8fac981517..1ed10ede03 100644 --- a/examples/python/RVC4/HFR/README.md +++ b/examples/python/HFR/README.md @@ -13,45 +13,6 @@ The current supported resolutions are: * 1280x720 @ 480 FPS -## Setup -To set up the HFR mode both a custom version of this (DepthAI) repository and a custom version of the LuxonisOS are required. - -### Installation of LuxonisOS: -First download the HFR version of the OS [here](https://luxonisos-rvc4.fra1.digitaloceanspaces.com/OTA/full_update_luxonis_ext4-1.17.0-imx+16bc91752168bda919ce87196bf57d18fb9c43ec.zip) -``` -wget https://luxonisos-rvc4.fra1.digitaloceanspaces.com/OTA/full_update_luxonis_ext4-1.17.0-imx+16bc91752168bda919ce87196bf57d18fb9c43ec.zip -``` - -then copy the OS to the device - -``` -scp full_update_luxonis_ext4-1.17.0-imx+16bc91752168bda919ce87196bf57d18fb9c43ec.zip root@:/data/hfr_os.zip -``` - -ssh into the device and update the OS - -``` -ssh root@ -recovery --update_package=/data/hfr_os.zip --wipe_cache && reboot -``` - -### DepthAI installation -Clone this repository and checkout the `feature/imx586_hfr` branch - -``` -git clone https://github.com/luxonis/depthai-core.git -cd depthai-core -git checkout feature/imx586_hfr -``` - -and install requirements - -``` -python examples/python/install_requirements.py -``` - -The examples can then be found in the `examples/python/RVC4/HFR` directory. - ## Example descriptions ### Object Detection The object detection [example](hfr_nn.py) demonstrates how to use the HFR mode with a YOLOv6 model for real-time object detection at **480 FPS**. @@ -67,4 +28,4 @@ All three examples have a `BenchmarkIn` node included that prints the framerate [2025-08-14 23:31:49.487] [ThreadedNode] [warning] FPS: 474.3766 [2025-08-14 23:31:49.487] [ThreadedNode] [warning] Messages took 1.0118543 s [2025-08-14 23:31:49.487] [ThreadedNode] [warning] Average latency: 0.05904912 s -``` \ No newline at end of file +``` diff --git a/examples/python/RVC4/HFR/hfr_nn.py b/examples/python/HFR/hfr_nn.py similarity index 89% rename from examples/python/RVC4/HFR/hfr_nn.py rename to examples/python/HFR/hfr_nn.py index ef4a9a9afc..fbfbf136cd 100644 --- a/examples/python/RVC4/HFR/hfr_nn.py +++ b/examples/python/HFR/hfr_nn.py @@ -4,6 +4,10 @@ FPS = 480 with dai.Pipeline() as pipeline: + platform = pipeline.getDefaultDevice().getPlatform() + if platform != dai.Platform.RVC4: + raise RuntimeError("This example is only supported on RVC4 devices") + # Download the model nnArchivePath = dai.getModelFromZoo(dai.NNModelDescription("yolov6-nano", platform="RVC4")) nnArchive = dai.NNArchive(nnArchivePath) diff --git a/examples/python/RVC4/HFR/hfr_save_encoded.py b/examples/python/HFR/hfr_save_encoded.py similarity index 93% rename from examples/python/RVC4/HFR/hfr_save_encoded.py rename to examples/python/HFR/hfr_save_encoded.py index 0dd51a5d43..8ffc9ed287 100644 --- a/examples/python/RVC4/HFR/hfr_save_encoded.py +++ b/examples/python/HFR/hfr_save_encoded.py @@ -31,6 +31,10 @@ def process(self, frame): frame.getData().tofile(self.file_handle) with dai.Pipeline() as pipeline: + platform = pipeline.getDefaultDevice().getPlatform() + if platform != dai.Platform.RVC4: + raise RuntimeError("This example is only supported on RVC4 devices") + camRgb = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A) output = camRgb.requestOutput(SIZE, fps=FPS) diff --git a/examples/python/RVC4/HFR/hfr_small_preview.py b/examples/python/HFR/hfr_small_preview.py similarity index 85% rename from examples/python/RVC4/HFR/hfr_small_preview.py rename to examples/python/HFR/hfr_small_preview.py index c0e667fcbc..a135ceb20f 100644 --- a/examples/python/RVC4/HFR/hfr_small_preview.py +++ b/examples/python/HFR/hfr_small_preview.py @@ -9,6 +9,10 @@ # SIZE = (1920, 1080) # FPS = 240 with dai.Pipeline() as pipeline: + platform = pipeline.getDefaultDevice().getPlatform() + if platform != dai.Platform.RVC4: + raise RuntimeError("This example is only supported on RVC4 devices") + cam = pipeline.create(dai.node.Camera).build() benchmarkIn = pipeline.create(dai.node.BenchmarkIn) benchmarkIn.setRunOnHost(True) From b233ad401e7e28c838d18a89c2136f9172102107 Mon Sep 17 00:00:00 2001 From: Jakob Socan Date: Fri, 30 Jan 2026 19:02:11 +0100 Subject: [PATCH 127/180] Removed some limits on RVC4 camera test --- .../pipeline/node/camera_fps_config_test.cpp | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp b/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp index 38de3e114c..cbdd9c88f0 100644 --- a/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp @@ -33,20 +33,8 @@ TEST_CASE("Camera sensor configs configurations") { for(const auto& cameraFeatures : connectedCameraFeautres) { for(const auto& config : cameraFeatures.configs) { - float maxFps = config.maxFps; - if(config.maxFps > 240.5f) { - std::cout << "Skipping testing high fps on camera " << cameraFeatures.socket << " with max fps " << config.maxFps << " on device " - << connectedDeviceInfo.getDeviceId() << "\n" - << std::flush; - continue; - } - if(config.maxFps > 120.0f) { - // round down to nearest integer fps for high fps values to avoid issues with non integer fps settings - // TODO(Jakob) - fix this on device side when HFR is implemented - maxFps = static_cast(static_cast(config.maxFps)); - } auto minimumFps = connectedDeviceInfo.platform == X_LINK_RVC4 ? std::max(MINIMUM_FPS_RVC4, config.minFps) : config.minFps; - for(const auto& fpsVariant : {maxFps, minimumFps}) { + for(const auto& fpsVariant : {config.maxFps, minimumFps}) { std::cout << "Testing camera " << cameraFeatures.socket << " with resolution " << config.width << "x" << config.height << " at fps " << fpsVariant << " on device " << connectedDeviceInfo.getDeviceId() << "\n" << std::flush; From e05a7bf43ee1b7d7378a175a687db69a435284cb Mon Sep 17 00:00:00 2001 From: AljazD Date: Sun, 1 Feb 2026 17:25:20 +0100 Subject: [PATCH 128/180] Added fd initialization inside XLinkInHost::parsePacketizedData() --- src/pipeline/node/internal/XLinkInHost.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/pipeline/node/internal/XLinkInHost.cpp b/src/pipeline/node/internal/XLinkInHost.cpp index c78b1af575..03eb6b8585 100644 --- a/src/pipeline/node/internal/XLinkInHost.cpp +++ b/src/pipeline/node/internal/XLinkInHost.cpp @@ -70,9 +70,10 @@ std::shared_ptr XLinkInHost::parsePacketizedData(const std::shared_pt + std::to_string(packetizedData->totalSize) + ", but received size: " + std::to_string(currentSize) + "."); } - streamPacketDesc_t packet; + streamPacketDesc_t packet{}; packet.data = payload.data(); packet.length = static_cast(payload.size()); + packet.fd = -1; return StreamMessageParser::parseMessage(&packet); } From cec417c4c7311e25f77d36d5abd51fbb5b33294a Mon Sep 17 00:00:00 2001 From: AljazD Date: Sun, 1 Feb 2026 19:17:03 +0100 Subject: [PATCH 129/180] Updated cached file naming & extensions in EventsManager --- include/depthai/utility/EventsManager.hpp | 2 +- src/utility/EventsManager.cpp | 26 +++++++++++++++++------ 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 75b072b861..5172a90da7 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -193,7 +193,7 @@ class EventsManager { /** * Fetch the configuration limits and quotas for snaps & events - * @param retryOnFail Retry fetching on failure + * @param retryOnFail Retry fetching on failure; when true, keeps retrying until successful * @return bool */ bool fetchConfigurationLimits(const bool retryOnFail); diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 3c093cf29f..822503cb35 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -221,10 +221,24 @@ bool FileData::toFile(const std::filesystem::path& inputPath) { return false; } std::string extension; - if(mimeType == "image/jpeg") { - extension = ".jpg"; - } else if(mimeType == "application/x-protobuf; proto=SnapAnnotation") { - extension = ".annotation"; + static const std::unordered_map mimeTypeToExtensionMap = { + {"image/jpeg", ".jpg"}, + {"image/png", ".png"}, + {"image/webp", ".webp"}, + {"image/bmp", ".bmp"}, + {"image/tiff", ".tiff"}, + {"image/gif", ".gif"}, + {"image/svg+xml", ".svg"}, + {"application/json", ".json"}, + {"text/plain", ".txt"}, + {"text/html", ".html"}, + {"text/css", ".css"}, + {"application/javascript", ".js"}, + {"application/x-protobuf; proto=SnapAnnotation", ".annotation"}, + }; + auto mimeIt = mimeTypeToExtensionMap.find(mimeType); + if(mimeIt != mimeTypeToExtensionMap.end()) { + extension = mimeIt->second; } // Choose a unique filename std::filesystem::path target = inputPath / (fileName + extension); @@ -906,9 +920,9 @@ void EventsManager::cacheEvents() { std::lock_guard lock(eventBufferMutex); for(const auto& eventData : eventBuffer) { std::filesystem::path inputPath(cacheDir); - std::filesystem::path path = inputPath / ("event_" + eventData->event->name() + "_" + std::to_string(eventData->event->created_at())); + std::filesystem::path path = inputPath / (fmt::format("event_{}_{}", eventData->event->name(), eventData->event->created_at())); for(int i = 1; std::filesystem::exists(path); ++i) { - path = inputPath / ("event_" + eventData->event->name() + "_" + std::to_string(eventData->event->created_at()) + "_" + std::to_string(i)); + path = inputPath / (fmt::format("event_{}_{}_{}", eventData->event->name(), eventData->event->created_at(), i)); } std::string eventDir = path.string(); logger::info("Caching event to {}", eventDir); From 67abc8660e4a877dad858e5cd285c6c0b570532e Mon Sep 17 00:00:00 2001 From: AljazD Date: Mon, 2 Feb 2026 08:48:00 +0100 Subject: [PATCH 130/180] Added libfontconfig.so to the sanitizer suppressed libraries --- tests/lsan.supp | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/lsan.supp b/tests/lsan.supp index 65aa706646..8a0b935b0c 100644 --- a/tests/lsan.supp +++ b/tests/lsan.supp @@ -1,3 +1,4 @@ leak:libGLX_mesa.so leak:g_malloc leak:g_realloc +leak:libfontconfig.so From ffaecf9441645940b97e658eece231dff02ecef4 Mon Sep 17 00:00:00 2001 From: AljazD Date: Mon, 2 Feb 2026 11:57:08 +0100 Subject: [PATCH 131/180] MimeType maps are now grouped together. Added application/octet-stream type --- src/utility/EventsManager.cpp | 66 ++++++++++++++++++++--------------- 1 file changed, 37 insertions(+), 29 deletions(-) diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 822503cb35..5fb81decc6 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -16,6 +16,41 @@ std::string generateLocalID(int64_t timestamp) { oss << timestamp << "_" << std::setw(6) << std::setfill('0') << counter++; return oss.str(); } + +const std::unordered_map extensionToMimeTypeMap = { + {".html", "text/html"}, + {".css", "text/css"}, + {".js", "application/javascript"}, + {".png", "image/png"}, + {".jpg", "image/jpeg"}, + {".jpeg", "image/jpeg"}, + {".gif", "image/gif"}, + {".webp", "image/webp"}, + {".bmp", "image/bmp"}, + {".tiff", "image/tiff"}, + {".svg", "image/svg+xml"}, + {".json", "application/json"}, + {".txt", "text/plain"}, + {".annotation", "application/x-protobuf; proto=SnapAnnotation"}, + {"", "application/octet-stream"}, +}; + +const std::unordered_map mimeTypeToExtensionMap = { + {"text/html", ".html"}, + {"text/css", ".css"}, + {"application/javascript", ".js"}, + {"image/png", ".png"}, + {"image/jpeg", ".jpg"}, + {"image/gif", ".gif"}, + {"image/webp", ".webp"}, + {"image/bmp", ".bmp"}, + {"image/tiff", ".tiff"}, + {"image/svg+xml", ".svg"}, + {"application/json", ".json"}, + {"text/plain", ".txt"}, + {"application/x-protobuf; proto=SnapAnnotation", ".annotation"}, + {"application/octet-stream", ""}, +}; } // namespace #include "Environment.hpp" @@ -109,18 +144,6 @@ FileData::FileData(std::string data, std::string fileName, std::string mimeType) classification(proto::event::PrepareFileUploadClass::UNKNOWN_FILE) {} FileData::FileData(std::filesystem::path filePath, std::string fileName) : fileName(std::move(fileName)) { - static const std::unordered_map mimeTypeExtensionMap = {{".html", "text/html"}, - {".htm", "text/html"}, - {".css", "text/css"}, - {".js", "application/javascript"}, - {".png", "image/png"}, - {".jpg", "image/jpeg"}, - {".jpeg", "image/jpeg"}, - {".gif", "image/gif"}, - {".svg", "image/svg+xml"}, - {".json", "application/json"}, - {".txt", "text/plain"}, - {".annotation", "application/x-protobuf; proto=SnapAnnotation"}}; // Read the data std::ifstream fileStream(filePath, std::ios::binary | std::ios::ate); if(!fileStream) { @@ -133,8 +156,8 @@ FileData::FileData(std::filesystem::path filePath, std::string fileName) : fileN size = data.size(); checksum = calculateSHA256Checksum(data); // Determine the mime type - auto it = mimeTypeExtensionMap.find(filePath.extension().string()); - if(it != mimeTypeExtensionMap.end()) { + auto it = extensionToMimeTypeMap.find(filePath.extension().string()); + if(it != extensionToMimeTypeMap.end()) { mimeType = it->second; } else { mimeType = "application/octet-stream"; @@ -221,21 +244,6 @@ bool FileData::toFile(const std::filesystem::path& inputPath) { return false; } std::string extension; - static const std::unordered_map mimeTypeToExtensionMap = { - {"image/jpeg", ".jpg"}, - {"image/png", ".png"}, - {"image/webp", ".webp"}, - {"image/bmp", ".bmp"}, - {"image/tiff", ".tiff"}, - {"image/gif", ".gif"}, - {"image/svg+xml", ".svg"}, - {"application/json", ".json"}, - {"text/plain", ".txt"}, - {"text/html", ".html"}, - {"text/css", ".css"}, - {"application/javascript", ".js"}, - {"application/x-protobuf; proto=SnapAnnotation", ".annotation"}, - }; auto mimeIt = mimeTypeToExtensionMap.find(mimeType); if(mimeIt != mimeTypeToExtensionMap.end()) { extension = mimeIt->second; From 2203afb3c7d9a23b99f511cc2194951792932731 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 2 Feb 2026 12:50:21 +0100 Subject: [PATCH 132/180] Formatting --- examples/cpp/HFR/hfr_nn.cpp | 6 +----- examples/cpp/HFR/hfr_small_preview.cpp | 2 +- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/examples/cpp/HFR/hfr_nn.cpp b/examples/cpp/HFR/hfr_nn.cpp index f97c32446e..b1124d972b 100644 --- a/examples/cpp/HFR/hfr_nn.cpp +++ b/examples/cpp/HFR/hfr_nn.cpp @@ -23,11 +23,7 @@ int main() { auto cameraNode = pipeline.create()->build(); // Configure the ImageManip as in HFR mode requesting arbitrary outputs is not yet supported - auto cameraOutput = cameraNode->requestOutput( - std::make_pair(1280, 720), - std::nullopt, - dai::ImgResizeMode::CROP, - static_cast(FPS)); + auto cameraOutput = cameraNode->requestOutput(std::make_pair(1280, 720), std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); auto imageManip = pipeline.create(); imageManip->initialConfig->setOutputSize(std::get<0>(inputSize), std::get<1>(inputSize)); diff --git a/examples/cpp/HFR/hfr_small_preview.cpp b/examples/cpp/HFR/hfr_small_preview.cpp index 9c022285c8..4a8c5b4fbc 100644 --- a/examples/cpp/HFR/hfr_small_preview.cpp +++ b/examples/cpp/HFR/hfr_small_preview.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include "depthai/depthai.hpp" From d995f7a08569859f80d0588498089692e470380f Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 2 Feb 2026 12:50:31 +0100 Subject: [PATCH 133/180] Fix the test for RVC2 --- .../pipeline/node/camera_fps_config_test.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp b/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp index cbdd9c88f0..9463232a43 100644 --- a/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/camera_fps_config_test.cpp @@ -4,6 +4,7 @@ #include #include +#include "XLink/XLinkPublicDefines.h" #include "depthai/capabilities/ImgFrameCapability.hpp" #include "depthai/common/CameraBoardSocket.hpp" #include "depthai/common/CameraFeatures.hpp" @@ -15,13 +16,14 @@ TEST_CASE("Camera sensor configs configurations") { // Test that cameras can stream at all advertised configs - // For RVC4 there are currently some limitations: - // Max 240 FPS and it has to be rounded down - // Minimum FPS is incorrectly advertised, so this test sets the minimum to 5 FPS. using std::chrono::steady_clock; - const auto RESET_REMOTE_TIMEOUT_MS = std::chrono::milliseconds(2000); - + constexpr auto RESET_REMOTE_TIMEOUT_MS = std::chrono::milliseconds(2000); + // Minimum FPS is incorrectly advertised, so this test sets the minimum to 8 FPS. constexpr float MINIMUM_FPS_RVC4 = 8.0f; // TODO(Jakob) - remove this when fixed on device side + + // Maximum FPS on RVC2 is 120, because of 3A limitations + constexpr float MAXIMUM_FPS_RVC2 = 120.0f; + dai::DeviceInfo connectedDeviceInfo; std::vector connectedCameraFeautres; { @@ -34,7 +36,8 @@ TEST_CASE("Camera sensor configs configurations") { for(const auto& cameraFeatures : connectedCameraFeautres) { for(const auto& config : cameraFeatures.configs) { auto minimumFps = connectedDeviceInfo.platform == X_LINK_RVC4 ? std::max(MINIMUM_FPS_RVC4, config.minFps) : config.minFps; - for(const auto& fpsVariant : {config.maxFps, minimumFps}) { + auto maximumFps = connectedDeviceInfo.platform == X_LINK_MYRIAD_X ? std::min(MAXIMUM_FPS_RVC2, config.maxFps) : config.maxFps; + for(const auto& fpsVariant : {maximumFps, minimumFps}) { std::cout << "Testing camera " << cameraFeatures.socket << " with resolution " << config.width << "x" << config.height << " at fps " << fpsVariant << " on device " << connectedDeviceInfo.getDeviceId() << "\n" << std::flush; From 7e7a18ecbcb85e66d7b7971420bf3a82404b2060 Mon Sep 17 00:00:00 2001 From: Jakob Socan Date: Mon, 2 Feb 2026 15:20:11 +0100 Subject: [PATCH 134/180] Improved HFR examples --- examples/cpp/HFR/hfr_nn.cpp | 10 ++++------ examples/cpp/HFR/hfr_save_encoded.cpp | 6 ++++-- examples/cpp/HFR/hfr_small_preview.cpp | 7 ++++--- examples/python/HFR/hfr_nn.py | 1 + examples/python/HFR/hfr_save_encoded.py | 1 + 5 files changed, 14 insertions(+), 11 deletions(-) diff --git a/examples/cpp/HFR/hfr_nn.cpp b/examples/cpp/HFR/hfr_nn.cpp index f97c32446e..ae6e951b7e 100644 --- a/examples/cpp/HFR/hfr_nn.cpp +++ b/examples/cpp/HFR/hfr_nn.cpp @@ -9,7 +9,8 @@ int main() { auto platform = pipeline.getDefaultDevice()->getPlatform(); if(platform != dai::Platform::RVC4) { - throw std::runtime_error("This example is only supported on RVC4 devices"); + std::cerr << "This example is only supported on RVC4 devices\n" << std::flush; + return -1; } dai::NNModelDescription modelDescription; @@ -23,16 +24,13 @@ int main() { auto cameraNode = pipeline.create()->build(); // Configure the ImageManip as in HFR mode requesting arbitrary outputs is not yet supported - auto cameraOutput = cameraNode->requestOutput( - std::make_pair(1280, 720), - std::nullopt, - dai::ImgResizeMode::CROP, - static_cast(FPS)); + auto* cameraOutput = cameraNode->requestOutput(std::make_pair(1280, 720), std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); auto imageManip = pipeline.create(); imageManip->initialConfig->setOutputSize(std::get<0>(inputSize), std::get<1>(inputSize)); imageManip->setMaxOutputFrameSize(static_cast(std::get<0>(inputSize) * std::get<1>(inputSize) * 3)); imageManip->initialConfig->setFrameType(dai::ImgFrame::Type::BGR888i); + imageManip->inputImage.setMaxSize(12); cameraOutput->link(imageManip->inputImage); auto detectionNetwork = pipeline.create(); diff --git a/examples/cpp/HFR/hfr_save_encoded.cpp b/examples/cpp/HFR/hfr_save_encoded.cpp index d8943694c5..6376b7a2ac 100644 --- a/examples/cpp/HFR/hfr_save_encoded.cpp +++ b/examples/cpp/HFR/hfr_save_encoded.cpp @@ -55,17 +55,19 @@ int main() { auto platform = pipeline.getDefaultDevice()->getPlatform(); if(platform != dai::Platform::RVC4) { - throw std::runtime_error("This example is only supported on RVC4 devices"); + std::cerr << "This example is only supported on RVC4 devices\n" << std::flush; + return -1; } auto camRgb = pipeline.create()->build(dai::CameraBoardSocket::CAM_A); - auto output = camRgb->requestOutput(SIZE, std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); + auto* output = camRgb->requestOutput(SIZE, std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); // ImageManip is added to workaround a limitation with VideoEncoder with native resolutions // This limitation will be lifted in the future auto imageManip = pipeline.create(); imageManip->initialConfig->setOutputSize(SIZE.first, SIZE.second + 10); imageManip->setMaxOutputFrameSize(static_cast(SIZE.first * (SIZE.second + 10) * 1.6)); + imageManip->inputImage.setMaxSize(12); output->link(imageManip->inputImage); auto encodedInput = imageManip->out; diff --git a/examples/cpp/HFR/hfr_small_preview.cpp b/examples/cpp/HFR/hfr_small_preview.cpp index 9c022285c8..ec19c4aa14 100644 --- a/examples/cpp/HFR/hfr_small_preview.cpp +++ b/examples/cpp/HFR/hfr_small_preview.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include "depthai/depthai.hpp" @@ -11,7 +11,8 @@ int main() { auto platform = pipeline.getDefaultDevice()->getPlatform(); if(platform != dai::Platform::RVC4) { - throw std::runtime_error("This example is only supported on RVC4 devices"); + std::cerr << "This example is only supported on RVC4 devices\n" << std::flush; + return -1; } auto cam = pipeline.create()->build(); @@ -25,7 +26,7 @@ int main() { // One of the two modes can be selected // NOTE: Generic resolutions are not yet supported through camera node when using HFR mode - auto output = cam->requestOutput(SIZE, std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); + auto* output = cam->requestOutput(SIZE, std::nullopt, dai::ImgResizeMode::CROP, static_cast(FPS)); output->link(imageManip->inputImage); imageManip->out.link(benchmarkIn->input); diff --git a/examples/python/HFR/hfr_nn.py b/examples/python/HFR/hfr_nn.py index fbfbf136cd..18cbd13d3e 100644 --- a/examples/python/HFR/hfr_nn.py +++ b/examples/python/HFR/hfr_nn.py @@ -20,6 +20,7 @@ imageManip.initialConfig.setOutputSize(inputSize[0], inputSize[1]) imageManip.setMaxOutputFrameSize(int(inputSize[0] * inputSize[1] * 3)) imageManip.initialConfig.setFrameType(dai.ImgFrame.Type.BGR888i) + imageManip.inputImage.setMaxSize(12) cameraOutput.link(imageManip.inputImage) # Configure the DetectionNetwork diff --git a/examples/python/HFR/hfr_save_encoded.py b/examples/python/HFR/hfr_save_encoded.py index 8ffc9ed287..1f2a52c6d0 100644 --- a/examples/python/HFR/hfr_save_encoded.py +++ b/examples/python/HFR/hfr_save_encoded.py @@ -43,6 +43,7 @@ def process(self, frame): imageManip = pipeline.create(dai.node.ImageManip) imageManip.initialConfig.setOutputSize(SIZE[0], SIZE[1] + 10) # To avoid a passthrough imageManip.setMaxOutputFrameSize(int(SIZE[0] * (SIZE[1] + 10) * 1.6)) + imageManip.inputImage.setMaxSize(12) output.link(imageManip.inputImage) output = imageManip.out From 7c08892ac270b26807a42fed4d3cd947320758f4 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 2 Feb 2026 15:53:33 +0100 Subject: [PATCH 135/180] Formatting --- examples/python/HFR/hfr_nn.py | 2 +- examples/python/HFR/hfr_small_preview.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/python/HFR/hfr_nn.py b/examples/python/HFR/hfr_nn.py index 18cbd13d3e..c61a07ee69 100644 --- a/examples/python/HFR/hfr_nn.py +++ b/examples/python/HFR/hfr_nn.py @@ -13,7 +13,7 @@ nnArchive = dai.NNArchive(nnArchivePath) inputSize = nnArchive.getInputSize() cameraNode = pipeline.create(dai.node.Camera).build() - + # Configure the ImageManip as in HFR mode requesting arbitrary outputs is not yet supported cameraOutput = cameraNode.requestOutput((1280, 720), fps=FPS) imageManip = pipeline.create(dai.node.ImageManip) diff --git a/examples/python/HFR/hfr_small_preview.py b/examples/python/HFR/hfr_small_preview.py index a135ceb20f..449a88f714 100644 --- a/examples/python/HFR/hfr_small_preview.py +++ b/examples/python/HFR/hfr_small_preview.py @@ -17,7 +17,7 @@ benchmarkIn = pipeline.create(dai.node.BenchmarkIn) benchmarkIn.setRunOnHost(True) benchmarkIn.sendReportEveryNMessages(FPS) - + imageManip = pipeline.create(dai.node.ImageManip) imageManip.initialConfig.setOutputSize(250, 250) imageManip.setMaxOutputFrameSize(int(250* 250 * 1.6)) From 9b125d050c48c28f668a4bca39f8e1da52ee04cc Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 2 Feb 2026 16:44:33 +0100 Subject: [PATCH 136/180] Bump RVC2,4 FW --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 94f7f63ca8..9bc0128c03 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+bb2705f681f80507098035bc33c1a27c1b0863fc") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+679d546bcfed18052a0282acd7143b4b6b8c4a4d") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index f9a2999b1b..94bac62dfd 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "f81edb9c2328ee1f80a6f80c3fade0af0076a3ff") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b3d7c6a54185ca2f91ec5d20743da3cee7f0e67e") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From a3d45d2104e01e69228f7e06497edd780e81cf5d Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 2 Feb 2026 17:56:16 +0100 Subject: [PATCH 137/180] Formatting --- bindings/python/src/pipeline/PipelineBindings.cpp | 11 +++++++++-- include/depthai/pipeline/Pipeline.hpp | 4 ++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/bindings/python/src/pipeline/PipelineBindings.cpp b/bindings/python/src/pipeline/PipelineBindings.cpp index 43cb97ae32..15e69ecdb4 100644 --- a/bindings/python/src/pipeline/PipelineBindings.cpp +++ b/bindings/python/src/pipeline/PipelineBindings.cpp @@ -216,8 +216,15 @@ void PipelineBindings::bind(pybind11::module& m, void* pCallstack) { static_cast(&Pipeline::getAssetManager), py::return_value_policy::reference_internal, DOC(dai, Pipeline, getAssetManager)) - .def("setCameraTuningBlobPath", py::overload_cast(&Pipeline::setCameraTuningBlobPath), py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath)) - .def("setCameraTuningBlobPath", py::overload_cast(&Pipeline::setCameraTuningBlobPath), py::arg("socket"), py::arg("path"), DOC(dai, Pipeline, setCameraTuningBlobPath, 2)) + .def("setCameraTuningBlobPath", + py::overload_cast(&Pipeline::setCameraTuningBlobPath), + py::arg("path"), + DOC(dai, Pipeline, setCameraTuningBlobPath)) + .def("setCameraTuningBlobPath", + py::overload_cast(&Pipeline::setCameraTuningBlobPath), + py::arg("socket"), + py::arg("path"), + DOC(dai, Pipeline, setCameraTuningBlobPath, 2)) .def("setXLinkChunkSize", &Pipeline::setXLinkChunkSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setXLinkChunkSize)) .def("setSippBufferSize", &Pipeline::setSippBufferSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setSippBufferSize)) .def("setSippDmaBufferSize", &Pipeline::setSippDmaBufferSize, py::arg("sizeBytes"), DOC(dai, Pipeline, setSippDmaBufferSize)) diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index 5193f7741b..788c95e0e3 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -20,13 +20,13 @@ #include "depthai/utility/AtomicBool.hpp" // shared +#include "depthai/common/CameraBoardSocket.hpp" #include "depthai/device/BoardConfig.hpp" #include "depthai/pipeline/InputQueue.hpp" #include "depthai/pipeline/PipelineSchema.hpp" #include "depthai/pipeline/datatype/PipelineState.hpp" #include "depthai/properties/GlobalProperties.hpp" #include "depthai/utility/RecordReplay.hpp" -#include "depthai/common/CameraBoardSocket.hpp" namespace dai { @@ -446,7 +446,7 @@ class Pipeline { void setCameraTuningBlobPath(const fs::path& path) { impl()->setCameraTuningBlobPath(path); } - + /// Set a camera IQ (Image Quality) tuning blob, used for specific board socket void setCameraTuningBlobPath(CameraBoardSocket socket, const fs::path& path) { impl()->setCameraTuningBlobPath(socket, path); From 0a8ba2704295dc796dde61bfc27f8ab1740bea47 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 2 Feb 2026 18:03:27 +0100 Subject: [PATCH 138/180] Fix test compilation --- tests/src/ondevice_tests/filesystem_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/src/ondevice_tests/filesystem_test.cpp b/tests/src/ondevice_tests/filesystem_test.cpp index 7a730dff91..c7cdf2861d 100644 --- a/tests/src/ondevice_tests/filesystem_test.cpp +++ b/tests/src/ondevice_tests/filesystem_test.cpp @@ -339,11 +339,11 @@ TEST_CASE("std::filesystem::path with AssetManager, StereoDepth") { CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(path4)); pipeline.getAssetManager().remove("camTuning"); - CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, string4.c_str())); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(dai::CameraBoardSocket::CAM_A, string4.c_str())); pipeline.getAssetManager().remove("camTuning_0"); - CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, string4)); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(dai::CameraBoardSocket::CAM_A, string4)); pipeline.getAssetManager().remove("camTuning_0"); - CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, path4)); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(dai::CameraBoardSocket::CAM_A, path4)); pipeline.getAssetManager().remove("camTuning_0"); auto depth = pipeline.create(); @@ -384,7 +384,7 @@ TEST_CASE("std::filesystem::path with AssetManager, StereoDepth") { const auto stdPath4 = std::filesystem::u8path(string4); #endif CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(stdPath4)); - CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, stdPath4)); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(dai::CameraBoardSocket::CAM_A, stdPath4)); CHECK_NOTHROW(depth->loadMeshFiles(stdPath4, stdPath4)); #endif From 69722e2579b14b25350c579c9783517fc1b852a4 Mon Sep 17 00:00:00 2001 From: AljazD Date: Tue, 3 Feb 2026 09:20:57 +0100 Subject: [PATCH 139/180] Renamed fileName to fileTag in EventsManager and examples --- .../src/utility/EventsManagerBindings.cpp | 22 ++--- examples/cpp/Events/events_file_group.cpp | 4 +- examples/python/Events/events_file_group.py | 4 +- include/depthai/utility/EventsManager.hpp | 38 ++++---- src/utility/EventsManager.cpp | 94 +++++++++---------- 5 files changed, 80 insertions(+), 82 deletions(-) diff --git a/bindings/python/src/utility/EventsManagerBindings.cpp b/bindings/python/src/utility/EventsManagerBindings.cpp index 292d7b4fd9..9a2c9a4d10 100644 --- a/bindings/python/src/utility/EventsManagerBindings.cpp +++ b/bindings/python/src/utility/EventsManagerBindings.cpp @@ -26,59 +26,59 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { .def(py::init<>()) .def("addFile", static_cast(&FileGroup::addFile), - py::arg("fileName"), + py::arg("fileTag"), py::arg("data"), py::arg("mimeType"), DOC(dai, utility, FileGroup, addFile)) .def("addFile", static_cast(&FileGroup::addFile), - py::arg("fileName"), + py::arg("fileTag"), py::arg("filePath"), DOC(dai, utility, FileGroup, addFile)) .def("addFile", static_cast&, const std::shared_ptr&)>(&FileGroup::addFile), - py::arg("fileName"), + py::arg("fileTag"), py::arg("imgFrame"), DOC(dai, utility, FileGroup, addFile)) .def("addFile", static_cast&, const std::shared_ptr&)>(&FileGroup::addFile), - py::arg("fileName"), + py::arg("fileTag"), py::arg("encodedFrame"), DOC(dai, utility, FileGroup, addFile)) //.def("addFile", // static_cast&)>(&FileGroup::addFile), - // py::arg("fileName"), + // py::arg("fileTag"), // py::arg("nnData"), // DOC(dai, utility, FileGroup, addFile)) .def("addFile", static_cast&, const std::shared_ptr&)>(&FileGroup::addFile), - py::arg("fileName"), + py::arg("fileTag"), py::arg("imgDetections"), DOC(dai, utility, FileGroup, addFile)) .def("addImageDetectionsPair", static_cast&, const std::shared_ptr&, const std::shared_ptr&)>( &FileGroup::addImageDetectionsPair), - py::arg("fileName"), + py::arg("fileTag"), py::arg("imgFrame"), py::arg("imgDetections"), DOC(dai, utility, FileGroup, addImageDetectionsPair)) .def("addImageDetectionsPair", static_cast&, const std::shared_ptr&, const std::shared_ptr&)>( &FileGroup::addImageDetectionsPair), - py::arg("fileName"), + py::arg("fileTag"), py::arg("encodedFrame"), py::arg("imgDetections"), DOC(dai, utility, FileGroup, addImageDetectionsPair)); //.def("addImageNNDataPair", // static_cast&, const std::shared_ptr&)>(&FileGroup::addImageNNDataPair), - // py::arg("fileName"), + // py::arg("fileTag"), // py::arg("imgFrame"), // py::arg("nnData"), // DOC(dai, utility, FileGroup, addImageNNDataPair)) //.def( // "addImageNNDataPair", // static_cast&, const std::shared_ptr&)>(&FileGroup::addImageNNDataPair), - // py::arg("fileName"), + // py::arg("fileTag"), // py::arg("encodedFrame"), // py::arg("nnData"), // DOC(dai, utility, FileGroup, addImageNNDataPair)); @@ -145,7 +145,7 @@ void EventsManagerBindings::bind(pybind11::module& m, void* pCallstack) { const std::function failureCallback)>( &EventsManager::sendSnap), py::arg("name"), - py::arg("fileName"), + py::arg("fileTag"), py::arg("imgFrame"), py::arg("imgDetections"), py::arg("tags") = std::vector(), diff --git a/examples/cpp/Events/events_file_group.cpp b/examples/cpp/Events/events_file_group.cpp index 6ca3b36feb..0934200d7b 100644 --- a/examples/cpp/Events/events_file_group.cpp +++ b/examples/cpp/Events/events_file_group.cpp @@ -108,9 +108,9 @@ int main() { // Are there any border detections if(borderDetections->detections.size() > 0) { - std::string fileName = "ImageDetection_"; + std::string fileTag = "ImageDetection_"; std::stringstream ss; - ss << fileName << counter; + ss << fileTag << counter; auto fileGroup = std::make_shared(); fileGroup->addImageDetectionsPair(ss.str(), inRgb, borderDetections); diff --git a/examples/python/Events/events_file_group.py b/examples/python/Events/events_file_group.py index b7bb407b76..469249a9f5 100644 --- a/examples/python/Events/events_file_group.py +++ b/examples/python/Events/events_file_group.py @@ -98,10 +98,10 @@ def frameNorm(frame, bbox): if len(borderDetectionsList) > 0: borderDetections = dai.ImgDetections() borderDetections.detections = borderDetectionsList - fileName = f"ImageDetection_{counter}" + fileTag = f"ImageDetection_{counter}" fileGroup = dai.FileGroup() - fileGroup.addImageDetectionsPair(fileName, inRgb, borderDetections) + fileGroup.addImageDetectionsPair(fileTag, inRgb, borderDetections) localSnapID = eventMan.sendSnap("LowConfidenceDetection", fileGroup, ["EventsExample", "Python"], {"key_0" : "value_0", "key_1" : "value_1"}, uploadSuccessCallback, uploadFailureCallback) if localSnapID != None: diff --git a/include/depthai/utility/EventsManager.hpp b/include/depthai/utility/EventsManager.hpp index 5172a90da7..35b6380251 100644 --- a/include/depthai/utility/EventsManager.hpp +++ b/include/depthai/utility/EventsManager.hpp @@ -29,17 +29,17 @@ namespace utility { class FileData { public: - FileData(std::string data, std::string fileName, std::string mimeType); - explicit FileData(std::filesystem::path filePath, std::string fileName); - explicit FileData(const std::shared_ptr& imgFrame, std::string fileName); - explicit FileData(const std::shared_ptr& encodedFrame, std::string fileName); - // explicit FileData(const std::shared_ptr& nnData, std::string fileName); - explicit FileData(const std::shared_ptr& imgDetections, std::string fileName); + FileData(std::string data, std::string fileTag, std::string mimeType); + explicit FileData(std::filesystem::path filePath, std::string fileTag); + explicit FileData(const std::shared_ptr& imgFrame, std::string fileTag); + explicit FileData(const std::shared_ptr& encodedFrame, std::string fileTag); + // explicit FileData(const std::shared_ptr& nnData, std::string fileTag); + explicit FileData(const std::shared_ptr& imgDetections, std::string fileTag); bool toFile(const std::filesystem::path& inputPath); private: std::string mimeType; - std::string fileName; + std::string fileTag; std::string data; uint64_t size; std::string checksum; @@ -49,20 +49,20 @@ class FileData { class FileGroup { public: - void addFile(std::string fileName, std::string data, std::string mimeType); - void addFile(std::string fileName, std::filesystem::path filePath); - void addFile(const std::optional& fileName, const std::shared_ptr& imgFrame); - void addFile(const std::optional& fileName, const std::shared_ptr& encodedFrame); - // void addFile(std::string fileName, const std::shared_ptr& nnData); - void addFile(const std::optional& fileName, const std::shared_ptr& imgDetections); - void addImageDetectionsPair(const std::optional& fileName, + void addFile(std::string fileTag, std::string data, std::string mimeType); + void addFile(std::string fileTag, std::filesystem::path filePath); + void addFile(const std::optional& fileTag, const std::shared_ptr& imgFrame); + void addFile(const std::optional& fileTag, const std::shared_ptr& encodedFrame); + // void addFile(std::string fileTag, const std::shared_ptr& nnData); + void addFile(const std::optional& fileTag, const std::shared_ptr& imgDetections); + void addImageDetectionsPair(const std::optional& fileTag, const std::shared_ptr& imgFrame, const std::shared_ptr& imgDetections); - void addImageDetectionsPair(const std::optional& fileName, + void addImageDetectionsPair(const std::optional& fileTag, const std::shared_ptr& encodedFrame, const std::shared_ptr& imgDetections); - // void addImageNNDataPair(std::string fileName, const std::shared_ptr& imgFrame, const std::shared_ptr& imgDetections); - // void addImageNNDataPair(std::string fileName, const std::shared_ptr& encodedFrame, const std::shared_ptr& imgDetections); + // void addImageNNDataPair(std::string fileTag, const std::shared_ptr& imgFrame, const std::shared_ptr& imgDetections); + // void addImageNNDataPair(std::string fileTag, const std::shared_ptr& encodedFrame, const std::shared_ptr& imgDetections); private: std::vector> fileData; @@ -124,7 +124,7 @@ class EventsManager { /** * Send a snap to the events service, with an ImgFrame and ImgDetections pair as files * @param name Name of the snap - * @param fileName File name used to create FileData + * @param fileTag File tag used to create FileData * @param imgFrame ImgFrame to send * @param imgDetections ImgDetections to sent * @param tags List of tags to send @@ -134,7 +134,7 @@ class EventsManager { * @return LocalID of the sent Snap */ std::optional sendSnap(const std::string& name, - const std::optional& fileName, + const std::optional& fileTag, const std::shared_ptr imgFrame, const std::optional>& imgDetections = std::nullopt, const std::vector& tags = {}, diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index a010d37124..d0eedf8ef4 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -72,43 +72,43 @@ void addToFileData(std::vector>& container, Args&&... } } -void FileGroup::addFile(std::string fileName, std::string data, std::string mimeType) { - addToFileData(fileData, std::move(data), std::move(fileName), std::move(mimeType)); +void FileGroup::addFile(std::string fileTag, std::string data, std::string mimeType) { + addToFileData(fileData, std::move(data), std::move(fileTag), std::move(mimeType)); } -void FileGroup::addFile(std::string fileName, std::filesystem::path filePath) { - addToFileData(fileData, std::move(filePath), std::move(fileName)); +void FileGroup::addFile(std::string fileTag, std::filesystem::path filePath) { + addToFileData(fileData, std::move(filePath), std::move(fileTag)); } -void FileGroup::addFile(const std::optional& fileName, const std::shared_ptr& imgFrame) { +void FileGroup::addFile(const std::optional& fileTag, const std::shared_ptr& imgFrame) { if(!imgFrame) { throw std::invalid_argument("FileGroup::addFile called with null ImgFrame"); } - std::string dataFileName = fileName.value_or("Image"); + std::string dataFileName = fileTag.value_or("Image"); addToFileData(fileData, imgFrame, std::move(dataFileName)); } -void FileGroup::addFile(const std::optional& fileName, const std::shared_ptr& encodedFrame) { +void FileGroup::addFile(const std::optional& fileTag, const std::shared_ptr& encodedFrame) { if(!encodedFrame) { throw std::invalid_argument("FileGroup::addFile called with null EncodedFrame"); } - std::string dataFileName = fileName.value_or("Image"); + std::string dataFileName = fileTag.value_or("Image"); addToFileData(fileData, encodedFrame, std::move(dataFileName)); } -// void FileGroup::addFile(std::string fileName, const std::shared_ptr& nnData) { -// addToFileData(fileData, nnData, std::move(fileName)); +// void FileGroup::addFile(std::string fileTag, const std::shared_ptr& nnData) { +// addToFileData(fileData, nnData, std::move(fileTag)); // } -void FileGroup::addFile(const std::optional& fileName, const std::shared_ptr& imgDetections) { +void FileGroup::addFile(const std::optional& fileTag, const std::shared_ptr& imgDetections) { if(!imgDetections) { throw std::invalid_argument("FileGroup::addFile called with null ImgDetections"); } - std::string dataFileName = fileName.value_or("Detections"); + std::string dataFileName = fileTag.value_or("Detections"); addToFileData(fileData, imgDetections, std::move(dataFileName)); } -void FileGroup::addImageDetectionsPair(const std::optional& fileName, +void FileGroup::addImageDetectionsPair(const std::optional& fileTag, const std::shared_ptr& imgFrame, const std::shared_ptr& imgDetections) { if(!imgFrame) { @@ -117,12 +117,12 @@ void FileGroup::addImageDetectionsPair(const std::optional& fileNam if(!imgDetections) { throw std::invalid_argument("FileGroup::addImageDetectionsPair called with null ImgDetections"); } - std::string dataFileName = fileName.value_or("ImageDetection"); + std::string dataFileName = fileTag.value_or("ImageDetection"); addToFileData(fileData, imgFrame, dataFileName); addToFileData(fileData, imgDetections, std::move(dataFileName)); } -void FileGroup::addImageDetectionsPair(const std::optional& fileName, +void FileGroup::addImageDetectionsPair(const std::optional& fileTag, const std::shared_ptr& encodedFrame, const std::shared_ptr& imgDetections) { if(!encodedFrame) { @@ -131,19 +131,19 @@ void FileGroup::addImageDetectionsPair(const std::optional& fileNam if(!imgDetections) { throw std::invalid_argument("FileGroup::addImageDetectionsPair called with null ImgDetections"); } - std::string dataFileName = fileName.value_or("ImageDetection"); + std::string dataFileName = fileTag.value_or("ImageDetection"); addToFileData(fileData, encodedFrame, dataFileName); addToFileData(fileData, imgDetections, std::move(dataFileName)); } -// void FileGroup::addImageNNDataPair(std::string fileName, const std::shared_ptr& imgFrame, const std::shared_ptr& nnData) { -// addToFileData(fileData, imgFrame, std::move(fileName)); -// addToFileData(fileData, nnData, std::move(fileName)); +// void FileGroup::addImageNNDataPair(std::string fileTag, const std::shared_ptr& imgFrame, const std::shared_ptr& nnData) { +// addToFileData(fileData, imgFrame, fileTag); +// addToFileData(fileData, nnData, std::move(fileTag)); // } -// void FileGroup::addImageNNDataPair(std::string fileName, const std::shared_ptr& encodedFrame, const std::shared_ptr& nnData) { -// addToFileData(fileData, encodedFrame, std::move(fileName)); -// addToFileData(fileData, nnData, std::move(fileName)); +// void FileGroup::addImageNNDataPair(std::string fileTag, const std::shared_ptr& encodedFrame, const std::shared_ptr& nnData) { +// addToFileData(fileData, encodedFrame, fileTag); +// addToFileData(fileData, nnData, std::move(fileTag)); // } std::string calculateSHA256Checksum(const std::string& data) { @@ -157,15 +157,15 @@ std::string calculateSHA256Checksum(const std::string& data) { return oss.str(); } -FileData::FileData(std::string data, std::string fileName, std::string mimeType) +FileData::FileData(std::string data, std::string fileTag, std::string mimeType) : mimeType(std::move(mimeType)), - fileName(std::move(fileName)), + fileTag(std::move(fileTag)), data(std::move(data)), size(data.size()), checksum(calculateSHA256Checksum(data)), classification(proto::event::PrepareFileUploadClass::UNKNOWN_FILE) {} -FileData::FileData(std::filesystem::path filePath, std::string fileName) : fileName(std::move(fileName)) { +FileData::FileData(std::filesystem::path filePath, std::string fileTag) : fileTag(std::move(fileTag)) { // Read the data std::ifstream fileStream(filePath, std::ios::binary | std::ios::ate); if(!fileStream) { @@ -194,8 +194,8 @@ FileData::FileData(std::filesystem::path filePath, std::string fileName) : fileN } } -FileData::FileData(const std::shared_ptr& imgFrame, std::string fileName) - : mimeType("image/jpeg"), fileName(std::move(fileName)), classification(proto::event::PrepareFileUploadClass::IMAGE_COLOR) { +FileData::FileData(const std::shared_ptr& imgFrame, std::string fileTag) + : mimeType("image/jpeg"), fileTag(std::move(fileTag)), classification(proto::event::PrepareFileUploadClass::IMAGE_COLOR) { // Convert ImgFrame to bytes std::vector buffer; try { @@ -214,8 +214,8 @@ FileData::FileData(const std::shared_ptr& imgFrame, std::string fileNa checksum = calculateSHA256Checksum(data); } -FileData::FileData(const std::shared_ptr& encodedFrame, std::string fileName) - : mimeType("image/jpeg"), fileName(std::move(fileName)), classification(proto::event::PrepareFileUploadClass::IMAGE_COLOR) { +FileData::FileData(const std::shared_ptr& encodedFrame, std::string fileTag) + : mimeType("image/jpeg"), fileTag(std::move(fileTag)), classification(proto::event::PrepareFileUploadClass::IMAGE_COLOR) { // Convert EncodedFrame to bytes if(encodedFrame->getProfile() != EncodedFrame::Profile::JPEG) { throw std::runtime_error("Only JPEG encoded frames are supported"); @@ -227,8 +227,8 @@ FileData::FileData(const std::shared_ptr& encodedFrame, std::strin checksum = calculateSHA256Checksum(data); } -// FileData::FileData(const std::shared_ptr& nnData, std::string fileName) -// : mimeType("application/octet-stream"), fileName(std::move(fileName)), classification(proto::event::PrepareFileUploadClass::UNKNOWN_FILE) { +// FileData::FileData(const std::shared_ptr& nnData, std::string fileTag) +// : mimeType("application/octet-stream"), fileTag(std::move(fileTag)), classification(proto::event::PrepareFileUploadClass::UNKNOWN_FILE) { // // Convert NNData to bytes // std::stringstream ss; // ss.write((const char*)nnData->data->getData().data(), nnData->data->getData().size()); @@ -237,10 +237,8 @@ FileData::FileData(const std::shared_ptr& encodedFrame, std::strin // checksum = calculateSHA256Checksum(data); // } -FileData::FileData(const std::shared_ptr& imgDetections, std::string fileName) - : mimeType("application/x-protobuf; proto=SnapAnnotation"), - fileName(std::move(fileName)), - classification(proto::event::PrepareFileUploadClass::ANNOTATION) { +FileData::FileData(const std::shared_ptr& imgDetections, std::string fileTag) + : mimeType("application/x-protobuf; proto=SnapAnnotation"), fileTag(std::move(fileTag)), classification(proto::event::PrepareFileUploadClass::ANNOTATION) { // Serialize imgDetections object, add it to SnapAnnotation proto proto::event::SnapAnnotations snapAnnotation; proto::img_detections::ImgDetections imgDetectionsProto; @@ -261,8 +259,8 @@ FileData::FileData(const std::shared_ptr& imgDetections, std::str } bool FileData::toFile(const std::filesystem::path& inputPath) { - if(fileName.empty()) { - logger::error("Filename is empty"); + if(fileTag.empty()) { + logger::error("FileTag is empty"); return false; } std::string extension; @@ -270,11 +268,11 @@ bool FileData::toFile(const std::filesystem::path& inputPath) { if(mimeIt != mimeTypeToExtensionMap.end()) { extension = mimeIt->second; } - // Choose a unique filename - std::filesystem::path target = inputPath / (fileName + extension); + // Choose a unique fileTag + std::filesystem::path target = inputPath / (fileTag + extension); for(int i = 1; std::filesystem::exists(target); ++i) { logger::warn("File {} exists, trying a new name", target.string()); - target = inputPath / (fileName + "_" + std::to_string(i) + extension); + target = inputPath / (fileTag + "_" + std::to_string(i) + extension); } std::ofstream fileStream(target, std::ios::binary); if(!fileStream) { @@ -447,7 +445,7 @@ void EventsManager::uploadFileBatch(std::deque> inputS addedFile->set_checksum(file->checksum); addedFile->set_mime_type(file->mimeType); addedFile->set_size(file->size); - addedFile->set_filename(file->fileName); + addedFile->set_filename(file->fileTag); addedFile->set_classification(file->classification); } fileGroupBatchPrepare->add_groups()->Swap(fileGroup.get()); @@ -616,7 +614,7 @@ bool EventsManager::uploadGroup(std::shared_ptr snapData, dai::proto:: } bool EventsManager::uploadFile(std::shared_ptr fileData, std::string uploadUrl) { - logger::info("Uploading file {} to: {}", fileData->fileName, uploadUrl); + logger::info("Uploading file {} to: {}", fileData->fileTag, uploadUrl); auto header = cpr::Header(); header["Content-Type"] = fileData->mimeType; for(int i = 0; i < uploadRetryPolicy.maxAttempts && !stopUploadThread; ++i) { @@ -638,14 +636,14 @@ bool EventsManager::uploadFile(std::shared_ptr fileData, std::string u return true; })); if(response.status_code != cpr::status::HTTP_OK && response.status_code != cpr::status::HTTP_CREATED) { - logger::error("Failed to upload file {}, status code: {}", fileData->fileName, response.status_code); + logger::error("Failed to upload file {}, status code: {}", fileData->fileTag, response.status_code); if(logResponse) { logger::info("Response {}", response.text); } // Apply exponential backoff auto factor = std::pow(uploadRetryPolicy.factor, i + 1); std::chrono::milliseconds duration = std::chrono::milliseconds(uploadRetryPolicy.baseDelay.count() * static_cast(factor)); - logger::info("Retrying upload of file {}, (attempt {}/{}) in {} ms", fileData->fileName, i + 1, uploadRetryPolicy.maxAttempts, duration.count()); + logger::info("Retrying upload of file {}, (attempt {}/{}) in {} ms", fileData->fileTag, i + 1, uploadRetryPolicy.maxAttempts, duration.count()); std::unique_lock lock(stopThreadConditionMutex); eventBufferCondition.wait_for(lock, duration, [this]() { return stopUploadThread.load(); }); @@ -851,7 +849,7 @@ std::optional EventsManager::sendSnap(const std::string& name, if(file->size >= maxFileSizeBytes) { logger::error( "Failed to send snap, file: {} is bigger than the current maximum file size limit: {} kB. To increase your maximum file size, contact support.", - file->fileName, + file->fileTag, maxFileSizeBytes / 1024); return std::nullopt; } @@ -863,7 +861,7 @@ std::optional EventsManager::sendSnap(const std::string& name, } std::optional EventsManager::sendSnap(const std::string& name, - const std::optional& fileName, + const std::optional& fileTag, const std::shared_ptr imgFrame, const std::optional>& imgDetections, const std::vector& tags, @@ -873,9 +871,9 @@ std::optional EventsManager::sendSnap(const std::string& name, // Create a FileGroup and send a snap containing it auto fileGroup = std::make_shared(); if(imgDetections.has_value()) { - fileGroup->addImageDetectionsPair(fileName, imgFrame, imgDetections.value()); + fileGroup->addImageDetectionsPair(fileTag, imgFrame, imgDetections.value()); } else { - fileGroup->addFile(fileName, imgFrame); + fileGroup->addFile(fileTag, imgFrame); } return sendSnap(name, fileGroup, tags, extras, successCallback, failureCallback); From 7b47e4c36dec650d21f1277dc48f43eb7be19351 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 3 Feb 2026 10:29:04 +0100 Subject: [PATCH 140/180] Fix windows build --- tests/src/ondevice_tests/filesystem_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/src/ondevice_tests/filesystem_test.cpp b/tests/src/ondevice_tests/filesystem_test.cpp index c7cdf2861d..28b96ac35a 100644 --- a/tests/src/ondevice_tests/filesystem_test.cpp +++ b/tests/src/ondevice_tests/filesystem_test.cpp @@ -364,9 +364,9 @@ TEST_CASE("std::filesystem::path with AssetManager, StereoDepth") { CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(widePath4)); pipeline.getAssetManager().remove("camTuning"); - CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, widePath4.c_str())); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(dai::CameraBoardSocket::CAM_A, widePath4.c_str())); pipeline.getAssetManager().remove("camTuning_0"); - CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(CameraBoardSocket::CAM_A, widePath4)); + CHECK_NOTHROW(pipeline.setCameraTuningBlobPath(dai::CameraBoardSocket::CAM_A, widePath4)); pipeline.getAssetManager().remove("camTuning_0"); CHECK_NOTHROW(depth->loadMeshFiles(widePath4.c_str(), widePath4.c_str())); From 123514172d8dcdc3433a11aae8bcd414ec7b3dd2 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 3 Feb 2026 13:15:35 +0100 Subject: [PATCH 141/180] Remove unused variable --- include/depthai/pipeline/Pipeline.hpp | 7 ++----- src/pipeline/Pipeline.cpp | 4 ++-- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index 82b49d7160..0ecc0a3f6d 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -39,15 +39,12 @@ class PipelineImpl : public std::enable_shared_from_this { friend class utility::PipelineImplHelper; public: - PipelineImpl(Pipeline& pipeline, bool createImplicitDevice = true) : assetManager("/pipeline/") { - (void)pipeline; + PipelineImpl(bool createImplicitDevice = true) : assetManager("/pipeline/") { if(createImplicitDevice) { defaultDevice = std::make_shared(); } } - PipelineImpl(Pipeline& pipeline, std::shared_ptr device) : assetManager("/pipeline/"), defaultDevice{std::move(device)} { - (void)pipeline; - } + PipelineImpl(std::shared_ptr device) : assetManager("/pipeline/"), defaultDevice{std::move(device)} {} PipelineImpl(const PipelineImpl&) = delete; PipelineImpl& operator=(const PipelineImpl&) = delete; PipelineImpl(PipelineImpl&&) = delete; diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 0107bc197b..3b3db06c17 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -60,9 +60,9 @@ Node::Id PipelineImpl::getNextUniqueId() { return latestId++; } -Pipeline::Pipeline(bool createImplicitDevice) : pimpl(std::make_shared(*this, createImplicitDevice)) {} +Pipeline::Pipeline(bool createImplicitDevice) : pimpl(std::make_shared(createImplicitDevice)) {} -Pipeline::Pipeline(std::shared_ptr device) : pimpl(std::make_shared(*this, device)) {} +Pipeline::Pipeline(std::shared_ptr device) : pimpl(std::make_shared(device)) {} Pipeline::Pipeline(std::shared_ptr pimpl) : pimpl(std::move(pimpl)) {} From 680b977433853e9429db5e8f34e09df152850a06 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Wed, 4 Feb 2026 13:56:12 +0100 Subject: [PATCH 142/180] improve pybindings --- .../python/src/pipeline/CommonBindings.cpp | 2 +- .../src/pipeline/datatype/BufferBindings.cpp | 6 +++--- .../datatype/ImgDetectionsBindings.cpp | 9 +++++++-- .../pipeline/datatype/ImgFrameBindings.cpp | 19 +++++++++++++------ .../datatype/MessageGroupBindings.cpp | 10 +++++----- .../src/pipeline/datatype/NNDataBindings.cpp | 15 ++++++++++----- .../datatype/NeuralDepthConfigBindings.cpp | 14 ++++++++++---- .../datatype/PipelineEventBindings.cpp | 7 ++++--- .../datatype/PipelineStateBindings.cpp | 7 ++++--- 9 files changed, 57 insertions(+), 32 deletions(-) diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index 31fe3c31ec..611f869fda 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -271,7 +271,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def("size", &Rect::size, DOC(dai, Rect, size)) .def("area", &Rect::area, DOC(dai, Rect, area)) .def("empty", &Rect::empty, DOC(dai, Rect, empty)) - .def("contains", &Rect::contains, DOC(dai, Rect, contains)) + .def("contains", &Rect::contains, py::arg("point"), DOC(dai, Rect, contains)) .def("isNormalized", &Rect::isNormalized, DOC(dai, Rect, isNormalized)) .def("denormalize", &Rect::denormalize, py::arg("width"), py::arg("height"), DOC(dai, Rect, denormalize)) .def("normalize", &Rect::normalize, py::arg("width"), py::arg("height"), DOC(dai, Rect, normalize)) diff --git a/bindings/python/src/pipeline/datatype/BufferBindings.cpp b/bindings/python/src/pipeline/datatype/BufferBindings.cpp index 607bb9d908..6c706cd079 100644 --- a/bindings/python/src/pipeline/datatype/BufferBindings.cpp +++ b/bindings/python/src/pipeline/datatype/BufferBindings.cpp @@ -95,8 +95,8 @@ void bind_buffer(pybind11::module& m, void* pCallstack) { .def("getTimestamp", &Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) - .def("setTimestamp", &Buffer::setTimestamp, DOC(dai, Buffer, setTimestamp)) - .def("setTimestampDevice", &Buffer::setTimestampDevice, DOC(dai, Buffer, setTimestampDevice)) - .def("setSequenceNum", &Buffer::setSequenceNum, DOC(dai, Buffer, setSequenceNum)) + .def("setTimestamp", &Buffer::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &Buffer::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) + .def("setSequenceNum", &Buffer::setSequenceNum, py::arg("sequenceNum"), DOC(dai, Buffer, setSequenceNum)) .def("getVisualizationMessage", &Buffer::getVisualizationMessage, DOC(dai, Buffer, getVisualizationMessage)); } diff --git a/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp b/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp index 6323304421..dbf5e2973e 100644 --- a/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp @@ -135,8 +135,13 @@ void bind_imgdetections(pybind11::module& m, void* pCallstack) { .def("getTimestamp", &dai::ImgDetectionsT::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &dai::ImgDetectionsT::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &dai::ImgDetectionsT::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) - .def("getTransformation", [](ImgDetections& msg) { return msg.transformation; }) - .def("setTransformation", [](ImgDetections& msg, const std::optional& transformation) { msg.transformation = transformation; }) + .def( + "getTransformation", [](ImgDetections& msg) { return msg.transformation; }, DOC(dai, ImgFrame, getTransformation)) + .def( + "setTransformation", + [](ImgDetections& msg, const std::optional& transformation) { msg.transformation = transformation; }, + py::arg("transformation"), + DOC(dai, ImgFrame, setTransformation)) .def("getSegmentationMaskWidth", &ImgDetections::getSegmentationMaskWidth, DOC(dai, ImgDetectionsT, getSegmentationMaskWidth)) .def("getSegmentationMaskHeight", &ImgDetections::getSegmentationMaskHeight, DOC(dai, ImgDetectionsT, getSegmentationMaskHeight)) .def("setSegmentationMask", diff --git a/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp b/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp index c56d99443e..c5ac2b5ae7 100644 --- a/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ImgFrameBindings.cpp @@ -10,6 +10,7 @@ #include "depthai/pipeline/datatype/ImgFrame.hpp" #include "ndarray_converter.h" // pybind +#include #include #include @@ -198,7 +199,7 @@ void bind_imgframe(pybind11::module& m, void* pCallstack) { .def("getWidth", &ImgFrame::getWidth, DOC(dai, ImgFrame, getWidth)) .def("getStride", &ImgFrame::getStride, DOC(dai, ImgFrame, getStride)) .def("getHeight", &ImgFrame::getHeight, DOC(dai, ImgFrame, getHeight)) - .def("getPlaneStride", &ImgFrame::getPlaneStride, DOC(dai, ImgFrame, getPlaneStride)) + .def("getPlaneStride", &ImgFrame::getPlaneStride, py::arg("planeIndex"), DOC(dai, ImgFrame, getPlaneStride)) .def("getPlaneHeight", &ImgFrame::getPlaneHeight, DOC(dai, ImgFrame, getPlaneHeight)) .def("getType", &ImgFrame::getType, DOC(dai, ImgFrame, getType)) .def("getBytesPerPixel", &ImgFrame::getBytesPerPixel, DOC(dai, ImgFrame, getBytesPerPixel)) @@ -219,10 +220,10 @@ void bind_imgframe(pybind11::module& m, void* pCallstack) { // The cast function itself does a copy, so we can avoid two copies by always not copying .def( "getFrame", [](ImgFrame& self) { return self.getFrame(false); }, DOC(dai, ImgFrame, getFrame)) - .def("setFrame", &ImgFrame::setFrame, DOC(dai, ImgFrame, setFrame)) + .def("setFrame", &ImgFrame::setFrame, py::arg("array"), DOC(dai, ImgFrame, setFrame)) .def( "getCvFrame", [](ImgFrame& self) { return self.getCvFrame(&g_numpyAllocator); }, DOC(dai, ImgFrame, getCvFrame)) - .def("setCvFrame", &ImgFrame::setCvFrame, DOC(dai, ImgFrame, setCvFrame)) + .def("setCvFrame", &ImgFrame::setCvFrame, py::arg("array"), py::arg("type"), DOC(dai, ImgFrame, setCvFrame)) #else .def( "getFrame", @@ -243,12 +244,18 @@ void bind_imgframe(pybind11::module& m, void* pCallstack) { DOC(dai, ImgFrame, getCvFrame)) .def( "setCvFrame", - [](cv::Mat frame) { throw std::runtime_error("OpenCV support is not available. Please recompile with OpenCV support to use this function."); }, + [](cv::Mat frame, ImgFrame::Type type) { + (void)frame; + (void)type; + throw std::runtime_error("OpenCV support is not available. Please recompile with OpenCV support to use this function."); + }, + py::arg("array"), + py::arg("type"), DOC(dai, ImgFrame, setCvFrame)) #endif // setters - // .def("setTimestamp", &ImgFrame::setTimestamp, py::arg("timestamp"), DOC(dai, ImgFrame, setTimestamp)) - // .def("setTimestampDevice", &ImgFrame::setTimestampDevice, DOC(dai, ImgFrame, setTimestampDevice)) + .def("setTimestamp", &ImgFrame::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &ImgFrame::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) .def("setInstanceNum", &ImgFrame::setInstanceNum, py::arg("instance"), DOC(dai, ImgFrame, setInstanceNum)) .def("setCategory", &ImgFrame::setCategory, py::arg("category"), DOC(dai, ImgFrame, setCategory)) // .def("setSequenceNum", &ImgFrame::setSequenceNum, py::arg("seq"), DOC(dai, ImgFrame, setSequenceNum)) diff --git a/bindings/python/src/pipeline/datatype/MessageGroupBindings.cpp b/bindings/python/src/pipeline/datatype/MessageGroupBindings.cpp index cc63d80248..cdd2c6ae05 100644 --- a/bindings/python/src/pipeline/datatype/MessageGroupBindings.cpp +++ b/bindings/python/src/pipeline/datatype/MessageGroupBindings.cpp @@ -9,6 +9,7 @@ #include "depthai/pipeline/datatype/MessageGroup.hpp" // pybind +#include #include #include @@ -47,15 +48,14 @@ void bind_message_group(pybind11::module& m, void* pCallstack) { "__iter__", [](MessageGroup& msg) { return py::make_iterator(msg.begin(), msg.end()); }, py::keep_alive<0, 1>() /* Essential: keep object alive while iterator exists */) - .def("isSynced", &MessageGroup::isSynced, DOC(dai, MessageGroup, isSynced)) + .def("isSynced", &MessageGroup::isSynced, py::arg("thresholdNs"), DOC(dai, MessageGroup, isSynced)) .def("getIntervalNs", &MessageGroup::getIntervalNs, DOC(dai, MessageGroup, getIntervalNs)) .def("getNumMessages", &MessageGroup::getNumMessages, DOC(dai, MessageGroup, getNumMessages)) .def("getMessageNames", &MessageGroup::getMessageNames, DOC(dai, MessageGroup, getMessageNames)) .def("getTimestamp", &MessageGroup::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &MessageGroup::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &MessageGroup::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) - // .def("setTimestamp", &MessageGroup::setTimestamp, DOC(dai, MessageGroup, setTimestamp)) - // .def("setTimestampDevice", &MessageGroup::setTimestampDevice, DOC(dai, MessageGroup, setTimestampDevice)) - // .def("setSequenceNum", &MessageGroup::setSequenceNum, DOC(dai, MessageGroup, setSequenceNum)) - ; + .def("setTimestamp", &MessageGroup::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &MessageGroup::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) + .def("setSequenceNum", &MessageGroup::setSequenceNum, py::arg("sequenceNum"), DOC(dai, Buffer, setSequenceNum)); } diff --git a/bindings/python/src/pipeline/datatype/NNDataBindings.cpp b/bindings/python/src/pipeline/datatype/NNDataBindings.cpp index 63f6a58944..2d91fc1044 100644 --- a/bindings/python/src/pipeline/datatype/NNDataBindings.cpp +++ b/bindings/python/src/pipeline/datatype/NNDataBindings.cpp @@ -9,6 +9,7 @@ #include "depthai/pipeline/datatype/NNData.hpp" // pybind +#include #include #include #include @@ -110,7 +111,7 @@ void bind_nndata(pybind11::module& m, void* pCallstack) { // Message nnData.def(py::init<>(), DOC(dai, NNData, NNData)) - .def(py::init(), DOC(dai, NNData, NNData, 2)) + .def(py::init(), py::arg("size"), DOC(dai, NNData, NNData, 2)) .def("__repr__", &NNData::str) // // setters // .def("setLayer", [](NNData& obj, const std::string& name, @@ -172,9 +173,9 @@ void bind_nndata(pybind11::module& m, void* pCallstack) { .def("getTimestamp", &NNData::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &NNData::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &NNData::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) - // .def("setTimestamp", &NNData::setTimestamp, DOC(dai, NNData, setTimestamp)) - // .def("setTimestampDevice", &NNData::setTimestampDevice, DOC(dai, NNData, setTimestampDevice)) - // .def("setSequenceNum", &NNData::setSequenceNum, DOC(dai, NNData, setSequenceNum)) + .def("setTimestamp", &NNData::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &NNData::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) + .def("setSequenceNum", &NNData::setSequenceNum, py::arg("sequenceNum"), DOC(dai, Buffer, setSequenceNum)) .def("addTensor", static_cast&, TensorInfo::StorageOrder)>(&NNData::addTensor), @@ -327,5 +328,9 @@ void bind_nndata(pybind11::module& m, void* pCallstack) { .def("getTensorDatatype", &NNData::getTensorDatatype, py::arg("name"), DOC(dai, NNData, getTensorDatatype)) .def("getTensorInfo", &NNData::getTensorInfo, py::arg("name"), DOC(dai, NNData, getTensorInfo)) .def("getTransformation", [](NNData& msg) { return msg.transformation; }) - .def("setTransformation", [](NNData& msg, const std::optional& transformation) { msg.transformation = transformation; }); + .def( + "setTransformation", + [](NNData& msg, const std::optional& transformation) { msg.transformation = transformation; }, + py::arg("transformation"), + DOC(dai, ImgFrame, setTransformation)); } diff --git a/bindings/python/src/pipeline/datatype/NeuralDepthConfigBindings.cpp b/bindings/python/src/pipeline/datatype/NeuralDepthConfigBindings.cpp index 2e92c0a67d..63209a4cb8 100644 --- a/bindings/python/src/pipeline/datatype/NeuralDepthConfigBindings.cpp +++ b/bindings/python/src/pipeline/datatype/NeuralDepthConfigBindings.cpp @@ -36,13 +36,19 @@ void bind_neuraldepthconfig(pybind11::module& m, void* pCallstack) { // Message neuralDepthConfig.def(py::init<>()) .def("__repr__", &NeuralDepthConfig::str) - .def("setConfidenceThreshold", &NeuralDepthConfig::setConfidenceThreshold, DOC(dai, NeuralDepthConfig, setConfidenceThreshold)) + .def("setConfidenceThreshold", + &NeuralDepthConfig::setConfidenceThreshold, + py::arg("confidenceThreshold"), + DOC(dai, NeuralDepthConfig, setConfidenceThreshold)) .def("getConfidenceThreshold", &NeuralDepthConfig::getConfidenceThreshold, DOC(dai, NeuralDepthConfig, getConfidenceThreshold)) - .def("setEdgeThreshold", &NeuralDepthConfig::setEdgeThreshold, DOC(dai, NeuralDepthConfig, setEdgeThreshold)) + .def("setEdgeThreshold", &NeuralDepthConfig::setEdgeThreshold, py::arg("edgeThreshold"), DOC(dai, NeuralDepthConfig, setEdgeThreshold)) .def("getEdgeThreshold", &NeuralDepthConfig::getEdgeThreshold, DOC(dai, NeuralDepthConfig, getEdgeThreshold)) - .def("setDepthUnit", &NeuralDepthConfig::setDepthUnit, DOC(dai, NeuralDepthConfig, setDepthUnit)) + .def("setDepthUnit", &NeuralDepthConfig::setDepthUnit, py::arg("depthUnit"), DOC(dai, NeuralDepthConfig, setDepthUnit)) .def("getDepthUnit", &NeuralDepthConfig::getDepthUnit, DOC(dai, NeuralDepthConfig, getDepthUnit)) - .def("setCustomDepthUnitMultiplier", &NeuralDepthConfig::setCustomDepthUnitMultiplier, DOC(dai, NeuralDepthConfig, setCustomDepthUnitMultiplier)) + .def("setCustomDepthUnitMultiplier", + &NeuralDepthConfig::setCustomDepthUnitMultiplier, + py::arg("customDepthUnitMultiplier"), + DOC(dai, NeuralDepthConfig, setCustomDepthUnitMultiplier)) .def("getCustomDepthUnitMultiplier", &NeuralDepthConfig::getCustomDepthUnitMultiplier, DOC(dai, NeuralDepthConfig, getCustomDepthUnitMultiplier)) .def_readwrite("postProcessing", &NeuralDepthConfig::postProcessing, DOC(dai, NeuralDepthConfig, postProcessing)); diff --git a/bindings/python/src/pipeline/datatype/PipelineEventBindings.cpp b/bindings/python/src/pipeline/datatype/PipelineEventBindings.cpp index fbba9c9d9d..a1180fe3d1 100644 --- a/bindings/python/src/pipeline/datatype/PipelineEventBindings.cpp +++ b/bindings/python/src/pipeline/datatype/PipelineEventBindings.cpp @@ -8,6 +8,7 @@ #include "depthai/pipeline/datatype/PipelineEvent.hpp" // pybind +#include #include #include @@ -59,7 +60,7 @@ void bind_pipelineevent(pybind11::module& m, void* pCallstack) { .def("getTimestamp", &PipelineEvent::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &PipelineEvent::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &PipelineEvent::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) - .def("setTimestamp", &PipelineEvent::setTimestamp, DOC(dai, Buffer, setTimestamp)) - .def("setTimestampDevice", &PipelineEvent::setTimestampDevice, DOC(dai, Buffer, setTimestampDevice)) - .def("setSequenceNum", &PipelineEvent::setSequenceNum, DOC(dai, Buffer, setSequenceNum)); + .def("setTimestamp", &PipelineEvent::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &PipelineEvent::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) + .def("setSequenceNum", &PipelineEvent::setSequenceNum, py::arg("sequenceNum"), DOC(dai, Buffer, setSequenceNum)); } diff --git a/bindings/python/src/pipeline/datatype/PipelineStateBindings.cpp b/bindings/python/src/pipeline/datatype/PipelineStateBindings.cpp index ccb97b63d2..6ede3ec124 100644 --- a/bindings/python/src/pipeline/datatype/PipelineStateBindings.cpp +++ b/bindings/python/src/pipeline/datatype/PipelineStateBindings.cpp @@ -6,6 +6,7 @@ #include "depthai/pipeline/datatype/PipelineState.hpp" // pybind +#include #include #include @@ -109,7 +110,7 @@ void bind_pipelinestate(pybind11::module& m, void* pCallstack) { .def("getTimestamp", &PipelineState::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) .def("getTimestampDevice", &PipelineState::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) .def("getSequenceNum", &PipelineState::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) - .def("setTimestamp", &PipelineState::setTimestamp, DOC(dai, Buffer, setTimestamp)) - .def("setTimestampDevice", &PipelineState::setTimestampDevice, DOC(dai, Buffer, setTimestampDevice)) - .def("setSequenceNum", &PipelineState::setSequenceNum, DOC(dai, Buffer, setSequenceNum)); + .def("setTimestamp", &PipelineState::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &PipelineState::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) + .def("setSequenceNum", &PipelineState::setSequenceNum, py::arg("sequenceNum"), DOC(dai, Buffer, setSequenceNum)); } From bdbcb5d99ff737980e0553d6a3ce179a865a2ca0 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Wed, 4 Feb 2026 14:25:34 +0100 Subject: [PATCH 143/180] improve img detections --- .../datatype/ImgDetectionsBindings.cpp | 1 + .../pipeline/datatype/ImgDetections.hpp | 23 ++++++---- src/pipeline/datatype/ImgDetections.cpp | 45 ++++++++++--------- 3 files changed, 40 insertions(+), 29 deletions(-) diff --git a/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp b/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp index 6323304421..3bc6c2cd78 100644 --- a/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp +++ b/bindings/python/src/pipeline/datatype/ImgDetectionsBindings.cpp @@ -66,6 +66,7 @@ void bind_imgdetections(pybind11::module& m, void* pCallstack) { .def("setBoundingBox", &ImgDetection::setBoundingBox, py::arg("boundingBox")) .def("getBoundingBox", &ImgDetection::getBoundingBox) .def("setOuterBoundingBox", &ImgDetection::setOuterBoundingBox, py::arg("xmin"), py::arg("ymin"), py::arg("xmax"), py::arg("ymax")) + .def("getOuterBoundingBox", &ImgDetection::getOuterBoundingBox, DOC(dai, ImgDetection, getOuterBoundingBox)) .def("setKeypoints", py::overload_cast(&ImgDetection::setKeypoints), py::arg("keypoints")) .def("setKeypoints", py::overload_cast>(&ImgDetection::setKeypoints), py::arg("keypoints")) .def("setKeypoints", diff --git a/include/depthai/pipeline/datatype/ImgDetections.hpp b/include/depthai/pipeline/datatype/ImgDetections.hpp index c6d139066c..8464c03669 100644 --- a/include/depthai/pipeline/datatype/ImgDetections.hpp +++ b/include/depthai/pipeline/datatype/ImgDetections.hpp @@ -40,10 +40,10 @@ struct ImgDetection { std::optional keypoints; ImgDetection() = default; - ImgDetection(const RotatedRect& boundingBox, float confidence, uint32_t label); - ImgDetection(const RotatedRect& boundingBox, std::string labelName, float confidence, uint32_t label); - ImgDetection(const RotatedRect& boundingBox, const KeypointsList& keypoints, float confidence, uint32_t label); - ImgDetection(const RotatedRect& boundingBox, const KeypointsList& keypoints, std::string labelName, float confidence, uint32_t label); + ImgDetection(const RotatedRect& boundingBox, float confidence = 0.f, uint32_t label = 0); + ImgDetection(const RotatedRect& boundingBox, std::string labelName, float confidence = 0.f, uint32_t label = 0); + ImgDetection(const RotatedRect& boundingBox, const KeypointsList& keypoints, float confidence = 0.f, uint32_t label = 0); + ImgDetection(const RotatedRect& boundingBox, const KeypointsList& keypoints, std::string labelName, float confidence = 0.f, uint32_t label = 0); /** * Sets the bounding box and the legacy coordinates of the detection. @@ -60,6 +60,11 @@ struct ImgDetection { */ void setOuterBoundingBox(const float xmin, const float ymin, const float xmax, const float ymax); + /** + * Returns the outer bounding box as [minx, miny, maxx, maxy]. + */ + std::array getOuterBoundingBox() const; + /** * Sets the keypoints of the detection. * @param keypoints list of Keypoint objects to set. @@ -123,27 +128,27 @@ struct ImgDetection { /** * Returns the X coordinate of the center of the bounding box. */ - float getCenterX() const noexcept; + float getCenterX() const; /** * Returns the Y coordinate of the center of the bounding box. */ - float getCenterY() const noexcept; + float getCenterY() const; /** * Returns the width of the (rotated) bounding box. */ - float getWidth() const noexcept; + float getWidth() const; /** * Returns the height of the (rotated) bounding box. */ - float getHeight() const noexcept; + float getHeight() const; /** * Returns the angle of the bounding box. */ - float getAngle() const noexcept; + float getAngle() const; DEPTHAI_SERIALIZE(ImgDetection, label, labelName, confidence, xmin, ymin, xmax, ymax, boundingBox, keypoints); }; diff --git a/src/pipeline/datatype/ImgDetections.cpp b/src/pipeline/datatype/ImgDetections.cpp index 1136ecb721..a84c09a1ab 100644 --- a/src/pipeline/datatype/ImgDetections.cpp +++ b/src/pipeline/datatype/ImgDetections.cpp @@ -19,23 +19,21 @@ namespace dai { // ImgDetection functions -ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, float conf = 0.f, uint32_t label = 0) { +ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, float conf, uint32_t label) { setBoundingBox(boundingBox); this->confidence = conf; this->label = label; } -ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, std::string labelName, float conf = 0.f, uint32_t label = 0) - : ImgDetection(boundingBox, conf, label) { +ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, std::string labelName, float conf, uint32_t label) : ImgDetection(boundingBox, conf, label) { this->labelName = std::move(labelName); } -ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, const dai::KeypointsList& keypoints, float conf = 0.f, uint32_t label = 0) +ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, const dai::KeypointsList& keypoints, float conf, uint32_t label) : ImgDetection(boundingBox, conf, label) { this->keypoints = keypoints; } -ImgDetection::ImgDetection( - const dai::RotatedRect& boundingBox, const dai::KeypointsList& keypoints, std::string labelName, float conf = 0.f, uint32_t label = 0) +ImgDetection::ImgDetection(const dai::RotatedRect& boundingBox, const dai::KeypointsList& keypoints, std::string labelName, float conf, uint32_t label) : ImgDetection(boundingBox, conf, label) { this->keypoints = keypoints; this->labelName = std::move(labelName); @@ -54,7 +52,8 @@ void ImgDetection::setBoundingBox(const RotatedRect boundingBox) { RotatedRect ImgDetection::getBoundingBox() const { if(boundingBox.has_value()) { return boundingBox.value(); - } else if(xmin == 0.f && xmax == 0.f && ymin == 0.f && ymax == 0.f) { + } + if(xmin == 0.f && xmax == 0.f && ymin == 0.f && ymax == 0.f) { throw std::runtime_error("All bounding box values are zero, no bounding box can be built."); } @@ -75,6 +74,16 @@ void ImgDetection::setOuterBoundingBox(const float xmin, const float ymin, const this->boundingBox = RotatedRect{Rect{topLeft, bottomRight}}; } +std::array ImgDetection::getOuterBoundingBox() const { + if(boundingBox.has_value()) { + return boundingBox->getOuterRect(); + } + if(xmin == 0.f && xmax == 0.f && ymin == 0.f && ymax == 0.f) { + throw std::runtime_error("All bounding box values are zero, no bounding box can be built."); + } + return {xmin, ymin, xmax, ymax}; +} + void ImgDetection::setKeypoints(const KeypointsList kp) { this->keypoints = kp; } @@ -100,25 +109,22 @@ void ImgDetection::setKeypoints(const std::vector kps2) { std::vector ImgDetection::getKeypoints() const { if(keypoints.has_value()) { return keypoints->getKeypoints(); - } else { - return {}; } + return {}; } std::vector ImgDetection::getKeypoints2f() const { if(keypoints.has_value()) { return keypoints->getPoints2f(); - } else { - return {}; } + return {}; } std::vector ImgDetection::getKeypoints3f() const { if(keypoints.has_value()) { return keypoints->getPoints3f(); - } else { - return {}; } + return {}; } void ImgDetection::setEdges(const std::vector edges) { @@ -131,28 +137,27 @@ void ImgDetection::setEdges(const std::vector edges) { std::vector ImgDetection::getEdges() const { if(keypoints.has_value()) { return keypoints->getEdges(); - } else { - return {}; } + return {}; } -float ImgDetection::getCenterX() const noexcept { +float ImgDetection::getCenterX() const { return getBoundingBox().center.x; } -float ImgDetection::getCenterY() const noexcept { +float ImgDetection::getCenterY() const { return getBoundingBox().center.y; } -float ImgDetection::getWidth() const noexcept { +float ImgDetection::getWidth() const { return getBoundingBox().size.width; } -float ImgDetection::getHeight() const noexcept { +float ImgDetection::getHeight() const { return getBoundingBox().size.height; } -float ImgDetection::getAngle() const noexcept { +float ImgDetection::getAngle() const { return getBoundingBox().angle; } From f7eb73c7f0fad2f0093df0837b61b0171f59a89a Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 5 Feb 2026 11:15:29 +0100 Subject: [PATCH 144/180] Fix Gate node's input queue to be of size 1 and non-blocking --- include/depthai/pipeline/node/Gate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 1a7e671f90..7c2a9b9db3 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -37,7 +37,7 @@ class Gate : public DeviceNodeCRTP { * * Default queue size: 4 * Blocking: False */ - Input input{*this, {"input", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + Input input{*this, {"input", DEFAULT_GROUP, false, 1, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; /** * @brief Main data output. From 3419b4a3d418dd6c1cc327f407946b44483d0028 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 5 Feb 2026 11:18:03 +0100 Subject: [PATCH 145/180] Add additional test for gate node --- .../pipeline/node/gate_node_tests.cpp | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/tests/src/ondevice_tests/pipeline/node/gate_node_tests.cpp b/tests/src/ondevice_tests/pipeline/node/gate_node_tests.cpp index c3315dd019..696ee9b505 100644 --- a/tests/src/ondevice_tests/pipeline/node/gate_node_tests.cpp +++ b/tests/src/ondevice_tests/pipeline/node/gate_node_tests.cpp @@ -135,3 +135,56 @@ TEST_CASE("Test Gate N Messages") { } } } + +TEST_CASE("Two Queue from one camera") { + dai::Pipeline pipeline; + + auto camera = pipeline.create()->build(); + + auto cameraOutGate = camera->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, 30); + auto cameraOut = camera->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, 30); + + auto gate = pipeline.create(); + + cameraOutGate->link(gate->input); + + auto cameraGateQueue = gate->output.createOutputQueue(8, false); + auto cameraQueue = cameraOut->createOutputQueue(); + + auto gateControlQueue = gate->inputControl.createInputQueue(); + + gate->initialConfig->open = false; + gate->initialConfig->numMessages = -1; + + pipeline.start(); + + int msgsFromGateCount = 0; + int msgsFromCameraCount = 0; + + const double testDuration = 3.0; // Run for 3 seconds + auto startTime = std::chrono::steady_clock::now(); + + while(pipeline.isRunning()) { + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - startTime; + + if(elapsed.count() >= testDuration) { + break; + } + + auto msgFromGate = cameraGateQueue->tryGet(); + auto msgFromCamera = cameraQueue->tryGet(); + + if(msgFromGate) { + msgsFromGateCount += 1; + } + if(msgFromCamera) { + msgsFromCameraCount += 1; + } + } + + CHECK(msgsFromGateCount == 0); + CHECK(msgsFromCameraCount > 60); + + std::cout << "Gate frames: " << msgsFromGateCount << " | Camera frames: " << msgsFromCameraCount << std::endl; +} From 277173e6fe0c284ecb94e72fa3faa9462686b9b5 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 5 Feb 2026 11:35:30 +0100 Subject: [PATCH 146/180] Update RVC2 FW --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 94bac62dfd..89d742aa2b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "b3d7c6a54185ca2f91ec5d20743da3cee7f0e67e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c67b7323499f77cec598e0841535fe1880006220") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From f9cc18a5bef76510b9c9539260daa5b931581123 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 5 Feb 2026 12:34:58 +0100 Subject: [PATCH 147/180] Update comment of the queue size --- include/depthai/pipeline/node/Gate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthai/pipeline/node/Gate.hpp b/include/depthai/pipeline/node/Gate.hpp index 7c2a9b9db3..d21869ec67 100644 --- a/include/depthai/pipeline/node/Gate.hpp +++ b/include/depthai/pipeline/node/Gate.hpp @@ -34,7 +34,7 @@ class Gate : public DeviceNodeCRTP { * * Accepts arbitrary Buffer messages (e.g., ImgFrame, NNData). * If the Gate is Open, messages received here are forwarded to 'output'. * If the Gate is Closed, messages received here are discarded/dropped. - * * Default queue size: 4 + * * Default queue size: 1 * Blocking: False */ Input input{*this, {"input", DEFAULT_GROUP, false, 1, {{{DatatypeEnum::Buffer, true}}}, DEFAULT_WAIT_FOR_MESSAGE}}; From a2061be2c2ea0f66ddd53f90bd5a01dd393e1583 Mon Sep 17 00:00:00 2001 From: AljazD Date: Fri, 6 Feb 2026 10:38:08 +0100 Subject: [PATCH 148/180] Fixed raw data addFile() overload --- src/utility/EventsManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index d0eedf8ef4..74e95f425c 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -161,8 +161,8 @@ FileData::FileData(std::string data, std::string fileTag, std::string mimeType) : mimeType(std::move(mimeType)), fileTag(std::move(fileTag)), data(std::move(data)), - size(data.size()), - checksum(calculateSHA256Checksum(data)), + size(this->data.size()), + checksum(calculateSHA256Checksum(this->data)), classification(proto::event::PrepareFileUploadClass::UNKNOWN_FILE) {} FileData::FileData(std::filesystem::path filePath, std::string fileTag) : fileTag(std::move(fileTag)) { From cfd377696331cdb38768b0d187708e844a423978 Mon Sep 17 00:00:00 2001 From: Danilo Pejovic <115164734+danilo-pejovic@users.noreply.github.com> Date: Fri, 6 Feb 2026 15:31:37 +0100 Subject: [PATCH 149/180] Mac tests (#1573) * mac tests --------- Co-authored-by: Danilo Co-authored-by: Matevz Morato --- .github/workflows/test.workflow.yml | 9 +++ .github/workflows/test_child_mac.yml | 52 ++++++++++++++++ .github/workflows/test_child_windows.yml | 2 +- examples/cpp/HostNodes/display.cpp | 8 +-- examples/cpp/HostNodes/host_node.cpp | 4 +- .../HostNodes/host_pipeline_synced_node.cpp | 4 +- examples/cpp/HostNodes/image_manip_host.cpp | 14 +++-- .../cpp/ImageManip/image_manip_multi_ops.cpp | 15 +++-- .../cpp/ImageManip/image_manip_resize.cpp | 17 +++--- .../spatial_detection.cpp | 4 +- examples/python/install_requirements.py | 5 +- scripts/hil/run_hil_tests.sh | 43 ------------- scripts/hil/run_hil_tests_mac.sh | 61 +++++++++++++++++++ vcpkg.json | 6 ++ 14 files changed, 171 insertions(+), 73 deletions(-) create mode 100644 .github/workflows/test_child_mac.yml delete mode 100755 scripts/hil/run_hil_tests.sh create mode 100755 scripts/hil/run_hil_tests_mac.sh diff --git a/.github/workflows/test.workflow.yml b/.github/workflows/test.workflow.yml index ca2bad9727..c4a9930be2 100644 --- a/.github/workflows/test.workflow.yml +++ b/.github/workflows/test.workflow.yml @@ -92,3 +92,12 @@ jobs: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} + run_vanilla_mac_tests: + needs: [precheck] + if: needs.precheck.outputs.should_run == 'true' + uses: ./.github/workflows/test_child_mac.yml + with: + flavor: "asan-ubsan" + luxonis_os_versions_to_test: "['1.23.1']" + secrets: + HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} diff --git a/.github/workflows/test_child_mac.yml b/.github/workflows/test_child_mac.yml new file mode 100644 index 0000000000..4a758efa73 --- /dev/null +++ b/.github/workflows/test_child_mac.yml @@ -0,0 +1,52 @@ +name: Run hil tests for mac os +on: + workflow_call: + inputs: + flavor: + required: true + type: string + default: "vanilla" + luxonis_os_versions_to_test: + required: true + type: string + secrets: + HIL_PAT_TOKEN: + required: true + +jobs: +# mac_rvc2_test: +# needs: [build_docker_container] +# runs-on: ['self-hosted', 'testbed-runner'] +# steps: +# - uses: actions/checkout@v3 + +# - name: Run RVC2 tests +# run: | +# source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} +# export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc2-${{ inputs.flavor }}" +# exec hil_runner --capabilities depthai-core-hil --platforms 'rvc2 and rvc2' --reservation-name $RESERVATION_NAME --wait --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc2" + + # Testing + mac_rvc4_test: + strategy: + matrix: + rvc4os: ${{ fromJson(inputs.luxonis_os_versions_to_test) }} + fail-fast: false + runs-on: ['self-hosted', 'testbed-runner'] + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + fetch-depth: 0 + + - name: Run RVC4 tests + run: | + BRANCH_NAME="${{ github.ref_name }}" + PULL_REQUEST="false" + if [[ -n "${{ github.head_ref }}" ]]; then + BRANCH_NAME="${{ github.ref }}" + PULL_REQUEST="true" + fi + source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} + export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-mac-${{ matrix.rvc4os }}-${{ inputs.flavor }}" + exec hil_runner --models "oak4_pro or oak4_d" -os mac --sync-workspace --rsync-args="--exclude=venv" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --commands "cd /tmp/depthai-core && ./scripts/hil/run_hil_tests_mac.sh $PULL_REQUEST ${{ inputs.flavor }} --rvc4" diff --git a/.github/workflows/test_child_windows.yml b/.github/workflows/test_child_windows.yml index e5482e1beb..45200892b8 100644 --- a/.github/workflows/test_child_windows.yml +++ b/.github/workflows/test_child_windows.yml @@ -114,7 +114,7 @@ jobs: pip install --upgrade pip pip install numpy pytest pytest-html $buildDir = 'C:/depthai-core/build' - $overlay = 'C:/depthai-core/cmake/triplets/release' # make it absolute + $overlay = 'C:/depthai-core/cmake/triplets/release' $isPR = "${{ github.head_ref }}" -ne "" $extraFlags = "" diff --git a/examples/cpp/HostNodes/display.cpp b/examples/cpp/HostNodes/display.cpp index 12daaa1ecf..958f804021 100644 --- a/examples/cpp/HostNodes/display.cpp +++ b/examples/cpp/HostNodes/display.cpp @@ -9,7 +9,7 @@ class HostDisplay : public dai::node::CustomNode { public: HostDisplay() { - sendProcessingToPipeline(false); + sendProcessingToPipeline(true); } std::shared_ptr processGroup(std::shared_ptr message) override { @@ -45,11 +45,7 @@ int main() { output->link(display->inputs["frame"]); // Start pipeline - pipeline.start(); - std::cout << "Display window opened. Press 'q' to quit." << std::endl; - - // Wait for pipeline to finish - pipeline.wait(); + pipeline.run(); return 0; } diff --git a/examples/cpp/HostNodes/host_node.cpp b/examples/cpp/HostNodes/host_node.cpp index 96eca042f2..724fc5207f 100644 --- a/examples/cpp/HostNodes/host_node.cpp +++ b/examples/cpp/HostNodes/host_node.cpp @@ -8,6 +8,7 @@ class Display : public dai::NodeCRTP { std::shared_ptr build(Output& out) { out.link(input); + sendProcessingToPipeline(true); return std::static_pointer_cast(this->shared_from_this()); } std::shared_ptr processGroup(std::shared_ptr in) override { @@ -68,7 +69,6 @@ int main() { pipeline.create()->build(*camRgb->requestOutput(std::make_pair(640, 480)), *camMono->requestOutput(std::make_pair(640, 480))); auto display = pipeline.create()->build(streamMerger->out); - pipeline.start(); - pipeline.wait(); + pipeline.run(); return 0; } \ No newline at end of file diff --git a/examples/cpp/HostNodes/host_pipeline_synced_node.cpp b/examples/cpp/HostNodes/host_pipeline_synced_node.cpp index 4d62eda1d1..d85d158ab8 100644 --- a/examples/cpp/HostNodes/host_pipeline_synced_node.cpp +++ b/examples/cpp/HostNodes/host_pipeline_synced_node.cpp @@ -48,10 +48,10 @@ int main() { auto camMono = pipeline.create()->build(dai::CameraBoardSocket::CAM_B); auto syncedDisplay = pipeline.create(); + syncedDisplay->sendProcessingToPipeline(true); camRgb->requestOutput(std::make_pair(640, 480))->link(syncedDisplay->inputRgb); camMono->requestOutput(std::make_pair(640, 480))->link(syncedDisplay->inputMono); - pipeline.start(); - pipeline.wait(); + pipeline.run(); return 0; } diff --git a/examples/cpp/HostNodes/image_manip_host.cpp b/examples/cpp/HostNodes/image_manip_host.cpp index db37c81972..2d962b4ba0 100644 --- a/examples/cpp/HostNodes/image_manip_host.cpp +++ b/examples/cpp/HostNodes/image_manip_host.cpp @@ -18,7 +18,6 @@ int main(int argc, char** argv) { dai::Pipeline pipeline(false); auto replay = pipeline.create(); - auto display = pipeline.create(); auto manip = pipeline.create(); manip->setRunOnHost(); // After doing the rest of the operations, resize the frame to 1270x710 and keep the aspect ratio by cropping from the center @@ -36,9 +35,16 @@ int main(int argc, char** argv) { replay->setSize(1280, 720); replay->out.link(manip->inputImage); - manip->out.link(display->input); + auto outputQueue = manip->out.createOutputQueue(); - pipeline.start(); - pipeline.wait(); + pipeline.start(); + while(pipeline.isRunning()) { + auto imgFrame = outputQueue->get(); + cv::imshow("Manipulated Frame", imgFrame->getCvFrame()); + int key = cv::waitKey(1); + if(key == 'q') { + break; + } + } } diff --git a/examples/cpp/ImageManip/image_manip_multi_ops.cpp b/examples/cpp/ImageManip/image_manip_multi_ops.cpp index e2062e7fa3..914c16db7c 100644 --- a/examples/cpp/ImageManip/image_manip_multi_ops.cpp +++ b/examples/cpp/ImageManip/image_manip_multi_ops.cpp @@ -14,7 +14,6 @@ int main(int argc, char** argv) { dai::Pipeline pipeline(device); auto camRgb = pipeline.create()->build(dai::CameraBoardSocket::CAM_A); - auto display = pipeline.create(); auto manip = pipeline.create(); manip->setMaxOutputFrameSize(4000000); @@ -27,8 +26,14 @@ int main(int argc, char** argv) { auto* rgbOut = camRgb->requestOutput({1920, 1080}); rgbOut->link(manip->inputImage); - manip->out.link(display->input); - + auto outputQueue = manip->out.createOutputQueue(); pipeline.start(); - pipeline.wait(); -} \ No newline at end of file + while(pipeline.isRunning()) { + auto imgFrame = outputQueue->get(); + cv::imshow("Manipulated Frame", imgFrame->getCvFrame()); + int key = cv::waitKey(1); + if(key == 'q') { + break; + } + } +} diff --git a/examples/cpp/ImageManip/image_manip_resize.cpp b/examples/cpp/ImageManip/image_manip_resize.cpp index b7ca47a6fc..110b836008 100644 --- a/examples/cpp/ImageManip/image_manip_resize.cpp +++ b/examples/cpp/ImageManip/image_manip_resize.cpp @@ -1,11 +1,7 @@ -#include - -#include "depthai/common/CameraBoardSocket.hpp" #include "depthai/depthai.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" #include "depthai/pipeline/node/Camera.hpp" #include "depthai/pipeline/node/ImageManip.hpp" -#include "depthai/pipeline/node/host/Display.hpp" int main(int argc, char** argv) { std::shared_ptr device = nullptr; @@ -17,7 +13,6 @@ int main(int argc, char** argv) { dai::Pipeline pipeline(device); auto camRgb = pipeline.create()->build(dai::CameraBoardSocket::CAM_A); - auto display = pipeline.create(); auto manip = pipeline.create(); // Resize to 400x400 and avoid stretching by cropping from the center @@ -26,7 +21,15 @@ int main(int argc, char** argv) { manip->initialConfig->setFrameType(dai::ImgFrame::Type::RGB888i); camRgb->requestOutput((std::make_pair(1920, 1080)))->link(manip->inputImage); - manip->out.link(display->input); + auto outputQueue = manip->out.createOutputQueue(); - pipeline.run(); + pipeline.start(); + while(pipeline.isRunning()) { + auto imgFrame = outputQueue->get(); + cv::imshow("Resized Frame", imgFrame->getCvFrame()); + int key = cv::waitKey(1); + if(key == 'q') { + break; + } + } } diff --git a/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp b/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp index b1131a8f0a..aa79a1ca9e 100644 --- a/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp +++ b/examples/cpp/SpatialDetectionNetwork/spatial_detection.cpp @@ -22,6 +22,7 @@ class SpatialVisualizer : public dai::NodeCRTP(this->shared_from_this()); } @@ -229,8 +230,7 @@ int main(int argc, char** argv) { std::cout << "Pipeline starting with depth source: " << depthSourceArg << '\n'; // Start pipeline - pipeline.start(); - pipeline.wait(); + pipeline.run(); } catch(const std::exception& e) { std::cerr << "Error: " << e.what() << '\n'; diff --git a/examples/python/install_requirements.py b/examples/python/install_requirements.py index f43e981152..6eb7ecde5b 100755 --- a/examples/python/install_requirements.py +++ b/examples/python/install_requirements.py @@ -64,7 +64,10 @@ def hasWhitespace(string): if args.install_rerun: DEPENDENCIES.append('rerun-sdk') if args.install_open3d_cpu: - DEPENDENCIES.append('open3d-cpu') + if platform.system() == "Darwin": + DEPENDENCIES.append("open3d") + else: + DEPENDENCIES.append("open3d-cpu") # Constants ARTIFACTORY_URL = 'https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local' diff --git a/scripts/hil/run_hil_tests.sh b/scripts/hil/run_hil_tests.sh deleted file mode 100755 index fa53b36067..0000000000 --- a/scripts/hil/run_hil_tests.sh +++ /dev/null @@ -1,43 +0,0 @@ -#!/bin/bash - -# Check if the argument is provided -if [ $# -lt 1 ]; then - echo "Usage: $0 [--rvc2 | --rvc4]" - echo " test_type Specify test flavor: vanilla, asan-ubsan, or tsan." - echo " --rvc2 Optional: Run tests with RVC2 configuration." - echo " --rvc4 Optional: Run tests with RVC4 configuration." - exit 1 -fi -# Get the test type from the argument -TEST_FLAVOR=$1 -TEST_ARGS=$2 - -# Set up a Python virtual environment -rm -rf venv -python3 -m venv venv -source venv/bin/activate -export LC_ALL=en_US.UTF-8 -locale - -if [ "$TEST_FLAVOR" == "vanilla" ]; then - echo $CMAKE_TOOLCHAIN_PATH -else - export CMAKE_TOOLCHAIN_PATH=$PWD/cmake/toolchain/${FLAVOR}.cmake -fi - -export PATH="$PATH:/home/hil/hil_framework/lib_testbed/tools" -export PYTHONPATH="$PYTHONPATH:/home/hil/hil_framework" -export HIL_FRAMEWORK_PATH="/home/hil/hil_framework" - -# Install required Python packages -pip install numpy pytest pytest-html > /dev/null 2>&1 -pushd /home/$USER/hil_framework/ > /dev/null 2>&1 && git pull && git submodule update --init --recursive > /dev/null 2>&1 && popd > /dev/null 2>&1 -pushd /home/$USER/hil_framework/ > /dev/null 2>&1 && pip install -r requirements.txt > /dev/null 2>&1 && popd > /dev/null 2>&1 - -cmake -S . -B build -D CMAKE_BUILD_TYPE=Release -D HUNTER_ROOT=$HOME/.hun2_$TEST_FLAVOR -D DEPTHAI_VCPKG_INTERNAL_ONLY=OFF -D DEPTHAI_BUILD_EXAMPLES=ON -D DEPTHAI_BUILD_TESTS=ON -D DEPTHAI_TEST_EXAMPLES=ON -D DEPTHAI_BUILD_PYTHON=ON -D DEPTHAI_PYTHON_TEST_EXAMPLES=ON -D DEPTHAI_PYTHON_ENABLE_EXAMPLES=ON -cmake --build build --parallel 2 --config Release - -export DISPLAY=:99 -xdpyinfo -display $DISPLAY >/dev/null 2>&1 || (Xvfb $DISPLAY &) -cd tests -python3 run_tests.py $TEST_ARGS diff --git a/scripts/hil/run_hil_tests_mac.sh b/scripts/hil/run_hil_tests_mac.sh new file mode 100755 index 0000000000..0fba9b569f --- /dev/null +++ b/scripts/hil/run_hil_tests_mac.sh @@ -0,0 +1,61 @@ +#!/bin/bash + +# Check if the argument is provided +if [ $# -lt 1 ]; then + echo "Usage: $0 [--rvc2 | --rvc4]" + echo " pull_request Specify are we doing short tests (pull request) or long tests (dispatch/merge)" + echo " test_type Specify test flavor: vanilla, asan-ubsan, or tsan." + echo " --rvc2 Optional: Run tests with RVC2 configuration." + echo " --rvc4 Optional: Run tests with RVC4 configuration." + exit 1 +fi +# Get the test type from the argument +PULL_REQUEST=$1 +TEST_FLAVOR=$2 +TEST_ARGS=$3 + +# Set up a Python virtual environment +rm -rf venv +python3.12 -m venv venv +source venv/bin/activate +export LC_ALL=en_US.UTF-8 +locale + +if [ "$TEST_FLAVOR" == "vanilla" ]; then + echo $CMAKE_TOOLCHAIN_PATH +else + export CMAKE_TOOLCHAIN_PATH=$PWD/cmake/toolchain/${TEST_FLAVOR}.cmake +fi + +pip install numpy pytest pytest-html > /dev/null 2>&1 +export VCPKG_BINARY_SOURCES="files,/tmp/vcpkg_binaries,readwrite" +rm -rf build +CMAKE_ARGS=( + -S . -B build + -D CMAKE_BUILD_TYPE=Release + -D DEPTHAI_VCPKG_INTERNAL_ONLY=OFF + -D VCPKG_OVERLAY_TRIPLETS="$PWD/cmake/triplets/release" + -D DEPTHAI_BUILD_TESTS=ON + -D CMAKE_SHARED_LINKER_FLAGS="-Wl,-w" +) + +if [ "$PULL_REQUEST" = "false" ]; then + CMAKE_ARGS+=( + -D DEPTHAI_BUILD_EXAMPLES=ON + -D DEPTHAI_TEST_EXAMPLES=ON + -D DEPTHAI_BUILD_PYTHON=ON + -D DEPTHAI_PYTHON_TEST_EXAMPLES=ON + -D DEPTHAI_PYTHON_ENABLE_EXAMPLES=ON + ) +fi + +echo "Running:" +printf 'cmake'; printf ' %q' "${CMAKE_ARGS[@]}"; echo + +cmake "${CMAKE_ARGS[@]}" + +cmake --build build --parallel 3 --config Release \ + || { echo "Build failed"; exit 3; } + +cd tests +python3 run_tests.py $TEST_ARGS diff --git a/vcpkg.json b/vcpkg.json index 84e3382b64..602fb6fd23 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -32,6 +32,12 @@ "host": true } ], + "overrides": [ + { + "name": "vcpkg-tool-meson", + "version": "1.9.0" + } + ], "features": { "opencv-support": { "description": "Enable OpenCV support", From ecc6be27b93722925a3b0a950fc759b4bff3a7cf Mon Sep 17 00:00:00 2001 From: asahtik Date: Fri, 6 Feb 2026 16:54:23 +0100 Subject: [PATCH 150/180] Fix compiler nullptr dereference compiler warning --- src/utility/PipelineImplHelper.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/utility/PipelineImplHelper.cpp b/src/utility/PipelineImplHelper.cpp index 5de9123ba7..2b52aa3ef4 100644 --- a/src/utility/PipelineImplHelper.cpp +++ b/src/utility/PipelineImplHelper.cpp @@ -16,6 +16,7 @@ namespace utility { void PipelineImplHelper::setupHolisticRecordAndReplay() { auto pipeline = pipelineWeak.lock(); + if(!pipeline) throw std::runtime_error("PipelineImplHelper: Pipeline is no longer available."); // TODO: Refactor this function to reduce complexity if(pipeline->buildingOnHost) { @@ -110,6 +111,7 @@ void PipelineImplHelper::setupHolisticRecordAndReplay() { } void PipelineImplHelper::setupPipelineDebuggingPre() { auto pipeline = pipelineWeak.lock(); + if(!pipeline) throw std::runtime_error("PipelineImplHelper: Pipeline is no longer available."); // Create pipeline event aggregator node and link bool envPipelineDebugging = utility::getEnvAs("DEPTHAI_PIPELINE_DEBUGGING", false); @@ -178,6 +180,7 @@ void PipelineImplHelper::setupPipelineDebuggingPre() { void PipelineImplHelper::setupPipelineDebuggingPost(std::unordered_map& bridgesOut, std::unordered_map& bridgesIn) { auto pipeline = pipelineWeak.lock(); + if(!pipeline) throw std::runtime_error("PipelineImplHelper: Pipeline is no longer available."); // Finish setting up pipeline debugging if(pipeline->buildingOnHost && pipeline->enablePipelineDebugging) { From 624efc1bffc84f81f5da938018af6867fe135483 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Fri, 6 Feb 2026 17:35:36 +0100 Subject: [PATCH 151/180] Update model hash --- .../src/ondevice_tests/pipeline/node/detection_parser_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp b/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp index 9749360e86..d76caeb10a 100644 --- a/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp @@ -326,7 +326,7 @@ TEST_CASE("DetectionParser replay test") { {"yolov8-instance-segmentation-large:coco-640x352:701031f", yoloV8InstanceSegmentationLargeCoco640x352GroundTruth, peopleWalkingVideo}, {"yolov10-nano:coco-512x288:007b5fe", yoloV10NanoCoco512x288GroundTruth, peopleWalkingVideo}, {"ppe-detection:640x640:419a4e5", ppeDetection640x640GroundTruth, peopleWalkingVideo}, - {"yolo-p:bdd100k-320x320:7019bb8", yoloPBdd100k320x320GroundTruth, peopleWalkingVideo}, + {"luxonis/yolo-p:bdd100k-320x320:e14d3d9", yoloPBdd100k320x320GroundTruth, peopleWalkingVideo}, {"fire-detection:512x288:4a8263c", fireDetection512x288GroundTruth, fireVideo}}; for(const auto& testCase : testCases) { From 40e31b71973807e6d92b6eaf342c7cb4f15fc626 Mon Sep 17 00:00:00 2001 From: jakaskerl Date: Tue, 10 Feb 2026 14:59:50 +0100 Subject: [PATCH 152/180] Fix device manager pybind crash --- utilities/device_manager.py | 42 ++++++++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/utilities/device_manager.py b/utilities/device_manager.py index d3878dfa80..ba0202a94e 100755 --- a/utilities/device_manager.py +++ b/utilities/device_manager.py @@ -197,7 +197,7 @@ def search_devices(self): else: rows = [] for info in self.infos: - if "X_LINK_GATE" == info.state.name: continue # Skip RVC4 devices + if "GATE" in info.state.name: continue # Skip RVC4 devices rows.append([info.getDeviceId(), info.name, deviceStateTxt(info.state)]) self.window['table'].update(values=rows) @@ -270,20 +270,37 @@ def factoryReset(device: dai.DeviceInfo, type: dai.DeviceBootloader.Type): try: pr = Progress('Preparing and connecting...') - blBinary = dai.DeviceBootloader.getEmbeddedBootloaderBinary(type) - # Clear 1 MiB for USB BL and 8 MiB for NETWORK BL - mib = 1 if type == dai.DeviceBootloader.Type.USB else 8 - blBinary = blBinary + ([0xFF] * ((mib * 1024 * 1024 + 512) - len(blBinary))) - tmpBlFw = tempfile.NamedTemporaryFile(delete=False) - tmpBlFw.write(bytes(blBinary)) - bl = dai.DeviceBootloader(device, True) - progress = lambda p : pr.update(p) - success, msg = bl.flashBootloader(progress, tmpBlFw.name) + if type == dai.DeviceBootloader.Type.AUTO: + type = bl.getType() + + tmpBlFw = None + try: + blBinary = dai.DeviceBootloader.getEmbeddedBootloaderBinary(type) + # Clear 1 MiB for USB BL and 8 MiB for NETWORK BL + mib = 1 if type == dai.DeviceBootloader.Type.USB else 8 + blBinary = blBinary + ([0xFF] * ((mib * 1024 * 1024 + 512) - len(blBinary))) + tmpBlFw = tempfile.NamedTemporaryFile(delete=False) + tmpBlFw.write(bytes(blBinary)) + tmpBlFw.flush() + success, msg = bl.flashBootloader(progress, tmpBlFw.name) + except Exception as ex: + # In some depthai Python builds vector return conversion is unavailable. + if "Unable to convert function return value to a Python type" in str(ex): + success, msg = bl.flashBootloader( + memory=dai.DeviceBootloader.Memory.FLASH, + type=type, + progressCallback=progress + ) + else: + raise + finally: + if tmpBlFw is not None: + tmpBlFw.close() + msg = "Factory reset was successful." if success else f"Factory reset failed. Error: {msg}" pr.finish(msg) - tmpBlFw.close() except Exception as ex: PrintException() sg.Popup(f'{ex}') @@ -864,7 +881,8 @@ def getDevices(self): else: for deviceInfo in deviceInfos: deviceTxt = deviceInfo.getDeviceId() - if "X_LINK_GATE" == deviceInfo.state.name: continue # Skip RVC4 devices + print(deviceInfo.state.name) + if "GATE" in deviceInfo.state.name: continue # Skip RVC4 devices listedDevices.append(deviceTxt) self.devices[deviceTxt] = deviceInfo From 551900de5192e48371a34aa5628d17c00d96553b Mon Sep 17 00:00:00 2001 From: aljazkonec1 <53098513+aljazkonec1@users.noreply.github.com> Date: Tue, 10 Feb 2026 17:13:47 +0100 Subject: [PATCH 153/180] Add SegmentationMask message and SegmentationParser (#1661) * Add SegmentationMask message and SegmentationParser support * Bindings now return numpy array instead of Object. * Apply suggestions from code review * Make attributes private and remove using declarators * Add timestamp and improve copy * Add const * Add scaling * remove compiler assumptions * Remove RVC2 friend class and make setSize public --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- CMakeLists.txt | 4 + bindings/python/CMakeLists.txt | 3 + bindings/python/src/DatatypeBindings.cpp | 6 + .../src/nn_archive/NNArchiveBindings.cpp | 1 + .../src/pipeline/datatype/NNDataBindings.cpp | 9 +- .../datatype/SegmentationMaskBindings.cpp | 144 ++++++ .../SegmentationParserConfigBindings.cpp | 45 ++ .../python/src/pipeline/node/NodeBindings.cpp | 2 + .../node/SegmentationParserBindings.cpp | 59 +++ cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/cpp/CMakeLists.txt | 1 + examples/cpp/Segmentation/CMakeLists.txt | 5 + .../Segmentation/semantic_segmentation.cpp | 152 +++++++ .../Segmentation/semantic_segmentation.py | 91 ++++ include/depthai/common/TensorInfo.hpp | 110 ++++- include/depthai/nn_archive/NNArchive.hpp | 8 + .../pipeline/datatype/DatatypeEnum.hpp | 2 + .../pipeline/datatype/SegmentationMask.hpp | 203 +++++++++ .../datatype/SegmentationParserConfig.hpp | 54 +++ include/depthai/pipeline/datatypes.hpp | 2 + .../pipeline/node/SegmentationParser.hpp | 129 ++++++ include/depthai/pipeline/nodes.hpp | 1 + .../SegmentationParserProperties.hpp | 28 ++ protos/SegmentationMask.proto | 15 + src/nn_archive/NNArchive.cpp | 8 + src/pipeline/datatype/DatatypeEnum.cpp | 6 + src/pipeline/datatype/SegmentationMask.cpp | 359 +++++++++++++++ .../datatype/SegmentationParserConfig.cpp | 28 ++ src/pipeline/datatype/StreamMessageParser.cpp | 10 + src/pipeline/node/SegmentationParser.cpp | 282 ++++++++++++ src/pipeline/node/host/Replay.cpp | 4 + .../SegmentationParserUtils.cpp | 412 ++++++++++++++++++ .../SegmentationParserUtils.hpp | 81 ++++ .../SpatialUtils.cpp | 1 - src/properties/Properties.cpp | 2 + src/utility/ProtoSerialize.cpp | 36 ++ src/utility/ProtoSerialize.hpp | 3 + tests/CMakeLists.txt | 4 + .../node/segmentation_parser_test.cpp | 379 ++++++++++++++++ 40 files changed, 2683 insertions(+), 10 deletions(-) create mode 100644 bindings/python/src/pipeline/datatype/SegmentationMaskBindings.cpp create mode 100644 bindings/python/src/pipeline/datatype/SegmentationParserConfigBindings.cpp create mode 100644 bindings/python/src/pipeline/node/SegmentationParserBindings.cpp create mode 100644 examples/cpp/Segmentation/CMakeLists.txt create mode 100644 examples/cpp/Segmentation/semantic_segmentation.cpp create mode 100644 examples/python/Segmentation/semantic_segmentation.py create mode 100644 include/depthai/pipeline/datatype/SegmentationMask.hpp create mode 100644 include/depthai/pipeline/datatype/SegmentationParserConfig.hpp create mode 100644 include/depthai/pipeline/node/SegmentationParser.hpp create mode 100644 include/depthai/properties/SegmentationParserProperties.hpp create mode 100644 protos/SegmentationMask.proto create mode 100644 src/pipeline/datatype/SegmentationMask.cpp create mode 100644 src/pipeline/datatype/SegmentationParserConfig.cpp create mode 100644 src/pipeline/node/SegmentationParser.cpp create mode 100644 src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.cpp create mode 100644 src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.hpp create mode 100644 tests/src/ondevice_tests/pipeline/node/segmentation_parser_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 08268de088..95e0bccce4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -319,8 +319,10 @@ set(TARGET_CORE_SOURCES src/pipeline/node/ImageAlign.cpp src/pipeline/node/ToF.cpp src/pipeline/node/DetectionParser.cpp + src/pipeline/node/SegmentationParser.cpp src/pipeline/utilities/DetectionParser/DetectionParserUtils.cpp src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp + src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.cpp src/pipeline/node/test/MyProducer.cpp src/pipeline/node/test/MyConsumer.cpp src/pipeline/node/UVC.cpp @@ -347,6 +349,7 @@ set(TARGET_CORE_SOURCES src/pipeline/datatype/CameraControl.cpp src/pipeline/datatype/NNData.cpp src/pipeline/datatype/ImgDetectionsT.cpp + src/pipeline/datatype/SegmentationMask.cpp src/pipeline/datatype/ImgDetections.cpp src/pipeline/datatype/SpatialImgDetections.cpp src/pipeline/datatype/SystemInformation.cpp @@ -354,6 +357,7 @@ set(TARGET_CORE_SOURCES src/pipeline/datatype/StreamMessageParser.cpp src/pipeline/datatype/SpatialLocationCalculatorData.cpp src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp + src/pipeline/datatype/SegmentationParserConfig.cpp src/pipeline/datatype/AprilTags.cpp src/pipeline/datatype/AprilTagConfig.cpp src/pipeline/datatype/Tracklets.cpp diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index f331b3101f..106118ef71 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -93,6 +93,7 @@ set(SOURCE_LIST src/pipeline/node/ToFBindings.cpp src/pipeline/node/AprilTagBindings.cpp src/pipeline/node/DetectionParserBindings.cpp + src/pipeline/node/SegmentationParserBindings.cpp src/pipeline/node/WarpBindings.cpp src/pipeline/node/SyncBindings.cpp src/pipeline/node/BenchmarkBindings.cpp @@ -123,6 +124,8 @@ set(SOURCE_LIST src/pipeline/datatype/ToFConfigBindings.cpp src/pipeline/datatype/ImageManipConfigBindings.cpp src/pipeline/datatype/ImgDetectionsBindings.cpp + src/pipeline/datatype/SegmentationParserConfigBindings.cpp + src/pipeline/datatype/SegmentationMaskBindings.cpp src/pipeline/datatype/ImgFrameBindings.cpp src/pipeline/datatype/EncodedFrameBindings.cpp src/pipeline/datatype/IMUDataBindings.cpp diff --git a/bindings/python/src/DatatypeBindings.cpp b/bindings/python/src/DatatypeBindings.cpp index 7822fc5f36..f2f4b6a196 100644 --- a/bindings/python/src/DatatypeBindings.cpp +++ b/bindings/python/src/DatatypeBindings.cpp @@ -21,6 +21,8 @@ void bind_message_group(pybind11::module& m, void* pCallstack); void bind_nndata(pybind11::module& m, void* pCallstack); void bind_neuraldepthconfig(pybind11::module& m, void* pCallstack); void bind_spatialimgdetections(pybind11::module& m, void* pCallstack); +void bind_segmentationparserconfig(pybind11::module& m, void* pCallstack); +void bind_segmentationmask(pybind11::module& m, void* pCallstack); void bind_spatiallocationcalculatorconfig(pybind11::module& m, void* pCallstack); void bind_spatiallocationcalculatordata(pybind11::module& m, void* pCallstack); void bind_stereodepthconfig(pybind11::module& m, void* pCallstack); @@ -68,6 +70,8 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_nndata); callstack.push_front(bind_neuraldepthconfig); callstack.push_front(bind_spatialimgdetections); + callstack.push_front(bind_segmentationparserconfig); + callstack.push_front(bind_segmentationmask); callstack.push_front(bind_spatiallocationcalculatorconfig); callstack.push_front(bind_spatiallocationcalculatordata); callstack.push_front(bind_stereodepthconfig); @@ -122,6 +126,8 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack) { .value("CameraControl", DatatypeEnum::CameraControl) .value("ImgDetections", DatatypeEnum::ImgDetections) .value("SpatialImgDetections", DatatypeEnum::SpatialImgDetections) + .value("SegmentationParserConfig", DatatypeEnum::SegmentationParserConfig) + .value("SegmentationMask", DatatypeEnum::SegmentationMask) .value("SystemInformation", DatatypeEnum::SystemInformation) .value("SystemInformationRVC4", DatatypeEnum::SystemInformationRVC4) .value("SpatialLocationCalculatorConfig", DatatypeEnum::SpatialLocationCalculatorConfig) diff --git a/bindings/python/src/nn_archive/NNArchiveBindings.cpp b/bindings/python/src/nn_archive/NNArchiveBindings.cpp index a1d143088a..9f9f7a41ff 100644 --- a/bindings/python/src/nn_archive/NNArchiveBindings.cpp +++ b/bindings/python/src/nn_archive/NNArchiveBindings.cpp @@ -86,6 +86,7 @@ void NNArchiveBindings::bind(pybind11::module& m, void* pCallstack) { nnArchive.def("getInputSize", &NNArchive::getInputSize, py::arg("index") = 0, DOC(dai, NNArchive, getInputSize)); nnArchive.def("getInputWidth", &NNArchive::getInputWidth, py::arg("index") = 0, DOC(dai, NNArchive, getInputWidth)); nnArchive.def("getInputHeight", &NNArchive::getInputHeight, py::arg("index") = 0, DOC(dai, NNArchive, getInputHeight)); + nnArchive.def("getHeadConfig", &NNArchive::getHeadConfig, py::arg("index") = 0, DOC(dai, NNArchive, getHeadConfig)); nnArchive.def("getSupportedPlatforms", &NNArchive::getSupportedPlatforms, DOC(dai, NNArchive, getSupportedPlatforms)); // Bind NNArchive options diff --git a/bindings/python/src/pipeline/datatype/NNDataBindings.cpp b/bindings/python/src/pipeline/datatype/NNDataBindings.cpp index 2d91fc1044..1d7b8e017f 100644 --- a/bindings/python/src/pipeline/datatype/NNDataBindings.cpp +++ b/bindings/python/src/pipeline/datatype/NNDataBindings.cpp @@ -84,7 +84,14 @@ void bind_nndata(pybind11::module& m, void* pCallstack) { .def_readwrite("offset", &TensorInfo::offset) .def_readwrite("quantization", &TensorInfo::quantization) .def_readwrite("qpScale", &TensorInfo::qpScale) - .def_readwrite("qpZp", &TensorInfo::qpZp); + .def_readwrite("qpZp", &TensorInfo::qpZp) + .def("getDataTypeSize", &TensorInfo::getDataTypeSize) + .def("getWidth", &TensorInfo::getWidth) + .def("getHeight", &TensorInfo::getHeight) + .def("getChannels", &TensorInfo::getChannels) + .def("getChannelStride", &TensorInfo::getChannelStride) + .def("getHeightStride", &TensorInfo::getHeightStride) + .def("getWidthStride", &TensorInfo::getWidthStride); tensorInfoDataType.value("FP16", TensorInfo::DataType::FP16) .value("U8F", TensorInfo::DataType::U8F) diff --git a/bindings/python/src/pipeline/datatype/SegmentationMaskBindings.cpp b/bindings/python/src/pipeline/datatype/SegmentationMaskBindings.cpp new file mode 100644 index 0000000000..38a4512606 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/SegmentationMaskBindings.cpp @@ -0,0 +1,144 @@ +#include +#include + +#include "DatatypeBindings.hpp" + +// depthai +#include "depthai/pipeline/datatype/Buffer.hpp" +#include "depthai/pipeline/datatype/SegmentationMask.hpp" +#include "ndarray_converter.h" +// pybind +#include +#include +#include +#include +#include + +namespace { +py::array_t toNumpyMask(const dai::SegmentationMask& mask, const std::vector& data) { + if(data.empty()) { + return py::array_t(); + } + + const auto width = static_cast(mask.getWidth()); + const auto height = static_cast(mask.getHeight()); + const auto count = static_cast(data.size()); + + if(width > 0 && height > 0 && width * height == count) { + py::array_t arr({height, width}); + std::memcpy(arr.mutable_data(), data.data(), data.size()); + return arr; + } + + py::array_t arr(count); + std::memcpy(arr.mutable_data(), data.data(), data.size()); + return arr; +} +} // namespace + +void bind_segmentationmask(pybind11::module& m, void* pCallstack) { + using namespace dai; + + py::class_, Buffer, std::shared_ptr> segmentationMask( + m, "SegmentationMask", DOC(dai, SegmentationMask)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + + // Actual bindings + ///////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + segmentationMask.def(py::init<>()) + .def(py::init&, size_t, size_t>(), + py::arg("mask"), + py::arg("width"), + py::arg("height"), + DOC(dai, SegmentationMask, SegmentationMask)) + .def("__repr__", &SegmentationMask::str) + .def("getWidth", &SegmentationMask::getWidth, DOC(dai, SegmentationMask, getWidth)) + .def("getHeight", &SegmentationMask::getHeight, DOC(dai, SegmentationMask, getHeight)) + .def( + "setMask", + [](SegmentationMask& self, const std::vector& mask, size_t width, size_t height) { + std::vector converted; + converted.reserve(mask.size()); + for(size_t i = 0; i < mask.size(); ++i) { + const int value = mask[i]; + if(value < 0 || value > 255) { + throw py::value_error("SegmentationMask.setMask: mask values must be in [0, 255]"); + } + converted.push_back(static_cast(value)); + } + self.setMask(converted, width, height); + }, + py::arg("mask"), + py::arg("width"), + py::arg("height"), + DOC(dai, SegmentationMask, setMask)) + .def("setMask", + static_cast(&SegmentationMask::setMask), + py::arg("frame"), + DOC(dai, SegmentationMask, setMask, 2)) + .def( + "getMaskData", [](SegmentationMask& self) { return toNumpyMask(self, self.getMaskData()); }, DOC(dai, SegmentationMask, getMaskData)) + .def("getFrame", &SegmentationMask::getFrame, DOC(dai, SegmentationMask, getFrame)) + .def("setLabels", &SegmentationMask::setLabels, py::arg("labels"), DOC(dai, SegmentationMask, setLabels)) + .def("getLabels", &SegmentationMask::getLabels, DOC(dai, SegmentationMask, getLabels)) + .def("getArea", &SegmentationMask::getArea, py::arg("index"), DOC(dai, SegmentationMask, getArea)) + .def("getCentroid", &SegmentationMask::getCentroid, py::arg("index"), DOC(dai, SegmentationMask, getCentroid)) + .def("getUniqueIndices", + [](SegmentationMask& self) { + const auto v = self.getUniqueIndices(); + py::list out; + for(auto x : v) out.append(py::int_(x)); + return out; + }) + .def( + "getMaskByIndex", + [](SegmentationMask& self, uint8_t index) { return toNumpyMask(self, self.getMaskByIndex(index)); }, + py::arg("index"), + DOC(dai, SegmentationMask, getMaskByIndex)) + .def( + "getMaskByLabel", + [](SegmentationMask& self, const std::string& label) { return toNumpyMask(self, self.getMaskByLabel(label)); }, + py::arg("label"), + DOC(dai, SegmentationMask, getMaskByLabel)) + .def("hasValidMask", &SegmentationMask::hasValidMask, DOC(dai, SegmentationMask, hasValidMask)) +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + .def("setCvMask", &SegmentationMask::setCvMask, py::arg("mask"), DOC(dai, SegmentationMask, setCvMask)) + .def( + "getCvMask", [](SegmentationMask& msg) { return msg.getCvMask(&g_numpyAllocator); }, DOC(dai, SegmentationMask, getCvMask)) + .def( + "getCvMaskByIndex", + [](SegmentationMask& msg, uint8_t index) { return msg.getCvMaskByIndex(index, &g_numpyAllocator); }, + py::arg("index"), + DOC(dai, SegmentationMask, getCvMaskByIndex)) + .def("getContour", &SegmentationMask::getContour, py::arg("index"), DOC(dai, SegmentationMask, getContour)) + .def("getBoundingBoxes", + &SegmentationMask::getBoundingBoxes, + py::arg("index"), + py::arg("calculateRotation") = false, + DOC(dai, SegmentationMask, getBoundingBoxes)) +#endif + .def("getTimestamp", &SegmentationMask::getTimestamp, DOC(dai, Buffer, getTimestamp)) + .def("getTimestampDevice", &SegmentationMask::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) + .def("getSequenceNum", &SegmentationMask::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) + .def( + "getTransformation", [](SegmentationMask& msg) { return msg.transformation; }, DOC(dai, ImgFrame, getTransformation)) + .def("setTimestamp", &SegmentationMask::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp)) + .def("setTimestampDevice", &SegmentationMask::setTimestampDevice, py::arg("timestampDevice"), DOC(dai, Buffer, setTimestampDevice)) + .def("setSequenceNum", &SegmentationMask::setSequenceNum, py::arg("sequenceNum"), DOC(dai, Buffer, setSequenceNum)) + .def( + "setTransformation", + [](SegmentationMask& msg, const ImgTransformation& transformation) { msg.transformation = transformation; }, + py::arg("transformation"), + DOC(dai, ImgFrame, setTransformation)); +} diff --git a/bindings/python/src/pipeline/datatype/SegmentationParserConfigBindings.cpp b/bindings/python/src/pipeline/datatype/SegmentationParserConfigBindings.cpp new file mode 100644 index 0000000000..2e95af82b3 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/SegmentationParserConfigBindings.cpp @@ -0,0 +1,45 @@ +#include +#include + +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" + +// depthai +#include "depthai/pipeline/datatype/SegmentationParserConfig.hpp" + +// pybind +#include +#include + +void bind_segmentationparserconfig(pybind11::module& m, void* pCallstack) { + using namespace dai; + + py::class_, Buffer, std::shared_ptr> segmentationParserConfig( + m, "SegmentationParserConfig", DOC(dai, SegmentationParserConfig)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + // Actual bindings + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + // Message + segmentationParserConfig.def(py::init<>()) + .def("__repr__", &SegmentationParserConfig::str) + .def_readwrite("confidenceThreshold", &SegmentationParserConfig::confidenceThreshold, DOC(dai, SegmentationParserConfig, confidenceThreshold)) + .def_readwrite("stepSize", &SegmentationParserConfig::stepSize, DOC(dai, SegmentationParserConfig, stepSize)) + .def("setConfidenceThreshold", + &SegmentationParserConfig::setConfidenceThreshold, + py::arg("threshold"), + DOC(dai, SegmentationParserConfig, setConfidenceThreshold)) + .def("getConfidenceThreshold", &SegmentationParserConfig::getConfidenceThreshold, DOC(dai, SegmentationParserConfig, getConfidenceThreshold)) + .def("setStepSize", &SegmentationParserConfig::setStepSize, py::arg("stepSize"), DOC(dai, SegmentationParserConfig, setStepSize)) + .def("getStepSize", &SegmentationParserConfig::getStepSize, DOC(dai, SegmentationParserConfig, getStepSize)); +} diff --git a/bindings/python/src/pipeline/node/NodeBindings.cpp b/bindings/python/src/pipeline/node/NodeBindings.cpp index 9c950a648c..528e85ef68 100644 --- a/bindings/python/src/pipeline/node/NodeBindings.cpp +++ b/bindings/python/src/pipeline/node/NodeBindings.cpp @@ -153,6 +153,7 @@ void bind_edgedetector(pybind11::module& m, void* pCallstack); void bind_featuretracker(pybind11::module& m, void* pCallstack); void bind_apriltag(pybind11::module& m, void* pCallstack); void bind_detectionparser(pybind11::module& m, void* pCallstack); +void bind_segmentationparser(pybind11::module& m, void* pCallstack); void bind_uvc(pybind11::module& m, void* pCallstack); void bind_thermal(pybind11::module& m, void* pCallstack); void bind_tof(pybind11::module& m, void* pCallstack); @@ -207,6 +208,7 @@ void NodeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_featuretracker); callstack.push_front(bind_apriltag); callstack.push_front(bind_detectionparser); + callstack.push_front(bind_segmentationparser); callstack.push_front(bind_uvc); callstack.push_front(bind_thermal); callstack.push_front(bind_tof); diff --git a/bindings/python/src/pipeline/node/SegmentationParserBindings.cpp b/bindings/python/src/pipeline/node/SegmentationParserBindings.cpp new file mode 100644 index 0000000000..050e44d88f --- /dev/null +++ b/bindings/python/src/pipeline/node/SegmentationParserBindings.cpp @@ -0,0 +1,59 @@ +#include "Common.hpp" +#include "NodeBindings.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/SegmentationParser.hpp" + +void bind_segmentationparser(pybind11::module& m, void* pCallstack) { + using namespace dai; + using namespace dai::node; + + // Node and Properties declare upfront + py::class_ segmentationParserProperties(m, "SegmentationParserProperties", DOC(dai, SegmentationParserProperties)); + auto segmentationParser = ADD_NODE(SegmentationParser); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // Call the rest of the type defines, then perform the actual bindings + Callstack* callstack = (Callstack*)pCallstack; + auto cb = callstack->top(); + callstack->pop(); + cb(m, pCallstack); + // Actual bindings + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + // Properties + segmentationParserProperties.def_readwrite("labels", &SegmentationParserProperties::labels, DOC(dai, SegmentationParserProperties, labels)) + .def_readwrite("networkOutputName", &SegmentationParserProperties::networkOutputName, DOC(dai, SegmentationParserProperties, networkOutputName)) + .def_readwrite("classesInOneLayer", &SegmentationParserProperties::classesInOneLayer, DOC(dai, SegmentationParserProperties, classesInOneLayer)) + .def_readwrite("backgroundClass", &SegmentationParserProperties::backgroundClass, DOC(dai, SegmentationParserProperties, backgroundClass)); + + // Node + segmentationParser.def_readonly("input", &SegmentationParser::input, DOC(dai, node, SegmentationParser, input)) + .def_readonly("inputConfig", &SegmentationParser::inputConfig, DOC(dai, node, SegmentationParser, inputConfig)) + .def_readonly("out", &SegmentationParser::out, DOC(dai, node, SegmentationParser, out)) + .def_readonly("initialConfig", &SegmentationParser::initialConfig, DOC(dai, node, SegmentationParser, initialConfig)) + .def( + "build", + [](SegmentationParser& self, Node::Output& input, const SegmentationParser::Model& model) { return self.build(input, model); }, + py::arg("input"), + py::arg("model"), + DOC(dai, node, SegmentationParser, build)) + .def("build", + py::overload_cast(&SegmentationParser::build), + py::arg("input"), + py::arg("head"), + DOC(dai, node, SegmentationParser, build, 2)) + .def("setLabels", &SegmentationParser::setLabels, py::arg("labels"), DOC(dai, node, SegmentationParser, setLabels)) + .def("getLabels", &SegmentationParser::getLabels, DOC(dai, node, SegmentationParser, getLabels)) + .def("setBackgroundClass", &SegmentationParser::setBackgroundClass, py::arg("backgroundClass"), DOC(dai, node, SegmentationParser, setBackgroundClass)) + .def("getBackgroundClass", &SegmentationParser::getBackgroundClass, DOC(dai, node, SegmentationParser, getBackgroundClass)) + .def("setRunOnHost", &SegmentationParser::setRunOnHost, py::arg("runOnHost"), DOC(dai, node, SegmentationParser, setRunOnHost)) + .def("runOnHost", &SegmentationParser::runOnHost, DOC(dai, node, SegmentationParser, runOnHost)); + + // ALIAS + daiNodeModule.attr("SegmentationParser").attr("Properties") = segmentationParserProperties; +} diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 9bc0128c03..a449d88f53 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+679d546bcfed18052a0282acd7143b4b6b8c4a4d") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+f88257340b5acb2d6839f95cb3d6724a8fe569e0") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 89d742aa2b..a83ecb3fa9 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "c67b7323499f77cec598e0841535fe1880006220") +set(DEPTHAI_DEVICE_SIDE_COMMIT "cc655921194a8d1b191c5219f2bf1331b115a400") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 58ccc681a3..1215d90e13 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -159,6 +159,7 @@ add_subdirectory(RGBD) add_subdirectory(Warp) add_subdirectory(ImageAlign) add_subdirectory(NeuralDepth) +add_subdirectory(Segmentation) add_subdirectory(Vpp) add_subdirectory(NeuralAssistedStereo) add_subdirectory(Gate) diff --git a/examples/cpp/Segmentation/CMakeLists.txt b/examples/cpp/Segmentation/CMakeLists.txt new file mode 100644 index 0000000000..9b3bed1150 --- /dev/null +++ b/examples/cpp/Segmentation/CMakeLists.txt @@ -0,0 +1,5 @@ +project(segmentation_mask_examples) +cmake_minimum_required(VERSION 3.10) + +dai_add_example(semantic_segmentation semantic_segmentation.cpp ON OFF) +dai_set_example_test_labels(semantic_segmentation rvc2_all rvc4 ci) diff --git a/examples/cpp/Segmentation/semantic_segmentation.cpp b/examples/cpp/Segmentation/semantic_segmentation.cpp new file mode 100644 index 0000000000..12435e6f4d --- /dev/null +++ b/examples/cpp/Segmentation/semantic_segmentation.cpp @@ -0,0 +1,152 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/depthai.hpp" + +namespace { +cv::Mat showColoredSegmentationFrame(dai::SegmentationMask& segMask, float colorScalingFactor) { + cv::Mat segFrame = segMask.getCvMask(); + if(segFrame.empty()) { + return {}; + } + + if(segFrame.type() != CV_8UC1) { + cv::Mat segFrameU8; + segFrame.convertTo(segFrameU8, CV_8UC1); + segFrame = segFrameU8; + } + + cv::Mat scaledAll; + segFrame.convertTo(scaledAll, CV_32F); + scaledAll *= colorScalingFactor; + scaledAll.convertTo(scaledAll, CV_8UC1); + + cv::Mat scaledMask = segFrame.clone(); + scaledAll.copyTo(scaledMask, segFrame < 255); + + cv::Mat coloredSegFrame; + cv::applyColorMap(scaledMask, coloredSegFrame, cv::COLORMAP_JET); + coloredSegFrame.setTo(cv::Scalar(0, 0, 0), segFrame == 255); + + const auto labels = segMask.getLabels(); + if(!labels.empty()) { + auto uniqueIndices = segMask.getUniqueIndices(); + for(int idx = 0; idx < uniqueIndices.size(); ++idx) { + const std::string& label = labels[uniqueIndices[idx]]; + uint8_t scaledValue = cv::saturate_cast(static_cast(uniqueIndices[idx] * colorScalingFactor)); + cv::Mat colorValue(1, 1, CV_8UC1, cv::Scalar(scaledValue)); + cv::Mat textColorMat; + cv::applyColorMap(colorValue, textColorMat, cv::COLORMAP_JET); + auto textColor = textColorMat.at(0, 0); + + cv::putText(coloredSegFrame, + std::to_string(static_cast(uniqueIndices[idx])) + ": " + label, + cv::Point(10, 20 + idx * 20), + cv::FONT_HERSHEY_TRIPLEX, + 0.5, + cv::Scalar(textColor[0], textColor[1], textColor[2]), + 1); + } + } + + return coloredSegFrame; +} +} // namespace + +int main() { + auto device = std::make_shared(); + dai::Pipeline pipeline{device}; + std::string modelName = "luxonis/deeplab-v3-plus:512x512"; + if(device && device->getPlatform() == dai::Platform::RVC2) { + modelName = "luxonis/deeplab-v3-plus:person-256x256"; + } + + std::cout << "Creating pipeline..." << std::endl; + + auto cameraNode = pipeline.create(); + cameraNode->build(); + + auto neuralNetwork = pipeline.create(); + neuralNetwork->build(cameraNode, modelName); + + auto segParser = pipeline.create(); + segParser->build(neuralNetwork->out, modelName); + + auto maskQueue = segParser->out.createOutputQueue(); + auto frameQueue = neuralNetwork->passthrough.createOutputQueue(); + auto configQueue = segParser->inputConfig.createInputQueue(); + auto config = segParser->initialConfig; + const auto labels = segParser->getLabels(); + const float colorScalingFactor = 255.0f / (static_cast(labels.size()) + 1.0f); + + pipeline.start(); + std::cout << "Pipeline created." << std::endl; + std::cout << "Controls: 'l' increase threshold, 'j' decrease threshold, 'q' quit." << std::endl; + + auto startTime = std::chrono::steady_clock::now(); + int frames = 0; + + while(pipeline.isRunning()) { + auto outSegMask = maskQueue->get(); + auto frameMsg = frameQueue->get(); + + cv::Mat frame = frameMsg->getCvFrame(); + cv::Mat coloredSegFrame = cv::Mat::zeros(frame.size(), frame.type()); + + cv::Mat segColored = showColoredSegmentationFrame(*outSegMask, colorScalingFactor); + if(!segColored.empty()) { + coloredSegFrame = segColored; + } + cv::imshow("Segmentation Mask", coloredSegFrame); + + cv::Mat copyColoredFrame = coloredSegFrame.clone(); + if(!copyColoredFrame.empty()) { + cv::Mat mask = copyColoredFrame == 255; + std::vector maskChannels; + std::vector coloredChannels; + std::vector frameChannels; + cv::split(mask, maskChannels); + cv::split(copyColoredFrame, coloredChannels); + cv::split(frame, frameChannels); + for(size_t i = 0; i < coloredChannels.size() && i < frameChannels.size(); ++i) { + frameChannels[i].copyTo(coloredChannels[i], maskChannels[i]); + } + cv::merge(coloredChannels, copyColoredFrame); + } + cv::addWeighted(frame, 0.5, copyColoredFrame, 0.5, 0, frame); + + ++frames; + auto now = std::chrono::steady_clock::now(); + float fps = frames / std::max(1e-6f, std::chrono::duration(now - startTime).count()); + std::ostringstream fpsStream; + fpsStream << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, "NN fps: " + fpsStream.str(), cv::Point(2, frame.rows - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, cv::Scalar(255, 255, 255)); + cv::imshow("Frame", frame); + + auto key = cv::waitKey(1) & 0xFF; + if(key == 'q') { + pipeline.stop(); + break; + } else if(key == 'l') { + float current = config->getConfidenceThreshold(); + config->setConfidenceThreshold(current + 0.1f); + configQueue->send(config); + std::cout << "Increased confidence threshold to " << std::fixed << std::setprecision(2) << config->getConfidenceThreshold() << std::endl; + } else if(key == 'j') { + float current = config->getConfidenceThreshold(); + config->setConfidenceThreshold(current - 0.1f); + configQueue->send(config); + std::cout << "Decreased confidence threshold to " << std::fixed << std::setprecision(2) << config->getConfidenceThreshold() << std::endl; + } + } + + return 0; +} diff --git a/examples/python/Segmentation/semantic_segmentation.py b/examples/python/Segmentation/semantic_segmentation.py new file mode 100644 index 0000000000..dc3700e1dc --- /dev/null +++ b/examples/python/Segmentation/semantic_segmentation.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 + +import time + +import cv2 +import depthai as dai +import numpy as np + +def showColoredSegmentationFrame(segMask: dai.SegmentationMask, colorScalingFactor: float): + segFrame = segMask.getCvMask() + segFrame = segFrame.astype(np.uint8) + segFrame[segFrame < 255] = (segFrame[segFrame < 255] * colorScalingFactor).astype(np.uint8) + coloredSegFrame = cv2.applyColorMap(segFrame, cv2.COLORMAP_JET) + coloredSegFrame[segFrame == 255] = 0 + uniqueLabelIndexes = segMask.getUniqueIndices() + if len(segMask.getLabels()) > 0: + for i, labelIndex in enumerate(uniqueLabelIndexes): + labelStr = segMask.getLabels()[labelIndex] + colorValue = np.array([[int(labelIndex * colorScalingFactor)]], dtype=np.uint8) + textColor = cv2.applyColorMap(colorValue, cv2.COLORMAP_JET)[0, 0].tolist() + cv2.putText(coloredSegFrame, f"{labelIndex}: {labelStr}", (10, 20 + i * 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, textColor, 1) + cv2.imshow("Segmentation Mask", coloredSegFrame) + return coloredSegFrame + +device = dai.Device() +modelName = "luxonis/deeplab-v3-plus:512x512" +if device.getPlatform() == dai.Platform.RVC2: + modelName = "luxonis/deeplab-v3-plus:person-256x256" + +with dai.Pipeline(device) as pipeline: + print("Creating pipeline...") + + cameraNode = pipeline.create(dai.node.Camera).build() + neuralNetwork = pipeline.create(dai.node.NeuralNetwork).build(cameraNode, modelName) + + segParser = pipeline.create(dai.node.SegmentationParser).build(neuralNetwork.out, modelName) + + maskQueue = segParser.out.createOutputQueue() + frameQueue = neuralNetwork.passthrough.createOutputQueue() + configQueue = segParser.inputConfig.createInputQueue() + config = segParser.initialConfig + colorScalingFactor = 255 / (len(segParser.getLabels()) + 1) + + pipeline.start() + print("Pipeline created.") + print("Controls: 'l' increase threshold, 'j' decrease threshold, 'q' quit.") + + start_time = time.monotonic() + frames = 0 + + while pipeline.isRunning(): + outSegMask: dai.SegmentationMask = maskQueue.get() + frameMsg: dai.ImgFrame = frameQueue.get() + + frame = frameMsg.getCvFrame() + coloredSegFrame = np.zeros_like(frame) + if outSegMask.hasValidMask(): + coloredSegFrame = showColoredSegmentationFrame(outSegMask, colorScalingFactor) + + if frame.shape[0] != coloredSegFrame.shape[0] or frame.shape[1] != coloredSegFrame.shape[1]: + coloredSegFrame = cv2.resize(coloredSegFrame, (frame.shape[1], frame.shape[0])) + + copyColoredFrame = coloredSegFrame.copy() + copyColoredFrame[copyColoredFrame == 255] = frame[copyColoredFrame == 255] + frame = cv2.addWeighted(frame, 0.5, copyColoredFrame, 0.5, 0) + frames += 1 + fps = frames / max(1e-6, time.monotonic() - start_time) + cv2.putText( + frame, + f"NN fps: {fps:.2f}", + (2, frame.shape[0] - 4), + cv2.FONT_HERSHEY_TRIPLEX, + 0.4, + (255, 255, 255), + ) + cv2.imshow("Frame", frame) + + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + pipeline.stop() + break + elif key == ord('l'): + current = config.getConfidenceThreshold() + config.setConfidenceThreshold(current + 0.1) + configQueue.send(config) + print(f"Increased confidence threshold to {config.getConfidenceThreshold():.2f}") + elif key == ord('j'): + current = config.getConfidenceThreshold() + config.setConfidenceThreshold(current - 0.1) + configQueue.send(config) + print(f"Decreased confidence threshold to {config.getConfidenceThreshold():.2f}") diff --git a/include/depthai/common/TensorInfo.hpp b/include/depthai/common/TensorInfo.hpp index ea3ae78e76..9dcf8a01ca 100644 --- a/include/depthai/common/TensorInfo.hpp +++ b/include/depthai/common/TensorInfo.hpp @@ -38,7 +38,7 @@ struct TensorInfo { FP64 = 5, // Double precision floating point }; - void validateStorageOrder() { + void validateStorageOrder() const { switch(order) { case StorageOrder::NHWC: case StorageOrder::NHCW: @@ -75,7 +75,7 @@ struct TensorInfo { } } - int getDataTypeSize() { + int getDataTypeSize() const { switch(dataType) { case DataType::U8F: case DataType::I8: @@ -93,7 +93,7 @@ struct TensorInfo { } } - int getWidth() { + int getWidth() const { validateStorageOrder(); switch(order) { case StorageOrder::NHWC: @@ -125,7 +125,7 @@ struct TensorInfo { } } - int getHeight() { + int getHeight() const { validateStorageOrder(); switch(order) { case StorageOrder::NHWC: @@ -157,7 +157,7 @@ struct TensorInfo { } } - std::size_t getTensorSize() { + std::size_t getTensorSize() const { uint32_t i = 0; // Handle the edge case if all dimensions are 1 @@ -182,7 +182,7 @@ struct TensorInfo { return dims[i] * strides[i]; } - int getChannels() { + int getChannels() const { validateStorageOrder(); switch(order) { case StorageOrder::NHWC: @@ -216,6 +216,104 @@ struct TensorInfo { } } + size_t getChannelStride() const { + validateStorageOrder(); + switch(order) { + case StorageOrder::NHWC: + return strides[3]; + case StorageOrder::NHCW: + return strides[2]; + case StorageOrder::NCHW: + return strides[1]; + case StorageOrder::HWC: + return strides[2]; + case StorageOrder::CHW: + return strides[0]; + case StorageOrder::WHC: + return strides[0]; + case StorageOrder::HCW: + return strides[1]; + case StorageOrder::WCH: + return strides[1]; + case StorageOrder::CWH: + return strides[0]; + case StorageOrder::C: + return strides[0]; + case StorageOrder::NC: + return strides[1]; + case StorageOrder::CN: + return strides[0]; + case StorageOrder::H: + case StorageOrder::W: + default: + return 0; + } + } + + size_t getWidthStride() const { + validateStorageOrder(); + switch(order) { + case StorageOrder::NHWC: + return strides[2]; + case StorageOrder::NHCW: + return strides[3]; + case StorageOrder::NCHW: + return strides[3]; + case StorageOrder::HWC: + return strides[1]; + case StorageOrder::CHW: + return strides[2]; + case StorageOrder::WHC: + return strides[0]; + case StorageOrder::HCW: + return strides[1]; + case StorageOrder::WCH: + return strides[0]; + case StorageOrder::CWH: + return strides[1]; + case StorageOrder::W: + return strides[0]; + case StorageOrder::NC: + case StorageOrder::CN: + case StorageOrder::H: + case StorageOrder::C: + default: + return 0; + } + } + + size_t getHeightStride() const { + validateStorageOrder(); + switch(order) { + case StorageOrder::NHWC: + return strides[1]; + case StorageOrder::NHCW: + return strides[1]; + case StorageOrder::NCHW: + return strides[2]; + case StorageOrder::HWC: + return strides[0]; + case StorageOrder::CHW: + return strides[1]; + case StorageOrder::WHC: + return strides[1]; + case StorageOrder::HCW: + return strides[0]; + case StorageOrder::WCH: + return strides[1]; + case StorageOrder::CWH: + return strides[1]; + case StorageOrder::H: + return strides[0]; + case StorageOrder::C: + case StorageOrder::NC: + case StorageOrder::CN: + case StorageOrder::W: + default: + return 0; + } + } + StorageOrder order = StorageOrder::NCHW; DataType dataType = DataType::FP16; unsigned int numDimensions = 0; diff --git a/include/depthai/nn_archive/NNArchive.hpp b/include/depthai/nn_archive/NNArchive.hpp index 0125200606..33baeb7f65 100644 --- a/include/depthai/nn_archive/NNArchive.hpp +++ b/include/depthai/nn_archive/NNArchive.hpp @@ -8,6 +8,7 @@ #include "depthai/device/Device.hpp" // For platform enum #include "depthai/nn_archive/NNArchiveEntry.hpp" #include "depthai/nn_archive/NNArchiveVersionedConfig.hpp" +#include "depthai/nn_archive/v1/Head.hpp" #include "depthai/openvino/OpenVINO.hpp" #include "depthai/utility/arg.hpp" @@ -104,6 +105,13 @@ class NNArchive { return getVersionedConfig().getConfig(); } + /** + * @brief Get specific head configuration from NNArchive. + * @param headIndex: Index of the head to retrieve + * @return dai::nn_archive::v1::Head: Head configuration + */ + dai::nn_archive::v1::Head getHeadConfig(uint32_t index = 0) const; + /** * @brief Get type of model contained in NNArchive * diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index 4d3bf7ab56..2ce0981bc3 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -9,6 +9,7 @@ enum class DatatypeEnum : std::int32_t { Buffer, ImgFrame, EncodedFrame, + SegmentationMask, GateControl, NNData, ImageManipConfig, @@ -46,6 +47,7 @@ enum class DatatypeEnum : std::int32_t { DynamicCalibrationResult, CalibrationQuality, CoverageData, + SegmentationParserConfig, PipelineEvent, PipelineState, PipelineEventAggregationConfig, diff --git a/include/depthai/pipeline/datatype/SegmentationMask.hpp b/include/depthai/pipeline/datatype/SegmentationMask.hpp new file mode 100644 index 0000000000..dceedff956 --- /dev/null +++ b/include/depthai/pipeline/datatype/SegmentationMask.hpp @@ -0,0 +1,203 @@ +#pragma once + +#include +#include +#include + +#include "depthai/common/ImgTransformations.hpp" +#include "depthai/common/RotatedRect.hpp" +#include "depthai/common/optional.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" +#include "depthai/utility/ProtoSerializable.hpp" +#include "depthai/utility/span.hpp" + +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + #include + #include +#endif + +namespace dai { + +/** + * SegmentationMask message. + * + * Segmentation mask of an image is stored as a single-channel UINT8 array, where each value represents a class or instance index. + * The value 255 is treated as background pixels (no class/instance). + */ +class SegmentationMask : public Buffer, public ProtoSerializable { + // Optimization option: if network is bottleneck, implement RLE compression for the mask data + private: + size_t width = 0; + size_t height = 0; + std::vector labels; + + public: + std::optional transformation; + + SegmentationMask(); + SegmentationMask(const std::vector& data, size_t width, size_t height); + virtual ~SegmentationMask(); + + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; + DatatypeEnum getDatatype() const override { + return DatatypeEnum::SegmentationMask; + } + + /** + * Sets the size of the segmentation mask. + * @note Use with caution as it sets the metadata of the mask without allocating or resizing the underlying data array. + */ + void setSize(size_t width, size_t height); + + /** + * Returns the width of the segmentation mask. + */ + std::size_t getWidth() const; + + /** + * Returns the height of the segmentation mask. + */ + std::size_t getHeight() const; + + /** + * Sets the segmentation mask from a vector of bytes. + * The size of the vector must be equal to width * height. + */ + void setMask(const std::vector& mask, size_t width, size_t height); + + /** + * Sets the segmentation mask from an ImgFrame. + * @param frame Frame must be of type GRAY8 + */ + void setMask(dai::ImgFrame& frame); + + /** + * Sets the segmentation mask from a byte span without an extra temporary vector. + * The span size must be equal to width * height. + */ + void setMask(span mask, size_t width, size_t height); + + /** + * Prepares internal storage for writing and returns a mutable view to it. + * The caller must fill exactly width * height bytes. + */ + span prepareMask(size_t width, size_t height); + + /** + * Sets the class labels associated with the segmentation mask. + * The label at index `i` in the `labels` vector corresponds to the value `i` in the segmentation mask data array. + * @param labels Vector of class labels + */ + void setLabels(const std::vector& labels); + + /** + * Returns a copy of the segmentation mask data as a vector of bytes. If mask data is not set, returns an empty vector. + */ + std::vector getMaskData() const; + /** + * Returns the segmentation mask as an ImgFrame. If mask data is not set, returns an empty frame with only metadata set. + */ + dai::ImgFrame getFrame() const; + + /** + * Returns the area (number of pixels) of the specified instance/class index in the segmentation mask. + * @param index Instance/Class index + * @note If index is not present in the mask, returns std::nullopt. + */ + std::optional getArea(uint8_t index) const; + + /** + * Returns the normalized centroid (x,y) coordinates of the specified instance/class index in the segmentation mask. + * @param index Instance/Class index + * @note If index is not present in the mask, returns std::nullopt. + */ + std::optional getCentroid(uint8_t index) const; + + /** + * Returns a list of sorted unique indices present in the segmentation mask. + */ + std::vector getUniqueIndices() const; + + /** + * Returns all class labels associated with the segmentation mask. If no labels are set, returns an empty vector. + */ + std::vector getLabels() const; + + /** + * Returns a binary mask where pixels belonging to the specified instance/class index are set to 1, others to 0. If mask data is not set, returns an + * empty vector. + */ + std::vector getMaskByIndex(uint8_t index) const; + + /** + * Returns a binary mask where pixels belonging to the specified class label are set to 1, others to 0. If labels are not set or label not found, + * returns an empty vector. + */ + std::vector getMaskByLabel(const std::string& label) const; + + /** + * Returns true if the mask data is not empty and has valid size (width * height). + */ + bool hasValidMask() const; + +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + /** + * @note This API only available if OpenCV support is enabled + */ + + /** + * Copies cv::Mat data to Segmentation Mask buffer + * + * @param frame Input cv::Mat frame from which to copy the data + * @note Throws if mask is not a single channel INT8 type. + */ + void setCvMask(cv::Mat mask); + + /** + * Retrieves mask data as a cv::Mat copy with specified width and height. If mask data is not set, returns an empty matrix. + * @param allocator Allows callers to supply a custom cv::MatAllocator for zero-copy/custom memory management; nullptr uses OpenCV’s default. + */ + cv::Mat getCvMask(cv::MatAllocator* allocator = nullptr); + + /** + * Returns a binary mask where pixels belonging to the instance index are set to 1, others to 0. If mask data is not set, returns an empty matrix. + * @param index Instance index + * @param allocator Allows callers to supply a custom cv::MatAllocator for zero-copy/custom memory management; nullptr uses OpenCV’s default. + */ + cv::Mat getCvMaskByIndex(uint8_t index, cv::MatAllocator* allocator = nullptr); + /** + * Calls the opencv findContours function and filters the results based on the provided index. Returns filtered contour as a vector of vectors of + * non-normalized points. If mask data is not set, returns an empty vector. + * @param index class index + */ + std::vector> getContour(uint8_t index); + + /** + * Returns a bounding box for each continous region with the specified index. + * @param index class index + * @param calculateRotation If true, returns rotated bounding boxes, otherwise returns the outer, axis-aligned bounding boxes. + */ + std::vector getBoundingBoxes(uint8_t index, bool calculateRotation = true); + +#endif + +#ifdef DEPTHAI_ENABLE_PROTOBUF + /** + * Serialize message to proto buffer + * + * @returns serialized message + */ + std::vector serializeProto(bool = false) const override; + + /** + * Serialize schema to proto buffer + * + * @returns serialized schema + */ + ProtoSerializable::SchemaPair serializeSchema() const override; +#endif + + DEPTHAI_SERIALIZE(SegmentationMask, Buffer::ts, Buffer::tsDevice, Buffer::sequenceNum, transformation, width, height, labels); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatype/SegmentationParserConfig.hpp b/include/depthai/pipeline/datatype/SegmentationParserConfig.hpp new file mode 100644 index 0000000000..7dfc675e91 --- /dev/null +++ b/include/depthai/pipeline/datatype/SegmentationParserConfig.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +class SegmentationParserConfig : public Buffer { + public: + float confidenceThreshold = -1.0f; + unsigned int stepSize = 1; + + /** + * Construct SegmentationParserConfig message. + */ + SegmentationParserConfig() = default; + virtual ~SegmentationParserConfig(); + + /** + * Add a confidence threshold to the argmax operation over the segmentation tensor. + * Pixels with confidence values below this threshold will be assigned the background class (255). + * @param threshold Confidence threshold for segmentation parsing + * @note Default is -1.0f, which means no thresholding is applied. + * @note Only applicable if output classes are not in a single layer (eg. classesInOneLayer = false). + */ + void setConfidenceThreshold(float threshold); + + /** + * Get confidence threshold + */ + float getConfidenceThreshold() const; + + /** + * Sets the step size for segmentation parsing. + * A step size of 1 means every pixel is processed, a step size of 2 means every second pixel is processed, and so on. + * This can be used to speed up processing at the cost of lower resolution masks. + * @param stepSize Step size for segmentation parsing + */ + void setStepSize(unsigned int stepSize); + + /** + * Gets the step size for segmentation parsing. + */ + unsigned int getStepSize() const; + + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override; + + DatatypeEnum getDatatype() const override { + return DatatypeEnum::SegmentationParserConfig; + } + + DEPTHAI_SERIALIZE(SegmentationParserConfig, confidenceThreshold, stepSize); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatypes.hpp b/include/depthai/pipeline/datatypes.hpp index e9a51052ad..5fc75073ce 100644 --- a/include/depthai/pipeline/datatypes.hpp +++ b/include/depthai/pipeline/datatypes.hpp @@ -23,6 +23,8 @@ #include "datatype/PointCloudConfig.hpp" #include "datatype/PointCloudData.hpp" #include "datatype/RGBDData.hpp" +#include "datatype/SegmentationMask.hpp" +#include "datatype/SegmentationParserConfig.hpp" #include "datatype/SpatialImgDetections.hpp" #include "datatype/SpatialLocationCalculatorConfig.hpp" #include "datatype/SpatialLocationCalculatorData.hpp" diff --git a/include/depthai/pipeline/node/SegmentationParser.hpp b/include/depthai/pipeline/node/SegmentationParser.hpp new file mode 100644 index 0000000000..f796c30783 --- /dev/null +++ b/include/depthai/pipeline/node/SegmentationParser.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include +#include + +#include "depthai/pipeline/datatype/SegmentationParserConfig.hpp" + +// standard +#include +#include +#include +#include +#include + +namespace dai { +namespace node { + +/** + * @brief SegmentationParser node. Parses raw segmentation output from segmentation neural networks into a dai::SegmentationMask datatype. + * The parser supports two output model types: + * 1. Single-channel output where the model argmaxes the class probabilities internally and outputs a single channel mask with class indices. + * 2. Multi-channel output where each channel corresponds to the probability map for a specific class. The parser will perform argmax across channels to + * generate the final mask. The parser can be configured to treat the first class (index 0) as the background class, which will be ignored in the final + * segmentation mask. + * + * @warning Only OAK4 supports running SegmentationParser on device. On other platforms, the node will automatically switch to host execution. + */ +class SegmentationParser : public DeviceNodeCRTP, public HostRunnable { + public: + constexpr static const char* NAME = "SegmentationParser"; + using DeviceNodeCRTP::DeviceNodeCRTP; + using Model = std::variant; + + ~SegmentationParser() override; + SegmentationParser() = default; + SegmentationParser(std::unique_ptr props) : DeviceNodeCRTP(std::move(props)) {} + + /** + * Initial config to use when parsing segmentation masks. + */ + std::shared_ptr initialConfig = std::make_shared(); + + /** + * Input NN results with segmentation data to parser + */ + Input input{*this, {"input", DEFAULT_GROUP, true, 5, {{{DatatypeEnum::NNData, true}}}, true}}; + + /** + * Input SegmentationParserConfig message with ability to modify parameters in runtime. + */ + Input inputConfig{*this, {"inputConfig", DEFAULT_GROUP, false, 4, {{{DatatypeEnum::SegmentationParserConfig, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; + + /** + * Outputs segmentation mask + */ + Output out{*this, {"out", DEFAULT_GROUP, {{{DatatypeEnum::SegmentationMask, false}}}}}; + + /** + * @brief Build SegmentationParser node + * @param nnInput: Output to link + * @param nnArchive: Neural network archive + */ + std::shared_ptr build(Node::Output& nnInput, const Model& model); + + /** + * @brief Build SegmentationParser node with the specific head from NNArchive. Useful when model outputs multiple segmentation heads. + * @param nnInput: Output to link + * @param head: Specific head from NNArchive to use for this parser + */ + std::shared_ptr build(Node::Output& nnInput, const dai::nn_archive::v1::Head& head); + + /** + * Sets the class labels associated with the segmentation mask. + * The label at index $i$ in the `labels` vector corresponds to the value $i$ in the segmentation mask data array. + * @param labels Vector of class labels + */ + void setLabels(const std::vector& labels); + + /** + * Returns the class labels associated with the segmentation mask. + */ + std::vector getLabels() const; + + /** + * Sets whether the first class (index 0) is considered the background class. + * If true, the pixels classified as index 0 will be treated as background. + * @param backgroundClass Boolean indicating if the first class is the background class + * + * @note Only applicable if the number of classes is greater than 1 and the output classes are not in a single layer (eg. classesInOneLayer = false). + */ + void setBackgroundClass(bool backgroundClass); + + /** + * Gets whether the first class (index 0) is considered the background class. + */ + bool getBackgroundClass() const; + + /** + * Specify whether to run on host or device + * By default, the node will run on device. + */ + void setRunOnHost(bool runOnHost); + + /** + * Check if the node is set to run on host + */ + bool runOnHost() const override; + + void run() override; + + void buildInternal() override; + + private: + bool runOnHostVar = false; + void setConfig(const dai::NNArchiveVersionedConfig& config); + void setConfig(const dai::nn_archive::v1::Head& head); + void validateTensor(std::optional& info); + std::optional archiveConfig; + std::shared_ptr inConfig; + // TODO (aljazkonec1): common functions that are shared with NeuralNetwork, DetectionNetwork, etc. should be moved to a common base class + NNArchive decodeModel(const Model& model); + NNArchive createNNArchive(NNModelDescription& modelDesc); + + protected: + Properties& getProperties() override; +}; + +} // namespace node +} // namespace dai diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 7a12feed91..255dc32899 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -9,6 +9,7 @@ #include "node/DetectionNetwork.hpp" #include "node/DetectionParser.hpp" #include "node/Rectification.hpp" +#include "node/SegmentationParser.hpp" #ifdef DEPTHAI_HAVE_DYNAMIC_CALIBRATION_SUPPORT #include "node/DynamicCalibrationNode.hpp" #endif diff --git a/include/depthai/properties/SegmentationParserProperties.hpp b/include/depthai/properties/SegmentationParserProperties.hpp new file mode 100644 index 0000000000..7e45bf12e6 --- /dev/null +++ b/include/depthai/properties/SegmentationParserProperties.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include "depthai/properties/Properties.hpp" + +namespace dai { + +/** + * Specify properties for SegmentationParser + * @ingroup properties + * @property labels Vector of class labels associated with the segmentation mask. The label at index $i$ in the `labels` vector corresponds to the value $i$-th + channel in the segmentation mask data array. + * @param networkOutputName Name of the output tensor from the neural network to parse. If empty, the first output will be used. + * @param classesInOneLayer If true, assumes that the segmentation classes are already encoded in a single layer as integer values. If false, an argmax + * operation is performed across multiple channels. + * @param backgroundClass If true, the first class (index 0) is considered as background. + */ +struct SegmentationParserProperties : PropertiesSerializable { + std::vector labels; + std::string networkOutputName; + bool classesInOneLayer = false; + bool backgroundClass = false; + + ~SegmentationParserProperties() override; +}; + +DEPTHAI_SERIALIZE_EXT(SegmentationParserProperties, labels, networkOutputName, classesInOneLayer, backgroundClass); + +} // namespace dai diff --git a/protos/SegmentationMask.proto b/protos/SegmentationMask.proto new file mode 100644 index 0000000000..d51e5cb364 --- /dev/null +++ b/protos/SegmentationMask.proto @@ -0,0 +1,15 @@ +syntax = "proto3"; + +import "common.proto"; +package dai.proto.segmentation_mask; + +message SegmentationMask { + common.Timestamp ts = 1; + common.Timestamp tsDevice = 2; + int64 sequenceNum = 3; + common.ImgTransformation transformation = 4; + int64 width = 5; + int64 height = 6; + bytes data = 7; + repeated string labels = 8; +} diff --git a/src/nn_archive/NNArchive.cpp b/src/nn_archive/NNArchive.cpp index ab62da4f79..7a3759f8e2 100644 --- a/src/nn_archive/NNArchive.cpp +++ b/src/nn_archive/NNArchive.cpp @@ -196,4 +196,12 @@ std::vector NNArchive::getSupportedPlatforms() const { } } +dai::nn_archive::v1::Head NNArchive::getHeadConfig(uint32_t headIndex) const { + const auto& configV1 = getConfig(); + DAI_CHECK_V(configV1.model.heads, "No heads defined in NNArchive config."); + DAI_CHECK_V(headIndex < configV1.model.heads->size(), "Head index out of bounds in NNArchive config."); + + return configV1.model.heads->at(headIndex); +} + } // namespace dai diff --git a/src/pipeline/datatype/DatatypeEnum.cpp b/src/pipeline/datatype/DatatypeEnum.cpp index a486586141..1a92ba3d81 100644 --- a/src/pipeline/datatype/DatatypeEnum.cpp +++ b/src/pipeline/datatype/DatatypeEnum.cpp @@ -14,6 +14,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::Buffer, DatatypeEnum::ImgFrame, DatatypeEnum::EncodedFrame, + DatatypeEnum::SegmentationMask, DatatypeEnum::NNData, DatatypeEnum::ImageManipConfig, DatatypeEnum::CameraControl, @@ -23,6 +24,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::SystemInformation, DatatypeEnum::SystemInformationRVC4, DatatypeEnum::SpatialLocationCalculatorConfig, + DatatypeEnum::SegmentationParserConfig, DatatypeEnum::SpatialLocationCalculatorData, DatatypeEnum::EdgeDetectorConfig, DatatypeEnum::Tracklets, @@ -60,6 +62,7 @@ const std::unordered_map> hierarchy = { { DatatypeEnum::ImgFrame, DatatypeEnum::EncodedFrame, + DatatypeEnum::SegmentationMask, DatatypeEnum::NNData, DatatypeEnum::ImageManipConfig, DatatypeEnum::CameraControl, @@ -69,6 +72,7 @@ const std::unordered_map> hierarchy = { DatatypeEnum::SystemInformation, DatatypeEnum::SystemInformationRVC4, DatatypeEnum::SpatialLocationCalculatorConfig, + DatatypeEnum::SegmentationParserConfig, DatatypeEnum::SpatialLocationCalculatorData, DatatypeEnum::EdgeDetectorConfig, DatatypeEnum::Tracklets, @@ -105,6 +109,7 @@ const std::unordered_map> hierarchy = { }}, {DatatypeEnum::ImgFrame, {}}, {DatatypeEnum::EncodedFrame, {}}, + {DatatypeEnum::SegmentationMask, {}}, {DatatypeEnum::NNData, {}}, {DatatypeEnum::ImageManipConfig, {}}, {DatatypeEnum::ImageAlignConfig, {}}, @@ -115,6 +120,7 @@ const std::unordered_map> hierarchy = { {DatatypeEnum::SystemInformationRVC4, {}}, {DatatypeEnum::SpatialLocationCalculatorConfig, {}}, {DatatypeEnum::SpatialLocationCalculatorData, {}}, + {DatatypeEnum::SegmentationParserConfig, {}}, {DatatypeEnum::EdgeDetectorConfig, {}}, {DatatypeEnum::Tracklets, {}}, {DatatypeEnum::IMUData, {}}, diff --git a/src/pipeline/datatype/SegmentationMask.cpp b/src/pipeline/datatype/SegmentationMask.cpp new file mode 100644 index 0000000000..82e8a6863b --- /dev/null +++ b/src/pipeline/datatype/SegmentationMask.cpp @@ -0,0 +1,359 @@ + +#include "depthai/pipeline/datatype/SegmentationMask.hpp" + +#include +#include +#include +#include +#include + +#include "depthai/common/RotatedRect.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" + +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + #include "utility/ErrorMacros.hpp" +#endif + +#ifdef DEPTHAI_ENABLE_PROTOBUF + #include "utility/ProtoSerialize.hpp" +#endif + +namespace dai { + +SegmentationMask::~SegmentationMask() = default; +SegmentationMask::SegmentationMask() = default; + +SegmentationMask::SegmentationMask(const std::vector& data, const size_t width, const size_t height) : SegmentationMask() { + setMask(data, width, height); +} + +void SegmentationMask::setSize(size_t width, size_t height) { + this->width = width; + this->height = height; +} + +std::size_t SegmentationMask::getWidth() const { + return width; +} +std::size_t SegmentationMask::getHeight() const { + return height; +} + +void SegmentationMask::setMask(const std::vector& mask, size_t width, size_t height) { + if(mask.size() != width * height) { + throw std::runtime_error("SegmentationMask: data size does not match width*height"); + } + setData(mask); + this->width = width; + this->height = height; +} +void SegmentationMask::setMask(span mask, size_t width, size_t height) { + if(mask.size() != width * height) { + throw std::runtime_error("SegmentationMask: data size does not match width*height"); + } + data->setSize(mask.size()); + std::memcpy(data->getData().data(), mask.data(), mask.size()); + this->width = width; + this->height = height; +} + +span SegmentationMask::prepareMask(size_t width, size_t height) { + const size_t size = width * height; + data->setSize(size); + this->width = width; + this->height = height; + return data->getData(); +} + +void SegmentationMask::setMask(dai::ImgFrame& frame) { + if(frame.getType() != dai::ImgFrame::Type::GRAY8) { + throw std::runtime_error("SegmentationMask: ImgFrame type must be GRAY8"); + } + const size_t width = frame.getWidth(); + const size_t height = frame.getHeight(); + const size_t stride = frame.getStride(); + if(stride == width) { + setMask(frame.getData(), width, height); + } else { // Need to repack the data + auto dataSpan = frame.getData(); + const size_t packedSize = static_cast(width) * static_cast(height); + const size_t minSourceSize = (static_cast(height - 1) * static_cast(stride)) + static_cast(width); + if(dataSpan.size() < minSourceSize) { + throw std::runtime_error("SegmentationMask: ImgFrame data size does not match width/height/stride"); + } + data->setSize(packedSize); + auto dst = data->getData(); + for(size_t y = 0; y < height; y++) { + const size_t srcOffset = y * static_cast(stride); + const size_t dstOffset = y * static_cast(width); + std::memcpy(dst.data() + dstOffset, dataSpan.data() + srcOffset, width); + } + } + this->transformation = frame.transformation; + setTimestamp(frame.getTimestamp()); + setTimestampDevice(frame.getTimestampDevice()); + setSequenceNum(frame.getSequenceNum()); +} + +std::vector SegmentationMask::getMaskData() const { + const auto& d = data->getData(); + if(d.empty()) { + return {}; + } + return std::vector(d.begin(), d.end()); +} + +dai::ImgFrame SegmentationMask::getFrame() const { + dai::ImgFrame img; + img.setSequenceNum(getSequenceNum()); + img.setTimestamp(getTimestamp()); + img.setTimestampDevice(getTimestampDevice()); + if(transformation) { + img.transformation = *transformation; + } + std::vector maskData = getMaskData(); + if(maskData.empty()) { + return img; + } + + img.setWidth(static_cast(width)); + img.setHeight(static_cast(height)); + img.setType(dai::ImgFrame::Type::GRAY8); + img.setData(maskData); + + return img; +} + +std::optional SegmentationMask::getArea(uint8_t index) const { + std::vector maskData = getMaskData(); + if(maskData.empty()) { + return std::nullopt; + } + uint32_t area = 0; + for(const auto& val : maskData) { + if(val == index) { + area++; + } + } + if(area == 0) { + return std::nullopt; + } + + return area; +} + +std::optional SegmentationMask::getCentroid(uint8_t index) const { + std::vector maskData = getMaskData(); + if(maskData.empty()) { + return std::nullopt; + } + + int32_t area = 0; + int32_t sumX = 0; + int32_t sumY = 0; + for(size_t y = 0; y < height; y++) { + for(size_t x = 0; x < width; x++) { + size_t idx = y * width + x; + if(maskData[idx] == index) { + area++; + sumX += static_cast(x); + sumY += static_cast(y); + } + } + } + if(area == 0) { + return std::nullopt; + } + float cx = static_cast(sumX) / static_cast(area) / static_cast(width); + float cy = static_cast(sumY) / static_cast(area) / static_cast(height); + + return dai::Point2f(cx, cy, true); +} + +std::vector SegmentationMask::getUniqueIndices() const { + std::vector maskData = getMaskData(); + std::vector uniqueIndices; + if(maskData.empty()) { + return uniqueIndices; + } + + std::vector indexPresent(256, false); + for(const auto& val : maskData) { + if(!indexPresent[val] && val != 255) { + indexPresent[val] = true; + uniqueIndices.push_back(val); + } + } + std::sort(uniqueIndices.begin(), uniqueIndices.end()); + return uniqueIndices; +} + +void SegmentationMask::setLabels(const std::vector& labels) { + this->labels = labels; +} + +std::vector SegmentationMask::getLabels() const { + return labels; +} + +std::vector SegmentationMask::getMaskByIndex(uint8_t index) const { + std::vector maskData = getMaskData(); + if(maskData.empty()) { + return {}; + } + + std::vector indexedMask(maskData.size(), 0); + for(size_t i = 0; i < maskData.size(); i++) { + if(maskData[i] == index) { + indexedMask[i] = 1; + } + } + return indexedMask; +} + +std::vector SegmentationMask::getMaskByLabel(const std::string& label) const { + if(labels.empty()) { + return {}; + } + + auto it = std::find(labels.begin(), labels.end(), label); + if(it == labels.end()) { + return {}; + } + return getMaskByIndex(static_cast(std::distance(labels.begin(), it))); +} + +bool SegmentationMask::hasValidMask() const { + return data->getSize() == width * height; +} + +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT +void SegmentationMask::setCvMask(cv::Mat mask) { + if(mask.type() != CV_8UC1) { + throw std::runtime_error("SetCvSegmentationMask: Mask must be of INT8 type, got opencv type " + cv::typeToString(mask.type()) + "."); + } + std::vector dataVec; + if(!mask.isContinuous()) { + for(int i = 0; i < mask.rows; i++) { + dataVec.insert(dataVec.end(), mask.ptr(i), mask.ptr(i) + mask.cols * mask.elemSize()); + } + } else { + dataVec.insert(dataVec.begin(), mask.datastart, mask.dataend); + } + setData(dataVec); + this->width = mask.cols; + this->height = mask.rows; +} + +cv::Mat SegmentationMask::getCvMask(cv::MatAllocator* allocator) { + cv::Mat mask; + if(data->getData().data() == nullptr || data->getSize() == 0) { + return mask; + } + cv::Size size(static_cast(getWidth()), static_cast(getHeight())); + int type = CV_8UC1; + + const size_t requiredSize = CV_ELEM_SIZE(type) * static_cast(size.area()); + const size_t actualSize = data->getSize(); + + DAI_CHECK_V(actualSize == requiredSize, "Segmentation mask data size does not match the expected size, required {}, actual {}.", requiredSize, actualSize); + + mask = cv::Mat(size, type, data->getData().data()); + + cv::Mat output; + if(allocator != nullptr) { + output.allocator = allocator; + } + (mask).copyTo(output); + return output; +} + +cv::Mat SegmentationMask::getCvMaskByIndex(uint8_t index, cv::MatAllocator* allocator) { + cv::Mat mask = getCvMask(allocator); + if(mask.empty()) { + return cv::Mat(); + } + + cv::Mat indexedMask; + cv::compare(mask, index, indexedMask, cv::CmpTypes::CMP_EQ); + return indexedMask; +} + +std::vector> SegmentationMask::getContour(uint8_t index) { + std::vector> result; + cv::Mat mask = getCvMaskByIndex(index); + if(mask.empty()) { + return result; + } + cv::Mat maskCopy = mask.clone(); + std::vector> contours; + + cv::findContours(maskCopy, contours, cv::RetrievalModes::RETR_EXTERNAL, cv::ContourApproximationModes::CHAIN_APPROX_SIMPLE); + for(const auto& contour : contours) { + std::vector daiContour; + for(const auto& point : contour) { + daiContour.emplace_back(static_cast(point.x), static_cast(point.y), false); + } + result.emplace_back(std::move(daiContour)); + } + + return result; +} + +std::vector SegmentationMask::getBoundingBoxes(uint8_t index, bool calculateRotation) { + std::vector boxes; + cv::Mat mask = getCvMaskByIndex(index); + if(mask.empty()) { + return {}; + } + + cv::Mat maskCopy = mask.clone(); + std::vector> contours; + cv::findContours(maskCopy, contours, cv::RetrievalModes::RETR_EXTERNAL, cv::ContourApproximationModes::CHAIN_APPROX_SIMPLE); + if(contours.empty()) { + return {}; + } + const float widthF = static_cast(width); + const float heightF = static_cast(height); + + for(const auto& contour : contours) { + dai::RotatedRect box; + if(calculateRotation) { + cv::RotatedRect cvBox = cv::minAreaRect(contour); + box = {dai::Point2f(cvBox.center.x / widthF, cvBox.center.y / heightF, true), + dai::Size2f(cvBox.size.width / widthF, cvBox.size.height / heightF, true), + cvBox.angle}; + } else { + cv::Rect boundingRect = cv::boundingRect(contour); + if(boundingRect.width == 0 || boundingRect.height == 0) { + continue; + } + box = {dai::Point2f((boundingRect.x + boundingRect.width / 2.0f) / widthF, (boundingRect.y + boundingRect.height / 2.0f) / heightF, true), + dai::Size2f(boundingRect.width / widthF, boundingRect.height / heightF, true), + 0.0f}; + } + + boxes.push_back(box); + } + + return boxes; +} + +#endif + +void SegmentationMask::serialize(std::vector& metadata, DatatypeEnum& datatype) const { + metadata = utility::serialize(*this); + datatype = this->getDatatype(); +} + +#ifdef DEPTHAI_ENABLE_PROTOBUF +ProtoSerializable::SchemaPair SegmentationMask::serializeSchema() const { + return utility::serializeSchema(utility::getProtoMessage(this)); +} + +std::vector SegmentationMask::serializeProto(bool) const { + return utility::serializeProto(utility::getProtoMessage(this)); +} +#endif + +} // namespace dai diff --git a/src/pipeline/datatype/SegmentationParserConfig.cpp b/src/pipeline/datatype/SegmentationParserConfig.cpp new file mode 100644 index 0000000000..94f1ee580f --- /dev/null +++ b/src/pipeline/datatype/SegmentationParserConfig.cpp @@ -0,0 +1,28 @@ +#include "depthai/pipeline/datatype/SegmentationParserConfig.hpp" + +namespace dai { + +SegmentationParserConfig::~SegmentationParserConfig() = default; + +void SegmentationParserConfig::serialize(std::vector& metadata, DatatypeEnum& datatype) const { + metadata = utility::serialize(*this); + datatype = DatatypeEnum::SegmentationParserConfig; +} + +void SegmentationParserConfig::setConfidenceThreshold(const float threshold) { + this->confidenceThreshold = threshold; +} + +float SegmentationParserConfig::getConfidenceThreshold() const { + return confidenceThreshold; +} + +void SegmentationParserConfig::setStepSize(unsigned int stepSize) { + this->stepSize = stepSize; +} + +unsigned int SegmentationParserConfig::getStepSize() const { + return stepSize; +} + +} // namespace dai diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index a53a915438..746dc97bca 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -42,6 +42,8 @@ #include "depthai/pipeline/datatype/PointCloudConfig.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/datatype/RGBDData.hpp" +#include "depthai/pipeline/datatype/SegmentationMask.hpp" +#include "depthai/pipeline/datatype/SegmentationParserConfig.hpp" #include "depthai/pipeline/datatype/SpatialImgDetections.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp" @@ -167,6 +169,10 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; + case DatatypeEnum::SegmentationMask: + return parseDatatype(metadataStart, serializedObjectSize, data, fd); + break; + case DatatypeEnum::EncodedFrame: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; @@ -215,6 +221,10 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; + case DatatypeEnum::SegmentationParserConfig: + return parseDatatype(metadataStart, serializedObjectSize, data, fd); + break; + case DatatypeEnum::AprilTags: return parseDatatype(metadataStart, serializedObjectSize, data, fd); break; diff --git a/src/pipeline/node/SegmentationParser.cpp b/src/pipeline/node/SegmentationParser.cpp new file mode 100644 index 0000000000..03c7357af7 --- /dev/null +++ b/src/pipeline/node/SegmentationParser.cpp @@ -0,0 +1,282 @@ +#include "depthai/pipeline/node/SegmentationParser.hpp" + +#include + +#include +#include +#include +#include +#include + +#include "common/TensorInfo.hpp" +#include "depthai/nn_archive/v1/Head.hpp" +#include "nn_archive/NNArchive.hpp" +#include "pipeline/ThreadedNodeImpl.hpp" +#include "pipeline/datatype/NNData.hpp" +#include "pipeline/datatype/SegmentationMask.hpp" +#include "pipeline/datatype/SegmentationParserConfig.hpp" +#include "pipeline/utilities/SegmentationParser/SegmentationParserUtils.hpp" +#include "properties/Properties.hpp" +#include "utility/ErrorMacros.hpp" + +#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT + #include +#endif + +namespace dai { +namespace node { + +SegmentationParser::~SegmentationParser() = default; + +void SegmentationParser::buildInternal() { + auto device = getDevice(); + if(device) { + auto platform = device->getPlatform(); + if(platform == Platform::RVC2) { + setRunOnHost(true); + std::cout << "SegmentationParser: For RVC2 platform, running on host." << std::endl; + } + } +} + +SegmentationParser::Properties& SegmentationParser::getProperties() { + return properties; +} + +NNArchive SegmentationParser::createNNArchive(NNModelDescription& modelDesc) { + // Download model from zoo + if(modelDesc.platform.empty()) { + DAI_CHECK(getDevice() != nullptr, "Device is not set."); + modelDesc.platform = getDevice()->getPlatformAsString(); + } + auto path = getModelFromZoo(modelDesc); + auto modelType = model::readModelType(path); + DAI_CHECK(modelType == model::ModelType::NNARCHIVE, "Model from zoo must be an NNArchive to use the build function"); + auto nnArchive = NNArchive(path); + return nnArchive; +} + +NNArchive SegmentationParser::decodeModel(const Model& model) { + std::optional nnArchive; + + if(const auto* s = std::get_if(&model)) { + NNModelDescription description; + description.model = *s; + nnArchive = createNNArchive(description); + } else if(const auto* desc = std::get_if(&model)) { + NNModelDescription tmpDesc = *desc; + nnArchive = createNNArchive(tmpDesc); + } else if(const auto* archive = std::get_if(&model)) { + nnArchive = *archive; + } + + DAI_CHECK_V(nnArchive.has_value(), "Unsupported model type passed to SegmentationParser::build"); + return *nnArchive; +} + +std::shared_ptr SegmentationParser::build(Node::Output& nnInput, const Model& model) { + auto nnArchive = decodeModel(model); + setConfig(nnArchive.getVersionedConfig()); + nnInput.link(input); + return std::static_pointer_cast(shared_from_this()); +} + +void SegmentationParser::setConfig(const dai::NNArchiveVersionedConfig& config) { + archiveConfig = config; + + DAI_CHECK_V(config.getVersion() == NNArchiveConfigVersion::V1, "Only NNArchive config V1 is supported."); + const auto& configV1 = config.getConfig(); + DAI_CHECK(configV1.model.heads, "Heads array is not defined in the NN Archive config file."); + + int segmentationHeads = 0; + auto segHead = dai::nn_archive::v1::Head{}; + for(const auto& head : *configV1.model.heads) { + if(head.parser == "SegmentationParser") { + segmentationHeads++; + segHead = head; + } + } + + DAI_CHECK_V(segmentationHeads > 0, "NNArchive does not contain a segmentation head."); + DAI_CHECK_V(segmentationHeads == 1, "NNArchive contains " + std::to_string(segmentationHeads) + " segmentation heads. Please build with a specific head."); + + setConfig(segHead); +} + +std::shared_ptr SegmentationParser::build(Node::Output& nnInput, const dai::nn_archive::v1::Head& head) { + setConfig(head); + nnInput.link(input); + return std::static_pointer_cast(shared_from_this()); +} + +void SegmentationParser::setConfig(const dai::nn_archive::v1::Head& head) { + DAI_CHECK_V(head.parser == "SegmentationParser", "The provided head is not a SegmentationParser head."); + + if(head.outputs->empty()) { + properties.networkOutputName = ""; + } else { + std::vector networkOutputs = *head.outputs; + DAI_CHECK_V(networkOutputs.size() <= 1, "SegmentationParser supports only a single output."); + properties.networkOutputName = networkOutputs[0]; + } + + if(head.metadata.extraParams.contains("classes_in_one_layer")) { + properties.classesInOneLayer = head.metadata.extraParams.at("classes_in_one_layer").get(); + } + + if(head.metadata.extraParams.contains("background_class")) { + properties.backgroundClass = head.metadata.extraParams.at("background_class").get(); + } + + if(head.metadata.classes) { + properties.labels = *head.metadata.classes; + } + + if(head.metadata.confThreshold) { + initialConfig->setConfidenceThreshold(static_cast(*head.metadata.confThreshold)); + } +} + +void SegmentationParser::setLabels(const std::vector& labels) { + properties.labels = labels; +} + +std::vector SegmentationParser::getLabels() const { + return properties.labels; +} + +void SegmentationParser::setBackgroundClass(bool backgroundClass) { + properties.backgroundClass = backgroundClass; +} + +bool SegmentationParser::getBackgroundClass() const { + return properties.backgroundClass; +} + +void SegmentationParser::setRunOnHost(bool runOnHost) { + runOnHostVar = runOnHost; +} + +bool SegmentationParser::runOnHost() const { + return runOnHostVar; +} + +std::string checkTensorName(const dai::NNData& nnData, const std::string& preferredName, std::shared_ptr& logger) { + const auto layerNames = nnData.getAllLayerNames(); + DAI_CHECK_V(!layerNames.empty(), "No tensors available in NNData."); + + if(preferredName != "") { + auto it = std::find(layerNames.begin(), layerNames.end(), preferredName); + DAI_CHECK_V(it != layerNames.end(), "Preferred Segmentation tensor name '" + preferredName + "' not found in NNData outputs."); + return preferredName; + } + + logger->debug("No network outputs specified, using first output only."); + return layerNames.front(); +} + +void SegmentationParser::validateTensor(std::optional& info) { + DAI_CHECK_V(info, "Tensor info for output layer is null."); + + auto maskWidth = static_cast(info->getWidth()); + auto maskHeight = static_cast(info->getHeight()); + int channels = info->getChannels(); + + DAI_CHECK_V(maskWidth > 0 && maskHeight > 0 && channels > 0, + "Invalid tensor dimensions retrieved for segmentation. Channels: " + std::to_string(channels) + ", height: " + std::to_string(maskHeight) + + ", width: " + std::to_string(maskWidth) + "."); + DAI_CHECK(channels <= 256, "SegmentationParser supports a maximum of 256 channels."); + + if(!properties.classesInOneLayer && properties.labels.size() > 0) { + int expectedNumLabels = properties.labels.size(); + DAI_CHECK_V(expectedNumLabels == channels, + fmt::format("Number of provided labels ({}) does not match number of channels ({}).{}", + expectedNumLabels, + channels, + properties.backgroundClass + ? " Note: background_class is set to true, make sure to add a background label to the beginning of the labels list." + : "")); + } +} + +void SegmentationParser::run() { + auto& logger = ThreadedNode::pimpl->logger; + using std::chrono::duration_cast; + using std::chrono::microseconds; + using std::chrono::steady_clock; + logger->debug("Start SegmentationParser"); + + const bool inputConfigSync = inputConfig.getWaitForMessage(); + const bool classesInSingleLayer = properties.classesInOneLayer; + std::string preferredTensorName = properties.networkOutputName; + if(!inConfig) { + inConfig = initialConfig; + } + DAI_CHECK_V(inConfig, "SegmentationParser config is not initialized."); + + while(mainLoop()) { + auto tAbsoluteBeginning = steady_clock::now(); + std::shared_ptr sharedNNData; + { + auto blockEvent = this->inputBlockEvent(); + auto cfg = inputConfigSync ? inputConfig.get() : inputConfig.tryGet(); + if(cfg) { + inConfig = cfg; + } else if(inputConfigSync) { + logger->error("Invalid input config."); + } + + sharedNNData = input.get(); + if(!sharedNNData) { + logger->error("NN Data is empty. Skipping processing."); + continue; + } + } + auto tAfterMessageBeginning = steady_clock::now(); + + std::string networkOutputName = checkTensorName(*sharedNNData, preferredTensorName, logger); + auto tensorInfo = sharedNNData->getTensorInfo(networkOutputName); + validateTensor(tensorInfo); + + if(properties.backgroundClass && tensorInfo->getChannels() <= 1) { + logger->warn("backgroundClass is set to true, but the output tensor has {} channel. Ignoring backgroundClass setting.", tensorInfo->getChannels()); + properties.backgroundClass = false; + } + + auto outMask = std::make_shared(); + if(!classesInSingleLayer) { + utilities::SegmentationParserUtils::computeSegmentationMask(*outMask, *sharedNNData, *tensorInfo, *inConfig, properties.backgroundClass, logger); + } else { + // assume data is stored as INT in shape N x H x W with N = 1 + DAI_CHECK_V(tensorInfo->dataType == dai::TensorInfo::DataType::INT, "When classes_in_one_layer is true, only INT data type is supported."); + utilities::SegmentationParserUtils::copySingleLayerMaskData(*outMask, *sharedNNData, *tensorInfo, *inConfig, logger); + } + + auto tBeforeSend = steady_clock::now(); + if(properties.labels.size() > 0) { + outMask->setLabels(properties.labels); + } + outMask->setSequenceNum(sharedNNData->getSequenceNum()); + outMask->setTimestamp(sharedNNData->getTimestamp()); + outMask->setTimestampDevice(sharedNNData->getTimestampDevice()); + outMask->transformation = sharedNNData->transformation; + const float invStep = 1.0f / static_cast(inConfig->stepSize); + outMask->transformation->addScale(invStep, invStep); + outMask->transformation->setSize(outMask->getWidth(), outMask->getHeight()); + + { + auto blockEvent = this->outputBlockEvent(); + out.send(outMask); + } + + auto tAbsoluteEnd = steady_clock::now(); + logger->trace("Seg parser {}ms, processing {}ms, getting_frames {}ms, sending_frames {}ms", + duration_cast(tAbsoluteEnd - tAbsoluteBeginning).count() / 1000, + duration_cast(tBeforeSend - tAfterMessageBeginning).count() / 1000, + duration_cast(tAfterMessageBeginning - tAbsoluteBeginning).count() / 1000, + duration_cast(tAbsoluteEnd - tBeforeSend).count() / 1000); + } +} + +} // namespace node +} // namespace dai diff --git a/src/pipeline/node/host/Replay.cpp b/src/pipeline/node/host/Replay.cpp index 9f0ddc61cc..498c0af0e3 100644 --- a/src/pipeline/node/host/Replay.cpp +++ b/src/pipeline/node/host/Replay.cpp @@ -73,6 +73,7 @@ inline std::shared_ptr getMessage(const std::shared_ptr getMessage(const std::shared_ptr getProtoMessage(utility::ByteP case DatatypeEnum::SystemInformation: case DatatypeEnum::SystemInformationRVC4: case DatatypeEnum::SpatialLocationCalculatorConfig: + case DatatypeEnum::SegmentationParserConfig: case DatatypeEnum::SpatialLocationCalculatorData: case DatatypeEnum::EdgeDetectorConfig: case DatatypeEnum::GateControl: @@ -180,6 +183,7 @@ inline std::shared_ptr getProtoMessage(utility::ByteP case DatatypeEnum::PipelineState: case DatatypeEnum::PipelineEventAggregationConfig: case DatatypeEnum::NeuralDepthConfig: + case DatatypeEnum::SegmentationMask: case DatatypeEnum::VppConfig: case DatatypeEnum::PacketizedData: throw std::runtime_error("Cannot replay message type: " + std::to_string((int)datatype)); diff --git a/src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.cpp b/src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.cpp new file mode 100644 index 0000000000..4386445951 --- /dev/null +++ b/src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.cpp @@ -0,0 +1,412 @@ +#include "SegmentationParserUtils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/common/TensorInfo.hpp" +#include "depthai/pipeline/datatype/NNData.hpp" +#include "pipeline/datatype/SegmentationParserConfig.hpp" + +namespace dai { +namespace utilities { +namespace SegmentationParserUtils { + +constexpr uint8_t BACKGROUND_INDEX = 255; + +template +inline T loadT(const uint8_t* p) noexcept { + static_assert(std::is_trivially_copyable_v, "Segmentation type must be trivially copyable"); + T v; + std::memcpy(&v, p, sizeof(T)); + return v; +} + +template +T getQuantizedThreshold(const dai::SegmentationParserConfig& config, const dai::TensorInfo& tensorInfo) { + if(config.confidenceThreshold < 0) { + // Minimum possible value for int8 and uint8 quantizations + return tensorInfo.dataType == dai::TensorInfo::DataType::I8 ? static_cast(-128) : static_cast(0); + } + return static_cast((config.confidenceThreshold / tensorInfo.qpScale) + tensorInfo.qpZp); +} + +/* + * @brief Optimized argmax function for NHWC tensors with aligned and typed channel strides. + * Data looks like [h1w1c1, h1w1c2, ..., h1w1cC, h1w2c1, h1w2c2, ..., h1w2cC, ..., hHwWcC] + */ +// Vectorized implementation if performance is needed. +// template +// void argmaxNHWCVectorizedTensor(span dst, +// const span tensorBase, +// const dai::TensorInfo& tensorInfo, +// const dai::SegmentationParserConfig& config) { +// const int channels = tensorInfo.getChannels(); +// const int width = tensorInfo.getWidth(); +// const int height = tensorInfo.getHeight(); + +// const size_t strideC = tensorInfo.getChannelStride() / sizeof(T); +// const size_t strideH = tensorInfo.getHeightStride() / sizeof(T); +// const size_t strideW = tensorInfo.getWidthStride() / sizeof(T); +// const int step = static_cast(config.stepSize); +// const int dstWidth = width / step; + +// const T* tensorPtr = castPtr(tensorBase.data()); + +// int outY = 0; +// uint8_t bestClass = 255; +// T quantizedThreshold = getQuantizedThreshold(config, tensorInfo); +// T bestVal = quantizedThreshold; +// const int cStart = backgroundClass ? 1 : 0; + +// for(int h = 0; h < height; h += step, ++outY) { +// const size_t rowBase = strideH * static_cast(h); +// const int rowOffset = outY * dstWidth; + +// int outX = 0; +// for(int w = 0; w < width; w += step, ++outX) { +// const size_t pixelBase = rowBase + (strideW * static_cast(w)); + +// bestClass = 255; +// if constexpr(backgroundClass) { +// bestVal = std::max(tensorPtr[pixelBase], quantizedThreshold); +// } else { +// bestVal = quantizedThreshold; +// } +// for(int c = cStart; c < channels; ++c) { +// const size_t idx = pixelBase + (strideC * static_cast(c)); +// T v = tensorPtr[idx]; +// if(v > bestVal) { +// bestVal = v; +// bestClass = static_cast(c); +// } +// } +// dst[rowOffset + outX] = bestClass; +// } +// } +// } + +/* + * @brief Parse NHWC formatted tensor to compute segmentation mask. + * Data looks like [h1w1c1, h1w1c2, ..., h1w1cC, h1w2c1, h1w2c2, ..., h1w2cC, ..., hHwWcC] + */ +template +void parseNHWCTensor(span dst, const span tensorBase, const dai::TensorInfo& tensorInfo, const dai::SegmentationParserConfig& config) { + const uint8_t* tensorPtr = tensorBase.data(); + const int channels = tensorInfo.getChannels(); + const int width = tensorInfo.getWidth(); + const int height = tensorInfo.getHeight(); + + const size_t strideC = tensorInfo.getChannelStride(); + const size_t strideH = tensorInfo.getHeightStride(); + const size_t strideW = tensorInfo.getWidthStride(); + const int step = static_cast(config.stepSize); + const int dstWidth = width / step; + + int outY = 0; + uint8_t bestClass = 255; + T quantizedThreshold = getQuantizedThreshold(config, tensorInfo); + T bestVal = quantizedThreshold; + const int cStart = backgroundClass ? 1 : 0; + + for(int h = 0; h < height; h += step, ++outY) { + const size_t rowBase = strideH * static_cast(h); + const int rowOffset = outY * dstWidth; + int outX = 0; + + for(int w = 0; w < width; w += step, ++outX) { + const size_t pixelBase = rowBase + (strideW * static_cast(w)); + bestClass = 255; + if constexpr(backgroundClass) { + bestVal = std::max(loadT(tensorPtr + pixelBase), quantizedThreshold); + } else { + bestVal = quantizedThreshold; + } + + for(int c = cStart; c < channels; ++c) { + const size_t idx = pixelBase + (strideC * static_cast(c)); + T v = loadT(tensorPtr + idx); + if(v > bestVal) { + bestVal = v; + bestClass = static_cast(c); + } + } + dst[rowOffset + outX] = bestClass; + } + } +} + +template +void fillBestVals(std::vector& bestValues, const uint8_t* tensorPtr, const dai::TensorInfo& tensorInfo, const dai::SegmentationParserConfig& config) { + const int width = tensorInfo.getWidth(); + const int height = tensorInfo.getHeight(); + + const size_t strideH = tensorInfo.getHeightStride(); + const size_t strideW = tensorInfo.getWidthStride(); + const int step = static_cast(config.stepSize); + const int dstWidth = width / step; + + const T quantizedThreshold = getQuantizedThreshold(config, tensorInfo); + + if constexpr(backgroundClass) { + int outY = 0; + for(int h = 0; h < height; h += step, ++outY) { + const size_t rowBase = (strideH * static_cast(h)); + int outX = 0; + for(int w = 0; w < width; w += step, ++outX) { + const size_t idx = rowBase + (strideW * static_cast(w)); + const T v0 = loadT(tensorPtr + idx); + bestValues[(outY * dstWidth) + outX] = std::max(v0, quantizedThreshold); + } + } + } else { + std::fill(bestValues.begin(), bestValues.end(), quantizedThreshold); + } +} + +/* + * @brief Optimized argmax function over NCHW formatted tensor. + * Data looks like [h1w1c1, h1w2c1, ..., hMwNc1, h1w1c2, h1w2c2, ..., hMwNcC] + */ + +template +void parseNCHWTensor(span dst, const span tensorBase, const dai::TensorInfo& tensorInfo, const dai::SegmentationParserConfig& config) { + const int channels = tensorInfo.getChannels(); + const int width = tensorInfo.getWidth(); + const int height = tensorInfo.getHeight(); + + const size_t strideC = tensorInfo.getChannelStride(); + const size_t strideH = tensorInfo.getHeightStride(); + const size_t strideW = tensorInfo.getWidthStride(); + const int step = static_cast(config.stepSize); + const int dstWidth = width / step; + + static thread_local std::vector bestValues; + bestValues.resize(dst.size()); + const uint8_t* tensorPtr = tensorBase.data(); + fillBestVals(bestValues, tensorPtr, tensorInfo, config); + + const int cStart = backgroundClass ? 1 : 0; + for(int c = cStart; c < channels; c++) { + const size_t channelBase = strideC * static_cast(c); + int outY = 0; + for(int h = 0; h < height; h += step, ++outY) { + const size_t rowBase = channelBase + (strideH * static_cast(h)); + int outX = 0; + for(int w = 0; w < width; w += step, ++outX) { + const size_t idx = rowBase + (strideW * static_cast(w)); + T v = loadT(tensorPtr + idx); + int coordinateIndex = (outY * dstWidth) + outX; + if(v > bestValues[coordinateIndex]) { + bestValues[coordinateIndex] = v; + dst[coordinateIndex] = static_cast(c); + } + } + } + } +} + +template +void tensorArgmax(span dstData, + const span tensorSpan, + const dai::TensorInfo& tensorInfo, + const dai::SegmentationParserConfig& config, + std::shared_ptr& logger) { + /* + * Should be moved to GPU. + */ + const bool isNHWC = (tensorInfo.order == dai::TensorInfo::StorageOrder::NHWC || tensorInfo.order == dai::TensorInfo::StorageOrder::HWC); + if(isNHWC) { + logger->trace("Using NHWC segmentation mask parsing."); + parseNHWCTensor(dstData, tensorSpan, tensorInfo, config); + } else { + logger->trace("Defaulting to NCHW/CHW segmentation mask parsing."); + parseNCHWTensor(dstData, tensorSpan, tensorInfo, config); + } +} + +template +void fillFP16BestVals(std::vector& bestValues, + const uint8_t* tensorPtr, + const dai::TensorInfo& tensorInfo, + const dai::SegmentationParserConfig& config) { + const int width = tensorInfo.getWidth(); + const int height = tensorInfo.getHeight(); + + const size_t strideH = tensorInfo.getHeightStride(); + const size_t strideW = tensorInfo.getWidthStride(); + const int step = static_cast(config.stepSize); + const int dstWidth = width / step; + + const float thr = (config.confidenceThreshold < 0.0f) ? -std::numeric_limits::infinity() : config.confidenceThreshold; + + if constexpr(backgroundClass) { + int outY = 0; + for(int h = 0; h < height; h += step, ++outY) { + const size_t rowBase = (strideH * static_cast(h)); + int outX = 0; + for(int w = 0; w < width; w += step, ++outX) { + const size_t idx = rowBase + (strideW * static_cast(w)); + float v = fp16_ieee_to_fp32_value(loadT(tensorPtr + idx)); + bestValues[(outY * dstWidth) + outX] = std::max(v, thr); + } + } + } else { + std::fill(bestValues.begin(), bestValues.end(), thr); + } +} + +template +void tensorArgmaxFP16(span dst, + const span tensorSpan, + const dai::TensorInfo& tensorInfo, + const dai::SegmentationParserConfig& config, + std::shared_ptr& logger) { + logger->trace("Using FP16 NCHW segmentation mask parsing."); + const uint8_t* tensorBase = tensorSpan.data(); + const int channels = tensorInfo.getChannels(); + const int width = tensorInfo.getWidth(); + const int height = tensorInfo.getHeight(); + + const size_t strideC = tensorInfo.getChannelStride(); + const size_t strideH = tensorInfo.getHeightStride(); + const size_t strideW = tensorInfo.getWidthStride(); + const int step = static_cast(config.stepSize); + const int dstWidth = width / step; + + static thread_local std::vector bestVals; + bestVals.resize(dst.size()); + fillFP16BestVals(bestVals, tensorBase, tensorInfo, config); + + const int cStart = backgroundClass ? 1 : 0; + for(int c = cStart; c < channels; c++) { + const size_t channelBase = strideC * static_cast(c); + + int outY = 0; + for(int h = 0; h < height; h += step, ++outY) { + const size_t rowBase = channelBase + (strideH * static_cast(h)); + int outX = 0; + for(int w = 0; w < width; w += step, ++outX) { + const size_t idx = rowBase + (strideW * static_cast(w)); + float v = fp16_ieee_to_fp32_value(loadT(tensorBase + idx)); + int coordinateIndex = (outY * dstWidth) + outX; + if(v > bestVals[coordinateIndex]) { + bestVals[coordinateIndex] = v; + dst[coordinateIndex] = static_cast(c); + } + } + } + } +} + +void computeSegmentationMask(dai::SegmentationMask& outputMask, + const dai::NNData& nnData, + const dai::TensorInfo& tensorInfo, + const dai::SegmentationParserConfig& config, + const bool backgroundClass, + std::shared_ptr& logger) { + span dstData = + outputMask.prepareMask(static_cast(tensorInfo.getWidth() / config.stepSize), static_cast(tensorInfo.getHeight() / config.stepSize)); + std::fill(dstData.begin(), dstData.end(), BACKGROUND_INDEX); + + auto nnMemory = nnData.data; + const span nnBytes = nnMemory->getData(); + const span allTensors{nnBytes.data(), nnBytes.size()}; + const span tensorSpan = allTensors.subspan(tensorInfo.offset, tensorInfo.getTensorSize()); + + switch((tensorInfo).dataType) { + case dai::TensorInfo::DataType::I8: + if(backgroundClass) { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } else { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } + break; + case dai::TensorInfo::DataType::U8F: + if(backgroundClass) { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } else { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } + break; + case dai::TensorInfo::DataType::INT: + if(backgroundClass) { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } else { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } + break; + case dai::TensorInfo::DataType::FP32: + if(backgroundClass) { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } else { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } + break; + case dai::TensorInfo::DataType::FP16: + if(backgroundClass) { + tensorArgmaxFP16(dstData, tensorSpan, tensorInfo, config, logger); + } else { + tensorArgmaxFP16(dstData, tensorSpan, tensorInfo, config, logger); + } + break; + case dai::TensorInfo::DataType::FP64: + if(backgroundClass) { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } else { + tensorArgmax(dstData, tensorSpan, tensorInfo, config, logger); + } + break; + default: + logger->error("Unsupported data type for segmentation parsing: {}", static_cast(tensorInfo.dataType)); + return; + } +} + +void copySingleLayerMaskData(dai::SegmentationMask& outputMask, + dai::NNData& nnData, + dai::TensorInfo& tensorInfo, + dai::SegmentationParserConfig& config, + std::shared_ptr& logger) { + logger->trace("Parsing single layer segmentation mask."); + const size_t stepSize = static_cast(config.stepSize); + const size_t maskWidth = static_cast(tensorInfo.getWidth()); + const size_t maskHeight = static_cast(tensorInfo.getHeight()); + const size_t strideH = tensorInfo.getHeightStride(); + const size_t strideW = tensorInfo.getWidthStride(); + const int dstWidth = static_cast(maskWidth / stepSize); + const int dstHeight = static_cast(maskHeight / stepSize); + span dst = outputMask.prepareMask(dstWidth, dstHeight); + + auto nnMemory = nnData.data; + const auto srcBytes = nnMemory->getData(); + const auto tensorSpan = srcBytes.subspan(tensorInfo.offset, tensorInfo.getTensorSize()); + const uint8_t* tensorBase = tensorSpan.data(); + + int outY = 0; + for(size_t h = 0; h < maskHeight; h += stepSize, ++outY) { + const size_t rowBase = strideH * h; + int outX = 0; + for(size_t w = 0; w < maskWidth; w += stepSize, ++outX) { + const size_t idx = rowBase + (strideW * w); + int32_t v; + std::memcpy(&v, tensorBase + idx, sizeof(v)); + v = std::max(v, 0); + v = std::min(v, 255); + dst[(outY * dstWidth) + outX] = static_cast(v); + } + } +} + +} // namespace SegmentationParserUtils +} // namespace utilities +} // namespace dai diff --git a/src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.hpp b/src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.hpp new file mode 100644 index 0000000000..cd262c6500 --- /dev/null +++ b/src/pipeline/utilities/SegmentationParser/SegmentationParserUtils.hpp @@ -0,0 +1,81 @@ +#pragma once +#include + +#include + +#include "depthai/pipeline/datatype/NNData.hpp" +#include "depthai/pipeline/datatype/SegmentationMask.hpp" +#include "depthai/pipeline/datatype/SegmentationParserConfig.hpp" +#include "depthai/utility/span.hpp" + +namespace dai { +namespace utilities { +namespace SegmentationParserUtils { + +/** + * @brief Compute segmentation mask from NNData tensor using the provided TensorInfo and SegmentationParserConfig. + * @param outputMask SegmentationMask object to store the computed mask. + * @param nnData NNData object containing the tensor data. + * @param tensorInfo TensorInfo object describing the tensor layout and data type. + * @param config SegmentationParserConfig object containing parsing parameters like confidenceThreshold and stepSize. + * @param backgroundClass If true, the first class (index 0) is considered as background and ignored during calculating argmax. + * @param logger Logger + */ +void computeSegmentationMask(dai::SegmentationMask& outputMask, + const dai::NNData& nnData, + const dai::TensorInfo& tensorInfo, + const dai::SegmentationParserConfig& config, + const bool backgroundClass, + std::shared_ptr& logger); + +/** + * @brief get the class index with the highest thresholded confidenceThreshold for each pixel in the tensor. Function is optimized for NHWC and NCHW tensor + * formats. + * @tparam T Data type of the tensor (e.g., float, int8_t, uint8_t, ...) + * @param dstData Output segmentation mask data (uint8_t) + * @param tensorSpan Input tensor data + * @param tensorInfo Information about the tensor (shape, data type, layout, etc.) + * @param config SegmentationParserConfig containing parameters like confidenceThreshold and stepSize + * @param logger Logger + */ +template +void tensorArgmax(span dstData, + span tensorSpan, + dai::TensorInfo& tensorInfo, + dai::SegmentationParserConfig& config, + std::shared_ptr& logger); + +/** + * @brief Specialized function for FP16 data type to avoid template instantiation issues. Optimized for NCHW tensor format, as FP16 is associated with RVC2 + * outputs. + * @param dst Output segmentation mask data (uint8_t) + * @param tensorSpan Input tensor data + * @param tensorInfo Information about the tensor (shape, data type, layout, etc.) + * @param config SegmentationParserConfig containing parameters like confidenceThreshold and stepSize + * @param logger Logger + */ +template +void tensorArgmaxFP16(span dst, + span tensorSpan, + dai::TensorInfo& tensorInfo, + dai::SegmentationParserConfig& config, + std::shared_ptr& logger); + +/** + * @brief Copy segmentation mask from NNData tensor to SegmentationMask object. This is used when the segmentation classes are already processed (argmax) in the + * NN. + * @param outputMask SegmentationMask object to store the copied mask. + * @param nnData NNData object containing the tensor data. + * @param tensorInfo TensorInfo object describing the tensor layout and data type. + * @param config SegmentationParserConfig object (not used in this function but kept for consistency). + * @param logger Logger + */ +void copySingleLayerMaskData(dai::SegmentationMask& outputMask, + dai::NNData& nnData, + dai::TensorInfo& tensorInfo, + dai::SegmentationParserConfig& config, + std::shared_ptr& logger); + +} // namespace SegmentationParserUtils +} // namespace utilities +} // namespace dai diff --git a/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp index 1cb646ac00..ce4d8a8c2c 100644 --- a/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp +++ b/src/pipeline/utilities/SpatialLocationCalculator/SpatialUtils.cpp @@ -107,7 +107,6 @@ float DepthStats::calculateDepth(SpatialLocationCalculatorAlgorithm algo) { currentCount = 1; } } - if(currentCount > maxCount) { modeValue = validPixels.back(); } diff --git a/src/properties/Properties.cpp b/src/properties/Properties.cpp index 6686782d09..391b509819 100644 --- a/src/properties/Properties.cpp +++ b/src/properties/Properties.cpp @@ -25,6 +25,7 @@ #include "depthai/properties/SPIInProperties.hpp" #include "depthai/properties/SPIOutProperties.hpp" #include "depthai/properties/ScriptProperties.hpp" +#include "depthai/properties/SegmentationParserProperties.hpp" #include "depthai/properties/SpatialDetectionNetworkProperties.hpp" #include "depthai/properties/SpatialLocationCalculatorProperties.hpp" #include "depthai/properties/StereoDepthProperties.hpp" @@ -84,6 +85,7 @@ SPIOutProperties::~SPIOutProperties() = default; ScriptProperties::~ScriptProperties() = default; SpatialDetectionNetworkProperties::~SpatialDetectionNetworkProperties() = default; SpatialLocationCalculatorProperties::~SpatialLocationCalculatorProperties() = default; +SegmentationParserProperties::~SegmentationParserProperties() = default; StereoDepthProperties::~StereoDepthProperties() = default; SyncProperties::~SyncProperties() = default; SystemLoggerProperties::~SystemLoggerProperties() = default; diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index 039e942d6e..41f3ae40b0 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -12,10 +12,12 @@ #include "depthai/schemas/PointCloudData.pb.h" #include "depthai/schemas/RGBDData.pb.h" +#include "depthai/schemas/SegmentationMask.pb.h" #include "depthai/schemas/common.pb.h" #include "pipeline/datatype/DatatypeEnum.hpp" #include "pipeline/datatype/ImgDetections.hpp" #include "pipeline/datatype/RGBDData.hpp" +#include "pipeline/datatype/SegmentationMask.hpp" namespace dai { namespace utility { @@ -154,10 +156,12 @@ bool deserializationSupported(DatatypeEnum datatype) { case DatatypeEnum::CameraControl: case DatatypeEnum::GateControl: case DatatypeEnum::ImgDetections: + case DatatypeEnum::SegmentationMask: case DatatypeEnum::SpatialImgDetections: case DatatypeEnum::SystemInformation: case DatatypeEnum::SystemInformationRVC4: case DatatypeEnum::SpatialLocationCalculatorConfig: + case DatatypeEnum::SegmentationParserConfig: case DatatypeEnum::SpatialLocationCalculatorData: case DatatypeEnum::EdgeDetectorConfig: case DatatypeEnum::AprilTagConfig: @@ -631,6 +635,38 @@ std::unique_ptr getProtoMessage(const ImgFrame* messa return imgFrame; } template <> +std::unique_ptr getProtoMessage(const SegmentationMask* message, bool metadataOnly) { + auto segmentationMask = std::make_unique(); + + segmentationMask->set_sequencenum(message->sequenceNum); + + auto timestamp = segmentationMask->mutable_ts(); + timestamp->set_sec(message->ts.sec); + timestamp->set_nsec(message->ts.nsec); + + auto timestampDevice = segmentationMask->mutable_tsdevice(); + timestampDevice->set_sec(message->tsDevice.sec); + timestampDevice->set_nsec(message->tsDevice.nsec); + + segmentationMask->set_width(message->getWidth()); + segmentationMask->set_height(message->getHeight()); + + if(message->transformation.has_value()) { + serializeImgTransformation(segmentationMask->mutable_transformation(), *message->transformation); + } + + const auto labels = message->getLabels(); + for(const auto& label : labels) { + segmentationMask->add_labels(label); + } + + if(!metadataOnly) { + segmentationMask->set_data(message->data->getData().data(), message->data->getSize()); + } + + return segmentationMask; +} +template <> std::unique_ptr getProtoMessage(const PointCloudData* message, bool metadataOnly) { auto pointCloudData = std::make_unique(); diff --git a/src/utility/ProtoSerialize.hpp b/src/utility/ProtoSerialize.hpp index be89e28425..816d2a35c8 100644 --- a/src/utility/ProtoSerialize.hpp +++ b/src/utility/ProtoSerialize.hpp @@ -19,6 +19,7 @@ #include "depthai/schemas/RGBDData.pb.h" #include "depthai/schemas/SpatialImgDetections.pb.h" #include "depthai/schemas/common.pb.h" +#include "pipeline/datatype/SegmentationMask.hpp" #include "utility/ProtoSerializable.hpp" namespace dai { @@ -59,6 +60,8 @@ std::unique_ptr getProtoMessage(const EncodedFrame* m template <> std::unique_ptr getProtoMessage(const ImgFrame* message, bool metadataOnly); template <> +std::unique_ptr getProtoMessage(const SegmentationMask* message, bool metadataOnly); +template <> std::unique_ptr getProtoMessage(const PointCloudData* message, bool metadataOnly); template <> std::unique_ptr getProtoMessage(const RGBDData* message, bool metadataOnly); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e0fb39764a..f5d64893f5 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -627,6 +627,10 @@ if(DEPTHAI_FETCH_ARTIFACTS) dai_set_test_labels(detection_parser_test ondevice rvc4 ci) endif() +# SegmentationParser test +dai_add_test(segmentation_parser_test src/ondevice_tests/pipeline/node/segmentation_parser_test.cpp) +dai_set_test_labels(segmentation_parser_test ondevice rvc4 ci) + # Spatial detection network test dai_add_test(spatial_detection_network_test src/ondevice_tests/pipeline/node/spatial_detection_network_test.cpp) dai_set_test_labels(spatial_detection_network_test ondevice rvc2_all rvc4 ci) diff --git a/tests/src/ondevice_tests/pipeline/node/segmentation_parser_test.cpp b/tests/src/ondevice_tests/pipeline/node/segmentation_parser_test.cpp new file mode 100644 index 0000000000..2186e479d2 --- /dev/null +++ b/tests/src/ondevice_tests/pipeline/node/segmentation_parser_test.cpp @@ -0,0 +1,379 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "depthai/common/ImgTransformations.hpp" +#include "depthai/common/TensorInfo.hpp" +#include "depthai/depthai.hpp" +#include "depthai/pipeline/datatype/NNData.hpp" +#include "depthai/pipeline/datatype/SegmentationMask.hpp" +#include "depthai/pipeline/datatype/SegmentationParserConfig.hpp" +#include "depthai/pipeline/node/SegmentationParser.hpp" + +namespace { + +size_t dataTypeSize(dai::TensorInfo::DataType type) { + switch(type) { + case dai::TensorInfo::DataType::FP16: + return sizeof(uint16_t); + case dai::TensorInfo::DataType::U8F: + return sizeof(uint8_t); + case dai::TensorInfo::DataType::INT: + return sizeof(int32_t); + case dai::TensorInfo::DataType::FP32: + return sizeof(float); + case dai::TensorInfo::DataType::I8: + return sizeof(int8_t); + case dai::TensorInfo::DataType::FP64: + return sizeof(double); + default: + return 0U; + } +} + +dai::TensorInfo makeNCHWTensorInfo( + const std::string& name, dai::TensorInfo::DataType type, int channels, int height, int width, size_t elementSizeOverride = 0U) { + dai::TensorInfo info; + info.name = name; + info.order = dai::TensorInfo::StorageOrder::NCHW; + info.dataType = type; + info.numDimensions = 4; + info.dims = {1U, static_cast(channels), static_cast(height), static_cast(width)}; + + const size_t elementSize = elementSizeOverride ? elementSizeOverride : dataTypeSize(type); + const size_t strideW = elementSize; + const size_t strideH = strideW * static_cast(width); + const size_t strideC = strideH * static_cast(height); + const size_t strideN = strideC * static_cast(channels); + + info.strides = {static_cast(strideN), static_cast(strideC), static_cast(strideH), static_cast(strideW)}; + return info; +} + +dai::TensorInfo makeNHWCTensorInfo(const std::string& name, dai::TensorInfo::DataType type, int height, int width, int channels, bool vectorizable) { + dai::TensorInfo info; + info.name = name; + info.order = dai::TensorInfo::StorageOrder::NHWC; + info.dataType = type; + info.numDimensions = 4; + info.dims = {1U, static_cast(height), static_cast(width), static_cast(channels)}; + + const size_t elementSize = dataTypeSize(type); + const size_t strideC = elementSize; + const size_t strideW = (vectorizable ? static_cast(channels) : static_cast(channels + 1)) * elementSize; + const size_t strideH = strideW * static_cast(width); + const size_t strideN = strideH * static_cast(height); + + info.strides = {static_cast(strideN), static_cast(strideH), static_cast(strideW), static_cast(strideC)}; + return info; +} + +size_t tensorOffset(const dai::TensorInfo& info, int n, int c, int h, int w) { + switch(info.order) { + case dai::TensorInfo::StorageOrder::NCHW: + return static_cast(n) * info.strides[0] + static_cast(c) * info.strides[1] + static_cast(h) * info.strides[2] + + static_cast(w) * info.strides[3]; + case dai::TensorInfo::StorageOrder::NHWC: + return static_cast(n) * info.strides[0] + static_cast(h) * info.strides[1] + static_cast(w) * info.strides[2] + + static_cast(c) * info.strides[3]; + default: + return 0U; + } +} + +template +std::shared_ptr createSyntheticNNData(const dai::TensorInfo& templateInfo, ValueFn valueFn) { + auto nnData = std::make_shared(); + dai::TensorInfo info = templateInfo; + auto tensorBytes = nnData->emplaceTensor(info); + std::fill(tensorBytes.begin(), tensorBytes.end(), 0); + + const int width = info.getWidth(); + const int height = info.getHeight(); + const int channels = info.getChannels(); + + nnData->transformation = dai::ImgTransformation(static_cast(width), static_cast(height)); + + for(int h = 0; h < height; ++h) { + for(int w = 0; w < width; ++w) { + for(int c = 0; c < channels; ++c) { + T value = valueFn(c, h, w); + const size_t offset = tensorOffset(info, 0, c, h, w); + REQUIRE(offset + sizeof(T) <= tensorBytes.size()); + std::memcpy(tensorBytes.data() + offset, &value, sizeof(T)); + } + } + } + + nnData->setSequenceNum(0); + nnData->setTimestamp(std::chrono::steady_clock::now()); + return nnData; +} + +template +std::vector computeExpectedMask(int width, int height, int channels, float threshold, bool backgroundClass, ValueFn valueFn, int stepSize = 1) { + const int outWidth = width / stepSize; + const int outHeight = height / stepSize; + std::vector mask(static_cast(outWidth) * static_cast(outHeight), 255); + const int channelStart = backgroundClass ? 1 : 0; + + int outY = 0; + for(int h = 0; h < height; h += stepSize, ++outY) { + int outX = 0; + for(int w = 0; w < width; w += stepSize, ++outX) { + float bestVal = threshold; + uint8_t bestClass = 255; + if(backgroundClass) { + bestVal = std::max(bestVal, valueFn(0, h, w)); + } + for(int c = channelStart; c < channels; ++c) { + float v = valueFn(c, h, w); + if(v > bestVal) { + bestVal = v; + bestClass = static_cast(c); + } + } + mask[static_cast(outY) * static_cast(outWidth) + static_cast(outX)] = bestClass; + } + } + + return mask; +} + +void checkMaskMatches(const dai::SegmentationMask& mask, const std::vector& expected, size_t width, size_t height) { + REQUIRE(mask.getWidth() == width); + REQUIRE(mask.getHeight() == height); + const auto data = mask.getMaskData(); + REQUIRE(!data.empty()); + REQUIRE(data.size() == expected.size()); + + for(size_t i = 0; i < expected.size(); ++i) { + const size_t row = i / width; + const size_t col = i % width; + const int actual = static_cast(data[i]); + const int expectedValue = static_cast(expected[i]); + CAPTURE(i, row, col, actual, expectedValue); + CHECK(actual == expectedValue); + } +} + +std::vector> processSegmentationFrames( + const dai::SegmentationParserConfig& initialConfig, + const std::vector>& frames, + bool classesInOneLayer, + bool backgroundClass, + const std::vector>& runtimeConfigs = {}, + bool syncConfig = false) { + dai::Pipeline pipeline; + auto parser = pipeline.create(); + parser->setRunOnHost(true); + parser->initialConfig = std::make_shared(initialConfig); + parser->properties.classesInOneLayer = classesInOneLayer; + parser->setBackgroundClass(backgroundClass); + if(syncConfig) { + parser->inputConfig.setWaitForMessage(true); + } + + auto inputQueue = parser->input.createInputQueue(); + auto configQueue = parser->inputConfig.createInputQueue(); + auto outputQueue = parser->out.createOutputQueue(); + + pipeline.start(); + + std::vector> outputs; + outputs.reserve(frames.size()); + + for(size_t i = 0; i < frames.size(); ++i) { + if(syncConfig) { + const auto& cfg = (i < runtimeConfigs.size() && runtimeConfigs[i].has_value()) ? *runtimeConfigs[i] : initialConfig; + configQueue->send(std::make_shared(cfg)); + } else if(i < runtimeConfigs.size() && runtimeConfigs[i].has_value()) { + configQueue->send(std::make_shared(*runtimeConfigs[i])); + } + inputQueue->send(frames[i]); + auto mask = outputQueue->get(); + REQUIRE(mask != nullptr); + outputs.push_back(mask); + } + + pipeline.stop(); + pipeline.wait(); + return outputs; +} + +std::vector makeArgmaxTestValues() { + return { + 0.9f, + 0.2f, + 0.1f, // (0,0) + 0.1f, + 0.8f, + 0.3f, // (0,1) + 0.2f, + 0.4f, + 0.7f, // (0,2) + 0.3f, + 0.6f, + 0.4f, // (1,0) + 0.2f, + 0.35f, + 0.45f, // (1,1) + 0.1f, + 0.2f, + 0.3f // (1,2) + }; +} + +} // namespace + +TEST_CASE("SegmentationParser argmax backends produce expected mask") { + constexpr int kWidth = 3; + constexpr int kHeight = 2; + constexpr int kChannels = 3; + + const auto values = makeArgmaxTestValues(); + auto valueFn = [&](int c, int h, int w) { return values[static_cast(h * kWidth + w) * static_cast(kChannels) + static_cast(c)]; }; + + dai::SegmentationParserConfig config; + config.setConfidenceThreshold(0.5f); + config.setStepSize(1); + + const auto expected = computeExpectedMask(kWidth, kHeight, kChannels, 0.5f, false, valueFn, static_cast(config.getStepSize())); + const size_t maskWidth = static_cast(kWidth) / config.getStepSize(); + const size_t maskHeight = static_cast(kHeight) / config.getStepSize(); + + SECTION("NCHW") { + auto info = makeNCHWTensorInfo("seg", dai::TensorInfo::DataType::FP32, kChannels, kHeight, kWidth); + auto nnData = createSyntheticNNData(info, valueFn); + auto outputs = processSegmentationFrames(config, {nnData}, false, false); + checkMaskMatches(*outputs.front(), expected, maskWidth, maskHeight); + } + + SECTION("NHWC vectorized") { + auto info = makeNHWCTensorInfo("seg", dai::TensorInfo::DataType::FP32, kHeight, kWidth, kChannels, true); + auto nnData = createSyntheticNNData(info, valueFn); + auto outputs = processSegmentationFrames(config, {nnData}, false, false); + checkMaskMatches(*outputs.front(), expected, maskWidth, maskHeight); + } + + SECTION("NHWC safe") { + auto info = makeNHWCTensorInfo("seg", dai::TensorInfo::DataType::FP32, kHeight, kWidth, kChannels, false); + auto nnData = createSyntheticNNData(info, valueFn); + auto outputs = processSegmentationFrames(config, {nnData}, false, false); + checkMaskMatches(*outputs.front(), expected, maskWidth, maskHeight); + } + + SECTION("FP16 argmax") { + auto info = makeNCHWTensorInfo("seg", dai::TensorInfo::DataType::FP16, kChannels, kHeight, kWidth); + auto fp16ValueFn = [&](int c, int h, int w) { return fp16_ieee_from_fp32_value(valueFn(c, h, w)); }; + auto nnData = createSyntheticNNData(info, fp16ValueFn); + auto roundedValueFn = [&](int c, int h, int w) { return fp16_ieee_to_fp32_value(fp16_ieee_from_fp32_value(valueFn(c, h, w))); }; + const auto expectedFp16 = computeExpectedMask(kWidth, kHeight, kChannels, 0.5f, false, roundedValueFn, static_cast(config.getStepSize())); + auto outputs = processSegmentationFrames(config, {nnData}, false, false); + checkMaskMatches(*outputs.front(), expectedFp16, maskWidth, maskHeight); + } + + SECTION("INT8 argmax") { + auto info = makeNCHWTensorInfo("seg", dai::TensorInfo::DataType::I8, kChannels, kHeight, kWidth); + info.qpScale = 1.0f; + info.qpZp = 0; + auto i8ValueFn = [&](int c, int h, int w) { + float v = valueFn(c, h, w) * 100.0f; + return static_cast(std::clamp(v, -128.0f, 127.0f)); + }; + auto nnData = createSyntheticNNData(info, i8ValueFn); + auto expectedValueFn = [&](int c, int h, int w) { return static_cast(i8ValueFn(c, h, w)); }; + const auto expectedI8 = computeExpectedMask(kWidth, kHeight, kChannels, 0.5f, false, expectedValueFn, static_cast(config.getStepSize())); + auto outputs = processSegmentationFrames(config, {nnData}, false, false); + checkMaskMatches(*outputs.front(), expectedI8, maskWidth, maskHeight); + } +} + +TEST_CASE("SegmentationParser background class ignores channel 0") { + constexpr int kWidth = 2; + constexpr int kHeight = 1; + constexpr int kChannels = 3; + + const std::vector values = { + 0.95f, + 0.8f, + 0.1f, // (0,0) + 0.2f, + 0.1f, + 0.9f // (0,1) + }; + + auto valueFn = [&](int c, int h, int w) { return values[static_cast(h * kWidth + w) * static_cast(kChannels) + static_cast(c)]; }; + + dai::SegmentationParserConfig config; + config.setConfidenceThreshold(0.3f); + config.setStepSize(1); + + auto info = makeNCHWTensorInfo("seg", dai::TensorInfo::DataType::FP32, kChannels, kHeight, kWidth); + auto nnData = createSyntheticNNData(info, valueFn); + + const auto expected = computeExpectedMask(kWidth, kHeight, kChannels, 0.3f, true, valueFn, static_cast(config.getStepSize())); + auto outputs = processSegmentationFrames(config, {nnData}, false, true); + const size_t maskWidth = static_cast(kWidth) / config.getStepSize(); + const size_t maskHeight = static_cast(kHeight) / config.getStepSize(); + checkMaskMatches(*outputs.front(), expected, maskWidth, maskHeight); +} + +TEST_CASE("SegmentationParser classes-in-one-layer passes through mask") { + constexpr int kWidth = 3; + constexpr int kHeight = 2; + + const std::vector expected = {0, 1, 2, 255, 1, 0}; + auto valueFn = [&](int, int h, int w) { return static_cast(expected[static_cast(h * kWidth + w)]); }; + + dai::SegmentationParserConfig config; + config.setStepSize(1); + + auto info = makeNCHWTensorInfo("seg", dai::TensorInfo::DataType::INT, 1, kHeight, kWidth); + auto nnData = createSyntheticNNData(info, valueFn); + auto outputs = processSegmentationFrames(config, {nnData}, true, false); + const size_t maskWidth = static_cast(kWidth) / config.getStepSize(); + const size_t maskHeight = static_cast(kHeight) / config.getStepSize(); + checkMaskMatches(*outputs.front(), expected, maskWidth, maskHeight); +} + +TEST_CASE("SegmentationParser runtime config update changes thresholding") { + constexpr int kWidth = 3; + constexpr int kHeight = 2; + constexpr int kChannels = 3; + + const auto values = makeArgmaxTestValues(); + auto valueFn = [&](int c, int h, int w) { return values[static_cast(h * kWidth + w) * static_cast(kChannels) + static_cast(c)]; }; + + auto info = makeNCHWTensorInfo("seg", dai::TensorInfo::DataType::FP32, kChannels, kHeight, kWidth); + auto nnData = createSyntheticNNData(info, valueFn); + + dai::SegmentationParserConfig initialConfig; + initialConfig.setConfidenceThreshold(0.3f); + initialConfig.setStepSize(1); + + dai::SegmentationParserConfig updatedConfig; + updatedConfig.setConfidenceThreshold(0.6f); + updatedConfig.setStepSize(1); + + const auto expectedInitial = computeExpectedMask(kWidth, kHeight, kChannels, 0.3f, false, valueFn, static_cast(initialConfig.getStepSize())); + const auto expectedUpdated = computeExpectedMask(kWidth, kHeight, kChannels, 0.6f, false, valueFn, static_cast(updatedConfig.getStepSize())); + const size_t initialWidth = static_cast(kWidth) / initialConfig.getStepSize(); + const size_t initialHeight = static_cast(kHeight) / initialConfig.getStepSize(); + const size_t updatedWidth = static_cast(kWidth) / updatedConfig.getStepSize(); + const size_t updatedHeight = static_cast(kHeight) / updatedConfig.getStepSize(); + + auto outputs = processSegmentationFrames(initialConfig, {nnData, nnData}, false, false, {initialConfig, updatedConfig}, true); + + REQUIRE(outputs.size() == 2); + checkMaskMatches(*outputs[0], expectedInitial, initialWidth, initialHeight); + checkMaskMatches(*outputs[1], expectedUpdated, updatedWidth, updatedHeight); +} From e283035f54ea0dee83e16f33c2b29dd58a0d8cb0 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Thu, 12 Feb 2026 10:06:24 +0100 Subject: [PATCH 154/180] Adjust Replay node behavior to stop pipeline only if loop false, add guard against joining own thread. --- src/pipeline/Node.cpp | 12 ++++++++++-- src/pipeline/ThreadedNode.cpp | 2 +- src/pipeline/node/host/Replay.cpp | 18 ++---------------- .../pipeline/node/detection_parser_test.cpp | 1 + 4 files changed, 14 insertions(+), 19 deletions(-) diff --git a/src/pipeline/Node.cpp b/src/pipeline/Node.cpp index 057462461b..53c294ae01 100644 --- a/src/pipeline/Node.cpp +++ b/src/pipeline/Node.cpp @@ -725,8 +725,16 @@ size_t Node::ConnectionInternal::Hash::operator()(const dai::Node::ConnectionInt } void Node::stopPipeline() { - auto pipeline = getParentPipeline(); - pipeline.stop(); + // FIXME - Not the best solution as the node now owns the pipeline. If it is the last reference to the pipeline, destructor will be called from the same + // thread. + try { + auto pipeline = getParentPipeline(); + pipeline.stop(); + } catch(const std::exception& e) { + if(e.what() != std::string("Pipeline is null")) { + throw; + } + } } void Node::Output::link(std::shared_ptr in) { diff --git a/src/pipeline/ThreadedNode.cpp b/src/pipeline/ThreadedNode.cpp index 282bb181f0..6fd9759c5c 100644 --- a/src/pipeline/ThreadedNode.cpp +++ b/src/pipeline/ThreadedNode.cpp @@ -62,7 +62,7 @@ void ThreadedNode::start() { } void ThreadedNode::wait() { - if(thread.joinable()) thread.join(); + if(thread.joinable() && thread.get_id() != std::this_thread::get_id()) thread.join(); } void ThreadedNode::stop() { diff --git a/src/pipeline/node/host/Replay.cpp b/src/pipeline/node/host/Replay.cpp index 498c0af0e3..92510b1908 100644 --- a/src/pipeline/node/host/Replay.cpp +++ b/src/pipeline/node/host/Replay.cpp @@ -247,6 +247,7 @@ void ReplayVideo::run() { } continue; } + stopPipeline(); break; } else { hasMetadata = false; @@ -323,14 +324,6 @@ void ReplayVideo::run() { first = false; } logger->info("Replay finished - stopping the pipeline!"); - try { - stopPipeline(); - } catch(const std::exception& e) { - // FIXME: This is a workaround for a bug in the pipeline - if(e.what() != std::string("Pipeline is null")) { - throw; - } - } #else throw std::runtime_error("ReplayVideo node requires protobuf support"); #endif @@ -373,6 +366,7 @@ void ReplayMetadataOnly::run() { bytePlayer.restart(); continue; } + stopPipeline(); break; } else { throw std::runtime_error("Metadata file contains no messages"); @@ -399,14 +393,6 @@ void ReplayMetadataOnly::run() { first = false; } - try { - stopPipeline(); - } catch(const std::exception& e) { - // FIXME: This is a workaround for a bug in the pipeline - if(e.what() != std::string("Pipeline is null")) { - throw; - } - } #else throw std::runtime_error("ReplayMetadataOnly node requires protobuf support"); #endif diff --git a/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp b/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp index d76caeb10a..1c989b018d 100644 --- a/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/detection_parser_test.cpp @@ -203,6 +203,7 @@ void runDetectionParserReplayTest(const std::string& modelName, const std::files auto outputQueue = detectionParser->out.createOutputQueue(4, true); p.start(); + REQUIRE(p.isRunning()); for(int i = 0; i < NUM_MSGS; i++) { INFO("Frame number: " << i); if(!p.isRunning()) break; From ab9779d6551a3dac34e8937b067b2470a796d0da Mon Sep 17 00:00:00 2001 From: asahtik <38485424+asahtik@users.noreply.github.com> Date: Thu, 12 Feb 2026 11:54:34 +0100 Subject: [PATCH 155/180] Fix manual calib being overwritten on RVC4 (#1670) * Fix calib being overwritten on pipeline build on RVC4 * RVC4 FW: Fix calib being overwritten on pipeline build * Fix PR issue Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * RVC4 FW: Merge develop --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- .../depthai/properties/GlobalProperties.hpp | 19 ++++++++++++++ src/pipeline/Pipeline.cpp | 2 +- .../runtime_calibration_test.cpp | 25 +++++++++++++++++++ 4 files changed, 46 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index a449d88f53..4c6c7d6e9e 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+f88257340b5acb2d6839f95cb3d6724a8fe569e0") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+e7bd88b6a9df049df08a5ab1a8522bf03f75c48f") diff --git a/include/depthai/properties/GlobalProperties.hpp b/include/depthai/properties/GlobalProperties.hpp index 6011917624..e0569c5428 100644 --- a/include/depthai/properties/GlobalProperties.hpp +++ b/include/depthai/properties/GlobalProperties.hpp @@ -79,6 +79,25 @@ struct GlobalProperties : PropertiesSerializable { uint32_t sippDmaBufferSize = SIPP_DMA_BUFFER_DEFAULT_SIZE; ~GlobalProperties() override; + + GlobalProperties& setFrom(const GlobalProperties& other) { + leonCssFrequencyHz = other.leonCssFrequencyHz; + leonMssFrequencyHz = other.leonMssFrequencyHz; + pipelineName = other.pipelineName; + pipelineVersion = other.pipelineVersion; + if(other.calibData) { + calibData = other.calibData; + eepromId = other.eepromId; + } + if(other.cameraTuningBlobSize) cameraTuningBlobSize = other.cameraTuningBlobSize; + cameraTuningBlobUri = other.cameraTuningBlobUri; + cameraSocketTuningBlobSize = other.cameraSocketTuningBlobSize; + cameraSocketTuningBlobUri = other.cameraSocketTuningBlobUri; + xlinkChunkSize = other.xlinkChunkSize; + sippBufferSize = other.sippBufferSize; + sippDmaBufferSize = other.sippDmaBufferSize; + return *this; + } }; DEPTHAI_SERIALIZE_EXT(GlobalProperties, diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 73bddffa50..258488364e 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -79,7 +79,7 @@ GlobalProperties PipelineImpl::getGlobalProperties() const { } void PipelineImpl::setGlobalProperties(GlobalProperties globalProperties) { - this->globalProperties = globalProperties; + this->globalProperties.setFrom(globalProperties); } std::shared_ptr PipelineImpl::getNode(Node::Id id) const { diff --git a/tests/src/ondevice_tests/runtime_calibration_test.cpp b/tests/src/ondevice_tests/runtime_calibration_test.cpp index bb9b5fd53b..b3861995ef 100644 --- a/tests/src/ondevice_tests/runtime_calibration_test.cpp +++ b/tests/src/ondevice_tests/runtime_calibration_test.cpp @@ -1,5 +1,6 @@ #include +#include "depthai/common/CameraBoardSocket.hpp" #include "depthai/depthai.hpp" #include "depthai/utility/CompilerWarnings.hpp" @@ -56,3 +57,27 @@ TEST_CASE("Test runtime calibration") { FAIL("Failed to read back distortion coefficients: " << e.what()); } } + +TEST_CASE("Test device setCalibration before pipeline build") { + dai::Pipeline p; + auto camQ = p.create()->build(dai::CameraBoardSocket::CAM_A)->requestOutput({640, 480})->createOutputQueue(); + dai::CalibrationHandler calibHandler = p.getDefaultDevice()->readCalibration(); + calibHandler.validateCalibrationHandler(); + calibHandler.setDeviceName("test_device_name"); + p.getDefaultDevice()->setCalibration(calibHandler); + + auto calibPreBuild = p.getDefaultDevice()->getCalibration(); + REQUIRE(calibPreBuild.getEepromData().deviceName == "test_device_name"); + + p.build(); + + auto calibPostBuild = p.getDefaultDevice()->getCalibration(); + REQUIRE(calibPostBuild.getEepromData().deviceName == "test_device_name"); + + p.start(); + + auto calibPostStart = p.getDefaultDevice()->getCalibration(); + REQUIRE(calibPostStart.getEepromData().deviceName == "test_device_name"); + + p.stop(); +} From 34b261c196488c84c83501211c7b18be968010d4 Mon Sep 17 00:00:00 2001 From: aljazkonec1 Date: Thu, 12 Feb 2026 12:29:05 +0100 Subject: [PATCH 156/180] remove guard for self join thread --- src/pipeline/Node.cpp | 2 +- src/pipeline/ThreadedNode.cpp | 2 +- src/pipeline/node/host/Replay.cpp | 4 ++++ 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/pipeline/Node.cpp b/src/pipeline/Node.cpp index 53c294ae01..5703f732b3 100644 --- a/src/pipeline/Node.cpp +++ b/src/pipeline/Node.cpp @@ -726,7 +726,7 @@ size_t Node::ConnectionInternal::Hash::operator()(const dai::Node::ConnectionInt void Node::stopPipeline() { // FIXME - Not the best solution as the node now owns the pipeline. If it is the last reference to the pipeline, destructor will be called from the same - // thread. + // thread and the pipeline will crash. try { auto pipeline = getParentPipeline(); pipeline.stop(); diff --git a/src/pipeline/ThreadedNode.cpp b/src/pipeline/ThreadedNode.cpp index 6fd9759c5c..282bb181f0 100644 --- a/src/pipeline/ThreadedNode.cpp +++ b/src/pipeline/ThreadedNode.cpp @@ -62,7 +62,7 @@ void ThreadedNode::start() { } void ThreadedNode::wait() { - if(thread.joinable() && thread.get_id() != std::this_thread::get_id()) thread.join(); + if(thread.joinable()) thread.join(); } void ThreadedNode::stop() { diff --git a/src/pipeline/node/host/Replay.cpp b/src/pipeline/node/host/Replay.cpp index 92510b1908..97ec18ed14 100644 --- a/src/pipeline/node/host/Replay.cpp +++ b/src/pipeline/node/host/Replay.cpp @@ -247,6 +247,7 @@ void ReplayVideo::run() { } continue; } + // This will stop even if there is still frames in the pipeline stopPipeline(); break; } else { @@ -266,6 +267,8 @@ void ReplayVideo::run() { videoPlayer.restart(); continue; } + // This will stop even if there is still frames in the pipeline + stopPipeline(); break; } else { hasVideo = false; @@ -366,6 +369,7 @@ void ReplayMetadataOnly::run() { bytePlayer.restart(); continue; } + // This will stop even if there is still frames in the pipeline stopPipeline(); break; } else { From 183eed1d211470a6f3c24ce57d737e6b1346b2ff Mon Sep 17 00:00:00 2001 From: Danilo Pejovic <115164734+danilo-pejovic@users.noreply.github.com> Date: Thu, 12 Feb 2026 12:35:59 +0100 Subject: [PATCH 157/180] Remove gperf download step from Dockerfile Removed the download step for gperf from the Dockerfile. --- tests/Dockerfile | 5 ----- 1 file changed, 5 deletions(-) diff --git a/tests/Dockerfile b/tests/Dockerfile index fb3ba4af83..63a7d9fcb6 100644 --- a/tests/Dockerfile +++ b/tests/Dockerfile @@ -97,11 +97,6 @@ RUN python3 -m venv venv && \ ENV PATH="/workspace/venv/bin:$PATH" -RUN --mount=type=cache,target=/opt/vcpkg-downloads \ - curl -fL --retry 5 --retry-delay 2 \ - https://mirrors.kernel.org/gnu/gperf/gperf-3.1.tar.gz \ - -o /opt/vcpkg-downloads/gperf-3.1.tar.gz - RUN --mount=type=cache,target=/opt/vcpkg-downloads \ --mount=type=cache,target=/opt/vcpkg-bincache \ if [ "$FLAVOR" != "vanilla" ] && [ "$FLAVOR" != "tsan" ]; then \ From 320f456ffdf3f81d8d3f725d8ab25a2115a34662 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Mon, 24 Nov 2025 17:25:17 +0100 Subject: [PATCH 158/180] Add ispOutput --- .../src/pipeline/node/CameraBindings.cpp | 5 + .../capabilities/ImgFrameCapability.hpp | 3 +- include/depthai/pipeline/node/Camera.hpp | 5 + src/pipeline/node/Camera.cpp | 14 + .../pipeline/node/camera_test.cpp | 402 ++++++++++++++++++ 5 files changed, 428 insertions(+), 1 deletion(-) diff --git a/bindings/python/src/pipeline/node/CameraBindings.cpp b/bindings/python/src/pipeline/node/CameraBindings.cpp index 5d00722d4d..221f8339a1 100644 --- a/bindings/python/src/pipeline/node/CameraBindings.cpp +++ b/bindings/python/src/pipeline/node/CameraBindings.cpp @@ -71,6 +71,11 @@ void bind_camera(pybind11::module& m, void* pCallstack) { "enableUndistortion"_a = std::nullopt, py::return_value_policy::reference_internal, DOC(dai, node, Camera, requestOutput)) + .def("requestIspOutput", + py::overload_cast>(&Camera::requestIspOutput), + "fps"_a = std::nullopt, + py::return_value_policy::reference_internal, + DOC(dai, node, Camera, requestIspOutput)) .def("requestOutput", py::overload_cast(&Camera::requestOutput), "capability"_a, diff --git a/include/depthai/capabilities/ImgFrameCapability.hpp b/include/depthai/capabilities/ImgFrameCapability.hpp index f40950adab..68e1665495 100644 --- a/include/depthai/capabilities/ImgFrameCapability.hpp +++ b/include/depthai/capabilities/ImgFrameCapability.hpp @@ -47,11 +47,12 @@ class ImgFrameCapability : public CapabilityCRTP std::optional type; ImgResizeMode resizeMode{ImgResizeMode::CROP}; std::optional enableUndistortion; + bool ispOutput = false; // TODO(jakgra) add optional CapabilityRange fov / max-min horiz. / vertical crop; ~ImgFrameCapability() override; - DEPTHAI_SERIALIZE(ImgFrameCapability, size, fps, type, resizeMode, enableUndistortion); + DEPTHAI_SERIALIZE(ImgFrameCapability, size, fps, type, resizeMode, enableUndistortion, ispOutput); private: class Impl; diff --git a/include/depthai/pipeline/node/Camera.hpp b/include/depthai/pipeline/node/Camera.hpp index c2cfb32613..d233d14409 100644 --- a/include/depthai/pipeline/node/Camera.hpp +++ b/include/depthai/pipeline/node/Camera.hpp @@ -45,6 +45,10 @@ class Camera : public DeviceNodeCRTP, publ std::optional fps = std::nullopt, bool useHighestResolution = false); + /** + * Request output with isp resolution. The fps does not vote. + */ + Node::Output* requestIspOutput(std::optional fps = std::nullopt); /** * Build with a specific board socket * @param boardSocket Board socket to use @@ -122,6 +126,7 @@ class Camera : public DeviceNodeCRTP, publ * Input for mocking 'isp' functionality on RVC2. * Default queue is blocking with size 8 */ + Input mockIsp{*this, {"mockIsp", DEFAULT_GROUP, true, 8, {{{DatatypeEnum::ImgFrame, false}}}, DEFAULT_WAIT_FOR_MESSAGE}}; /** diff --git a/src/pipeline/node/Camera.cpp b/src/pipeline/node/Camera.cpp index 3cb6ef070f..4be85fbac6 100644 --- a/src/pipeline/node/Camera.cpp +++ b/src/pipeline/node/Camera.cpp @@ -246,9 +246,23 @@ Node::Output* Camera::requestOutput(std::pair size, return pimpl->requestOutput(*this, cap, false); } +Node::Output* Camera::requestIspOutput(std::optional fps) { + ImgFrameCapability cap; + + if(fps.has_value()) { + cap.fps.fixed(fps.value()); + } + + cap.type = std::nullopt; + cap.enableUndistortion = false; + cap.ispOutput = true; + return pimpl->requestOutput(*this, cap, false); +} + Node::Output* Camera::requestOutput(const Capability& capability, bool onHost) { return pimpl->requestOutput(*this, capability, onHost); } + #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT Camera& Camera::setMockIsp(ReplayVideo& replay) { if(!replay.getReplayVideoFile().empty()) { diff --git a/tests/src/ondevice_tests/pipeline/node/camera_test.cpp b/tests/src/ondevice_tests/pipeline/node/camera_test.cpp index 5c2cbdb865..3d6a4a8cc4 100644 --- a/tests/src/ondevice_tests/pipeline/node/camera_test.cpp +++ b/tests/src/ondevice_tests/pipeline/node/camera_test.cpp @@ -239,3 +239,405 @@ TEST_CASE("Camera run without calibration") { REQUIRE(frame->getHeight() > 0); } } + +TEST_CASE("Camera: Test isp output - Correct resolution") { + // This will run the entire TEST_CASE for each pair in the list + auto testResolution = GENERATE(values>({{1280, 800}, {1280, 500}, {1000, 800}, {640, 400}, {540, 400}})); + + SECTION("Testing resolution: " + std::to_string(testResolution.first) + "x" + std::to_string(testResolution.second)) { + auto device = std::make_shared(); + dai::Pipeline p(device); + auto cameraFeatures = device->getConnectedCameraFeatures(); + + struct IspTestCase { + std::shared_ptr outputQueue; + std::shared_ptr outputIspQueue; + std::pair resolution; + dai::CameraBoardSocket socket; + }; + + std::vector ispTestCases; + + for(const auto& feature : cameraFeatures) { + auto camera = p.create()->build(feature.socket); + ispTestCases.push_back( + {camera->requestOutput(testResolution)->createOutputQueue(), camera->requestIspOutput()->createOutputQueue(), testResolution, feature.socket}); + } + + p.start(); + + for(auto& testCase : ispTestCases) { + auto output = testCase.outputQueue->get(); + auto outputIsp = testCase.outputIspQueue->get(); + + if(testCase.socket == dai::CameraBoardSocket::CAM_B || testCase.socket == dai::CameraBoardSocket::CAM_C) { + REQUIRE(output->getWidth() == testResolution.first); + REQUIRE(output->getHeight() == testResolution.second); + + // Note: ISP output usually matches sensor max or specific ISP scales + REQUIRE((outputIsp->getWidth() == 1280 || outputIsp->getWidth() == 640)); + } + + REQUIRE(output->getWidth() <= outputIsp->getWidth()); + REQUIRE(output->getHeight() <= outputIsp->getHeight()); + } + } +} + +TEST_CASE("Camera: Test isp output - Isp fps lower then maxFPS") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[0]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputQueue = camera->requestOutput({width, height}, std::nullopt, dai::ImgResizeMode::CROP, 30., std::nullopt)->createOutputQueue(); + auto outputIspQueue = camera->requestIspOutput(10.)->createOutputQueue(); + + p.start(); + int outCounter = 0; + int outIspCounter = 0; + + // wait for the first img + outputQueue->get(); + outputIspQueue->get(); + + using clock = std::chrono::steady_clock; + auto startTime = clock::now(); + auto testDuration = std::chrono::seconds(10); + + while(true) { + auto out = outputQueue->tryGet(); + auto outIsp = outputIspQueue->tryGet(); + if(out) { + outCounter += 1; + } + if(outIsp) { + outIspCounter += 1; + } + + auto now = clock::now(); + if(now - startTime > testDuration) { + break; + } + } + double fps = static_cast(outCounter) / testDuration.count(); + double fpsIsp = static_cast(outIspCounter) / testDuration.count(); + p.stop(); + p.wait(); + REQUIRE(fps > 26.0); + REQUIRE(fps < 34.0); + REQUIRE(fpsIsp > 6.0); + REQUIRE(fpsIsp < 14.0); +} + +TEST_CASE("Camera: Test isp output - Isp fps bigger then maxFPS") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[1]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputQueue = camera->requestOutput({width, height}, std::nullopt, dai::ImgResizeMode::CROP, 10., std::nullopt)->createOutputQueue(); + auto outputIspQueue = camera->requestIspOutput(30.)->createOutputQueue(); + + p.start(); + int outCounter = 0; + int outIspCounter = 0; + + // wait for the first img + outputQueue->get(); + outputIspQueue->get(); + + using clock = std::chrono::steady_clock; + auto startTime = clock::now(); + auto testDuration = std::chrono::seconds(10); + + while(true) { + auto out = outputQueue->tryGet(); + auto outIsp = outputIspQueue->tryGet(); + if(out) { + outCounter += 1; + } + if(outIsp) { + outIspCounter += 1; + } + + auto now = clock::now(); + if(now - startTime > testDuration) { + break; + } + } + p.stop(); + p.wait(); + double fps = static_cast(outCounter) / testDuration.count(); + double fpsIsp = static_cast(outIspCounter) / testDuration.count(); + REQUIRE(fps > 6.0); + REQUIRE(fps < 14.0); + REQUIRE(fpsIsp > 6.0); + REQUIRE(fpsIsp < 14.0); +} + +TEST_CASE("Camera: Test isp output - Only Isp") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[1]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputIspQueue = camera->requestIspOutput(10.)->createOutputQueue(); + + p.start(); + int outIspCounter = 0; + + // wait for the first img + outputIspQueue->get(); + + using clock = std::chrono::steady_clock; + auto startTime = clock::now(); + auto testDuration = std::chrono::seconds(10); + + while(true) { + auto outIsp = outputIspQueue->tryGet(); + if(outIsp) { + outIspCounter += 1; + } + + auto now = clock::now(); + if(now - startTime > testDuration) { + break; + } + } + p.stop(); + p.wait(); + double fpsIsp = static_cast(outIspCounter) / testDuration.count(); + REQUIRE(fpsIsp > 6.0); + REQUIRE(fpsIsp < 14.0); +} + +TEST_CASE("Camera: Test isp output - Only Isp default fps") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[1]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputIspQueue = camera->requestIspOutput()->createOutputQueue(); + + p.start(); + int outIspCounter = 0; + + // wait for the first img + outputIspQueue->get(); + + using clock = std::chrono::steady_clock; + + auto startTime = clock::now(); + auto testDuration = std::chrono::seconds(3); + + while(true) { + auto outIsp = outputIspQueue->tryGet(); + if(outIsp) { + outIspCounter += 1; + } + + auto now = clock::now(); + if(now - startTime > testDuration) { + break; + } + } + p.stop(); + p.wait(); + REQUIRE(outIspCounter > 0); +} + +TEST_CASE("Camera: Test two isp outputs") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[1]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputIspQueue1 = camera->requestIspOutput()->createOutputQueue(); + auto outputIspQueue2 = camera->requestIspOutput()->createOutputQueue(); + + p.start(); + int outIspCounter1 = 0; + int outIspCounter2 = 0; + + // wait for the first img + outputIspQueue1->get(); + outputIspQueue2->get(); + + using clock = std::chrono::steady_clock; + + auto startTime = clock::now(); + auto testDuration = std::chrono::seconds(3); + + while(true) { + auto outIsp1 = outputIspQueue1->tryGet(); + auto outIsp2 = outputIspQueue2->tryGet(); + if(outIsp1) { + outIspCounter1 += 1; + } + if(outIsp2) { + outIspCounter2 += 1; + } + + auto now = clock::now(); + if(now - startTime > testDuration) { + break; + } + } + p.stop(); + p.wait(); + REQUIRE(outIspCounter1 > 0); + REQUIRE(outIspCounter2 > 0); +} + +TEST_CASE("Camera: Test two isp outputs FPS") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[1]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputIspQueue1 = camera->requestIspOutput(10.)->createOutputQueue(); + auto outputIspQueue2 = camera->requestIspOutput(15.)->createOutputQueue(); + + p.start(); + int outIspCounter1 = 0; + int outIspCounter2 = 0; + + // wait for the first img + outputIspQueue1->get(); + outputIspQueue2->get(); + + using clock = std::chrono::steady_clock; + + auto startTime = clock::now(); + auto testDuration = std::chrono::seconds(3); + + while(true) { + auto outIsp1 = outputIspQueue1->tryGet(); + auto outIsp2 = outputIspQueue2->tryGet(); + if(outIsp1) { + outIspCounter1 += 1; + } + if(outIsp2) { + outIspCounter2 += 1; + } + + auto now = clock::now(); + if(now - startTime > testDuration) { + break; + } + } + p.stop(); + p.wait(); + REQUIRE(outIspCounter1 > 3 * 8); + REQUIRE(outIspCounter1 < 3 * 12); + + REQUIRE(outIspCounter2 > 3 * 13); + REQUIRE(outIspCounter2 < 3 * 17); +} + +TEST_CASE("Camera: Test three outputs + isp output") { + auto device = std::make_shared(); + dai::Pipeline p(device); + + auto cameraFeatures = device->getConnectedCameraFeatures(); + + if(cameraFeatures.size() == 0) { + return; + } + + const auto& feature = cameraFeatures[1]; + auto camera = p.create()->build(feature.socket); + + int width = 200; + int height = 200; + + auto outputIspQueue1 = camera->requestOutput(std::make_pair(640, 400), std::nullopt, dai::ImgResizeMode::CROP, 10)->createOutputQueue(); + auto outputIspQueue2 = camera->requestOutput(std::make_pair(440, 300), std::nullopt, dai::ImgResizeMode::CROP, 10)->createOutputQueue(); + auto outputIspQueueIsp = camera->requestIspOutput()->createOutputQueue(); + auto outputIspQueue3 = camera->requestOutput(std::make_pair(240, 200), std::nullopt, dai::ImgResizeMode::CROP, 10)->createOutputQueue(); + + p.start(); + + // wait for the first img + auto output1 = outputIspQueue1->get(); + auto output2 = outputIspQueue2->get(); + auto output3 = outputIspQueue3->get(); + auto outputIsp = outputIspQueueIsp->get(); + + REQUIRE(output1); + REQUIRE(output1->getWidth() == 640); + REQUIRE(output1->getHeight() == 400); + + REQUIRE(output2); + REQUIRE(output2->getWidth() == 440); + REQUIRE(output2->getHeight() == 300); + + REQUIRE(output3); + REQUIRE(output3->getWidth() == 240); + REQUIRE(output3->getHeight() == 200); + + REQUIRE(outputIsp); + REQUIRE(outputIsp->getWidth() >= 640); + REQUIRE(outputIsp->getHeight() >= 400); +} From e81a1d757d726684e5590fae85cb9bafc6ddcd5f Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Tue, 25 Nov 2025 11:16:28 +0100 Subject: [PATCH 159/180] Add examples --- examples/cpp/Camera/CMakeLists.txt | 3 ++ examples/cpp/Camera/camera_isp.cpp | 48 ++++++++++++++++++++++++++++ examples/python/Camera/camera_isp.py | 31 ++++++++++++++++++ 3 files changed, 82 insertions(+) create mode 100644 examples/cpp/Camera/camera_isp.cpp create mode 100644 examples/python/Camera/camera_isp.py diff --git a/examples/cpp/Camera/CMakeLists.txt b/examples/cpp/Camera/CMakeLists.txt index 895f53e450..20ca11e358 100644 --- a/examples/cpp/Camera/CMakeLists.txt +++ b/examples/cpp/Camera/CMakeLists.txt @@ -30,3 +30,6 @@ dai_set_example_test_labels(camera_undistortion ondevice rvc2_all rvc4 ci) dai_add_example(camera_hdr camera_hdr.cpp ON OFF) dai_set_example_test_labels(camera_hdr ondevice rvc4 ci) + +dai_add_example(camera_isp camera_isp.cpp ON OFF) +dai_set_example_test_labels(camera_isp ondevice rvc2_all ci) diff --git a/examples/cpp/Camera/camera_isp.cpp b/examples/cpp/Camera/camera_isp.cpp new file mode 100644 index 0000000000..56b489bdb1 --- /dev/null +++ b/examples/cpp/Camera/camera_isp.cpp @@ -0,0 +1,48 @@ +#include +#include + +#include "depthai/depthai.hpp" + +int main() { + using namespace dai; + using namespace std; + + dai::Pipeline pipeline; + + auto cam = pipeline.create()->build(dai::CameraBoardSocket::CAM_B); + + // Request outputs + auto videoOut = cam->requestOutput(std::make_pair(800, 400), std::nullopt, ImgResizeMode::CROP, 30.0f, std::nullopt); + auto ispOut = cam->requestIspOutput(2.0f); + + // Create output queues + auto videoQueue = videoOut->createOutputQueue(); + auto ispQueue = ispOut->createOutputQueue(); + + pipeline.start(); + + // Get first frames and print resolutions + auto videoIn = videoQueue->get(); + auto videoInIsp = ispQueue->get(); + + cout << "Standard output resolution = " << videoIn->getCvFrame().cols << " x " << videoIn->getCvFrame().rows << endl; + + cout << "Isp output resolution = " << videoInIsp->getCvFrame().cols << " x " << videoInIsp->getCvFrame().rows << endl; + + // Main loop + while(pipeline.isRunning()) { + auto videoIn = videoQueue->tryGet(); + auto videoInIsp = ispQueue->tryGet(); + + if(videoIn) { + cv::imshow("video", videoIn->getCvFrame()); + } + if(videoInIsp) { + cv::imshow("videoIsp", videoInIsp->getCvFrame()); + } + + if(cv::waitKey(1) == 'q') break; + } + + return 0; +} diff --git a/examples/python/Camera/camera_isp.py b/examples/python/Camera/camera_isp.py new file mode 100644 index 0000000000..43bac4eda8 --- /dev/null +++ b/examples/python/Camera/camera_isp.py @@ -0,0 +1,31 @@ +import cv2 +import depthai as dai + +# Create pipeline +with dai.Pipeline() as pipeline: + # Define source and output + cam = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B) + videoQueue = cam.requestOutput((800,400), fps=30).createOutputQueue() + videoIsp = cam.requestIspOutput(fps=2).createOutputQueue() + # Connect to device and start pipeline + pipeline.start() + videoIn = videoQueue.get() + videoInIsp = videoIsp.get() + print( + "Standart output resolution = " + f"{ videoIn.getCvFrame().shape[1]} x { videoIn.getCvFrame().shape[0]}" + ) + print( + f"Isp output resolution = " + f"{ videoInIsp.getCvFrame().shape[1]} x { videoInIsp.getCvFrame().shape[0]}" + ) + while pipeline.isRunning(): + videoIn = videoQueue.tryGet() + videoInIsp = videoIsp.tryGet() # Returns 640x400 + if videoIn: + cv2.imshow("video", videoIn.getCvFrame()) + if videoInIsp: + cv2.imshow("videoIsp", videoInIsp.getCvFrame()) + + if cv2.waitKey(1) == ord("q"): + break From 656a0865e9b3f8dd22637c17120a965185713e8e Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Wed, 11 Feb 2026 16:37:14 +0100 Subject: [PATCH 160/180] Bump FWs --- cmake/Depthai/DepthaiDeviceRVC4Config.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake index 4c6c7d6e9e..41dd02f927 100644 --- a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake +++ b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake @@ -3,4 +3,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+e7bd88b6a9df049df08a5ab1a8522bf03f75c48f") +set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+b73c7903883f8316dca6795245b50d4640c59392") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index a83ecb3fa9..96e412f828 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "cc655921194a8d1b191c5219f2bf1331b115a400") +set(DEPTHAI_DEVICE_SIDE_COMMIT "21da095007e982d95b9b77043979ccc415217609") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 58d33d9a6c333d322400d22b9cf877a7ead7cbbd Mon Sep 17 00:00:00 2001 From: aljazdu <74094620+aljazdu@users.noreply.github.com> Date: Fri, 13 Feb 2026 13:25:44 +0100 Subject: [PATCH 161/180] Feature/hil testing summary (#1680) * Added Job Summary for HIL testing workflows * Workflow summaries will now be aggregated into a single summary * Added script which handles parsing and creating workflow summary for HIL testing * Updated tests log parsing * Summaries are now grouped together * Updated actions/upload-artifact inside of test_child.yml * Temp Commit * Log links will now point to raw log views * Extended dorny tester * Removed passed placeholder, added tailed truncation * Updated workflow scripts * Truncation fix, time testing * Removed dorny test reporter, cleanup * Added windows rvc4 test reports * Clean up * Test Summaries are now collapsible * Fixed collapsed summary tables --- .github/workflows/test_child.yml | 90 +++++++- .github/workflows/test_child_windows.yml | 68 +++++- scripts/ci/ctest_summary.py | 170 +++++++++++++++ scripts/ci/ctest_to_junit.py | 261 +++++++++++++++++++++++ tests/run_tests.py | 16 +- 5 files changed, 600 insertions(+), 5 deletions(-) create mode 100644 scripts/ci/ctest_summary.py create mode 100644 scripts/ci/ctest_to_junit.py diff --git a/.github/workflows/test_child.yml b/.github/workflows/test_child.yml index f222dc67d1..db4445d015 100644 --- a/.github/workflows/test_child.yml +++ b/.github/workflows/test_child.yml @@ -55,7 +55,16 @@ jobs: run: | source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc2-${{ inputs.flavor }}" - exec hil_runner --capabilities depthai-core-hil --platforms 'rvc2 and rvc2' --reservation-name $RESERVATION_NAME --wait --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc2" + set -o pipefail + hil_runner --capabilities depthai-core-hil --platforms 'rvc2 and rvc2' --reservation-name $RESERVATION_NAME --wait --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc2" 2>&1 | tee rvc2_test_output.log + + - name: Upload test log + if: always() + uses: actions/upload-artifact@v4 + with: + name: ctest-log-${{ inputs.flavor }}-linux_rvc2_test + path: rvc2_test_output.log + if-no-files-found: warn # Testing linux_rvc4_test: @@ -72,7 +81,16 @@ jobs: run: | source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}" - exec hil_runner --models "oak4_pro or oak4_d" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc4" + set -o pipefail + hil_runner --models "oak4_pro or oak4_d" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc4" 2>&1 | tee rvc4_test_output.log + + - name: Upload test log + if: always() + uses: actions/upload-artifact@v4 + with: + name: ctest-log-${{ inputs.flavor }}-linux_rvc4_test-rvc4os=${{ matrix.rvc4os }} + path: rvc4_test_output.log + if-no-files-found: warn linux_rvc4_rgb_test: needs: [build_docker_container] @@ -88,7 +106,73 @@ jobs: run: | source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-${{ matrix.rvc4os }}-${{ inputs.flavor }}-rgb" - exec hil_runner --models "oak4_s" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc4rgb" + set -o pipefail + hil_runner --models "oak4_s" --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --docker-image ${{ secrets.CONTAINER_REGISTRY }}/depthai-core-hil:${{ needs.build_docker_container.outputs.tag }} --commands "./tests/run_tests_entrypoint.sh rvc4rgb" 2>&1 | tee rvc4_rgb_test_output.log + + - name: Upload test log + if: always() + uses: actions/upload-artifact@v4 + with: + name: ctest-log-${{ inputs.flavor }}-linux_rvc4_rgb_test-rvc4os=${{ matrix.rvc4os }} + path: rvc4_rgb_test_output.log + if-no-files-found: warn + + report_linux_test_results: + name: Report Linux test results (${{ inputs.flavor }}) + needs: [linux_rvc2_test, linux_rvc4_test, linux_rvc4_rgb_test] + if: always() + runs-on: ubuntu-latest + permissions: + contents: read + steps: + - uses: actions/checkout@v3 + - name: Download test logs + uses: actions/download-artifact@v4 + with: + pattern: ctest-log-${{ inputs.flavor }}-* + path: ctest-logs + - name: Generate combined summary + run: | + if ! command -v python3 >/dev/null 2>&1; then + echo "python3 not available, skipping test summary." >> "$GITHUB_STEP_SUMMARY" + exit 0 + fi + + shopt -s nullglob + found_logs=0 + first_summary=1 + mkdir -p ctest-reports + artifact_prefix="ctest-log-${{ inputs.flavor }}-" + for dir in ctest-logs/${artifact_prefix}*; do + [ -d "$dir" ] || continue + log_files=("$dir"/*.log) + [ ${#log_files[@]} -gt 0 ] || continue + found_logs=1 + context="$(basename "$dir")" + context="${context#${artifact_prefix}}" + if [ "$first_summary" -eq 0 ]; then + echo "" >> "$GITHUB_STEP_SUMMARY" + echo "---" >> "$GITHUB_STEP_SUMMARY" + echo "" >> "$GITHUB_STEP_SUMMARY" + fi + first_summary=0 + python3 scripts/ci/ctest_summary.py "${log_files[0]}" "$context" >> "$GITHUB_STEP_SUMMARY" + report_context="${context//\//_}" + report_context="${report_context// /_}" + python3 scripts/ci/ctest_to_junit.py "${log_files[0]}" "ctest-reports/${report_context}.xml" "$context" + echo "" >> "$GITHUB_STEP_SUMMARY" + done + + if [ "$found_logs" -eq 0 ]; then + echo "No test logs found to summarize." >> "$GITHUB_STEP_SUMMARY" + fi + - name: Upload JUnit XML reports + if: always() && hashFiles('ctest-reports/*.xml') != '' + uses: actions/upload-artifact@v4 + with: + name: junit-reports-${{ inputs.flavor }} + path: ctest-reports/*.xml + if-no-files-found: warn # linux_rvc4_fsync_test: # needs: [build_docker_container] diff --git a/.github/workflows/test_child_windows.yml b/.github/workflows/test_child_windows.yml index 45200892b8..33cb2e2c23 100644 --- a/.github/workflows/test_child_windows.yml +++ b/.github/workflows/test_child_windows.yml @@ -176,4 +176,70 @@ jobs: run: | source scripts/hil/prepare_hil_framework.sh ${{ secrets.HIL_PAT_TOKEN }} export RESERVATION_NAME="https://github.com/$GITHUB_REPOSITORY/actions/runs/$GITHUB_RUN_ID#rvc4-windows-${{ matrix.rvc4os }}" - exec hil --models "oak4_pro or oak4_d" -os windows --no-shell --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --commands "rmdir /S /Q depthai-core & git clone https://github.com/luxonis/depthai-core.git && cd depthai-core && git fetch origin ${{ github.sha }} && git checkout ${{ github.sha }} && scripts\hil\get_artifacts_and_run_tests.cmd \"${{ github.repository }}\" \"${{ github.token }}\" \"C:\depthai-core\" \"${{ needs.build_windows_tests.outputs.artifact_id }}\"" + set -o pipefail + hil --models "oak4_pro or oak4_d" -os windows --no-shell --reservation-name $RESERVATION_NAME --wait --rvc4-os-version ${{ matrix.rvc4os }} --commands "rmdir /S /Q depthai-core & git clone https://github.com/luxonis/depthai-core.git && cd depthai-core && git fetch origin ${{ github.sha }} && git checkout ${{ github.sha }} && scripts\hil\get_artifacts_and_run_tests.cmd \"${{ github.repository }}\" \"${{ github.token }}\" \"C:\depthai-core\" \"${{ needs.build_windows_tests.outputs.artifact_id }}\"" 2>&1 | tee windows_rvc4_test_output.log + + - name: Upload test log + if: always() + uses: actions/upload-artifact@v4 + with: + name: ctest-log-windows-rvc4_test-rvc4os=${{ matrix.rvc4os }} + path: windows_rvc4_test_output.log + if-no-files-found: warn + + report_windows_test_results: + name: Report Windows test results + needs: [run_windows_tests_rvc4] + if: always() + runs-on: ubuntu-latest + permissions: + contents: read + steps: + - uses: actions/checkout@v3 + - name: Download test logs + uses: actions/download-artifact@v4 + with: + pattern: ctest-log-windows-* + path: ctest-logs + - name: Generate combined summary + run: | + if ! command -v python3 >/dev/null 2>&1; then + echo "python3 not available, skipping test summary." >> "$GITHUB_STEP_SUMMARY" + exit 0 + fi + + shopt -s nullglob + found_logs=0 + first_summary=1 + mkdir -p ctest-reports + artifact_prefix="ctest-log-windows-" + for dir in ctest-logs/${artifact_prefix}*; do + [ -d "$dir" ] || continue + log_files=("$dir"/*.log) + [ ${#log_files[@]} -gt 0 ] || continue + found_logs=1 + context="$(basename "$dir")" + context="${context#${artifact_prefix}}" + if [ "$first_summary" -eq 0 ]; then + echo "" >> "$GITHUB_STEP_SUMMARY" + echo "---" >> "$GITHUB_STEP_SUMMARY" + echo "" >> "$GITHUB_STEP_SUMMARY" + fi + first_summary=0 + python3 scripts/ci/ctest_summary.py "${log_files[0]}" "$context" >> "$GITHUB_STEP_SUMMARY" + report_context="${context//\//_}" + report_context="${report_context// /_}" + python3 scripts/ci/ctest_to_junit.py "${log_files[0]}" "ctest-reports/${report_context}.xml" "$context" + echo "" >> "$GITHUB_STEP_SUMMARY" + done + + if [ "$found_logs" -eq 0 ]; then + echo "No test logs found to summarize." >> "$GITHUB_STEP_SUMMARY" + fi + - name: Upload JUnit XML reports + if: always() && hashFiles('ctest-reports/*.xml') != '' + uses: actions/upload-artifact@v4 + with: + name: junit-reports-windows + path: ctest-reports/*.xml + if-no-files-found: warn diff --git a/scripts/ci/ctest_summary.py b/scripts/ci/ctest_summary.py new file mode 100644 index 0000000000..1e2ea7bac3 --- /dev/null +++ b/scripts/ci/ctest_summary.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python3 +import re +import sys +from collections import OrderedDict +from pathlib import Path + + +ANSI_RE = re.compile(r"\x1b\[[0-9;]*[A-Za-z]") +SUMMARY_RE = re.compile( + r"\[(?P[^\]]+)\]\s+\d+% tests passed,\s+(?P\d+) tests failed out of (?P\d+)" +) +FAILED_LIST_RE = re.compile( + r"\[(?P[^\]]+)\]\s+(?P\d+)\s*-\s*(?P.+?)" + r"(?:\s+\((?P[^)]+)\))?\s*$" +) +TEST_OUTPUT_RE = re.compile( + r"\[(?P[^\]]+)\]\s+(?P\d+):\s*(?P.*)$" +) + +MAX_SNIPPET_LINES = 30 +MAX_SNIPPET_CHARS = 20000 + + +def normalize_name(name: str) -> str: + return " ".join(name.strip().split()) + + +def parse_args(argv): + if len(argv) < 2: + return None, "" + + log_path = Path(argv[1]) + context = " ".join(argv[2:]).strip() + return log_path, context + + +def clip_output_lines(lines): + clipped = [line for line in lines if line.strip()] + if not clipped: + return [] + if len(clipped) > MAX_SNIPPET_LINES: + clipped = clipped[-MAX_SNIPPET_LINES:] + + selected_reversed = [] + chars = 0 + for line in reversed(clipped): + separator = 1 if selected_reversed else 0 + needed = len(line) + separator + if chars + needed > MAX_SNIPPET_CHARS: + if not selected_reversed and MAX_SNIPPET_CHARS > 0: + # Keep tail of very long single lines. + fragment = line[-MAX_SNIPPET_CHARS:] + if fragment: + selected_reversed.append(fragment) + break + selected_reversed.append(line) + chars += needed + return list(reversed(selected_reversed)) + + +def main() -> int: + log_path, context = parse_args(sys.argv) + if log_path is None: + print("No log path provided.") + return 0 + + header_suffix = f" ({context})" if context else "" + if not log_path.exists(): + print(f"Log file not found: {log_path}") + return 0 + + order = [] + summaries = {} + failures = {} + test_outputs = {} + + def ensure_config(config: str) -> None: + if config not in summaries: + summaries[config] = None + failures[config] = OrderedDict() + test_outputs[config] = {} + order.append(config) + + with log_path.open("r", errors="ignore") as handle: + for raw_line in handle: + line = ANSI_RE.sub("", raw_line.rstrip()) + if not line: + continue + + summary_match = SUMMARY_RE.search(line) + if summary_match: + config = summary_match.group("config").strip() + ensure_config(config) + failed = int(summary_match.group("failed")) + total = int(summary_match.group("total")) + passed = total - failed + summaries[config] = (passed, failed, total) + + output_match = TEST_OUTPUT_RE.search(line) + if output_match: + config = output_match.group("config").strip() + ensure_config(config) + num = output_match.group("num").strip() + text = output_match.group("text").rstrip() + if text: + test_outputs[config].setdefault(num, []).append(text) + + failed_match = FAILED_LIST_RE.search(line) + if failed_match: + config = failed_match.group("config").strip() + ensure_config(config) + num = failed_match.group("num").strip() + name = normalize_name(failed_match.group("name")) + cause = failed_match.group("cause") + cause = normalize_name(cause) if cause else "" + if num not in failures[config]: + failures[config][num] = (name, cause) + else: + prev_name, prev_cause = failures[config][num] + if not prev_cause and cause: + failures[config][num] = (name, cause) + else: + failures[config][num] = (prev_name, prev_cause) + + print(f"## Test Summary{header_suffix}:") + if not summaries: + print("No test summary lines found in the log.") + else: + print("| Configuration | Passed | Failed | Total |") + print("| --- | ---: | ---: | ---: |") + for config in order: + summary = summaries.get(config) + if summary is None: + print(f"| {config} | n/a | n/a | n/a |") + else: + passed, failed, total = summary + print(f"| {config} | {passed} | {failed} | {total} |") + + print() + print("
") + print("Failed Tests") + print() + any_failed = False + for config in order: + config_failures = failures.get(config, {}) + for num, entry in config_failures.items(): + name, cause = entry + if cause: + print(f"- [{config}] #{num} {name} - {cause}") + else: + print(f"- [{config}] #{num} {name}") + + snippet = clip_output_lines(test_outputs.get(config, {}).get(num, [])) + if snippet: + print("```text") + for snippet_line in snippet: + print(snippet_line) + print("```") + any_failed = True + if not any_failed: + print("No tests failed.") + print() + print("
") + print() + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/ci/ctest_to_junit.py b/scripts/ci/ctest_to_junit.py new file mode 100644 index 0000000000..45205e5be9 --- /dev/null +++ b/scripts/ci/ctest_to_junit.py @@ -0,0 +1,261 @@ +#!/usr/bin/env python3 +import re +import sys +from collections import OrderedDict +from pathlib import Path +from xml.etree import ElementTree as ET + + +ANSI_RE = re.compile(r"\x1b\[[0-9;]*[A-Za-z]") +SUMMARY_RE = re.compile( + r"\[(?P[^\]]+)\]\s+\d+% tests passed,\s+(?P\d+) tests failed out of (?P\d+)" +) +FAILED_LIST_RE = re.compile( + r"\[(?P[^\]]+)\]\s+(?P\d+)\s*-\s*(?P.+?)" + r"(?:\s+\((?P[^)]+)\))?\s*$" +) +TEST_OUTPUT_RE = re.compile( + r"\[(?P[^\]]+)\]\s+(?P\d+):\s*(?P.*)$" +) + +def normalize_name(name: str) -> str: + return " ".join(name.strip().split()) + + +def parse_args(argv): + if len(argv) < 3: + return None, None, "" + + log_path = Path(argv[1]) + junit_path = Path(argv[2]) + context = " ".join(argv[3:]).strip() + return log_path, junit_path, context + + +def parse_log(log_path: Path): + order = [] + summaries = {} + failures = {} + test_outputs = {} + + def ensure_config(config: str) -> None: + if config not in summaries: + summaries[config] = None + failures[config] = OrderedDict() + test_outputs[config] = {} + order.append(config) + + with log_path.open("r", errors="ignore") as handle: + for raw_line in handle: + line = ANSI_RE.sub("", raw_line.rstrip()) + if not line: + continue + + summary_match = SUMMARY_RE.search(line) + if summary_match: + config = summary_match.group("config").strip() + ensure_config(config) + failed = int(summary_match.group("failed")) + total = int(summary_match.group("total")) + passed = total - failed + summaries[config] = (passed, failed, total) + + output_match = TEST_OUTPUT_RE.search(line) + if output_match: + config = output_match.group("config").strip() + ensure_config(config) + num = output_match.group("num").strip() + text = output_match.group("text").rstrip() + if text: + test_outputs[config].setdefault(num, []).append(text) + + failed_match = FAILED_LIST_RE.search(line) + if failed_match: + config = failed_match.group("config").strip() + ensure_config(config) + num = failed_match.group("num").strip() + name = normalize_name(failed_match.group("name")) + cause = failed_match.group("cause") + cause = normalize_name(cause) if cause else "" + if num not in failures[config]: + failures[config][num] = (name, cause) + else: + prev_name, prev_cause = failures[config][num] + if not prev_cause and cause: + failures[config][num] = (name, cause) + else: + failures[config][num] = (prev_name, prev_cause) + + return order, summaries, failures, test_outputs + + +def iter_configs(order, summaries, failures): + configs = list(order) + for config in summaries: + if config not in configs: + configs.append(config) + for config in failures: + if config not in configs: + configs.append(config) + return configs + + +def build_failure_text(cause: str, output_lines): + details = [] + if cause: + details.append(f"Cause: {cause}") + clipped_lines = [line for line in output_lines if line.strip()] + if clipped_lines: + details.append("Relevant test output:") + details.extend(clipped_lines) + else: + details.append("Relevant test output: not captured.") + + return "\n".join(details) + + +def write_junit( + junit_path: Path, + context: str, + order, + summaries, + failures, + test_outputs, +): + root = ET.Element("testsuites") + all_tests = 0 + all_failures = 0 + + configs = iter_configs(order, summaries, failures) + if not configs: + empty_suite = ET.SubElement( + root, + "testsuite", + name=f"{context or 'ctest'}", + tests="1", + failures="0", + errors="0", + skipped="0", + time="0", + ) + empty_case = ET.SubElement( + empty_suite, + "testcase", + classname=f"{context or 'ctest'}", + name="summary", + time="0", + ) + system_out = ET.SubElement(empty_case, "system-out") + system_out.text = "No CTest summary lines found in the log." + all_tests = 1 + + for config in configs: + summary = summaries.get(config) + parsed_failures = failures.get(config, OrderedDict()) + + declared_failed = summary[1] if summary else len(parsed_failures) + declared_total = summary[2] if summary else len(parsed_failures) + declared_passed = summary[0] if summary else max(declared_total - declared_failed, 0) + + suite_failures = max(declared_failed, len(parsed_failures)) + suite_name = f"{context} / {config}" if context else config + # Report only emitted failed/unknown testcases in JUnit. + suite_tests = suite_failures + + suite = ET.SubElement( + root, + "testsuite", + name=suite_name, + tests=str(suite_tests), + failures=str(suite_failures), + errors="0", + skipped="0", + time="0", + ) + + props = ET.SubElement(suite, "properties") + ET.SubElement( + props, + "property", + name="ctest.summary", + value=f"Passed={declared_passed}, Failed={declared_failed}, Total={declared_total}", + ) + + for num, entry in parsed_failures.items(): + test_name, cause = entry + output_lines = test_outputs.get(config, {}).get(num, []) + case = ET.SubElement( + suite, + "testcase", + classname=suite_name, + name=f"#{num} {test_name}", + time="0", + ) + failure = ET.SubElement( + case, + "failure", + type="failure", + message=cause or "Failed", + ) + failure.text = build_failure_text( + cause=cause, + output_lines=output_lines, + ) + + missing_failures = suite_failures - len(parsed_failures) + for index in range(1, missing_failures + 1): + case = ET.SubElement( + suite, + "testcase", + classname=suite_name, + name=f"unknown failure {index}", + time="0", + ) + failure = ET.SubElement( + case, + "failure", + type="failure", + message="Unknown failure", + ) + failure.text = "CTest reported an extra failure that was not listed by name." + + all_tests += suite_tests + all_failures += suite_failures + + root.set("tests", str(all_tests)) + root.set("failures", str(all_failures)) + root.set("errors", "0") + root.set("time", "0") + + tree = ET.ElementTree(root) + if hasattr(ET, "indent"): + ET.indent(tree, space=" ") + junit_path.parent.mkdir(parents=True, exist_ok=True) + tree.write(junit_path, encoding="utf-8", xml_declaration=True) + + +def main() -> int: + log_path, junit_path, context = parse_args(sys.argv) + if log_path is None or junit_path is None: + print("Usage: ctest_to_junit.py [context]") + return 0 + + if not log_path.exists(): + print(f"Log file not found: {log_path}") + return 0 + + order, summaries, failures, test_outputs = parse_log(log_path) + write_junit( + junit_path=junit_path, + context=context, + order=order, + summaries=summaries, + failures=failures, + test_outputs=test_outputs, + ) + print(f"Wrote JUnit report: {junit_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/run_tests.py b/tests/run_tests.py index df8b7938b9..56c95e5c92 100644 --- a/tests/run_tests.py +++ b/tests/run_tests.py @@ -58,7 +58,21 @@ def run_ctest(env_vars, labels, blocking=True, name=""): env.update(env_vars) - cmd = ["ctest", "--no-tests=error", "-VV", "-L", "^ci$", "--timeout", "1000", "-C", "Release"] + cmd = [ + "ctest", + "--no-tests=error", + "-VV", + "-L", + "^ci$", + "--timeout", + "1000", + "-C", + "Release", + "--test-output-size-failed", + "500000", + "--test-output-truncation", + "tail", + ] if os.name == "nt": cmd.extend(["-LE", "^nowindows$"]) From 32a028a22cc436a3c13443c3e7cf469331f9a3d7 Mon Sep 17 00:00:00 2001 From: lnotspotl Date: Tue, 17 Feb 2026 20:34:56 +0100 Subject: [PATCH 162/180] disable camera output undistortion on cameras with outdated EEPROM data (#1654) * bump firmware hash * bump rvc2 fw hash * add undistortion tests * bump rvc2 fw commit hash * bump rvc2 fw hash * fix rvc4 tests * bump rvc2 fw hash * bump rvc2 fw hash --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- tests/CMakeLists.txt | 4 ++ .../eeprom_undistortion_test.cpp | 67 +++++++++++++++++++ 3 files changed, 72 insertions(+), 1 deletion(-) create mode 100644 tests/src/ondevice_tests/eeprom_undistortion_test.cpp diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 96e412f828..e4f3b9bd90 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "21da095007e982d95b9b77043979ccc415217609") +set(DEPTHAI_DEVICE_SIDE_COMMIT "7c1c190e45409c6e3ade394b31e33859f9cea725") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f5d64893f5..a0f0827182 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -646,6 +646,10 @@ endif() dai_add_test(img_transformation_test src/ondevice_tests/img_transformation_test.cpp CXX_STANDARD 17) dai_set_test_labels(img_transformation_test ondevice rvc2_all rvc4 ci) +# EEPROM undistortion test (RVC2 only) +dai_add_test(eeprom_undistortion_test src/ondevice_tests/eeprom_undistortion_test.cpp) +dai_set_test_labels(eeprom_undistortion_test ondevice rvc2_all ci) + # Regression tests for encountered bugs dai_add_test(regression_camera_concurrency_test src/ondevice_tests/regression/camera_concurrency.cpp) dai_set_test_labels(regression_camera_concurrency_test ondevice rvc4 ci) diff --git a/tests/src/ondevice_tests/eeprom_undistortion_test.cpp b/tests/src/ondevice_tests/eeprom_undistortion_test.cpp new file mode 100644 index 0000000000..bccf98b9d5 --- /dev/null +++ b/tests/src/ondevice_tests/eeprom_undistortion_test.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include + +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/node/Camera.hpp" + +// ----------------------------------------------------------------------------- +// Undistortion disabled on old EEPROM (stereoEnableDistortionCorrection=false) +// Purpose: +// Ensures that when a device has an older EEPROM format +// (stereoEnableDistortionCorrection == false), requesting camera output with +// enableUndistortion=true does NOT produce distortion coefficients in the +// undistortion request when the EEPROM format is old. +// ----------------------------------------------------------------------------- +TEST_CASE("Undistortion disabled on old EEPROM") { + dai::Pipeline pipeline; + + // Read the device's current calibration and force old EEPROM format + auto device = pipeline.getDefaultDevice(); + auto calib = device->readCalibration(); + auto eeprom = calib.getEepromData(); + bool newEEPROM = false; + + auto socket = dai::CameraBoardSocket::CAM_B; + + // Force <= 85 degrees FOV + eeprom.cameraData[socket].intrinsicMatrix[0][0] = 1000000.0f; // huge focal length <=> small fov + eeprom.cameraData[socket].width = 640; + eeprom.cameraData[socket].distortionCoeff = {3.0f, 1.0f, 4.0f, 1.0f, 5.0f, 9.0f, 2.0f, 6.0f, 5.0f, 3.0f, 5.0f, 8.0f, 9.0f, 7.0f}; + + SECTION("stereoEnableDistortionCorrection = false (old EEPROM format)") { + newEEPROM = false; + } + + SECTION("stereoEnableDistortionCorrection = true (new EEPROM format)") { + newEEPROM = true; + } + + eeprom.stereoEnableDistortionCorrection = newEEPROM; + device->setCalibration(dai::CalibrationHandler(eeprom)); + + auto camera = pipeline.create()->build(socket); + + // Request output with enableUndistortion explicitly set to true + auto* output = camera->requestOutput({640, 400}, dai::ImgFrame::Type::NV12, dai::ImgResizeMode::CROP, std::nullopt, true); + REQUIRE(output != nullptr); + auto queue = output->createOutputQueue(); + + pipeline.start(); + auto frame = queue->get(); + REQUIRE(frame != nullptr); + pipeline.stop(); + + auto distCoeffs = frame->transformation.getDistortionCoefficients(); + + if(newEEPROM) { + // New EEPROM: firmware applies undistortion and then zeros out the coefficients + bool allZero = distCoeffs.empty() || std::all_of(distCoeffs.begin(), distCoeffs.end(), [](float v) { return v == 0.0f; }); + REQUIRE(allZero); + } else { + // Old EEPROM: firmware skips undistortion, distortion coefficients are passed through + bool someNonZero = !distCoeffs.empty() && std::any_of(distCoeffs.begin(), distCoeffs.end(), [](float v) { return v != 0.0f; }); + REQUIRE(someNonZero); + } +} From fac5e294ac985752207378ae8c10b9bd758fa601 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Thu, 19 Feb 2026 09:15:44 +0100 Subject: [PATCH 163/180] Add isp example to testing framework --- examples/python/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/python/CMakeLists.txt b/examples/python/CMakeLists.txt index 6582d78772..61f3c0c1b1 100644 --- a/examples/python/CMakeLists.txt +++ b/examples/python/CMakeLists.txt @@ -372,3 +372,7 @@ endif() ## NeuralAssistedStereo add_python_example(neural_assisted_stereo NeuralAssistedStereo/neural_assisted_stereo.py) dai_set_example_test_labels(neural_assisted_stereo ondevice rvc4 ci) + +# Ips output +add_python_example(camera_isp Camera/camera_isp.py) +dai_set_example_test_labels(camera_isp rvc2_all rvc4 ci) From e30b55feb8a17e861d77381b89d1e18c60ce6def Mon Sep 17 00:00:00 2001 From: Danilo Pejovic <115164734+danilo-pejovic@users.noreply.github.com> Date: Fri, 20 Feb 2026 19:36:24 +0100 Subject: [PATCH 164/180] Change flavor from 'asan-ubsan' to 'vanilla' (#1682) * Change flavor from 'asan-ubsan' to 'vanilla' * bump os version --------- Co-authored-by: Danilo --- .github/workflows/test.workflow.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/test.workflow.yml b/.github/workflows/test.workflow.yml index c4a9930be2..7f32549b0e 100644 --- a/.github/workflows/test.workflow.yml +++ b/.github/workflows/test.workflow.yml @@ -54,7 +54,7 @@ jobs: uses: ./.github/workflows/test_child.yml with: flavor: "vanilla" - luxonis_os_versions_to_test: "['1.14.1', '1.18.3', '1.23.1']" + luxonis_os_versions_to_test: "['1.14.1', '1.18.3', '1.25.3']" luxonis_os_versions_to_test_rgb: "['1.23.1']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} @@ -66,7 +66,7 @@ jobs: uses: ./.github/workflows/test_child.yml with: flavor: "tsan" - luxonis_os_versions_to_test: "['1.23.1']" + luxonis_os_versions_to_test: "['1.25.3']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} @@ -77,7 +77,7 @@ jobs: uses: ./.github/workflows/test_child.yml with: flavor: "asan-ubsan" - luxonis_os_versions_to_test: "['1.23.1']" + luxonis_os_versions_to_test: "['1.25.3']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} @@ -87,7 +87,7 @@ jobs: if: needs.precheck.outputs.should_run == 'true' uses: ./.github/workflows/test_child_windows.yml with: - luxonis_os_versions_to_test: "['1.23.1']" + luxonis_os_versions_to_test: "['1.25.3']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} @@ -97,7 +97,7 @@ jobs: if: needs.precheck.outputs.should_run == 'true' uses: ./.github/workflows/test_child_mac.yml with: - flavor: "asan-ubsan" - luxonis_os_versions_to_test: "['1.23.1']" + flavor: "vanilla" + luxonis_os_versions_to_test: "['1.25.3']" secrets: HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} From 3aea27d6419d56aa0978f61c3d2d193cbc3a927c Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 23 Feb 2026 11:09:43 +0100 Subject: [PATCH 165/180] Bump the OS for RGB testing --- .github/workflows/test.workflow.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.workflow.yml b/.github/workflows/test.workflow.yml index 7f32549b0e..41a02c1da4 100644 --- a/.github/workflows/test.workflow.yml +++ b/.github/workflows/test.workflow.yml @@ -55,7 +55,7 @@ jobs: with: flavor: "vanilla" luxonis_os_versions_to_test: "['1.14.1', '1.18.3', '1.25.3']" - luxonis_os_versions_to_test_rgb: "['1.23.1']" + luxonis_os_versions_to_test_rgb: "['1.25.3']" secrets: CONTAINER_REGISTRY: ${{ secrets.CONTAINER_REGISTRY }} HIL_PAT_TOKEN: ${{ secrets.HIL_PAT_TOKEN }} From e59dc48bbd41016bd2dec334c02050b1e9732459 Mon Sep 17 00:00:00 2001 From: asahtik <38485424+asahtik@users.noreply.github.com> Date: Tue, 24 Feb 2026 10:58:24 +0100 Subject: [PATCH 166/180] Include node groups in full pipeline schema (#1688) * Include node groups in the full pipeline schema * Use if else instead of try catch --- src/pipeline/Pipeline.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 258488364e..743f70ea55 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -217,10 +217,6 @@ PipelineSchema PipelineImpl::getPipelineSchema(SerializationType type, bool incl // Loop over all nodes, and add them to schema for(const auto& node : getAllNodes()) { - // const auto& node = kv.second; - if(std::string(node->getName()) == std::string("NodeGroup") || std::string(node->getName()) == std::string("DeviceNodeGroup")) { - continue; - } if(!includePipelineDebugging && std::find(pipelineDebuggingNodeIds.begin(), pipelineDebuggingNodeIds.end(), node->id) != pipelineDebuggingNodeIds.end()) { continue; @@ -240,7 +236,11 @@ PipelineSchema PipelineImpl::getPipelineSchema(SerializationType type, bool incl } if(deviceNode) { deviceNode->getProperties().serialize(info.properties, type); - info.logLevel = deviceNode->getLogLevel(); + if(std::string(deviceNode->getName()) == "DeviceNodeGroup") { + info.logLevel = LogLevel::OFF; + } else { + info.logLevel = deviceNode->getLogLevel(); + } } // Create Io information auto inputs = node->getInputs(); @@ -372,9 +372,9 @@ PipelineSchema PipelineImpl::getDevicePipelineSchema(SerializationType type, boo auto schema = getPipelineSchema(type, includePipelineDebugging); // Remove bridge info schema.bridges.clear(); - // Remove host nodes + // Remove host and group nodes for(auto it = schema.nodes.begin(); it != schema.nodes.end();) { - if(!it->second.deviceNode) { + if(!it->second.deviceNode || it->second.name == "NodeGroup" || it->second.name == "DeviceNodeGroup") { it = schema.nodes.erase(it); } else { ++it; From e5df723401c2bf7a697fd5e2f1d068333b858ff1 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 09:51:36 +0100 Subject: [PATCH 167/180] Add comments to basic script Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync.py | 148 +++++++++++------- 1 file changed, 95 insertions(+), 53 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index c17565f8e4..10336346c6 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -6,16 +6,17 @@ import cv2 import depthai as dai import time -import math import argparse import signal -import numpy as np -from typing import Optional, Dict, List -# --------------------------------------------------------------------------- -# Configuration -# --------------------------------------------------------------------------- +from typing import Optional, Dict + +# This example shows how to use the external frame sync node to synchronize multiple devices +# This example assumes that all devices are connected to the same host +# The script identifies the master device and slave devices and then starts the master first +# This is necessary because otherwise slave devices will not be able to detect the master FSYNC signal + SET_MANUAL_EXPOSURE = False # Set to True to use manual exposure settings # --------------------------------------------------------------------------- # Helpers @@ -35,71 +36,95 @@ def getFps(self): # Calculate the FPS return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) - # --------------------------------------------------------------------------- -# Pipeline creation (unchanged API – only uses TARGET_FPS constant) +# Create camera outputs # --------------------------------------------------------------------------- -def createPipeline(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, sensorFps: float, role: dai.ExternalFrameSyncRole): +def createCameraOutputs(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, sensorFps: float, role: dai.ExternalFrameSyncRole): cam = None + + # Only specify FPS if camera is master if role == dai.ExternalFrameSyncRole.MASTER: cam = ( pipeline.create(dai.node.Camera) .build(socket, sensorFps=sensorFps) ) + + # Slave cameras will lock to the master's FPS else: cam = ( pipeline.create(dai.node.Camera) .build(socket) ) - output = ( - cam.requestOutput( - (640, 480), dai.ImgFrame.Type.NV12, dai.ImgResizeMode.STRETCH + if socket == dai.CameraBoardSocket.CAM_A: + output = ( + cam.requestOutput( + (1920, 1080), dai.ImgFrame.Type.NV12, dai.ImgResizeMode.CROP + ) + ) + else: + output = ( + cam.requestFullResolutionOutput() ) - ) return pipeline, output +# --------------------------------------------------------------------------- +# Create synchronization node +# --------------------------------------------------------------------------- def createSyncNode(syncThreshold: datetime.timedelta): global masterPipeline, masterNode, slaveQueues, inputQueues, outputNames + sync = masterPipeline.create(dai.node.Sync) + # Sync node will run on the host, since it needs to sync multiple devices sync.setRunOnHost(True) sync.setSyncThreshold(syncThreshold) - for k, v in masterNode.items(): - name = f"master_{k}" - v.link(sync.inputs[name]) + + # Link master camera outputs to the sync node + for socketName, camOutput in masterNode.items(): + name = f"master_{socketName}" + camOutput.link(sync.inputs[name]) outputNames.append(name) - for k, v in slaveQueues.items(): - for k2, v2 in v.items(): - name = f"slave_{k}_{k2}" + # For slaves, we must create an input queue for each output + # We will then manually forward the frames from each input queue to the output queue + # This is because slave devices have separate pipelines from the master + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + name = f"slave_{deviceName}_{socketName}" outputNames.append(name) input_queue = sync.inputs[name].createInputQueue() inputQueues[name] = input_queue return sync +# --------------------------------------------------------------------------- +# Set up for individual camera sockets +# --------------------------------------------------------------------------- def setUpCameraSocket( pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, - name: str, + deviceName: str, targetFps: float, role: dai.ExternalFrameSyncRole): global masterNode, slaveQueues, camSockets - pipeline, outNode = createPipeline(pipeline, socket, targetFps, role) + pipeline, outNode = createCameraOutputs(pipeline, socket, targetFps, role) + + # Master cameras will be linked to the sync node directly if role == dai.ExternalFrameSyncRole.MASTER: if masterNode is None: masterNode = {} - masterNode[socket.name] = outNode + + # Gather all slave camera outputs elif role == dai.ExternalFrameSyncRole.SLAVE: - if slaveQueues.get(name) is None: - slaveQueues[name] = {} - - slaveQueues[name][socket.name] = outNode.createOutputQueue() + if slaveQueues.get(deviceName) is None: + slaveQueues[deviceName] = {} + slaveQueues[deviceName][socket.name] = outNode.createOutputQueue() else: raise RuntimeError(f"Don't know how to handle role {role}") + # Keep track of all camera socket names if socket.name not in camSockets: camSockets.append(socket.name) @@ -108,9 +133,10 @@ def setUpCameraSocket( def setupDevice( stack: contextlib.ExitStack, deviceInfo: dai.DeviceInfo, - sockets: Optional[List[str]], targetFps: float): global masterPipeline, masterNode, slavePipelines, slaveQueues, camSockets + + # Create pipeline for device pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) device = pipeline.getDefaultDevice() @@ -125,8 +151,6 @@ def setupDevice( print(" Num of cameras:", len(device.getConnectedCameras())) for socket in device.getConnectedCameras(): - if sockets is not None and socket.name not in sockets: - continue pipeline = setUpCameraSocket(pipeline, socket, name, targetFps, role) if role == dai.ExternalFrameSyncRole.MASTER: @@ -143,7 +167,6 @@ def setupDevice( else: raise RuntimeError(f"Don't know how to handle role {role}") - running = True def interruptHandler(sig, frame): @@ -160,49 +183,45 @@ def interruptHandler(sig, frame): parser = argparse.ArgumentParser(add_help=False) parser.add_argument("-f", "--fps", type=float, default=30.0, help="Target FPS", required=False) parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs", required=False) -parser.add_argument("--sockets", type=str, help="Socket ids to sync, comma separated", required=False) parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) -parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=3e-3, help="Sync threshold in seconds", required=False) +parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) args = parser.parse_args() +# if user did not specify device IPs, use all available devices if len(args.devices) == 0: deviceInfos = dai.Device.getAllAvailableDevices() else: - deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] #The master camera needs to be first here - + deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] assert len(deviceInfos) > 1, "At least two devices are required for this example." -targetFps = args.fps # Must match sensorFps in createPipeline() - -if args.sockets: - sockets = args.sockets.split(",") -else: - sockets = None - +targetFps = args.fps recvAllTimeoutSec = args.recv_all_timeout_sec - syncThresholdSec = args.sync_threshold_sec initialSyncTimeoutSec = args.initial_sync_timeout_sec + # --------------------------------------------------------------------------- # Main # --------------------------------------------------------------------------- with contextlib.ExitStack() as stack: + # Variables to keep track of master and slave pipelines and outputs masterPipeline: Optional[dai.Pipeline] = None masterNode: Optional[Dict[str, dai.Node.Output]] = None - slavePipelines: Dict[str, dai.Pipeline] = {} slaveQueues: Dict[str, Dict[str, dai.MessageQueue]] = {} + # keep track of sync node inputs for slaves inputQueues = {} + # keep track of all sync node output names outputNames = [] + # keep track of all camera socket names camSockets = [] for idx, deviceInfo in enumerate(deviceInfos): - setupDevice(stack, deviceInfo, sockets, targetFps) + setupDevice(stack, deviceInfo, targetFps) if masterPipeline is None or masterNode is None: raise RuntimeError("No master detected!") @@ -210,12 +229,16 @@ def interruptHandler(sig, frame): if len(slavePipelines) < 1: raise RuntimeError("No slaves detected!") + # Create sync node + # Sync node groups the frames so that all synced frames are timestamped to within one frame time sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() + # Start pipelines + # The master pipeline will be started first, then the slave pipelines masterPipeline.start() - for k, v in slavePipelines.items(): - v.start() + for k, sockets in slavePipelines.items(): + sockets.start() fpsCounter = FPSCounter() @@ -228,12 +251,13 @@ def interruptHandler(sig, frame): waitingForSync = True while running: + # Send frames from slave output queues to sync node input queues + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + while camOutputQueue.has(): + inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) - for k, v in slaveQueues.items(): - for k2, v2 in v.items(): - while v2.has(): - inputQueues[f"slave_{k}_{k2}"].send(v2.get()) - + # Get frames from sync node output queue while queue.has(): latestFrameGroup = queue.get() if not firstReceived: @@ -242,6 +266,7 @@ def interruptHandler(sig, frame): prevReceived = datetime.datetime.now() fpsCounter.tick() + # Timeout if we dont receive any frames at the beginning if not firstReceived: endTime = datetime.datetime.now() elapsedSec = (endTime - startTime).total_seconds() @@ -251,19 +276,25 @@ def interruptHandler(sig, frame): # ------------------------------------------------------------------- # Synchronise: we need at least one frame from every camera and their - # timestamps must align within SYNC_THRESHOLD_SEC. + # timestamps must align within syncThresholdSec. # ------------------------------------------------------------------- if latestFrameGroup is not None and latestFrameGroup.getNumMessages() == len(outputNames): + + # Get timestamps for each frame tsValues = {} for name in outputNames: tsValues[name] = latestFrameGroup[name].getTimestamp(dai.CameraExposureOffset.END).total_seconds() - # Build composite image side‑by‑side + + # Build individual image arrays for each camera socket imgs = [] for name in camSockets: imgs.append([]) fps = fpsCounter.getFps() + # Create a image frame with sync info for each output for outputName in outputNames: + + # Find out which camera socket this output belongs to idx = -1 for i, name in enumerate(camSockets): if name in outputName: @@ -272,8 +303,11 @@ def interruptHandler(sig, frame): if idx == -1: raise RuntimeError(f"Could not find camera socket for {outputName}") + # Get frame for this output msg = latestFrameGroup[outputName] frame = msg.getCvFrame() + + # Add output name to frame cv2.putText( frame, f"{outputName}", @@ -284,6 +318,8 @@ def interruptHandler(sig, frame): 2, cv2.LINE_AA, ) + + # Add timestamp and FPS to frame cv2.putText( frame, f"Timestamp: {tsValues[outputName]} | FPS:{fps:.2f}", @@ -294,13 +330,16 @@ def interruptHandler(sig, frame): 2, cv2.LINE_AA, ) + imgs[idx].append(frame) + # calculate the greatest time difference between all frames delta = max(tsValues.values()) - min(tsValues.values()) syncStatus = abs(delta) < syncThresholdSec syncStatusStr = "in sync" if syncStatus else "out of sync" + # Timeout if frames don't get synced in time if not syncStatus and waitingForSync: endTime = datetime.datetime.now() elapsedSec = (endTime - initialSyncTime).total_seconds() @@ -312,12 +351,14 @@ def interruptHandler(sig, frame): print(f"Sync status: {syncStatusStr}") waitingForSync = False + # Exit if sync is lost if not syncStatus and not waitingForSync: print(f"Sync error: Sync lost, threshold exceeded {delta * 1e6} us") running = False color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + # Add sync status and delta to the frame for i, img in enumerate(imgs): cv2.putText( imgs[i][0], @@ -330,6 +371,7 @@ def interruptHandler(sig, frame): cv2.LINE_AA, ) + # Show the frame for i, img in enumerate(imgs): cv2.imshow(f"synced_view_{camSockets[i]}", cv2.hconcat(imgs[i])) From aecdfb97925ef1ab5169699ee86e7023b9926609 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 09:57:33 +0100 Subject: [PATCH 168/180] Add warnings regarding loss of sync Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync.py | 53 ++++++++++--------- 1 file changed, 27 insertions(+), 26 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index 10336346c6..3caf1a5152 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -291,6 +291,32 @@ def interruptHandler(sig, frame): imgs.append([]) fps = fpsCounter.getFps() + # calculate the greatest time difference between all frames + delta = max(tsValues.values()) - min(tsValues.values()) + + syncStatus = abs(delta) < syncThresholdSec + syncStatusStr = "in sync" if syncStatus else "out of sync" + + # Timeout if frames don't get synced in time + if not syncStatus and waitingForSync: + endTime = datetime.datetime.now() + elapsedSec = (endTime - initialSyncTime).total_seconds() + if elapsedSec >= initialSyncTimeoutSec: + print("Timeout: Didn't sync frames in time") + running = False + + if syncStatus and waitingForSync: + print(f"Sync status: {syncStatusStr}") + waitingForSync = False + + # Print warning if delta is too big + if not syncStatus and not waitingForSync: + print(f"Sync error: Sync lost, threshold exceeded {delta * 1e6} us") + print("Either the signal is lost or the network is congested.") + continue + + color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + # Create a image frame with sync info for each output for outputName in outputNames: @@ -332,33 +358,8 @@ def interruptHandler(sig, frame): ) imgs[idx].append(frame) - - # calculate the greatest time difference between all frames - delta = max(tsValues.values()) - min(tsValues.values()) - - syncStatus = abs(delta) < syncThresholdSec - syncStatusStr = "in sync" if syncStatus else "out of sync" - - # Timeout if frames don't get synced in time - if not syncStatus and waitingForSync: - endTime = datetime.datetime.now() - elapsedSec = (endTime - initialSyncTime).total_seconds() - if elapsedSec >= initialSyncTimeoutSec: - print("Timeout: Didn't sync frames in time") - running = False - - if syncStatus and waitingForSync: - print(f"Sync status: {syncStatusStr}") - waitingForSync = False - - # Exit if sync is lost - if not syncStatus and not waitingForSync: - print(f"Sync error: Sync lost, threshold exceeded {delta * 1e6} us") - running = False - - color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) - # Add sync status and delta to the frame + # Add sync status and delta to the frame for each camera socket for i, img in enumerate(imgs): cv2.putText( imgs[i][0], From 438cc3c5f060ede6eeb63666b167eecb386bdbac Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 10:28:46 +0100 Subject: [PATCH 169/180] Allow the script to be run without master Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync.py | 47 ++++++++++++++----- 1 file changed, 36 insertions(+), 11 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index 3caf1a5152..601a68c255 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -72,18 +72,24 @@ def createCameraOutputs(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, s # Create synchronization node # --------------------------------------------------------------------------- def createSyncNode(syncThreshold: datetime.timedelta): - global masterPipeline, masterNode, slaveQueues, inputQueues, outputNames + global masterPipeline, masterNode, slaveQueues, inputQueues, outputNames, firstSlaveName, slavePipelines + + # If no master is specified, create a sync node on the first slave pipeline + if firstSlaveName is None: + sync = masterPipeline.create(dai.node.Sync) + else: + sync = slavePipelines[firstSlaveName].create(dai.node.Sync) - sync = masterPipeline.create(dai.node.Sync) # Sync node will run on the host, since it needs to sync multiple devices sync.setRunOnHost(True) sync.setSyncThreshold(syncThreshold) # Link master camera outputs to the sync node - for socketName, camOutput in masterNode.items(): - name = f"master_{socketName}" - camOutput.link(sync.inputs[name]) - outputNames.append(name) + if not noMaster: + for socketName, camOutput in masterNode.items(): + name = f"master_{socketName}" + camOutput.link(sync.inputs[name]) + outputNames.append(name) # For slaves, we must create an input queue for each output # We will then manually forward the frames from each input queue to the output queue @@ -112,6 +118,9 @@ def setUpCameraSocket( # Master cameras will be linked to the sync node directly if role == dai.ExternalFrameSyncRole.MASTER: + if noMaster: + raise RuntimeError("No master specified, but master device detected") + if masterNode is None: masterNode = {} masterNode[socket.name] = outNode @@ -134,7 +143,7 @@ def setupDevice( stack: contextlib.ExitStack, deviceInfo: dai.DeviceInfo, targetFps: float): - global masterPipeline, masterNode, slavePipelines, slaveQueues, camSockets + global masterPipeline, slavePipelines, slaveQueues, camSockets, noMaster, firstSlaveName # Create pipeline for device pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) @@ -143,7 +152,7 @@ def setupDevice( if device.getPlatform() != dai.Platform.RVC4: raise RuntimeError("This example supports only RVC4 platform!") - name = deviceInfo.getXLinkDeviceDesc().name + name = deviceInfo.getXLinkDeviceDesc().mxid role = device.getExternalFrameSyncRole() print("=== Connected to", deviceInfo.getDeviceId()) @@ -154,6 +163,9 @@ def setupDevice( pipeline = setUpCameraSocket(pipeline, socket, name, targetFps, role) if role == dai.ExternalFrameSyncRole.MASTER: + if noMaster: + raise RuntimeError("No master specified, but master device detected") + device.setExternalStrobeEnable(True) print(f"{device.getDeviceId()} is master") @@ -164,6 +176,9 @@ def setupDevice( elif role == dai.ExternalFrameSyncRole.SLAVE: slavePipelines[name] = pipeline print(f"{device.getDeviceId()} is slave") + + if firstSlaveName is None and noMaster: + firstSlaveName = name else: raise RuntimeError(f"Don't know how to handle role {role}") @@ -186,6 +201,7 @@ def interruptHandler(sig, frame): parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) +parser.add_argument("-nm", "--no-master", action="store_true", help="Run the script without a master device", required=False) args = parser.parse_args() # if user did not specify device IPs, use all available devices @@ -200,6 +216,9 @@ def interruptHandler(sig, frame): syncThresholdSec = args.sync_threshold_sec initialSyncTimeoutSec = args.initial_sync_timeout_sec +noMaster = False +if args.no_master: + noMaster = True # --------------------------------------------------------------------------- # Main # --------------------------------------------------------------------------- @@ -220,11 +239,15 @@ def interruptHandler(sig, frame): # keep track of all camera socket names camSockets = [] + # keep track of the first slave name if no master is specified + firstSlaveName: Optional[str] = None + for idx, deviceInfo in enumerate(deviceInfos): setupDevice(stack, deviceInfo, targetFps) - if masterPipeline is None or masterNode is None: - raise RuntimeError("No master detected!") + if not noMaster: + if masterPipeline is None or masterNode is None: + raise RuntimeError("No master detected!") if len(slavePipelines) < 1: raise RuntimeError("No slaves detected!") @@ -236,7 +259,9 @@ def interruptHandler(sig, frame): # Start pipelines # The master pipeline will be started first, then the slave pipelines - masterPipeline.start() + if not noMaster: + masterPipeline.start() + for k, sockets in slavePipelines.items(): sockets.start() From 8812922a3cba9ce75bdd4ade708e6d8aae864527 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 10:31:35 +0100 Subject: [PATCH 170/180] Fix help Signed-off-by: stas.bucik --- .../python/Misc/MultiDevice/multi_device_external_frame_sync.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index 601a68c255..1cdfc2386d 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -197,7 +197,7 @@ def interruptHandler(sig, frame): parser = argparse.ArgumentParser(add_help=False) parser.add_argument("-f", "--fps", type=float, default=30.0, help="Target FPS", required=False) -parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs", required=False) +parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs or IDs", required=False) parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) From 4bd5854fa0c7d5f483a45dc9d9b34b5afa2fc217 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 10:55:39 +0100 Subject: [PATCH 171/180] Add Bayer stereo example Signed-off-by: stas.bucik --- ...multi_device_external_frame_sync_stereo.py | 450 ++++++++++++++++++ 1 file changed, 450 insertions(+) create mode 100644 examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py new file mode 100644 index 0000000000..e455b26220 --- /dev/null +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py @@ -0,0 +1,450 @@ +#!/usr/bin/env python3 + +import contextlib +import datetime + +import cv2 +import depthai as dai +import time + +import argparse +import signal +import numpy as np + +from typing import Optional, Dict + +# This example shows how to use the external frame sync node to synchronize multiple devices +# This example assumes that all devices are connected to the same host +# The script identifies the master device and slave devices and then starts the master first +# This is necessary because otherwise slave devices will not be able to detect the master FSYNC signal + +SET_MANUAL_EXPOSURE = False # Set to True to use manual exposure settings + +STEREO_SOCKET = "STEREO" +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + +# --------------------------------------------------------------------------- +# Create camera outputs +# --------------------------------------------------------------------------- +def createCameraOutputs(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, sensorFps: float, role: dai.ExternalFrameSyncRole): + cam = None + + # Only specify FPS if camera is master + if role == dai.ExternalFrameSyncRole.MASTER: + cam = ( + pipeline.create(dai.node.Camera) + .build(socket, sensorFps=sensorFps) + ) + + # Slave cameras will lock to the master's FPS + else: + cam = ( + pipeline.create(dai.node.Camera) + .build(socket) + ) + + if socket == dai.CameraBoardSocket.CAM_A: + output = ( + cam.requestOutput( + (1920, 1080), dai.ImgFrame.Type.NV12, dai.ImgResizeMode.CROP + ) + ) + else: + output = ( + cam.requestFullResolutionOutput() + ) + return pipeline, output + +# --------------------------------------------------------------------------- +# Create synchronization node +# --------------------------------------------------------------------------- +def createSyncNode(syncThreshold: datetime.timedelta): + global masterPipeline, masterNode, slaveQueues, inputQueues, outputNames, firstSlaveName, slavePipelines + + # If no master is specified, create a sync node on the first slave pipeline + if firstSlaveName is None: + sync = masterPipeline.create(dai.node.Sync) + else: + sync = slavePipelines[firstSlaveName].create(dai.node.Sync) + + # Sync node will run on the host, since it needs to sync multiple devices + sync.setRunOnHost(True) + sync.setSyncThreshold(syncThreshold) + + # Link master camera outputs to the sync node + if not noMaster: + for socketName, camOutput in masterNode.items(): + name = f"master_{socketName}" + camOutput.link(sync.inputs[name]) + outputNames.append(name) + + # For slaves, we must create an input queue for each output + # We will then manually forward the frames from each input queue to the output queue + # This is because slave devices have separate pipelines from the master + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + name = f"slave_{deviceName}_{socketName}" + outputNames.append(name) + input_queue = sync.inputs[name].createInputQueue() + inputQueues[name] = input_queue + + return sync + +# --------------------------------------------------------------------------- +# Set up for individual camera sockets +# --------------------------------------------------------------------------- +def setUpCameraSocket( + pipeline: dai.Pipeline, + socket: dai.CameraBoardSocket, + deviceName: str, + targetFps: float, + role: dai.ExternalFrameSyncRole): + global masterNode, slaveQueues, camSockets + + pipeline, outNode = createCameraOutputs(pipeline, socket, targetFps, role) + + # Master cameras will be linked to the sync node directly + if role == dai.ExternalFrameSyncRole.MASTER: + if noMaster: + raise RuntimeError("No master specified, but master device detected") + + if masterNode is None: + masterNode = {} + masterNode[socket.name] = outNode + + # Gather all slave camera outputs + elif role == dai.ExternalFrameSyncRole.SLAVE: + if slaveQueues.get(deviceName) is None: + slaveQueues[deviceName] = {} + slaveQueues[deviceName][socket.name] = outNode.createOutputQueue() + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + # Keep track of all camera socket names + if socket.name not in camSockets: + camSockets.append(socket.name) + + return pipeline, outNode + +def setupDevice( + stack: contextlib.ExitStack, + deviceInfo: dai.DeviceInfo, + targetFps: float): + global masterPipeline, slavePipelines, slaveQueues, camSockets, noMaster, firstSlaveName, masterNode + + # Create pipeline for device + pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) + device = pipeline.getDefaultDevice() + + if device.getPlatform() != dai.Platform.RVC4: + raise RuntimeError("This example supports only RVC4 platform!") + + name = deviceInfo.getXLinkDeviceDesc().mxid + role = device.getExternalFrameSyncRole() + + print("=== Connected to", deviceInfo.getDeviceId()) + print(" Device ID:", device.getDeviceId()) + print(" Num of cameras:", len(device.getConnectedCameras())) + + # create stereo node + stereo = pipeline.create(dai.node.StereoDepth) + + for socket in device.getConnectedCameras(): + pipeline, outNode = setUpCameraSocket(pipeline, socket, name, targetFps, role) + + # link stereo node + if socket == dai.CameraBoardSocket.CAM_B: + outNode.link(stereo.left) + elif socket == dai.CameraBoardSocket.CAM_C: + outNode.link(stereo.right) + + stereo.setRectification(True) + stereo.setExtendedDisparity(True) + stereo.setLeftRightCheck(True) + + if role == dai.ExternalFrameSyncRole.MASTER: + if noMaster: + raise RuntimeError("No master specified, but master device detected") + + device.setExternalStrobeEnable(True) + print(f"{device.getDeviceId()} is master") + + if masterPipeline is not None: + raise RuntimeError("Only one master pipeline is supported") + + masterPipeline = pipeline + + if masterNode is None: + masterNode = {} + + masterNode[STEREO_SOCKET] = stereo.disparity + elif role == dai.ExternalFrameSyncRole.SLAVE: + slavePipelines[name] = pipeline + print(f"{device.getDeviceId()} is slave") + + if slaveQueues.get(name) is None: + slaveQueues[name] = {} + + slaveQueues[name][STEREO_SOCKET] = stereo.disparity.createOutputQueue() + + if firstSlaveName is None and noMaster: + firstSlaveName = name + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + if STEREO_SOCKET not in camSockets: + camSockets.append(STEREO_SOCKET) + +running = True + +def interruptHandler(sig, frame): + global running + if running: + print("Interrupted! Exiting...") + running = False + else: + print("Exiting now!") + exit(0) + +signal.signal(signal.SIGINT, interruptHandler) + +parser = argparse.ArgumentParser(add_help=False) +parser.add_argument("-f", "--fps", type=float, default=30.0, help="Target FPS", required=False) +parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs or IDs", required=False) +parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) +parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) +parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) +parser.add_argument("-nm", "--no-master", action="store_true", help="Run the script without a master device", required=False) +args = parser.parse_args() + +# if user did not specify device IPs, use all available devices +if len(args.devices) == 0: + deviceInfos = dai.Device.getAllAvailableDevices() +else: + deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] +assert len(deviceInfos) > 1, "At least two devices are required for this example." + +targetFps = args.fps +recvAllTimeoutSec = args.recv_all_timeout_sec +syncThresholdSec = args.sync_threshold_sec +initialSyncTimeoutSec = args.initial_sync_timeout_sec + +noMaster = False +if args.no_master: + noMaster = True +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- + +colorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) +colorMap[0] = [0, 0, 0] # to make zero-disparity pixels black +maxDisparity = 1 + +with contextlib.ExitStack() as stack: + + # Variables to keep track of master and slave pipelines and outputs + masterPipeline: Optional[dai.Pipeline] = None + masterNode: Optional[Dict[str, dai.Node.Output]] = None + slavePipelines: Dict[str, dai.Pipeline] = {} + slaveQueues: Dict[str, Dict[str, dai.MessageQueue]] = {} + + # keep track of sync node inputs for slaves + inputQueues = {} + + # keep track of all sync node output names + outputNames = [] + + # keep track of all camera socket names + camSockets = [] + + # keep track of the first slave name if no master is specified + firstSlaveName: Optional[str] = None + + for idx, deviceInfo in enumerate(deviceInfos): + setupDevice(stack, deviceInfo, targetFps) + + if not noMaster: + if masterPipeline is None or masterNode is None: + raise RuntimeError("No master detected!") + + if len(slavePipelines) < 1: + raise RuntimeError("No slaves detected!") + + # Create sync node + # Sync node groups the frames so that all synced frames are timestamped to within one frame time + sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) + queue = sync.out.createOutputQueue() + + # Start pipelines + # The master pipeline will be started first, then the slave pipelines + if not noMaster: + masterPipeline.start() + + for k, sockets in slavePipelines.items(): + sockets.start() + + fpsCounter = FPSCounter() + + latestFrameGroup = None + firstReceived = False + startTime = datetime.datetime.now() + prevReceived = datetime.datetime.now() + + initialSyncTime = None + waitingForSync = True + + while running: + # Send frames from slave output queues to sync node input queues + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + while camOutputQueue.has(): + inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + + # Get frames from sync node output queue + while queue.has(): + latestFrameGroup = queue.get() + if not firstReceived: + firstReceived = True + initialSyncTime = datetime.datetime.now() + prevReceived = datetime.datetime.now() + fpsCounter.tick() + + # Timeout if we dont receive any frames at the beginning + if not firstReceived: + endTime = datetime.datetime.now() + elapsedSec = (endTime - startTime).total_seconds() + if elapsedSec >= recvAllTimeoutSec: + print(f"Timeout: Didn't receive all frames in time: {elapsedSec:.2f} sec") + running = False + + # ------------------------------------------------------------------- + # Synchronise: we need at least one frame from every camera and their + # timestamps must align within syncThresholdSec. + # ------------------------------------------------------------------- + if latestFrameGroup is not None and latestFrameGroup.getNumMessages() == len(outputNames): + + # Get timestamps for each frame + tsValues = {} + for name in outputNames: + tsValues[name] = latestFrameGroup[name].getTimestamp(dai.CameraExposureOffset.END).total_seconds() + + # Build individual image arrays for each camera socket + imgs = [] + for name in camSockets: + imgs.append([]) + fps = fpsCounter.getFps() + + # calculate the greatest time difference between all frames + delta = max(tsValues.values()) - min(tsValues.values()) + + syncStatus = abs(delta) < syncThresholdSec + syncStatusStr = "in sync" if syncStatus else "out of sync" + + # Timeout if frames don't get synced in time + if not syncStatus and waitingForSync: + endTime = datetime.datetime.now() + elapsedSec = (endTime - initialSyncTime).total_seconds() + if elapsedSec >= initialSyncTimeoutSec: + print("Timeout: Didn't sync frames in time") + running = False + + if syncStatus and waitingForSync: + print(f"Sync status: {syncStatusStr}") + waitingForSync = False + + # Print warning if delta is too big + if not syncStatus and not waitingForSync: + print(f"Sync error: Sync lost, threshold exceeded {delta * 1e6} us") + print("Either the signal is lost or the network is congested.") + continue + + color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + + # Create a image frame with sync info for each output + for outputName in outputNames: + + # Find out which camera socket this output belongs to + idx = -1 + isStereo = False + for i, name in enumerate(camSockets): + if name in outputName: + idx = i + isStereo = name == STEREO_SOCKET + break + if idx == -1: + raise RuntimeError(f"Could not find camera socket for {outputName}") + + # Get frame for this output + msg = latestFrameGroup[outputName] + if isStereo: + npDisparity = msg.getFrame() + maxDisparity = max(maxDisparity, np.max(npDisparity)) + frame = cv2.applyColorMap(((npDisparity / maxDisparity) * 255).astype(np.uint8), colorMap) + else: + frame = msg.getCvFrame() + + # Add output name to frame + cv2.putText( + frame, + f"{outputName}", + (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (0, 127, 255), + 2, + cv2.LINE_AA, + ) + + # Add timestamp and FPS to frame + cv2.putText( + frame, + f"Timestamp: {tsValues[outputName]} | FPS:{fps:.2f}", + (20, 80), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 0, 50), + 2, + cv2.LINE_AA, + ) + + imgs[idx].append(frame) + + # Add sync status and delta to the frame for each camera socket + for i, img in enumerate(imgs): + cv2.putText( + imgs[i][0], + f"{syncStatusStr} | delta = {delta*1e3:.3f} ms", + (20, 120), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + color, + 2, + cv2.LINE_AA, + ) + + # Show the frame + for i, img in enumerate(imgs): + cv2.imshow(f"synced_view_{camSockets[i]}", cv2.hconcat(imgs[i])) + + latestFrameGroup = None # Wait for next batch + + if cv2.waitKey(1) & 0xFF == ord("q"): + break + +cv2.destroyAllWindows() From bd16bde6ad29fc69531d367c64629a1c58fdbea1 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 11:02:11 +0100 Subject: [PATCH 172/180] Change flag to slave only Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index 1cdfc2386d..5d5612ec21 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -85,7 +85,7 @@ def createSyncNode(syncThreshold: datetime.timedelta): sync.setSyncThreshold(syncThreshold) # Link master camera outputs to the sync node - if not noMaster: + if not slaveOnly: for socketName, camOutput in masterNode.items(): name = f"master_{socketName}" camOutput.link(sync.inputs[name]) @@ -118,8 +118,8 @@ def setUpCameraSocket( # Master cameras will be linked to the sync node directly if role == dai.ExternalFrameSyncRole.MASTER: - if noMaster: - raise RuntimeError("No master specified, but master device detected") + if slaveOnly: + raise RuntimeError("Slave-only specified, but master device detected") if masterNode is None: masterNode = {} @@ -143,7 +143,7 @@ def setupDevice( stack: contextlib.ExitStack, deviceInfo: dai.DeviceInfo, targetFps: float): - global masterPipeline, slavePipelines, slaveQueues, camSockets, noMaster, firstSlaveName + global masterPipeline, slavePipelines, slaveQueues, camSockets, slaveOnly, firstSlaveName # Create pipeline for device pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) @@ -163,8 +163,8 @@ def setupDevice( pipeline = setUpCameraSocket(pipeline, socket, name, targetFps, role) if role == dai.ExternalFrameSyncRole.MASTER: - if noMaster: - raise RuntimeError("No master specified, but master device detected") + if slaveOnly: + raise RuntimeError("Slave-only specified, but master device detected") device.setExternalStrobeEnable(True) print(f"{device.getDeviceId()} is master") @@ -177,7 +177,7 @@ def setupDevice( slavePipelines[name] = pipeline print(f"{device.getDeviceId()} is slave") - if firstSlaveName is None and noMaster: + if firstSlaveName is None and slaveOnly: firstSlaveName = name else: raise RuntimeError(f"Don't know how to handle role {role}") @@ -201,7 +201,7 @@ def interruptHandler(sig, frame): parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) -parser.add_argument("-nm", "--no-master", action="store_true", help="Run the script without a master device", required=False) +parser.add_argument("-s", "--slave-only", action="store_true", help="Run the script without a master device", required=False) args = parser.parse_args() # if user did not specify device IPs, use all available devices @@ -216,9 +216,9 @@ def interruptHandler(sig, frame): syncThresholdSec = args.sync_threshold_sec initialSyncTimeoutSec = args.initial_sync_timeout_sec -noMaster = False -if args.no_master: - noMaster = True +slaveOnly = False +if args.slave_only: + slaveOnly = True # --------------------------------------------------------------------------- # Main # --------------------------------------------------------------------------- @@ -245,7 +245,7 @@ def interruptHandler(sig, frame): for idx, deviceInfo in enumerate(deviceInfos): setupDevice(stack, deviceInfo, targetFps) - if not noMaster: + if not slaveOnly: if masterPipeline is None or masterNode is None: raise RuntimeError("No master detected!") @@ -259,7 +259,7 @@ def interruptHandler(sig, frame): # Start pipelines # The master pipeline will be started first, then the slave pipelines - if not noMaster: + if not slaveOnly: masterPipeline.start() for k, sockets in slavePipelines.items(): From 6993069effff1591a84e17f219b713b5f95e4f00 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 11:07:30 +0100 Subject: [PATCH 173/180] Add neural depth option Signed-off-by: stas.bucik --- ...multi_device_external_frame_sync_stereo.py | 43 +++++++++++-------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py index e455b26220..38020faef2 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py @@ -88,7 +88,7 @@ def createSyncNode(syncThreshold: datetime.timedelta): sync.setSyncThreshold(syncThreshold) # Link master camera outputs to the sync node - if not noMaster: + if not slaveOnly: for socketName, camOutput in masterNode.items(): name = f"master_{socketName}" camOutput.link(sync.inputs[name]) @@ -121,8 +121,8 @@ def setUpCameraSocket( # Master cameras will be linked to the sync node directly if role == dai.ExternalFrameSyncRole.MASTER: - if noMaster: - raise RuntimeError("No master specified, but master device detected") + if slaveOnly: + raise RuntimeError("Slave-only specified, but master device detected") if masterNode is None: masterNode = {} @@ -146,7 +146,7 @@ def setupDevice( stack: contextlib.ExitStack, deviceInfo: dai.DeviceInfo, targetFps: float): - global masterPipeline, slavePipelines, slaveQueues, camSockets, noMaster, firstSlaveName, masterNode + global masterPipeline, slavePipelines, slaveQueues, camSockets, slaveOnly, firstSlaveName, masterNode # Create pipeline for device pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) @@ -163,7 +163,10 @@ def setupDevice( print(" Num of cameras:", len(device.getConnectedCameras())) # create stereo node - stereo = pipeline.create(dai.node.StereoDepth) + if neuralStereo: + stereo = pipeline.create(dai.node.NeuralDepth) + else: + stereo = pipeline.create(dai.node.StereoDepth) for socket in device.getConnectedCameras(): pipeline, outNode = setUpCameraSocket(pipeline, socket, name, targetFps, role) @@ -174,13 +177,14 @@ def setupDevice( elif socket == dai.CameraBoardSocket.CAM_C: outNode.link(stereo.right) - stereo.setRectification(True) - stereo.setExtendedDisparity(True) - stereo.setLeftRightCheck(True) + if not neuralStereo: + stereo.setRectification(True) + stereo.setExtendedDisparity(True) + stereo.setLeftRightCheck(True) if role == dai.ExternalFrameSyncRole.MASTER: - if noMaster: - raise RuntimeError("No master specified, but master device detected") + if slaveOnly: + raise RuntimeError("Slave-only specified, but master device detected") device.setExternalStrobeEnable(True) print(f"{device.getDeviceId()} is master") @@ -203,7 +207,7 @@ def setupDevice( slaveQueues[name][STEREO_SOCKET] = stereo.disparity.createOutputQueue() - if firstSlaveName is None and noMaster: + if firstSlaveName is None and slaveOnly: firstSlaveName = name else: raise RuntimeError(f"Don't know how to handle role {role}") @@ -230,7 +234,8 @@ def interruptHandler(sig, frame): parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) -parser.add_argument("-nm", "--no-master", action="store_true", help="Run the script without a master device", required=False) +parser.add_argument("-nn", "--neural-depth", action="store_true", help="Use neural depth instead", required=False) +parser.add_argument("-s", "--slave-only", action="store_true", help="Run the script without a master device", required=False) args = parser.parse_args() # if user did not specify device IPs, use all available devices @@ -245,9 +250,13 @@ def interruptHandler(sig, frame): syncThresholdSec = args.sync_threshold_sec initialSyncTimeoutSec = args.initial_sync_timeout_sec -noMaster = False -if args.no_master: - noMaster = True +slaveOnly = False +if args.slave_only: + slaveOnly = True + +neuralStereo = False +if args.neural_depth: + neuralStereo = True # --------------------------------------------------------------------------- # Main # --------------------------------------------------------------------------- @@ -279,7 +288,7 @@ def interruptHandler(sig, frame): for idx, deviceInfo in enumerate(deviceInfos): setupDevice(stack, deviceInfo, targetFps) - if not noMaster: + if not slaveOnly: if masterPipeline is None or masterNode is None: raise RuntimeError("No master detected!") @@ -293,7 +302,7 @@ def interruptHandler(sig, frame): # Start pipelines # The master pipeline will be started first, then the slave pipelines - if not noMaster: + if not slaveOnly: masterPipeline.start() for k, sockets in slavePipelines.items(): From b94469a26ba6fd84cf0728b0eff0c0f40e4f5bbd Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 11:25:11 +0100 Subject: [PATCH 174/180] Add initial plc example Signed-off-by: stas.bucik --- ...vice_external_frame_sync_pcl_processing.py | 459 ++++++++++++++++++ 1 file changed, 459 insertions(+) create mode 100644 examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py new file mode 100644 index 0000000000..38020faef2 --- /dev/null +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -0,0 +1,459 @@ +#!/usr/bin/env python3 + +import contextlib +import datetime + +import cv2 +import depthai as dai +import time + +import argparse +import signal +import numpy as np + +from typing import Optional, Dict + +# This example shows how to use the external frame sync node to synchronize multiple devices +# This example assumes that all devices are connected to the same host +# The script identifies the master device and slave devices and then starts the master first +# This is necessary because otherwise slave devices will not be able to detect the master FSYNC signal + +SET_MANUAL_EXPOSURE = False # Set to True to use manual exposure settings + +STEREO_SOCKET = "STEREO" +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- +class FPSCounter: + def __init__(self): + self.frameTimes = [] + + def tick(self): + now = time.time() + self.frameTimes.append(now) + self.frameTimes = self.frameTimes[-100:] + + def getFps(self): + if len(self.frameTimes) <= 1: + return 0 + # Calculate the FPS + return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) + +# --------------------------------------------------------------------------- +# Create camera outputs +# --------------------------------------------------------------------------- +def createCameraOutputs(pipeline: dai.Pipeline, socket: dai.CameraBoardSocket, sensorFps: float, role: dai.ExternalFrameSyncRole): + cam = None + + # Only specify FPS if camera is master + if role == dai.ExternalFrameSyncRole.MASTER: + cam = ( + pipeline.create(dai.node.Camera) + .build(socket, sensorFps=sensorFps) + ) + + # Slave cameras will lock to the master's FPS + else: + cam = ( + pipeline.create(dai.node.Camera) + .build(socket) + ) + + if socket == dai.CameraBoardSocket.CAM_A: + output = ( + cam.requestOutput( + (1920, 1080), dai.ImgFrame.Type.NV12, dai.ImgResizeMode.CROP + ) + ) + else: + output = ( + cam.requestFullResolutionOutput() + ) + return pipeline, output + +# --------------------------------------------------------------------------- +# Create synchronization node +# --------------------------------------------------------------------------- +def createSyncNode(syncThreshold: datetime.timedelta): + global masterPipeline, masterNode, slaveQueues, inputQueues, outputNames, firstSlaveName, slavePipelines + + # If no master is specified, create a sync node on the first slave pipeline + if firstSlaveName is None: + sync = masterPipeline.create(dai.node.Sync) + else: + sync = slavePipelines[firstSlaveName].create(dai.node.Sync) + + # Sync node will run on the host, since it needs to sync multiple devices + sync.setRunOnHost(True) + sync.setSyncThreshold(syncThreshold) + + # Link master camera outputs to the sync node + if not slaveOnly: + for socketName, camOutput in masterNode.items(): + name = f"master_{socketName}" + camOutput.link(sync.inputs[name]) + outputNames.append(name) + + # For slaves, we must create an input queue for each output + # We will then manually forward the frames from each input queue to the output queue + # This is because slave devices have separate pipelines from the master + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + name = f"slave_{deviceName}_{socketName}" + outputNames.append(name) + input_queue = sync.inputs[name].createInputQueue() + inputQueues[name] = input_queue + + return sync + +# --------------------------------------------------------------------------- +# Set up for individual camera sockets +# --------------------------------------------------------------------------- +def setUpCameraSocket( + pipeline: dai.Pipeline, + socket: dai.CameraBoardSocket, + deviceName: str, + targetFps: float, + role: dai.ExternalFrameSyncRole): + global masterNode, slaveQueues, camSockets + + pipeline, outNode = createCameraOutputs(pipeline, socket, targetFps, role) + + # Master cameras will be linked to the sync node directly + if role == dai.ExternalFrameSyncRole.MASTER: + if slaveOnly: + raise RuntimeError("Slave-only specified, but master device detected") + + if masterNode is None: + masterNode = {} + masterNode[socket.name] = outNode + + # Gather all slave camera outputs + elif role == dai.ExternalFrameSyncRole.SLAVE: + if slaveQueues.get(deviceName) is None: + slaveQueues[deviceName] = {} + slaveQueues[deviceName][socket.name] = outNode.createOutputQueue() + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + # Keep track of all camera socket names + if socket.name not in camSockets: + camSockets.append(socket.name) + + return pipeline, outNode + +def setupDevice( + stack: contextlib.ExitStack, + deviceInfo: dai.DeviceInfo, + targetFps: float): + global masterPipeline, slavePipelines, slaveQueues, camSockets, slaveOnly, firstSlaveName, masterNode + + # Create pipeline for device + pipeline = stack.enter_context(dai.Pipeline(dai.Device(deviceInfo))) + device = pipeline.getDefaultDevice() + + if device.getPlatform() != dai.Platform.RVC4: + raise RuntimeError("This example supports only RVC4 platform!") + + name = deviceInfo.getXLinkDeviceDesc().mxid + role = device.getExternalFrameSyncRole() + + print("=== Connected to", deviceInfo.getDeviceId()) + print(" Device ID:", device.getDeviceId()) + print(" Num of cameras:", len(device.getConnectedCameras())) + + # create stereo node + if neuralStereo: + stereo = pipeline.create(dai.node.NeuralDepth) + else: + stereo = pipeline.create(dai.node.StereoDepth) + + for socket in device.getConnectedCameras(): + pipeline, outNode = setUpCameraSocket(pipeline, socket, name, targetFps, role) + + # link stereo node + if socket == dai.CameraBoardSocket.CAM_B: + outNode.link(stereo.left) + elif socket == dai.CameraBoardSocket.CAM_C: + outNode.link(stereo.right) + + if not neuralStereo: + stereo.setRectification(True) + stereo.setExtendedDisparity(True) + stereo.setLeftRightCheck(True) + + if role == dai.ExternalFrameSyncRole.MASTER: + if slaveOnly: + raise RuntimeError("Slave-only specified, but master device detected") + + device.setExternalStrobeEnable(True) + print(f"{device.getDeviceId()} is master") + + if masterPipeline is not None: + raise RuntimeError("Only one master pipeline is supported") + + masterPipeline = pipeline + + if masterNode is None: + masterNode = {} + + masterNode[STEREO_SOCKET] = stereo.disparity + elif role == dai.ExternalFrameSyncRole.SLAVE: + slavePipelines[name] = pipeline + print(f"{device.getDeviceId()} is slave") + + if slaveQueues.get(name) is None: + slaveQueues[name] = {} + + slaveQueues[name][STEREO_SOCKET] = stereo.disparity.createOutputQueue() + + if firstSlaveName is None and slaveOnly: + firstSlaveName = name + else: + raise RuntimeError(f"Don't know how to handle role {role}") + + if STEREO_SOCKET not in camSockets: + camSockets.append(STEREO_SOCKET) + +running = True + +def interruptHandler(sig, frame): + global running + if running: + print("Interrupted! Exiting...") + running = False + else: + print("Exiting now!") + exit(0) + +signal.signal(signal.SIGINT, interruptHandler) + +parser = argparse.ArgumentParser(add_help=False) +parser.add_argument("-f", "--fps", type=float, default=30.0, help="Target FPS", required=False) +parser.add_argument("-d", "--devices", default=[], nargs="+", help="Device IPs or IDs", required=False) +parser.add_argument("-t1", "--recv-all-timeout-sec", type=float, default=10, help="Timeout for receiving the first frame from all devices", required=False) +parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) +parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) +parser.add_argument("-nn", "--neural-depth", action="store_true", help="Use neural depth instead", required=False) +parser.add_argument("-s", "--slave-only", action="store_true", help="Run the script without a master device", required=False) +args = parser.parse_args() + +# if user did not specify device IPs, use all available devices +if len(args.devices) == 0: + deviceInfos = dai.Device.getAllAvailableDevices() +else: + deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] +assert len(deviceInfos) > 1, "At least two devices are required for this example." + +targetFps = args.fps +recvAllTimeoutSec = args.recv_all_timeout_sec +syncThresholdSec = args.sync_threshold_sec +initialSyncTimeoutSec = args.initial_sync_timeout_sec + +slaveOnly = False +if args.slave_only: + slaveOnly = True + +neuralStereo = False +if args.neural_depth: + neuralStereo = True +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- + +colorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) +colorMap[0] = [0, 0, 0] # to make zero-disparity pixels black +maxDisparity = 1 + +with contextlib.ExitStack() as stack: + + # Variables to keep track of master and slave pipelines and outputs + masterPipeline: Optional[dai.Pipeline] = None + masterNode: Optional[Dict[str, dai.Node.Output]] = None + slavePipelines: Dict[str, dai.Pipeline] = {} + slaveQueues: Dict[str, Dict[str, dai.MessageQueue]] = {} + + # keep track of sync node inputs for slaves + inputQueues = {} + + # keep track of all sync node output names + outputNames = [] + + # keep track of all camera socket names + camSockets = [] + + # keep track of the first slave name if no master is specified + firstSlaveName: Optional[str] = None + + for idx, deviceInfo in enumerate(deviceInfos): + setupDevice(stack, deviceInfo, targetFps) + + if not slaveOnly: + if masterPipeline is None or masterNode is None: + raise RuntimeError("No master detected!") + + if len(slavePipelines) < 1: + raise RuntimeError("No slaves detected!") + + # Create sync node + # Sync node groups the frames so that all synced frames are timestamped to within one frame time + sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) + queue = sync.out.createOutputQueue() + + # Start pipelines + # The master pipeline will be started first, then the slave pipelines + if not slaveOnly: + masterPipeline.start() + + for k, sockets in slavePipelines.items(): + sockets.start() + + fpsCounter = FPSCounter() + + latestFrameGroup = None + firstReceived = False + startTime = datetime.datetime.now() + prevReceived = datetime.datetime.now() + + initialSyncTime = None + waitingForSync = True + + while running: + # Send frames from slave output queues to sync node input queues + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + while camOutputQueue.has(): + inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + + # Get frames from sync node output queue + while queue.has(): + latestFrameGroup = queue.get() + if not firstReceived: + firstReceived = True + initialSyncTime = datetime.datetime.now() + prevReceived = datetime.datetime.now() + fpsCounter.tick() + + # Timeout if we dont receive any frames at the beginning + if not firstReceived: + endTime = datetime.datetime.now() + elapsedSec = (endTime - startTime).total_seconds() + if elapsedSec >= recvAllTimeoutSec: + print(f"Timeout: Didn't receive all frames in time: {elapsedSec:.2f} sec") + running = False + + # ------------------------------------------------------------------- + # Synchronise: we need at least one frame from every camera and their + # timestamps must align within syncThresholdSec. + # ------------------------------------------------------------------- + if latestFrameGroup is not None and latestFrameGroup.getNumMessages() == len(outputNames): + + # Get timestamps for each frame + tsValues = {} + for name in outputNames: + tsValues[name] = latestFrameGroup[name].getTimestamp(dai.CameraExposureOffset.END).total_seconds() + + # Build individual image arrays for each camera socket + imgs = [] + for name in camSockets: + imgs.append([]) + fps = fpsCounter.getFps() + + # calculate the greatest time difference between all frames + delta = max(tsValues.values()) - min(tsValues.values()) + + syncStatus = abs(delta) < syncThresholdSec + syncStatusStr = "in sync" if syncStatus else "out of sync" + + # Timeout if frames don't get synced in time + if not syncStatus and waitingForSync: + endTime = datetime.datetime.now() + elapsedSec = (endTime - initialSyncTime).total_seconds() + if elapsedSec >= initialSyncTimeoutSec: + print("Timeout: Didn't sync frames in time") + running = False + + if syncStatus and waitingForSync: + print(f"Sync status: {syncStatusStr}") + waitingForSync = False + + # Print warning if delta is too big + if not syncStatus and not waitingForSync: + print(f"Sync error: Sync lost, threshold exceeded {delta * 1e6} us") + print("Either the signal is lost or the network is congested.") + continue + + color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + + # Create a image frame with sync info for each output + for outputName in outputNames: + + # Find out which camera socket this output belongs to + idx = -1 + isStereo = False + for i, name in enumerate(camSockets): + if name in outputName: + idx = i + isStereo = name == STEREO_SOCKET + break + if idx == -1: + raise RuntimeError(f"Could not find camera socket for {outputName}") + + # Get frame for this output + msg = latestFrameGroup[outputName] + if isStereo: + npDisparity = msg.getFrame() + maxDisparity = max(maxDisparity, np.max(npDisparity)) + frame = cv2.applyColorMap(((npDisparity / maxDisparity) * 255).astype(np.uint8), colorMap) + else: + frame = msg.getCvFrame() + + # Add output name to frame + cv2.putText( + frame, + f"{outputName}", + (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (0, 127, 255), + 2, + cv2.LINE_AA, + ) + + # Add timestamp and FPS to frame + cv2.putText( + frame, + f"Timestamp: {tsValues[outputName]} | FPS:{fps:.2f}", + (20, 80), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 0, 50), + 2, + cv2.LINE_AA, + ) + + imgs[idx].append(frame) + + # Add sync status and delta to the frame for each camera socket + for i, img in enumerate(imgs): + cv2.putText( + imgs[i][0], + f"{syncStatusStr} | delta = {delta*1e3:.3f} ms", + (20, 120), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + color, + 2, + cv2.LINE_AA, + ) + + # Show the frame + for i, img in enumerate(imgs): + cv2.imshow(f"synced_view_{camSockets[i]}", cv2.hconcat(imgs[i])) + + latestFrameGroup = None # Wait for next batch + + if cv2.waitKey(1) & 0xFF == ord("q"): + break + +cv2.destroyAllWindows() From 804d2b5e6d53304e3d15c81076f7271b59790598 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 13:23:32 +0100 Subject: [PATCH 175/180] Add point cloud data Signed-off-by: stas.bucik --- ...vice_external_frame_sync_pcl_processing.py | 44 ++++++++++--------- 1 file changed, 24 insertions(+), 20 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py index 38020faef2..b3fb763b26 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -20,7 +20,7 @@ SET_MANUAL_EXPOSURE = False # Set to True to use manual exposure settings -STEREO_SOCKET = "STEREO" +PCL_SOCKET = "PCL" # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- @@ -168,8 +168,10 @@ def setupDevice( else: stereo = pipeline.create(dai.node.StereoDepth) + socketOutputs = {} for socket in device.getConnectedCameras(): pipeline, outNode = setUpCameraSocket(pipeline, socket, name, targetFps, role) + socketOutputs[socket] = outNode # link stereo node if socket == dai.CameraBoardSocket.CAM_B: @@ -182,6 +184,16 @@ def setupDevice( stereo.setExtendedDisparity(True) stereo.setLeftRightCheck(True) + # Create RGBD for point cloud generation + if dai.CameraBoardSocket.CAM_A not in socketOutputs: + raise RuntimeError("RGBD requires CAM_A to be present for color input") + rgbd = pipeline.create(dai.node.RGBD).build() + align = pipeline.create(dai.node.ImageAlign) + stereo.depth.link(align.input) + socketOutputs[dai.CameraBoardSocket.CAM_A].link(align.inputAlignTo) + align.outputAligned.link(rgbd.inDepth) + socketOutputs[dai.CameraBoardSocket.CAM_A].link(rgbd.inColor) + if role == dai.ExternalFrameSyncRole.MASTER: if slaveOnly: raise RuntimeError("Slave-only specified, but master device detected") @@ -197,7 +209,7 @@ def setupDevice( if masterNode is None: masterNode = {} - masterNode[STEREO_SOCKET] = stereo.disparity + masterNode[PCL_SOCKET] = rgbd.pcl elif role == dai.ExternalFrameSyncRole.SLAVE: slavePipelines[name] = pipeline print(f"{device.getDeviceId()} is slave") @@ -205,15 +217,12 @@ def setupDevice( if slaveQueues.get(name) is None: slaveQueues[name] = {} - slaveQueues[name][STEREO_SOCKET] = stereo.disparity.createOutputQueue() + slaveQueues[name][PCL_SOCKET] = rgbd.pcl.createOutputQueue() if firstSlaveName is None and slaveOnly: firstSlaveName = name else: raise RuntimeError(f"Don't know how to handle role {role}") - - if STEREO_SOCKET not in camSockets: - camSockets.append(STEREO_SOCKET) running = True @@ -261,10 +270,6 @@ def interruptHandler(sig, frame): # Main # --------------------------------------------------------------------------- -colorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) -colorMap[0] = [0, 0, 0] # to make zero-disparity pixels black -maxDisparity = 1 - with contextlib.ExitStack() as stack: # Variables to keep track of master and slave pipelines and outputs @@ -351,7 +356,10 @@ def interruptHandler(sig, frame): # Get timestamps for each frame tsValues = {} for name in outputNames: - tsValues[name] = latestFrameGroup[name].getTimestamp(dai.CameraExposureOffset.END).total_seconds() + if PCL_SOCKET in name: + tsValues[name] = latestFrameGroup[name].getTimestamp().total_seconds() + else: + tsValues[name] = latestFrameGroup[name].getTimestamp(dai.CameraExposureOffset.END).total_seconds() # Build individual image arrays for each camera socket imgs = [] @@ -388,25 +396,21 @@ def interruptHandler(sig, frame): # Create a image frame with sync info for each output for outputName in outputNames: + if PCL_SOCKET in outputName: + continue + # Find out which camera socket this output belongs to idx = -1 - isStereo = False for i, name in enumerate(camSockets): if name in outputName: idx = i - isStereo = name == STEREO_SOCKET break if idx == -1: raise RuntimeError(f"Could not find camera socket for {outputName}") - + # Get frame for this output msg = latestFrameGroup[outputName] - if isStereo: - npDisparity = msg.getFrame() - maxDisparity = max(maxDisparity, np.max(npDisparity)) - frame = cv2.applyColorMap(((npDisparity / maxDisparity) * 255).astype(np.uint8), colorMap) - else: - frame = msg.getCvFrame() + frame = msg.getCvFrame() # Add output name to frame cv2.putText( From f810bc19ab165ca595cdfc8dab9554999ae60e07 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Mon, 2 Mar 2026 15:10:38 +0100 Subject: [PATCH 176/180] Add comment Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync_pcl_processing.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py index b3fb763b26..f9ed212630 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -397,6 +397,15 @@ def interruptHandler(sig, frame): for outputName in outputNames: if PCL_SOCKET in outputName: + ######################################################################## + # Do processing of pointclouds here + ######################################################################## + + msg = latestFrameGroup[outputName] + assert isinstance(msg, dai.PointCloudData) + points, colors = msg.getPointsRGB() + # Do something with the point cloud + # For example, we can filter out points that are too close to the camera continue # Find out which camera socket this output belongs to From 0216a32ea52862b02ea4c2f820a23a6bcdd9478c Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Tue, 3 Mar 2026 15:09:30 +0100 Subject: [PATCH 177/180] Fix queue getting and sending Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync.py | 24 ++++++++++++++---- ...vice_external_frame_sync_pcl_processing.py | 25 ++++++++++++++----- ...multi_device_external_frame_sync_stereo.py | 24 ++++++++++++++---- 3 files changed, 57 insertions(+), 16 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index 5d5612ec21..b504f8dc20 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -9,6 +9,7 @@ import argparse import signal +import threading from typing import Optional, Dict @@ -275,13 +276,23 @@ def interruptHandler(sig, frame): initialSyncTime = None waitingForSync = True - while running: + def data_collector(deviceName, socketName): # Send frames from slave output queues to sync node input queues - for deviceName, sockets in slaveQueues.items(): - for socketName, camOutputQueue in sockets.items(): - while camOutputQueue.has(): - inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + camOutputQueue = slaveQueues[deviceName][socketName] + while running: + if camOutputQueue.has(): + inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + else: + time.sleep(0.001) + + threads = {} + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + threads[f"slave_{deviceName}_{socketName}"] = threading.Thread(target=data_collector, args=(deviceName, socketName)) + threads[f"slave_{deviceName}_{socketName}"].start() + + while running: # Get frames from sync node output queue while queue.has(): latestFrameGroup = queue.get() @@ -406,4 +417,7 @@ def interruptHandler(sig, frame): if cv2.waitKey(1) & 0xFF == ord("q"): break + for t in threads.keys(): + threads[t].join() + cv2.destroyAllWindows() diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py index f9ed212630..08fb3948a8 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -9,7 +9,7 @@ import argparse import signal -import numpy as np +import threading from typing import Optional, Dict @@ -323,13 +323,23 @@ def interruptHandler(sig, frame): initialSyncTime = None waitingForSync = True - while running: + def data_collector(deviceName, socketName): # Send frames from slave output queues to sync node input queues - for deviceName, sockets in slaveQueues.items(): - for socketName, camOutputQueue in sockets.items(): - while camOutputQueue.has(): - inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + camOutputQueue = slaveQueues[deviceName][socketName] + while running: + if camOutputQueue.has(): + inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + else: + time.sleep(0.001) + + threads = {} + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + threads[f"slave_{deviceName}_{socketName}"] = threading.Thread(target=data_collector, args=(deviceName, socketName)) + threads[f"slave_{deviceName}_{socketName}"].start() + + while running: # Get frames from sync node output queue while queue.has(): latestFrameGroup = queue.get() @@ -469,4 +479,7 @@ def interruptHandler(sig, frame): if cv2.waitKey(1) & 0xFF == ord("q"): break + for t in threads.keys(): + threads[t].join() + cv2.destroyAllWindows() diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py index 38020faef2..5dee139cb6 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py @@ -9,6 +9,7 @@ import argparse import signal +import threading import numpy as np from typing import Optional, Dict @@ -318,13 +319,23 @@ def interruptHandler(sig, frame): initialSyncTime = None waitingForSync = True - while running: + def data_collector(deviceName, socketName): # Send frames from slave output queues to sync node input queues - for deviceName, sockets in slaveQueues.items(): - for socketName, camOutputQueue in sockets.items(): - while camOutputQueue.has(): - inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + camOutputQueue = slaveQueues[deviceName][socketName] + while running: + if camOutputQueue.has(): + inputQueues[f"slave_{deviceName}_{socketName}"].send(camOutputQueue.get()) + else: + time.sleep(0.001) + + threads = {} + for deviceName, sockets in slaveQueues.items(): + for socketName, camOutputQueue in sockets.items(): + threads[f"slave_{deviceName}_{socketName}"] = threading.Thread(target=data_collector, args=(deviceName, socketName)) + threads[f"slave_{deviceName}_{socketName}"].start() + + while running: # Get frames from sync node output queue while queue.has(): latestFrameGroup = queue.get() @@ -456,4 +467,7 @@ def interruptHandler(sig, frame): if cv2.waitKey(1) & 0xFF == ord("q"): break + for t in threads.keys(): + threads[t].join() + cv2.destroyAllWindows() From 3ed4f6ff623f1ec14ace17033bbf9c0856d0435e Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Tue, 3 Mar 2026 15:47:09 +0100 Subject: [PATCH 178/180] Move image align to host Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync_pcl_processing.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py index 08fb3948a8..f59c48e615 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -189,6 +189,7 @@ def setupDevice( raise RuntimeError("RGBD requires CAM_A to be present for color input") rgbd = pipeline.create(dai.node.RGBD).build() align = pipeline.create(dai.node.ImageAlign) + align.setRunOnHost(True) stereo.depth.link(align.input) socketOutputs[dai.CameraBoardSocket.CAM_A].link(align.inputAlignTo) align.outputAligned.link(rgbd.inDepth) From 8f0ee3b1fa94c0efca83892ab5c6ba2e0af7dba2 Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 15:47:44 +0100 Subject: [PATCH 179/180] Add debug versions Signed-off-by: stas.bucik --- .../multi_device_external_frame_sync.py | 21 +++++++++++++++++-- ...vice_external_frame_sync_pcl_processing.py | 21 +++++++++++++++++-- ...multi_device_external_frame_sync_stereo.py | 20 ++++++++++++++++-- 3 files changed, 56 insertions(+), 6 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index b504f8dc20..9b5737cacb 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -13,6 +13,8 @@ from typing import Optional, Dict +import os + # This example shows how to use the external frame sync node to synchronize multiple devices # This example assumes that all devices are connected to the same host # The script identifies the master device and slave devices and then starts the master first @@ -184,10 +186,14 @@ def setupDevice( raise RuntimeError(f"Don't know how to handle role {role}") running = True +freeze = False def interruptHandler(sig, frame): - global running - if running: + global running, freeze + if not freeze: + freeze = True + print("Interrupted! Freezing...") + elif running: print("Interrupted! Exiting...") running = False else: @@ -203,8 +209,12 @@ def interruptHandler(sig, frame): parser.add_argument("-t2", "--sync-threshold-sec", type=float, default=1e-3, help="Sync threshold in seconds", required=False) parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) parser.add_argument("-s", "--slave-only", action="store_true", help="Run the script without a master device", required=False) +parser.add_argument("--display", type=str, help="X11 display name") args = parser.parse_args() +if args.display: + os.environ["DISPLAY"] = args.display + # if user did not specify device IPs, use all available devices if len(args.devices) == 0: deviceInfos = dai.Device.getAllAvailableDevices() @@ -258,6 +268,11 @@ def interruptHandler(sig, frame): sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() + benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + benchmarkAll.setRunOnHost(True) + benchmarkAll.sendReportEveryNMessages(10) + sync.out.link(benchmarkAll.input) + # Start pipelines # The master pipeline will be started first, then the slave pipelines if not slaveOnly: @@ -353,6 +368,8 @@ def data_collector(deviceName, socketName): color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + if freeze: + continue # Create a image frame with sync info for each output for outputName in outputNames: diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py index f59c48e615..6c37ca01f1 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -13,6 +13,8 @@ from typing import Optional, Dict +import os + # This example shows how to use the external frame sync node to synchronize multiple devices # This example assumes that all devices are connected to the same host # The script identifies the master device and slave devices and then starts the master first @@ -226,10 +228,14 @@ def setupDevice( raise RuntimeError(f"Don't know how to handle role {role}") running = True +freeze = False def interruptHandler(sig, frame): - global running - if running: + global running, freeze + if not freeze: + freeze = True + print("Interrupted! Freezing...") + elif running: print("Interrupted! Exiting...") running = False else: @@ -246,8 +252,12 @@ def interruptHandler(sig, frame): parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) parser.add_argument("-nn", "--neural-depth", action="store_true", help="Use neural depth instead", required=False) parser.add_argument("-s", "--slave-only", action="store_true", help="Run the script without a master device", required=False) +parser.add_argument("--display", type=str, help="X11 display name") args = parser.parse_args() +if args.display: + os.environ["DISPLAY"] = args.display + # if user did not specify device IPs, use all available devices if len(args.devices) == 0: deviceInfos = dai.Device.getAllAvailableDevices() @@ -306,6 +316,11 @@ def interruptHandler(sig, frame): sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() + benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + benchmarkAll.setRunOnHost(True) + benchmarkAll.sendReportEveryNMessages(100) + sync.out.link(benchmarkAll.input) + # Start pipelines # The master pipeline will be started first, then the slave pipelines if not slaveOnly: @@ -404,6 +419,8 @@ def data_collector(deviceName, socketName): color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + if freeze: + continue # Create a image frame with sync info for each output for outputName in outputNames: diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py index 5dee139cb6..2442574c9f 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py @@ -11,6 +11,7 @@ import signal import threading import numpy as np +import os from typing import Optional, Dict @@ -217,10 +218,14 @@ def setupDevice( camSockets.append(STEREO_SOCKET) running = True +freeze = False def interruptHandler(sig, frame): - global running - if running: + global running, freeze + if not freeze: + freeze = True + print("Interrupted! Freezing...") + elif running: print("Interrupted! Exiting...") running = False else: @@ -237,8 +242,12 @@ def interruptHandler(sig, frame): parser.add_argument("-t3", "--initial-sync-timeout-sec", type=float, default=4, help="Timeout for synchronization to complete", required=False) parser.add_argument("-nn", "--neural-depth", action="store_true", help="Use neural depth instead", required=False) parser.add_argument("-s", "--slave-only", action="store_true", help="Run the script without a master device", required=False) +parser.add_argument("--display", type=str, help="X11 display name") args = parser.parse_args() +if args.display: + os.environ["DISPLAY"] = args.display + # if user did not specify device IPs, use all available devices if len(args.devices) == 0: deviceInfos = dai.Device.getAllAvailableDevices() @@ -301,6 +310,11 @@ def interruptHandler(sig, frame): sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() + benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + benchmarkAll.setRunOnHost(True) + benchmarkAll.sendReportEveryNMessages(100) + sync.out.link(benchmarkAll.input) + # Start pipelines # The master pipeline will be started first, then the slave pipelines if not slaveOnly: @@ -396,6 +410,8 @@ def data_collector(deviceName, socketName): color = (0, 255, 0) if syncStatusStr == "in sync" else (0, 0, 255) + if freeze: + continue # Create a image frame with sync info for each output for outputName in outputNames: From e3011aad9210cd2dfafb5121a3c5212d0c559abe Mon Sep 17 00:00:00 2001 From: "stas.bucik" Date: Fri, 27 Feb 2026 16:43:57 +0100 Subject: [PATCH 180/180] Add single device capability Signed-off-by: stas.bucik --- .../MultiDevice/multi_device_external_frame_sync.py | 11 +++++++---- ...multi_device_external_frame_sync_pcl_processing.py | 11 +++++++---- .../multi_device_external_frame_sync_stereo.py | 11 +++++++---- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py index 9b5737cacb..9f9ca9ede7 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync.py @@ -220,7 +220,7 @@ def interruptHandler(sig, frame): deviceInfos = dai.Device.getAllAvailableDevices() else: deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] -assert len(deviceInfos) > 1, "At least two devices are required for this example." +# assert len(deviceInfos) > 1, "At least two devices are required for this example." targetFps = args.fps recvAllTimeoutSec = args.recv_all_timeout_sec @@ -260,15 +260,18 @@ def interruptHandler(sig, frame): if masterPipeline is None or masterNode is None: raise RuntimeError("No master detected!") - if len(slavePipelines) < 1: - raise RuntimeError("No slaves detected!") + # if len(slavePipelines) < 1: + # raise RuntimeError("No slaves detected!") # Create sync node # Sync node groups the frames so that all synced frames are timestamped to within one frame time sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() - benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + if not slaveOnly: + benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + else: + benchmarkAll = slavePipelines[firstSlaveName].create(dai.node.BenchmarkIn) benchmarkAll.setRunOnHost(True) benchmarkAll.sendReportEveryNMessages(10) sync.out.link(benchmarkAll.input) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py index 6c37ca01f1..157d7cb06a 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_pcl_processing.py @@ -263,7 +263,7 @@ def interruptHandler(sig, frame): deviceInfos = dai.Device.getAllAvailableDevices() else: deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] -assert len(deviceInfos) > 1, "At least two devices are required for this example." +# assert len(deviceInfos) > 1, "At least two devices are required for this example." targetFps = args.fps recvAllTimeoutSec = args.recv_all_timeout_sec @@ -308,15 +308,18 @@ def interruptHandler(sig, frame): if masterPipeline is None or masterNode is None: raise RuntimeError("No master detected!") - if len(slavePipelines) < 1: - raise RuntimeError("No slaves detected!") + # if len(slavePipelines) < 1: + # raise RuntimeError("No slaves detected!") # Create sync node # Sync node groups the frames so that all synced frames are timestamped to within one frame time sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() - benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + if not slaveOnly: + benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + else: + benchmarkAll = slavePipelines[firstSlaveName].create(dai.node.BenchmarkIn) benchmarkAll.setRunOnHost(True) benchmarkAll.sendReportEveryNMessages(100) sync.out.link(benchmarkAll.input) diff --git a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py index 2442574c9f..61611d40ec 100644 --- a/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py +++ b/examples/python/Misc/MultiDevice/multi_device_external_frame_sync_stereo.py @@ -253,7 +253,7 @@ def interruptHandler(sig, frame): deviceInfos = dai.Device.getAllAvailableDevices() else: deviceInfos = [dai.DeviceInfo(ip) for ip in args.devices] -assert len(deviceInfos) > 1, "At least two devices are required for this example." +# assert len(deviceInfos) > 1, "At least two devices are required for this example." targetFps = args.fps recvAllTimeoutSec = args.recv_all_timeout_sec @@ -302,15 +302,18 @@ def interruptHandler(sig, frame): if masterPipeline is None or masterNode is None: raise RuntimeError("No master detected!") - if len(slavePipelines) < 1: - raise RuntimeError("No slaves detected!") + # if len(slavePipelines) < 1: + # raise RuntimeError("No slaves detected!") # Create sync node # Sync node groups the frames so that all synced frames are timestamped to within one frame time sync = createSyncNode(datetime.timedelta(milliseconds=1000 / (2 * targetFps))) queue = sync.out.createOutputQueue() - benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + if not slaveOnly: + benchmarkAll = masterPipeline.create(dai.node.BenchmarkIn) + else: + benchmarkAll = slavePipelines[firstSlaveName].create(dai.node.BenchmarkIn) benchmarkAll.setRunOnHost(True) benchmarkAll.sendReportEveryNMessages(100) sync.out.link(benchmarkAll.input)