diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 284cc6a84..d91e4bbd9 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -132,7 +132,7 @@ jobs: needs: build-docstrings strategy: matrix: - rpi-os: [rpi-buster, rpi-bullseye] + rpi-os: [rpi-buster, rpi-bullseye, rpi-bookworm] runs-on: ${{ matrix.rpi-os }} steps: - name: Print home directory @@ -155,7 +155,7 @@ jobs: run: python3 -m pip wheel . -w ./wheelhouse/ --verbose - name: Auditing wheels and adding armv6l tag (Running on RPi, binaries compiled as armv6l) run: | - python3 -m pip install -U wheel auditwheel + # python3 -m pip install -U wheel auditwheel # Called once when setting up the runner for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat linux_armv7l -w wheelhouse/preaudited/; done for whl in wheelhouse/preaudited/*.whl; do python3 -m wheel tags --platform-tag +linux_armv6l "$whl"; done mkdir -p wheelhouse/audited/ @@ -556,8 +556,8 @@ jobs: # event-type: python-hil-event # client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' - notify_hil_workflow_linux_x86_64: - needs: [build-linux-x86_64] + notify_hil_workflow: + needs: [build-linux-armhf, build-linux-x86_64] runs-on: ubuntu-latest steps: - name: Dispatch an action and get the run ID diff --git a/.gitignore b/.gitignore index 98f9cb60a..590c7222e 100644 --- a/.gitignore +++ b/.gitignore @@ -22,7 +22,7 @@ depthai-core/build/ # builds _builds/ - +.cache/ #git *.orig *_REMOTE_* @@ -44,4 +44,4 @@ env.bak/ venv.bak/ # PyCharm -.idea/ \ No newline at end of file +.idea/ diff --git a/CMakeLists.txt b/CMakeLists.txt index cddfd09ba..0f58e9e45 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,6 +129,7 @@ pybind11_add_module(${TARGET_NAME} src/pipeline/node/WarpBindings.cpp src/pipeline/node/UVCBindings.cpp src/pipeline/node/ToFBindings.cpp + src/pipeline/node/PointCloudBindings.cpp src/pipeline/node/SyncBindings.cpp src/pipeline/node/MessageDemuxBindings.cpp @@ -154,7 +155,8 @@ pybind11_add_module(${TARGET_NAME} src/pipeline/datatype/SystemInformationBindings.cpp src/pipeline/datatype/TrackedFeaturesBindings.cpp src/pipeline/datatype/TrackletsBindings.cpp - + src/pipeline/datatype/PointCloudConfigBindings.cpp + src/pipeline/datatype/PointCloudDataBindings.cpp ) if(WIN32) diff --git a/depthai-core b/depthai-core index 6628488ef..7f98aa4ca 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 6628488ef8956f73f1c7bf4c8f1da218ad327a6f +Subproject commit 7f98aa4ca474083767b98907b83352f82a85f0f9 diff --git a/depthai_cli/__init__.py b/depthai_cli/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/depthai_cli/depthai_cli.py b/depthai_cli/depthai_cli.py new file mode 100644 index 000000000..e847a217e --- /dev/null +++ b/depthai_cli/depthai_cli.py @@ -0,0 +1,49 @@ +import os +from pathlib import Path +import subprocess + +here = os.path.dirname(os.path.realpath(__file__)) + +if os.path.exists(os.path.join(here, "cam_test.py")): # Installed package + CAM_TEST_PATH = Path(here) / "cam_test.py" +else: + CAM_TEST_PATH = ( + Path(here) / ".." / "utilities" / "cam_test.py" + ) # Execution from source +CAM_TEST_PATH = str(CAM_TEST_PATH) + + +def cli() -> int: + import argparse + import sys + import depthai as dai + parser = argparse.ArgumentParser(description="DepthAI CLI", add_help=True) + parser.add_argument("-v", "--version", action="store_true", help="Print version and exit.") + parser.add_argument("-l", "--list-devices", action="store_true", help="List connected devices.") + subparsers = parser.add_subparsers(dest="command", help="Sub-commands") + # Define the parser for the "cam_test" command + cam_test_parser = subparsers.add_parser("cam_test", help="Commands and options for cam_test", add_help=False) + cam_test_parser.add_argument("args", nargs=argparse.REMAINDER, help="Arguments to pass to cam_test") + + # subparser REMINDER args would get parsed too if we used parse_args, so we have to handle unknown args manually + args, unknown_args = parser.parse_known_args() + if args.command == "cam_test": + cam_test_path = CAM_TEST_PATH + return subprocess.run([sys.executable, cam_test_path] + cam_test_parser.parse_args().args[1:]).returncode + # Parse other subcommands here + elif unknown_args: + parser.error(f"Unrecognized arguments: {unknown_args}") # handles exit internally + elif args.version: + print(dai.__version__) + return 0 + elif args.list_devices: + print(dai.Device.getAllConnectedDevices()) + return 0 + else: + # No recognized commands, print help + parser.print_help() + return 1 + + +if __name__ == "__main__": + exit(cli()) diff --git a/examples/Camera/thermal_cam.py b/examples/Camera/thermal_cam.py new file mode 100644 index 000000000..4fff60ca9 --- /dev/null +++ b/examples/Camera/thermal_cam.py @@ -0,0 +1,107 @@ +import depthai as dai +import cv2 +import numpy as np + +mouseX, mouseY = 0, 0 + + +def onMouse(event, x, y, *args): + global mouseX, mouseY + mouseX = x + mouseY = y + + +device = dai.Device() +pipeline = dai.Pipeline() + +# Thermal camera +thermalCam = pipeline.create(dai.node.Camera) +width, height = -1, -1 +thermalFound = False +for features in device.getConnectedCameraFeatures(): + if dai.CameraSensorType.THERMAL in features.supportedTypes: + thermalFound = True + thermalCam.setBoardSocket(features.socket) + width, height = features.width, features.height + break +if not thermalFound: + raise RuntimeError("No thermal camera found!") +thermalCam.setPreviewSize(width, height) + +# Output raw: FP16 temperature data (degrees Celsius) +xoutRaw = pipeline.create(dai.node.XLinkOut) +xoutRaw.setStreamName("thermal_raw") +thermalCam.raw.link(xoutRaw.input) + +# Output preview,video, isp: RGB or NV12 or YUV420 thermal image. +xoutImage = pipeline.create(dai.node.XLinkOut) +xoutImage.setStreamName("image") +thermalCam.preview.link(xoutImage.input) +device.startPipeline(pipeline) + +qRaw = device.getOutputQueue("thermal_raw", 2, False) +qImage = device.getOutputQueue("image", 2, False) + + +RAW_WINDOW_NAME = "temperature" +IMAGE_WINDOW_NAME = "image" +# Scale 4x and position one next to another +cv2.namedWindow(RAW_WINDOW_NAME, cv2.WINDOW_NORMAL) +cv2.namedWindow(IMAGE_WINDOW_NAME, cv2.WINDOW_NORMAL) +cv2.moveWindow(RAW_WINDOW_NAME, 0, 0) +cv2.resizeWindow(RAW_WINDOW_NAME, width * 4, height * 4) +cv2.moveWindow(IMAGE_WINDOW_NAME, width * 4, 0) +cv2.resizeWindow(IMAGE_WINDOW_NAME, width * 4, height * 4) +cv2.setMouseCallback(RAW_WINDOW_NAME, onMouse) +cv2.setMouseCallback(IMAGE_WINDOW_NAME, onMouse) + +while True: + inRaw = qRaw.get() + inImg = qImage.get() + + # Retrieve one point of fp16 data + frame = inRaw.getCvFrame().astype(np.float32) + colormappedFrame = cv2.normalize(frame, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U) + colormappedFrame = cv2.applyColorMap(colormappedFrame, cv2.COLORMAP_MAGMA) + if ( + mouseX < 0 + or mouseY < 0 + or mouseX >= frame.shape[1] + or mouseY >= frame.shape[0] + ): + mouseX = max(0, min(mouseX, frame.shape[1] - 1)) + mouseY = max(0, min(mouseY, frame.shape[0] - 1)) + textColor = (255, 255, 255) + # Draw crosshair + cv2.line( + colormappedFrame, + (mouseX - 10, mouseY), + (mouseX + 10, mouseY), + textColor, + 1, + ) + cv2.line( + colormappedFrame, + (mouseX, mouseY - 10), + (mouseX, mouseY + 10), + textColor, + 1, + ) + # Draw deg C + text = f"{frame[mouseY, mouseX]:.2f} deg C" + putTextLeft = mouseX > colormappedFrame.shape[1] / 2 + cv2.putText( + colormappedFrame, + text, + (mouseX - 100 if putTextLeft else mouseX + 10, mouseY - 10), + cv2.FONT_HERSHEY_SIMPLEX, + 0.5, + textColor, + 1, + ) + cv2.imshow(RAW_WINDOW_NAME, colormappedFrame) + + cv2.imshow(IMAGE_WINDOW_NAME, inImg.getCvFrame()) + + if cv2.waitKey(1) == ord("q"): + break diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py new file mode 100644 index 000000000..2ff669491 --- /dev/null +++ b/examples/PointCloud/visualize_pointcloud.py @@ -0,0 +1,114 @@ +import depthai as dai +from time import sleep +import numpy as np +import cv2 +import time +import sys +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)) + +FPS = 30 +class FPSCounter: + def __init__(self): + self.frameCount = 0 + self.fps = 0 + self.startTime = time.time() + + def tick(self): + self.frameCount += 1 + if self.frameCount % 10 == 0: + elapsedTime = time.time() - self.startTime + self.fps = self.frameCount / elapsedTime + self.frameCount = 0 + self.startTime = time.time() + return self.fps + +pipeline = dai.Pipeline() +camRgb = pipeline.create(dai.node.ColorCamera) +monoLeft = pipeline.create(dai.node.MonoCamera) +monoRight = pipeline.create(dai.node.MonoCamera) +depth = pipeline.create(dai.node.StereoDepth) +pointcloud = pipeline.create(dai.node.PointCloud) +sync = pipeline.create(dai.node.Sync) +xOut = pipeline.create(dai.node.XLinkOut) +xOut.input.setBlocking(False) + + +camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) +camRgb.setBoardSocket(dai.CameraBoardSocket.CAM_A) +camRgb.setIspScale(1,3) +camRgb.setFps(FPS) + +monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) +monoLeft.setCamera("left") +monoLeft.setFps(FPS) +monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) +monoRight.setCamera("right") +monoRight.setFps(FPS) + +depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) +depth.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) +depth.setLeftRightCheck(True) +depth.setExtendedDisparity(False) +depth.setSubpixel(True) +depth.setDepthAlign(dai.CameraBoardSocket.CAM_A) + +monoLeft.out.link(depth.left) +monoRight.out.link(depth.right) +depth.depth.link(pointcloud.inputDepth) +camRgb.isp.link(sync.inputs["rgb"]) +pointcloud.outputPointCloud.link(sync.inputs["pcl"]) +sync.out.link(xOut.input) +xOut.setStreamName("out") + + + + +with dai.Device(pipeline) as device: + isRunning = True + def key_callback(vis, action, mods): + global isRunning + if action == 0: + isRunning = False + + q = device.getOutputQueue(name="out", maxSize=4, blocking=False) + pc = o3d.geometry.PointCloud() + vis = o3d.visualization.VisualizerWithKeyCallback() + vis.create_window() + vis.register_key_action_callback(81, key_callback) + pcd = o3d.geometry.PointCloud() + coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0,0,0]) + vis.add_geometry(coordinateFrame) + + first = True + fpsCounter = FPSCounter() + while isRunning: + inMessage = q.get() + inColor = inMessage["rgb"] + inPointCloud = inMessage["pcl"] + cvColorFrame = inColor.getCvFrame() + # Convert the frame to RGB + cvRGBFrame = cv2.cvtColor(cvColorFrame, cv2.COLOR_BGR2RGB) + fps = fpsCounter.tick() + # Display the FPS on the frame + cv2.putText(cvColorFrame, f"FPS: {fps:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + cv2.imshow("color", cvColorFrame) + key = cv2.waitKey(1) + if key == ord('q'): + break + if inPointCloud: + t_before = time.time() + points = inPointCloud.getPoints().astype(np.float64) + pcd.points = o3d.utility.Vector3dVector(points) + colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64) + pcd.colors = o3d.utility.Vector3dVector(colors) + if first: + vis.add_geometry(pcd) + first = False + else: + vis.update_geometry(pcd) + vis.poll_events() + vis.update_renderer() + vis.destroy_window() diff --git a/setup.py b/setup.py index 2860af5ba..4d29979b5 100644 --- a/setup.py +++ b/setup.py @@ -14,6 +14,7 @@ ### NAME MODULE_NAME = 'depthai' +DEPTHAI_CLI_MODULE_NAME = 'depthai_cli' ### VERSION here = os.path.abspath(os.path.dirname(__file__)) @@ -92,6 +93,20 @@ def run(self): self.build_extension(ext) def build_extension(self, ext): + if ext.name == DEPTHAI_CLI_MODULE_NAME: + # Copy cam_test.py and it's dependencies to depthai_cli/ + cam_test_path = os.path.join(here, "utilities", "cam_test.py") + cam_test_dest = os.path.join(self.build_lib, DEPTHAI_CLI_MODULE_NAME, "cam_test.py") + cam_test_gui_path = os.path.join(here, "utilities", "cam_test_gui.py") + cam_test_gui_dest = os.path.join(self.build_lib, DEPTHAI_CLI_MODULE_NAME, "cam_test_gui.py") + stress_test_path = os.path.join(here, "utilities", "stress_test.py") + stress_test_dest = os.path.join(self.build_lib, DEPTHAI_CLI_MODULE_NAME, "stress_test.py") + files_to_copy = [(cam_test_path, cam_test_dest), (cam_test_gui_path, cam_test_gui_dest), (stress_test_path, stress_test_dest)] + for src, dst in files_to_copy: + with open(src, "r") as f: + with open(dst, "w") as f2: + f2.write(f.read()) + return extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) # required for auto-detection of auxiliary "native" libs @@ -205,10 +220,11 @@ def build_extension(self, ext): long_description=long_description, long_description_content_type="text/markdown", url="https://github.com/luxonis/depthai-python", - ext_modules=[CMakeExtension(MODULE_NAME)], + ext_modules=[CMakeExtension(MODULE_NAME), Extension(DEPTHAI_CLI_MODULE_NAME, sources=[])], cmdclass={ - 'build_ext': CMakeBuild + 'build_ext': CMakeBuild, }, + packages=[DEPTHAI_CLI_MODULE_NAME], zip_safe=False, classifiers=[ "Development Status :: 4 - Beta", @@ -235,4 +251,9 @@ def build_extension(self, ext): "Topic :: Software Development", ], python_requires='>=3.6', + entry_points={ + "console_scripts": [ + f'depthai={DEPTHAI_CLI_MODULE_NAME}.depthai_cli:cli' + ] + } ) diff --git a/src/DatatypeBindings.cpp b/src/DatatypeBindings.cpp index fab061f01..1e4ea63a6 100644 --- a/src/DatatypeBindings.cpp +++ b/src/DatatypeBindings.cpp @@ -25,6 +25,8 @@ void bind_systeminformation(pybind11::module& m, void* pCallstack); void bind_trackedfeatures(pybind11::module& m, void* pCallstack); void bind_tofconfig(pybind11::module& m, void* pCallstack); void bind_tracklets(pybind11::module& m, void* pCallstack); +void bind_pointcloudconfig(pybind11::module& m, void* pCallstack); +void bind_pointclouddata(pybind11::module& m, void* pCallstack); void DatatypeBindings::addToCallstack(std::deque& callstack) { // Bind common datatypebindings @@ -53,6 +55,8 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_trackedfeatures); callstack.push_front(bind_tofconfig); callstack.push_front(bind_tracklets); + callstack.push_front(bind_pointcloudconfig); + callstack.push_front(bind_pointclouddata); } void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){ @@ -95,6 +99,8 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){ .value("StereoDepthConfig", DatatypeEnum::StereoDepthConfig) .value("FeatureTrackerConfig", DatatypeEnum::FeatureTrackerConfig) .value("TrackedFeatures", DatatypeEnum::TrackedFeatures) + .value("PointCloudConfig", DatatypeEnum::PointCloudConfig) + .value("PointCloudData", DatatypeEnum::PointCloudData) ; } diff --git a/src/DeviceBindings.cpp b/src/DeviceBindings.cpp index f994e9022..b92831cf3 100644 --- a/src/DeviceBindings.cpp +++ b/src/DeviceBindings.cpp @@ -614,6 +614,8 @@ void DeviceBindings::bind(pybind11::module& m, void* pCallstack){ .def("getConnectionInterfaces", [](DeviceBase& d) { py::gil_scoped_release release; return d.getConnectionInterfaces(); }, DOC(dai, DeviceBase, getConnectionInterfaces)) .def("getConnectedCameraFeatures", [](DeviceBase& d) { py::gil_scoped_release release; return d.getConnectedCameraFeatures(); }, DOC(dai, DeviceBase, getConnectedCameraFeatures)) .def("getCameraSensorNames", [](DeviceBase& d) { py::gil_scoped_release release; return d.getCameraSensorNames(); }, DOC(dai, DeviceBase, getCameraSensorNames)) + .def("getStereoPairs", [](DeviceBase& d) { py::gil_scoped_release release; return d.getStereoPairs(); }, DOC(dai, DeviceBase, getStereoPairs)) + .def("getAvailableStereoPairs", [](DeviceBase& d) { py::gil_scoped_release release; return d.getAvailableStereoPairs(); }, DOC(dai, DeviceBase, getAvailableStereoPairs)) .def("getConnectedIMU", [](DeviceBase& d) { py::gil_scoped_release release; return d.getConnectedIMU(); }, DOC(dai, DeviceBase, getConnectedIMU)) .def("getIMUFirmwareVersion", [](DeviceBase& d) { py::gil_scoped_release release; return d.getIMUFirmwareVersion(); }, DOC(dai, DeviceBase, getIMUFirmwareVersion)) .def("getEmbeddedIMUFirmwareVersion", [](DeviceBase& d) { py::gil_scoped_release release; return d.getEmbeddedIMUFirmwareVersion(); }, DOC(dai, DeviceBase, getEmbeddedIMUFirmwareVersion)) diff --git a/src/pipeline/CommonBindings.cpp b/src/pipeline/CommonBindings.cpp index 5e03660d6..3aa29751c 100644 --- a/src/pipeline/CommonBindings.cpp +++ b/src/pipeline/CommonBindings.cpp @@ -23,6 +23,7 @@ #include "depthai-shared/common/DetectionParserOptions.hpp" #include "depthai-shared/common/RotatedRect.hpp" #include "depthai-shared/common/Rect.hpp" +#include "depthai-shared/common/StereoPair.hpp" #include "depthai-shared/common/Colormap.hpp" #include "depthai-shared/common/FrameEvent.hpp" #include "depthai-shared/common/Interpolation.hpp" @@ -30,6 +31,7 @@ // depthai #include "depthai/common/CameraFeatures.hpp" #include "depthai/common/CameraExposureOffset.hpp" +#include "depthai/common/StereoPair.hpp" #include "depthai/utility/ProfilingData.hpp" void CommonBindings::bind(pybind11::module& m, void* pCallstack){ @@ -61,6 +63,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack){ py::class_ detectionParserOptions(m, "DetectionParserOptions", DOC(dai, DetectionParserOptions)); py::class_ rotatedRect(m, "RotatedRect", DOC(dai, RotatedRect)); py::class_ rect(m, "Rect", DOC(dai, Rect)); + py::class_ stereoPair(m, "StereoPair", DOC(dai, StereoPair)); py::enum_ cameraExposureOffset(m, "CameraExposureOffset"); py::enum_ colormap(m, "Colormap", DOC(dai, Colormap)); py::enum_ frameEvent(m, "FrameEvent", DOC(dai, FrameEvent)); @@ -108,6 +111,19 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack){ .def_readwrite("height", &Rect::height) ; + stereoPair + .def(py::init<>()) + .def_readwrite("left", &StereoPair::left) + .def_readwrite("right", &StereoPair::right) + .def_readwrite("baseline", &StereoPair::baseline) + .def_readwrite("isVertical", &StereoPair::isVertical) + .def("__repr__", [](StereoPair& stereoPair) { + std::stringstream stream; + stream << stereoPair; + return stream.str(); + }) + ; + timestamp .def(py::init<>()) .def_readwrite("sec", &Timestamp::sec) @@ -213,6 +229,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack){ .def_readwrite("hasAutofocusIC", &CameraFeatures::hasAutofocusIC) .def_readwrite("name", &CameraFeatures::name) .def_readwrite("configs", &CameraFeatures::configs) + .def_readwrite("calibrationResolution", &CameraFeatures::calibrationResolution) .def("__repr__", [](CameraFeatures& camera) { std::stringstream stream; stream << camera; @@ -228,6 +245,12 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack){ .def_readwrite("minFps", &CameraSensorConfig::minFps) .def_readwrite("maxFps", &CameraSensorConfig::maxFps) .def_readwrite("type", &CameraSensorConfig::type) + .def_readwrite("fov", &CameraSensorConfig::fov) + .def("__repr__", [](CameraSensorConfig& config) { + std::stringstream stream; + stream << config; + return stream.str(); + }) ; // MemoryInfo diff --git a/src/pipeline/datatype/CameraControlBindings.cpp b/src/pipeline/datatype/CameraControlBindings.cpp index 07a77d843..ebaf0e751 100644 --- a/src/pipeline/datatype/CameraControlBindings.cpp +++ b/src/pipeline/datatype/CameraControlBindings.cpp @@ -177,6 +177,7 @@ std::vector camCtrlAttr; .def_readwrite("cmdMask", &RawCameraControl::cmdMask) .def_readwrite("autoFocusMode", &RawCameraControl::autoFocusMode) .def_readwrite("lensPosition", &RawCameraControl::lensPosition) + .def_readwrite("lensPositionRaw", &RawCameraControl::lensPositionRaw) .def_readwrite("expManual", &RawCameraControl::expManual) .def_readwrite("afRegion", &RawCameraControl::afRegion) .def_readwrite("awbMode", &RawCameraControl::awbMode) @@ -218,6 +219,7 @@ std::vector camCtrlAttr; .def("setAutoFocusLensRange", &CameraControl::setAutoFocusLensRange, py::arg("infinityPosition"), py::arg("macroPosition"), DOC(dai, CameraControl, setAutoFocusLensRange)) .def("setAutoFocusRegion", &CameraControl::setAutoFocusRegion, py::arg("startX"), py::arg("startY"), py::arg("width"), py::arg("height"), DOC(dai, CameraControl, setAutoFocusRegion)) .def("setManualFocus", &CameraControl::setManualFocus, py::arg("lensPosition"), DOC(dai, CameraControl, setManualFocus)) + .def("setManualFocusRaw", &CameraControl::setManualFocusRaw, py::arg("lensPositionRaw"), DOC(dai, CameraControl, setManualFocusRaw)) .def("setAutoExposureEnable", &CameraControl::setAutoExposureEnable, DOC(dai, CameraControl, setAutoExposureEnable)) .def("setAutoExposureLock", &CameraControl::setAutoExposureLock, py::arg("lock"), DOC(dai, CameraControl, setAutoExposureLock)) .def("setAutoExposureRegion", &CameraControl::setAutoExposureRegion, py::arg("startX"), py::arg("startY"), py::arg("width"), py::arg("height"), DOC(dai, CameraControl, setAutoExposureRegion)) @@ -246,6 +248,7 @@ std::vector camCtrlAttr; .def("getExposureTime", &CameraControl::getExposureTime, DOC(dai, CameraControl, getExposureTime)) .def("getSensitivity", &CameraControl::getSensitivity, DOC(dai, CameraControl, getSensitivity)) .def("getLensPosition", &CameraControl::getLensPosition, DOC(dai, CameraControl, getLensPosition)) + .def("getLensPositionRaw", &CameraControl::getLensPositionRaw, DOC(dai, CameraControl, getLensPositionRaw)) .def("get", &CameraControl::get, DOC(dai, CameraControl, get)) ; // Add also enum attributes from RawCameraControl diff --git a/src/pipeline/datatype/EncodedFrameBindings.cpp b/src/pipeline/datatype/EncodedFrameBindings.cpp index 91bda7f1f..546fb93eb 100644 --- a/src/pipeline/datatype/EncodedFrameBindings.cpp +++ b/src/pipeline/datatype/EncodedFrameBindings.cpp @@ -99,6 +99,8 @@ void bind_encodedframe(pybind11::module &m, void *pCallstack) { DOC(dai, EncodedFrame, getColorTemperature)) .def("getLensPosition", &EncodedFrame::getLensPosition, DOC(dai, EncodedFrame, getLensPosition)) + .def("getLensPositionRaw", &EncodedFrame::getLensPositionRaw, + DOC(dai, EncodedFrame, getLensPositionRaw)) .def("getQuality", &EncodedFrame::getQuality, DOC(dai, EncodedFrame, getQuality)) .def("getBitrate", &EncodedFrame::getBitrate, diff --git a/src/pipeline/datatype/ImgFrameBindings.cpp b/src/pipeline/datatype/ImgFrameBindings.cpp index 98c87d2b5..e3dbd67e2 100644 --- a/src/pipeline/datatype/ImgFrameBindings.cpp +++ b/src/pipeline/datatype/ImgFrameBindings.cpp @@ -131,6 +131,7 @@ void bind_imgframe(pybind11::module& m, void* pCallstack){ .def("getSensitivity", &ImgFrame::getSensitivity, DOC(dai, ImgFrame, getSensitivity)) .def("getColorTemperature", &ImgFrame::getColorTemperature, DOC(dai, ImgFrame, getColorTemperature)) .def("getLensPosition", &ImgFrame::getLensPosition, DOC(dai, ImgFrame, getLensPosition)) + .def("getLensPositionRaw", &ImgFrame::getLensPositionRaw, DOC(dai, ImgFrame, getLensPositionRaw)) // OpenCV Support section .def("setFrame", [](dai::ImgFrame& frm, py::array arr){ diff --git a/src/pipeline/datatype/PointCloudConfigBindings.cpp b/src/pipeline/datatype/PointCloudConfigBindings.cpp new file mode 100644 index 000000000..b7f03d8a2 --- /dev/null +++ b/src/pipeline/datatype/PointCloudConfigBindings.cpp @@ -0,0 +1,63 @@ +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" +#include +#include + +// depthai +#include "depthai/pipeline/datatype/PointCloudConfig.hpp" + +//pybind +#include +#include + +// #include "spdlog/spdlog.h" + +void bind_pointcloudconfig(pybind11::module& m, void* pCallstack){ + + using namespace dai; + + py::class_> rawConfig(m, "RawPointCloudConfig", DOC(dai, RawPointCloudConfig)); + py::class_> config(m, "PointCloudConfig", DOC(dai, PointCloudConfig)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // 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 + rawConfig + .def(py::init<>()) + .def_readwrite("sparse", &RawPointCloudConfig::sparse, DOC(dai, RawPointCloudConfig, sparse)) + .def_readwrite("transformationMatrix", &RawPointCloudConfig::transformationMatrix, DOC(dai, RawPointCloudConfig, transformationMatrix)) + ; + + // Message + config + .def(py::init<>()) + .def(py::init>()) + + .def("set", &PointCloudConfig::set, py::arg("config"), DOC(dai, PointCloudConfig, set)) + .def("get", &PointCloudConfig::get, DOC(dai, PointCloudConfig, get)) + .def("getSparse", &PointCloudConfig::getSparse, DOC(dai, PointCloudConfig, getSparse)) + .def("getTransformationMatrix", &PointCloudConfig::getTransformationMatrix, DOC(dai, PointCloudConfig, getTransformationMatrix)) + .def("setSparse", &PointCloudConfig::setSparse, DOC(dai, PointCloudConfig, setSparse)) + .def("setTransformationMatrix", [](PointCloudConfig& cfg, std::array, 3> mat) { + return cfg.setTransformationMatrix(mat); + }, DOC(dai, PointCloudConfig, setTransformationMatrix)) + + .def("setTransformationMatrix", [](PointCloudConfig& cfg, std::array, 4> mat) { + return cfg.setTransformationMatrix(mat); + }, DOC(dai, PointCloudConfig, setTransformationMatrix)) + ; + + // add aliases + +} diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp new file mode 100644 index 000000000..99a340571 --- /dev/null +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -0,0 +1,98 @@ +#include "DatatypeBindings.hpp" +#include "depthai-shared/datatype/RawPointCloudData.hpp" +#include "pipeline/CommonBindings.hpp" +#include +#include + +// depthai +#include "depthai/pipeline/datatype/PointCloudData.hpp" + +//pybind +#include +#include + +// #include "spdlog/spdlog.h" + +void bind_pointclouddata(pybind11::module& m, void* pCallstack){ + + using namespace dai; + + py::class_> rawPointCloudData(m, "RawPointCloudData", DOC(dai, RawPointCloudData)); + py::class_> pointCloudData(m, "PointCloudData", DOC(dai, PointCloudData)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // 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 + rawPointCloudData + .def(py::init<>()) + .def_readwrite("width", &RawPointCloudData::width) + .def_readwrite("height", &RawPointCloudData::height) + .def_readwrite("sparse", &RawPointCloudData::sparse) + .def_readwrite("instanceNum", &RawPointCloudData::instanceNum) + .def_readwrite("minx", &RawPointCloudData::minx) + .def_readwrite("miny", &RawPointCloudData::miny) + .def_readwrite("minz", &RawPointCloudData::minz) + .def_readwrite("maxx", &RawPointCloudData::maxx) + .def_readwrite("maxy", &RawPointCloudData::maxy) + .def_readwrite("maxz", &RawPointCloudData::maxz) + ; + + // Message + pointCloudData + .def(py::init<>()) + .def_property("points", [](PointCloudData& data) { return &data.getPoints(); }, [](PointCloudData& data, std::vector points) {data.getPoints() = points;}) + .def("getPoints", [](py::object &obj){ + // creates numpy array (zero-copy) which holds correct information such as shape, ... + dai::PointCloudData& data = obj.cast(); + Point3f* points = (Point3f*)data.getData().data(); + unsigned long size = data.getData().size() / sizeof(Point3f); + py::array_t arr({size, 3UL}); + auto ra = arr.mutable_unchecked(); + for (int i = 0; i < size; i++) { + ra(i, 0) = points[i].x; + ra(i, 1) = points[i].y; + ra(i, 2) = points[i].z; + } + return arr; + }) + .def("getWidth", &PointCloudData::getWidth, DOC(dai, PointCloudData, getWidth)) + .def("getHeight", &PointCloudData::getHeight, DOC(dai, PointCloudData, getHeight)) + .def("isSparse", &PointCloudData::isSparse, DOC(dai, PointCloudData, isSparse)) + .def("getMinX", &PointCloudData::getMinX, DOC(dai, PointCloudData, getMinX)) + .def("getMinY", &PointCloudData::getMinY, DOC(dai, PointCloudData, getMinY)) + .def("getMinZ", &PointCloudData::getMinZ, DOC(dai, PointCloudData, getMinZ)) + .def("getMaxX", &PointCloudData::getMaxX, DOC(dai, PointCloudData, getMaxX)) + .def("getMaxY", &PointCloudData::getMaxY, DOC(dai, PointCloudData, getMaxY)) + .def("getMaxZ", &PointCloudData::getMaxZ, DOC(dai, PointCloudData, getMaxZ)) + .def("getInstanceNum", &PointCloudData::getInstanceNum, DOC(dai, PointCloudData, getInstanceNum)) + .def("getTimestamp", &PointCloudData::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp)) + .def("getTimestampDevice", &PointCloudData::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice)) + .def("getSequenceNum", &PointCloudData::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum)) + .def("setWidth", &PointCloudData::setWidth, DOC(dai, PointCloudData, setWidth)) + .def("setHeight", &PointCloudData::setHeight, DOC(dai, PointCloudData, setHeight)) + .def("setSize", static_cast(&PointCloudData::setSize), py::arg("width"), py::arg("height"), DOC(dai, PointCloudData, setSize)) + .def("setSize", static_cast)>(&PointCloudData::setSize), py::arg("size"), DOC(dai, PointCloudData, setSize, 2)) + .def("setMinX", &PointCloudData::setMinX, DOC(dai, PointCloudData, setMinX)) + .def("setMinY", &PointCloudData::setMinY, DOC(dai, PointCloudData, setMinY)) + .def("setMinZ", &PointCloudData::setMinZ, DOC(dai, PointCloudData, setMinZ)) + .def("setMaxX", &PointCloudData::setMaxX, DOC(dai, PointCloudData, setMaxX)) + .def("setMaxY", &PointCloudData::setMaxY, DOC(dai, PointCloudData, setMaxY)) + .def("setMaxZ", &PointCloudData::setMaxZ, DOC(dai, PointCloudData, setMaxZ)) + .def("setInstanceNum", &PointCloudData::setInstanceNum, DOC(dai, PointCloudData, setInstanceNum)) + .def("setTimestamp", &PointCloudData::setTimestamp, DOC(dai, PointCloudData, setTimestamp)) + .def("setTimestampDevice", &PointCloudData::setTimestampDevice, DOC(dai, PointCloudData, setTimestampDevice)) + .def("setSequenceNum", &PointCloudData::setSequenceNum, DOC(dai, PointCloudData, setSequenceNum)) + ; + +} diff --git a/src/pipeline/node/NodeBindings.cpp b/src/pipeline/node/NodeBindings.cpp index 72eb93356..543a78a9f 100644 --- a/src/pipeline/node/NodeBindings.cpp +++ b/src/pipeline/node/NodeBindings.cpp @@ -116,6 +116,7 @@ void bind_apriltag(pybind11::module& m, void* pCallstack); void bind_detectionparser(pybind11::module& m, void* pCallstack); void bind_uvc(pybind11::module& m, void* pCallstack); void bind_tof(pybind11::module& m, void* pCallstack); +void bind_pointcloud(pybind11::module& m, void* pCallstack); void bind_sync(pybind11::module& m, void* pCallstack); void bind_messagedemux(pybind11::module& m, void* pCallstack); @@ -149,6 +150,7 @@ void NodeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_detectionparser); callstack.push_front(bind_uvc); callstack.push_front(bind_tof); + callstack.push_front(bind_pointcloud); callstack.push_front(bind_sync); callstack.push_front(bind_messagedemux); } diff --git a/src/pipeline/node/PointCloudBindings.cpp b/src/pipeline/node/PointCloudBindings.cpp new file mode 100644 index 000000000..63ad94393 --- /dev/null +++ b/src/pipeline/node/PointCloudBindings.cpp @@ -0,0 +1,48 @@ +#include "NodeBindings.hpp" +#include "Common.hpp" + +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/Node.hpp" +#include "depthai/pipeline/node/PointCloud.hpp" + +void bind_pointcloud(pybind11::module& m, void* pCallstack){ + + using namespace dai; + using namespace dai::node; + + // Node and Properties declare upfront + py::class_ properties(m, "PointCloudProperties", DOC(dai, PointCloudProperties)); + auto node = ADD_NODE(PointCloud); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // 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 + properties + .def_readwrite("initialConfig", &PointCloudProperties::initialConfig, DOC(dai, PointCloudProperties, initialConfig)) + .def_readwrite("numFramesPool", &PointCloudProperties::numFramesPool, DOC(dai, PointCloudProperties, numFramesPool)) + ; + + // Node + node + .def_readonly("inputConfig", &PointCloud::inputConfig, DOC(dai, node, PointCloud, inputConfig), DOC(dai, node, PointCloud, inputConfig)) + .def_readonly("inputDepth", &PointCloud::inputDepth, DOC(dai, node, PointCloud, inputDepth), DOC(dai, node, PointCloud, inputDepth)) + .def_readonly("outputPointCloud", &PointCloud::outputPointCloud, DOC(dai, node, PointCloud, outputPointCloud), DOC(dai, node, PointCloud, outputPointCloud)) + .def_readonly("passthroughDepth", &PointCloud::passthroughDepth, DOC(dai, node, PointCloud, passthroughDepth), DOC(dai, node, PointCloud, passthroughDepth)) + .def_readonly("initialConfig", &PointCloud::initialConfig, DOC(dai, node, PointCloud, initialConfig), DOC(dai, node, PointCloud, initialConfig)) + .def("setNumFramesPool", &PointCloud::setNumFramesPool, DOC(dai, node, PointCloud, setNumFramesPool)) + ; + // ALIAS + daiNodeModule.attr("PointCloud").attr("Properties") = properties; + +} diff --git a/src/pipeline/node/ToFBindings.cpp b/src/pipeline/node/ToFBindings.cpp index ae67b149e..21199a3fc 100644 --- a/src/pipeline/node/ToFBindings.cpp +++ b/src/pipeline/node/ToFBindings.cpp @@ -38,6 +38,7 @@ void bind_tof(pybind11::module& m, void* pCallstack){ .def_readonly("input", &ToF::input, DOC(dai, node, ToF, input), DOC(dai, node, ToF, input)) .def_readonly("depth", &ToF::depth, DOC(dai, node, ToF, depth), DOC(dai, node, ToF, depth)) .def_readonly("amplitude", &ToF::amplitude, DOC(dai, node, ToF, amplitude), DOC(dai, node, ToF, amplitude)) + .def_readonly("intensity", &ToF::intensity, DOC(dai, node, ToF, intensity), DOC(dai, node, ToF, intensity)) .def_readonly("error", &ToF::error, DOC(dai, node, ToF, error), DOC(dai, node, ToF, error)) .def_readonly("initialConfig", &ToF::initialConfig, DOC(dai, node, ToF, initialConfig), DOC(dai, node, ToF, initialConfig)) ; diff --git a/utilities/cam_test.py b/utilities/cam_test.py index 1f47523a9..c12b79cec 100755 --- a/utilities/cam_test.py +++ b/utilities/cam_test.py @@ -37,8 +37,9 @@ '/' to toggle printing camera settings: exposure, ISO, lens position, color temperature """ +import depthai as dai import os -#os.environ["DEPTHAI_LEVEL"] = "debug" +# os.environ["DEPTHAI_LEVEL"] = "debug" import cv2 import numpy as np @@ -49,25 +50,28 @@ from pathlib import Path import sys import signal +from stress_test import stress_test, YOLO_LABELS, create_yolo + +ALL_SOCKETS = ['rgb', 'left', 'right', 'cama', 'camb', 'camc', 'camd', 'came'] def socket_type_pair(arg): socket, type = arg.split(',') - if not (socket in ['rgb', 'left', 'right', 'cama', 'camb', 'camc', 'camd']): + if not (socket in ALL_SOCKETS): raise ValueError("") - if not (type in ['m', 'mono', 'c', 'color', 't', 'tof']): + if not (type in ['m', 'mono', 'c', 'color', 't', 'tof', 'th', 'thermal']): raise ValueError("") is_color = True if type in ['c', 'color'] else False is_tof = True if type in ['t', 'tof'] else False - return [socket, is_color, is_tof] + is_thermal = True if type in ['th', 'thermal'] else False + return [socket, is_color, is_tof, is_thermal] -parser = argparse.ArgumentParser() +parser = argparse.ArgumentParser(add_help=False) parser.add_argument('-cams', '--cameras', type=socket_type_pair, nargs='+', - default=[['rgb', True, False], ['left', False, False], - ['right', False, False], ['camd', True, False]], - help="Which camera sockets to enable, and type: c[olor] / m[ono] / t[of]. " - "E.g: -cams rgb,m right,c . Default: rgb,c left,m right,m camd,c") + default=[], + help="Which camera sockets to enable, and type: c[olor] / m[ono] / t[of] / th[ermal]. " + "E.g: -cams rgb,m right,c . If not specified, all connected cameras will be used.") parser.add_argument('-mres', '--mono-resolution', type=int, default=800, choices={480, 400, 720, 800}, help="Select mono camera resolution (height). Default: %(default)s") parser.add_argument('-cres', '--color-resolution', default='1080', choices={'720', '800', '1080', '1012', '1200', '1520', '4k', '5mp', '12mp', '13mp', '48mp'}, @@ -108,38 +112,50 @@ def socket_type_pair(arg): parser.add_argument('-btimeout', '--boot-timeout', default=30000, help="Boot timeout in ms. Default: %(default)s (sets DEPTHAI_BOOT_TIMEOUT environment variable)") +parser.add_argument('-stress', action='store_true', + help="Run stress test. This will override all other options (except -d/--device) and will run a heavy pipeline until the user stops it.") + +parser.add_argument("-stereo", action="store_true", default=False, + help="Create a stereo depth node if the device has a stereo pair.") + +parser.add_argument("-yolo", type=str, default="", + help=f"Create a yolo detection network on the specified camera. E.g: -yolo cama. Available cameras: {ALL_SOCKETS}") + +parser.add_argument("-gui", action="store_true", + help="Use GUI instead of CLI") +parser.add_argument("-h", "--help", action="store_true", default=False, + help="Show this help message and exit") # So you can forward --help to stress test, without it being consumed by cam_test.py + args = parser.parse_args() # Set timeouts before importing depthai os.environ["DEPTHAI_CONNECTION_TIMEOUT"] = str(args.connection_timeout) os.environ["DEPTHAI_BOOT_TIMEOUT"] = str(args.boot_timeout) -import depthai as dai -if len(sys.argv) == 1: +if args.stress: + stress_test(args.device) + exit(0) + +if args.help: + parser.print_help() + exit(0) + +if args.gui: import cam_test_gui cam_test_gui.main() -cam_list = [] -cam_type_color = {} -cam_type_tof = {} -print("Enabled cameras:") -for socket, is_color, is_tof in args.cameras: - cam_list.append(socket) - cam_type_color[socket] = is_color - cam_type_tof[socket] = is_tof - print(socket.rjust(7), ':', 'tof' if is_tof else 'color' if is_color else 'mono') - print("DepthAI version:", dai.__version__) print("DepthAI path:", dai.__file__) cam_socket_opts = { - 'rgb' : dai.CameraBoardSocket.CAM_A, - 'left' : dai.CameraBoardSocket.CAM_B, + 'rgb': dai.CameraBoardSocket.CAM_A, + 'left': dai.CameraBoardSocket.CAM_B, 'right': dai.CameraBoardSocket.CAM_C, - 'cama' : dai.CameraBoardSocket.CAM_A, - 'camb' : dai.CameraBoardSocket.CAM_B, - 'camc' : dai.CameraBoardSocket.CAM_C, - 'camd' : dai.CameraBoardSocket.CAM_D, + 'cama': dai.CameraBoardSocket.CAM_A, + 'camb': dai.CameraBoardSocket.CAM_B, + 'camc': dai.CameraBoardSocket.CAM_C, + 'camd': dai.CameraBoardSocket.CAM_D, + 'came': dai.CameraBoardSocket.CAM_E, } rotate = { @@ -150,6 +166,7 @@ def socket_type_pair(arg): 'camb': args.rotate in ['all', 'mono'], 'camc': args.rotate in ['all', 'mono'], 'camd': args.rotate in ['all', 'rgb'], + 'came': args.rotate in ['all', 'mono'], } mono_res_opts = { @@ -174,10 +191,13 @@ def socket_type_pair(arg): '48mp': dai.ColorCameraProperties.SensorResolution.THE_48_MP, } + def clamp(num, v0, v1): return max(v0, min(num, v1)) # Calculates FPS over a moving window, configurable + + class FPS: def __init__(self, window_size=30): self.dq = collections.deque(maxlen=window_size) @@ -210,115 +230,231 @@ def next(self): def prev(self): return self.step(-1) -# Start defining a pipeline -pipeline = dai.Pipeline() -# Uncomment to get better throughput -# pipeline.setXLinkChunkSize(0) - -control = pipeline.createXLinkIn() -control.setStreamName('control') - -xinTofConfig = pipeline.createXLinkIn() -xinTofConfig.setStreamName('tofConfig') - -cam = {} -tof = {} -xout = {} -xout_raw = {} -xout_tof_amp = {} -streams = [] -tofConfig = {} -for c in cam_list: - tofEnableRaw = False - xout[c] = pipeline.createXLinkOut() - xout[c].setStreamName(c) - streams.append(c) - if cam_type_tof[c]: - cam[c] = pipeline.create(dai.node.ColorCamera) # .Camera - if args.tof_raw: - tofEnableRaw = True - else: - tof[c] = pipeline.create(dai.node.ToF) - cam[c].raw.link(tof[c].input) - tof[c].depth.link(xout[c].input) - xinTofConfig.out.link(tof[c].inputConfig) - tofConfig = tof[c].initialConfig.get() - tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN - tofConfig.depthParams.avgPhaseShuffle = False - tofConfig.depthParams.minimumAmplitude = 3.0 - - if args.tof_median == 0: - tofConfig.depthParams.median = dai.MedianFilter.MEDIAN_OFF - elif args.tof_median == 3: - tofConfig.depthParams.median = dai.MedianFilter.KERNEL_3x3 - elif args.tof_median == 5: - tofConfig.depthParams.median = dai.MedianFilter.KERNEL_5x5 - elif args.tof_median == 7: - tofConfig.depthParams.median = dai.MedianFilter.KERNEL_7x7 - - tof[c].initialConfig.set(tofConfig) - if args.tof_amplitude: - amp_name = 'tof_amplitude_' + c - xout_tof_amp[c] = pipeline.create(dai.node.XLinkOut) - xout_tof_amp[c].setStreamName(amp_name) - streams.append(amp_name) - tof[c].amplitude.link(xout_tof_amp[c].input) - elif cam_type_color[c]: - cam[c] = pipeline.createColorCamera() - cam[c].setResolution(color_res_opts[args.color_resolution]) - cam[c].setIspScale(1, args.isp_downscale) - # cam[c].initialControl.setManualFocus(85) # TODO - if args.rgb_preview: - cam[c].preview.link(xout[c].input) - else: - cam[c].isp.link(xout[c].input) - else: - cam[c] = pipeline.createMonoCamera() - cam[c].setResolution(mono_res_opts[args.mono_resolution]) - cam[c].out.link(xout[c].input) - cam[c].setBoardSocket(cam_socket_opts[c]) - # Num frames to capture on trigger, with first to be discarded (due to degraded quality) - # cam[c].initialControl.setExternalTrigger(2, 1) - # cam[c].initialControl.setStrobeExternal(48, 1) - # cam[c].initialControl.setFrameSyncMode(dai.CameraControl.FrameSyncMode.INPUT) - - # cam[c].initialControl.setManualExposure(15000, 400) # exposure [us], iso - # When set, takes effect after the first 2 frames - # cam[c].initialControl.setManualWhiteBalance(4000) # light temperature in K, 1000..12000 - # cam[c].initialControl.setAutoExposureLimit(5000) # can also be updated at runtime - control.out.link(cam[c].inputControl) - if rotate[c]: - cam[c].setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) - cam[c].setFps(args.fps) - if args.isp3afps: - cam[c].setIsp3aFps(args.isp3afps) - - if args.enable_raw or tofEnableRaw: - raw_name = 'raw_' + c - xout_raw[c] = pipeline.create(dai.node.XLinkOut) - xout_raw[c].setStreamName(raw_name) - streams.append(raw_name) - cam[c].raw.link(xout_raw[c].input) - cam[c].setRawOutputPacked(False) - -if args.camera_tuning: - pipeline.setCameraTuningBlobPath(str(args.camera_tuning)) - def exit_cleanly(signum, frame): print("Exiting cleanly") cv2.destroyAllWindows() sys.exit(0) -signal.signal(signal.SIGINT, exit_cleanly) + +def socket_to_socket_opt(socket: dai.CameraBoardSocket) -> str: + return str(socket).split('.')[-1].replace("_", "").lower() -# Pipeline is defined, now we can connect to the device -device = dai.Device.getDeviceByMxId(args.device) -dai_device_args = [pipeline] -if device[0]: - dai_device_args.append(device[1]) +signal.signal(signal.SIGINT, exit_cleanly) + +# Connect to device, so that we can get connected cameras in case of no args +success, device = dai.Device.getDeviceByMxId(args.device) +dai_device_args = [] +if success: + dai_device_args.append(device) with dai.Device(*dai_device_args) as device: - # print('Connected cameras:', [c.name for c in device.getConnectedCameras()]) + cam_list = [] + cam_type_color = {} + cam_type_tof = {} + cam_type_thermal = {} + + if not args.cameras: + connected_cameras = device.getConnectedCameraFeatures() + args.cameras = [(socket_to_socket_opt(cam.socket), cam.supportedTypes[0] == + dai.CameraSensorType.COLOR, cam.supportedTypes[0] == dai.CameraSensorType.TOF, cam.supportedTypes[0] == dai.CameraSensorType.THERMAL) for cam in connected_cameras] + if not args.cameras: + print("No cameras found!") + exit(1) + + print("Enabled cameras:") + for socket, is_color, is_tof, is_thermal in args.cameras: + cam_list.append(socket) + cam_type_color[socket] = is_color + cam_type_tof[socket] = is_tof + cam_type_thermal[socket] = is_thermal + print(socket.rjust(7), ':', 'tof' if is_tof else 'color' if is_color else 'thermal' if is_thermal else 'mono') + + # Start defining a pipeline + pipeline = dai.Pipeline() + # Uncomment to get better throughput + # pipeline.setXLinkChunkSize(0) + + control = pipeline.createXLinkIn() + control.setStreamName('control') + + xinTofConfig = pipeline.createXLinkIn() + xinTofConfig.setStreamName('tofConfig') + + cam = {} + tof = {} + xout = {} + xout_raw = {} + xout_tof_amp = {} + streams = [] + tofConfig = {} + yolo_passthrough_q_name = None + for c in cam_list: + print("CAM: ", c) + tofEnableRaw = False + xout[c] = pipeline.createXLinkOut() + xout[c].setStreamName(c) + streams.append(c) + if cam_type_tof[c]: + cam[c] = pipeline.create(dai.node.ColorCamera) # .Camera + if args.tof_raw: + tofEnableRaw = True + else: + tof[c] = pipeline.create(dai.node.ToF) + cam[c].raw.link(tof[c].input) + tof[c].depth.link(xout[c].input) + xinTofConfig.out.link(tof[c].inputConfig) + tofConfig = tof[c].initialConfig.get() + tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN + tofConfig.depthParams.avgPhaseShuffle = False + tofConfig.depthParams.minimumAmplitude = 3.0 + tof[c].initialConfig.set(tofConfig) + + if args.tof_median == 0: + tofConfig.depthParams.median = dai.MedianFilter.MEDIAN_OFF + elif args.tof_median == 3: + tofConfig.depthParams.median = dai.MedianFilter.KERNEL_3x3 + elif args.tof_median == 5: + tofConfig.depthParams.median = dai.MedianFilter.KERNEL_5x5 + elif args.tof_median == 7: + tofConfig.depthParams.median = dai.MedianFilter.KERNEL_7x7 + tof[c].initialConfig.set(tofConfig) + if args.tof_amplitude: + amp_name = 'tof_amplitude_' + c + xout_tof_amp[c] = pipeline.create(dai.node.XLinkOut) + xout_tof_amp[c].setStreamName(amp_name) + streams.append(amp_name) + tof[c].amplitude.link(xout_tof_amp[c].input) + elif cam_type_thermal[c]: + cam[c] = pipeline.create(dai.node.Camera) + cam[c].setBoardSocket(cam_socket_opts[c]) + cam[c].setPreviewSize(256, 192) + cam[c].raw.link(xout[c].input) + xout_preview = pipeline.create(dai.node.XLinkOut) + xout_preview.setStreamName('preview_' + c) + cam[c].preview.link(xout_preview.input) + streams.append('preview_' + c) + elif cam_type_color[c]: + cam[c] = pipeline.createColorCamera() + cam[c].setResolution(color_res_opts[args.color_resolution]) + cam[c].setIspScale(1, args.isp_downscale) + # cam[c].initialControl.setManualFocus(85) # TODO + if args.rgb_preview: + cam[c].preview.link(xout[c].input) + else: + cam[c].isp.link(xout[c].input) + if args.yolo == c: + yolo_passthrough_q_name, yolo_q_name = create_yolo(pipeline, cam[c]) + streams.append(yolo_q_name) + else: + cam[c] = pipeline.createMonoCamera() + cam[c].setResolution(mono_res_opts[args.mono_resolution]) + cam[c].out.link(xout[c].input) + cam[c].setBoardSocket(cam_socket_opts[c]) + # Num frames to capture on trigger, with first to be discarded (due to degraded quality) + # cam[c].initialControl.setExternalTrigger(2, 1) + # cam[c].initialControl.setStrobeExternal(48, 1) + # cam[c].initialControl.setFrameSyncMode(dai.CameraControl.FrameSyncMode.INPUT) + + # cam[c].initialControl.setManualExposure(15000, 400) # exposure [us], iso + # When set, takes effect after the first 2 frames + # cam[c].initialControl.setManualWhiteBalance(4000) # light temperature in K, 1000..12000 + control.out.link(cam[c].inputControl) + if rotate[c]: + cam[c].setImageOrientation( + dai.CameraImageOrientation.ROTATE_180_DEG) + cam[c].setFps(args.fps) + if args.isp3afps: + cam[c].setIsp3aFps(args.isp3afps) + + if args.enable_raw or tofEnableRaw: + raw_name = 'raw_' + c + xout_raw[c] = pipeline.create(dai.node.XLinkOut) + xout_raw[c].setStreamName(raw_name) + streams.append(raw_name) + cam[c].raw.link(xout_raw[c].input) + cam[c].setRawOutputPacked(False) + + if args.camera_tuning: + pipeline.setCameraTuningBlobPath(str(args.camera_tuning)) + + stereo = None + + if args.stereo: + try: + try: + calib = device.readCalibration2() + except: + raise Exception("Device is not calibrated.") + eeprom = calib.getEepromData() + left, right = eeprom.stereoRectificationData.leftCameraSocket, eeprom.stereoRectificationData.rightCameraSocket + # Get the actual camera nodes + # The cameras may have been specified with -cams rgb,c left,m right,m kind of names, so we need to handle these edge cases + left_sock_opt = socket_to_socket_opt(left) + right_sock_opt = socket_to_socket_opt(right) + left_cam = cam.get(left_sock_opt, None) + right_cam = cam.get(right_sock_opt, None) + if not left_cam: + if left == dai.CameraBoardSocket.CAM_A: + left_sock_opt = "rgb" + elif left == dai.CameraBoardSocket.CAM_B: + left_sock_opt = "left" + elif left == dai.CameraBoardSocket.CAM_C: + left_sock_opt = "right" + left_cam = cam.get(left_sock_opt, None) + if not right_cam: + if right == dai.CameraBoardSocket.CAM_A: + right_sock_opt = "rgb" + elif right == dai.CameraBoardSocket.CAM_B: + right_sock_opt = "left" + elif right == dai.CameraBoardSocket.CAM_C: + right_sock_opt = "right" + right_cam = cam.get(right_sock_opt, None) + + if left_cam and right_cam: + cam_features = device.getConnectedCameraFeatures() + left_cam_features = next( + filter(lambda c: c.socket == left, cam_features)) + right_cam_features = next( + filter(lambda c: c.socket == right, cam_features)) + if left_cam_features.width > 1280: + if args.isp_downscale == 1: + raise Exception( + "Can't create stereo depth with left cam width > 1280. Use --isp-downscale to downscale the image.") + if right_cam_features.width > 1280: + if args.isp_downscale == 1: + raise Exception( + "Can't create stereo depth with right cam width > 1280. Use --isp-downscale to downscale the image.") + left_out = "out" + right_out = "out" + if cam_type_color[left_sock_opt]: + left_out = "video" + if cam_type_color[right_sock_opt]: + right_out = "video" + + print( + "Device is calibrated and has a stereo pair, creating StereoDepth node.") + stereo = pipeline.createStereoDepth() + stereo.setDefaultProfilePreset( + dai.node.StereoDepth.PresetMode.HIGH_DENSITY) + stereo.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) + stereo.setLeftRightCheck(True) + stereo.setSubpixel(True) + stereo.setLeftRightCheck(True) + getattr(left_cam, left_out).link(stereo.left) + getattr(right_cam, right_out).link(stereo.right) + xout_stereo = pipeline.createXLinkOut() + depth_stream = "stereo_depth" + xout_stereo.setStreamName(depth_stream) + stereo.disparity.link(xout_stereo.input) + streams.append(depth_stream) + else: + print("Couldn't create stereo depth node. Device has invalid calibration.") + except Exception as e: + print("Couldn't create depth:", e) + + # Pipeline is defined, now we can start it + device.startPipeline(pipeline) + print('Connected cameras:') cam_name = {} for p in device.getConnectedCameraFeatures(): @@ -354,16 +490,16 @@ def exit_cleanly(signum, frame): # Manual exposure/focus set step EXP_STEP = 500 # us ISO_STEP = 50 - LENS_STEP = 3 + LENS_STEP = 1 / 1024 DOT_STEP = 0.05 FLOOD_STEP = 0.05 DOT_MAX = 1 FLOOD_MAX = 1 # Defaults and limits for manual focus/exposure controls - lensPos = 150 - lensMin = 0 - lensMax = 255 + lensPos = 0.59 + lensMin = 0.0 + lensMax = 1.0 expTime = 20000 expMin = 1 @@ -395,13 +531,17 @@ def exit_cleanly(signum, frame): control = 'none' show = args.show_meta - jet_custom = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) + jet_custom = cv2.applyColorMap( + np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) jet_custom[0] = [0, 0, 0] print("Cam:", *[' ' + c.ljust(8) - for c in cam_list], "[host | capture timestamp]") + for c in cam_list], "[host | capture timestamp]") capture_list = [] + yolo_passthrough_q = None + if yolo_passthrough_q_name is not None: + yolo_passthrough_q = device.getOutputQueue(yolo_passthrough_q_name, maxSize=1, blocking=False) while True: for c in streams: try: @@ -412,17 +552,45 @@ def exit_cleanly(signum, frame): if pkt is not None: fps_host[c].update() fps_capt[c].update(pkt.getTimestamp().total_seconds()) + if args.yolo and isinstance(pkt, dai.ImgDetections): + if yolo_passthrough_q is None: + continue + frame_pkt = yolo_passthrough_q.get() + frame = frame_pkt.getCvFrame() + if frame is None: + continue # No frame to draw on + for detection in pkt.detections: + bbox = np.array([detection.xmin * frame.shape[1], detection.ymin * frame.shape[0], detection.xmax * frame.shape[1], detection.ymax * frame.shape[0]], dtype=np.int32) + cv2.putText(frame, YOLO_LABELS[detection.label], (bbox[0] + 10, bbox[1] + 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) + cv2.putText(frame, f"{int(detection.confidence)}%", (bbox[0] + 10, bbox[1] + 40), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) + cv2.rectangle(frame, (bbox[0], bbox[1]), (bbox[2], bbox[3]), (255, 0, 0), 2) + cv2.imshow(c, frame) + continue width, height = pkt.getWidth(), pkt.getHeight() frame = pkt.getCvFrame() cam_skt = c.split('_')[-1] - if cam_type_tof[cam_skt] and not (c.startswith('raw_') or c.startswith('tof_amplitude_')): + + if c == "stereo_depth" and stereo is not None: + maxDisp = stereo.initialConfig.getMaxDisparity() + disp = (pkt.getCvFrame() * (255.0 / maxDisp)).astype(np.uint8) + disp = cv2.applyColorMap(disp, cv2.COLORMAP_JET) + cv2.imshow(c, disp) + continue + + + if cam_type_tof.get(cam_skt, None) and not (c.startswith('raw_') or c.startswith('tof_amplitude_')): if args.tof_cm: # pixels represent `cm`, capped to 255. Value can be checked hovering the mouse frame = (frame // 10).clip(0, 255).astype(np.uint8) else: frame = (frame.view(np.int16).astype(float)) - frame = cv2.normalize(frame, frame, alpha=255, beta=0, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) + frame = cv2.normalize( + frame, frame, alpha=255, beta=0, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) frame = cv2.applyColorMap(frame, jet_custom) + elif cam_type_thermal[cam_skt] and c.startswith('cam'): + frame = frame.astype(np.float32) + frame = cv2.normalize(frame, frame, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) + frame = cv2.applyColorMap(frame, cv2.COLORMAP_MAGMA) if show: txt = f"[{c:5}, {pkt.getSequenceNum():4}, {pkt.getTimestamp().total_seconds():.6f}] " txt += f"Exp: {pkt.getExposureTime().total_seconds()*1000:6.3f} ms, " @@ -454,8 +622,10 @@ def exit_cleanly(signum, frame): # Full range for display, use bits [15:6] of the 16-bit pixels type = pkt.getType() multiplier = 1 - if type == dai.ImgFrame.Type.RAW10: multiplier = (1 << (16-10)) - if type == dai.ImgFrame.Type.RAW12: multiplier = (1 << (16-4)) + if type == dai.ImgFrame.Type.RAW10: + multiplier = (1 << (16-10)) + if type == dai.ImgFrame.Type.RAW12: + multiplier = (1 << (16-4)) frame = frame * multiplier # Debayer as color for preview/png if cam_type_color[cam_skt]: @@ -476,7 +646,8 @@ def exit_cleanly(signum, frame): cv2.imwrite(filename, frame) cv2.imshow(c, frame) print("\rFPS:", - *["{:6.2f}|{:6.2f}".format(fps_host[c].get(), fps_capt[c].get()) for c in cam_list], + *["{:6.2f}|{:6.2f}".format(fps_host[c].get(), + fps_capt[c].get()) for c in cam_list], end=' ', flush=True) needs_newline = True @@ -491,13 +662,14 @@ def exit_cleanly(signum, frame): capture_list = streams.copy() capture_time = time.strftime('%Y%m%d_%H%M%S') elif key == ord('g') and tof: - f_mod = dai.RawToFConfig.DepthParams.TypeFMod.MAX if tofConfig.depthParams.freqModUsed == dai.RawToFConfig.DepthParams.TypeFMod.MIN else dai.RawToFConfig.DepthParams.TypeFMod.MIN + f_mod = dai.RawToFConfig.DepthParams.TypeFMod.MAX if tofConfig.depthParams.freqModUsed == dai.RawToFConfig.DepthParams.TypeFMod.MIN else dai.RawToFConfig.DepthParams.TypeFMod.MIN print("ToF toggling f_mod value to:", f_mod) tofConfig.depthParams.freqModUsed = f_mod tofCfgQueue.send(tofConfig) elif key == ord('h') and tof: tofConfig.depthParams.avgPhaseShuffle = not tofConfig.depthParams.avgPhaseShuffle - print("ToF toggling avgPhaseShuffle value to:", tofConfig.depthParams.avgPhaseShuffle) + print("ToF toggling avgPhaseShuffle value to:", + tofConfig.depthParams.avgPhaseShuffle) tofCfgQueue.send(tofConfig) elif key == ord('t'): print("Autofocus trigger (and disable continuous)") @@ -524,7 +696,7 @@ def exit_cleanly(signum, frame): lensPos = clamp(lensPos, lensMin, lensMax) print("Setting manual focus, lens position: ", lensPos) ctrl = dai.CameraControl() - ctrl.setManualFocus(lensPos) + ctrl.setManualFocusRaw(lensPos) controlQueue.send(ctrl) elif key in [ord('i'), ord('o'), ord('k'), ord('l')]: if key == ord('i'): @@ -669,7 +841,8 @@ def exit_cleanly(signum, frame): print("Chroma denoise:", chroma_denoise) ctrl.setChromaDenoise(chroma_denoise) elif control == 'tof_amplitude_min' and tof: - amp_min = clamp(tofConfig.depthParams.minimumAmplitude + change, 0, 50) + amp_min = clamp( + tofConfig.depthParams.minimumAmplitude + change, 0, 50) print("Setting min amplitude(confidence) to:", amp_min) tofConfig.depthParams.minimumAmplitude = amp_min tofCfgQueue.send(tofConfig) diff --git a/utilities/cam_test_gui.py b/utilities/cam_test_gui.py index 0617d8b90..1bdb0e89e 100644 --- a/utilities/cam_test_gui.py +++ b/utilities/cam_test_gui.py @@ -319,6 +319,8 @@ def connect(self): started_successfully, self.test_process_pid = self.test_process.startDetached( sys.executable, args, "") else: + if "-gui" in sys.argv: + sys.argv.remove("-gui") started_successfully, self.test_process_pid = self.test_process.startDetached( sys.executable, sys.argv + args, "") if not started_successfully: diff --git a/utilities/stress_test.py b/utilities/stress_test.py new file mode 100644 index 000000000..0dc43320e --- /dev/null +++ b/utilities/stress_test.py @@ -0,0 +1,502 @@ +import depthai as dai +from typing import Any, List, Optional, Tuple, Dict +import cv2 +import numpy as np +import signal + +def on_exit(sig, frame): + cv2.destroyAllWindows() + exit(0) + +signal.signal(signal.SIGINT, on_exit) + +color_resolutions: Dict[Tuple[int, int], dai.ColorCameraProperties.SensorResolution] = { + # IMX582 cropped + (5312, 6000): dai.ColorCameraProperties.SensorResolution.THE_5312X6000, + (4208, 3120): dai.ColorCameraProperties.SensorResolution.THE_13_MP, # AR214 + # IMX378, IMX477, IMX577 + (4056, 3040): dai.ColorCameraProperties.SensorResolution.THE_12_MP, + # IMX582 with binning enabled + (4000, 3000): dai.ColorCameraProperties.SensorResolution.THE_4000X3000, + (3840, 2160): dai.ColorCameraProperties.SensorResolution.THE_4_K, + (1920, 1200): dai.ColorCameraProperties.SensorResolution.THE_1200_P, # AR0234 + (1920, 1080): dai.ColorCameraProperties.SensorResolution.THE_1080_P, + (1440, 1080): dai.ColorCameraProperties.SensorResolution.THE_1440X1080, + (2592, 1944): dai.ColorCameraProperties.SensorResolution.THE_5_MP, # OV5645 + (1280, 800): dai.ColorCameraProperties.SensorResolution.THE_800_P, # OV9782 + (1280, 720): dai.ColorCameraProperties.SensorResolution.THE_720_P, +} + +YOLO_LABELS = [ + "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", + "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", + "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", + "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", + "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", + "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", + "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", + "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", + "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", + "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", + "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", + "teddy bear", "hair drier", "toothbrush" +] + +def print_system_information(info: dai.SystemInformation): + print( + "Ddr: used / total - %.2f / %.2f MiB" + % (info.ddrMemoryUsage.used + / (1024.0 * 1024.0), + info.ddrMemoryUsage.total / (1024.0 * 1024.0),) + ) + print( + "Cmx: used / total - %.2f / %.2f MiB" + % (info.cmxMemoryUsage.used + / (1024.0 * 1024.0), + info.cmxMemoryUsage.total / (1024.0 * 1024.0),) + ) + print( + "LeonCss heap: used / total - %.2f / %.2f MiB" + % (info.leonCssMemoryUsage.used + / (1024.0 * 1024.0), + info.leonCssMemoryUsage.total / (1024.0 * 1024.0),) + ) + print( + "LeonMss heap: used / total - %.2f / %.2f MiB" + % (info.leonMssMemoryUsage.used + / (1024.0 * 1024.0), + info.leonMssMemoryUsage.total / (1024.0 * 1024.0),) + ) + t = info.chipTemperature + print( + "Chip temperature - average: %.2f, css: %.2f, mss: %.2f, upa: %.2f, dss: %.2f" + % (t.average, + t.css, + t.mss, + t.upa, + t.dss,) + ) + print( + "Cpu usage - Leon CSS: %.2f %%, Leon MSS: %.2f %%" + % (info.leonCssCpuUsage.average + * 100, + info.leonMssCpuUsage.average * 100) + ) + + +def get_or_download_yolo_blob() -> str: + import os + import subprocess + import sys + from pathlib import Path + + this_file = os.path.realpath(__file__) + this_dir = os.path.dirname(this_file) + examples_dir = os.path.join(this_dir, "..", "examples") + models_dir = os.path.join(examples_dir, "models") + downloader_cmd = [sys.executable, f"{examples_dir}/downloader/downloader.py", "--name", "tiny-yolo", + "--cache_dir", f"{examples_dir}/downloader/", "--num_attempts", "5", "-o", f"{examples_dir}/models"] + subprocess.run(downloader_cmd, check=True) + blob_path = Path(os.path.join( + models_dir, "yolo-v4-tiny-tf_openvino_2021.4_6shave.blob")) + return str(Path.resolve(blob_path)) + + +def create_yolo(pipeline: dai.Pipeline, camera: dai.node.ColorCamera) -> Tuple[str, str]: + """Create a yolo detection network and return a tuple of (passthrough_q_name, yolo_q_name)""" + camera.setInterleaved(False) + camera.setPreviewSize(416, 416) + nn_blob_path = get_or_download_yolo_blob() + yoloDet = pipeline.create(dai.node.YoloDetectionNetwork) + yoloDet.setBlobPath(nn_blob_path) + # Yolo specific parameters + yoloDet.setConfidenceThreshold(0.5) + yoloDet.setNumClasses(80) + yoloDet.setCoordinateSize(4) + yoloDet.setAnchors([10,14, 23,27, 37,58, 81,82, 135,169, 344,319]) + yoloDet.setAnchorMasks({"side26": [1, 2, 3], "side13": [3, 4, 5]}) + yoloDet.setIouThreshold(0.5) + yoloDet.input.setBlocking(False) + camera.preview.link(yoloDet.input) + xoutColor = pipeline.createXLinkOut() + passthrough_q_name = f"preview_{camera.getBoardSocket()}" + xoutColor.setStreamName(passthrough_q_name) + yoloDet.passthrough.link(xoutColor.input) + xout_yolo = pipeline.createXLinkOut() + yolo_q_name = "yolo" + xout_yolo.setStreamName(yolo_q_name) + yoloDet.out.link(xout_yolo.input) + return (passthrough_q_name, yolo_q_name) + + +last_frame = {} # Store latest frame for each queue +jet_custom = cv2.applyColorMap( + np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET) +jet_custom[0] = [0, 0, 0] + +def clamp(num, v0, v1): + return max(v0, min(num, v1)) + +class PipelineContext: + q_name_yolo_passthrough: Optional[str] = None + """The name of the queue that the YOLO spatial detection network passthrough is connected to.""" + +def stress_test(mxid: str = ""): + # Parse args + import argparse + parser = argparse.ArgumentParser() + parser.add_argument("-ne", "--n-edge-detectors", default=0, type=int, help="Number of edge detectors to create.") + parser.add_argument("--no-nnet", action="store_true", default=False, help="Don't create a neural network.") + + # May have some unknown args + args, _ = parser.parse_known_args() + dot_intensity = 500 + flood_intensity = 500 + iso = 800 + exp_time = 20000 + + import time + success, device_info = dai.Device.getDeviceByMxId(mxid) + cam_args = [] # Device info or no args at all + if success: + cam_args.append(device_info) + with dai.Device(*cam_args) as device: + print("Setting default dot intensity to", dot_intensity) + device.setIrLaserDotProjectorBrightness(dot_intensity) + print("Setting default flood intensity to", flood_intensity) + device.setIrFloodLightBrightness(flood_intensity) + pipeline, outputs, pipeline_context = build_pipeline(device, args) + device.startPipeline(pipeline) + start_time = time.time() + queues = [device.getOutputQueue(name, size, False) + for name, size in outputs if name != "sys_log"] + camera_control_q = device.getInputQueue("cam_control") + sys_info_q = device.getOutputQueue("sys_log", 1, False) + usb_speed = device.getUsbSpeed() + while True: + for queue in queues: + packet = queue.tryGet() + # print("QUEUE", queue.getName(), "PACKET", packet) + if packet is not None: + if queue.getName() == "tof": + frame = packet.getCvFrame() + frame = (frame.view(np.int16).astype(float)) + frame = cv2.normalize( + frame, frame, alpha=255, beta=0, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U) + frame = cv2.applyColorMap(frame, jet_custom) + last_frame[queue.getName()] = frame + elif queue.getName() == "stereo depth": + frame = packet.getFrame() + depth_downscaled = frame[::4] + try: + min_depth = np.percentile( + depth_downscaled[depth_downscaled != 0], 1) + max_depth = np.percentile(depth_downscaled, 99) + except IndexError: + continue + frame = np.interp( + frame, (min_depth, max_depth), (0, 255)).astype(np.uint8) + frame = cv2.applyColorMap( + frame, jet_custom) + frame = cv2.applyColorMap(frame, jet_custom) + last_frame[queue.getName()] = frame + elif isinstance(packet, dai.ImgFrame): + # Skip encoded frames as decoding is heavy on the host machine + if packet.getType() == dai.ImgFrame.Type.BITSTREAM: + continue + else: + last_frame[queue.getName()] = packet.getCvFrame() + elif isinstance(packet, dai.ImgDetections): + frame = last_frame.get(pipeline_context.q_name_yolo_passthrough, None) + if frame is None: + continue # No frame to draw on + for detection in packet.detections: + bbox = np.array([detection.xmin * frame.shape[1], detection.ymin * frame.shape[0], detection.xmax * frame.shape[1], detection.ymax * frame.shape[0]], dtype=np.int32) + cv2.putText(frame, YOLO_LABELS[detection.label], (bbox[0] + 10, bbox[1] + 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) + cv2.putText(frame, f"{int(detection.confidence)}%", (bbox[0] + 10, bbox[1] + 40), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) + cv2.rectangle(frame, (bbox[0], bbox[1]), (bbox[2], bbox[3]), (255, 0, 0), 2) + sys_info: dai.SystemInformation = sys_info_q.tryGet() + if sys_info: + print("----------------------------------------") + print(f"[{int(time.time() - start_time)}s] Usb speed {usb_speed}") + print("----------------------------------------") + print_system_information(sys_info) + for name, frame in last_frame.items(): + cv2.imshow(name, frame) + + # Parse keyboard input + key = cv2.waitKey(1) + if key == ord("q"): + print("Q Pressed, exiting stress test...") + break + elif key == ord('a'): + dot_intensity = clamp(dot_intensity - 100, 0, 1200) + print("Decreasing dot intensity by 100, new value:", dot_intensity) + device.setIrLaserDotProjectorBrightness(dot_intensity) + elif key == ord('d'): + dot_intensity = clamp(dot_intensity + 100, 0, 1200) + print("Increasing dot intensity by 100, new value:", dot_intensity) + device.setIrLaserDotProjectorBrightness(dot_intensity) + elif key == ord('w'): + flood_intensity = clamp(flood_intensity + 100, 0, 1500) + print("Increasing flood intensity by 100, new value:", flood_intensity) + device.setIrFloodLightBrightness(flood_intensity) + elif key == ord('s'): + flood_intensity = clamp(flood_intensity - 100, 0, 1500) + print("Decreasing flood intensity by 100, new value:", flood_intensity) + device.setIrFloodLightBrightness(flood_intensity) + elif key == ord('k'): + iso = clamp(iso - 50, 0, 1600) + print("Decreasing iso by 50, new value:", iso) + cam_ctrl = dai.CameraControl() + cam_ctrl.setManualExposure(exp_time, iso) + camera_control_q.send(cam_ctrl) + elif key == ord('l'): + iso = clamp(iso + 50, 0, 1600) + print("Increasing iso by 50, new value:", iso) + cam_ctrl = dai.CameraControl() + cam_ctrl.setManualExposure(exp_time, iso) + camera_control_q.send(cam_ctrl) + elif key == ord('i'): + exp_time = clamp(exp_time - 500, 0, 33000) + print("Decreasing exposure time by 500, new value:", exp_time) + cam_ctrl = dai.CameraControl() + cam_ctrl.setManualExposure(exp_time, iso) + camera_control_q.send(cam_ctrl) + elif key == ord('o'): + exp_time = clamp(exp_time + 500, 0, 33000) + print("Increasing exposure time by 500, new value:", exp_time) + cam_ctrl = dai.CameraControl() + cam_ctrl.setManualExposure(exp_time, iso) + camera_control_q.send(cam_ctrl) + +RGB_FPS = 20 +MONO_FPS = 20 +ENCODER_FPS = 10 + +def build_pipeline(device: dai.Device, args) -> Tuple[dai.Pipeline, List[Tuple[str, int]], PipelineContext]: + """ + Build a pipeline based on device capabilities. Return a tuple of (pipeline, output_queue_names, PipelineContext) + """ + camera_features = device.getConnectedCameraFeatures() + context = PipelineContext() + calib = None + left_socket = None + right_socket = None + align_socket = None + try: + calib = device.readCalibration2() + except: + print("Couln't read calibration data from device, continue without it...") + + if calib: + eeprom = calib.getEepromData() + left_socket = eeprom.stereoRectificationData.leftCameraSocket + right_socket = eeprom.stereoRectificationData.rightCameraSocket + align_socket = [ + cam.socket + for cam in camera_features + if cam.supportedTypes[0] == dai.CameraSensorType.COLOR + ] + is_align_socket_color = len(align_socket) != 0 + if not is_align_socket_color: + print(f"No color camera found, aligning depth with {left_socket}") + align_socket = [left_socket] + align_socket = align_socket[0] + + xlink_outs: List[Tuple[str, int]] = [] # [(name, size), ...] + + pipeline = dai.Pipeline() + sys_log = pipeline.createSystemLogger() + sys_log.setRate(0.2) + sys_log_out = pipeline.createXLinkOut() + sys_log_out.setStreamName("sys_log") + sys_log.out.link(sys_log_out.input) + sys_log_out.input.setBlocking(False) + sys_log_out.input.setQueueSize(1) + + cam_control = pipeline.createXLinkIn() + cam_control.setStreamName("cam_control") + + left: dai.Node = None + right: dai.Node = None + # Used for spatial detection network (if available) + color_cam: dai.Node = None + + n_color_cams = 0 + n_edge_detectors = 0 + for cam in camera_features: + print(f"{cam.socket} Supported Sensor Resolutions:", [(conf.width, conf.height) for conf in cam.configs], "Supported Types:", cam.supportedTypes) + sorted_configs = sorted(cam.configs, key=lambda conf: conf.width * conf.height) + if len(sorted_configs) == 0: + max_sensor_size = (1920, 1080) + else: + max_sensor_size = (sorted_configs[-1].width, sorted_configs[-1].height) + node = None + cam_kind = cam.supportedTypes[0] + if cam_kind == dai.CameraSensorType.MONO: + mono = pipeline.createMonoCamera() + node = mono + mono.setBoardSocket(cam.socket) + # Default to 400p. Video encoder crashes on Oak-D PRO if set to highest (800p) + mono.setResolution( + dai.MonoCameraProperties.SensorResolution.THE_400_P) + mono.setFps(MONO_FPS) + xlink_preview = pipeline.createXLinkOut() + stream_name = "preview_" + cam.socket.name + xlink_preview.setStreamName(stream_name) + mono.out.link(xlink_preview.input) + xlink_outs.append((stream_name, 4)) + elif cam_kind == dai.CameraSensorType.COLOR: + print("Camera socket:", cam.socket, "IS COLOR") + n_color_cams += 1 + color = pipeline.createColorCamera() + node = color + color.setBoardSocket(cam.socket) + print(max_sensor_size, "FOR CCAM ", cam.socket) + resolution = color_resolutions.get(max_sensor_size, None) + if resolution is None: + print( + f"Skipping color camera on board socket {cam.socket}. Unknown resolution: {max_sensor_size}") + continue + color.setResolution(resolution) + color.setFps(RGB_FPS) + if n_color_cams == 1: + color_cam = color + color.setPreviewSize(416, 416) + color.setColorOrder( + dai.ColorCameraProperties.ColorOrder.BGR) + color.setInterleaved(False) + color.setNumFramesPool(2, 2, 2, 2, 2); + + # Only create a preview here if we're not creating a detection network + # And create a preview for other color cameras, that are not used for yolo + if args.no_nnet or n_color_cams > 1: + xlink_preview = pipeline.createXLinkOut() + stream_name = "preview_" + cam.socket.name + xlink_preview.setStreamName(stream_name) + color.preview.link(xlink_preview.input) + xlink_outs.append((stream_name, 2)) + + elif cam_kind == dai.CameraSensorType.TOF: + xin_tof_config = pipeline.createXLinkIn() + xin_tof_config.setStreamName("tof_config") + tof = pipeline.create(dai.node.ToF) + xin_tof_config.out.link(tof.inputConfig) + cam_node = pipeline.create(dai.node.ColorCamera) + cam_node.setFps(RGB_FPS) + cam_node.setBoardSocket(cam.socket) + cam_node.raw.link(tof.input) + tof_xout = pipeline.createXLinkOut() + tof_xout.setStreamName("tof") + tof.depth.link(tof_xout.input) + tofConfig = tof.initialConfig.get() + tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN + tofConfig.depthParams.avgPhaseShuffle = False + tofConfig.depthParams.minimumAmplitude = 3.0 + tof.initialConfig.set(tofConfig) + xlink_outs.append(("tof", 4)) + continue # No video encoder and edge detector for TOF + else: + print(f"Unsupported camera type: {cam.supportedTypes[0]}") + exit(-1) + if node is None: + continue + cam_control.out.link(node.inputControl) + + output = "out" if cam_kind == dai.CameraSensorType.MONO else "video" + if cam.socket == left_socket: + left = node + elif cam.socket == right_socket: + right = node + + if n_color_cams < 2: # For hardcode max 1 color cam video encoders, to avoid out of memory errors + video_encoder = pipeline.createVideoEncoder() + video_encoder.setDefaultProfilePreset( + ENCODER_FPS, dai.VideoEncoderProperties.Profile.H264_MAIN + ) + getattr(node, output).link(video_encoder.input) + ve_xlink = pipeline.createXLinkOut() + stream_name = f"{cam.socket}.ve_out" + ve_xlink.setStreamName(stream_name) + video_encoder.bitstream.link(ve_xlink.input) + xlink_outs.append((stream_name, 5)) + if n_edge_detectors < args.n_edge_detectors: + n_edge_detectors += 1 + edge_detector = pipeline.createEdgeDetector() + if cam_kind == dai.CameraSensorType.COLOR: + edge_detector.setMaxOutputFrameSize(8294400) + getattr(node, output).link(edge_detector.inputImage) + edge_detector_xlink = pipeline.createXLinkOut() + stream_name = f"{cam.socket}.edge_detector" + edge_detector_xlink.setStreamName(stream_name) + edge_detector.outputImage.link(edge_detector_xlink.input) + xlink_outs.append((stream_name, 5)) + + if left and right: + if left.getResolutionWidth() > 1280: + print("Left camera width is greater than 1280, setting ISP scale to 2/3") + left.setIspScale(2, 3) + if right.getResolutionWidth() > 1280: + print("Right camera width is greater than 1280, setting ISP scale to 2/3") + right.setIspScale(2, 3) + stereo = pipeline.createStereoDepth() + output = "out" if hasattr(left, "out") else "video" + getattr(left, output).link(stereo.left) + getattr(right, output).link(stereo.right) + stereo.setDefaultProfilePreset( + dai.node.StereoDepth.PresetMode.HIGH_DENSITY) + stereo.setOutputSize(left.getResolutionWidth(), + left.getResolutionHeight()) + stereo.setLeftRightCheck(True) + stereo.setSubpixel(True) + stereo.setDepthAlign(align_socket) + else: + print("Device doesn't have a stereo pair, skipping stereo depth creation...") + if color_cam is not None: + if not args.no_nnet: + if left is not None and right is not None: # Create spatial detection net + print("Creating spatial detection network...") + yolo = pipeline.createYoloSpatialDetectionNetwork() + blob_path = get_or_download_yolo_blob() + yolo.setBlobPath(blob_path) + yolo.setConfidenceThreshold(0.5) + yolo.input.setBlocking(False) + yolo.setBoundingBoxScaleFactor(0.5) + yolo.setDepthLowerThreshold(100) + yolo.setDepthUpperThreshold(5000) + yolo.setNumClasses(80) + yolo.setCoordinateSize(4) + yolo.setAnchors( + [10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319]) + yolo.setAnchorMasks({"side26": [1, 2, 3], "side13": [3, 4, 5]}) + yolo.setIouThreshold(0.5) + color_cam.preview.link(yolo.input) + stereo.depth.link(yolo.inputDepth) + + xout_depth = pipeline.createXLinkOut() + depth_q_name = "stereo depth" + xout_depth.setStreamName(depth_q_name) + yolo.passthroughDepth.link(xout_depth.input) + xlink_outs.append((depth_q_name, 4)) + + xout_yolo = pipeline.createXLinkOut() + yolo_q_name = "yolo" + xout_yolo.setStreamName(yolo_q_name) + yolo.out.link(xout_yolo.input) + xlink_outs.append((yolo_q_name, 4)) + else: + print("Creating YOLO detection network...") + passthrough_q_name, yolo_q_name = create_yolo(pipeline, color_cam) + xlink_outs.append((passthrough_q_name, 4)) + context.q_name_yolo_passthrough = passthrough_q_name + xlink_outs.append((yolo_q_name, 4)) + else: + print("Skipping YOLO detection network creation...") + else: + print("No color camera found, skipping YOLO detection network creation...") + return (pipeline, xlink_outs, context) + + +if __name__ == "__main__": + stress_test()