From 017beae66ec19d6c182aa2f4c075526e24fd7501 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 15 Jun 2023 20:32:06 +0300 Subject: [PATCH 01/53] Initial pointcloud implementation --- CMakeLists.txt | 2 + depthai-core | 2 +- src/DatatypeBindings.cpp | 2 + .../datatype/PointCloudConfigBindings.cpp | 52 +++++++++++++++++++ src/pipeline/node/NodeBindings.cpp | 2 + src/pipeline/node/PointCloudBindings.cpp | 47 +++++++++++++++++ 6 files changed, 106 insertions(+), 1 deletion(-) create mode 100644 src/pipeline/datatype/PointCloudConfigBindings.cpp create mode 100644 src/pipeline/node/PointCloudBindings.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4bdd118e6..ee920916c 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/datatype/ADatatypeBindings.cpp src/pipeline/datatype/AprilTagConfigBindings.cpp @@ -150,6 +151,7 @@ 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 ) diff --git a/depthai-core b/depthai-core index 5f75ae74a..f056c59d4 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 5f75ae74ade6ae12bd56c71c0149c0482f23c18f +Subproject commit f056c59d40094298f8ddd6bd8b267fde0f31f504 diff --git a/src/DatatypeBindings.cpp b/src/DatatypeBindings.cpp index 5ff683f3d..75ba3cb40 100644 --- a/src/DatatypeBindings.cpp +++ b/src/DatatypeBindings.cpp @@ -23,6 +23,7 @@ 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 DatatypeBindings::addToCallstack(std::deque& callstack) { // Bind common datatypebindings @@ -49,6 +50,7 @@ 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); } void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){ diff --git a/src/pipeline/datatype/PointCloudConfigBindings.cpp b/src/pipeline/datatype/PointCloudConfigBindings.cpp new file mode 100644 index 000000000..5cff5e8e0 --- /dev/null +++ b/src/pipeline/datatype/PointCloudConfigBindings.cpp @@ -0,0 +1,52 @@ +#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("config", &RawPointCloudConfig::config, DOC(dai, RawPointCloudConfig, config)) + ; + + // 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)) + ; + + // add aliases + +} diff --git a/src/pipeline/node/NodeBindings.cpp b/src/pipeline/node/NodeBindings.cpp index 06e790377..b4715ca04 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 NodeBindings::addToCallstack(std::deque& callstack) { // Bind Node et al @@ -147,6 +148,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); } void NodeBindings::bind(pybind11::module& m, void* pCallstack){ diff --git a/src/pipeline/node/PointCloudBindings.cpp b/src/pipeline/node/PointCloudBindings.cpp new file mode 100644 index 000000000..ed471c483 --- /dev/null +++ b/src/pipeline/node/PointCloudBindings.cpp @@ -0,0 +1,47 @@ +#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)) + ; + // ALIAS + daiNodeModule.attr("PointCloud").attr("Properties") = properties; + +} From 70b821a4997c1587781d925419ff9d8503dae468 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 15 Jun 2023 20:38:45 +0300 Subject: [PATCH 02/53] Update core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index f056c59d4..7c8a96531 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit f056c59d40094298f8ddd6bd8b267fde0f31f504 +Subproject commit 7c8a9653163d45ef896bcaaf645bcee08a1c3cd0 From 1da43176cd73a86482819319a4bbf9a617201b27 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 8 Jan 2024 11:16:55 +0100 Subject: [PATCH 03/53] Fixed large buffer processing in MessageGroups --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 6628488ef..6a64599e5 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 6628488ef8956f73f1c7bf4c8f1da218ad327a6f +Subproject commit 6a64599e5771a6a8c500b628200d673c406d04aa From fbb9e3dfc550cffad37ce54550a1609119d69aab Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 8 Jan 2024 13:12:15 +0100 Subject: [PATCH 04/53] Added PointCloudDataBindings --- depthai-core | 2 +- src/DatatypeBindings.cpp | 4 + .../datatype/PointCloudDataBindings.cpp | 82 +++++++++++++++++++ 3 files changed, 87 insertions(+), 1 deletion(-) create mode 100644 src/pipeline/datatype/PointCloudDataBindings.cpp diff --git a/depthai-core b/depthai-core index af46cec77..f681e4571 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit af46cec775bb5e3f7c02cd822a8592562d9bda6d +Subproject commit f681e45718da3a361f127b62e51ce30741bbcd8b diff --git a/src/DatatypeBindings.cpp b/src/DatatypeBindings.cpp index e68862186..1e4ea63a6 100644 --- a/src/DatatypeBindings.cpp +++ b/src/DatatypeBindings.cpp @@ -26,6 +26,7 @@ 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 @@ -55,6 +56,7 @@ void DatatypeBindings::addToCallstack(std::deque& callstack) { 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){ @@ -97,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/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp new file mode 100644 index 000000000..027775798 --- /dev/null +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -0,0 +1,82 @@ +#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_message_group(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("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.points; }, [](PointCloudData& data, std::vector points) {data.points = points}) + .def("getWidth", &PointCloudData::getWidth, DOC(dai, PointCloudData, getWidth)) + .def("getHeight", &PointCloudData::getHeight, DOC(dai, PointCloudData, getHeight)) + .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::Buffer::setWidth, DOC(dai, Buffer, setWidth)) + .def("setHeight", &PointCloudData::Buffer::setHeight, DOC(dai, Buffer, 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::Buffer::setInstanceNum, DOC(dai, Buffer, 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)) + ; + +} From 4516699e8ba595b6027ccad4a60e3f09360c2a55 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 8 Jan 2024 16:31:38 +0100 Subject: [PATCH 05/53] Bump core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 6a64599e5..7c7a4db3e 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 6a64599e5771a6a8c500b628200d673c406d04aa +Subproject commit 7c7a4db3eeb2353e7e91acbad2626b6fecaa6cf1 From 369c01a5b99acfaef67a5f07598b2aadb21cab17 Mon Sep 17 00:00:00 2001 From: asahtik Date: Wed, 10 Jan 2024 14:34:02 +0100 Subject: [PATCH 06/53] clangformat --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index f681e4571..6ebd00a80 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit f681e45718da3a361f127b62e51ce30741bbcd8b +Subproject commit 6ebd00a801971c9f1b09fbc05f11a3eac624656f From 0517a4d5e09454012de939c7c9892c537e4ddae9 Mon Sep 17 00:00:00 2001 From: Jakob Socan <10517617+jakgra@users.noreply.github.com> Date: Wed, 10 Jan 2024 15:12:25 +0100 Subject: [PATCH 07/53] Run HIL tests after all wheels are available --- .github/workflows/main.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 79889f628..79697236a 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -556,8 +556,9 @@ 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: + if: '!cancelled() && success()' # run when jobs are skipped, but don't run when one fails + needs: [pytest, build-linux-armhf, build-windows-x86_64, build-macos-x86_64, build-macos-arm64, build-linux-x86_64, build-linux-arm64, deploy] runs-on: ubuntu-latest steps: - name: Dispatch an action and get the run ID @@ -573,4 +574,4 @@ jobs: workflow_timeout_seconds: 300 # was 120 Default: 300 - name: Release - run: echo "https://github.com/luxonis/depthai-core-hil-tests/actions/runs/${{steps.return_dispatch.outputs.run_id}}" >> $GITHUB_STEP_SUMMARY \ No newline at end of file + run: echo "https://github.com/luxonis/depthai-core-hil-tests/actions/runs/${{steps.return_dispatch.outputs.run_id}}" >> $GITHUB_STEP_SUMMARY From 4ff848c3beca848eb03ac972eb7451da6695e1a3 Mon Sep 17 00:00:00 2001 From: Jakob Socan <10517617+jakgra@users.noreply.github.com> Date: Mon, 15 Jan 2024 10:08:52 +0100 Subject: [PATCH 08/53] Make notify hil workflow depend only on builds it needs --- .github/workflows/main.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 79697236a..b0af5e080 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -557,8 +557,7 @@ jobs: # client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' notify_hil_workflow: - if: '!cancelled() && success()' # run when jobs are skipped, but don't run when one fails - needs: [pytest, build-linux-armhf, build-windows-x86_64, build-macos-x86_64, build-macos-arm64, build-linux-x86_64, build-linux-arm64, deploy] + needs: [build-linux-armhf, build-linux-x86_64] runs-on: ubuntu-latest steps: - name: Dispatch an action and get the run ID From 845880bf264d94a5bd30650996eef763faf7b7bf Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 15 Jan 2024 13:57:12 +0100 Subject: [PATCH 09/53] Bugfixes, added python pointcloud visualization example with open3d --- CMakeLists.txt | 2 +- examples/PointCloud/visualize_pointcloud.py | 55 +++++++++++++++++++ .../datatype/PointCloudDataBindings.cpp | 22 ++++++-- 3 files changed, 73 insertions(+), 6 deletions(-) create mode 100644 examples/PointCloud/visualize_pointcloud.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 15ed5bd7c..0f58e9e45 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -156,7 +156,7 @@ pybind11_add_module(${TARGET_NAME} 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/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py new file mode 100644 index 000000000..e29dfcc22 --- /dev/null +++ b/examples/PointCloud/visualize_pointcloud.py @@ -0,0 +1,55 @@ +import depthai as dai +import open3d as o3d +from time import sleep + +pipeline = dai.Pipeline() +monoLeft = pipeline.create(dai.node.MonoCamera); +monoRight = pipeline.create(dai.node.MonoCamera); +depth = pipeline.create(dai.node.StereoDepth); +pointcloud = pipeline.create(dai.node.PointCloud); +xout = pipeline.create(dai.node.XLinkOut); +xoutDepth = pipeline.create(dai.node.XLinkOut); + +monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) +monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT) +monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) +monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT) + +depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_ACCURACY) +depth.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) +depth.setLeftRightCheck(True) +depth.setExtendedDisparity(False) +depth.setSubpixel(True) + +xout.setStreamName("out") +xoutDepth.setStreamName("depth") + +monoLeft.out.link(depth.left) +monoRight.out.link(depth.right) +depth.depth.link(pointcloud.inputDepth) +pointcloud.passthroughDepth.link(xoutDepth.input) +pointcloud.outputPointCloud.link(xout.input) + +with dai.Device(pipeline) as device: + q = device.getOutputQueue(name="out", maxSize=4, blocking=False) + qDepth = device.getOutputQueue(name="depth", maxSize=4, blocking=False) + + pc = o3d.geometry.PointCloud() + vis = o3d.visualization.Visualizer() + vis.create_window() + pcd = o3d.geometry.PointCloud() + + first = True + while True: + inDepth = qDepth.get() + inPointCloud = q.get() + if inPointCloud: + pcd.points = o3d.utility.Vector3dVector(inPointCloud.getPoints()) + if first: + vis.add_geometry(pcd) + first = False + else: + vis.update_geometry(pcd) + vis.poll_events() + vis.update_renderer() + sleep(0.01) diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 027775798..56cc3168b 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -13,7 +13,7 @@ // #include "spdlog/spdlog.h" -void bind_message_group(pybind11::module& m, void* pCallstack){ +void bind_pointclouddata(pybind11::module& m, void* pCallstack){ using namespace dai; @@ -50,7 +50,19 @@ void bind_message_group(pybind11::module& m, void* pCallstack){ // Message pointCloudData .def(py::init<>()) - .def_property("points", [](PointCloudData& data) { return &data.points; }, [](PointCloudData& data, std::vector points) {data.points = points}) + .def_property("points", [](PointCloudData& data) { return &data.points; }, [](PointCloudData& data, std::vector points) {data.points = points;}) + .def("getPoints", [](py::object &obj){ + // creates numpy array (zero-copy) which holds correct information such as shape, ... + dai::PointCloudData& data = obj.cast(); + py::array_t arr({data.points.size(), 3UL}); + auto ra = arr.mutable_unchecked(); + for (int i = 0; i < data.points.size(); i++) { + ra(i, 0) = data.points[i].x; + ra(i, 1) = data.points[i].y; + ra(i, 2) = data.points[i].z; + } + return arr; + }) .def("getWidth", &PointCloudData::getWidth, DOC(dai, PointCloudData, getWidth)) .def("getHeight", &PointCloudData::getHeight, DOC(dai, PointCloudData, getHeight)) .def("getMinX", &PointCloudData::getMinX, DOC(dai, PointCloudData, getMinX)) @@ -63,8 +75,8 @@ void bind_message_group(pybind11::module& m, void* pCallstack){ .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::Buffer::setWidth, DOC(dai, Buffer, setWidth)) - .def("setHeight", &PointCloudData::Buffer::setHeight, DOC(dai, Buffer, setHeight)) + .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)) @@ -73,7 +85,7 @@ void bind_message_group(pybind11::module& m, void* pCallstack){ .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::Buffer::setInstanceNum, DOC(dai, Buffer, setInstanceNum)) + .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)) From 50a648558a772f725f2d801bac31c07200d49b64 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 15 Jan 2024 19:30:41 +0100 Subject: [PATCH 10/53] Update core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 7c7a4db3e..c942ac67e 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 7c7a4db3eeb2353e7e91acbad2626b6fecaa6cf1 +Subproject commit c942ac67e56d64653c859714685d931d88ad63a8 From 8545d25d43c3966c8847b64329bb67fe24db2cf7 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 15 Jan 2024 20:36:01 +0100 Subject: [PATCH 11/53] Update core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index c942ac67e..e98694f02 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit c942ac67e56d64653c859714685d931d88ad63a8 +Subproject commit e98694f02c5a9a5fcb2a0c69cad134064df4d803 From 54e85758f8182fa841b417b5216de6147301774e Mon Sep 17 00:00:00 2001 From: asahtik Date: Wed, 17 Jan 2024 15:27:34 +0100 Subject: [PATCH 12/53] Added rgb pointcloud capability --- depthai-core | 2 +- src/pipeline/datatype/PointCloudDataBindings.cpp | 12 ++++++------ src/pipeline/node/PointCloudBindings.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/depthai-core b/depthai-core index 6ebd00a80..56e3c0b2e 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 6ebd00a801971c9f1b09fbc05f11a3eac624656f +Subproject commit 56e3c0b2e76716e1e47a4dc6c4bbe0f2623cf125 diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 56cc3168b..654ce51bd 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -50,16 +50,16 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ // Message pointCloudData .def(py::init<>()) - .def_property("points", [](PointCloudData& data) { return &data.points; }, [](PointCloudData& data, std::vector points) {data.points = points;}) .def("getPoints", [](py::object &obj){ // creates numpy array (zero-copy) which holds correct information such as shape, ... dai::PointCloudData& data = obj.cast(); - py::array_t arr({data.points.size(), 3UL}); + auto points = data.getPointsXYZ(); + py::array_t arr({points.size(), 3UL}); auto ra = arr.mutable_unchecked(); - for (int i = 0; i < data.points.size(); i++) { - ra(i, 0) = data.points[i].x; - ra(i, 1) = data.points[i].y; - ra(i, 2) = data.points[i].z; + for (int i = 0; i < points.size(); i++) { + ra(i, 0) = points[i].x; + ra(i, 1) = points[i].y; + ra(i, 2) = points[i].z; } return arr; }) diff --git a/src/pipeline/node/PointCloudBindings.cpp b/src/pipeline/node/PointCloudBindings.cpp index ed471c483..7b5d9a1e7 100644 --- a/src/pipeline/node/PointCloudBindings.cpp +++ b/src/pipeline/node/PointCloudBindings.cpp @@ -37,7 +37,7 @@ void bind_pointcloud(pybind11::module& m, void* pCallstack){ 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("out", &PointCloud::out, DOC(dai, node, PointCloud, out), DOC(dai, node, PointCloud, out)) .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)) ; From c21f7837ad6ea0d919faa38617478d727a53e146 Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 18 Jan 2024 16:46:21 +0100 Subject: [PATCH 13/53] Revert "Added rgb pointcloud capability" This reverts commit 54e85758f8182fa841b417b5216de6147301774e. --- depthai-core | 2 +- src/pipeline/datatype/PointCloudDataBindings.cpp | 12 ++++++------ src/pipeline/node/PointCloudBindings.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/depthai-core b/depthai-core index 56e3c0b2e..6ebd00a80 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 56e3c0b2e76716e1e47a4dc6c4bbe0f2623cf125 +Subproject commit 6ebd00a801971c9f1b09fbc05f11a3eac624656f diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 654ce51bd..56cc3168b 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -50,16 +50,16 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ // Message pointCloudData .def(py::init<>()) + .def_property("points", [](PointCloudData& data) { return &data.points; }, [](PointCloudData& data, std::vector points) {data.points = points;}) .def("getPoints", [](py::object &obj){ // creates numpy array (zero-copy) which holds correct information such as shape, ... dai::PointCloudData& data = obj.cast(); - auto points = data.getPointsXYZ(); - py::array_t arr({points.size(), 3UL}); + py::array_t arr({data.points.size(), 3UL}); auto ra = arr.mutable_unchecked(); - for (int i = 0; i < points.size(); i++) { - ra(i, 0) = points[i].x; - ra(i, 1) = points[i].y; - ra(i, 2) = points[i].z; + for (int i = 0; i < data.points.size(); i++) { + ra(i, 0) = data.points[i].x; + ra(i, 1) = data.points[i].y; + ra(i, 2) = data.points[i].z; } return arr; }) diff --git a/src/pipeline/node/PointCloudBindings.cpp b/src/pipeline/node/PointCloudBindings.cpp index 7b5d9a1e7..ed471c483 100644 --- a/src/pipeline/node/PointCloudBindings.cpp +++ b/src/pipeline/node/PointCloudBindings.cpp @@ -37,7 +37,7 @@ void bind_pointcloud(pybind11::module& m, void* pCallstack){ 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("out", &PointCloud::out, DOC(dai, node, PointCloud, out), DOC(dai, node, PointCloud, out)) + .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)) ; From fba55591876e3849b7fb73e4d4d2264de3fe430f Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 18 Jan 2024 16:51:20 +0100 Subject: [PATCH 14/53] Bump core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 6ebd00a80..b79afa334 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 6ebd00a801971c9f1b09fbc05f11a3eac624656f +Subproject commit b79afa334a6791ac57f4891ee15b895a15692b00 From e357a7639e355868d490f9267f39c2b857f6cecc Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 22 Jan 2024 07:40:58 +0100 Subject: [PATCH 15/53] Revert shared --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index b79afa334..2495d096a 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit b79afa334a6791ac57f4891ee15b895a15692b00 +Subproject commit 2495d096a663eddd72c1ff3a8e8b12291b3fc63c From d1d37542856f1e03d430074ac2ea5eafc0a80543 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 22 Jan 2024 08:05:36 +0100 Subject: [PATCH 16/53] Bump core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 2495d096a..0aa13feed 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 2495d096a663eddd72c1ff3a8e8b12291b3fc63c +Subproject commit 0aa13feed96f2dbc54274cbaa73dc92a95824f3f From dbb1d9afe081885d576f431d736a015429dccc44 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 22 Jan 2024 13:53:26 +0100 Subject: [PATCH 17/53] Sparse pointcloud bindings --- depthai-core | 2 +- src/pipeline/datatype/PointCloudConfigBindings.cpp | 3 ++- src/pipeline/datatype/PointCloudDataBindings.cpp | 2 ++ 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/depthai-core b/depthai-core index 0aa13feed..935c128b7 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 0aa13feed96f2dbc54274cbaa73dc92a95824f3f +Subproject commit 935c128b7a2b7d7c9fac6fb15b6a2a4612a58322 diff --git a/src/pipeline/datatype/PointCloudConfigBindings.cpp b/src/pipeline/datatype/PointCloudConfigBindings.cpp index 5cff5e8e0..806073090 100644 --- a/src/pipeline/datatype/PointCloudConfigBindings.cpp +++ b/src/pipeline/datatype/PointCloudConfigBindings.cpp @@ -35,7 +35,7 @@ void bind_pointcloudconfig(pybind11::module& m, void* pCallstack){ // Metadata / raw rawConfig .def(py::init<>()) - .def_readwrite("config", &RawPointCloudConfig::config, DOC(dai, RawPointCloudConfig, config)) + .def_readwrite("sparse", &RawPointCloudConfig::sparse, DOC(dai, RawPointCloudConfig, sparse)) ; // Message @@ -45,6 +45,7 @@ void bind_pointcloudconfig(pybind11::module& m, void* pCallstack){ .def("set", &PointCloudConfig::set, py::arg("config"), DOC(dai, PointCloudConfig, set)) .def("get", &PointCloudConfig::get, DOC(dai, PointCloudConfig, get)) + .def("setSparse", &PointCloudConfig::setSparse, py::arg("enable"), DOC(dai, PointCloudConfig, setSparse)) ; // add aliases diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 56cc3168b..89c032fdd 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -38,6 +38,7 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ .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) @@ -65,6 +66,7 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ }) .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)) From c6c2032a9c72a5a8f06c73706ffbfeebe2835c50 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 22 Jan 2024 14:00:05 +0100 Subject: [PATCH 18/53] Bump core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 935c128b7..112985a33 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 935c128b7a2b7d7c9fac6fb15b6a2a4612a58322 +Subproject commit 112985a335e13cc465372741068092fc5b41e910 From 72b0b729f71d611a9062f83982dc5f8b51a31457 Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Wed, 24 Jan 2024 15:24:22 +0100 Subject: [PATCH 19/53] [RVC2] Added getStereoPairs and getAvailableStereoPairs API (#959) * Added bindings for getStereoPairs * Available stereo pairs bindings. * Bump core. * Update core with merged get_stereo_pairs --- depthai-core | 2 +- src/DeviceBindings.cpp | 2 ++ src/pipeline/CommonBindings.cpp | 16 ++++++++++++++++ 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index e98694f02..3e08b71c0 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit e98694f02c5a9a5fcb2a0c69cad134064df4d803 +Subproject commit 3e08b71c0498428888e4324d7412e39fdafcf1fc 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..df5469a8a 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) From 30630ef29e31e65e9bd05280ee3e811b6f6fe713 Mon Sep 17 00:00:00 2001 From: asahtik Date: Fri, 26 Jan 2024 15:24:00 +0100 Subject: [PATCH 20/53] Added c++ tests for pointcloud --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 112985a33..cec135144 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 112985a335e13cc465372741068092fc5b41e910 +Subproject commit cec13514423956b8035d4f0f08fae4664ab7515b From bea383ef9f8ede9bbfd5fa62d784fb8804c9b8d7 Mon Sep 17 00:00:00 2001 From: Tommy Date: Wed, 31 Jan 2024 13:15:40 +0100 Subject: [PATCH 21/53] Add intensity output --- depthai-core | 2 +- src/pipeline/node/ToFBindings.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 3e08b71c0..f389487d1 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 3e08b71c0498428888e4324d7412e39fdafcf1fc +Subproject commit f389487d1a5924fd433b81ec738dce889a20128b 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)) ; From adbd8c775c21691e8b68812cfa4c2bc4c72f1f00 Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 1 Feb 2024 08:19:44 +0100 Subject: [PATCH 22/53] Bump core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index cec135144..48b22c1a4 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit cec13514423956b8035d4f0f08fae4664ab7515b +Subproject commit 48b22c1a44e64baf9f63d98fc6c33899804a3ae8 From cbe0c768ad617fdf4de233fd1a2904ffabbac4dc Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Mon, 5 Feb 2024 16:43:24 +0100 Subject: [PATCH 23/53] Added ability to set the lens position via a float, to enable a more precise movement. (#964) * Added ability to set the lens position with a float value (more precise) * I think this also includes an rpc fix by @jakgra --- depthai-core | 2 +- src/pipeline/datatype/CameraControlBindings.cpp | 3 +++ src/pipeline/datatype/EncodedFrameBindings.cpp | 2 ++ src/pipeline/datatype/ImgFrameBindings.cpp | 1 + utilities/cam_test.py | 10 +++++----- 5 files changed, 12 insertions(+), 6 deletions(-) diff --git a/depthai-core b/depthai-core index f389487d1..636118a72 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit f389487d1a5924fd433b81ec738dce889a20128b +Subproject commit 636118a7286215b087eaf6dd838a28106f4425be 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/utilities/cam_test.py b/utilities/cam_test.py index 1f47523a9..adc40a00c 100755 --- a/utilities/cam_test.py +++ b/utilities/cam_test.py @@ -354,16 +354,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 @@ -524,7 +524,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'): From 2a4bff26f92c9bc4ba8c1a7abbf65e686638fa18 Mon Sep 17 00:00:00 2001 From: Tommy Date: Fri, 9 Feb 2024 14:16:00 +0100 Subject: [PATCH 24/53] Update depthai-core --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 636118a72..06a09b24f 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 636118a7286215b087eaf6dd838a28106f4425be +Subproject commit 06a09b24f74a0d32a2d2d550805bec810c1200e1 From 99fca066d8bc983f5677ba7481697945c874a2ff Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 13 Feb 2024 23:20:07 +0200 Subject: [PATCH 25/53] FW: fix CAM_C failing to stream in certain cases, with same sensor type on CAM_B --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 06a09b24f..eb51d419f 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 06a09b24f74a0d32a2d2d550805bec810c1200e1 +Subproject commit eb51d419f0616c90b17e2a13c09ee7f59764f8ea From c682f16697132a861fa4a1d3d41d2515d1ed0f10 Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Thu, 15 Feb 2024 12:36:49 +0100 Subject: [PATCH 26/53] Added the initial version of the depthai binary. (#979) * Added the initial version of the depthai binary. * --list-devices -> list connected devices instead of available. --- depthai_cli/__init__.py | 0 depthai_cli/depthai.py | 20 ++++++++++++++++++++ setup.py | 6 ++++++ 3 files changed, 26 insertions(+) create mode 100644 depthai_cli/__init__.py create mode 100644 depthai_cli/depthai.py 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.py b/depthai_cli/depthai.py new file mode 100644 index 000000000..7e793ad49 --- /dev/null +++ b/depthai_cli/depthai.py @@ -0,0 +1,20 @@ +def cli(): + import argparse + import sys + import depthai as dai + + parser = argparse.ArgumentParser(description="DepthAI CLI") + 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.") + + args = parser.parse_args() + if args.version: + print(dai.__version__) + elif args.list_devices: + print(dai.Device.getAllConnectedDevices()) + elif len(sys.argv) == 1: + parser.print_help() + + +if __name__ == "__main__": + cli() diff --git a/setup.py b/setup.py index 7dfd75015..de2617f7e 100644 --- a/setup.py +++ b/setup.py @@ -209,6 +209,7 @@ def build_extension(self, ext): cmdclass={ 'build_ext': CMakeBuild }, + packages=["depthai_cli"], zip_safe=False, classifiers=[ "Development Status :: 4 - Beta", @@ -234,4 +235,9 @@ def build_extension(self, ext): "Topic :: Software Development", ], python_requires='>=3.6', + entry_points={ + "console_scripts": [ + 'depthai=depthai_cli.depthai:cli' + ] + } ) From c014e27e224f7ef3f6407be6b3f05be6c2fffd13 Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Tue, 20 Feb 2024 15:25:13 +0100 Subject: [PATCH 27/53] OAK-T Support (#957) * Added thermal to cam test * Bump core * Fix thermal on cam test, added thermal_camera.py example * Link cam control for thermal aswell, doesn't do anything. bump depthai-core * Bump core * [FW] detect oak-ts with the new board name * shared * Drive oak_t flood with torch mode * Extend default cam socket pairs with false for thermal. * Fix signal integrity issues at high ambient temperatures. * Added some comments to thermal_camera example, future proof the example a bit by querying the sensor width, height. * Convert thermal_camera example from snake to camel case. * Bump core * Rename thermal_camera example to thermal_cam.py --- depthai-core | 2 +- examples/Camera/thermal_cam.py | 107 +++++++++++++++++++++++++++++++++ utilities/cam_test.py | 35 ++++++++--- 3 files changed, 135 insertions(+), 9 deletions(-) create mode 100644 examples/Camera/thermal_cam.py diff --git a/depthai-core b/depthai-core index eb51d419f..bd8011f9e 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit eb51d419f0616c90b17e2a13c09ee7f59764f8ea +Subproject commit bd8011f9e90f776652bcee6713d8d79693fd1fc8 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/utilities/cam_test.py b/utilities/cam_test.py index adc40a00c..eb09ec8de 100755 --- a/utilities/cam_test.py +++ b/utilities/cam_test.py @@ -53,20 +53,21 @@ def socket_type_pair(arg): socket, type = arg.split(',') - if not (socket in ['rgb', 'left', 'right', 'cama', 'camb', 'camc', 'camd']): + if not (socket in ['rgb', 'left', 'right', 'cama', 'camb', 'camc', 'camd', 'came']): 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.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]. " + default=[['rgb', True, False, False], ['left', False, False, False], + ['right', False, False, False], ['camd', True, False, False]], + help="Which camera sockets to enable, and type: c[olor] / m[ono] / t[of] / th[ermal]." "E.g: -cams rgb,m right,c . Default: rgb,c left,m right,m camd,c") parser.add_argument('-mres', '--mono-resolution', type=int, default=800, choices={480, 400, 720, 800}, help="Select mono camera resolution (height). Default: %(default)s") @@ -122,12 +123,14 @@ def socket_type_pair(arg): cam_list = [] cam_type_color = {} cam_type_tof = {} +cam_type_thermal = {} print("Enabled cameras:") -for socket, is_color, is_tof in args.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 - print(socket.rjust(7), ':', 'tof' if is_tof else 'color' if is_color else 'mono') + 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') print("DepthAI version:", dai.__version__) print("DepthAI path:", dai.__file__) @@ -140,6 +143,7 @@ def socket_type_pair(arg): 'camb' : dai.CameraBoardSocket.CAM_B, 'camc' : dai.CameraBoardSocket.CAM_C, 'camd' : dai.CameraBoardSocket.CAM_D, + 'came' : dai.CameraBoardSocket.CAM_E, } rotate = { @@ -150,6 +154,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 = { @@ -263,6 +268,16 @@ def prev(self): 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]) @@ -423,6 +438,10 @@ def exit_cleanly(signum, frame): 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) + 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, " From 393aa6d228102bdc2d07735e6e9f9ebd26dee1c9 Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 22 Feb 2024 09:55:25 +0100 Subject: [PATCH 28/53] Example fixes & improvements, bump fw --- depthai-core | 2 +- examples/PointCloud/visualize_pointcloud.py | 14 +++++++++++--- src/pipeline/datatype/PointCloudDataBindings.cpp | 4 ++-- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/depthai-core b/depthai-core index df6842abd..352861548 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit df6842abdcfdee1ccd9ffd387e14fd0e052f7be2 +Subproject commit 352861548f6c5466d9e7de4c0865db963d9eaa14 diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index e29dfcc22..9c3ef1e64 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -15,7 +15,7 @@ monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT) -depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_ACCURACY) +depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) depth.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) depth.setLeftRightCheck(True) depth.setExtendedDisparity(False) @@ -31,16 +31,23 @@ pointcloud.outputPointCloud.link(xout.input) 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) qDepth = device.getOutputQueue(name="depth", maxSize=4, blocking=False) pc = o3d.geometry.PointCloud() - vis = o3d.visualization.Visualizer() + vis = o3d.visualization.VisualizerWithKeyCallback() vis.create_window() + vis.register_key_action_callback(81, key_callback) pcd = o3d.geometry.PointCloud() first = True - while True: + while isRunning: inDepth = qDepth.get() inPointCloud = q.get() if inPointCloud: @@ -53,3 +60,4 @@ vis.poll_events() vis.update_renderer() sleep(0.01) + vis.destroy_window() diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 89c032fdd..f744f0cb9 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -59,8 +59,8 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ auto ra = arr.mutable_unchecked(); for (int i = 0; i < data.points.size(); i++) { ra(i, 0) = data.points[i].x; - ra(i, 1) = data.points[i].y; - ra(i, 2) = data.points[i].z; + ra(i, 1) = -data.points[i].y; + ra(i, 2) = -data.points[i].z; } return arr; }) From 57c36aa600278c3bb052067eedc0936f47f4909f Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 22 Feb 2024 09:57:54 +0100 Subject: [PATCH 29/53] Clangformat --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 352861548..b5867440f 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 352861548f6c5466d9e7de4c0865db963d9eaa14 +Subproject commit b5867440f960a54a72a24d7a79940f8f28d85f96 From 9f67ed8ba5b4164441fe461252bcea1fb00cec2e Mon Sep 17 00:00:00 2001 From: asahtik Date: Thu, 22 Feb 2024 10:42:57 +0100 Subject: [PATCH 30/53] Bugfix --- depthai-core | 2 +- src/pipeline/datatype/PointCloudDataBindings.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/depthai-core b/depthai-core index b5867440f..8cc801f5d 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit b5867440f960a54a72a24d7a79940f8f28d85f96 +Subproject commit 8cc801f5d3c7f8f5a753de426cf3d86cb3cc7968 diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index f744f0cb9..f2c12ff4d 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -60,7 +60,7 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ for (int i = 0; i < data.points.size(); i++) { ra(i, 0) = data.points[i].x; ra(i, 1) = -data.points[i].y; - ra(i, 2) = -data.points[i].z; + ra(i, 2) = data.points[i].z; } return arr; }) From 8b8a4da186d9d671a5758e2093d153658691c19f Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Sun, 25 Feb 2024 16:12:54 +0100 Subject: [PATCH 31/53] Default cam test (#845) * Added stress test to cam_test.py * Default cam test: Stream from all connected cameras. If the device is calibrated and a stero pair is available, create a StereoDepth node and stream depth aswell * Added a --no-stereo option * improve stability of stress test, add dot projector and flood light brightness prints * Add LR support * Visualize outputs (not pretty) * Added flood light and dot projector to stress test * Only create one video encoder. Visualize tof * Limit to one edge detector. add exposure time and iso settings * fix clamp * zero edge detectors * no yolo * add another video encoder * Add all sockets * Add back yolo to stress test, added args to stress test * Forward stress test help to stress_test.py without being consumed by cam_test.py * Default no stereo, convert --no-stereo into --stereo option. Fix stress test index out of range when no resolution config is available. fix stress test stereo depth colorization * Added --yolo option to cam_test for ability to stress the device a bit more, while retaining full size streams * Modify stress test to not use stereo if calibration isn't available. * Lower frame pool sizes * Move setNumFramesPool * Pool sizes * Visualize disparity * Fix cam_test.py after merge with develop --- utilities/cam_test.py | 454 ++++++++++++++++++++++------------ utilities/cam_test_gui.py | 2 + utilities/stress_test.py | 502 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 808 insertions(+), 150 deletions(-) create mode 100644 utilities/stress_test.py diff --git a/utilities/cam_test.py b/utilities/cam_test.py index eb09ec8de..87fa150d1 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,11 +50,14 @@ 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', 'came']): + if not (socket in ALL_SOCKETS): raise ValueError("") if not (type in ['m', 'mono', 'c', 'color', 't', 'tof', 'th', 'thermal']): raise ValueError("") @@ -63,12 +67,11 @@ def socket_type_pair(arg): 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, False], ['left', False, False, False], - ['right', False, False, False], ['camd', True, False, False]], - help="Which camera sockets to enable, and type: c[olor] / m[ono] / t[of] / th[ermal]." - "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'}, @@ -109,41 +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)") -args = parser.parse_args() +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, _unknown = parser.parse_known_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 = {} -cam_type_thermal = {} -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') - 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, - 'came' : dai.CameraBoardSocket.CAM_E, + '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 = { @@ -179,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) @@ -215,125 +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_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) - 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(): @@ -410,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: @@ -427,16 +552,40 @@ 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) @@ -473,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]: @@ -495,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 @@ -510,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)") @@ -688,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() From ebca0f2ef7de22c5f638c8fc566bbca90a712aea Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 26 Feb 2024 11:57:47 +0100 Subject: [PATCH 32/53] Reduce amount of pointcloud copying --- depthai-core | 2 +- src/pipeline/datatype/PointCloudDataBindings.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/depthai-core b/depthai-core index 8cc801f5d..5b3dab767 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 8cc801f5d3c7f8f5a753de426cf3d86cb3cc7968 +Subproject commit 5b3dab76738778cd46e06577325fbfeea632007f diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index f2c12ff4d..560e219e0 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -51,16 +51,16 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ // Message pointCloudData .def(py::init<>()) - .def_property("points", [](PointCloudData& data) { return &data.points; }, [](PointCloudData& data, std::vector points) {data.points = points;}) + .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(); - py::array_t arr({data.points.size(), 3UL}); + py::array_t arr({data.getPoints().size(), 3UL}); auto ra = arr.mutable_unchecked(); - for (int i = 0; i < data.points.size(); i++) { - ra(i, 0) = data.points[i].x; - ra(i, 1) = -data.points[i].y; - ra(i, 2) = data.points[i].z; + for (int i = 0; i < data.getPoints().size(); i++) { + ra(i, 0) = data.getPoints()[i].x; + ra(i, 1) = -data.getPoints()[i].y; + ra(i, 2) = data.getPoints()[i].z; } return arr; }) From ee001db07588b8e4a6424bb5762d50ddbfefeae5 Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Mon, 26 Feb 2024 13:08:26 +0100 Subject: [PATCH 33/53] Make cam_test.py available through the depthai binary. (#983) * Expose cam_test through depthai_cli too. * Change the argument parsing to allow more checks to be done by argparse * Use subparsers for subcommands (like cam_test) to print better help. --------- Co-authored-by: Matevz Morato --- depthai_cli/depthai.py | 20 ---------------- depthai_cli/depthai_cli.py | 49 ++++++++++++++++++++++++++++++++++++++ setup.py | 23 ++++++++++++++---- utilities/cam_test.py | 2 +- 4 files changed, 69 insertions(+), 25 deletions(-) delete mode 100644 depthai_cli/depthai.py create mode 100644 depthai_cli/depthai_cli.py diff --git a/depthai_cli/depthai.py b/depthai_cli/depthai.py deleted file mode 100644 index 7e793ad49..000000000 --- a/depthai_cli/depthai.py +++ /dev/null @@ -1,20 +0,0 @@ -def cli(): - import argparse - import sys - import depthai as dai - - parser = argparse.ArgumentParser(description="DepthAI CLI") - 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.") - - args = parser.parse_args() - if args.version: - print(dai.__version__) - elif args.list_devices: - print(dai.Device.getAllConnectedDevices()) - elif len(sys.argv) == 1: - parser.print_help() - - -if __name__ == "__main__": - cli() 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/setup.py b/setup.py index de2617f7e..116762103 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,11 +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"], + packages=[DEPTHAI_CLI_MODULE_NAME], zip_safe=False, classifiers=[ "Development Status :: 4 - Beta", @@ -237,7 +252,7 @@ def build_extension(self, ext): python_requires='>=3.6', entry_points={ "console_scripts": [ - 'depthai=depthai_cli.depthai:cli' + f'depthai={DEPTHAI_CLI_MODULE_NAME}.depthai_cli:cli' ] } ) diff --git a/utilities/cam_test.py b/utilities/cam_test.py index 87fa150d1..c12b79cec 100755 --- a/utilities/cam_test.py +++ b/utilities/cam_test.py @@ -126,7 +126,7 @@ def socket_type_pair(arg): 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, _unknown = parser.parse_known_args() +args = parser.parse_args() # Set timeouts before importing depthai os.environ["DEPTHAI_CONNECTION_TIMEOUT"] = str(args.connection_timeout) From b592eeb8932364a3070600e7bfc4973f09314e93 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 26 Feb 2024 16:38:25 +0100 Subject: [PATCH 34/53] Fix tests --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 5b3dab767..40a2b8eb7 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 5b3dab76738778cd46e06577325fbfeea632007f +Subproject commit 40a2b8eb71fd8ddcb7bd47274c56ae42d071dda0 From 4af09fd7a025e3e52f845c16f311bcc4e0054adb Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 26 Feb 2024 19:14:29 +0100 Subject: [PATCH 35/53] Add colorized example for pointcloud --- examples/PointCloud/visualize_pointcloud.py | 59 +++++++++++++++------ 1 file changed, 42 insertions(+), 17 deletions(-) diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index 9c3ef1e64..9af470369 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -1,34 +1,51 @@ import depthai as dai import open3d as o3d from time import sleep +import numpy as np +import cv2 + +FPS = 20 pipeline = dai.Pipeline() -monoLeft = pipeline.create(dai.node.MonoCamera); -monoRight = pipeline.create(dai.node.MonoCamera); -depth = pipeline.create(dai.node.StereoDepth); -pointcloud = pipeline.create(dai.node.PointCloud); -xout = pipeline.create(dai.node.XLinkOut); -xoutDepth = pipeline.create(dai.node.XLinkOut); +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.setBoardSocket(dai.CameraBoardSocket.LEFT) +monoLeft.setCamera("left") +monoLeft.setFps(FPS) monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) -monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT) +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) - -xout.setStreamName("out") -xoutDepth.setStreamName("depth") +depth.setDepthAlign(dai.CameraBoardSocket.CAM_A) monoLeft.out.link(depth.left) monoRight.out.link(depth.right) depth.depth.link(pointcloud.inputDepth) -pointcloud.passthroughDepth.link(xoutDepth.input) -pointcloud.outputPointCloud.link(xout.input) +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 @@ -38,8 +55,6 @@ def key_callback(vis, action, mods): isRunning = False q = device.getOutputQueue(name="out", maxSize=4, blocking=False) - qDepth = device.getOutputQueue(name="depth", maxSize=4, blocking=False) - pc = o3d.geometry.PointCloud() vis = o3d.visualization.VisualizerWithKeyCallback() vis.create_window() @@ -48,10 +63,20 @@ def key_callback(vis, action, mods): first = True while isRunning: - inDepth = qDepth.get() - inPointCloud = q.get() + inMessage = q.get() + inColor = inMessage["rgb"] + inPointCloud = inMessage["pcl"] + cvColorFrame = inColor.getCvFrame() + # Convert the frame to RGB + cvRGBFrame = cv2.cvtColor(cvColorFrame, cv2.COLOR_BGR2RGB) + cv2.imshow("color", cvColorFrame) + key = cv2.waitKey(1) + if key == ord('q'): + break if inPointCloud: pcd.points = o3d.utility.Vector3dVector(inPointCloud.getPoints()) + colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64) + pcd.colors = o3d.utility.Vector3dVector(colors) if first: vis.add_geometry(pcd) first = False From 8bfa6758f3df2b0ea323c21ad36e610db3fe1f86 Mon Sep 17 00:00:00 2001 From: asahtik Date: Wed, 28 Feb 2024 17:34:17 +0100 Subject: [PATCH 36/53] Implemented transformation matrix for pointcloud --- depthai-core | 2 +- src/pipeline/datatype/PointCloudConfigBindings.cpp | 12 +++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/depthai-core b/depthai-core index 40a2b8eb7..47e7b5c21 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 40a2b8eb71fd8ddcb7bd47274c56ae42d071dda0 +Subproject commit 47e7b5c21206a4b4e31b331807fffd178b284166 diff --git a/src/pipeline/datatype/PointCloudConfigBindings.cpp b/src/pipeline/datatype/PointCloudConfigBindings.cpp index 806073090..b7f03d8a2 100644 --- a/src/pipeline/datatype/PointCloudConfigBindings.cpp +++ b/src/pipeline/datatype/PointCloudConfigBindings.cpp @@ -36,6 +36,7 @@ void bind_pointcloudconfig(pybind11::module& m, void* pCallstack){ rawConfig .def(py::init<>()) .def_readwrite("sparse", &RawPointCloudConfig::sparse, DOC(dai, RawPointCloudConfig, sparse)) + .def_readwrite("transformationMatrix", &RawPointCloudConfig::transformationMatrix, DOC(dai, RawPointCloudConfig, transformationMatrix)) ; // Message @@ -45,7 +46,16 @@ void bind_pointcloudconfig(pybind11::module& m, void* pCallstack){ .def("set", &PointCloudConfig::set, py::arg("config"), DOC(dai, PointCloudConfig, set)) .def("get", &PointCloudConfig::get, DOC(dai, PointCloudConfig, get)) - .def("setSparse", &PointCloudConfig::setSparse, py::arg("enable"), DOC(dai, PointCloudConfig, setSparse)) + .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 From 5248efe69ff116e7b58e722c2bea6c96280393ac Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 4 Mar 2024 20:33:51 +0100 Subject: [PATCH 37/53] Fixed windows build --- src/pipeline/datatype/PointCloudDataBindings.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 560e219e0..7cf99053d 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -55,8 +55,9 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ .def("getPoints", [](py::object &obj){ // creates numpy array (zero-copy) which holds correct information such as shape, ... dai::PointCloudData& data = obj.cast(); - py::array_t arr({data.getPoints().size(), 3UL}); + py::array_t arr({(unsigned long)(data.getData().size() / sizeof(float)), 3UL}); auto ra = arr.mutable_unchecked(); + #pragma clang for (int i = 0; i < data.getPoints().size(); i++) { ra(i, 0) = data.getPoints()[i].x; ra(i, 1) = -data.getPoints()[i].y; From 3dcae95c357d321386cf38e303e859ac7d8ece9f Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 4 Mar 2024 20:43:59 +0100 Subject: [PATCH 38/53] Optimized getPoints --- src/pipeline/datatype/PointCloudDataBindings.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index 7cf99053d..a1d98341f 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -55,13 +55,13 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ .def("getPoints", [](py::object &obj){ // creates numpy array (zero-copy) which holds correct information such as shape, ... dai::PointCloudData& data = obj.cast(); - py::array_t arr({(unsigned long)(data.getData().size() / sizeof(float)), 3UL}); + py::array_t arr({(unsigned long)(data.getData().size() / sizeof(Point3f)), 3UL}); + Point3f* points = (Point3f*)data.getData().data(); auto ra = arr.mutable_unchecked(); - #pragma clang - for (int i = 0; i < data.getPoints().size(); i++) { - ra(i, 0) = data.getPoints()[i].x; - ra(i, 1) = -data.getPoints()[i].y; - ra(i, 2) = data.getPoints()[i].z; + for (int i = 0; i < data.getData().size() / sizeof(Point3f); i++) { + ra(i, 0) = points[i].x; + ra(i, 1) = -points[i].y; + ra(i, 2) = points[i].z; } return arr; }) From 42c3a2dae74d24eb92f6090513e2e37f120e6757 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 4 Mar 2024 20:49:43 +0100 Subject: [PATCH 39/53] Minor fixes --- src/pipeline/datatype/PointCloudDataBindings.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index a1d98341f..d2e9299b7 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -55,10 +55,11 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ .def("getPoints", [](py::object &obj){ // creates numpy array (zero-copy) which holds correct information such as shape, ... dai::PointCloudData& data = obj.cast(); - py::array_t arr({(unsigned long)(data.getData().size() / sizeof(Point3f)), 3UL}); 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 < data.getData().size() / sizeof(Point3f); i++) { + 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; From 6a26eeed22213ba71dfcc7994e4168ee8519ca8c Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Mon, 4 Mar 2024 21:50:21 +0100 Subject: [PATCH 40/53] Enhance camera features (#987) * Improved reporting of camera features. Added calibrationResolution and added fov rect to CameraSensorConfig --- depthai-core | 2 +- src/pipeline/CommonBindings.cpp | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 47e7b5c21..f2c83a13f 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 47e7b5c21206a4b4e31b331807fffd178b284166 +Subproject commit f2c83a13f0fed5b34dcc907c49da080fea675776 diff --git a/src/pipeline/CommonBindings.cpp b/src/pipeline/CommonBindings.cpp index df5469a8a..3aa29751c 100644 --- a/src/pipeline/CommonBindings.cpp +++ b/src/pipeline/CommonBindings.cpp @@ -229,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; @@ -244,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 From 00f0d3208096ce55461fc3d7166c416f472923a4 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 5 Mar 2024 01:03:58 +0200 Subject: [PATCH 41/53] FW: -fix `setAutoExposureLimit` flicker during AF lens move, - IMX214 4K resolution timings were slightly off, also bump max FPS for it (up to 34), - VideoEncoder fix artifacts for multiple monochrome streams with different resolutions (OV9282 and AR0234), - fix potential crash with Script GPIO.setInterrupt with too high priority (<= 15, low value = high prio) --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index f2c83a13f..991144d36 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit f2c83a13f0fed5b34dcc907c49da080fea675776 +Subproject commit 991144d36b7b712c7a885c3b87af8c8bb312330f From e37db4f5b15ddb1ad0ef67b81a46c10be3cf181b Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 5 Mar 2024 13:39:45 +0100 Subject: [PATCH 42/53] Improve the pointcloud example to work with 30FPS --- examples/PointCloud/visualize_pointcloud.py | 28 ++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index 9af470369..75ab1f260 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -3,8 +3,23 @@ from time import sleep import numpy as np import cv2 +import time -FPS = 20 +FPS = 30 +class FPSCounter: + def __init__(self): + self.frame_count = 0 + self.fps = 0 + self.start_time = time.time() + + def tick(self): + self.frame_count += 1 + if self.frame_count % 10 == 0: + elapsed_time = time.time() - self.start_time + self.fps = self.frame_count / elapsed_time + self.frame_count = 0 + self.start_time = time.time() + return self.fps pipeline = dai.Pipeline() camRgb = pipeline.create(dai.node.ColorCamera) @@ -60,8 +75,11 @@ def key_callback(vis, action, mods): 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"] @@ -69,12 +87,17 @@ def key_callback(vis, action, mods): 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: - pcd.points = o3d.utility.Vector3dVector(inPointCloud.getPoints()) + 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: @@ -84,5 +107,4 @@ def key_callback(vis, action, mods): vis.update_geometry(pcd) vis.poll_events() vis.update_renderer() - sleep(0.01) vis.destroy_window() From b505473ed3d5b0118c8c6b973ea6859d3a702e95 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 5 Mar 2024 13:47:54 +0100 Subject: [PATCH 43/53] Change the example to camel case for consistency --- examples/PointCloud/visualize_pointcloud.py | 22 ++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index 75ab1f260..c1258c2e2 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -5,20 +5,20 @@ import cv2 import time -FPS = 30 +FPS = 15 class FPSCounter: def __init__(self): - self.frame_count = 0 + self.frameCount = 0 self.fps = 0 - self.start_time = time.time() + self.startTime = time.time() def tick(self): - self.frame_count += 1 - if self.frame_count % 10 == 0: - elapsed_time = time.time() - self.start_time - self.fps = self.frame_count / elapsed_time - self.frame_count = 0 - self.start_time = time.time() + 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() @@ -37,10 +37,10 @@ def tick(self): camRgb.setIspScale(1,3) camRgb.setFps(FPS) -monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) +monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P) monoLeft.setCamera("left") monoLeft.setFps(FPS) -monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) +monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P) monoRight.setCamera("right") monoRight.setFps(FPS) From 50e368313264a4cb1ab0429f0f837581f4a30219 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 5 Mar 2024 13:50:45 +0100 Subject: [PATCH 44/53] Minor fixes --- .gitignore | 4 ++-- examples/PointCloud/visualize_pointcloud.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) 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/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index c1258c2e2..9aa804d2c 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -5,7 +5,7 @@ import cv2 import time -FPS = 15 +FPS = 30 class FPSCounter: def __init__(self): self.frameCount = 0 @@ -37,10 +37,10 @@ def tick(self): camRgb.setIspScale(1,3) camRgb.setFps(FPS) -monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P) +monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) monoLeft.setCamera("left") monoLeft.setFps(FPS) -monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P) +monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) monoRight.setCamera("right") monoRight.setFps(FPS) From 37f655338a2607d243506847d3ab127422405c1c Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 5 Mar 2024 18:03:17 +0100 Subject: [PATCH 45/53] Print a nice error of Open3D is not installed --- examples/PointCloud/visualize_pointcloud.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index 9aa804d2c..3ea338cbe 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -1,9 +1,13 @@ import depthai as dai -import open3d as o3d from time import sleep import numpy as np import cv2 import time +try: + import open3d as o3d +except ImportError: + print("Open3D not installed, to be able to run the example install Open3D with: python3 -m pip install open3d") + exit(1) FPS = 30 class FPSCounter: From 11b925f64bb8c468be7df67706d2680793dbc0af Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 5 Mar 2024 18:16:28 +0100 Subject: [PATCH 46/53] Update the handling for uninstalled open3d --- examples/PointCloud/visualize_pointcloud.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/PointCloud/visualize_pointcloud.py b/examples/PointCloud/visualize_pointcloud.py index 3ea338cbe..2ff669491 100644 --- a/examples/PointCloud/visualize_pointcloud.py +++ b/examples/PointCloud/visualize_pointcloud.py @@ -3,11 +3,11 @@ import numpy as np import cv2 import time +import sys try: import open3d as o3d except ImportError: - print("Open3D not installed, to be able to run the example install Open3D with: python3 -m pip install open3d") - exit(1) + 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: From e13e2318c628f16078b352e915061260e5ad48cb Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 6 Mar 2024 15:33:53 +0100 Subject: [PATCH 47/53] Bump version to 2.25.0.0 --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 991144d36..8851bfde0 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 991144d36b7b712c7a885c3b87af8c8bb312330f +Subproject commit 8851bfde04f9edc646431682328182197d47febe From d3917a75f2714c51506a90be6d745922e7a07281 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 6 Mar 2024 18:35:10 +0100 Subject: [PATCH 48/53] Add manylinux wheels for 32 bit arm --- .github/workflows/main.yml | 59 +++++++++++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index aa65e6d33..3749d5bb6 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -468,9 +468,66 @@ jobs: ARTIFACTORY_USER: ${{ secrets.ARTIFACTORY_USER }} ARTIFACTORY_PASS: ${{ secrets.ARTIFACTORY_PASS }} + # This job builds wheels for ARM32 arch + build-linux-armv7l: + needs: build-docstrings + runs-on: [self-hosted, linux, ARM] + container: + image: quay.io/pypa/manylinux2014_armv7l:2024-01-08-eb135ed + env: + PLAT: manylinux2014_armv7l + # Mount local hunter cache directory, instead of transfering to Github and back + volumes: + - /.hunter:/github/home/.hunter + steps: + - uses: actions/checkout@v3 + with: + submodules: 'recursive' + - name: Installing libusb1-devel dependency + run: yum install -y --disableplugin=fastestmirror libusb1-devel + - name: Installing cmake dependency + run: | + /opt/python/cp38-cp38/bin/python3.8 -m pip install cmake + ln -s /opt/python/cp38-cp38/bin/cmake /bin/ + - name: Create folder structure + run: mkdir -p wheelhouse/audited/ + + - uses: actions/download-artifact@v3 + with: + name: 'docstrings' + path: docstrings + - name: Specify docstring to use while building the wheel + run: echo "DEPTHAI_PYTHON_DOCSTRINGS_INPUT=$PWD/docstrings/depthai_python_docstring.hpp" >> $GITHUB_ENV + + - name: Build and install depthai-core + run: | + cmake -S depthai-core/ -B build_core -D CMAKE_BUILD_TYPE=Release -D CMAKE_TOOLCHAIN_FILE=$PWD/cmake/toolchain/pic.cmake + cmake --build build_core --target install --parallel 4 + echo "DEPTHAI_INSTALLATION_DIR=$PWD/build_core/install/" >> $GITHUB_ENV + + - name: Append build hash if not a tagged commit + if: startsWith(github.ref, 'refs/tags/v') != true + run: echo "BUILD_COMMIT_HASH=${{github.sha}}" >> $GITHUB_ENV + - name: Building wheels + run: for PYBIN in /opt/python/cp3{6..12}*/bin; do "${PYBIN}/pip" wheel . -w ./wheelhouse/ --verbose; done + - name: Auditing wheels + run: for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat $PLAT -w wheelhouse/audited/; done + - name: Archive wheel artifacts + uses: actions/upload-artifact@v3 + with: + name: audited-wheels + path: wheelhouse/audited/ + - name: Deploy wheels to artifactory (if not a release) + if: startsWith(github.ref, 'refs/tags/v') != true + run: bash ./ci/upload-artifactory.sh + env: + ARTIFACTORY_URL: ${{ secrets.ARTIFACTORY_URL }} + ARTIFACTORY_USER: ${{ secrets.ARTIFACTORY_USER }} + ARTIFACTORY_PASS: ${{ secrets.ARTIFACTORY_PASS }} + release: if: startsWith(github.ref, 'refs/tags/v') - needs: [pytest, build-linux-armhf, build-windows-x86_64, build-macos-x86_64, build-macos-arm64, build-linux-x86_64, build-linux-arm64] + needs: [pytest, build-linux-armhf, build-windows-x86_64, build-macos-x86_64, build-macos-arm64, build-linux-x86_64, build-linux-arm64, build-linux-armv7l] runs-on: ubuntu-latest steps: From a03c5850797eb967876b17da82642eabf8ca0c30 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 6 Mar 2024 21:45:33 +0100 Subject: [PATCH 49/53] Add only the wheel for bookworm --- .github/workflows/main.yml | 61 ++------------------------------------ 1 file changed, 2 insertions(+), 59 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 3749d5bb6..0822d845f 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 @@ -468,66 +468,9 @@ jobs: ARTIFACTORY_USER: ${{ secrets.ARTIFACTORY_USER }} ARTIFACTORY_PASS: ${{ secrets.ARTIFACTORY_PASS }} - # This job builds wheels for ARM32 arch - build-linux-armv7l: - needs: build-docstrings - runs-on: [self-hosted, linux, ARM] - container: - image: quay.io/pypa/manylinux2014_armv7l:2024-01-08-eb135ed - env: - PLAT: manylinux2014_armv7l - # Mount local hunter cache directory, instead of transfering to Github and back - volumes: - - /.hunter:/github/home/.hunter - steps: - - uses: actions/checkout@v3 - with: - submodules: 'recursive' - - name: Installing libusb1-devel dependency - run: yum install -y --disableplugin=fastestmirror libusb1-devel - - name: Installing cmake dependency - run: | - /opt/python/cp38-cp38/bin/python3.8 -m pip install cmake - ln -s /opt/python/cp38-cp38/bin/cmake /bin/ - - name: Create folder structure - run: mkdir -p wheelhouse/audited/ - - - uses: actions/download-artifact@v3 - with: - name: 'docstrings' - path: docstrings - - name: Specify docstring to use while building the wheel - run: echo "DEPTHAI_PYTHON_DOCSTRINGS_INPUT=$PWD/docstrings/depthai_python_docstring.hpp" >> $GITHUB_ENV - - - name: Build and install depthai-core - run: | - cmake -S depthai-core/ -B build_core -D CMAKE_BUILD_TYPE=Release -D CMAKE_TOOLCHAIN_FILE=$PWD/cmake/toolchain/pic.cmake - cmake --build build_core --target install --parallel 4 - echo "DEPTHAI_INSTALLATION_DIR=$PWD/build_core/install/" >> $GITHUB_ENV - - - name: Append build hash if not a tagged commit - if: startsWith(github.ref, 'refs/tags/v') != true - run: echo "BUILD_COMMIT_HASH=${{github.sha}}" >> $GITHUB_ENV - - name: Building wheels - run: for PYBIN in /opt/python/cp3{6..12}*/bin; do "${PYBIN}/pip" wheel . -w ./wheelhouse/ --verbose; done - - name: Auditing wheels - run: for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat $PLAT -w wheelhouse/audited/; done - - name: Archive wheel artifacts - uses: actions/upload-artifact@v3 - with: - name: audited-wheels - path: wheelhouse/audited/ - - name: Deploy wheels to artifactory (if not a release) - if: startsWith(github.ref, 'refs/tags/v') != true - run: bash ./ci/upload-artifactory.sh - env: - ARTIFACTORY_URL: ${{ secrets.ARTIFACTORY_URL }} - ARTIFACTORY_USER: ${{ secrets.ARTIFACTORY_USER }} - ARTIFACTORY_PASS: ${{ secrets.ARTIFACTORY_PASS }} - release: if: startsWith(github.ref, 'refs/tags/v') - needs: [pytest, build-linux-armhf, build-windows-x86_64, build-macos-x86_64, build-macos-arm64, build-linux-x86_64, build-linux-arm64, build-linux-armv7l] + needs: [pytest, build-linux-armhf, build-windows-x86_64, build-macos-x86_64, build-macos-arm64, build-linux-x86_64, build-linux-arm64] runs-on: ubuntu-latest steps: From 8402008937e9c367b49a020f4a1943449e82c01c Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 6 Mar 2024 23:02:52 +0100 Subject: [PATCH 50/53] Don't install global packages --- .github/workflows/main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 0822d845f..d91e4bbd9 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -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/ From 5a8a85d88a3fc527b5ab5fb087bd2039b61b70b8 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 7 Mar 2024 21:34:00 +0100 Subject: [PATCH 51/53] Add missing PCL API & change th RH system --- depthai-core | 2 +- src/pipeline/datatype/PointCloudDataBindings.cpp | 2 +- src/pipeline/node/PointCloudBindings.cpp | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/depthai-core b/depthai-core index 8851bfde0..e21b295ba 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 8851bfde04f9edc646431682328182197d47febe +Subproject commit e21b295baf3362ee00331057a6dc6f9270b87e62 diff --git a/src/pipeline/datatype/PointCloudDataBindings.cpp b/src/pipeline/datatype/PointCloudDataBindings.cpp index d2e9299b7..99a340571 100644 --- a/src/pipeline/datatype/PointCloudDataBindings.cpp +++ b/src/pipeline/datatype/PointCloudDataBindings.cpp @@ -61,7 +61,7 @@ void bind_pointclouddata(pybind11::module& m, void* pCallstack){ 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, 1) = points[i].y; ra(i, 2) = points[i].z; } return arr; diff --git a/src/pipeline/node/PointCloudBindings.cpp b/src/pipeline/node/PointCloudBindings.cpp index ed471c483..63ad94393 100644 --- a/src/pipeline/node/PointCloudBindings.cpp +++ b/src/pipeline/node/PointCloudBindings.cpp @@ -40,6 +40,7 @@ void bind_pointcloud(pybind11::module& m, void* pCallstack){ .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; From a5793649d8434da32026a988ef270da96ca914c3 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 8 Mar 2024 11:46:07 +0100 Subject: [PATCH 52/53] Update FW to develop --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index e21b295ba..84e8a9c7c 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit e21b295baf3362ee00331057a6dc6f9270b87e62 +Subproject commit 84e8a9c7cce036d9cc55191ddd6b442cd7ee3262 From 0465260d0652e985801743335af6645ca65e5004 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 8 Mar 2024 14:40:46 +0100 Subject: [PATCH 53/53] Update core to main --- depthai-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai-core b/depthai-core index 84e8a9c7c..7f98aa4ca 160000 --- a/depthai-core +++ b/depthai-core @@ -1 +1 @@ -Subproject commit 84e8a9c7cce036d9cc55191ddd6b442cd7ee3262 +Subproject commit 7f98aa4ca474083767b98907b83352f82a85f0f9