From 5c579c924169f053dfddd12b17e52ab591b744fd Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 6 Dec 2024 19:06:44 +0100 Subject: [PATCH 01/51] rgb node init impl --- CMakeLists.txt | 1 + examples/cpp/CMakeLists.txt | 3 + examples/cpp/RVC2/VSLAM/rgbd.cpp | 82 +++++++++++++ include/depthai/basalt/BasaltVIO.hpp | 2 +- .../pipeline/datatype/PointCloudData.hpp | 2 + include/depthai/pipeline/node/host/RGBD.hpp | 49 ++++++++ include/depthai/pipeline/nodes.hpp | 1 + src/pipeline/datatype/PointCloudData.cpp | 21 ++++ src/pipeline/node/host/RGBD.cpp | 110 ++++++++++++++++++ 9 files changed, 270 insertions(+), 1 deletion(-) create mode 100644 examples/cpp/RVC2/VSLAM/rgbd.cpp create mode 100644 include/depthai/pipeline/node/host/RGBD.hpp create mode 100644 src/pipeline/node/host/RGBD.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7955d451f..64d83e3a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -365,6 +365,7 @@ set(TARGET_CORE_SOURCES src/pipeline/node/host/XLinkInHost.cpp src/pipeline/node/host/XLinkOutHost.cpp src/pipeline/node/host/HostNode.cpp + src/pipeline/node/host/RGBD.cpp src/pipeline/datatype/DatatypeEnum.cpp src/pipeline/node/PointCloud.cpp src/pipeline/datatype/Buffer.cpp diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 46ad027ff..97a9dcc28 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -309,6 +309,7 @@ if(DEPTHAI_RTABMAP_SUPPORT) endif() + if(DEPTHAI_BASALT_SUPPORT) include(FetchContent) FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.1/rerun_cpp_sdk.zip) @@ -316,6 +317,8 @@ if(DEPTHAI_BASALT_SUPPORT) dai_add_example(basalt_vio RVC2/VSLAM/basalt_vio.cpp OFF OFF) target_link_libraries(basalt_vio PRIVATE depthai::basalt rerun_sdk) +dai_add_example(rgbd RVC2/VSLAM/rgbd.cpp OFF OFF) + target_link_libraries(rgbd PRIVATE rerun_sdk) endif() diff --git a/examples/cpp/RVC2/VSLAM/rgbd.cpp b/examples/cpp/RVC2/VSLAM/rgbd.cpp new file mode 100644 index 000000000..7e9ac26bc --- /dev/null +++ b/examples/cpp/RVC2/VSLAM/rgbd.cpp @@ -0,0 +1,82 @@ +#include "depthai/depthai.hpp" +#include "rerun.hpp" + +class RerunNode : public dai::NodeCRTP { + public: + constexpr static const char* NAME = "RerunNode"; + + public: + void build() {} + + Input inputPCL{*this, {.name = "inPCL", .types = {{dai::DatatypeEnum::PointCloudData, true}}}}; + + void run() override { + const auto rec = rerun::RecordingStream("rerun"); + rec.spawn().exit_on_failure(); + rec.log_static("world", rerun::ViewCoordinates::FLU); + rec.log("world/ground", rerun::Boxes3D::from_half_sizes({{3.f, 3.f, 0.00001f}})); + while(isRunning()) { + auto pclIn = inputPCL.tryGet(); + if(pclIn != nullptr) { + std::vector points; + std::vector colors; + const auto& size = pclIn->getWidth() * pclIn->getHeight(); + points.reserve(size); + colors.reserve(size); + const auto& pclData = pclIn->getPointsRGB(); + for(size_t i = 0; i < size; ++i) { + points.emplace_back(pclData[i].x, pclData[i].y, pclData[i].z); + colors.emplace_back(pclData[i].r, pclData[i].g, pclData[i].b); + } + rec.log("world/obstacle_pcl", rerun::Points3D(points).with_colors(colors).with_radii({0.01f})); + } + } + } +}; +int main() { + using namespace std; + // Create pipeline + dai::Pipeline pipeline; + // Define sources and outputs + int fps = 30; + int width = 640; + int height = 400; + // Define sources and outputs + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + auto rgbd = pipeline.create()->build(); + auto color = pipeline.create(); + auto rerun = pipeline.create(); + stereo->setExtendedDisparity(false); + color->build(); + + left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + left->setCamera("left"); + left->setFps(fps); + right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + right->setCamera("right"); + right->setFps(fps); + stereo->setSubpixel(true); + stereo->setExtendedDisparity(false); + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setLeftRightCheck(true); + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->enableDistortionCorrection(true); + stereo->initialConfig.setLeftRightCheckThreshold(10); + stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); + + auto *out = color->requestOutput(std::pair(1280, 720)); + left->out.link(stereo->left); + right->out.link(stereo->right); + + stereo->depth.link(rgbd->inDepth); + out->link(rgbd->inColor); + + // Linking + rgbd->pointCloud.link(rerun->inputPCL); + pipeline.start(); + auto device = pipeline.getDefaultDevice(); + device->setIrLaserDotProjectorIntensity(0.7); + pipeline.wait(); +} diff --git a/include/depthai/basalt/BasaltVIO.hpp b/include/depthai/basalt/BasaltVIO.hpp index cbc49f01b..a01cb36d3 100644 --- a/include/depthai/basalt/BasaltVIO.hpp +++ b/include/depthai/basalt/BasaltVIO.hpp @@ -1,7 +1,7 @@ #pragma once #define SOPHUS_USE_BASIC_LOGGING -#include +#include "depthai/pipeline/Subnode.hpp" #include "basalt/calibration/calibration.hpp" #include "basalt/serialization/headers_serialization.h" diff --git a/include/depthai/pipeline/datatype/PointCloudData.hpp b/include/depthai/pipeline/datatype/PointCloudData.hpp index de06cba01..6b5e52cb1 100644 --- a/include/depthai/pipeline/datatype/PointCloudData.hpp +++ b/include/depthai/pipeline/datatype/PointCloudData.hpp @@ -41,6 +41,8 @@ class PointCloudData : public Buffer, public ProtoSerializable { std::vector getPoints(); std::vector getPointsRGB(); + void setPoints(const std::vector& points); + void setPointsRGB(const std::vector& points); /** * Retrieves instance number diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp new file mode 100644 index 000000000..024ad72e8 --- /dev/null +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -0,0 +1,49 @@ +#pragma once +#include "depthai/pipeline/Subnode.hpp" +#include "depthai/pipeline/ThreadedHostNode.hpp" +#include "depthai/pipeline/node/Sync.hpp" +#include "depthai/pipeline/datatype/PointCloudData.hpp" + +namespace dai { +namespace node { + +/** + * @brief RGBD node. Combines depth and color frames into a single point cloud. + */ +class RGBD : public NodeCRTP { + public: + constexpr static const char* NAME = "RGBD"; + + Subnode sync{*this, "sync"}; + InputMap& inputs = sync->inputs; + + std::string colorInputName = "color"; + std::string depthInputName = "depth"; + + /** + * Input color frame. + */ + Input& inColor = inputs[colorInputName]; + /** + * Input depth frame. + */ + Input& inDepth = inputs[depthInputName]; + + /** + * Output point cloud. + */ + Output pointCloud{*this, {"pointCloud", DEFAULT_GROUP, {{DatatypeEnum::PointCloudData, true}}}}; + + std::shared_ptr build(); + std::shared_ptr build(bool autocreate); +private: + void run() override; + void initialize(std::vector> frames); + Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; + bool initialized = false; + float fx, fy, cx, cy; +}; + +} // namespace node +} // namespace dai + diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index f1b1bccf6..34797fe51 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -34,6 +34,7 @@ #include "node/Warp.hpp" #include "node/XLinkIn.hpp" #include "node/XLinkOut.hpp" +#include "node/host/RGBD.hpp" #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT #include "node/host/Display.hpp" #include "node/host/HostCamera.hpp" diff --git a/src/pipeline/datatype/PointCloudData.cpp b/src/pipeline/datatype/PointCloudData.cpp index 5f5046630..69b3ced8f 100644 --- a/src/pipeline/datatype/PointCloudData.cpp +++ b/src/pipeline/datatype/PointCloudData.cpp @@ -37,6 +37,27 @@ std::vector PointCloudData::getPointsRGB() { return points; } +void PointCloudData::setPoints(const std::vector& points) { + auto size = points.size(); + std::vector data(size * sizeof(Point3f)); + auto* dataPtr = reinterpret_cast(data.data()); + for(unsigned int i = 0; i < size; i++) { + dataPtr[i] = points[i]; + } + setData(data); +} + +void PointCloudData::setPointsRGB(const std::vector& points) { + auto size = points.size(); + std::vector data(size * sizeof(Point3fRGB)); + auto* dataPtr = reinterpret_cast(data.data()); + for(unsigned int i = 0; i < size; i++) { + dataPtr[i] = points[i]; + } + setData(data); + color = true; +} + unsigned int PointCloudData::getInstanceNum() const { return instanceNum; } diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp new file mode 100644 index 000000000..f6ccbfdac --- /dev/null +++ b/src/pipeline/node/host/RGBD.cpp @@ -0,0 +1,110 @@ +#include "depthai/pipeline/node/host/RGBD.hpp" + +#include "common/Point3fRGB.hpp" +#include "depthai/common/Point3fRGB.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" +#include "depthai/pipeline/datatype/MessageGroup.hpp" +#include "depthai/pipeline/datatype/PointCloudData.hpp" +#include "depthai/pipeline/node/Camera.hpp" +#include "depthai/pipeline/node/StereoDepth.hpp" +#include "depthai/pipeline/node/Sync.hpp" + +namespace dai { +namespace node { + +std::shared_ptr RGBD::build() { + // Link the inputs + sync->setRunOnHost(false); + sync->out.link(inSync); + inColor.setBlocking(false); + inColor.setMaxSize(1); + inDepth.setBlocking(false); + inDepth.setMaxSize(1); + inSync.setMaxSize(1); + inSync.setBlocking(false); + return std::static_pointer_cast(shared_from_this()); +} + +// std::shared_ptr RGBD::build(bool autocreate) { +// if(!autocreate) { +// return std::static_pointer_cast(shared_from_this()); +// } +// auto pipeline = getParentPipeline(); +// auto colorCam = pipeline.create(); +// auto depth = pipeline.create(); +// return build(, depth->depth); +// } + +void RGBD::initialize(std::vector> frames) { + // Initialize the camera intrinsics + auto frame = frames.at(0); + auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); + auto camID = static_cast(frame->getInstanceNum()); + auto intrinsics = calibHandler.getCameraIntrinsics(camID, frame->getWidth(), frame->getHeight()); + fx = intrinsics[0][0]; + fy = intrinsics[1][1]; + cx = intrinsics[0][2]; + cy = intrinsics[1][2]; + initialized = true; +} + +void RGBD::run() { + while(isRunning()) { + // Get the color and depth frames + auto group = inSync.tryGet(); + if(group == nullptr) continue; + if(!initialized) { + std::vector> imgFrames; + for(auto& msg : *group) { + imgFrames.emplace_back(std::dynamic_pointer_cast(msg.second)); + } + + initialize(imgFrames); + } + auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColor.getName())); + auto depthFrame = std::dynamic_pointer_cast(group->group.at(inDepth.getName())); + + // Create the point cloud + auto pc = std::make_shared(); + pc->setTimestamp(colorFrame->getTimestamp()); + pc->setTimestampDevice(colorFrame->getTimestampDevice()); + pc->setSequenceNum(colorFrame->getSequenceNum()); + pc->setInstanceNum(colorFrame->getInstanceNum()); + uint width = colorFrame->getWidth(); + uint height = colorFrame->getHeight(); + pc->setSize(width, height); + + std::vector points; + const auto& size = width * height; + points.reserve(size); + // Fill the point cloud + auto depthData = depthFrame->getCvFrame(); + auto colorData = colorFrame->getCvFrame(); + for(unsigned int i = 0; i < height; i++) { + for(unsigned int j = 0; j < width; j++) { + // Get the depth value as UINT16 + uint16_t depthValue = depthData.at(i, j); + // Convert the depth value to meters + float z = depthValue / 1000.0f; + // limit the depth value to 10m + if(z > 10.0f) { + z = 10.0f; + } + + // Calculate the 3D point + float x = (j - cy) * z / fy; + float y = (i - cx) * z / fx; + + // Get the color + auto color = colorData.at(i, j); + // Add the point to the point cloud + points.emplace_back(Point3fRGB{x, y, z, static_cast(color[2]), static_cast(color[1]), static_cast(color[0])}); + } + } + pc->setPointsRGB(points); + pointCloud.send(pc); + } +} +} // namespace node +} // namespace dai From 111386d67b52ea80a3e9817f8c62ef0f140bee36 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 9 Dec 2024 13:48:44 +0100 Subject: [PATCH 02/51] add python bindings for rgbd --- bindings/python/CMakeLists.txt | 1 + .../python/src/pipeline/node/NodeBindings.cpp | 2 + .../python/src/pipeline/node/RGBDBindings.cpp | 37 +++++++++ examples/cpp/CMakeLists.txt | 9 ++- .../cpp/{RVC2/VSLAM => HostNodes}/rgbd.cpp | 3 +- examples/cpp/Visualizer/visualizer_rgbd.cpp | 76 ++++++++++++++++++ examples/python/HostNodes/rgbd.py | 79 +++++++++++++++++++ examples/python/Visualizer/visualizer_rgbd.py | 63 +++++++++++++++ include/depthai/pipeline/node/host/RGBD.hpp | 3 +- 9 files changed, 268 insertions(+), 5 deletions(-) create mode 100644 bindings/python/src/pipeline/node/RGBDBindings.cpp rename examples/cpp/{RVC2/VSLAM => HostNodes}/rgbd.cpp (98%) create mode 100644 examples/cpp/Visualizer/visualizer_rgbd.cpp create mode 100644 examples/python/HostNodes/rgbd.py create mode 100644 examples/python/Visualizer/visualizer_rgbd.py diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 1b9155794..efe1b36de 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -108,6 +108,7 @@ set(SOURCE_LIST src/pipeline/node/RecordBindings.cpp src/pipeline/node/ReplayBindings.cpp src/pipeline/node/ImageAlignBindings.cpp + src/pipeline/node/RGBDBindings.cpp src/pipeline/datatype/ADatatypeBindings.cpp src/pipeline/datatype/AprilTagConfigBindings.cpp diff --git a/bindings/python/src/pipeline/node/NodeBindings.cpp b/bindings/python/src/pipeline/node/NodeBindings.cpp index fae2706ca..b45bbd64a 100644 --- a/bindings/python/src/pipeline/node/NodeBindings.cpp +++ b/bindings/python/src/pipeline/node/NodeBindings.cpp @@ -161,6 +161,7 @@ void bind_hostnode(pybind11::module& m, void* pCallstack); void bind_record(pybind11::module& m, void* pCallstack); void bind_replay(pybind11::module& m, void* pCallstack); void bind_imagealign(pybind11::module& m, void* pCallstack); +void bind_rgbd(pybind11::module& m, void* pCallstack); #ifdef DEPTHAI_HAVE_BASALT_SUPPORT void bind_basaltnode(pybind11::module& m, void* pCallstack); #endif @@ -208,6 +209,7 @@ void NodeBindings::addToCallstack(std::deque& callstack) { callstack.push_front(bind_record); callstack.push_front(bind_replay); callstack.push_front(bind_imagealign); + callstack.push_front(bind_rgbd); #ifdef DEPTHAI_HAVE_BASALT_SUPPORT callstack.push_front(bind_basaltnode); #endif diff --git a/bindings/python/src/pipeline/node/RGBDBindings.cpp b/bindings/python/src/pipeline/node/RGBDBindings.cpp new file mode 100644 index 000000000..24ba44ad9 --- /dev/null +++ b/bindings/python/src/pipeline/node/RGBDBindings.cpp @@ -0,0 +1,37 @@ + +#include "Common.hpp" +#include "NodeBindings.hpp" +#include "depthai/pipeline/ThreadedHostNode.hpp" +#include "depthai/pipeline/node/host/RGBD.hpp" + +#include + +extern py::handle daiNodeModule; + +void bind_rgbd(pybind11::module& m, void* pCallstack){ + using namespace dai; + using namespace dai::node; + + // declare upfront + auto rgbdNode = ADD_NODE_DERIVED(RGBD, ThreadedHostNode); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // 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 + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + + // RGBD Node + rgbdNode + .def_property_readonly("inColor", [](RGBD& node){ return &node.inColor; }, py::return_value_policy::reference_internal) + .def_property_readonly("inDepth", [](RGBD& node){ return &node.inColor; }, py::return_value_policy::reference_internal) + .def_readonly("pcl", &RGBD::pcl, DOC(dai, node, RGBD, pcl)) + .def("build", &RGBD::build, DOC(dai, node, RGBD, build)); +} diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 97a9dcc28..ae70dc987 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -291,6 +291,13 @@ dai_add_example(image_align RVC2/ImageAlign/image_align.cpp OFF OFF) dai_add_example(visualizer_yolo Visualizer/visualizer_yolo.cpp ON OFF) dai_set_example_test_labels(visualizer_yolo ondevice rvc2_all rvc4) +include(FetchContent) +FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.1/rerun_cpp_sdk.zip) +FetchContent_MakeAvailable(rerun_sdk) +dai_add_example(rgbd HostNodes/rgbd.cpp OFF OFF) +target_link_libraries(rgbd PRIVATE rerun_sdk) + +dai_add_example(visualizer_rgbd Visualizer/visualizer_rgbd.cpp OFF OFF) if(DEPTHAI_RTABMAP_SUPPORT) include(FetchContent) @@ -317,8 +324,6 @@ if(DEPTHAI_BASALT_SUPPORT) dai_add_example(basalt_vio RVC2/VSLAM/basalt_vio.cpp OFF OFF) target_link_libraries(basalt_vio PRIVATE depthai::basalt rerun_sdk) -dai_add_example(rgbd RVC2/VSLAM/rgbd.cpp OFF OFF) - target_link_libraries(rgbd PRIVATE rerun_sdk) endif() diff --git a/examples/cpp/RVC2/VSLAM/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp similarity index 98% rename from examples/cpp/RVC2/VSLAM/rgbd.cpp rename to examples/cpp/HostNodes/rgbd.cpp index 7e9ac26bc..535f20332 100644 --- a/examples/cpp/RVC2/VSLAM/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -1,3 +1,4 @@ + #include "depthai/depthai.hpp" #include "rerun.hpp" @@ -74,7 +75,7 @@ int main() { out->link(rgbd->inColor); // Linking - rgbd->pointCloud.link(rerun->inputPCL); + rgbd->pcl.link(rerun->inputPCL); pipeline.start(); auto device = pipeline.getDefaultDevice(); device->setIrLaserDotProjectorIntensity(0.7); diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/Visualizer/visualizer_rgbd.cpp new file mode 100644 index 000000000..0f52b9bdd --- /dev/null +++ b/examples/cpp/Visualizer/visualizer_rgbd.cpp @@ -0,0 +1,76 @@ +#include "depthai/depthai.hpp" + +#include +#include +#include + +// Signal handling for clean shutdown +static bool isRunning = true; +void signalHandler(int signum) { + isRunning = false; +} + +int main() { + using namespace std; + // Default port values + int webSocketPort = 8765; + int httpPort = 8080; + + // Register signal handler + std::signal(SIGINT, signalHandler); + + // Create RemoteConnection + dai::RemoteConnection remoteConnector(dai::RemoteConnection::DEFAULT_ADDRESS, webSocketPort, true, httpPort); + // Create pipeline + dai::Pipeline pipeline; + // Define sources and outputs + int fps = 30; + int width = 640; + int height = 400; + // Define sources and outputs + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + auto rgbd = pipeline.create()->build(); + auto color = pipeline.create(); + stereo->setExtendedDisparity(false); + color->build(); + + left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + left->setCamera("left"); + left->setFps(fps); + right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + right->setCamera("right"); + right->setFps(fps); + stereo->setSubpixel(true); + stereo->setExtendedDisparity(false); + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setLeftRightCheck(true); + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->enableDistortionCorrection(true); + stereo->initialConfig.setLeftRightCheckThreshold(10); + stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); + + auto *out = color->requestOutput(std::pair(1280, 720)); + left->out.link(stereo->left); + right->out.link(stereo->right); + + stereo->depth.link(rgbd->inDepth); + out->link(rgbd->inColor); + + remoteConnector.addTopic("pcl", rgbd->pcl); + pipeline.start(); + auto device = pipeline.getDefaultDevice(); + device->setIrLaserDotProjectorIntensity(0.7); + // Main loop + while(isRunning && pipeline.isRunning()) { + int key = remoteConnector.waitKey(1); + if(key == 'q') { + std::cout << "Got 'q' key from the remote connection!" << std::endl; + break; + } + } + + std::cout << "Pipeline stopped." << std::endl; + return 0; +} diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/HostNodes/rgbd.py new file mode 100644 index 000000000..4e9019a7b --- /dev/null +++ b/examples/python/HostNodes/rgbd.py @@ -0,0 +1,79 @@ +import time +import depthai as dai +import sys + +from pathlib import Path +installExamplesStr = Path(__file__).absolute().parents[2] / 'install_requirements.py --install_rerun' +try: + import rerun as rr +except ImportError: + sys.exit("Critical dependency missing: Rerun. Please install it using the command: '{} {}' and then rerun the script.".format(sys.executable, installExamplesStr)) + +import cv2 + +class RerunNode(dai.node.ThreadedHostNode): + def __init__(self): + dai.node.ThreadedHostNode.__init__(self) + self.inputPCL = self.createInput() + + + def run(self): + rr.init("", spawn=True) + rr.log("world", rr.ViewCoordinates.FLU) + rr.log("world/ground", rr.Boxes3D(half_sizes=[3.0, 3.0, 0.00001])) + while self.isRunning(): + pclObstData = self.inputPCL.tryGet() + if pclObstData is not None: + print("runnnn") + points, colors = pclObstData.getPointsRGB() + rr.log("world/pcl", rr.Points3D(points, colors=colors, radii=[0.01])) + +# Create pipeline + +with dai.Pipeline() as p: + fps = 30 + width = 640 + height = 400 + # Define sources and outputs + left = p.create(dai.node.MonoCamera) + right = p.create(dai.node.MonoCamera) + color = p.create(dai.node.ColorCamera) + stereo = p.create(dai.node.StereoDepth) + rgbd = p.create(dai.node.RGBD).build() + + rerunViewer = RerunNode() + left.setBoardSocket(dai.CameraBoardSocket.CAM_B) + left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) + left.setFps(fps) + right.setBoardSocket(dai.CameraBoardSocket.CAM_C) + right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) + right.setFps(fps) + color.setBoardSocket(dai.CameraBoardSocket.CAM_A) + color.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) + color.setInterleaved(False) + color.setIspScale(2, 3) + color.setFps(fps) + + stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) + stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) + + stereo.setExtendedDisparity(False) + stereo.setLeftRightCheck(True) + stereo.setRectifyEdgeFillColor(0) + stereo.enableDistortionCorrection(True) + stereo.initialConfig.setLeftRightCheckThreshold(10) + stereo.setSubpixel(True) + + + # Linking + + left.out.link(stereo.left) + right.out.link(stereo.right) + stereo.depth.link(rgbd.inDepth) + color.preview.link(rgbd.inColor) + + rgbd.pcl.link(rerunViewer.inputPCL) + + p.start() + while p.isRunning(): + time.sleep(1) diff --git a/examples/python/Visualizer/visualizer_rgbd.py b/examples/python/Visualizer/visualizer_rgbd.py new file mode 100644 index 000000000..dc4e047ef --- /dev/null +++ b/examples/python/Visualizer/visualizer_rgbd.py @@ -0,0 +1,63 @@ +import time +import depthai as dai + +from argparse import ArgumentParser + +parser = ArgumentParser() +parser.add_argument("--webSocketPort", type=int, default=8765) +parser.add_argument("--httpPort", type=int, default=8080) +args = parser.parse_args() + +with dai.Pipeline() as p: + remoteConnector = dai.RemoteConnection(webSocketPort=args.webSocketPort, httpPort=args.httpPort) + fps = 30 + width = 640 + height = 400 + # Define sources and outputs + left = p.create(dai.node.MonoCamera) + right = p.create(dai.node.MonoCamera) + color = p.create(dai.node.ColorCamera) + stereo = p.create(dai.node.StereoDepth) + rgbd = p.create(dai.node.RGBD).build() + + left.setBoardSocket(dai.CameraBoardSocket.CAM_B) + left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) + left.setFps(fps) + right.setBoardSocket(dai.CameraBoardSocket.CAM_C) + right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) + right.setFps(fps) + color.setBoardSocket(dai.CameraBoardSocket.CAM_A) + color.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) + color.setInterleaved(False) + color.setIspScale(2, 3) + + stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) + stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) + + stereo.setExtendedDisparity(False) + stereo.setLeftRightCheck(True) + stereo.setRectifyEdgeFillColor(0) + stereo.enableDistortionCorrection(True) + stereo.initialConfig.setLeftRightCheckThreshold(10) + stereo.setSubpixel(True) + + + remoteConnector.addTopic("pcl", rgbd.pcl, "common") + remoteConnector.addTopic("depth", stereo.depth, "common") + remoteConnector.addTopic("color", color.video, "common") + # Linking + + left.out.link(stereo.left) + right.out.link(stereo.right) + stereo.depth.link(rgbd.inDepth) + color.video.link(rgbd.inColor) + + + p.start() + remoteConnector.registerPipeline(p) + + while p.isRunning(): + key = remoteConnector.waitKey(1) + if key == ord("q"): + print("Got q key from the remote connection!") + break diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 024ad72e8..591148575 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -32,10 +32,9 @@ class RGBD : public NodeCRTP { /** * Output point cloud. */ - Output pointCloud{*this, {"pointCloud", DEFAULT_GROUP, {{DatatypeEnum::PointCloudData, true}}}}; + Output pcl{*this, {"pcl", DEFAULT_GROUP, {{DatatypeEnum::PointCloudData, true}}}}; std::shared_ptr build(); - std::shared_ptr build(bool autocreate); private: void run() override; void initialize(std::vector> frames); From 90a9ee6dab9e4621c8d5072d5856ff3bc1ec76bf Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 9 Dec 2024 15:49:13 +0100 Subject: [PATCH 03/51] fix bindings --- .../python/src/pipeline/node/RGBDBindings.cpp | 2 +- examples/python/HostNodes/rgbd.py | 20 +++++--------- examples/python/Visualizer/visualizer_rgbd.py | 26 +++++++------------ include/depthai/pipeline/node/host/RGBD.hpp | 4 +-- src/pipeline/node/host/RGBD.cpp | 2 +- 5 files changed, 20 insertions(+), 34 deletions(-) diff --git a/bindings/python/src/pipeline/node/RGBDBindings.cpp b/bindings/python/src/pipeline/node/RGBDBindings.cpp index 24ba44ad9..361e648d8 100644 --- a/bindings/python/src/pipeline/node/RGBDBindings.cpp +++ b/bindings/python/src/pipeline/node/RGBDBindings.cpp @@ -31,7 +31,7 @@ void bind_rgbd(pybind11::module& m, void* pCallstack){ // RGBD Node rgbdNode .def_property_readonly("inColor", [](RGBD& node){ return &node.inColor; }, py::return_value_policy::reference_internal) - .def_property_readonly("inDepth", [](RGBD& node){ return &node.inColor; }, py::return_value_policy::reference_internal) + .def_property_readonly("inDepth", [](RGBD& node){ return &node.inDepth; }, py::return_value_policy::reference_internal) .def_readonly("pcl", &RGBD::pcl, DOC(dai, node, RGBD, pcl)) .def("build", &RGBD::build, DOC(dai, node, RGBD, build)); } diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/HostNodes/rgbd.py index 4e9019a7b..9f5517b57 100644 --- a/examples/python/HostNodes/rgbd.py +++ b/examples/python/HostNodes/rgbd.py @@ -24,7 +24,6 @@ def run(self): while self.isRunning(): pclObstData = self.inputPCL.tryGet() if pclObstData is not None: - print("runnnn") points, colors = pclObstData.getPointsRGB() rr.log("world/pcl", rr.Points3D(points, colors=colors, radii=[0.01])) @@ -32,33 +31,28 @@ def run(self): with dai.Pipeline() as p: fps = 30 - width = 640 - height = 400 # Define sources and outputs left = p.create(dai.node.MonoCamera) right = p.create(dai.node.MonoCamera) - color = p.create(dai.node.ColorCamera) + color = p.create(dai.node.Camera) stereo = p.create(dai.node.StereoDepth) rgbd = p.create(dai.node.RGBD).build() - + color.build() rerunViewer = RerunNode() - left.setBoardSocket(dai.CameraBoardSocket.CAM_B) + left.setCamera("left") left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) left.setFps(fps) - right.setBoardSocket(dai.CameraBoardSocket.CAM_C) + right.setCamera("right") right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) right.setFps(fps) - color.setBoardSocket(dai.CameraBoardSocket.CAM_A) - color.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) - color.setInterleaved(False) - color.setIspScale(2, 3) - color.setFps(fps) + out = color.requestOutput((1280,720)) stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) + stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) stereo.setRectifyEdgeFillColor(0) stereo.enableDistortionCorrection(True) stereo.initialConfig.setLeftRightCheckThreshold(10) @@ -70,7 +64,7 @@ def run(self): left.out.link(stereo.left) right.out.link(stereo.right) stereo.depth.link(rgbd.inDepth) - color.preview.link(rgbd.inColor) + out.link(rgbd.inColor) rgbd.pcl.link(rerunViewer.inputPCL) diff --git a/examples/python/Visualizer/visualizer_rgbd.py b/examples/python/Visualizer/visualizer_rgbd.py index dc4e047ef..60ff55663 100644 --- a/examples/python/Visualizer/visualizer_rgbd.py +++ b/examples/python/Visualizer/visualizer_rgbd.py @@ -10,48 +10,40 @@ with dai.Pipeline() as p: remoteConnector = dai.RemoteConnection(webSocketPort=args.webSocketPort, httpPort=args.httpPort) - fps = 30 - width = 640 - height = 400 - # Define sources and outputs + fps = 30.0 left = p.create(dai.node.MonoCamera) right = p.create(dai.node.MonoCamera) - color = p.create(dai.node.ColorCamera) + color = p.create(dai.node.Camera) stereo = p.create(dai.node.StereoDepth) rgbd = p.create(dai.node.RGBD).build() - - left.setBoardSocket(dai.CameraBoardSocket.CAM_B) + color.build() + left.setCamera("left") left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) left.setFps(fps) - right.setBoardSocket(dai.CameraBoardSocket.CAM_C) + right.setCamera("right") right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) right.setFps(fps) - color.setBoardSocket(dai.CameraBoardSocket.CAM_A) - color.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) - color.setInterleaved(False) - color.setIspScale(2, 3) + out = color.requestOutput((1280,720)) stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) + stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) stereo.setRectifyEdgeFillColor(0) stereo.enableDistortionCorrection(True) stereo.initialConfig.setLeftRightCheckThreshold(10) stereo.setSubpixel(True) - remoteConnector.addTopic("pcl", rgbd.pcl, "common") - remoteConnector.addTopic("depth", stereo.depth, "common") - remoteConnector.addTopic("color", color.video, "common") # Linking left.out.link(stereo.left) right.out.link(stereo.right) stereo.depth.link(rgbd.inDepth) - color.video.link(rgbd.inColor) - + out.link(rgbd.inColor) + remoteConnector.addTopic("pcl", rgbd.pcl, "common") p.start() remoteConnector.registerPipeline(p) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 591148575..e7782935f 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -17,8 +17,8 @@ class RGBD : public NodeCRTP { Subnode sync{*this, "sync"}; InputMap& inputs = sync->inputs; - std::string colorInputName = "color"; - std::string depthInputName = "depth"; + std::string colorInputName = "inColor"; + std::string depthInputName = "inDepth"; /** * Input color frame. diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index f6ccbfdac..702aabad4 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -103,7 +103,7 @@ void RGBD::run() { } } pc->setPointsRGB(points); - pointCloud.send(pc); + pcl.send(pc); } } } // namespace node From 858d4ba16ec63754e6daf61248902224d25c2138 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 11 Dec 2024 08:57:20 +0100 Subject: [PATCH 04/51] remove byte padding for point3frgb to fix issues with visualizer --- include/depthai/common/Point3fRGB.hpp | 7 +++++-- src/pipeline/datatype/PointCloudData.cpp | 3 ++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/include/depthai/common/Point3fRGB.hpp b/include/depthai/common/Point3fRGB.hpp index c37086746..cbcb69c7f 100644 --- a/include/depthai/common/Point3fRGB.hpp +++ b/include/depthai/common/Point3fRGB.hpp @@ -1,5 +1,6 @@ #pragma once - +#pragma pack(push) +#pragma pack(1) // std #include @@ -22,4 +23,6 @@ struct Point3fRGB { DEPTHAI_SERIALIZE_EXT(Point3fRGB, x, y, z, r, g, b); -} // namespace dai \ No newline at end of file +} // namespace dai +#pragma pack(pop) + diff --git a/src/pipeline/datatype/PointCloudData.cpp b/src/pipeline/datatype/PointCloudData.cpp index 69b3ced8f..721e9fa1f 100644 --- a/src/pipeline/datatype/PointCloudData.cpp +++ b/src/pipeline/datatype/PointCloudData.cpp @@ -55,7 +55,7 @@ void PointCloudData::setPointsRGB(const std::vector& points) { dataPtr[i] = points[i]; } setData(data); - color = true; + setColor(true); } unsigned int PointCloudData::getInstanceNum() const { @@ -189,4 +189,5 @@ ProtoSerializable::SchemaPair PointCloudData::serializeSchema() const { #endif static_assert(sizeof(Point3f) == 12, "Point3f size must be 12 bytes"); +static_assert(sizeof(Point3fRGB) == 15, "Point3fRGB size must be 15 bytes"); } // namespace dai From a2d0158420e729a38949cb4868ec699b67995049 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 11 Dec 2024 10:23:43 +0100 Subject: [PATCH 05/51] add internal imagealign --- include/depthai/pipeline/node/host/RGBD.hpp | 20 ++++++--- src/pipeline/node/host/RGBD.cpp | 45 +++++++++++---------- 2 files changed, 39 insertions(+), 26 deletions(-) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index e7782935f..57c30b7de 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -2,6 +2,7 @@ #include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/node/Sync.hpp" +#include "depthai/pipeline/node/ImageAlign.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" namespace dai { @@ -15,19 +16,17 @@ class RGBD : public NodeCRTP { constexpr static const char* NAME = "RGBD"; Subnode sync{*this, "sync"}; + std::shared_ptr align; InputMap& inputs = sync->inputs; - std::string colorInputName = "inColor"; - std::string depthInputName = "inDepth"; - /** * Input color frame. */ - Input& inColor = inputs[colorInputName]; + Input inColor{*this, {"inColor", DEFAULT_GROUP, false, 0, {{DatatypeEnum::ImgFrame, true}}}}; /** * Input depth frame. */ - Input& inDepth = inputs[depthInputName]; + Input inDepth{*this, {"inDepth", DEFAULT_GROUP, false, 0, {{DatatypeEnum::ImgFrame, true}}}}; /** * Output point cloud. @@ -35,12 +34,23 @@ class RGBD : public NodeCRTP { Output pcl{*this, {"pcl", DEFAULT_GROUP, {{DatatypeEnum::PointCloudData, true}}}}; std::shared_ptr build(); + std::shared_ptr build(bool autocreate, std::pair size); + void setOutputMeters(bool outputMeters) { + this->outputMeters = outputMeters; + } private: + std::string colorInputName = "inColorSync"; + std::string depthInputName = "inDepthSync"; + Input& inColorSync = inputs[colorInputName]; + Input& inDepthSync = inputs[depthInputName]; + Output colorMux{*this, {"colorMux", DEFAULT_GROUP, {{DatatypeEnum::ImgFrame, true}}}}; + Output depthPT{*this, {"depthPT", DEFAULT_GROUP, {{DatatypeEnum::ImgFrame, true}}}}; void run() override; void initialize(std::vector> frames); Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; bool initialized = false; float fx, fy, cx, cy; + bool outputMeters=false; }; } // namespace node diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 702aabad4..869786aea 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -14,27 +14,32 @@ namespace dai { namespace node { std::shared_ptr RGBD::build() { - // Link the inputs + align = getParentPipeline().create(); + align->outputAligned.link(inDepthSync); + colorMux.link(inColorSync); + colorMux.link(align->inputAlignTo); + depthPT.link(align->input); sync->setRunOnHost(false); sync->out.link(inSync); - inColor.setBlocking(false); - inColor.setMaxSize(1); - inDepth.setBlocking(false); - inDepth.setMaxSize(1); inSync.setMaxSize(1); inSync.setBlocking(false); + inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); + inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); return std::static_pointer_cast(shared_from_this()); } -// std::shared_ptr RGBD::build(bool autocreate) { -// if(!autocreate) { -// return std::static_pointer_cast(shared_from_this()); -// } -// auto pipeline = getParentPipeline(); -// auto colorCam = pipeline.create(); -// auto depth = pipeline.create(); -// return build(, depth->depth); -// } +std::shared_ptr RGBD::build(bool autocreate, std::pair size) { + if(!autocreate) { + return std::static_pointer_cast(shared_from_this()); + } + auto pipeline = getParentPipeline(); + auto colorCam = pipeline.create()->build(); + auto depth = pipeline.create()->build(true); + auto* out = colorCam->requestOutput(size); + out->link(inColor); + depth->depth.link(inDepth); + return build(); +} void RGBD::initialize(std::vector> frames) { // Initialize the camera intrinsics @@ -62,8 +67,8 @@ void RGBD::run() { initialize(imgFrames); } - auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColor.getName())); - auto depthFrame = std::dynamic_pointer_cast(group->group.at(inDepth.getName())); + auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColorSync.getName())); + auto depthFrame = std::dynamic_pointer_cast(group->group.at(inDepthSync.getName())); // Create the point cloud auto pc = std::make_shared(); @@ -86,12 +91,10 @@ void RGBD::run() { // Get the depth value as UINT16 uint16_t depthValue = depthData.at(i, j); // Convert the depth value to meters - float z = depthValue / 1000.0f; - // limit the depth value to 10m - if(z > 10.0f) { - z = 10.0f; + float z = depthValue; + if(outputMeters) { + z /= 1000.0f; } - // Calculate the 3D point float x = (j - cy) * z / fy; float y = (i - cx) * z / fx; From 6be50eab00a15a4c6f3169bf783d10bebbf65163 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 12 Dec 2024 13:32:03 +0100 Subject: [PATCH 06/51] add compute --- CMakeLists.txt | 20 ++- cmake/depthaiDependencies.cmake | 3 + cmake/ports/kompute/portfile.cmake | 27 +++ cmake/ports/kompute/usage | 17 ++ cmake/ports/kompute/vcpkg.json | 18 ++ examples/cpp/Visualizer/visualizer_rgbd.cpp | 1 - include/depthai/pipeline/node/host/RGBD.hpp | 30 +++- shaders/rgbd2pointcloud.comp | 60 +++++++ src/pipeline/node/host/RGBD.cpp | 182 +++++++++++++++++--- vcpkg.json | 1 + 10 files changed, 330 insertions(+), 29 deletions(-) create mode 100644 cmake/ports/kompute/portfile.cmake create mode 100644 cmake/ports/kompute/usage create mode 100644 cmake/ports/kompute/vcpkg.json create mode 100644 shaders/rgbd2pointcloud.comp diff --git a/CMakeLists.txt b/CMakeLists.txt index ed1372557..2ecb71a9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.16) +cmake_minimum_required(VERSION 3.20) # Early options option(DEPTHAI_ENABLE_LIBUSB "Enable usage of libusb and interaction with USB devices" ON) # Build AprilTag node code (note, that even if set to OFF, an AprilTagNode can be used with setRunOnHost(false)) @@ -277,6 +277,7 @@ include(depthaiDependencies) # Add threads preference set(THREADS_PREFER_PTHREAD_FLAG ON) + # TODO Remove shared naming set(DEPTHAI_SHARED_3RDPARTY_INCLUDE ${CMAKE_CURRENT_LIST_DIR}/include/3rdparty/ @@ -284,6 +285,20 @@ set(DEPTHAI_SHARED_3RDPARTY_INCLUDE # Add depthai-bootloader-shared include(${CMAKE_CURRENT_LIST_DIR}/shared/depthai-bootloader-shared.cmake) +message(STATUS "################################################################################") +message(STATUS "COMPILING SHADERS") +#create shader directory in build +file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/shaders") +vulkan_compile_shader( + INFILE shaders/rgbd2pointcloud.comp + OUTFILE shaders/rgbd2pointcloud.hpp + NAMESPACE "shaders") +add_library(shaders INTERFACE "shaders/rgbd2pointcloud.hpp") + +target_include_directories(shaders INTERFACE $) + +list(APPEND targets_to_export shaders) + # Add flags helpers include(Flags) @@ -504,6 +519,9 @@ set_target_properties(${TARGET_CORE_NAME} PROPERTIES EXPORT_NAME ${TARGET_CORE_A # Add to list of targets to export and install list(APPEND targets_to_export ${TARGET_CORE_NAME}) + # target_include_directories(shader INTERFACE $) + +target_link_libraries(${TARGET_CORE_NAME} PRIVATE shaders kompute::kompute) # Add model_zoo helper binary add_executable(zoo_helper src/modelzoo/zoo_helper.cpp) target_compile_definitions(zoo_helper PRIVATE DEPTHAI_TARGET_CORE) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index eb0b72259..16bc504af 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -38,6 +38,9 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT DEPTHAI_SHARED_LIBS)) find_path(FP16_INCLUDE_DIR NAMES fp16.h) find_package(PNG REQUIRED) + + find_package(kompute ${_QUIET} CONFIG REQUIRED) + # libarchive for firmware packages find_package(LibArchive ${_QUIET} REQUIRED) find_package(liblzma ${_QUIET} CONFIG REQUIRED) diff --git a/cmake/ports/kompute/portfile.cmake b/cmake/ports/kompute/portfile.cmake new file mode 100644 index 000000000..4d5a198eb --- /dev/null +++ b/cmake/ports/kompute/portfile.cmake @@ -0,0 +1,27 @@ +vcpkg_from_github( + OUT_SOURCE_PATH SOURCE_PATH + REPO KomputeProject/kompute + REF 48c127d37750ed7033bf0f74972adad0ce099716 + SHA512 d46984e2f49b6b0c3eb313c03a935e2a1e3d480e40295bb2016f82ea1a23b484323f5bb534bffc309e1b09d219a7da8cf6dcea6e0b5ab8ee9f7aa65b6440b87d + HEAD_REF master +) + +vcpkg_cmake_configure( + SOURCE_PATH ${SOURCE_PATH} + OPTIONS + -DKOMPUTE_OPT_USE_BUILT_IN_VULKAN_HEADER=OFF + -DKOMPUTE_OPT_USE_BUILT_IN_FMT=OFF + -DKOMPUTE_OPT_DISABLE_VULKAN_VERSION_CHECK=ON # Tmp fix for mac + -DKOMPUTE_OPT_INSTALL=ON + -DKOMPUTE_OPT_LOG_LEVEL=Warn +) + +vcpkg_cmake_install() + +vcpkg_cmake_config_fixup(PACKAGE_NAME "kompute" CONFIG_PATH "lib/cmake/kompute") + +file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug/include") + +file(INSTALL "${SOURCE_PATH}/LICENSE" DESTINATION "${CURRENT_PACKAGES_DIR}/share/${PORT}" RENAME copyright) + +configure_file("${CMAKE_CURRENT_LIST_DIR}/usage" "${CURRENT_PACKAGES_DIR}/share/${PORT}/usage" COPYONLY) diff --git a/cmake/ports/kompute/usage b/cmake/ports/kompute/usage new file mode 100644 index 000000000..6347e5208 --- /dev/null +++ b/cmake/ports/kompute/usage @@ -0,0 +1,17 @@ +The package kompute provides CMake targets: + + find_package(kompute CONFIG REQUIRED) + + # Compiling shader + # To add more shaders simply copy the vulkan_compile_shader command and replace it with your new shader + vulkan_compile_shader( + INFILE shader/my_shader.comp + OUTFILE shader/my_shader.hpp + NAMESPACE "shader") + + # Then add it to the library, so you can access it later in your code + add_library(shader INTERFACE "shader/my_shader.hpp") + target_include_directories(shader INTERFACE $) + + target_link_libraries(main PRIVATE shader kompute::kompute) + diff --git a/cmake/ports/kompute/vcpkg.json b/cmake/ports/kompute/vcpkg.json new file mode 100644 index 000000000..49ae5a92c --- /dev/null +++ b/cmake/ports/kompute/vcpkg.json @@ -0,0 +1,18 @@ +{ + "name": "kompute", + "version": "0.9.0", + "homepage": "https://github.com/KomputeProject/kompute", + "description": "General purpose GPU compute framework built on Vulkan to support 1000s of cross vendor graphics cards (AMD, Qualcomm, NVIDIA & friends).", + "license": "Apache-2.0", + "dependencies": [ + { + "name": "vcpkg-cmake", + "host": true + }, + { + "name": "vcpkg-cmake-config", + "host": true + }, + "vulkan" + ] +} diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/Visualizer/visualizer_rgbd.cpp index 0f52b9bdd..66e2222f7 100644 --- a/examples/cpp/Visualizer/visualizer_rgbd.cpp +++ b/examples/cpp/Visualizer/visualizer_rgbd.cpp @@ -49,7 +49,6 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); auto *out = color->requestOutput(std::pair(1280, 720)); left->out.link(stereo->left); diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 57c30b7de..c8a02cdf2 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -1,9 +1,10 @@ #pragma once #include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" -#include "depthai/pipeline/node/Sync.hpp" -#include "depthai/pipeline/node/ImageAlign.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" +#include "depthai/pipeline/node/ImageAlign.hpp" +#include "depthai/pipeline/node/Sync.hpp" +#include "kompute/Kompute.hpp" namespace dai { namespace node { @@ -38,7 +39,24 @@ class RGBD : public NodeCRTP { void setOutputMeters(bool outputMeters) { this->outputMeters = outputMeters; } -private: + void useCPU() { + computeMethod = ComputeMethod::CPU; + } + void useCpuMt() { + computeMethod = ComputeMethod::CPU_MT; + } + void useGPU() { + computeMethod = ComputeMethod::GPU; + } + void setGPUDevice(uint32_t deviceIndex) { + this->deviceIndex = deviceIndex; + } + + private: + void computePointCloudGPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& xyzOut, std::vector& rgbOut); + void computePointCloudCPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points); + void computePointCloudCPUMT(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points); + enum class ComputeMethod { CPU, CPU_MT, GPU }; std::string colorInputName = "inColorSync"; std::string depthInputName = "inDepthSync"; Input& inColorSync = inputs[colorInputName]; @@ -50,9 +68,11 @@ class RGBD : public NodeCRTP { Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; bool initialized = false; float fx, fy, cx, cy; - bool outputMeters=false; + bool outputMeters = false; + std::shared_ptr mgr; + ComputeMethod computeMethod = ComputeMethod::CPU; + uint32_t deviceIndex = 0; }; } // namespace node } // namespace dai - diff --git a/shaders/rgbd2pointcloud.comp b/shaders/rgbd2pointcloud.comp new file mode 100644 index 000000000..ab7b3da76 --- /dev/null +++ b/shaders/rgbd2pointcloud.comp @@ -0,0 +1,60 @@ +#version 450 core +layout(local_size_x = 256, local_size_y = 1, local_size_z = 1) in; + +// Binding 0: Depth input (float array) +layout(std430, binding = 0) buffer DepthBuffer { + float depth[]; +}; + +// Binding 1: RGB input (float array, assume each pixel has 3 floats: R,G,B) +layout(std430, binding = 1) buffer RGBBuffer { + float rgb[]; +}; + +// Binding 2: Intrinsics buffer [fx, fy, cx, cy, scale, width, height] +layout(std430, binding = 2) buffer IntrinsicsBuffer { + float intrinsics[]; +}; + +// Binding 3: Output XYZ buffer (float array, 3 floats per pixel) +layout(std430, binding = 3) buffer XYZBuffer { + float xyz[]; +}; + +// Binding 4: Output RGB buffer (float array, 3 floats per pixel) +layout(std430, binding = 4) buffer OutRGBBuffer { + float outRgb[]; +}; + +void main() { + uint i = gl_GlobalInvocationID.x; + + float fx = intrinsics[0]; + float fy = intrinsics[1]; + float cx = intrinsics[2]; + float cy = intrinsics[3]; + float scale = intrinsics[4]; + float width = intrinsics[5]; + float height = intrinsics[6]; + + if (i >= uint(width * height)) { + return; + } + + uint u = i % uint(width); + uint v = i / uint(width); + + float z = depth[i] * scale; + float x = (float(u) - cx) * z / fx; + float y = (float(v) - cy) * z / fy; + + xyz[i * 3 + 0] = x; + xyz[i * 3 + 1] = y; + xyz[i * 3 + 2] = z; + + // Copy RGB directly + outRgb[i * 3 + 0] = rgb[i * 3 + 0]; + outRgb[i * 3 + 1] = rgb[i * 3 + 1]; + outRgb[i * 3 + 2] = rgb[i * 3 + 2]; +} + diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 869786aea..141554e16 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -10,6 +10,11 @@ #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai/pipeline/node/Sync.hpp" +#include "kompute/Kompute.hpp" +#include "shaders/rgbd2pointcloud.hpp" + +#include + namespace dai { namespace node { @@ -25,6 +30,7 @@ std::shared_ptr RGBD::build() { inSync.setBlocking(false); inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); + mgr = std::make_shared(deviceIndex); return std::static_pointer_cast(shared_from_this()); } @@ -43,7 +49,7 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { void RGBD::initialize(std::vector> frames) { // Initialize the camera intrinsics - auto frame = frames.at(0); + auto frame = frames.at(1); auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); auto camID = static_cast(frame->getInstanceNum()); auto intrinsics = calibHandler.getCameraIntrinsics(camID, frame->getWidth(), frame->getHeight()); @@ -54,6 +60,142 @@ void RGBD::initialize(std::vector> frames) { initialized = true; } +// GPU-based function to compute the point cloud using vulkan-kompute +void RGBD::computePointCloudGPU( + const cv::Mat& depthMat, + const cv::Mat& colorMat, + std::vector& xyzOut, + std::vector& rgbOut +) { + using namespace kp; + + int width = depthMat.cols; + int height = depthMat.rows; + size_t size = width * height; + + xyzOut.resize(size * 3); + rgbOut.resize(size * 3); + + // Convert depth to float + // Depth is in mm by default, convert to meters if outputMeters == true + // If outputMeters is true, scale = 1/1000.0 else scale = 1.0 + float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; + + std::vector depthData(size); + for(size_t i = 0; i < size; i++) { + uint16_t d = depthMat.at(i / width, i % width); + depthData[i] = (float)d; // will multiply by scale in shader + } + + // Convert color to float (R,G,B) + std::vector colorDataFloat(size * 3); + for(size_t i = 0; i < size; i++) { + auto c = colorMat.at(i / width, i % width); + // OpenCV: c = [B, G, R] + colorDataFloat[i * 3 + 0] = (float)c[2]; // R + colorDataFloat[i * 3 + 1] = (float)c[1]; // G + colorDataFloat[i * 3 + 2] = (float)c[0]; // B + } + + // Intrinsics: [fx, fy, cx, cy, scale, width, height] + std::vector intrinsics = {fx, fy, cx, cy, scale, (float)width, (float)height}; + + + // Create Kompute tensors + auto depthTensor = mgr->tensor(depthData); + auto rgbTensor = mgr->tensor(colorDataFloat); + auto intrinsicsTensor = mgr->tensor(intrinsics); + auto xyzTensor = mgr->tensor(xyzOut); + // We'll store output RGB as float as well, then convert back + auto outRgbTensor = mgr->tensorT(std::vector(rgbOut.size())); + + // Load shader + const std::vector shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); + const std::vector> tensors = {depthTensor, rgbTensor, intrinsicsTensor, xyzTensor, outRgbTensor}; + auto algo = mgr->algorithm(tensors, shader); + mgr->sequence()->record(tensors)->record(algo)->record(tensors)->eval(); + // Retrieve results + xyzOut = xyzTensor->vector(); + std::vector outRgbFloat = outRgbTensor->vector(); + for(size_t i = 0; i < rgbOut.size(); i++) { + rgbOut[i] = static_cast(std::round(outRgbFloat[i])); + } +} + + void RGBD::computePointCloudCPU( + const cv::Mat& depthMat, + const cv::Mat& colorMat, + std::vector& points +) { + int width = depthMat.cols; + int height = depthMat.rows; + points.resize(width * height); + + for (int i = 0; i < height; i++) { + for (int j = 0; j < width; j++) { + uint16_t depthValue = depthMat.at(i, j); + float z = depthValue ; + float x = (j - cx) * z / fx; + float y = (i - cy) * z / fy; + + auto color = colorMat.at(i, j); + points[i * width + j] = Point3fRGB{ + x, y, z, + static_cast(color[2]), + static_cast(color[1]), + static_cast(color[0]) + }; + } + } +} + +void RGBD::computePointCloudCPUMT( + const cv::Mat& depthMat, + const cv::Mat& colorMat, + std::vector& points +) { + + int width = depthMat.cols; + int height = depthMat.rows; + points.resize(width * height); + // Lambda function for processing a block of rows + auto processRows = [&](int startRow, int endRow) { + for (int i = startRow; i < endRow; i++) { + for (int j = 0; j < width; j++) { + uint16_t depthValue = depthMat.at(i, j); + float z = depthValue ; + float x = (j - cx) * z / fx; + float y = (i - cy) * z / fy; + + auto color = colorMat.at(i, j); + points[i * width + j] = Point3fRGB{ + x, y, z, + static_cast(color[2]), + static_cast(color[1]), + static_cast(color[0]) + }; + } + } + }; + + // Divide rows into chunks and process in parallel + const int numThreads = std::thread::hardware_concurrency(); + const int rowsPerThread = height / numThreads; + std::vector> futures; + + for (int t = 0; t < numThreads; ++t) { + int startRow = t * rowsPerThread; + int endRow = (t == numThreads - 1) ? height : startRow + rowsPerThread; + + futures.emplace_back(std::async(std::launch::async, processRows, startRow, endRow)); + } + + // Wait for all threads to finish + for (auto& future : futures) { + future.get(); + } + +} void RGBD::run() { while(isRunning()) { // Get the color and depth frames @@ -80,30 +222,26 @@ void RGBD::run() { uint height = colorFrame->getHeight(); pc->setSize(width, height); - std::vector points; - const auto& size = width * height; - points.reserve(size); + std::vector points(width * height); // Fill the point cloud auto depthData = depthFrame->getCvFrame(); auto colorData = colorFrame->getCvFrame(); - for(unsigned int i = 0; i < height; i++) { - for(unsigned int j = 0; j < width; j++) { - // Get the depth value as UINT16 - uint16_t depthValue = depthData.at(i, j); - // Convert the depth value to meters - float z = depthValue; - if(outputMeters) { - z /= 1000.0f; - } - // Calculate the 3D point - float x = (j - cy) * z / fy; - float y = (i - cx) * z / fx; - - // Get the color - auto color = colorData.at(i, j); - // Add the point to the point cloud - points.emplace_back(Point3fRGB{x, y, z, static_cast(color[2]), static_cast(color[1]), static_cast(color[0])}); - } + // Use GPU to compute point cloud + std::vector xyzBuffer; + std::vector rgbBuffer; + computePointCloudGPU(depthData, colorData, xyzBuffer, rgbBuffer); + + // Convert xyzBuffer & rgbBuffer to Point3fRGB vector + points.reserve(width * height); + for(size_t i = 0; i < width*height; i++) { + Point3fRGB p; + p.x = xyzBuffer[i*3+0]; + p.y = xyzBuffer[i*3+1]; + p.z = xyzBuffer[i*3+2]; + p.r = rgbBuffer[i*3+0]; + p.g = rgbBuffer[i*3+1]; + p.b = rgbBuffer[i*3+2]; + points.push_back(p); } pc->setPointsRGB(points); pcl.send(pc); diff --git a/vcpkg.json b/vcpkg.json index ffd192ce3..9520b2fbc 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -2,6 +2,7 @@ "name": "depthai", "dependencies": [ "cpp-httplib", + "kompute", "libpng", "tiff", "yaml-cpp", From 9da491624326a95eb78ca009ba681f33f0253570 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 12 Dec 2024 19:42:34 +0100 Subject: [PATCH 07/51] update shaders compilation --- CMakeLists.txt | 25 ++++---- cmake/ports/kompute/vcpkg.json | 3 +- shaders/CMakeLists.txt | 19 ++++++ shaders/bin2h.cmake | 107 +++++++++++++++++++++++++++++++ shaders/bin_file_to_header.cmake | 20 ++++++ shaders/compileShader.cmake | 55 ++++++++++++++++ src/pipeline/node/host/RGBD.cpp | 14 ++-- vcpkg.json | 6 ++ 8 files changed, 230 insertions(+), 19 deletions(-) create mode 100644 shaders/CMakeLists.txt create mode 100644 shaders/bin2h.cmake create mode 100644 shaders/bin_file_to_header.cmake create mode 100644 shaders/compileShader.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 2ecb71a9f..d02ce3e88 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ option(DEPTHAI_ENABLE_PROTOBUF "Enable Protobuf support" ON) option(DEPTHAI_BUILD_PYTHON "Build python bindings" OFF) option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) +OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" ON) option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BOOTSTRAP_VCPKG "Automatically bootstrap VCPKG" ON) option(DEPTHAI_MERGED_TARGET "Enable merged target build" ON) @@ -52,6 +53,9 @@ endif() if(DEPTHAI_PCL_SUPPORT) list(APPEND VCPKG_MANIFEST_FEATURES "pcl-support") endif() +if(DEPTHAI_ENABLE_KOMPUTE) +list(APPEND VCPKG_MANIFEST_FEATURES "kompute-support") +endif() # Enable backward stack printing on crash if(ANDROID OR EMSCRIPTEN) @@ -285,19 +289,11 @@ set(DEPTHAI_SHARED_3RDPARTY_INCLUDE # Add depthai-bootloader-shared include(${CMAKE_CURRENT_LIST_DIR}/shared/depthai-bootloader-shared.cmake) -message(STATUS "################################################################################") -message(STATUS "COMPILING SHADERS") -#create shader directory in build -file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/shaders") -vulkan_compile_shader( - INFILE shaders/rgbd2pointcloud.comp - OUTFILE shaders/rgbd2pointcloud.hpp - NAMESPACE "shaders") -add_library(shaders INTERFACE "shaders/rgbd2pointcloud.hpp") - -target_include_directories(shaders INTERFACE $) +if(DEPTHAI_ENABLE_KOMPUTE) + add_subdirectory(shaders) -list(APPEND targets_to_export shaders) + list(APPEND targets_to_export shaders) +endif() # Add flags helpers include(Flags) @@ -521,7 +517,10 @@ list(APPEND targets_to_export ${TARGET_CORE_NAME}) # target_include_directories(shader INTERFACE $) -target_link_libraries(${TARGET_CORE_NAME} PRIVATE shaders kompute::kompute) +if(DEPTHAI_ENABLE_KOMPUTE) + target_link_libraries(${TARGET_CORE_NAME} PRIVATE shaders kompute::kompute) + target_compile_definitions(${TARGET_CORE_NAME} PRIVATE DEPTHAI_ENABLE_KOMPUTE) +endif() # Add model_zoo helper binary add_executable(zoo_helper src/modelzoo/zoo_helper.cpp) target_compile_definitions(zoo_helper PRIVATE DEPTHAI_TARGET_CORE) diff --git a/cmake/ports/kompute/vcpkg.json b/cmake/ports/kompute/vcpkg.json index 49ae5a92c..58f91ff5d 100644 --- a/cmake/ports/kompute/vcpkg.json +++ b/cmake/ports/kompute/vcpkg.json @@ -13,6 +13,7 @@ "name": "vcpkg-cmake-config", "host": true }, - "vulkan" + "vulkan", + "fmt" ] } diff --git a/shaders/CMakeLists.txt b/shaders/CMakeLists.txt new file mode 100644 index 000000000..eb32d6dc5 --- /dev/null +++ b/shaders/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.20) + +include(compileShader.cmake) + +# create shader directory in build +file(MAKE_DIRECTORY "${CMAKE_BINARY_DIR}/shaders") +set(SHADER_HEADER_PATH "${CMAKE_BINARY_DIR}/include/depthai/shaders") +# vulkan_compile_shader generates headers in source directory +compile_shader( + INFILE rgbd2pointcloud.comp OUTFILE + ${SHADER_HEADER_PATH}/rgbd2pointcloud.hpp NAMESPACE + "dai::shaders") +add_library(shaders INTERFACE + ${SHADER_HEADER_PATH}/rgbd2pointcloud.hpp) + +target_include_directories( + shaders INTERFACE $ + $) + diff --git a/shaders/bin2h.cmake b/shaders/bin2h.cmake new file mode 100644 index 000000000..82824622c --- /dev/null +++ b/shaders/bin2h.cmake @@ -0,0 +1,107 @@ + +################################################################################## +# Based on: https://github.com/sivachandran/cmake-bin2h +# +# Copyright 2020 Sivachandran Paramasivam +# +# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +################################################################################## + +include(CMakeParseArguments) + +# Function to wrap a given string into multiple lines at the given column position. +# Parameters: +# VARIABLE - The name of the CMake variable holding the string. +# AT_COLUMN - The column position at which string will be wrapped. +function(WRAP_STRING) + set(oneValueArgs VARIABLE AT_COLUMN) + cmake_parse_arguments(WRAP_STRING "${options}" "${oneValueArgs}" "" ${ARGN}) + + string(LENGTH ${${WRAP_STRING_VARIABLE}} stringLength) + math(EXPR offset "0") + + while(stringLength GREATER 0) + + if(stringLength GREATER ${WRAP_STRING_AT_COLUMN}) + math(EXPR length "${WRAP_STRING_AT_COLUMN}") + else() + math(EXPR length "${stringLength}") + endif() + + string(SUBSTRING ${${WRAP_STRING_VARIABLE}} ${offset} ${length} line) + set(lines "${lines}\n${line}") + + math(EXPR stringLength "${stringLength} - ${length}") + math(EXPR offset "${offset} + ${length}") + endwhile() + + set(${WRAP_STRING_VARIABLE} "${lines}" PARENT_SCOPE) +endfunction() + +# Function to embed contents of a file as byte array in C/C++ header file(.h). The header file +# will contain a byte array and integer variable holding the size of the array. +# Parameters +# SOURCE_FILE - The path of source file whose contents will be embedded in the header file. +# VARIABLE_NAME - The name of the variable for the byte array. The string "_SIZE" will be append +# to this name and will be used a variable name for size variable. +# HEADER_FILE - The path of header file. +# APPEND - If specified appends to the header file instead of overwriting it +# NULL_TERMINATE - If specified a null byte(zero) will be append to the byte array. This will be +# useful if the source file is a text file and we want to use the file contents +# as string. But the size variable holds size of the byte array without this +# null byte. +# HEADER_NAMESPACE - The namespace, where the array should be located in. +# IS_BIG_ENDIAN - If set to true, will not revers the byte order for the uint32_t to match the +# big endian system architecture +# Usage: +# bin2h(SOURCE_FILE "Logo.png" HEADER_FILE "Logo.h" VARIABLE_NAME "LOGO_PNG") +function(BIN2H) + set(options APPEND NULL_TERMINATE) + set(oneValueArgs SOURCE_FILE VARIABLE_NAME HEADER_FILE) + cmake_parse_arguments(BIN2H "${options}" "${oneValueArgs}" "" ${ARGN}) + + # reads source file contents as hex string + file(READ ${BIN2H_SOURCE_FILE} hexString HEX) + string(LENGTH ${hexString} hexStringLength) + + # appends null byte if asked + if(BIN2H_NULL_TERMINATE) + set(hexString "${hexString}00") + endif() + + # wraps the hex string into multiple lines at column 32(i.e. 16 bytes per line) + wrap_string(VARIABLE hexString AT_COLUMN 32) + math(EXPR arraySize "${hexStringLength} / 8") + + # adds '0x' prefix and comma suffix before and after every byte respectively + if(IS_BIG_ENDIAN) + message(STATUS "Interpreting shader in big endian...") + string(REGEX REPLACE "([0-9a-f][0-9a-f])([0-9a-f][0-9a-f])([0-9a-f][0-9a-f])([0-9a-f][0-9a-f])" "0x\\1\\2\\3\\4, " arrayValues ${hexString}) + else() + message(STATUS "Interpreting shader in little endian...") + string(REGEX REPLACE "([0-9a-f][0-9a-f])([0-9a-f][0-9a-f])([0-9a-f][0-9a-f])([0-9a-f][0-9a-f])" "0x\\4\\3\\2\\1, " arrayValues ${hexString}) + endif() + # removes trailing comma + string(REGEX REPLACE ", $" "" arrayValues ${arrayValues}) + + # converts the variable name into proper C identifier + string(MAKE_C_IDENTIFIER "${BIN2H_VARIABLE_NAME}" BIN2H_VARIABLE_NAME) + string(TOUPPER "${BIN2H_VARIABLE_NAME}" BIN2H_VARIABLE_NAME) + + # declares byte array and the length variables + set(namespaceStart "namespace ${HEADER_NAMESPACE} {") + set(namespaceEnd "} // namespace ${HEADER_NAMESPACE}") + set(arrayIncludes "#pragma once\n#include \n#include ") + set(arrayDefinition "const std::array ${BIN2H_VARIABLE_NAME} = { ${arrayValues} };") + + set(declarations "${arrayIncludes}\n\n${namespaceStart}\n${arrayDefinition}\n${namespaceEnd}\n\n") + if(BIN2H_APPEND) + file(APPEND ${BIN2H_HEADER_FILE} "${declarations}") + else() + file(WRITE ${BIN2H_HEADER_FILE} "${declarations}") + endif() +endfunction() diff --git a/shaders/bin_file_to_header.cmake b/shaders/bin_file_to_header.cmake new file mode 100644 index 000000000..3df6bfcaa --- /dev/null +++ b/shaders/bin_file_to_header.cmake @@ -0,0 +1,20 @@ + +cmake_minimum_required(VERSION 3.20) + +if(${INPUT_SHADER_FILE} STREQUAL "") + message(FATAL_ERROR "No input file path provided via 'INPUT_SHADER_FILE'.") +endif() + +if(${OUTPUT_HEADER_FILE} STREQUAL "") + message(FATAL_ERROR "No output file path provided via 'OUTPUT_HEADER_FILE'.") +endif() + +if(${HEADER_NAMESPACE} STREQUAL "") + message(FATAL_ERROR "No header namespace provided via 'HEADER_NAMESPACE'.") +endif() + +include(bin2h.cmake) + +get_filename_component(BINARY_FILE_CONTENT ${INPUT_SHADER_FILE} NAME) +bin2h(SOURCE_FILE ${INPUT_SHADER_FILE} HEADER_FILE ${OUTPUT_HEADER_FILE} VARIABLE_NAME ${BINARY_FILE_CONTENT} HEADER_NAMESPACE ${HEADER_NAMESPACE}) +file(APPEND ${OUTPUT_HEADER_FILE} "\n") diff --git a/shaders/compileShader.cmake b/shaders/compileShader.cmake new file mode 100644 index 000000000..5463cf299 --- /dev/null +++ b/shaders/compileShader.cmake @@ -0,0 +1,55 @@ + +# Setting CWD as later on it's not possible if called from outside +set_property(GLOBAL PROPERTY + VULKAN_COMPILE_SHADER_CWD_PROPERTY "${CMAKE_CURRENT_LIST_DIR}") + +function(compile_shader) + find_program(GLS_LANG_VALIDATOR_PATH NAMES glslangValidator) + if(GLS_LANG_VALIDATOR_PATH STREQUAL "GLS_LANG_VALIDATOR_PATH-NOTFOUND") + message(FATAL_ERROR "glslangValidator not found.") + return() + endif() + + cmake_parse_arguments(SHADER_COMPILE "" "INFILE;OUTFILE;NAMESPACE" "" ${ARGN}) + set(SHADER_COMPILE_INFILE_FULL "${CMAKE_CURRENT_SOURCE_DIR}/${SHADER_COMPILE_INFILE}") + set(SHADER_COMPILE_SPV_FILE_FULL "${CMAKE_CURRENT_BINARY_DIR}/${SHADER_COMPILE_INFILE}.spv") + set(SHADER_COMPILE_HEADER_FILE_FULL "${SHADER_COMPILE_OUTFILE}") + + # .comp -> .spv + add_custom_command(OUTPUT "${SHADER_COMPILE_SPV_FILE_FULL}" + COMMAND "${GLS_LANG_VALIDATOR_PATH}" + ARGS "-V" + "${SHADER_COMPILE_INFILE_FULL}" + "-o" + "${SHADER_COMPILE_SPV_FILE_FULL}" + COMMENT "Compile vulkan compute shader from file '${SHADER_COMPILE_INFILE_FULL}' to '${SHADER_COMPILE_SPV_FILE_FULL}'." + MAIN_DEPENDENCY "${SHADER_COMPILE_INFILE_FULL}") + + # Check if big or little endian + include (TestBigEndian) + TEST_BIG_ENDIAN(IS_BIG_ENDIAN) + + # .spv -> .hpp + ## Property needs to be retrieved explicitly from globals + get_property(VULKAN_COMPILE_SHADER_CWD GLOBAL PROPERTY VULKAN_COMPILE_SHADER_CWD_PROPERTY) + ## The directory may not be created so we need to ensure its present + get_filename_component(SHADER_COMPILE_SPV_PATH ${SHADER_COMPILE_SPV_FILE_FULL} DIRECTORY) + if(NOT EXISTS ${SHADER_COMPILE_SPV_PATH}) + get_filename_component(SHADER_COMPILE_SPV_FILENAME ${SHADER_COMPILE_SPV_FILE_FULL} NAME) + add_custom_target(build-time-make-directory-${SHADER_COMPILE_SPV_FILENAME} ALL + COMMAND ${CMAKE_COMMAND} -E make_directory ${SHADER_COMPILE_SPV_PATH}) + endif() + ## Requires custom command function as this is the only way to call + ## a function during compile time from cmake (ie through cmake script) + add_custom_command(OUTPUT "${SHADER_COMPILE_HEADER_FILE_FULL}" + COMMAND ${CMAKE_COMMAND} + ARGS "-DINPUT_SHADER_FILE=${SHADER_COMPILE_SPV_FILE_FULL}" + "-DOUTPUT_HEADER_FILE=${SHADER_COMPILE_HEADER_FILE_FULL}" + "-DHEADER_NAMESPACE=${SHADER_COMPILE_NAMESPACE}" + "-DIS_BIG_ENDIAN=${IS_BIG_ENDIAN}" + "-P" + "${VULKAN_COMPILE_SHADER_CWD}/bin_file_to_header.cmake" + WORKING_DIRECTORY "${VULKAN_COMPILE_SHADER_CWD}" + COMMENT "Converting compiled shader '${SHADER_COMPILE_SPV_FILE_FULL}' to header file '${SHADER_COMPILE_HEADER_FILE_FULL}'." + MAIN_DEPENDENCY "${SHADER_COMPILE_SPV_FILE_FULL}") +endfunction() diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 141554e16..6bd9ba878 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -11,7 +11,7 @@ #include "depthai/pipeline/node/Sync.hpp" #include "kompute/Kompute.hpp" -#include "shaders/rgbd2pointcloud.hpp" +#include "depthai/shaders/rgbd2pointcloud.hpp" #include @@ -30,7 +30,12 @@ std::shared_ptr RGBD::build() { inSync.setBlocking(false); inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); - mgr = std::make_shared(deviceIndex); + mgr = std::make_shared(1); + auto devices = mgr->listDevices(); + for(auto& dev : devices) { + std::cout << "Device ID: " << dev.getProperties().deviceID << std::endl; + std::cout << "Device Name: " << dev.getProperties().deviceName << std::endl; + } return std::static_pointer_cast(shared_from_this()); } @@ -108,7 +113,6 @@ void RGBD::computePointCloudGPU( auto xyzTensor = mgr->tensor(xyzOut); // We'll store output RGB as float as well, then convert back auto outRgbTensor = mgr->tensorT(std::vector(rgbOut.size())); - // Load shader const std::vector shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); const std::vector> tensors = {depthTensor, rgbTensor, intrinsicsTensor, xyzTensor, outRgbTensor}; @@ -222,7 +226,7 @@ void RGBD::run() { uint height = colorFrame->getHeight(); pc->setSize(width, height); - std::vector points(width * height); + std::vector points; // Fill the point cloud auto depthData = depthFrame->getCvFrame(); auto colorData = colorFrame->getCvFrame(); @@ -241,7 +245,7 @@ void RGBD::run() { p.r = rgbBuffer[i*3+0]; p.g = rgbBuffer[i*3+1]; p.b = rgbBuffer[i*3+2]; - points.push_back(p); + points.emplace_back(p); } pc->setPointsRGB(points); pcl.send(pc); diff --git a/vcpkg.json b/vcpkg.json index 9520b2fbc..0363c1bdf 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -71,6 +71,12 @@ "cpr" ] }, + "kompute-support": { + "description": "Enable Kompute support", + "dependencies": [ + "kompute" + ] + }, "protobuf-support": { "description": "Enable Protobuf support", "dependencies": [ From 97fe9fb41d6c8abcfd339b9c8dc3b54c99cbd0e1 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 13 Dec 2024 09:15:02 +0100 Subject: [PATCH 08/51] add glsllang-tools to ci --- .github/workflows/python-main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 4602c8302..d8d565e51 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -52,7 +52,7 @@ jobs: run: | sudo apt update python -m pip install --upgrade pip - sudo apt install libusb-1.0-0-dev pkg-config bison autoconf libtool libxi-dev libxtst-dev libxrandr-dev libx11-dev libxft-dev libxext-dev nasm flex libudev-dev + sudo apt install libusb-1.0-0-dev pkg-config bison autoconf libtool libxi-dev libxtst-dev libxrandr-dev libx11-dev libxft-dev libxext-dev nasm flex libudev-dev glslang-tools python -m pip install -r bindings/python/docs/requirements_mkdoc.txt - name: Configure project run: cmake -S . -B build -DDEPTHAI_BUILD_PYTHON=ON -DDEPTHAI_PYTHON_FORCE_DOCSTRINGS=ON -DDEPTHAI_BASALT_SUPPORT=ON -DDEPTHAI_PCL_SUPPORT=ON -DDEPTHAI_RTABMAP_SUPPORT=ON -DDEPTHAI_PYTHON_DOCSTRINGS_OUTPUT="$PWD/bindings/python/docstrings/depthai_python_docstring.hpp" From d7be197763934aab4963bd523d2b2db32200f426 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 13 Dec 2024 09:55:11 +0100 Subject: [PATCH 09/51] pimpl in rgbd --- include/depthai/pipeline/node/host/RGBD.hpp | 36 +- src/pipeline/node/host/RGBD.cpp | 371 +++++++++++--------- 2 files changed, 216 insertions(+), 191 deletions(-) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index c8a02cdf2..f0101fe33 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -4,7 +4,7 @@ #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/node/ImageAlign.hpp" #include "depthai/pipeline/node/Sync.hpp" -#include "kompute/Kompute.hpp" +#include "depthai/utility/Pimpl.hpp" namespace dai { namespace node { @@ -16,6 +16,7 @@ class RGBD : public NodeCRTP { public: constexpr static const char* NAME = "RGBD"; + RGBD(); Subnode sync{*this, "sync"}; std::shared_ptr align; InputMap& inputs = sync->inputs; @@ -36,27 +37,15 @@ class RGBD : public NodeCRTP { std::shared_ptr build(); std::shared_ptr build(bool autocreate, std::pair size); - void setOutputMeters(bool outputMeters) { - this->outputMeters = outputMeters; - } - void useCPU() { - computeMethod = ComputeMethod::CPU; - } - void useCpuMt() { - computeMethod = ComputeMethod::CPU_MT; - } - void useGPU() { - computeMethod = ComputeMethod::GPU; - } - void setGPUDevice(uint32_t deviceIndex) { - this->deviceIndex = deviceIndex; - } - + void setOutputMeters(bool outputMeters); + void useCPU(); + void useCpuMt(); + void useGPU(); + void setGPUDevice(uint32_t deviceIndex); + void printDevices(); private: - void computePointCloudGPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& xyzOut, std::vector& rgbOut); - void computePointCloudCPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points); - void computePointCloudCPUMT(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points); - enum class ComputeMethod { CPU, CPU_MT, GPU }; + class Impl; + Pimpl pimpl; std::string colorInputName = "inColorSync"; std::string depthInputName = "inDepthSync"; Input& inColorSync = inputs[colorInputName]; @@ -67,11 +56,6 @@ class RGBD : public NodeCRTP { void initialize(std::vector> frames); Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; bool initialized = false; - float fx, fy, cx, cy; - bool outputMeters = false; - std::shared_ptr mgr; - ComputeMethod computeMethod = ComputeMethod::CPU; - uint32_t deviceIndex = 0; }; } // namespace node diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 6bd9ba878..ccaa70e1f 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -1,5 +1,8 @@ #include "depthai/pipeline/node/host/RGBD.hpp" +#include + +#include "utility/PimplImpl.hpp" #include "common/Point3fRGB.hpp" #include "depthai/common/Point3fRGB.hpp" #include "depthai/pipeline/Pipeline.hpp" @@ -9,175 +12,127 @@ #include "depthai/pipeline/node/Camera.hpp" #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai/pipeline/node/Sync.hpp" - -#include "kompute/Kompute.hpp" #include "depthai/shaders/rgbd2pointcloud.hpp" - -#include +#include "kompute/Kompute.hpp" namespace dai { namespace node { -std::shared_ptr RGBD::build() { - align = getParentPipeline().create(); - align->outputAligned.link(inDepthSync); - colorMux.link(inColorSync); - colorMux.link(align->inputAlignTo); - depthPT.link(align->input); - sync->setRunOnHost(false); - sync->out.link(inSync); - inSync.setMaxSize(1); - inSync.setBlocking(false); - inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); - inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); - mgr = std::make_shared(1); - auto devices = mgr->listDevices(); - for(auto& dev : devices) { - std::cout << "Device ID: " << dev.getProperties().deviceID << std::endl; - std::cout << "Device Name: " << dev.getProperties().deviceName << std::endl; +class RGBD::Impl { + public: + Impl() = default; + void initializeGPU() { + // Initialize Kompute + mgr = std::make_shared(deviceIndex); } - return std::static_pointer_cast(shared_from_this()); -} - -std::shared_ptr RGBD::build(bool autocreate, std::pair size) { - if(!autocreate) { - return std::static_pointer_cast(shared_from_this()); + void computePointCloud(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { + switch(computeMethod) { + case ComputeMethod::CPU: + computePointCloudCPU(depthMat, colorMat, points); + break; + case ComputeMethod::CPU_MT: + computePointCloudCPUMT(depthMat, colorMat, points); + break; + case ComputeMethod::GPU: + computePointCloudGPU(depthMat, colorMat, points); + break; + } } - auto pipeline = getParentPipeline(); - auto colorCam = pipeline.create()->build(); - auto depth = pipeline.create()->build(true); - auto* out = colorCam->requestOutput(size); - out->link(inColor); - depth->depth.link(inDepth); - return build(); -} + void computePointCloudGPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { + std::vector xyzOut; + std::vector rgbOut; + int width = depthMat.cols; + int height = depthMat.rows; + size_t size = width * height; -void RGBD::initialize(std::vector> frames) { - // Initialize the camera intrinsics - auto frame = frames.at(1); - auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); - auto camID = static_cast(frame->getInstanceNum()); - auto intrinsics = calibHandler.getCameraIntrinsics(camID, frame->getWidth(), frame->getHeight()); - fx = intrinsics[0][0]; - fy = intrinsics[1][1]; - cx = intrinsics[0][2]; - cy = intrinsics[1][2]; - initialized = true; -} + xyzOut.resize(size * 3); + rgbOut.resize(size * 3); -// GPU-based function to compute the point cloud using vulkan-kompute -void RGBD::computePointCloudGPU( - const cv::Mat& depthMat, - const cv::Mat& colorMat, - std::vector& xyzOut, - std::vector& rgbOut -) { - using namespace kp; - - int width = depthMat.cols; - int height = depthMat.rows; - size_t size = width * height; - - xyzOut.resize(size * 3); - rgbOut.resize(size * 3); - - // Convert depth to float - // Depth is in mm by default, convert to meters if outputMeters == true - // If outputMeters is true, scale = 1/1000.0 else scale = 1.0 - float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; - - std::vector depthData(size); - for(size_t i = 0; i < size; i++) { - uint16_t d = depthMat.at(i / width, i % width); - depthData[i] = (float)d; // will multiply by scale in shader - } + // Convert depth to float + // Depth is in mm by default, convert to meters if outputMeters == true + // If outputMeters is true, scale = 1/1000.0 else scale = 1.0 + float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; - // Convert color to float (R,G,B) - std::vector colorDataFloat(size * 3); - for(size_t i = 0; i < size; i++) { - auto c = colorMat.at(i / width, i % width); - // OpenCV: c = [B, G, R] - colorDataFloat[i * 3 + 0] = (float)c[2]; // R - colorDataFloat[i * 3 + 1] = (float)c[1]; // G - colorDataFloat[i * 3 + 2] = (float)c[0]; // B - } + std::vector depthData(size); + for(size_t i = 0; i < size; i++) { + uint16_t d = depthMat.at(i / width, i % width); + depthData[i] = (float)d; // will multiply by scale in shader + } - // Intrinsics: [fx, fy, cx, cy, scale, width, height] - std::vector intrinsics = {fx, fy, cx, cy, scale, (float)width, (float)height}; - - - // Create Kompute tensors - auto depthTensor = mgr->tensor(depthData); - auto rgbTensor = mgr->tensor(colorDataFloat); - auto intrinsicsTensor = mgr->tensor(intrinsics); - auto xyzTensor = mgr->tensor(xyzOut); - // We'll store output RGB as float as well, then convert back - auto outRgbTensor = mgr->tensorT(std::vector(rgbOut.size())); - // Load shader - const std::vector shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); - const std::vector> tensors = {depthTensor, rgbTensor, intrinsicsTensor, xyzTensor, outRgbTensor}; - auto algo = mgr->algorithm(tensors, shader); - mgr->sequence()->record(tensors)->record(algo)->record(tensors)->eval(); - // Retrieve results - xyzOut = xyzTensor->vector(); - std::vector outRgbFloat = outRgbTensor->vector(); - for(size_t i = 0; i < rgbOut.size(); i++) { - rgbOut[i] = static_cast(std::round(outRgbFloat[i])); - } -} + // Convert color to float (R,G,B) + std::vector colorDataFloat(size * 3); + for(size_t i = 0; i < size; i++) { + auto c = colorMat.at(i / width, i % width); + // OpenCV: c = [B, G, R] + colorDataFloat[i * 3 + 0] = (float)c[2]; // R + colorDataFloat[i * 3 + 1] = (float)c[1]; // G + colorDataFloat[i * 3 + 2] = (float)c[0]; // B + } + + // Intrinsics: [fx, fy, cx, cy, scale, width, height] + std::vector intrinsics = {fx, fy, cx, cy, scale, (float)width, (float)height}; - void RGBD::computePointCloudCPU( - const cv::Mat& depthMat, - const cv::Mat& colorMat, - std::vector& points -) { - int width = depthMat.cols; - int height = depthMat.rows; - points.resize(width * height); - - for (int i = 0; i < height; i++) { - for (int j = 0; j < width; j++) { - uint16_t depthValue = depthMat.at(i, j); - float z = depthValue ; - float x = (j - cx) * z / fx; - float y = (i - cy) * z / fy; - - auto color = colorMat.at(i, j); - points[i * width + j] = Point3fRGB{ - x, y, z, - static_cast(color[2]), - static_cast(color[1]), - static_cast(color[0]) - }; + // Create Kompute tensors + auto depthTensor = mgr->tensor(depthData); + auto rgbTensor = mgr->tensor(colorDataFloat); + auto intrinsicsTensor = mgr->tensor(intrinsics); + auto xyzTensor = mgr->tensor(xyzOut); + // We'll store output RGB as float as well, then convert back + auto outRgbTensor = mgr->tensorT(std::vector(rgbOut.size())); + // Load shader + const std::vector shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); + const std::vector> tensors = {depthTensor, rgbTensor, intrinsicsTensor, xyzTensor, outRgbTensor}; + auto algo = mgr->algorithm(tensors, shader); + mgr->sequence()->record(tensors)->record(algo)->record(tensors)->eval(); + // Retrieve results + xyzOut = xyzTensor->vector(); + std::vector outRgbFloat = outRgbTensor->vector(); + for(size_t i = 0; i < rgbOut.size(); i++) { + rgbOut[i] = static_cast(std::round(outRgbFloat[i])); + } + for(int i = 0; i < width * height; i++) { + Point3fRGB p; + p.x = xyzOut[i * 3 + 0]; + p.y = xyzOut[i * 3 + 1]; + p.z = xyzOut[i * 3 + 2]; + p.r = rgbOut[i * 3 + 0]; + p.g = rgbOut[i * 3 + 1]; + p.b = rgbOut[i * 3 + 2]; + points.emplace_back(p); } } -} + void computePointCloudCPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { + int width = depthMat.cols; + int height = depthMat.rows; + points.resize(width * height); -void RGBD::computePointCloudCPUMT( - const cv::Mat& depthMat, - const cv::Mat& colorMat, - std::vector& points -) { + for(int i = 0; i < height; i++) { + for(int j = 0; j < width; j++) { + uint16_t depthValue = depthMat.at(i, j); + float z = depthValue; + float x = (j - cx) * z / fx; + float y = (i - cy) * z / fy; - int width = depthMat.cols; - int height = depthMat.rows; - points.resize(width * height); - // Lambda function for processing a block of rows + auto color = colorMat.at(i, j); + points[i * width + j] = Point3fRGB{x, y, z, static_cast(color[2]), static_cast(color[1]), static_cast(color[0])}; + } + } + } + void computePointCloudCPUMT(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { + int width = depthMat.cols; + int height = depthMat.rows; + points.resize(width * height); + // Lambda function for processing a block of rows auto processRows = [&](int startRow, int endRow) { - for (int i = startRow; i < endRow; i++) { - for (int j = 0; j < width; j++) { + for(int i = startRow; i < endRow; i++) { + for(int j = 0; j < width; j++) { uint16_t depthValue = depthMat.at(i, j); - float z = depthValue ; + float z = depthValue; float x = (j - cx) * z / fx; float y = (i - cy) * z / fy; auto color = colorMat.at(i, j); - points[i * width + j] = Point3fRGB{ - x, y, z, - static_cast(color[2]), - static_cast(color[1]), - static_cast(color[0]) - }; + points[i * width + j] = Point3fRGB{x, y, z, static_cast(color[2]), static_cast(color[1]), static_cast(color[0])}; } } }; @@ -187,7 +142,7 @@ void RGBD::computePointCloudCPUMT( const int rowsPerThread = height / numThreads; std::vector> futures; - for (int t = 0; t < numThreads; ++t) { + for(int t = 0; t < numThreads; ++t) { int startRow = t * rowsPerThread; int endRow = (t == numThreads - 1) ? height : startRow + rowsPerThread; @@ -195,11 +150,90 @@ void RGBD::computePointCloudCPUMT( } // Wait for all threads to finish - for (auto& future : futures) { + for(auto& future : futures) { future.get(); } + } + void setOutputMeters(bool outputMeters) { + this->outputMeters = outputMeters; + } + void printDevices() { + auto devices = mgr->listDevices(); + for(auto& device : devices) { + std::cout << "Device: " << device.getProperties().deviceName << std::endl; + } + } + void setGPUDevice(uint32_t deviceIndex) { + this->deviceIndex = deviceIndex; + mgr = std::make_shared(deviceIndex); + } + void useCPU() { + computeMethod = ComputeMethod::CPU; + } + void useCpuMt() { + computeMethod = ComputeMethod::CPU_MT; + } + void useGPU() { + initializeGPU(); + computeMethod = ComputeMethod::GPU; + } + void setIntrinsics(float fx, float fy, float cx, float cy) { + this->fx = fx; + this->fy = fy; + this->cx = cx; + this->cy = cy; + } + enum class ComputeMethod { CPU, CPU_MT, GPU }; + std::shared_ptr mgr; + ComputeMethod computeMethod = ComputeMethod::CPU; + uint32_t deviceIndex = 0; + bool outputMeters = false; + float fx, fy, cx, cy; +}; + +RGBD::RGBD() = default;; +std::shared_ptr RGBD::build() { + align = getParentPipeline().create(); + align->outputAligned.link(inDepthSync); + colorMux.link(inColorSync); + colorMux.link(align->inputAlignTo); + depthPT.link(align->input); + sync->setRunOnHost(false); + sync->out.link(inSync); + inSync.setMaxSize(1); + inSync.setBlocking(false); + inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); + inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); + return std::static_pointer_cast(shared_from_this()); +} + +std::shared_ptr RGBD::build(bool autocreate, std::pair size) { + if(!autocreate) { + return std::static_pointer_cast(shared_from_this()); + } + auto pipeline = getParentPipeline(); + auto colorCam = pipeline.create()->build(); + auto depth = pipeline.create()->build(true); + auto* out = colorCam->requestOutput(size); + out->link(inColor); + depth->depth.link(inDepth); + return build(); +} +void RGBD::initialize(std::vector> frames) { + // Initialize the camera intrinsics + auto frame = frames.at(1); + auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); + auto camID = static_cast(frame->getInstanceNum()); + auto intrinsics = calibHandler.getCameraIntrinsics(camID, frame->getWidth(), frame->getHeight()); + float fx = intrinsics[0][0]; + float fy = intrinsics[1][1]; + float cx = intrinsics[0][2]; + float cy = intrinsics[1][2]; + pimpl->setIntrinsics(fx, fy, cx, cy); + initialized = true; } + void RGBD::run() { while(isRunning()) { // Get the color and depth frames @@ -222,8 +256,8 @@ void RGBD::run() { pc->setTimestampDevice(colorFrame->getTimestampDevice()); pc->setSequenceNum(colorFrame->getSequenceNum()); pc->setInstanceNum(colorFrame->getInstanceNum()); - uint width = colorFrame->getWidth(); - uint height = colorFrame->getHeight(); + auto width = colorFrame->getWidth(); + auto height = colorFrame->getHeight(); pc->setSize(width, height); std::vector points; @@ -231,25 +265,32 @@ void RGBD::run() { auto depthData = depthFrame->getCvFrame(); auto colorData = colorFrame->getCvFrame(); // Use GPU to compute point cloud - std::vector xyzBuffer; - std::vector rgbBuffer; - computePointCloudGPU(depthData, colorData, xyzBuffer, rgbBuffer); + points.reserve(width * height); + pimpl->computePointCloud(depthData, colorData, points); // Convert xyzBuffer & rgbBuffer to Point3fRGB vector - points.reserve(width * height); - for(size_t i = 0; i < width*height; i++) { - Point3fRGB p; - p.x = xyzBuffer[i*3+0]; - p.y = xyzBuffer[i*3+1]; - p.z = xyzBuffer[i*3+2]; - p.r = rgbBuffer[i*3+0]; - p.g = rgbBuffer[i*3+1]; - p.b = rgbBuffer[i*3+2]; - points.emplace_back(p); - } pc->setPointsRGB(points); pcl.send(pc); } } +void RGBD::setOutputMeters(bool outputMeters) { + pimpl->setOutputMeters(outputMeters); +} +void RGBD::useCPU() { + pimpl->useCPU(); +} +void RGBD::useCpuMt() { + pimpl->useCpuMt(); +} +void RGBD::useGPU() { + pimpl->useGPU(); +} +void RGBD::setGPUDevice(uint32_t deviceIndex) { + pimpl->setGPUDevice(deviceIndex); +} +void RGBD::printDevices() { + pimpl->printDevices(); +} + } // namespace node } // namespace dai From 60bbe9f241412f61522d1e2b4670a07e0b3da1ae Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 13 Dec 2024 17:17:25 +0100 Subject: [PATCH 10/51] update pimpl --- examples/cpp/HostNodes/rgbd.cpp | 10 +- examples/cpp/Visualizer/visualizer_rgbd.cpp | 7 +- include/depthai/pipeline/node/host/RGBD.hpp | 1 + shaders/rgbd2pointcloud.comp | 17 +- src/pipeline/datatype/PointCloudData.cpp | 15 +- src/pipeline/node/host/RGBD.cpp | 192 +++++++++++--------- 6 files changed, 121 insertions(+), 121 deletions(-) diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp index 535f20332..ad64817df 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -17,7 +17,7 @@ class RerunNode : public dai::NodeCRTP { rec.log_static("world", rerun::ViewCoordinates::FLU); rec.log("world/ground", rerun::Boxes3D::from_half_sizes({{3.f, 3.f, 0.00001f}})); while(isRunning()) { - auto pclIn = inputPCL.tryGet(); + auto pclIn = inputPCL.get(); if(pclIn != nullptr) { std::vector points; std::vector colors; @@ -40,8 +40,6 @@ int main() { dai::Pipeline pipeline; // Define sources and outputs int fps = 30; - int width = 640; - int height = 400; // Define sources and outputs auto left = pipeline.create(); auto right = pipeline.create(); @@ -65,9 +63,11 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); + rgbd->setOutputMeters(true); + rgbd->setGPUDevice(1); + rgbd->useGPU(); - auto *out = color->requestOutput(std::pair(1280, 720)); + auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); left->out.link(stereo->left); right->out.link(stereo->right); diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/Visualizer/visualizer_rgbd.cpp index 66e2222f7..640fb0330 100644 --- a/examples/cpp/Visualizer/visualizer_rgbd.cpp +++ b/examples/cpp/Visualizer/visualizer_rgbd.cpp @@ -25,8 +25,6 @@ int main() { dai::Pipeline pipeline; // Define sources and outputs int fps = 30; - int width = 640; - int height = 400; // Define sources and outputs auto left = pipeline.create(); auto right = pipeline.create(); @@ -50,7 +48,9 @@ int main() { stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - auto *out = color->requestOutput(std::pair(1280, 720)); + rgbd->setGPUDevice(1); + rgbd->useGPU(); + auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); left->out.link(stereo->left); right->out.link(stereo->right); @@ -59,6 +59,7 @@ int main() { remoteConnector.addTopic("pcl", rgbd->pcl); pipeline.start(); + remoteConnector.registerPipeline(pipeline); auto device = pipeline.getDefaultDevice(); device->setIrLaserDotProjectorIntensity(0.7); // Main loop diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index f0101fe33..f00e207aa 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -42,6 +42,7 @@ class RGBD : public NodeCRTP { void useCpuMt(); void useGPU(); void setGPUDevice(uint32_t deviceIndex); + void setCPUThreadNum(uint32_t numThreads); void printDevices(); private: class Impl; diff --git a/shaders/rgbd2pointcloud.comp b/shaders/rgbd2pointcloud.comp index ab7b3da76..b4700cff4 100644 --- a/shaders/rgbd2pointcloud.comp +++ b/shaders/rgbd2pointcloud.comp @@ -6,25 +6,16 @@ layout(std430, binding = 0) buffer DepthBuffer { float depth[]; }; -// Binding 1: RGB input (float array, assume each pixel has 3 floats: R,G,B) -layout(std430, binding = 1) buffer RGBBuffer { - float rgb[]; -}; - // Binding 2: Intrinsics buffer [fx, fy, cx, cy, scale, width, height] -layout(std430, binding = 2) buffer IntrinsicsBuffer { +layout(std430, binding = 1) buffer IntrinsicsBuffer { float intrinsics[]; }; // Binding 3: Output XYZ buffer (float array, 3 floats per pixel) -layout(std430, binding = 3) buffer XYZBuffer { +layout(std430, binding = 2) buffer XYZBuffer { float xyz[]; }; -// Binding 4: Output RGB buffer (float array, 3 floats per pixel) -layout(std430, binding = 4) buffer OutRGBBuffer { - float outRgb[]; -}; void main() { uint i = gl_GlobalInvocationID.x; @@ -52,9 +43,5 @@ void main() { xyz[i * 3 + 1] = y; xyz[i * 3 + 2] = z; - // Copy RGB directly - outRgb[i * 3 + 0] = rgb[i * 3 + 0]; - outRgb[i * 3 + 1] = rgb[i * 3 + 1]; - outRgb[i * 3 + 2] = rgb[i * 3 + 2]; } diff --git a/src/pipeline/datatype/PointCloudData.cpp b/src/pipeline/datatype/PointCloudData.cpp index 721e9fa1f..1ad576d33 100644 --- a/src/pipeline/datatype/PointCloudData.cpp +++ b/src/pipeline/datatype/PointCloudData.cpp @@ -40,21 +40,16 @@ std::vector PointCloudData::getPointsRGB() { void PointCloudData::setPoints(const std::vector& points) { auto size = points.size(); std::vector data(size * sizeof(Point3f)); - auto* dataPtr = reinterpret_cast(data.data()); - for(unsigned int i = 0; i < size; i++) { - dataPtr[i] = points[i]; - } - setData(data); + std::memcpy(data.data(), points.data(), size * sizeof(Point3f)); + setData(std::move(data)); + setColor(false); } void PointCloudData::setPointsRGB(const std::vector& points) { auto size = points.size(); std::vector data(size * sizeof(Point3fRGB)); - auto* dataPtr = reinterpret_cast(data.data()); - for(unsigned int i = 0; i < size; i++) { - dataPtr[i] = points[i]; - } - setData(data); + std::memcpy(data.data(), points.data(), size * sizeof(Point3fRGB)); + setData(std::move(data)); setColor(true); } diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index ccaa70e1f..9677d5070 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -2,7 +2,6 @@ #include -#include "utility/PimplImpl.hpp" #include "common/Point3fRGB.hpp" #include "depthai/common/Point3fRGB.hpp" #include "depthai/pipeline/Pipeline.hpp" @@ -14,6 +13,7 @@ #include "depthai/pipeline/node/Sync.hpp" #include "depthai/shaders/rgbd2pointcloud.hpp" #include "kompute/Kompute.hpp" +#include "utility/PimplImpl.hpp" namespace dai { namespace node { @@ -24,121 +24,107 @@ class RGBD::Impl { void initializeGPU() { // Initialize Kompute mgr = std::make_shared(deviceIndex); + shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); } - void computePointCloud(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { + void computePointCloud(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { + if(!intrinsicsSet) { + throw std::runtime_error("Intrinsics not set"); + } + points.reserve(size); switch(computeMethod) { case ComputeMethod::CPU: - computePointCloudCPU(depthMat, colorMat, points); + computePointCloudCPU(depthData, colorData, points); break; case ComputeMethod::CPU_MT: - computePointCloudCPUMT(depthMat, colorMat, points); + computePointCloudCPUMT(depthData, colorData, points); break; case ComputeMethod::GPU: - computePointCloudGPU(depthMat, colorMat, points); + computePointCloudGPU(depthData, colorData, points); break; } } - void computePointCloudGPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { + void computePointCloudGPU(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { std::vector xyzOut; - std::vector rgbOut; - int width = depthMat.cols; - int height = depthMat.rows; - size_t size = width * height; xyzOut.resize(size * 3); - rgbOut.resize(size * 3); // Convert depth to float // Depth is in mm by default, convert to meters if outputMeters == true // If outputMeters is true, scale = 1/1000.0 else scale = 1.0 float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; - std::vector depthData(size); - for(size_t i = 0; i < size; i++) { - uint16_t d = depthMat.at(i / width, i % width); - depthData[i] = (float)d; // will multiply by scale in shader - } - - // Convert color to float (R,G,B) - std::vector colorDataFloat(size * 3); - for(size_t i = 0; i < size; i++) { - auto c = colorMat.at(i / width, i % width); - // OpenCV: c = [B, G, R] - colorDataFloat[i * 3 + 0] = (float)c[2]; // R - colorDataFloat[i * 3 + 1] = (float)c[1]; // G - colorDataFloat[i * 3 + 2] = (float)c[0]; // B + std::vector depthDataFloat(size); + for(int i = 0; i < size; i++) { + uint16_t depthValue = *(reinterpret_cast(depthData + i * 2)); + depthDataFloat[i] = static_cast(depthValue); // will multiply by scale in shader } // Intrinsics: [fx, fy, cx, cy, scale, width, height] - std::vector intrinsics = {fx, fy, cx, cy, scale, (float)width, (float)height}; + std::vector intrinsics = {fx, fy, cx, cy, scale, static_cast(width), static_cast(height)}; // Create Kompute tensors - auto depthTensor = mgr->tensor(depthData); - auto rgbTensor = mgr->tensor(colorDataFloat); - auto intrinsicsTensor = mgr->tensor(intrinsics); - auto xyzTensor = mgr->tensor(xyzOut); - // We'll store output RGB as float as well, then convert back - auto outRgbTensor = mgr->tensorT(std::vector(rgbOut.size())); + if(!tensorsInitialized) { + depthTensor = mgr->tensor(depthDataFloat); + intrinsicsTensor = mgr->tensor(intrinsics); + xyzTensor = mgr->tensor(xyzOut); + tensorsInitialized = true; + } else{ + depthTensor->setData(depthDataFloat); + } // Load shader - const std::vector shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); - const std::vector> tensors = {depthTensor, rgbTensor, intrinsicsTensor, xyzTensor, outRgbTensor}; - auto algo = mgr->algorithm(tensors, shader); + if(!algoInitialized) { + tensors.emplace_back(depthTensor); + tensors.emplace_back(intrinsicsTensor); + tensors.emplace_back(xyzTensor); + algo = mgr->algorithm(tensors, shader); + algoInitialized = true; + } mgr->sequence()->record(tensors)->record(algo)->record(tensors)->eval(); // Retrieve results - xyzOut = xyzTensor->vector(); - std::vector outRgbFloat = outRgbTensor->vector(); - for(size_t i = 0; i < rgbOut.size(); i++) { - rgbOut[i] = static_cast(std::round(outRgbFloat[i])); - } - for(int i = 0; i < width * height; i++) { + xyzOut = xyzTensor->vector(); + for(int i = 0; i < size; i++) { Point3fRGB p; p.x = xyzOut[i * 3 + 0]; p.y = xyzOut[i * 3 + 1]; p.z = xyzOut[i * 3 + 2]; - p.r = rgbOut[i * 3 + 0]; - p.g = rgbOut[i * 3 + 1]; - p.b = rgbOut[i * 3 + 2]; + p.r = colorData[i * 3 + 0]; + p.g = colorData[i * 3 + 1]; + p.b = colorData[i * 3 + 2]; points.emplace_back(p); } } - void computePointCloudCPU(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { - int width = depthMat.cols; - int height = depthMat.rows; - points.resize(width * height); - - for(int i = 0; i < height; i++) { - for(int j = 0; j < width; j++) { - uint16_t depthValue = depthMat.at(i, j); - float z = depthValue; - float x = (j - cx) * z / fx; - float y = (i - cy) * z / fy; - - auto color = colorMat.at(i, j); - points[i * width + j] = Point3fRGB{x, y, z, static_cast(color[2]), static_cast(color[1]), static_cast(color[0])}; - } + void calcPoints(const uint8_t* depthData, const uint8_t* colorData, std::vector& points, int startRow, int endRow) { + float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; + for(int i = startRow * width; i < endRow * width; i++) { + float x = i % width; + float y = i / width; + uint16_t depthValue = *(reinterpret_cast(depthData + i * 2)); + float z = static_cast(depthValue) * scale; + x = (x - cx) * z / fx; + y = (y - cy) * z / fy; + uint8_t r = colorData[i * 3 + 0]; + uint8_t b = colorData[i * 3 + 1]; + uint8_t g = colorData[i * 3 + 2]; + Point3fRGB p; + p.x = x; + p.y = y; + p.z = z; + p.r = r; + p.g = g; + p.b = b; + std::lock_guard lock(pointsMtx); + points.emplace_back(p); } } - void computePointCloudCPUMT(const cv::Mat& depthMat, const cv::Mat& colorMat, std::vector& points) { - int width = depthMat.cols; - int height = depthMat.rows; - points.resize(width * height); + void computePointCloudCPU(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { + calcPoints(depthData, colorData, points, 0, height); + } + void computePointCloudCPUMT(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { // Lambda function for processing a block of rows - auto processRows = [&](int startRow, int endRow) { - for(int i = startRow; i < endRow; i++) { - for(int j = 0; j < width; j++) { - uint16_t depthValue = depthMat.at(i, j); - float z = depthValue; - float x = (j - cx) * z / fx; - float y = (i - cy) * z / fy; - - auto color = colorMat.at(i, j); - points[i * width + j] = Point3fRGB{x, y, z, static_cast(color[2]), static_cast(color[1]), static_cast(color[0])}; - } - } - }; + auto processRows = [&](int startRow, int endRow) { calcPoints(depthData, colorData, points, startRow, endRow); }; // Divide rows into chunks and process in parallel - const int numThreads = std::thread::hardware_concurrency(); + const int numThreads = threadNum; const int rowsPerThread = height / numThreads; std::vector> futures; @@ -167,6 +153,9 @@ class RGBD::Impl { this->deviceIndex = deviceIndex; mgr = std::make_shared(deviceIndex); } + void setCPUThreadNum(uint32_t numThreads) { + threadNum = numThreads; + } void useCPU() { computeMethod = ComputeMethod::CPU; } @@ -177,21 +166,39 @@ class RGBD::Impl { initializeGPU(); computeMethod = ComputeMethod::GPU; } - void setIntrinsics(float fx, float fy, float cx, float cy) { + void setIntrinsics(float fx, float fy, float cx, float cy, unsigned int width, unsigned int height) { this->fx = fx; this->fy = fy; this->cx = cx; this->cy = cy; + this->width = width; + this->height = height; + size = this->width * this->height; + intrinsicsSet = true; } enum class ComputeMethod { CPU, CPU_MT, GPU }; - std::shared_ptr mgr; ComputeMethod computeMethod = ComputeMethod::CPU; + std::shared_ptr mgr; uint32_t deviceIndex = 0; + std::vector shader; + std::shared_ptr algo; + std::shared_ptr depthTensor; + std::shared_ptr intrinsicsTensor; + std::shared_ptr xyzTensor; + std::vector> tensors; + bool algoInitialized = false; + bool tensorsInitialized = false; bool outputMeters = false; float fx, fy, cx, cy; + int width, height; + int size; + bool intrinsicsSet = false; + std::mutex pointsMtx; + int threadNum = 2; }; -RGBD::RGBD() = default;; +RGBD::RGBD() = default; +; std::shared_ptr RGBD::build() { align = getParentPipeline().create(); align->outputAligned.link(inDepthSync); @@ -202,6 +209,12 @@ std::shared_ptr RGBD::build() { sync->out.link(inSync); inSync.setMaxSize(1); inSync.setBlocking(false); + inDepth.setBlocking(false); + inColor.setBlocking(false); + inDepthSync.setBlocking(false); + inDepthSync.setMaxSize(1); + inColorSync.setBlocking(false); + inColorSync.setMaxSize(1); inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); return std::static_pointer_cast(shared_from_this()); @@ -214,7 +227,7 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { auto pipeline = getParentPipeline(); auto colorCam = pipeline.create()->build(); auto depth = pipeline.create()->build(true); - auto* out = colorCam->requestOutput(size); + auto* out = colorCam->requestOutput(size, dai::ImgFrame::Type::RGB888p); out->link(inColor); depth->depth.link(inDepth); return build(); @@ -225,19 +238,21 @@ void RGBD::initialize(std::vector> frames) { auto frame = frames.at(1); auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); auto camID = static_cast(frame->getInstanceNum()); - auto intrinsics = calibHandler.getCameraIntrinsics(camID, frame->getWidth(), frame->getHeight()); + auto width = frame->getWidth(); + auto height = frame->getHeight(); + auto intrinsics = calibHandler.getCameraIntrinsics(camID, width, height); float fx = intrinsics[0][0]; float fy = intrinsics[1][1]; float cx = intrinsics[0][2]; float cy = intrinsics[1][2]; - pimpl->setIntrinsics(fx, fy, cx, cy); + pimpl->setIntrinsics(fx, fy, cx, cy, width, height); initialized = true; } void RGBD::run() { while(isRunning()) { // Get the color and depth frames - auto group = inSync.tryGet(); + auto group = inSync.get(); if(group == nullptr) continue; if(!initialized) { std::vector> imgFrames; @@ -262,13 +277,11 @@ void RGBD::run() { std::vector points; // Fill the point cloud - auto depthData = depthFrame->getCvFrame(); - auto colorData = colorFrame->getCvFrame(); + auto* depthData = depthFrame->getData().data(); + auto* colorData = colorFrame->getData().data(); // Use GPU to compute point cloud - points.reserve(width * height); pimpl->computePointCloud(depthData, colorData, points); - // Convert xyzBuffer & rgbBuffer to Point3fRGB vector pc->setPointsRGB(points); pcl.send(pc); } @@ -288,6 +301,9 @@ void RGBD::useGPU() { void RGBD::setGPUDevice(uint32_t deviceIndex) { pimpl->setGPUDevice(deviceIndex); } +void RGBD::setCPUThreadNum(uint32_t numThreads) { + pimpl->setCPUThreadNum(numThreads); +} void RGBD::printDevices() { pimpl->printDevices(); } From 0a4a933a1d327f68355f7e7564e6619bc8cd0b68 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 13:05:38 +0100 Subject: [PATCH 11/51] fix basalt --- cmake/ports/basalt/portfile.cmake | 8 +- cmake/ports/basalt/win.patch | 262 ------------------------- cmake/ports/tbb/overflow-warning.patch | 14 -- cmake/ports/tbb/portfile.cmake | 23 --- cmake/ports/tbb/vcpkg.json | 15 -- src/basalt/BasaltVIO.cpp | 3 + 6 files changed, 6 insertions(+), 319 deletions(-) delete mode 100644 cmake/ports/basalt/win.patch delete mode 100644 cmake/ports/tbb/overflow-warning.patch delete mode 100644 cmake/ports/tbb/portfile.cmake delete mode 100644 cmake/ports/tbb/vcpkg.json diff --git a/cmake/ports/basalt/portfile.cmake b/cmake/ports/basalt/portfile.cmake index 2dba7e7fb..fdc313f26 100644 --- a/cmake/ports/basalt/portfile.cmake +++ b/cmake/ports/basalt/portfile.cmake @@ -2,11 +2,9 @@ vcpkg_from_github( OUT_SOURCE_PATH SOURCE_PATH REPO luxonis/basalt - REF ac3cd0fbef91e889db27a1a779a5105ba715fef7 - SHA512 1e7694168f92af5f48f462a793fff528c46a4bd01d3417daf2031b651acba70ee9d61d38a33a1be217cc6e97baa67425e1a8fcc4e57a46033954757112326206 - HEAD_REF deps_test - PATCHES - win.patch + REF bf3d2c33685b4315fa2f1407619c301c3135e891 + SHA512 346104eee47e08afd82d134b89c44163dee4528ed98875afd8bd6bd153d28f185145c5f938d0ff52423edca1434391ae6478ab3646f014c7598075babeadc054 + HEAD_REF depthai ) vcpkg_cmake_configure( SOURCE_PATH "${SOURCE_PATH}" diff --git a/cmake/ports/basalt/win.patch b/cmake/ports/basalt/win.patch deleted file mode 100644 index 2628522ae..000000000 --- a/cmake/ports/basalt/win.patch +++ /dev/null @@ -1,262 +0,0 @@ -diff --git a/CMakeLists.txt b/CMakeLists.txt -index 4094b79..99df803 100644 ---- a/CMakeLists.txt -+++ b/CMakeLists.txt -@@ -78,8 +78,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - - - # Flags used for CHECK_CXX_SOURCE_COMPILES -+if(NOT WIN32) - set(CMAKE_REQUIRED_FLAGS "-Wno-error") -- -+endif() - - # save flags passed by user - set(BASALT_PASSED_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -@@ -103,7 +104,9 @@ endif() - - # base set of compile flags - set (BASALT_CXX_FLAGS) -- -+if(WIN32) -+ set(BASALT_CXX_FLAGS "/bigobj") -+endif() - # For avoiding libfmt >9 issues, see https://github.com/strasdat/Sophus/issues/366 #issuecomment-1229178088 - add_definitions(-DFMT_DEPRECATED_OSTREAM=1) - -@@ -269,7 +272,7 @@ set(CMAKE_CXX_FLAGS "${BASALT_CXX_FLAGS}${BASALT_PASSED_CXX_FLAGS}") - find_package(OpenCV REQUIRED COMPONENTS core imgproc calib3d highgui) - message(STATUS "Found OpenCV ${OpenCV_VERSION} headers in: ${OpenCV_INCLUDE_DIRS}") - message(STATUS "Found OpenCV_LIBS: ${OpenCV_LIBS}") -- -+set(BASALT_SDK_ONLY ON) - - - if(BASALT_SDK_ONLY) -@@ -290,6 +293,7 @@ if(BASALT_SDK_ONLY) - ${PROJECT_BINARY_DIR}/include/basalt/utils/build_config.h - ) - include_directories(include) -+ - add_library(basalt_sdk - src/io/marg_data_io.cpp - src/linearization/landmark_block.cpp -@@ -314,6 +318,9 @@ if(BASALT_SDK_ONLY) - set_property(TARGET basalt_sdk PROPERTY CXX_STANDARD 17) - set_property(TARGET basalt_sdk PROPERTY CXX_STANDARD_REQUIRED ON) - set_property(TARGET basalt_sdk PROPERTY CXX_EXTENSIONS OFF) -+ include(GenerateExportHeader) -+ generate_export_header(basalt_sdk -+ EXPORT_FILE_NAME ${CMAKE_CURRENT_BINARY_DIR}/include/basalt/exports/basalt_sdkExport.h) - # target_compile_definitions(basalt_sdk PUBLIC BASALT_SDK_ONLY) - target_include_directories(basalt_sdk PRIVATE - "$" -@@ -321,7 +328,7 @@ if(BASALT_SDK_ONLY) - ) - target_include_directories(basalt_sdk PRIVATE ${PROJECT_BINARY_DIR}/include) - -- target_link_libraries(basalt_sdk PUBLIC ${OpenCV_LIBS} Threads::Threads TBB::tbb Eigen3::Eigen Sophus::Sophus magic_enum::magic_enum fmt::fmt) -+ target_link_libraries(basalt_sdk PRIVATE ${OpenCV_LIBS} Threads::Threads TBB::tbb Eigen3::Eigen Sophus::Sophus magic_enum::magic_enum fmt::fmt) - - target_compile_definitions(basalt_sdk PUBLIC ${BASALT_COMPILE_DEFINITIONS}) - -@@ -341,6 +348,12 @@ if(BASALT_SDK_ONLY) - DESTINATION include/basalt/utils - ) - -+ # Install generated export header -+ install( -+ FILES ${CMAKE_CURRENT_BINARY_DIR}/include/basalt/exports/basalt_sdkExport.h -+ DESTINATION include/basalt/exports -+ ) -+ - # install headers - install(DIRECTORY include/basalt - DESTINATION include -@@ -360,6 +373,7 @@ if(BASALT_SDK_ONLY) - ) - - -+ - else() - # Add our own custom scoped opencv target since none is provided by OpenCV itself - add_library(basalt::opencv INTERFACE IMPORTED) -diff --git a/CMakePresets.json b/CMakePresets.json -index 70f8f23..0c82642 100644 ---- a/CMakePresets.json -+++ b/CMakePresets.json -@@ -1,6 +1,6 @@ - - { -- "version": 2, -+ "version": 3, - "configurePresets": [ - { - "name": "default", -diff --git a/cmake/ports/basalt-headers/portfile.cmake b/cmake/ports/basalt-headers/portfile.cmake -index 2b38d49..fe98885 100644 ---- a/cmake/ports/basalt-headers/portfile.cmake -+++ b/cmake/ports/basalt-headers/portfile.cmake -@@ -1,10 +1,8 @@ -- -- - vcpkg_from_github( - OUT_SOURCE_PATH SOURCE_PATH - REPO luxonis/basalt-headers -- REF 9172c4e6d8c8533d5ebae8289bba2a299f30eb50 -- SHA512 39b4b88b147f4d8fb9b2423559c8f07b6e7e43a5136ff461058ab2fe33edfbd1790e6e8684abc2cbe647457752f4ff835c4ed0ed4a370141e345d1e529af2369 -+ REF 5392967dc6825838a52fa6d6ed38188a55a6acf7 -+ SHA512 7b22090aacf609e9b8417dd852607d09f0d0db144980852ec1cd73ae2e5fa0f095ef450011a8d7bf6b630f9af8a8ca5870d590de25b8c0783d843344aa48866b - HEAD_REF vcpkg_deps - ) - vcpkg_cmake_configure( -@@ -14,3 +12,7 @@ vcpkg_cmake_configure( - vcpkg_cmake_install() - - vcpkg_cmake_config_fixup(CONFIG_PATH "lib/cmake/basalt-headers") -+file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug/include") -+file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug/lib" "${CURRENT_PACKAGES_DIR}/lib") -+file(REMOVE_RECURSE "${CURRENT_PACKAGES_DIR}/debug") -+vcpkg_install_copyright(FILE_LIST "${SOURCE_PATH}/LICENSE") -\ No newline at end of file -diff --git a/cmake/ports/basalt-headers/vcpkg.json b/cmake/ports/basalt-headers/vcpkg.json -index 35912be..3a11ef7 100644 ---- a/cmake/ports/basalt-headers/vcpkg.json -+++ b/cmake/ports/basalt-headers/vcpkg.json -@@ -1,17 +1,19 @@ -- -- - { - "name": "basalt-headers", - "version-string": "0.1.0", - "description": "Reusable components of Basalt project as header-only library", - "dependencies": [ -- { -+ { - "name": "vcpkg-cmake", - "host": true - }, - { - "name": "vcpkg-cmake-config", - "host": true -- } --] -+ }, -+ "eigen3", -+ "sophus", -+ "cereal", -+ "fmt" -+ ] - } -\ No newline at end of file -diff --git a/include/basalt/optical_flow/optical_flow.h b/include/basalt/optical_flow/optical_flow.h -index b20df49..450ad63 100644 ---- a/include/basalt/optical_flow/optical_flow.h -+++ b/include/basalt/optical_flow/optical_flow.h -@@ -59,6 +59,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - #include - #endif - #include -+#include "basalt/exports/basalt_sdkExport.h" - - namespace basalt { - -@@ -249,6 +250,6 @@ class OpticalFlowTyped : public OpticalFlowBase { - - class OpticalFlowFactory { - public: -- static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config, const Calibration& cam); -+ BASALT_SDK_EXPORT static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config, const Calibration& cam); - }; - } // namespace basalt -diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h -index 4d38532..ee96efa 100644 ---- a/include/basalt/utils/vio_config.h -+++ b/include/basalt/utils/vio_config.h -@@ -36,7 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - #include - #include -- -+#include "basalt/exports/basalt_sdkExport.h" - namespace basalt { - - enum class LinearizationType { ABS_QR, ABS_SC, REL_SC }; -@@ -44,8 +44,8 @@ enum class MatchingGuessType { SAME_PIXEL, REPROJ_FIX_DEPTH, REPROJ_AVG_DEPTH }; - enum class KeyframeMargCriteria { KF_MARG_DEFAULT, KF_MARG_FORWARD_VECTOR }; - - struct VioConfig { -- VioConfig(); -- void load(const std::string& filename); -+ BASALT_SDK_EXPORT VioConfig(); -+ BASALT_SDK_EXPORT void load(const std::string& filename); - void save(const std::string& filename); - - std::string optical_flow_type; -diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h -index 48222eb..d95ed19 100644 ---- a/include/basalt/vi_estimator/vio_estimator.h -+++ b/include/basalt/vi_estimator/vio_estimator.h -@@ -40,6 +40,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - #include - #include - #include -+#include "basalt/exports/basalt_sdkExport.h" -+ - - namespace basalt { - -@@ -157,7 +159,7 @@ class VioEstimatorBase { - - class VioEstimatorFactory { - public: -- static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config, const Calibration& cam, -+ BASALT_SDK_EXPORT static VioEstimatorBase::Ptr getVioEstimator(const VioConfig& config, const Calibration& cam, - const Eigen::Vector3d& g, bool use_imu, bool use_double); - }; - -diff --git a/thirdparty/CLI11 b/thirdparty/CLI11 -index 4af78be..c2ea58c 160000 ---- a/thirdparty/CLI11 -+++ b/thirdparty/CLI11 -@@ -1 +1 @@ --Subproject commit 4af78beef777e313814b4daff70e2da9171a385a -+Subproject commit c2ea58c7f9bb2a1da2d3d7f5b462121ac6a07f16 -diff --git a/thirdparty/magic_enum b/thirdparty/magic_enum -index 3d1f6a5..43a9272 160000 ---- a/thirdparty/magic_enum -+++ b/thirdparty/magic_enum -@@ -1 +1 @@ --Subproject commit 3d1f6a5a2a3fbcba077e00ad0ccc2dd9fefc2ca7 -+Subproject commit 43a9272f450fd07e85868ed277ebd793e40806df -diff --git a/vcpkg-configuration.json b/vcpkg-configuration.json -index 66747fb..2a03369 100644 ---- a/vcpkg-configuration.json -+++ b/vcpkg-configuration.json -@@ -2,7 +2,7 @@ - { - "default-registry": { - "kind": "git", -- "baseline": "509f71e53f45e46c13fa7935d2f6a45803580c07", -+ "baseline": "f56b56e01683ba9fde12b64de246ec0b3eb69b18", - "repository": "https://github.com/microsoft/vcpkg" - }, - "registries": [ -diff --git a/vcpkg.json b/vcpkg.json -index 64107c7..6889ff7 100644 ---- a/vcpkg.json -+++ b/vcpkg.json -@@ -9,7 +9,11 @@ - "cereal", - "fmt", - "tbb", -- "nlohmann-json" -+ "nlohmann-json", -+ { -+ "name": "opencv4", -+ "default-features": false -+ } - ], - "builtin-baseline": "f5ec6f30ff70f04f841436a0f36600bdbabfcfbf" - } - diff --git a/cmake/ports/tbb/overflow-warning.patch b/cmake/ports/tbb/overflow-warning.patch deleted file mode 100644 index 941b85b66..000000000 --- a/cmake/ports/tbb/overflow-warning.patch +++ /dev/null @@ -1,14 +0,0 @@ -diff --git a/cmake/compilers/GNU.cmake b/cmake/compilers/GNU.cmake -index 871b72a..76e1900 100644 ---- a/cmake/compilers/GNU.cmake -+++ b/cmake/compilers/GNU.cmake -@@ -26,7 +26,7 @@ else() - set(TBB_DEF_FILE_PREFIX lin${TBB_ARCH}) - endif() - --set(TBB_WARNING_LEVEL -Wall -Wextra $<$:-Werror> -Wfatal-errors) -+set(TBB_WARNING_LEVEL -Wall -Wextra $<$:-Werror> -Wfatal-errors -Wno-error=stringop-overflow) - set(TBB_TEST_WARNING_FLAGS -Wshadow -Wcast-qual -Woverloaded-virtual -Wnon-virtual-dtor) - - # Depfile options (e.g. -MD) are inserted automatically in some cases. - diff --git a/cmake/ports/tbb/portfile.cmake b/cmake/ports/tbb/portfile.cmake deleted file mode 100644 index e11cdd844..000000000 --- a/cmake/ports/tbb/portfile.cmake +++ /dev/null @@ -1,23 +0,0 @@ - -vcpkg_from_github( - OUT_SOURCE_PATH SOURCE_PATH - REPO oneapi-src/oneTBB - REF v2021.12.0 - SHA512 64022bcb61cf7b2030a1bcc11168445ef9f0d69b70290233a7febb71cc7a12cc2282dddc045f84e30893efe276342f02fd78d176706268eeaefe9aac7446d4e9 - HEAD_REF master - PATCHES - overflow-warning.patch -) -vcpkg_cmake_configure( - SOURCE_PATH "${SOURCE_PATH}" - OPTIONS - -DTBB_TEST=OFF - -DCMAKE_CXX_VISIBILITY_PRESET=hidden - -DCMAKE_C_VISIBILITY_PRESET=hidden - -DCMAKE_CXX_STANDARD=17 - -DCMAKE_C_STANDARD=11 -) - -vcpkg_cmake_install() - - diff --git a/cmake/ports/tbb/vcpkg.json b/cmake/ports/tbb/vcpkg.json deleted file mode 100644 index eec1ba823..000000000 --- a/cmake/ports/tbb/vcpkg.json +++ /dev/null @@ -1,15 +0,0 @@ -{ - "name": "tbb", - "version-string": "2021.12.0", - "description": "oneAPI Threading Building Blocks (oneTBB)", - "dependencies": [ - { - "name": "vcpkg-cmake", - "host": true - }, - { - "name": "vcpkg-cmake-config", - "host": true - } -] -} diff --git a/src/basalt/BasaltVIO.cpp b/src/basalt/BasaltVIO.cpp index 7e1e8f89c..4ba1bdf0d 100644 --- a/src/basalt/BasaltVIO.cpp +++ b/src/basalt/BasaltVIO.cpp @@ -1,6 +1,7 @@ #include "depthai/basalt/BasaltVIO.hpp" #include "../utility/PimplImpl.hpp" +#include "basalt/vi_estimator/vio_estimator.h" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" @@ -208,6 +209,7 @@ void BasaltVIO::initialize(std::vector> frames) { } optFlowPtr = basalt::OpticalFlowFactory::getOpticalFlow(vioConfig, *calib); + optFlowPtr->show_gui = false; optFlowPtr->start(); pimpl->imageDataQueue = optFlowPtr->input_img_queue; vio = basalt::VioEstimatorFactory::getVioEstimator(vioConfig, *calib, basalt::constants::g, true, true); @@ -218,6 +220,7 @@ void BasaltVIO::initialize(std::vector> frames) { vio->out_state_queue = pimpl->outStateQueue; vio->opt_flow_depth_guess_queue = optFlowPtr->input_depth_queue; vio->opt_flow_state_queue = optFlowPtr->input_state_queue; + vio->opt_flow_lm_bundle_queue = optFlowPtr->input_lm_bundle_queue; initialized = true; } From b8f9ba4b4502db820d9f38d0c4e7ac0aca468778 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 13:33:21 +0100 Subject: [PATCH 12/51] remove kompute from default deps --- vcpkg.json | 1 - 1 file changed, 1 deletion(-) diff --git a/vcpkg.json b/vcpkg.json index 0363c1bdf..7fe0d6eff 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -2,7 +2,6 @@ "name": "depthai", "dependencies": [ "cpp-httplib", - "kompute", "libpng", "tiff", "yaml-cpp", From 86be142f533049f05764a2d0afcfec397a2e2b38 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 16:39:43 +0100 Subject: [PATCH 13/51] remove internal imagealign --- CMakeLists.txt | 2 +- .../python/src/pipeline/node/RGBDBindings.cpp | 10 +- cmake/depthaiDependencies.cmake | 5 +- examples/cpp/HostNodes/rgbd.cpp | 5 +- include/depthai/pipeline/node/host/RGBD.hpp | 20 +-- src/pipeline/node/host/RGBD.cpp | 136 +++++++++--------- 6 files changed, 91 insertions(+), 87 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2b8552e8e..8a808f8a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ option(DEPTHAI_ENABLE_PROTOBUF "Enable Protobuf support" ON) option(DEPTHAI_BUILD_PYTHON "Build python bindings" OFF) option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) -OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" ON) +OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" OFF) option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BOOTSTRAP_VCPKG "Automatically bootstrap VCPKG" ON) option(DEPTHAI_MERGED_TARGET "Enable merged target build" ON) diff --git a/bindings/python/src/pipeline/node/RGBDBindings.cpp b/bindings/python/src/pipeline/node/RGBDBindings.cpp index 361e648d8..7ad53e448 100644 --- a/bindings/python/src/pipeline/node/RGBDBindings.cpp +++ b/bindings/python/src/pipeline/node/RGBDBindings.cpp @@ -33,5 +33,13 @@ void bind_rgbd(pybind11::module& m, void* pCallstack){ .def_property_readonly("inColor", [](RGBD& node){ return &node.inColor; }, py::return_value_policy::reference_internal) .def_property_readonly("inDepth", [](RGBD& node){ return &node.inDepth; }, py::return_value_policy::reference_internal) .def_readonly("pcl", &RGBD::pcl, DOC(dai, node, RGBD, pcl)) - .def("build", &RGBD::build, DOC(dai, node, RGBD, build)); + .def("build", &RGBD::build, DOC(dai, node, RGBD, build)) + .def("build", &RGBD::build, py::arg("autocreate"), py::arg("size"), DOC(dai, node, RGBD, build, 2)) + .def("setOutputMeters", &RGBD::setOutputMeters, py::arg("outputMeters"), DOC(dai, node, RGBD, setOutputMeters)) + .def("useCPU", &RGBD::useCPU, DOC(dai, node, RGBD, useCPU)) + .def("useCPUMT", &RGBD::useCPUMT, DOC(dai, node, RGBD, useCPUMT)) + .def("useGPU", &RGBD::useGPU, DOC(dai, node, RGBD, useGPU)) + .def("setGPUDevice", &RGBD::setGPUDevice, py::arg("deviceIndex"), DOC(dai, node, RGBD, setGPUDevice)) + .def("setCPUThreadNum", &RGBD::setCPUThreadNum, py::arg("numThreads"), DOC(dai, node, RGBD, setCPUThreadNum)) + .def("printDevices", &RGBD::printDevices, DOC(dai, node, RGBD, printDevices)); } diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index e3ced7e94..96d103f05 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -43,8 +43,9 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT DEPTHAI_SHARED_LIBS)) find_package(PNG REQUIRED) - find_package(kompute ${_QUIET} CONFIG REQUIRED) - + if(DEPTHAI_ENABLE_KOMPUTE) + find_package(kompute ${_QUIET} CONFIG REQUIRED) + endif() # libarchive for firmware packages find_package(LibArchive ${_QUIET} REQUIRED) find_package(liblzma ${_QUIET} CONFIG REQUIRED) diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp index ad64817df..e5cd1dfe3 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -17,7 +17,7 @@ class RerunNode : public dai::NodeCRTP { rec.log_static("world", rerun::ViewCoordinates::FLU); rec.log("world/ground", rerun::Boxes3D::from_half_sizes({{3.f, 3.f, 0.00001f}})); while(isRunning()) { - auto pclIn = inputPCL.get(); + auto pclIn = inputPCL.tryGet(); if(pclIn != nullptr) { std::vector points; std::vector colors; @@ -63,9 +63,8 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); + stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); rgbd->setOutputMeters(true); - rgbd->setGPUDevice(1); - rgbd->useGPU(); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); left->out.link(stereo->left); diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index f00e207aa..2bc00556e 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -21,14 +21,10 @@ class RGBD : public NodeCRTP { std::shared_ptr align; InputMap& inputs = sync->inputs; - /** - * Input color frame. - */ - Input inColor{*this, {"inColor", DEFAULT_GROUP, false, 0, {{DatatypeEnum::ImgFrame, true}}}}; - /** - * Input depth frame. - */ - Input inDepth{*this, {"inDepth", DEFAULT_GROUP, false, 0, {{DatatypeEnum::ImgFrame, true}}}}; + std::string colorInputName = "inColorSync"; + std::string depthInputName = "inDepthSync"; + Input& inColor = inputs[colorInputName]; + Input& inDepth = inputs[depthInputName]; /** * Output point cloud. @@ -39,7 +35,7 @@ class RGBD : public NodeCRTP { std::shared_ptr build(bool autocreate, std::pair size); void setOutputMeters(bool outputMeters); void useCPU(); - void useCpuMt(); + void useCPUMT(); void useGPU(); void setGPUDevice(uint32_t deviceIndex); void setCPUThreadNum(uint32_t numThreads); @@ -47,12 +43,6 @@ class RGBD : public NodeCRTP { private: class Impl; Pimpl pimpl; - std::string colorInputName = "inColorSync"; - std::string depthInputName = "inDepthSync"; - Input& inColorSync = inputs[colorInputName]; - Input& inDepthSync = inputs[depthInputName]; - Output colorMux{*this, {"colorMux", DEFAULT_GROUP, {{DatatypeEnum::ImgFrame, true}}}}; - Output depthPT{*this, {"depthPT", DEFAULT_GROUP, {{DatatypeEnum::ImgFrame, true}}}}; void run() override; void initialize(std::vector> frames); Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 9677d5070..597902549 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -12,7 +12,9 @@ #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai/pipeline/node/Sync.hpp" #include "depthai/shaders/rgbd2pointcloud.hpp" -#include "kompute/Kompute.hpp" +#ifdef DEPTHAI_ENABLE_KOMPUTE + #include "kompute/Kompute.hpp" +#endif #include "utility/PimplImpl.hpp" namespace dai { @@ -21,11 +23,6 @@ namespace node { class RGBD::Impl { public: Impl() = default; - void initializeGPU() { - // Initialize Kompute - mgr = std::make_shared(deviceIndex); - shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); - } void computePointCloud(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { if(!intrinsicsSet) { throw std::runtime_error("Intrinsics not set"); @@ -43,7 +40,54 @@ class RGBD::Impl { break; } } + void setOutputMeters(bool outputMeters) { + this->outputMeters = outputMeters; + } + void printDevices() { + auto devices = mgr->listDevices(); + for(auto& device : devices) { + std::cout << "Device: " << device.getProperties().deviceName << std::endl; + } + } + void setGPUDevice(uint32_t deviceIndex) { + this->deviceIndex = deviceIndex; + } + void setCPUThreadNum(uint32_t numThreads) { + threadNum = numThreads; + } + void useCPU() { + computeMethod = ComputeMethod::CPU; + } + void useCPUMT() { + computeMethod = ComputeMethod::CPU_MT; + } + void useGPU() { + initializeGPU(); + } + void setIntrinsics(float fx, float fy, float cx, float cy, unsigned int width, unsigned int height) { + this->fx = fx; + this->fy = fy; + this->cx = cx; + this->cy = cy; + this->width = width; + this->height = height; + size = this->width * this->height; + intrinsicsSet = true; + } + + private: + void initializeGPU() { +#ifdef DEPTHAI_ENABLE_KOMPUTE + // Initialize Kompute + mgr = std::make_shared(deviceIndex); + shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); + computeMethod = ComputeMethod::GPU; +#else + loggerr::log->error("Kompute not enabled. Please enable it in CMake."); +#endif + } void computePointCloudGPU(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { +#ifdef DEPTHAI_ENABLE_KOMPUTE std::vector xyzOut; xyzOut.resize(size * 3); @@ -68,7 +112,7 @@ class RGBD::Impl { intrinsicsTensor = mgr->tensor(intrinsics); xyzTensor = mgr->tensor(xyzOut); tensorsInitialized = true; - } else{ + } else { depthTensor->setData(depthDataFloat); } // Load shader @@ -92,6 +136,7 @@ class RGBD::Impl { p.b = colorData[i * 3 + 2]; points.emplace_back(p); } +#endif } void calcPoints(const uint8_t* depthData, const uint8_t* colorData, std::vector& points, int startRow, int endRow) { float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; @@ -140,47 +185,12 @@ class RGBD::Impl { future.get(); } } - void setOutputMeters(bool outputMeters) { - this->outputMeters = outputMeters; - } - void printDevices() { - auto devices = mgr->listDevices(); - for(auto& device : devices) { - std::cout << "Device: " << device.getProperties().deviceName << std::endl; - } - } - void setGPUDevice(uint32_t deviceIndex) { - this->deviceIndex = deviceIndex; - mgr = std::make_shared(deviceIndex); - } - void setCPUThreadNum(uint32_t numThreads) { - threadNum = numThreads; - } - void useCPU() { - computeMethod = ComputeMethod::CPU; - } - void useCpuMt() { - computeMethod = ComputeMethod::CPU_MT; - } - void useGPU() { - initializeGPU(); - computeMethod = ComputeMethod::GPU; - } - void setIntrinsics(float fx, float fy, float cx, float cy, unsigned int width, unsigned int height) { - this->fx = fx; - this->fy = fy; - this->cx = cx; - this->cy = cy; - this->width = width; - this->height = height; - size = this->width * this->height; - intrinsicsSet = true; - } enum class ComputeMethod { CPU, CPU_MT, GPU }; ComputeMethod computeMethod = ComputeMethod::CPU; +#ifdef DEPTHAI_ENABLE_KOMPUTE std::shared_ptr mgr; uint32_t deviceIndex = 0; - std::vector shader; + std::vector shader; std::shared_ptr algo; std::shared_ptr depthTensor; std::shared_ptr intrinsicsTensor; @@ -188,6 +198,7 @@ class RGBD::Impl { std::vector> tensors; bool algoInitialized = false; bool tensorsInitialized = false; +#endif bool outputMeters = false; float fx, fy, cx, cy; int width, height; @@ -200,23 +211,14 @@ class RGBD::Impl { RGBD::RGBD() = default; ; std::shared_ptr RGBD::build() { - align = getParentPipeline().create(); - align->outputAligned.link(inDepthSync); - colorMux.link(inColorSync); - colorMux.link(align->inputAlignTo); - depthPT.link(align->input); - sync->setRunOnHost(false); sync->out.link(inSync); - inSync.setMaxSize(1); - inSync.setBlocking(false); - inDepth.setBlocking(false); + sync->setRunOnHost(false); inColor.setBlocking(false); - inDepthSync.setBlocking(false); - inDepthSync.setMaxSize(1); - inColorSync.setBlocking(false); - inColorSync.setMaxSize(1); - inDepth.addCallback([this](const std::shared_ptr& data) { depthPT.send(data); }); - inColor.addCallback([this](const std::shared_ptr& data) { colorMux.send(data); }); + inColor.setMaxSize(1); + inDepth.setBlocking(false); + inDepth.setMaxSize(1); + inSync.setBlocking(false); + inSync.setMaxSize(1); return std::static_pointer_cast(shared_from_this()); } @@ -227,7 +229,7 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { auto pipeline = getParentPipeline(); auto colorCam = pipeline.create()->build(); auto depth = pipeline.create()->build(true); - auto* out = colorCam->requestOutput(size, dai::ImgFrame::Type::RGB888p); + auto* out = colorCam->requestOutput(size, dai::ImgFrame::Type::RGB888i); out->link(inColor); depth->depth.link(inDepth); return build(); @@ -235,6 +237,10 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { void RGBD::initialize(std::vector> frames) { // Initialize the camera intrinsics + // Check if width and height match + if(frames.at(0)->getWidth() != frames.at(1)->getWidth() || frames.at(0)->getHeight() != frames.at(1)->getHeight()) { + throw std::runtime_error("Color and depth frame sizes do not match"); + } auto frame = frames.at(1); auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); auto camID = static_cast(frame->getInstanceNum()); @@ -252,7 +258,7 @@ void RGBD::initialize(std::vector> frames) { void RGBD::run() { while(isRunning()) { // Get the color and depth frames - auto group = inSync.get(); + auto group = inSync.tryGet(); if(group == nullptr) continue; if(!initialized) { std::vector> imgFrames; @@ -262,8 +268,8 @@ void RGBD::run() { initialize(imgFrames); } - auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColorSync.getName())); - auto depthFrame = std::dynamic_pointer_cast(group->group.at(inDepthSync.getName())); + auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColor.getName())); + auto depthFrame = std::dynamic_pointer_cast(group->group.at(inDepth.getName())); // Create the point cloud auto pc = std::make_shared(); @@ -292,8 +298,8 @@ void RGBD::setOutputMeters(bool outputMeters) { void RGBD::useCPU() { pimpl->useCPU(); } -void RGBD::useCpuMt() { - pimpl->useCpuMt(); +void RGBD::useCPUMT() { + pimpl->useCPUMT(); } void RGBD::useGPU() { pimpl->useGPU(); From b015ea1616c0426a3c15d4eb3a0a770df4d1353f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 16:40:15 +0100 Subject: [PATCH 14/51] remove gpu code from visualizer example --- examples/cpp/Visualizer/visualizer_rgbd.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/Visualizer/visualizer_rgbd.cpp index 640fb0330..2eda684b0 100644 --- a/examples/cpp/Visualizer/visualizer_rgbd.cpp +++ b/examples/cpp/Visualizer/visualizer_rgbd.cpp @@ -47,9 +47,7 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - - rgbd->setGPUDevice(1); - rgbd->useGPU(); + stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); left->out.link(stereo->left); right->out.link(stereo->right); From 594343ef8c9169ebb66129b03615050c9bc5916f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 16:45:24 +0100 Subject: [PATCH 15/51] remove glslang tools from ci --- .github/workflows/python-main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index cb2878a8e..370abd88a 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -52,7 +52,7 @@ jobs: run: | sudo apt update python -m pip install --upgrade pip - sudo apt install libusb-1.0-0-dev pkg-config bison autoconf libtool libxi-dev libxtst-dev libxrandr-dev libx11-dev libxft-dev libxext-dev nasm flex libudev-dev glslang-tools + sudo apt install libusb-1.0-0-dev pkg-config bison autoconf libtool libxi-dev libxtst-dev libxrandr-dev libx11-dev libxft-dev libxext-dev nasm flex libudev-dev python -m pip install -r bindings/python/docs/requirements_mkdoc.txt - name: Configure project run: cmake -S . -B build -DDEPTHAI_BUILD_PYTHON=ON -DDEPTHAI_PYTHON_FORCE_DOCSTRINGS=ON -DDEPTHAI_BASALT_SUPPORT=ON -DDEPTHAI_PCL_SUPPORT=ON -DDEPTHAI_RTABMAP_SUPPORT=ON -DDEPTHAI_PYTHON_DOCSTRINGS_OUTPUT="$PWD/bindings/python/docstrings/depthai_python_docstring.hpp" From 1ccc2ee157cb29de99430b76fc679f393f3d369e Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 17:35:13 +0100 Subject: [PATCH 16/51] fix bindings --- CMakeLists.txt | 2 -- .../python/src/pipeline/node/RGBDBindings.cpp | 22 ++++++++++++------- examples/cpp/CMakeLists.txt | 2 +- examples/python/HostNodes/rgbd.py | 6 ++--- examples/python/Visualizer/visualizer_rgbd.py | 8 +++---- include/depthai/pipeline/node/host/RGBD.hpp | 1 + src/pipeline/node/host/RGBD.cpp | 12 +++++++--- 7 files changed, 31 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a808f8a7..1aa75e09e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -515,8 +515,6 @@ set_target_properties(${TARGET_CORE_NAME} PROPERTIES EXPORT_NAME ${TARGET_CORE_A # Add to list of targets to export and install list(APPEND targets_to_export ${TARGET_CORE_NAME}) - # target_include_directories(shader INTERFACE $) - if(DEPTHAI_ENABLE_KOMPUTE) target_link_libraries(${TARGET_CORE_NAME} PRIVATE shaders kompute::kompute) target_compile_definitions(${TARGET_CORE_NAME} PRIVATE DEPTHAI_ENABLE_KOMPUTE) diff --git a/bindings/python/src/pipeline/node/RGBDBindings.cpp b/bindings/python/src/pipeline/node/RGBDBindings.cpp index 7ad53e448..e412044d5 100644 --- a/bindings/python/src/pipeline/node/RGBDBindings.cpp +++ b/bindings/python/src/pipeline/node/RGBDBindings.cpp @@ -1,14 +1,14 @@ +#include + #include "Common.hpp" #include "NodeBindings.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/node/host/RGBD.hpp" -#include - extern py::handle daiNodeModule; -void bind_rgbd(pybind11::module& m, void* pCallstack){ +void bind_rgbd(pybind11::module& m, void* pCallstack) { using namespace dai; using namespace dai::node; @@ -19,7 +19,7 @@ void bind_rgbd(pybind11::module& m, void* pCallstack){ /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// // Call the rest of the type defines, then perform the actual bindings - Callstack* callstack = (Callstack*) pCallstack; + Callstack* callstack = (Callstack*)pCallstack; auto cb = callstack->top(); callstack->pop(); cb(m, pCallstack); @@ -30,11 +30,17 @@ void bind_rgbd(pybind11::module& m, void* pCallstack){ // RGBD Node rgbdNode - .def_property_readonly("inColor", [](RGBD& node){ return &node.inColor; }, py::return_value_policy::reference_internal) - .def_property_readonly("inDepth", [](RGBD& node){ return &node.inDepth; }, py::return_value_policy::reference_internal) + .def_property_readonly( + "inColor", [](RGBD& node) { return &node.inColor; }, py::return_value_policy::reference_internal) + .def_property_readonly( + "inDepth", [](RGBD& node) { return &node.inDepth; }, py::return_value_policy::reference_internal) .def_readonly("pcl", &RGBD::pcl, DOC(dai, node, RGBD, pcl)) - .def("build", &RGBD::build, DOC(dai, node, RGBD, build)) - .def("build", &RGBD::build, py::arg("autocreate"), py::arg("size"), DOC(dai, node, RGBD, build, 2)) + .def("build", static_cast (RGBD::*)()>(&RGBD::build), DOC(dai, node, RGBD, build, 1)) + .def("build", + static_cast (RGBD::*)(bool, std::pair)>(&RGBD::build), + py::arg("autocreate"), + py::arg("size"), + DOC(dai, node, RGBD, build, 2)) .def("setOutputMeters", &RGBD::setOutputMeters, py::arg("outputMeters"), DOC(dai, node, RGBD, setOutputMeters)) .def("useCPU", &RGBD::useCPU, DOC(dai, node, RGBD, useCPU)) .def("useCPUMT", &RGBD::useCPUMT, DOC(dai, node, RGBD, useCPUMT)) diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 42400fe42..b0ded2941 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -299,7 +299,7 @@ include(FetchContent) FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.1/rerun_cpp_sdk.zip) FetchContent_MakeAvailable(rerun_sdk) dai_add_example(rgbd HostNodes/rgbd.cpp OFF OFF) -target_link_libraries(rgbd PRIVATE rerun_sdk) +target_link_libraries(rgbd PRIVATE rerun_sdk) dai_add_example(visualizer_rgbd Visualizer/visualizer_rgbd.cpp OFF OFF) diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/HostNodes/rgbd.py index 9f5517b57..37e25aa6b 100644 --- a/examples/python/HostNodes/rgbd.py +++ b/examples/python/HostNodes/rgbd.py @@ -45,11 +45,11 @@ def run(self): right.setCamera("right") right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) right.setFps(fps) - out = color.requestOutput((1280,720)) + out = color.requestOutput((1280,720), dai.ImgFrame.Type.BGR888i) stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) - stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) + stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) @@ -58,9 +58,7 @@ def run(self): stereo.initialConfig.setLeftRightCheckThreshold(10) stereo.setSubpixel(True) - # Linking - left.out.link(stereo.left) right.out.link(stereo.right) stereo.depth.link(rgbd.inDepth) diff --git a/examples/python/Visualizer/visualizer_rgbd.py b/examples/python/Visualizer/visualizer_rgbd.py index 60ff55663..83552a6a3 100644 --- a/examples/python/Visualizer/visualizer_rgbd.py +++ b/examples/python/Visualizer/visualizer_rgbd.py @@ -9,7 +9,9 @@ args = parser.parse_args() with dai.Pipeline() as p: - remoteConnector = dai.RemoteConnection(webSocketPort=args.webSocketPort, httpPort=args.httpPort) + remoteConnector = dai.RemoteConnection( + webSocketPort=args.webSocketPort, httpPort=args.httpPort + ) fps = 30.0 left = p.create(dai.node.MonoCamera) right = p.create(dai.node.MonoCamera) @@ -23,7 +25,7 @@ right.setCamera("right") right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) right.setFps(fps) - out = color.requestOutput((1280,720)) + out = color.requestOutput((1280, 720)) stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) @@ -36,9 +38,7 @@ stereo.initialConfig.setLeftRightCheckThreshold(10) stereo.setSubpixel(True) - # Linking - left.out.link(stereo.left) right.out.link(stereo.right) stereo.depth.link(rgbd.inDepth) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 2bc00556e..0031ddadc 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -17,6 +17,7 @@ class RGBD : public NodeCRTP { constexpr static const char* NAME = "RGBD"; RGBD(); + ~RGBD(); Subnode sync{*this, "sync"}; std::shared_ptr align; InputMap& inputs = sync->inputs; diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 597902549..0127f0d37 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -11,8 +11,8 @@ #include "depthai/pipeline/node/Camera.hpp" #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai/pipeline/node/Sync.hpp" -#include "depthai/shaders/rgbd2pointcloud.hpp" #ifdef DEPTHAI_ENABLE_KOMPUTE + #include "depthai/shaders/rgbd2pointcloud.hpp" #include "kompute/Kompute.hpp" #endif #include "utility/PimplImpl.hpp" @@ -44,13 +44,17 @@ class RGBD::Impl { this->outputMeters = outputMeters; } void printDevices() { +#ifdef DEPTHAI_ENABLE_KOMPUTE auto devices = mgr->listDevices(); for(auto& device : devices) { std::cout << "Device: " << device.getProperties().deviceName << std::endl; } +#endif } void setGPUDevice(uint32_t deviceIndex) { +#ifdef DEPTHAI_ENABLE_KOMPUTE this->deviceIndex = deviceIndex; +#endif } void setCPUThreadNum(uint32_t numThreads) { threadNum = numThreads; @@ -83,7 +87,7 @@ class RGBD::Impl { shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); computeMethod = ComputeMethod::GPU; #else - loggerr::log->error("Kompute not enabled. Please enable it in CMake."); + throw std::runtime_error("Kompute not enabled in this build"); #endif } void computePointCloudGPU(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { @@ -209,7 +213,9 @@ class RGBD::Impl { }; RGBD::RGBD() = default; -; + +RGBD::~RGBD() = default; + std::shared_ptr RGBD::build() { sync->out.link(inSync); sync->setRunOnHost(false); From 899e8cff38d8d51bcf776afd7d26020970aeae43 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 18 Dec 2024 17:43:01 +0100 Subject: [PATCH 17/51] remove align from header --- include/depthai/pipeline/node/host/RGBD.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 0031ddadc..1739542f0 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -2,7 +2,6 @@ #include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" -#include "depthai/pipeline/node/ImageAlign.hpp" #include "depthai/pipeline/node/Sync.hpp" #include "depthai/utility/Pimpl.hpp" @@ -19,7 +18,6 @@ class RGBD : public NodeCRTP { RGBD(); ~RGBD(); Subnode sync{*this, "sync"}; - std::shared_ptr align; InputMap& inputs = sync->inputs; std::string colorInputName = "inColorSync"; From d1083b97e76f12cf12b6e2eab3f3b8d343da2e2f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 13:53:06 +0100 Subject: [PATCH 18/51] fix examples --- examples/cpp/HostNodes/rgbd.cpp | 6 +++--- examples/cpp/Visualizer/visualizer_rgbd.cpp | 2 +- examples/python/HostNodes/rgbd.py | 5 ++--- examples/python/Visualizer/visualizer_rgbd.py | 3 +-- src/pipeline/node/host/RGBD.cpp | 9 ++++++--- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp index e5cd1dfe3..b2e33fe8b 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -15,7 +15,6 @@ class RerunNode : public dai::NodeCRTP { const auto rec = rerun::RecordingStream("rerun"); rec.spawn().exit_on_failure(); rec.log_static("world", rerun::ViewCoordinates::FLU); - rec.log("world/ground", rerun::Boxes3D::from_half_sizes({{3.f, 3.f, 0.00001f}})); while(isRunning()) { auto pclIn = inputPCL.tryGet(); if(pclIn != nullptr) { @@ -47,7 +46,6 @@ int main() { auto rgbd = pipeline.create()->build(); auto color = pipeline.create(); auto rerun = pipeline.create(); - stereo->setExtendedDisparity(false); color->build(); left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); @@ -63,10 +61,12 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); + // stereo->initialConfig.postProcessing.thresholdFilter.maxRange = 10000; rgbd->setOutputMeters(true); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); + + out->link(stereo->inputAlignTo); left->out.link(stereo->left); right->out.link(stereo->right); diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/Visualizer/visualizer_rgbd.cpp index 2eda684b0..afbfdc14c 100644 --- a/examples/cpp/Visualizer/visualizer_rgbd.cpp +++ b/examples/cpp/Visualizer/visualizer_rgbd.cpp @@ -47,11 +47,11 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::CENTER); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); left->out.link(stereo->left); right->out.link(stereo->right); + out->link(stereo->inputAlignTo); stereo->depth.link(rgbd->inDepth); out->link(rgbd->inColor); diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/HostNodes/rgbd.py index 37e25aa6b..9b2b14237 100644 --- a/examples/python/HostNodes/rgbd.py +++ b/examples/python/HostNodes/rgbd.py @@ -45,11 +45,10 @@ def run(self): right.setCamera("right") right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) right.setFps(fps) - out = color.requestOutput((1280,720), dai.ImgFrame.Type.BGR888i) + out = color.requestOutput((1280,720), dai.ImgFrame.Type.RGB888i) - stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) - stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) + out.link(stereo.inputAlignTo) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) diff --git a/examples/python/Visualizer/visualizer_rgbd.py b/examples/python/Visualizer/visualizer_rgbd.py index 83552a6a3..5a0d10a48 100644 --- a/examples/python/Visualizer/visualizer_rgbd.py +++ b/examples/python/Visualizer/visualizer_rgbd.py @@ -27,9 +27,8 @@ right.setFps(fps) out = color.requestOutput((1280, 720)) - stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A) - stereo.setOutputSize(left.getResolutionWidth(), left.getResolutionHeight()) + out.link(stereo.inputAlignTo) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 0127f0d37..469d58f42 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -151,9 +151,9 @@ class RGBD::Impl { float z = static_cast(depthValue) * scale; x = (x - cx) * z / fx; y = (y - cy) * z / fy; - uint8_t r = colorData[i * 3 + 0]; - uint8_t b = colorData[i * 3 + 1]; - uint8_t g = colorData[i * 3 + 2]; + uint8_t r = static_cast(colorData[i * 3 + 0]); + uint8_t g = static_cast(colorData[i * 3 + 1]); + uint8_t b = static_cast(colorData[i * 3 + 2]); Point3fRGB p; p.x = x; p.y = y; @@ -275,6 +275,9 @@ void RGBD::run() { initialize(imgFrames); } auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColor.getName())); + if(colorFrame->getType() != dai::ImgFrame::Type::RGB888i) { + throw std::runtime_error("RGBD node only supports RGB888i frames"); + } auto depthFrame = std::dynamic_pointer_cast(group->group.at(inDepth.getName())); // Create the point cloud From 0eba35b40f90126fe479e7f12bc3c7cdd67917ee Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 13:53:12 +0100 Subject: [PATCH 19/51] add test --- tests/CMakeLists.txt | 4 ++ tests/src/ondevice_tests/rgbd_test.cpp | 54 ++++++++++++++++++++++++++ 2 files changed, 58 insertions(+) create mode 100644 tests/src/ondevice_tests/rgbd_test.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 8b5481734..78f51beed 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -339,6 +339,10 @@ dai_set_test_labels(message_group_frame_test ondevice rvc2_all rvc4 ci) dai_add_test(pointcloud_test src/ondevice_tests/pointcloud_test.cpp) dai_set_test_labels(pointcloud_test ondevice rvc2_all ci) +# RGBD test +dai_add_test(rgbd_test src/ondevice_tests/rgbd_test.cpp) +dai_set_test_labels(rgbd_test ondevice rvc2_all ci) + # Resolutions test dai_add_test(resolutions_test src/ondevice_tests/resolutions_test.cpp) dai_set_test_labels(resolutions_test ondevice) # TODO(jakob) Make the test runnable in CI diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp new file mode 100644 index 000000000..53e4afdc0 --- /dev/null +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -0,0 +1,54 @@ +#include +#include +#include + +TEST_CASE("basic rgbd") { + // Create pipeline + dai::Pipeline pipeline; + // Define sources and outputs + int fps = 30; + // Define sources and outputs + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + auto rgbd = pipeline.create()->build(); + auto color = pipeline.create(); + stereo->setExtendedDisparity(false); + color->build(); + + left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + left->setCamera("left"); + left->setFps(fps); + right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + right->setCamera("right"); + right->setFps(fps); + stereo->setSubpixel(true); + stereo->setExtendedDisparity(false); + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setLeftRightCheck(true); + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->enableDistortionCorrection(true); + stereo->initialConfig.setLeftRightCheckThreshold(10); + + auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); + out->link(stereo->inputAlignTo); + left->out.link(stereo->left); + right->out.link(stereo->right); + + stereo->depth.link(rgbd->inDepth); + out->link(rgbd->inColor); + + auto outQ = rgbd->pcl.createOutputQueue(); + pipeline.start(); + for(int i = 0; i < 10; ++i) { + auto pcl = outQ->get(); + REQUIRE(pcl != nullptr); + REQUIRE(pcl->getWidth() == 1280); + REQUIRE(pcl->getHeight() == 720); + REQUIRE(pcl->getPoints().size() == 1280UL * 720UL); + REQUIRE(pcl->isColor() == true); + REQUIRE(pcl->getMinX() <= pcl->getMaxX()); + REQUIRE(pcl->getMinY() <= pcl->getMaxY()); + REQUIRE(pcl->getMinZ() <= pcl->getMaxZ()); + } +} From d2f45f7aef9e97061a2f05e35ee117328361669d Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 14:04:11 +0100 Subject: [PATCH 20/51] add autocreate test --- tests/src/ondevice_tests/rgbd_test.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp index 53e4afdc0..6b250a86a 100644 --- a/tests/src/ondevice_tests/rgbd_test.cpp +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -52,3 +52,22 @@ TEST_CASE("basic rgbd") { REQUIRE(pcl->getMinZ() <= pcl->getMaxZ()); } } +TEST_CASE("rgbd autocreate") { + // Create pipeline + dai::Pipeline pipeline; + auto rgbd = pipeline.create()->build(true, {1280, 720}); + + auto outQ = rgbd->pcl.createOutputQueue(); + pipeline.start(); + for(int i = 0; i < 10; ++i) { + auto pcl = outQ->get(); + REQUIRE(pcl != nullptr); + REQUIRE(pcl->getWidth() == 1280); + REQUIRE(pcl->getHeight() == 720); + REQUIRE(pcl->getPoints().size() == 1280UL * 720UL); + REQUIRE(pcl->isColor() == true); + REQUIRE(pcl->getMinX() <= pcl->getMaxX()); + REQUIRE(pcl->getMinY() <= pcl->getMaxY()); + REQUIRE(pcl->getMinZ() <= pcl->getMaxZ()); + } +} From dd463544ba013f43237abee8c95f27790e57cc0d Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 14:04:18 +0100 Subject: [PATCH 21/51] add docstrings --- include/depthai/pipeline/node/host/RGBD.hpp | 23 +++++++++++++++++++++ src/pipeline/node/host/RGBD.cpp | 1 + 2 files changed, 24 insertions(+) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 1739542f0..cc58d1f7c 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -31,13 +31,36 @@ class RGBD : public NodeCRTP { Output pcl{*this, {"pcl", DEFAULT_GROUP, {{DatatypeEnum::PointCloudData, true}}}}; std::shared_ptr build(); + /** + * @brief Build RGBD node with specified size + * @param autocreate If true, will create color and depth frames if they don't exist + * @param size Size of the frames + */ std::shared_ptr build(bool autocreate, std::pair size); void setOutputMeters(bool outputMeters); + /** + * @brief Use single-threaded CPU for processing + */ void useCPU(); + /** + * @brief Use multi-threaded CPU for processing + */ void useCPUMT(); + /** + * @brief Use GPU for processing (needs to be compiled with Kompute support) + */ void useGPU(); + /** + * @brief Set GPU device index + */ void setGPUDevice(uint32_t deviceIndex); + /** + * @brief Set number of threads for CPU processing + */ void setCPUThreadNum(uint32_t numThreads); + /** + * @brief Print available GPU devices + */ void printDevices(); private: class Impl; diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 469d58f42..b382ef12a 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -237,6 +237,7 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { auto depth = pipeline.create()->build(true); auto* out = colorCam->requestOutput(size, dai::ImgFrame::Type::RGB888i); out->link(inColor); + out->link(depth->inputAlignTo); depth->depth.link(inDepth); return build(); } From d3ef82b7c70a7c6a9a9d203d4ddb2312aad1ad65 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 14:54:29 +0100 Subject: [PATCH 22/51] fix docstrings --- .../python/src/pipeline/node/RGBDBindings.cpp | 8 ++-- examples/cpp/HostNodes/rgbd.cpp | 1 - include/depthai/pipeline/node/host/RGBD.hpp | 14 ++----- src/pipeline/node/host/RGBD.cpp | 37 ++++++------------- 4 files changed, 19 insertions(+), 41 deletions(-) diff --git a/bindings/python/src/pipeline/node/RGBDBindings.cpp b/bindings/python/src/pipeline/node/RGBDBindings.cpp index e412044d5..04cdebd72 100644 --- a/bindings/python/src/pipeline/node/RGBDBindings.cpp +++ b/bindings/python/src/pipeline/node/RGBDBindings.cpp @@ -35,7 +35,7 @@ void bind_rgbd(pybind11::module& m, void* pCallstack) { .def_property_readonly( "inDepth", [](RGBD& node) { return &node.inDepth; }, py::return_value_policy::reference_internal) .def_readonly("pcl", &RGBD::pcl, DOC(dai, node, RGBD, pcl)) - .def("build", static_cast (RGBD::*)()>(&RGBD::build), DOC(dai, node, RGBD, build, 1)) + .def("build", static_cast (RGBD::*)()>(&RGBD::build)) .def("build", static_cast (RGBD::*)(bool, std::pair)>(&RGBD::build), py::arg("autocreate"), @@ -43,9 +43,7 @@ void bind_rgbd(pybind11::module& m, void* pCallstack) { DOC(dai, node, RGBD, build, 2)) .def("setOutputMeters", &RGBD::setOutputMeters, py::arg("outputMeters"), DOC(dai, node, RGBD, setOutputMeters)) .def("useCPU", &RGBD::useCPU, DOC(dai, node, RGBD, useCPU)) - .def("useCPUMT", &RGBD::useCPUMT, DOC(dai, node, RGBD, useCPUMT)) - .def("useGPU", &RGBD::useGPU, DOC(dai, node, RGBD, useGPU)) - .def("setGPUDevice", &RGBD::setGPUDevice, py::arg("deviceIndex"), DOC(dai, node, RGBD, setGPUDevice)) - .def("setCPUThreadNum", &RGBD::setCPUThreadNum, py::arg("numThreads"), DOC(dai, node, RGBD, setCPUThreadNum)) + .def("useCPUMT", &RGBD::useCPUMT, py::arg("numThreads") = 2, DOC(dai, node, RGBD, useCPUMT)) + .def("useGPU", &RGBD::useGPU, py::arg("device") = 0, DOC(dai, node, RGBD, useGPU)) .def("printDevices", &RGBD::printDevices, DOC(dai, node, RGBD, printDevices)); } diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp index b2e33fe8b..5013c78d0 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -61,7 +61,6 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - // stereo->initialConfig.postProcessing.thresholdFilter.maxRange = 10000; rgbd->setOutputMeters(true); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index cc58d1f7c..92c0e41dc 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -44,20 +44,14 @@ class RGBD : public NodeCRTP { void useCPU(); /** * @brief Use multi-threaded CPU for processing + * @param numThreads Number of threads to use */ - void useCPUMT(); + void useCPUMT(uint32_t numThreads=2); /** * @brief Use GPU for processing (needs to be compiled with Kompute support) + * @param device GPU device index */ - void useGPU(); - /** - * @brief Set GPU device index - */ - void setGPUDevice(uint32_t deviceIndex); - /** - * @brief Set number of threads for CPU processing - */ - void setCPUThreadNum(uint32_t numThreads); + void useGPU(uint32_t device=0); /** * @brief Print available GPU devices */ diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index b382ef12a..4cc272a69 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -51,22 +51,16 @@ class RGBD::Impl { } #endif } - void setGPUDevice(uint32_t deviceIndex) { -#ifdef DEPTHAI_ENABLE_KOMPUTE - this->deviceIndex = deviceIndex; -#endif - } - void setCPUThreadNum(uint32_t numThreads) { - threadNum = numThreads; - } void useCPU() { computeMethod = ComputeMethod::CPU; } - void useCPUMT() { + void useCPUMT(uint32_t numThreads) { + threadNum = numThreads; computeMethod = ComputeMethod::CPU_MT; } - void useGPU() { - initializeGPU(); + void useGPU(uint32_t device) { + + initializeGPU(device); } void setIntrinsics(float fx, float fy, float cx, float cy, unsigned int width, unsigned int height) { this->fx = fx; @@ -80,10 +74,10 @@ class RGBD::Impl { } private: - void initializeGPU() { + void initializeGPU(uint32_t device) { #ifdef DEPTHAI_ENABLE_KOMPUTE // Initialize Kompute - mgr = std::make_shared(deviceIndex); + mgr = std::make_shared(device); shader = std::vector(shaders::RGBD2POINTCLOUD_COMP_SPV.begin(), shaders::RGBD2POINTCLOUD_COMP_SPV.end()); computeMethod = ComputeMethod::GPU; #else @@ -146,7 +140,7 @@ class RGBD::Impl { float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; for(int i = startRow * width; i < endRow * width; i++) { float x = i % width; - float y = i / width; + float y = static_cast(i) / width; uint16_t depthValue = *(reinterpret_cast(depthData + i * 2)); float z = static_cast(depthValue) * scale; x = (x - cx) * z / fx; @@ -193,7 +187,6 @@ class RGBD::Impl { ComputeMethod computeMethod = ComputeMethod::CPU; #ifdef DEPTHAI_ENABLE_KOMPUTE std::shared_ptr mgr; - uint32_t deviceIndex = 0; std::vector shader; std::shared_ptr algo; std::shared_ptr depthTensor; @@ -308,17 +301,11 @@ void RGBD::setOutputMeters(bool outputMeters) { void RGBD::useCPU() { pimpl->useCPU(); } -void RGBD::useCPUMT() { - pimpl->useCPUMT(); -} -void RGBD::useGPU() { - pimpl->useGPU(); -} -void RGBD::setGPUDevice(uint32_t deviceIndex) { - pimpl->setGPUDevice(deviceIndex); +void RGBD::useCPUMT(uint32_t numThreads) { + pimpl->useCPUMT(numThreads); } -void RGBD::setCPUThreadNum(uint32_t numThreads) { - pimpl->setCPUThreadNum(numThreads); +void RGBD::useGPU(uint32_t device) { + pimpl->useGPU(device); } void RGBD::printDevices() { pimpl->printDevices(); From fa935ef970a281193781e7c6aa40f789f6609982 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 15:12:18 +0100 Subject: [PATCH 23/51] try compiling shaders --- CMakeLists.txt | 2 +- vcpkg.json | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bc28419ce..b8904594f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ option(DEPTHAI_ENABLE_PROTOBUF "Enable Protobuf support" ON) option(DEPTHAI_BUILD_PYTHON "Build python bindings" OFF) option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) -OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" OFF) +OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" ON) option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BOOTSTRAP_VCPKG "Automatically bootstrap VCPKG" ON) option(DEPTHAI_MERGED_TARGET "Enable merged target build" ON) diff --git a/vcpkg.json b/vcpkg.json index 7fe0d6eff..245d0962b 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -73,7 +73,14 @@ "kompute-support": { "description": "Enable Kompute support", "dependencies": [ - "kompute" + "kompute", + { + "name": "glslang", + "default-features": false, + "features": [ + "tools" + ] + } ] }, "protobuf-support": { From a9878ba707afdd2f4df72439c168b75af75158fa Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 19 Dec 2024 17:44:00 +0100 Subject: [PATCH 24/51] use blocking get --- examples/cpp/HostNodes/rgbd.cpp | 3 ++- examples/python/HostNodes/rgbd.py | 2 +- src/pipeline/node/host/RGBD.cpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp index 5013c78d0..53365b2a8 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -16,7 +16,7 @@ class RerunNode : public dai::NodeCRTP { rec.spawn().exit_on_failure(); rec.log_static("world", rerun::ViewCoordinates::FLU); while(isRunning()) { - auto pclIn = inputPCL.tryGet(); + auto pclIn = inputPCL.get(); if(pclIn != nullptr) { std::vector points; std::vector colors; @@ -61,6 +61,7 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); + stereo->initialConfig.postProcessing.thresholdFilter.maxRange = 10000; rgbd->setOutputMeters(true); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/HostNodes/rgbd.py index 9b2b14237..dde2b94bd 100644 --- a/examples/python/HostNodes/rgbd.py +++ b/examples/python/HostNodes/rgbd.py @@ -22,7 +22,7 @@ def run(self): rr.log("world", rr.ViewCoordinates.FLU) rr.log("world/ground", rr.Boxes3D(half_sizes=[3.0, 3.0, 0.00001])) while self.isRunning(): - pclObstData = self.inputPCL.tryGet() + pclObstData = self.inputPCL.get() if pclObstData is not None: points, colors = pclObstData.getPointsRGB() rr.log("world/pcl", rr.Points3D(points, colors=colors, radii=[0.01])) diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 4cc272a69..7b357868d 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -258,7 +258,7 @@ void RGBD::initialize(std::vector> frames) { void RGBD::run() { while(isRunning()) { // Get the color and depth frames - auto group = inSync.tryGet(); + auto group = inSync.get(); if(group == nullptr) continue; if(!initialized) { std::vector> imgFrames; From 893f3c1bb2277cacd592e68521bbe3af4e626553 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 20 Dec 2024 11:49:47 +0100 Subject: [PATCH 25/51] better performance on cpuMt --- src/pipeline/node/host/RGBD.cpp | 100 +++++++++++++++++++------------- 1 file changed, 61 insertions(+), 39 deletions(-) diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 7b357868d..6a1050d7c 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -136,53 +136,76 @@ class RGBD::Impl { } #endif } - void calcPoints(const uint8_t* depthData, const uint8_t* colorData, std::vector& points, int startRow, int endRow) { - float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; - for(int i = startRow * width; i < endRow * width; i++) { - float x = i % width; - float y = static_cast(i) / width; + void calcPointsChunk( + const uint8_t* depthData, + const uint8_t* colorData, + std::vector& outChunk, + int startRow, int endRow +) { + float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; + outChunk.reserve((endRow - startRow) * width); + + for (int row = startRow; row < endRow; row++) { + int rowStart = row * width; + for (int col = 0; col < width; col++) { + size_t i = rowStart + col; + uint16_t depthValue = *(reinterpret_cast(depthData + i * 2)); float z = static_cast(depthValue) * scale; - x = (x - cx) * z / fx; - y = (y - cy) * z / fy; - uint8_t r = static_cast(colorData[i * 3 + 0]); - uint8_t g = static_cast(colorData[i * 3 + 1]); - uint8_t b = static_cast(colorData[i * 3 + 2]); - Point3fRGB p; - p.x = x; - p.y = y; - p.z = z; - p.r = r; - p.g = g; - p.b = b; - std::lock_guard lock(pointsMtx); - points.emplace_back(p); + + float xCoord = (col - cx) * z / fx; + float yCoord = (row - cy) * z / fy; + + // BGR order in colorData + uint8_t r = colorData[i * 3 + 0]; + uint8_t g = colorData[i * 3 + 1]; + uint8_t b = colorData[i * 3 + 2]; + + outChunk.push_back(Point3fRGB{xCoord, yCoord, z, r, g, b}); } } - void computePointCloudCPU(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { - calcPoints(depthData, colorData, points, 0, height); - } - void computePointCloudCPUMT(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { - // Lambda function for processing a block of rows - auto processRows = [&](int startRow, int endRow) { calcPoints(depthData, colorData, points, startRow, endRow); }; +} - // Divide rows into chunks and process in parallel - const int numThreads = threadNum; - const int rowsPerThread = height / numThreads; - std::vector> futures; +void computePointCloudCPU( + const uint8_t* depthData, + const uint8_t* colorData, + std::vector& points +) { - for(int t = 0; t < numThreads; ++t) { - int startRow = t * rowsPerThread; - int endRow = (t == numThreads - 1) ? height : startRow + rowsPerThread; + // Single-threaded directly writes into points + calcPointsChunk(depthData, colorData, points, 0, height); +} - futures.emplace_back(std::async(std::launch::async, processRows, startRow, endRow)); - } +void computePointCloudCPUMT( + const uint8_t* depthData, + const uint8_t* colorData, + std::vector& points +) { - // Wait for all threads to finish - for(auto& future : futures) { - future.get(); - } + int rowsPerThread = height / threadNum; + std::vector>> futures; + + // Each thread returns a local vector + auto processRows = [&](int startRow, int endRow) { + std::vector localPoints; + calcPointsChunk(depthData, colorData, localPoints, startRow, endRow); + return localPoints; + }; + + for (int t = 0; t < threadNum; ++t) { + int startRow = t * rowsPerThread; + int endRow = (t == threadNum - 1) ? height : (startRow + rowsPerThread); + futures.emplace_back(std::async(std::launch::async, processRows, startRow, endRow)); + } + + // Merge all results + for (auto& f : futures) { + auto localPoints = f.get(); + // Now we do one lock per merge if needed + // If this is not called concurrently from multiple places, no lock needed + points.insert(points.end(), localPoints.begin(), localPoints.end()); } +} enum class ComputeMethod { CPU, CPU_MT, GPU }; ComputeMethod computeMethod = ComputeMethod::CPU; #ifdef DEPTHAI_ENABLE_KOMPUTE @@ -201,7 +224,6 @@ class RGBD::Impl { int width, height; int size; bool intrinsicsSet = false; - std::mutex pointsMtx; int threadNum = 2; }; From 9d5fda82e42e619e11c0c697b2088dc3ae4e5afd Mon Sep 17 00:00:00 2001 From: Serafadam Date: Fri, 20 Dec 2024 17:20:09 +0100 Subject: [PATCH 26/51] debug arm --- cmake/triplets/arm64-linux.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/triplets/arm64-linux.cmake b/cmake/triplets/arm64-linux.cmake index 1553d02eb..5b172f655 100644 --- a/cmake/triplets/arm64-linux.cmake +++ b/cmake/triplets/arm64-linux.cmake @@ -2,6 +2,7 @@ set(VCPKG_TARGET_ARCHITECTURE arm64) set(VCPKG_CRT_LINKAGE dynamic) set(VCPKG_LIBRARY_LINKAGE static) set(VCPKG_BUILD_TYPE release) +set(PORT_DEBUG ON) if(PORT MATCHES "libusb|ffmpeg") set(VCPKG_LIBRARY_LINKAGE dynamic) From b69ca759350cd260d059ebafba8ded1868188793 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 30 Dec 2024 11:04:53 +0100 Subject: [PATCH 27/51] kompute support off --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b8904594f..bc28419ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ option(DEPTHAI_ENABLE_PROTOBUF "Enable Protobuf support" ON) option(DEPTHAI_BUILD_PYTHON "Build python bindings" OFF) option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) -OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" ON) +OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" OFF) option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BOOTSTRAP_VCPKG "Automatically bootstrap VCPKG" ON) option(DEPTHAI_MERGED_TARGET "Enable merged target build" ON) From 9e2965e0806ff9f030142a1419163c964f01d72b Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 7 Jan 2025 09:12:06 +0100 Subject: [PATCH 28/51] try printing logs for kompute --- .github/workflows/python-main.yml | 24 ++++++++++++++++++++++++ CMakeLists.txt | 2 +- 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 370abd88a..9cfba6264 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -534,6 +534,30 @@ jobs: - name: Building wheels run: | cd bindings/python && for PYBIN in /opt/python/${{ matrix.python-set }}/bin; do "${PYBIN}/pip" wheel . -w ./wheelhouse/ --verbose; done + - name: Print logs for the failed port(s) only + if: failure() # Only run this if the build step fails + run: | + echo "Searching vcpkg-manifest-install.log for failing port logs..." + LOGFILE=$(find . -name "vcpkg-manifest-install.log" | head -n 1) + if [ -z "$LOGFILE" ]; then + echo "No vcpkg-manifest-install.log found!" + exit 1 + fi + + # 1) Grab lines following "See logs for more information:" + # 2) From those lines, extract only the file paths ending in ".log" + # 3) Remove any leading spaces + grep -A8 "See logs for more information:" $LOGFILE \ + | grep "\.log" \ + | sed 's/^[[:space:]]*//' \ + > failed_logs.txt + + # Now read each log path we found, and print it + while IFS= read -r log; do + echo "==== Showing log: $log ====" + cat "$log" || true + echo + done < failed_logs.txt - name: Auditing wheels run: cd bindings/python && for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat $PLAT -w wheelhouse/audited/; done - name: Archive wheel artifacts diff --git a/CMakeLists.txt b/CMakeLists.txt index bc28419ce..b8904594f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ option(DEPTHAI_ENABLE_PROTOBUF "Enable Protobuf support" ON) option(DEPTHAI_BUILD_PYTHON "Build python bindings" OFF) option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) -OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" OFF) +OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" ON) option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BOOTSTRAP_VCPKG "Automatically bootstrap VCPKG" ON) option(DEPTHAI_MERGED_TARGET "Enable merged target build" ON) From 812a0048987bdbd39503d0ec3ce7a7ef1d158a3c Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 7 Jan 2025 10:24:35 +0100 Subject: [PATCH 29/51] install additional x deps for arm --- .github/workflows/python-main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 9cfba6264..c66372f78 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -505,7 +505,7 @@ jobs: with: submodules: 'recursive' - name: Installing libusb1-devel dependency - run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake + run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig xcb-proto xorg-x11-proto-devel wayland-devel wayland-protocols-devel - name: Installing cmake dependency run: | cd bindings/python From 74babb48c3cbd40a9cb44644f8b67c6505a90a74 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 7 Jan 2025 11:14:00 +0100 Subject: [PATCH 30/51] try debugging pkgconfig --- cmake/triplets/arm64-linux.cmake | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/cmake/triplets/arm64-linux.cmake b/cmake/triplets/arm64-linux.cmake index 5b172f655..2c480f2e4 100644 --- a/cmake/triplets/arm64-linux.cmake +++ b/cmake/triplets/arm64-linux.cmake @@ -8,5 +8,10 @@ if(PORT MATCHES "libusb|ffmpeg") set(VCPKG_LIBRARY_LINKAGE dynamic) set(VCPKG_FIXUP_ELF_RPATH ON) endif() +if(PORT MATCHES "vulkan-loader") + # set env variable for pkg config + set(ENV{PKG_CONFIG_DEBUG_SPEW} 1) + set(CMAKE_FIND_DEBUG_MODE ON) +endif() set(VCPKG_CMAKE_SYSTEM_NAME Linux) From 2b2ee641ddef79d7c464d08e922626bc2206d281 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 7 Jan 2025 13:46:47 +0100 Subject: [PATCH 31/51] install xcb-devel --- .github/workflows/python-main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index c66372f78..04c6ed85c 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -505,7 +505,7 @@ jobs: with: submodules: 'recursive' - name: Installing libusb1-devel dependency - run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig xcb-proto xorg-x11-proto-devel wayland-devel wayland-protocols-devel + run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig - name: Installing cmake dependency run: | cd bindings/python From 6a449d70118673b7a8f2b3525e2e3c851aa0b2dc Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 7 Jan 2025 14:08:22 +0100 Subject: [PATCH 32/51] more logs --- .github/workflows/python-main.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 04c6ed85c..c41cffd50 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -557,7 +557,25 @@ jobs: echo "==== Showing log: $log ====" cat "$log" || true echo + + # Also show CMakeOutput.log (and/or CMakeError.log) within the same port’s directory + # Often it’s in the same or neighboring folder (e.g. arm64-linux-rel/CMakeFiles/CMakeOutput.log). + # We'll "walk up" one directory and search for CMakeOutput.log in CMakeFiles/. + port_dir="$(dirname "$log")" + # In some cases (e.g. config-arm64-linux-out.log), you may need to go up another level: + # port_dir="$(dirname "$port_dir")" + + # We'll now look for any CMakeOutput.log within this port’s subdirectories + found_outputs=$(find "$port_dir" -name "CMakeOutput.log" -o -name "CMakeError.log" -print 2>/dev/null) + if [ -n "$found_outputs" ]; then + for cof in $found_outputs; do + echo "==== Showing $cof ====" + cat "$cof" || true + echo + done + fi done < failed_logs.txt + - name: Auditing wheels run: cd bindings/python && for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat $PLAT -w wheelhouse/audited/; done - name: Archive wheel artifacts From 1cbe1eea9a0f0d9e12c42207f6c3740fa01e6378 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 8 Jan 2025 07:59:20 +0100 Subject: [PATCH 33/51] check pkgconfig --- .github/workflows/python-main.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index c41cffd50..78788256a 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -511,6 +511,11 @@ jobs: cd bindings/python /opt/python/cp38-cp38/bin/python3.8 -m pip install cmake ln -s /opt/python/cp38-cp38/bin/cmake /bin/ + - name: Check pkgconfig for XCB, X11 and Wayland + run: | + pkg-config --exists xcb && echo "xcb found" || echo "xcb missing" + pkg-config --exists x11 && echo "x11 found" || echo "x11 missing" + pkg-config --exists wayland-client && echo "wayland-client found" || echo "wayland-client missing" - name: Setup ninja required for arm64 builds run: | git clone https://github.com/ninja-build/ninja.git From 391a6ed324548e15fdd1352700436d8b94a86f0f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 8 Jan 2025 09:13:40 +0100 Subject: [PATCH 34/51] pkg config path --- .github/workflows/python-main.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 78788256a..12d32a78d 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -513,6 +513,7 @@ jobs: ln -s /opt/python/cp38-cp38/bin/cmake /bin/ - name: Check pkgconfig for XCB, X11 and Wayland run: | + pkg-config --variable pc_path pkg-config pkg-config --exists xcb && echo "xcb found" || echo "xcb missing" pkg-config --exists x11 && echo "x11 found" || echo "x11 missing" pkg-config --exists wayland-client && echo "wayland-client found" || echo "wayland-client missing" From 538e03a3d365175e99595e4c4cbbf9d765cde605 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 8 Jan 2025 10:40:08 +0100 Subject: [PATCH 35/51] remove cmake install from arm --- .github/workflows/python-main.yml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 12d32a78d..6105ff25d 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -506,11 +506,6 @@ jobs: submodules: 'recursive' - name: Installing libusb1-devel dependency run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig - - name: Installing cmake dependency - run: | - cd bindings/python - /opt/python/cp38-cp38/bin/python3.8 -m pip install cmake - ln -s /opt/python/cp38-cp38/bin/cmake /bin/ - name: Check pkgconfig for XCB, X11 and Wayland run: | pkg-config --variable pc_path pkg-config From 8da3fd6a6d87b1a2b460779647b5a749e3cca03f Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 8 Jan 2025 10:56:19 +0100 Subject: [PATCH 36/51] set pkgconfig path --- .github/workflows/python-main.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 6105ff25d..87feaba71 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -494,6 +494,7 @@ jobs: DEPTHAI_BUILD_PCL: ON DEPTHAI_BUILD_RTABMAP: ON VCPKG_MAX_CONCURRENCY: "2" + PKG_CONFIG_PATH: /usr/local/lib/pkgconfig steps: - name: Export GitHub Actions cache environment variables uses: actions/github-script@v7 @@ -548,7 +549,7 @@ jobs: # 1) Grab lines following "See logs for more information:" # 2) From those lines, extract only the file paths ending in ".log" # 3) Remove any leading spaces - grep -A8 "See logs for more information:" $LOGFILE \ + grep -A50 "See logs for more information:" $LOGFILE \ | grep "\.log" \ | sed 's/^[[:space:]]*//' \ > failed_logs.txt From 0578e270ff9157bfb6f8f85c8d05da98d05b9f94 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 8 Jan 2025 11:20:31 +0100 Subject: [PATCH 37/51] set pkgconfig executable directly --- cmake/triplets/arm64-linux.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/triplets/arm64-linux.cmake b/cmake/triplets/arm64-linux.cmake index 2c480f2e4..36dda8843 100644 --- a/cmake/triplets/arm64-linux.cmake +++ b/cmake/triplets/arm64-linux.cmake @@ -12,6 +12,7 @@ if(PORT MATCHES "vulkan-loader") # set env variable for pkg config set(ENV{PKG_CONFIG_DEBUG_SPEW} 1) set(CMAKE_FIND_DEBUG_MODE ON) + set(PKG_CONFIG_EXECUTABLE /usr/bin/pkg-config) endif() set(VCPKG_CMAKE_SYSTEM_NAME Linux) From de4e727e3ef554ad2fd6f71756b6bd348b823524 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 8 Jan 2025 14:19:06 +0100 Subject: [PATCH 38/51] try setting via other way --- cmake/triplets/arm64-linux.cmake | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/triplets/arm64-linux.cmake b/cmake/triplets/arm64-linux.cmake index 36dda8843..29bf061af 100644 --- a/cmake/triplets/arm64-linux.cmake +++ b/cmake/triplets/arm64-linux.cmake @@ -12,7 +12,8 @@ if(PORT MATCHES "vulkan-loader") # set env variable for pkg config set(ENV{PKG_CONFIG_DEBUG_SPEW} 1) set(CMAKE_FIND_DEBUG_MODE ON) - set(PKG_CONFIG_EXECUTABLE /usr/bin/pkg-config) + set(VCPKG_CMAKE_CONFIGURE_OPTIONS "-DPKG_CONFIG_EXECUTABLE=/usr/bin/pkg-config") + endif() set(VCPKG_CMAKE_SYSTEM_NAME Linux) From 72d519c8c12d402d4badad8abcab9e4e3288d387 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 9 Jan 2025 08:17:41 +0100 Subject: [PATCH 39/51] debug cleanup --- .github/workflows/python-main.yml | 70 +++++++-------------------- bindings/python/ci/show_vcpkg_logs.sh | 51 +++++++++++++++++++ cmake/triplets/arm64-linux.cmake | 4 +- 3 files changed, 70 insertions(+), 55 deletions(-) create mode 100644 bindings/python/ci/show_vcpkg_logs.sh diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 87feaba71..91f3d9df3 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -58,6 +58,10 @@ jobs: run: cmake -S . -B build -DDEPTHAI_BUILD_PYTHON=ON -DDEPTHAI_PYTHON_FORCE_DOCSTRINGS=ON -DDEPTHAI_BASALT_SUPPORT=ON -DDEPTHAI_PCL_SUPPORT=ON -DDEPTHAI_RTABMAP_SUPPORT=ON -DDEPTHAI_PYTHON_DOCSTRINGS_OUTPUT="$PWD/bindings/python/docstrings/depthai_python_docstring.hpp" - name: Build target 'pybind11_mkdoc' run: cmake --build build --target pybind11_mkdoc --parallel 4 + - name: Print out vcpkg logs if building port fails + if: failure() # Only run this if the build step fails + run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh + - name: Upload docstring artifacts uses: actions/upload-artifact@v3 with: @@ -140,6 +144,9 @@ jobs: run: | cmake -S . -B build -DDEPTHAI_BUILD_PYTHON=ON -D CMAKE_BUILD_TYPE=Release -D DEPTHAI_PYTHON_DOCSTRINGS_INPUT=$PWD/bindings/python/docstrings/depthai_python_docstring.hpp -D DEPTHAI_PYTHON_ENABLE_TESTS=ON cmake --build build --parallel 4 + - name: Print out vcpkg logs if building port fails + if: failure() # Only run this if the build step fails + run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh - name: Test run: | cmake --build build --target pytest --config Release @@ -247,6 +254,9 @@ jobs: python -m pip install --upgrade pip - name: Building wheels run: cd bindings/python && python -m pip wheel . -w ./wheelhouse/audited/ --verbose + - name: Print out vcpkg logs if building port fails + if: failure() # Only run this if the build step fails + run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh - name: Archive wheel artifacts uses: actions/upload-artifact@v3 with: @@ -317,6 +327,9 @@ jobs: python -m pip install delocate - name: Building wheels run: cd bindings/python && python -m pip wheel . -w ./wheelhouse/ --verbose + - name: Print out vcpkg logs if building port fails + if: failure() # Only run this if the build step fails + run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh - name: Auditing wheels run: cd bindings/python && ci/repair-whl-macos.sh `pwd`/wheelhouse/* `pwd`/wheelhouse/audited - name: Archive wheel artifacts @@ -428,11 +441,6 @@ jobs: submodules: 'recursive' - name: Installing libusb1-devel dependency run: yum install -y libusb1-devel perl-core curl zip unzip tar ninja-build zlib-devel curl-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake - - name: Installing cmake dependency - run: | - cd bindings/python - /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: cd bindings/python && mkdir -p wheelhouse/audited/ @@ -455,6 +463,9 @@ jobs: - name: Build wheels run: | cd bindings/python && for PYBIN in /opt/python/${{ matrix.python-set }}/bin; do "${PYBIN}/pip" wheel . -w ./wheelhouse/ --verbose; done + - name: Print out vcpkg logs if building port fails + if: failure() # Only run this if the build step fails + run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh - name: Audit wheels run: cd bindings/python && for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat $PLAT -w wheelhouse/audited/; done @@ -494,7 +505,6 @@ jobs: DEPTHAI_BUILD_PCL: ON DEPTHAI_BUILD_RTABMAP: ON VCPKG_MAX_CONCURRENCY: "2" - PKG_CONFIG_PATH: /usr/local/lib/pkgconfig steps: - name: Export GitHub Actions cache environment variables uses: actions/github-script@v7 @@ -507,12 +517,6 @@ jobs: submodules: 'recursive' - name: Installing libusb1-devel dependency run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig - - name: Check pkgconfig for XCB, X11 and Wayland - run: | - pkg-config --variable pc_path pkg-config - pkg-config --exists xcb && echo "xcb found" || echo "xcb missing" - pkg-config --exists x11 && echo "x11 found" || echo "x11 missing" - pkg-config --exists wayland-client && echo "wayland-client found" || echo "wayland-client missing" - name: Setup ninja required for arm64 builds run: | git clone https://github.com/ninja-build/ninja.git @@ -536,47 +540,9 @@ jobs: - name: Building wheels run: | cd bindings/python && for PYBIN in /opt/python/${{ matrix.python-set }}/bin; do "${PYBIN}/pip" wheel . -w ./wheelhouse/ --verbose; done - - name: Print logs for the failed port(s) only + - name: Print out vcpkg logs if building port fails if: failure() # Only run this if the build step fails - run: | - echo "Searching vcpkg-manifest-install.log for failing port logs..." - LOGFILE=$(find . -name "vcpkg-manifest-install.log" | head -n 1) - if [ -z "$LOGFILE" ]; then - echo "No vcpkg-manifest-install.log found!" - exit 1 - fi - - # 1) Grab lines following "See logs for more information:" - # 2) From those lines, extract only the file paths ending in ".log" - # 3) Remove any leading spaces - grep -A50 "See logs for more information:" $LOGFILE \ - | grep "\.log" \ - | sed 's/^[[:space:]]*//' \ - > failed_logs.txt - - # Now read each log path we found, and print it - while IFS= read -r log; do - echo "==== Showing log: $log ====" - cat "$log" || true - echo - - # Also show CMakeOutput.log (and/or CMakeError.log) within the same port’s directory - # Often it’s in the same or neighboring folder (e.g. arm64-linux-rel/CMakeFiles/CMakeOutput.log). - # We'll "walk up" one directory and search for CMakeOutput.log in CMakeFiles/. - port_dir="$(dirname "$log")" - # In some cases (e.g. config-arm64-linux-out.log), you may need to go up another level: - # port_dir="$(dirname "$port_dir")" - - # We'll now look for any CMakeOutput.log within this port’s subdirectories - found_outputs=$(find "$port_dir" -name "CMakeOutput.log" -o -name "CMakeError.log" -print 2>/dev/null) - if [ -n "$found_outputs" ]; then - for cof in $found_outputs; do - echo "==== Showing $cof ====" - cat "$cof" || true - echo - done - fi - done < failed_logs.txt + run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh - name: Auditing wheels run: cd bindings/python && for whl in wheelhouse/*.whl; do auditwheel repair "$whl" --plat $PLAT -w wheelhouse/audited/; done diff --git a/bindings/python/ci/show_vcpkg_logs.sh b/bindings/python/ci/show_vcpkg_logs.sh new file mode 100644 index 000000000..b6c7630e7 --- /dev/null +++ b/bindings/python/ci/show_vcpkg_logs.sh @@ -0,0 +1,51 @@ +echo "Searching vcpkg-manifest-install.log for failing port logs..." +LOGFILE=$(find . -name "vcpkg-manifest-install.log" | head -n 1) +if [ -z "$LOGFILE" ]; then + echo "No vcpkg-manifest-install.log found!" + exit 1 +else + echo "Found log file: $LOGFILE" +fi + +# 1) Grab lines following "See logs for more information:" +# 2) From those lines, extract only the file paths ending in ".log" +# 3) Remove any leading spaces +grep -A50 "See logs for more information:" $LOGFILE \ + | grep "\.log" \ + | sed 's/^[[:space:]]*//' \ + > failed_logs.txt + +# Check if we found any logs +# If not, print a message and exit +# Otherwise, print the log paths +if [ ! -s failed_logs.txt ]; then + echo "No failed logs found!" + exit 1 +else + echo "Found the following failed logs:" + cat failed_logs.txt +fi + +# Now read each log path we found, and print it +while IFS= read -r log; do + echo "==== Showing log: $log ====" + cat "$log" || true +echo + +# Also show CMakeOutput.log (and/or CMakeError.log) within the same port’s directory +# Often it’s in the same or neighboring folder (e.g. arm64-linux-rel/CMakeFiles/CMakeOutput.log). +# We'll "walk up" one directory and search for CMakeOutput.log in CMakeFiles/. +port_dir="$(dirname "$log")" +# In some cases (e.g. config-arm64-linux-out.log), you may need to go up another level: +# port_dir="$(dirname "$port_dir")" + +# We'll now look for any CMakeOutput.log within this port’s subdirectories +found_outputs=$(find "$port_dir" -name "CMakeOutput.log" -o -name "CMakeError.log" -print 2>/dev/null) +if [ -n "$found_outputs" ]; then + for cof in $found_outputs; do + echo "==== Showing $cof ====" + cat "$cof" || true + echo + done +fi +done < failed_logs.txt diff --git a/cmake/triplets/arm64-linux.cmake b/cmake/triplets/arm64-linux.cmake index 29bf061af..9dbd0733a 100644 --- a/cmake/triplets/arm64-linux.cmake +++ b/cmake/triplets/arm64-linux.cmake @@ -9,9 +9,7 @@ if(PORT MATCHES "libusb|ffmpeg") set(VCPKG_FIXUP_ELF_RPATH ON) endif() if(PORT MATCHES "vulkan-loader") - # set env variable for pkg config - set(ENV{PKG_CONFIG_DEBUG_SPEW} 1) - set(CMAKE_FIND_DEBUG_MODE ON) + # When building in GH Actions environment, vcpkg's pkg-conf can't find XCB/X11 libraries set(VCPKG_CMAKE_CONFIGURE_OPTIONS "-DPKG_CONFIG_EXECUTABLE=/usr/bin/pkg-config") endif() From 9e0a58bc0e97dd0235bc217cb11ef69e866111fa Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 9 Jan 2025 08:34:31 +0100 Subject: [PATCH 40/51] update stereo properties in examples, refactor initialization method --- examples/cpp/HostNodes/rgbd.cpp | 2 +- examples/cpp/Visualizer/visualizer_rgbd.cpp | 2 +- examples/python/HostNodes/rgbd.py | 2 +- examples/python/Visualizer/visualizer_rgbd.py | 2 +- include/depthai/pipeline/node/host/RGBD.hpp | 3 +- src/pipeline/node/host/RGBD.cpp | 127 ++++++++---------- 6 files changed, 61 insertions(+), 77 deletions(-) diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/HostNodes/rgbd.cpp index 53365b2a8..eedfdadf2 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/HostNodes/rgbd.cpp @@ -56,7 +56,7 @@ int main() { right->setFps(fps); stereo->setSubpixel(true); stereo->setExtendedDisparity(false); - stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); stereo->setLeftRightCheck(true); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/Visualizer/visualizer_rgbd.cpp index afbfdc14c..1887127dc 100644 --- a/examples/cpp/Visualizer/visualizer_rgbd.cpp +++ b/examples/cpp/Visualizer/visualizer_rgbd.cpp @@ -42,7 +42,7 @@ int main() { right->setFps(fps); stereo->setSubpixel(true); stereo->setExtendedDisparity(false); - stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); stereo->setLeftRightCheck(true); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/HostNodes/rgbd.py index dde2b94bd..f395c5fd3 100644 --- a/examples/python/HostNodes/rgbd.py +++ b/examples/python/HostNodes/rgbd.py @@ -51,7 +51,7 @@ def run(self): out.link(stereo.inputAlignTo) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) - stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) + stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT) stereo.setRectifyEdgeFillColor(0) stereo.enableDistortionCorrection(True) stereo.initialConfig.setLeftRightCheckThreshold(10) diff --git a/examples/python/Visualizer/visualizer_rgbd.py b/examples/python/Visualizer/visualizer_rgbd.py index 5a0d10a48..d0f12db13 100644 --- a/examples/python/Visualizer/visualizer_rgbd.py +++ b/examples/python/Visualizer/visualizer_rgbd.py @@ -31,7 +31,7 @@ out.link(stereo.inputAlignTo) stereo.setExtendedDisparity(False) stereo.setLeftRightCheck(True) - stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) + stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT) stereo.setRectifyEdgeFillColor(0) stereo.enableDistortionCorrection(True) stereo.initialConfig.setLeftRightCheckThreshold(10) diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index 92c0e41dc..f5223c91e 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -3,6 +3,7 @@ #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/node/Sync.hpp" +#include "depthai/pipeline/datatype/MessageGroup.hpp" #include "depthai/utility/Pimpl.hpp" namespace dai { @@ -60,7 +61,7 @@ class RGBD : public NodeCRTP { class Impl; Pimpl pimpl; void run() override; - void initialize(std::vector> frames); + void initialize(std::shared_ptr frames); Input inSync{*this, {"inSync", DEFAULT_GROUP, false, 0, {{DatatypeEnum::MessageGroup, true}}}}; bool initialized = false; }; diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 6a1050d7c..50d386b91 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -59,7 +59,6 @@ class RGBD::Impl { computeMethod = ComputeMethod::CPU_MT; } void useGPU(uint32_t device) { - initializeGPU(device); } void setIntrinsics(float fx, float fy, float cx, float cy, unsigned int width, unsigned int height) { @@ -136,76 +135,61 @@ class RGBD::Impl { } #endif } - void calcPointsChunk( - const uint8_t* depthData, - const uint8_t* colorData, - std::vector& outChunk, - int startRow, int endRow -) { - float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; - outChunk.reserve((endRow - startRow) * width); + void calcPointsChunk(const uint8_t* depthData, const uint8_t* colorData, std::vector& outChunk, int startRow, int endRow) { + float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; + outChunk.reserve((endRow - startRow) * width); - for (int row = startRow; row < endRow; row++) { - int rowStart = row * width; - for (int col = 0; col < width; col++) { - size_t i = rowStart + col; + for(int row = startRow; row < endRow; row++) { + int rowStart = row * width; + for(int col = 0; col < width; col++) { + size_t i = rowStart + col; - uint16_t depthValue = *(reinterpret_cast(depthData + i * 2)); - float z = static_cast(depthValue) * scale; + uint16_t depthValue = *(reinterpret_cast(depthData + i * 2)); + float z = static_cast(depthValue) * scale; - float xCoord = (col - cx) * z / fx; - float yCoord = (row - cy) * z / fy; + float xCoord = (col - cx) * z / fx; + float yCoord = (row - cy) * z / fy; - // BGR order in colorData - uint8_t r = colorData[i * 3 + 0]; - uint8_t g = colorData[i * 3 + 1]; - uint8_t b = colorData[i * 3 + 2]; + // BGR order in colorData + uint8_t r = colorData[i * 3 + 0]; + uint8_t g = colorData[i * 3 + 1]; + uint8_t b = colorData[i * 3 + 2]; - outChunk.push_back(Point3fRGB{xCoord, yCoord, z, r, g, b}); + outChunk.push_back(Point3fRGB{xCoord, yCoord, z, r, g, b}); + } } } -} - -void computePointCloudCPU( - const uint8_t* depthData, - const uint8_t* colorData, - std::vector& points -) { - - // Single-threaded directly writes into points - calcPointsChunk(depthData, colorData, points, 0, height); -} - -void computePointCloudCPUMT( - const uint8_t* depthData, - const uint8_t* colorData, - std::vector& points -) { - int rowsPerThread = height / threadNum; - std::vector>> futures; - - // Each thread returns a local vector - auto processRows = [&](int startRow, int endRow) { - std::vector localPoints; - calcPointsChunk(depthData, colorData, localPoints, startRow, endRow); - return localPoints; - }; - - for (int t = 0; t < threadNum; ++t) { - int startRow = t * rowsPerThread; - int endRow = (t == threadNum - 1) ? height : (startRow + rowsPerThread); - futures.emplace_back(std::async(std::launch::async, processRows, startRow, endRow)); + void computePointCloudCPU(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { + // Single-threaded directly writes into points + calcPointsChunk(depthData, colorData, points, 0, height); } - // Merge all results - for (auto& f : futures) { - auto localPoints = f.get(); - // Now we do one lock per merge if needed - // If this is not called concurrently from multiple places, no lock needed - points.insert(points.end(), localPoints.begin(), localPoints.end()); + void computePointCloudCPUMT(const uint8_t* depthData, const uint8_t* colorData, std::vector& points) { + int rowsPerThread = height / threadNum; + std::vector>> futures; + + // Each thread returns a local vector + auto processRows = [&](int startRow, int endRow) { + std::vector localPoints; + calcPointsChunk(depthData, colorData, localPoints, startRow, endRow); + return localPoints; + }; + + for(int t = 0; t < threadNum; ++t) { + int startRow = t * rowsPerThread; + int endRow = (t == threadNum - 1) ? height : (startRow + rowsPerThread); + futures.emplace_back(std::async(std::launch::async, processRows, startRow, endRow)); + } + + // Merge all results + for(auto& f : futures) { + auto localPoints = f.get(); + // Now we do one lock per merge if needed + // If this is not called concurrently from multiple places, no lock needed + points.insert(points.end(), localPoints.begin(), localPoints.end()); + } } -} enum class ComputeMethod { CPU, CPU_MT, GPU }; ComputeMethod computeMethod = ComputeMethod::CPU; #ifdef DEPTHAI_ENABLE_KOMPUTE @@ -257,17 +241,21 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { return build(); } -void RGBD::initialize(std::vector> frames) { +void RGBD::initialize(std::shared_ptr frames) { // Initialize the camera intrinsics // Check if width and height match - if(frames.at(0)->getWidth() != frames.at(1)->getWidth() || frames.at(0)->getHeight() != frames.at(1)->getHeight()) { + auto colorFrame = std::dynamic_pointer_cast(frames->group.at(inColor.getName())); + if(colorFrame->getType() != dai::ImgFrame::Type::RGB888i) { + throw std::runtime_error("RGBD node only supports RGB888i frames"); + } + auto depthFrame = std::dynamic_pointer_cast(frames->group.at(inDepth.getName())); + if(colorFrame->getWidth() != depthFrame->getWidth() || colorFrame->getHeight() != depthFrame->getHeight()) { throw std::runtime_error("Color and depth frame sizes do not match"); } - auto frame = frames.at(1); auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); - auto camID = static_cast(frame->getInstanceNum()); - auto width = frame->getWidth(); - auto height = frame->getHeight(); + auto camID = static_cast(colorFrame->getInstanceNum()); + auto width = colorFrame->getWidth(); + auto height = colorFrame->getHeight(); auto intrinsics = calibHandler.getCameraIntrinsics(camID, width, height); float fx = intrinsics[0][0]; float fy = intrinsics[1][1]; @@ -283,12 +271,7 @@ void RGBD::run() { auto group = inSync.get(); if(group == nullptr) continue; if(!initialized) { - std::vector> imgFrames; - for(auto& msg : *group) { - imgFrames.emplace_back(std::dynamic_pointer_cast(msg.second)); - } - - initialize(imgFrames); + initialize(group); } auto colorFrame = std::dynamic_pointer_cast(group->group.at(inColor.getName())); if(colorFrame->getType() != dai::ImgFrame::Type::RGB888i) { From 641df1c101b568e44eec9282e02a000e2333528a Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 13 Jan 2025 14:07:51 +0100 Subject: [PATCH 41/51] pr updates --- CMakeLists.txt | 1 + .../pipeline/datatype/RGBDDataBindings.cpp | 39 +++++++++ .../python/src/pipeline/node/RGBDBindings.cpp | 2 +- examples/cpp/CMakeLists.txt | 4 +- examples/cpp/{HostNodes => RGBD}/rgbd.cpp | 36 +++++---- .../{Visualizer => RGBD}/visualizer_rgbd.cpp | 25 +++--- examples/python/{HostNodes => RGBD}/rgbd.py | 26 +++--- examples/python/RGBD/rgbd_o3d.py | 80 +++++++++++++++++++ .../{Visualizer => RGBD}/visualizer_rgbd.py | 22 ++--- .../pipeline/datatype/DatatypeEnum.hpp | 1 + .../depthai/pipeline/datatype/RGBDData.hpp | 30 +++++++ include/depthai/pipeline/datatypes.hpp | 3 +- include/depthai/pipeline/node/host/RGBD.hpp | 12 ++- src/pipeline/datatype/RGBDData.cpp | 5 ++ src/pipeline/node/host/RGBD.cpp | 43 ++++++++-- tests/CMakeLists.txt | 2 +- tests/src/ondevice_tests/rgbd_test.cpp | 18 ++--- 17 files changed, 258 insertions(+), 91 deletions(-) create mode 100644 bindings/python/src/pipeline/datatype/RGBDDataBindings.cpp rename examples/cpp/{HostNodes => RGBD}/rgbd.cpp (67%) rename examples/cpp/{Visualizer => RGBD}/visualizer_rgbd.cpp (74%) rename examples/python/{HostNodes => RGBD}/rgbd.py (68%) create mode 100644 examples/python/RGBD/rgbd_o3d.py rename examples/python/{Visualizer => RGBD}/visualizer_rgbd.py (66%) create mode 100644 include/depthai/pipeline/datatype/RGBDData.hpp create mode 100644 src/pipeline/datatype/RGBDData.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 391f8422f..ad1ed1797 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -430,6 +430,7 @@ set(TARGET_CORE_SOURCES src/pipeline/datatype/BenchmarkReport.cpp src/pipeline/datatype/PointCloudConfig.cpp src/pipeline/datatype/PointCloudData.cpp + src/pipeline/datatype/RGBDData.cpp src/pipeline/datatype/MessageGroup.cpp src/pipeline/datatype/TransformData.cpp src/utility/H26xParsers.cpp diff --git a/bindings/python/src/pipeline/datatype/RGBDDataBindings.cpp b/bindings/python/src/pipeline/datatype/RGBDDataBindings.cpp new file mode 100644 index 000000000..6ba54ca88 --- /dev/null +++ b/bindings/python/src/pipeline/datatype/RGBDDataBindings.cpp @@ -0,0 +1,39 @@ + +#include "DatatypeBindings.hpp" +#include "pipeline/CommonBindings.hpp" +#include +#include + +// depthai +#include "depthai/pipeline/datatype/RGBDData.hpp" + +//pybind +#include +#include + +void bind_transformdata(pybind11::module& m, void* pCallstack){ + + using namespace dai; + + py::class_, Buffer, std::shared_ptr> rgbdData(m, "RGBDData", DOC(dai, RGBDData)); + + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////// + // 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 + rgbdData + .def(py::init<>()) + .def("__repr__", &RGBDData::str) + .def_readwrite("rgbFrame", &RGBDData::rgbFrame, DOC(dai, RGBDData, rgbFrame)) + .def_readwrite("depthFrame", &RGBDData::depthFrame, DOC(dai, RGBDData, depthFrame)); +} diff --git a/bindings/python/src/pipeline/node/RGBDBindings.cpp b/bindings/python/src/pipeline/node/RGBDBindings.cpp index 04cdebd72..08d5b57f9 100644 --- a/bindings/python/src/pipeline/node/RGBDBindings.cpp +++ b/bindings/python/src/pipeline/node/RGBDBindings.cpp @@ -41,7 +41,7 @@ void bind_rgbd(pybind11::module& m, void* pCallstack) { py::arg("autocreate"), py::arg("size"), DOC(dai, node, RGBD, build, 2)) - .def("setOutputMeters", &RGBD::setOutputMeters, py::arg("outputMeters"), DOC(dai, node, RGBD, setOutputMeters)) + .def("setDepthUnits", &RGBD::setDepthUnit, py::arg("units"), DOC(dai, node, RGBD, setDepthUnit)) .def("useCPU", &RGBD::useCPU, DOC(dai, node, RGBD, useCPU)) .def("useCPUMT", &RGBD::useCPUMT, py::arg("numThreads") = 2, DOC(dai, node, RGBD, useCPUMT)) .def("useGPU", &RGBD::useGPU, py::arg("device") = 0, DOC(dai, node, RGBD, useGPU)) diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 81e66d31a..812205593 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -298,10 +298,10 @@ dai_set_example_test_labels(visualizer_yolo ondevice rvc2_all rvc4) include(FetchContent) FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.1/rerun_cpp_sdk.zip) FetchContent_MakeAvailable(rerun_sdk) -dai_add_example(rgbd HostNodes/rgbd.cpp OFF OFF) +dai_add_example(rgbd RGBD/rgbd.cpp ON OFF) target_link_libraries(rgbd PRIVATE rerun_sdk) -dai_add_example(visualizer_rgbd Visualizer/visualizer_rgbd.cpp OFF OFF) +dai_add_example(visualizer_rgbd RGBD/visualizer_rgbd.cpp ON OFF) if(DEPTHAI_RTABMAP_SUPPORT) include(FetchContent) diff --git a/examples/cpp/HostNodes/rgbd.cpp b/examples/cpp/RGBD/rgbd.cpp similarity index 67% rename from examples/cpp/HostNodes/rgbd.cpp rename to examples/cpp/RGBD/rgbd.cpp index eedfdadf2..c4fc51d2d 100644 --- a/examples/cpp/HostNodes/rgbd.cpp +++ b/examples/cpp/RGBD/rgbd.cpp @@ -1,7 +1,10 @@ - #include "depthai/depthai.hpp" #include "rerun.hpp" +#include "rerun/archetypes/depth_image.hpp" +rerun::Collection tensorShape(const cv::Mat& img) { + return {size_t(img.rows), size_t(img.cols), size_t(img.channels())}; +}; class RerunNode : public dai::NodeCRTP { public: constexpr static const char* NAME = "RerunNode"; @@ -10,6 +13,7 @@ class RerunNode : public dai::NodeCRTP { void build() {} Input inputPCL{*this, {.name = "inPCL", .types = {{dai::DatatypeEnum::PointCloudData, true}}}}; + Input inputRGBD{*this, {.name = "inRGBD", .types = {{dai::DatatypeEnum::RGBDData, true}}}}; void run() override { const auto rec = rerun::RecordingStream("rerun"); @@ -17,6 +21,7 @@ class RerunNode : public dai::NodeCRTP { rec.log_static("world", rerun::ViewCoordinates::FLU); while(isRunning()) { auto pclIn = inputPCL.get(); + auto rgbdIn = inputRGBD.get(); if(pclIn != nullptr) { std::vector points; std::vector colors; @@ -29,6 +34,10 @@ class RerunNode : public dai::NodeCRTP { colors.emplace_back(pclData[i].r, pclData[i].g, pclData[i].b); } rec.log("world/obstacle_pcl", rerun::Points3D(points).with_colors(colors).with_radii({0.01f})); + auto colorFrame = rgbdIn->rgbFrame.getCvFrame(); + cv::cvtColor(colorFrame, colorFrame, cv::COLOR_BGR2RGB); + rec.log("rgb", + rerun::Image(tensorShape(colorFrame), reinterpret_cast(colorFrame.data))); } } } @@ -38,22 +47,16 @@ int main() { // Create pipeline dai::Pipeline pipeline; // Define sources and outputs - int fps = 30; - // Define sources and outputs - auto left = pipeline.create(); - auto right = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); auto stereo = pipeline.create(); auto rgbd = pipeline.create()->build(); auto color = pipeline.create(); auto rerun = pipeline.create(); color->build(); - left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); - left->setCamera("left"); - left->setFps(fps); - right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); - right->setCamera("right"); - right->setFps(fps); + left->build(dai::CameraBoardSocket::LEFT); + right->build(dai::CameraBoardSocket::RIGHT); stereo->setSubpixel(true); stereo->setExtendedDisparity(false); stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); @@ -62,19 +65,20 @@ int main() { stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); stereo->initialConfig.postProcessing.thresholdFilter.maxRange = 10000; - rgbd->setOutputMeters(true); + rgbd->setDepthUnit(dai::StereoDepthConfig::AlgorithmControl::DepthUnit::METER); + + auto* out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); - auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); - out->link(stereo->inputAlignTo); - left->out.link(stereo->left); - right->out.link(stereo->right); + left->requestOutput(std::pair(1280, 729))->link(stereo->left); + right->requestOutput(std::pair(1280, 729))->link(stereo->right); stereo->depth.link(rgbd->inDepth); out->link(rgbd->inColor); // Linking rgbd->pcl.link(rerun->inputPCL); + rgbd->rgbd.link(rerun->inputRGBD); pipeline.start(); auto device = pipeline.getDefaultDevice(); device->setIrLaserDotProjectorIntensity(0.7); diff --git a/examples/cpp/Visualizer/visualizer_rgbd.cpp b/examples/cpp/RGBD/visualizer_rgbd.cpp similarity index 74% rename from examples/cpp/Visualizer/visualizer_rgbd.cpp rename to examples/cpp/RGBD/visualizer_rgbd.cpp index 1887127dc..0a8e768ee 100644 --- a/examples/cpp/Visualizer/visualizer_rgbd.cpp +++ b/examples/cpp/RGBD/visualizer_rgbd.cpp @@ -23,23 +23,15 @@ int main() { dai::RemoteConnection remoteConnector(dai::RemoteConnection::DEFAULT_ADDRESS, webSocketPort, true, httpPort); // Create pipeline dai::Pipeline pipeline; - // Define sources and outputs - int fps = 30; - // Define sources and outputs - auto left = pipeline.create(); - auto right = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); auto stereo = pipeline.create(); auto rgbd = pipeline.create()->build(); auto color = pipeline.create(); - stereo->setExtendedDisparity(false); color->build(); - left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); - left->setCamera("left"); - left->setFps(fps); - right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); - right->setCamera("right"); - right->setFps(fps); + left->build(dai::CameraBoardSocket::LEFT); + right->build(dai::CameraBoardSocket::RIGHT); stereo->setSubpixel(true); stereo->setExtendedDisparity(false); stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); @@ -47,11 +39,14 @@ int main() { stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout stereo->enableDistortionCorrection(true); stereo->initialConfig.setLeftRightCheckThreshold(10); - auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); - left->out.link(stereo->left); - right->out.link(stereo->right); + + auto* out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); out->link(stereo->inputAlignTo); + left->requestOutput(std::pair(1280, 729))->link(stereo->left); + right->requestOutput(std::pair(1280, 729))->link(stereo->right); + + stereo->depth.link(rgbd->inDepth); out->link(rgbd->inColor); diff --git a/examples/python/HostNodes/rgbd.py b/examples/python/RGBD/rgbd.py similarity index 68% rename from examples/python/HostNodes/rgbd.py rename to examples/python/RGBD/rgbd.py index f395c5fd3..c62ab6368 100644 --- a/examples/python/HostNodes/rgbd.py +++ b/examples/python/RGBD/rgbd.py @@ -22,9 +22,9 @@ def run(self): rr.log("world", rr.ViewCoordinates.FLU) rr.log("world/ground", rr.Boxes3D(half_sizes=[3.0, 3.0, 0.00001])) while self.isRunning(): - pclObstData = self.inputPCL.get() - if pclObstData is not None: - points, colors = pclObstData.getPointsRGB() + inPointCloud = self.inputPCL.get() + if inPointCloud is not None: + points, colors = inPointCloud.getPointsRGB() rr.log("world/pcl", rr.Points3D(points, colors=colors, radii=[0.01])) # Create pipeline @@ -32,34 +32,26 @@ def run(self): with dai.Pipeline() as p: fps = 30 # Define sources and outputs - left = p.create(dai.node.MonoCamera) - right = p.create(dai.node.MonoCamera) + left = p.create(dai.node.Camera) + right = p.create(dai.node.Camera) color = p.create(dai.node.Camera) stereo = p.create(dai.node.StereoDepth) rgbd = p.create(dai.node.RGBD).build() color.build() rerunViewer = RerunNode() - left.setCamera("left") - left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) - left.setFps(fps) - right.setCamera("right") - right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) - right.setFps(fps) + left.build(dai.CameraBoardSocket.LEFT) + right.build(dai.CameraBoardSocket.RIGHT) out = color.requestOutput((1280,720), dai.ImgFrame.Type.RGB888i) out.link(stereo.inputAlignTo) - stereo.setExtendedDisparity(False) - stereo.setLeftRightCheck(True) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT) stereo.setRectifyEdgeFillColor(0) stereo.enableDistortionCorrection(True) - stereo.initialConfig.setLeftRightCheckThreshold(10) - stereo.setSubpixel(True) # Linking - left.out.link(stereo.left) - right.out.link(stereo.right) + left.requestOutput((1280, 720)).link(stereo.left) + right.requestOutput((1280, 720)).link(stereo.right) stereo.depth.link(rgbd.inDepth) out.link(rgbd.inColor) diff --git a/examples/python/RGBD/rgbd_o3d.py b/examples/python/RGBD/rgbd_o3d.py new file mode 100644 index 000000000..3e7bf3aa6 --- /dev/null +++ b/examples/python/RGBD/rgbd_o3d.py @@ -0,0 +1,80 @@ + +import time +import depthai as dai +import sys +import numpy as np + +try: + import open3d as o3d +except ImportError: + sys.exit("Critical dependency missing: Open3D. Please install it using the command: '{} -m pip install open3d' and then rerun the script.".format(sys.executable)) + + +class O3DNode(dai.node.ThreadedHostNode): + def __init__(self): + dai.node.ThreadedHostNode.__init__(self) + self.inputPCL = self.createInput() + + + def run(self): + def key_callback(vis, action, mods): + global isRunning + if action == 0: + isRunning = False + pc = o3d.geometry.PointCloud() + vis = o3d.visualization.VisualizerWithKeyCallback() + vis.create_window() + vis.register_key_action_callback(81, key_callback) + pcd = o3d.geometry.PointCloud() + coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0,0,0]) + vis.add_geometry(coordinateFrame) + first = True + while self.isRunning(): + inPointCloud = self.inputPCL.tryGet() + if inPointCloud is not None: + points, colors = inPointCloud.getPointsRGB() + pcd.points = o3d.utility.Vector3dVector(points.astype(np.float64)) + colors = (colors.reshape(-1, 3) / 255.0).astype(np.float64) + pcd.colors = o3d.utility.Vector3dVector(colors) + if first: + vis.add_geometry(pcd) + first = False + else: + vis.update_geometry(pcd) + vis.poll_events() + vis.update_renderer() + vis.destroy_window() + +# Create pipeline + +with dai.Pipeline() as p: + fps = 30 + # Define sources and outputs + left = p.create(dai.node.Camera) + right = p.create(dai.node.Camera) + color = p.create(dai.node.Camera) + stereo = p.create(dai.node.StereoDepth) + rgbd = p.create(dai.node.RGBD).build() + color.build() + o3dViewer = O3DNode() + left.build(dai.CameraBoardSocket.LEFT) + right.build(dai.CameraBoardSocket.RIGHT) + out = color.requestOutput((1280,720), dai.ImgFrame.Type.RGB888i) + + + out.link(stereo.inputAlignTo) + stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT) + stereo.setRectifyEdgeFillColor(0) + stereo.enableDistortionCorrection(True) + + # Linking + left.requestOutput((1280, 720)).link(stereo.left) + right.requestOutput((1280, 720)).link(stereo.right) + stereo.depth.link(rgbd.inDepth) + out.link(rgbd.inColor) + + rgbd.pcl.link(o3dViewer.inputPCL) + + p.start() + while p.isRunning(): + time.sleep(1) diff --git a/examples/python/Visualizer/visualizer_rgbd.py b/examples/python/RGBD/visualizer_rgbd.py similarity index 66% rename from examples/python/Visualizer/visualizer_rgbd.py rename to examples/python/RGBD/visualizer_rgbd.py index d0f12db13..1830c328a 100644 --- a/examples/python/Visualizer/visualizer_rgbd.py +++ b/examples/python/RGBD/visualizer_rgbd.py @@ -12,34 +12,26 @@ remoteConnector = dai.RemoteConnection( webSocketPort=args.webSocketPort, httpPort=args.httpPort ) - fps = 30.0 - left = p.create(dai.node.MonoCamera) - right = p.create(dai.node.MonoCamera) + left = p.create(dai.node.Camera) + right = p.create(dai.node.Camera) color = p.create(dai.node.Camera) stereo = p.create(dai.node.StereoDepth) rgbd = p.create(dai.node.RGBD).build() + color.build() - left.setCamera("left") - left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) - left.setFps(fps) - right.setCamera("right") - right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) - right.setFps(fps) + left.build(dai.CameraBoardSocket.LEFT) + right.build(dai.CameraBoardSocket.RIGHT) out = color.requestOutput((1280, 720)) out.link(stereo.inputAlignTo) - stereo.setExtendedDisparity(False) - stereo.setLeftRightCheck(True) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT) stereo.setRectifyEdgeFillColor(0) stereo.enableDistortionCorrection(True) - stereo.initialConfig.setLeftRightCheckThreshold(10) - stereo.setSubpixel(True) # Linking - left.out.link(stereo.left) - right.out.link(stereo.right) + left.requestOutput((1280, 720)).link(stereo.left) + right.requestOutput((1280, 720)).link(stereo.right) stereo.depth.link(rgbd.inDepth) out.link(rgbd.inColor) remoteConnector.addTopic("pcl", rgbd.pcl, "common") diff --git a/include/depthai/pipeline/datatype/DatatypeEnum.hpp b/include/depthai/pipeline/datatype/DatatypeEnum.hpp index 9684e321d..a37066ccf 100644 --- a/include/depthai/pipeline/datatype/DatatypeEnum.hpp +++ b/include/depthai/pipeline/datatype/DatatypeEnum.hpp @@ -34,6 +34,7 @@ enum class DatatypeEnum : std::int32_t { TransformData, PointCloudConfig, PointCloudData, + RGBDData, ImageAlignConfig, ImgAnnotations }; diff --git a/include/depthai/pipeline/datatype/RGBDData.hpp b/include/depthai/pipeline/datatype/RGBDData.hpp new file mode 100644 index 000000000..8c7a793ce --- /dev/null +++ b/include/depthai/pipeline/datatype/RGBDData.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "depthai/pipeline/datatype/Buffer.hpp" +#include "depthai/pipeline/datatype/ImgFrame.hpp" + +namespace dai { + +/** + * RGBD message. Carries RGB and Depth frames. + */ +class RGBDData : public Buffer { + public: + /** + * Construct RGBD message. + */ + RGBDData() = default; + + virtual ~RGBDData() = default; + + ImgFrame rgbFrame; + ImgFrame depthFrame; + + void serialize(std::vector& metadata, DatatypeEnum& datatype) const override { + metadata = utility::serialize(*this); + datatype = DatatypeEnum::RGBDData; + }; + DEPTHAI_SERIALIZE(RGBDData, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, rgbFrame, depthFrame); +}; + +} // namespace dai diff --git a/include/depthai/pipeline/datatypes.hpp b/include/depthai/pipeline/datatypes.hpp index dca47c5e6..54e8c9055 100644 --- a/include/depthai/pipeline/datatypes.hpp +++ b/include/depthai/pipeline/datatypes.hpp @@ -20,6 +20,7 @@ #include "datatype/NNData.hpp" #include "datatype/PointCloudConfig.hpp" #include "datatype/PointCloudData.hpp" +#include "datatype/RGBDData.hpp" #include "datatype/SpatialImgDetections.hpp" #include "datatype/SpatialLocationCalculatorConfig.hpp" #include "datatype/SpatialLocationCalculatorData.hpp" @@ -30,4 +31,4 @@ #include "datatype/ToFConfig.hpp" #include "datatype/TrackedFeatures.hpp" #include "datatype/Tracklets.hpp" -#include "datatype/TransformData.hpp" \ No newline at end of file +#include "datatype/TransformData.hpp" diff --git a/include/depthai/pipeline/node/host/RGBD.hpp b/include/depthai/pipeline/node/host/RGBD.hpp index f5223c91e..1ff0c1979 100644 --- a/include/depthai/pipeline/node/host/RGBD.hpp +++ b/include/depthai/pipeline/node/host/RGBD.hpp @@ -2,6 +2,8 @@ #include "depthai/pipeline/Subnode.hpp" #include "depthai/pipeline/ThreadedHostNode.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" +#include "depthai/pipeline/datatype/RGBDData.hpp" +#include "depthai/pipeline/datatype/StereoDepthConfig.hpp" #include "depthai/pipeline/node/Sync.hpp" #include "depthai/pipeline/datatype/MessageGroup.hpp" #include "depthai/utility/Pimpl.hpp" @@ -30,15 +32,19 @@ class RGBD : public NodeCRTP { * Output point cloud. */ Output pcl{*this, {"pcl", DEFAULT_GROUP, {{DatatypeEnum::PointCloudData, true}}}}; + /** + * Output RGBD frames. + */ + Output rgbd{*this, {"rgbd", DEFAULT_GROUP, {{DatatypeEnum::RGBDData, true}}}}; std::shared_ptr build(); /** - * @brief Build RGBD node with specified size - * @param autocreate If true, will create color and depth frames if they don't exist + * @brief Build RGBD node with specified size. Note that this API is global and if used autocreated cameras can't be reused. + * @param autocreate If true, will create color and depth nodes if they don't exist. * @param size Size of the frames */ std::shared_ptr build(bool autocreate, std::pair size); - void setOutputMeters(bool outputMeters); + void setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit); /** * @brief Use single-threaded CPU for processing */ diff --git a/src/pipeline/datatype/RGBDData.cpp b/src/pipeline/datatype/RGBDData.cpp new file mode 100644 index 000000000..92a3dcbcb --- /dev/null +++ b/src/pipeline/datatype/RGBDData.cpp @@ -0,0 +1,5 @@ +#include "depthai/pipeline/datatype/RGBDData.hpp" + +namespace dai { +; // TODO - no impl needed +} // namespace dai diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index 50d386b91..f95aeb5b6 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -40,8 +40,28 @@ class RGBD::Impl { break; } } - void setOutputMeters(bool outputMeters) { - this->outputMeters = outputMeters; + void setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit){ + // Default is millimeter + switch(depthUnit){ + case StereoDepthConfig::AlgorithmControl::DepthUnit::MILLIMETER: + scaleFactor = 1.0f; + break; + case StereoDepthConfig::AlgorithmControl::DepthUnit::METER: + scaleFactor = 0.001f; + break; + case StereoDepthConfig::AlgorithmControl::DepthUnit::CENTIMETER: + scaleFactor = 0.01f; + break; + case StereoDepthConfig::AlgorithmControl::DepthUnit::FOOT: + scaleFactor = 0.3048f; + break; + case StereoDepthConfig::AlgorithmControl::DepthUnit::INCH: + scaleFactor = 0.0254f; + break; + case StereoDepthConfig::AlgorithmControl::DepthUnit::CUSTOM: + scaleFactor = 1.0f; + break; + } } void printDevices() { #ifdef DEPTHAI_ENABLE_KOMPUTE @@ -92,7 +112,7 @@ class RGBD::Impl { // Convert depth to float // Depth is in mm by default, convert to meters if outputMeters == true // If outputMeters is true, scale = 1/1000.0 else scale = 1.0 - float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; + float scale = scaleFactor; std::vector depthDataFloat(size); for(int i = 0; i < size; i++) { @@ -136,7 +156,7 @@ class RGBD::Impl { #endif } void calcPointsChunk(const uint8_t* depthData, const uint8_t* colorData, std::vector& outChunk, int startRow, int endRow) { - float scale = outputMeters ? (1.0f / 1000.0f) : 1.0f; + float scale = scaleFactor; outChunk.reserve((endRow - startRow) * width); for(int row = startRow; row < endRow; row++) { @@ -203,7 +223,7 @@ class RGBD::Impl { bool algoInitialized = false; bool tensorsInitialized = false; #endif - bool outputMeters = false; + float scaleFactor = 1.0f; float fx, fy, cx, cy; int width, height; int size; @@ -243,7 +263,7 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { void RGBD::initialize(std::shared_ptr frames) { // Initialize the camera intrinsics - // Check if width and height match + // Check if width, width and cameraID match auto colorFrame = std::dynamic_pointer_cast(frames->group.at(inColor.getName())); if(colorFrame->getType() != dai::ImgFrame::Type::RGB888i) { throw std::runtime_error("RGBD node only supports RGB888i frames"); @@ -252,6 +272,9 @@ void RGBD::initialize(std::shared_ptr frames) { if(colorFrame->getWidth() != depthFrame->getWidth() || colorFrame->getHeight() != depthFrame->getHeight()) { throw std::runtime_error("Color and depth frame sizes do not match"); } + if(colorFrame->getInstanceNum() != depthFrame->getInstanceNum()) { + throw std::runtime_error("Depth is not aligned to color"); + } auto calibHandler = getParentPipeline().getDefaultDevice()->readCalibration(); auto camID = static_cast(colorFrame->getInstanceNum()); auto width = colorFrame->getWidth(); @@ -298,10 +321,14 @@ void RGBD::run() { pc->setPointsRGB(points); pcl.send(pc); + auto rgbdData = std::make_shared(); + rgbdData->depthFrame = *depthFrame; + rgbdData->rgbFrame = *colorFrame; + rgbd.send(rgbdData); } } -void RGBD::setOutputMeters(bool outputMeters) { - pimpl->setOutputMeters(outputMeters); +void RGBD::setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit){ + pimpl->setDepthUnit(depthUnit); } void RGBD::useCPU() { pimpl->useCPU(); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 174769548..eee7aeb3b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -341,7 +341,7 @@ dai_set_test_labels(pointcloud_test ondevice rvc2_all ci) # RGBD test dai_add_test(rgbd_test src/ondevice_tests/rgbd_test.cpp) -dai_set_test_labels(rgbd_test ondevice rvc2_all ci) +dai_set_test_labels(rgbd_test ondevice rvc2_all rvc4 ci) # Resolutions test dai_add_test(resolutions_test src/ondevice_tests/resolutions_test.cpp) diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp index 6b250a86a..25f83be04 100644 --- a/tests/src/ondevice_tests/rgbd_test.cpp +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -6,22 +6,16 @@ TEST_CASE("basic rgbd") { // Create pipeline dai::Pipeline pipeline; // Define sources and outputs - int fps = 30; - // Define sources and outputs - auto left = pipeline.create(); - auto right = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); auto stereo = pipeline.create(); auto rgbd = pipeline.create()->build(); auto color = pipeline.create(); stereo->setExtendedDisparity(false); color->build(); - left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); - left->setCamera("left"); - left->setFps(fps); - right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); - right->setCamera("right"); - right->setFps(fps); + left->build(dai::CameraBoardSocket::LEFT); + right->build(dai::CameraBoardSocket::RIGHT); stereo->setSubpixel(true); stereo->setExtendedDisparity(false); stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); @@ -32,8 +26,8 @@ TEST_CASE("basic rgbd") { auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); out->link(stereo->inputAlignTo); - left->out.link(stereo->left); - right->out.link(stereo->right); + left->requestOutput(std::pair(1280, 720))->link(stereo->left); + right->requestOutput(std::pair(1280, 720))->link(stereo->right); stereo->depth.link(rgbd->inDepth); out->link(rgbd->inColor); From 85760034c143933ee4ad85bf0dab10b0d5cbc651 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 13 Jan 2025 14:35:13 +0100 Subject: [PATCH 42/51] update vcpkg logs path for build_docstrings --- .github/workflows/python-main.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 91f3d9df3..0b53d9b3b 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -60,7 +60,7 @@ jobs: run: cmake --build build --target pybind11_mkdoc --parallel 4 - name: Print out vcpkg logs if building port fails if: failure() # Only run this if the build step fails - run: cd bindings/python && bash ./ci/show_vcpkg_logs.sh + run: bash ./bindings/python/ci/show_vcpkg_logs.sh - name: Upload docstring artifacts uses: actions/upload-artifact@v3 From a6b4b4793f9d2fd2f3cccb575173ef0934dffec1 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 13 Jan 2025 17:30:37 +0100 Subject: [PATCH 43/51] add python example tests --- examples/python/CMakeLists.txt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/examples/python/CMakeLists.txt b/examples/python/CMakeLists.txt index 7ac0f52e6..324264f0f 100644 --- a/examples/python/CMakeLists.txt +++ b/examples/python/CMakeLists.txt @@ -166,6 +166,17 @@ dai_set_example_test_labels(image_manip_resize ondevice rvc2_all rvc4 ci) add_python_example(reconnect_callback Misc/AutoReconnect/reconnect_callback.py) dai_set_example_test_labels(reconnect_callback ondevice rvc2_all rvc4 ci) + +## RGBD +add_python_example(rgbd RGBD/rgbd.py) +dai_set_example_test_labels(rgbd ondevice rvc2_all rvc4 ci) + +add_python_example(visualizer_rgbd RGBD/visualizer_rgbd.py) +dai_set_example_test_labels(visualizer_rgbd ondevice rvc2_all rvc4 ci) + +add_python_example(rgbd_o3d.py RGBD/rgbd_o3d.py) +dai_set_example_test_labels(rgbd_o3d ondevice rvc2_all rvc4 ci) + ## NeuralNetwork add_python_example(neural_network NeuralNetwork/neural_network.py) dai_set_example_test_labels(neural_network ondevice rvc2_all rvc4 ci) From fe17fe75de0d5bd51eccd4274542e6fbbd6f29f9 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 13 Jan 2025 17:44:33 +0100 Subject: [PATCH 44/51] fix typo --- examples/python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python/CMakeLists.txt b/examples/python/CMakeLists.txt index 324264f0f..64d8a60eb 100644 --- a/examples/python/CMakeLists.txt +++ b/examples/python/CMakeLists.txt @@ -174,7 +174,7 @@ dai_set_example_test_labels(rgbd ondevice rvc2_all rvc4 ci) add_python_example(visualizer_rgbd RGBD/visualizer_rgbd.py) dai_set_example_test_labels(visualizer_rgbd ondevice rvc2_all rvc4 ci) -add_python_example(rgbd_o3d.py RGBD/rgbd_o3d.py) +add_python_example(rgbd_o3d RGBD/rgbd_o3d.py) dai_set_example_test_labels(rgbd_o3d ondevice rvc2_all rvc4 ci) ## NeuralNetwork From b2e3d6fbbcd816744a7f01b4ddeaa22ec89f5c24 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Mon, 13 Jan 2025 18:24:41 +0100 Subject: [PATCH 45/51] update env vars for eventsmanager --- src/utility/EventsManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/utility/EventsManager.cpp b/src/utility/EventsManager.cpp index 77cc239e1..3f8b6b4a6 100644 --- a/src/utility/EventsManager.cpp +++ b/src/utility/EventsManager.cpp @@ -109,8 +109,8 @@ EventsManager::EventsManager(std::string url, bool uploadCachedOnStart, float pu uploadCachedOnStart(uploadCachedOnStart), cacheIfCannotSend(false), stopEventBuffer(false) { - sourceAppId = utility::getEnv("AGENT_APP_ID"); - sourceAppIdentifier = utility::getEnv("AGENT_APP_IDENTIFIER"); + sourceAppId = utility::getEnv("OAKAGENT_APP_VERSION"); + sourceAppIdentifier = utility::getEnv("OAKAGENT_APP_IDENTIFIER"); token = utility::getEnv("DEPTHAI_HUB_API_KEY"); if(token.empty()) { throw std::runtime_error("Missing token, please set DEPTHAI_HUB_API_KEY environment variable or use setToken method"); From 78d697fc73b8bac12c4ed3dd2e3ec800ca67b288 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Jan 2025 13:37:33 +0100 Subject: [PATCH 46/51] fix ci, make compute optional --- .github/workflows/python-main.yml | 6 +++++- CMakeLists.txt | 2 +- bindings/python/setup.py | 2 ++ 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/.github/workflows/python-main.yml b/.github/workflows/python-main.yml index 0b53d9b3b..36d2134f3 100644 --- a/.github/workflows/python-main.yml +++ b/.github/workflows/python-main.yml @@ -52,7 +52,7 @@ jobs: run: | sudo apt update python -m pip install --upgrade pip - sudo apt install libusb-1.0-0-dev pkg-config bison autoconf libtool libxi-dev libxtst-dev libxrandr-dev libx11-dev libxft-dev libxext-dev nasm flex libudev-dev + sudo apt install libusb-1.0-0-dev pkg-config bison autoconf libtool libxi-dev libxtst-dev libxrandr-dev libx11-dev libxft-dev libxext-dev nasm flex libudev-dev automake libltdl-dev python -m pip install -r bindings/python/docs/requirements_mkdoc.txt - name: Configure project run: cmake -S . -B build -DDEPTHAI_BUILD_PYTHON=ON -DDEPTHAI_PYTHON_FORCE_DOCSTRINGS=ON -DDEPTHAI_BASALT_SUPPORT=ON -DDEPTHAI_PCL_SUPPORT=ON -DDEPTHAI_RTABMAP_SUPPORT=ON -DDEPTHAI_PYTHON_DOCSTRINGS_OUTPUT="$PWD/bindings/python/docstrings/depthai_python_docstring.hpp" @@ -214,6 +214,7 @@ jobs: DEPTHAI_BUILD_BASALT: OFF DEPTHAI_BUILD_PCL: ON DEPTHAI_BUILD_RTABMAP: ON + DEPTHAI_BUILD_KOMPUTE: ON VCPKG_BINARY_SOURCES: "clear;x-gha,readwrite" steps: - name: Cache .hunter folder @@ -284,6 +285,7 @@ jobs: DEPTHAI_BUILD_BASALT: ON DEPTHAI_BUILD_PCL: ON DEPTHAI_BUILD_RTABMAP: ON + DEPTHAI_BUILD_KOMPUTE: ON steps: - name: Cache .hunter folder uses: actions/cache@v3 @@ -423,6 +425,7 @@ jobs: DEPTHAI_BUILD_BASALT: ON DEPTHAI_BUILD_PCL: ON DEPTHAI_BUILD_RTABMAP: ON + DEPTHAI_BUILD_KOMPUTE: ON VCPKG_BINARY_SOURCES: "clear;x-gha,readwrite" steps: - name: Cache .hunter folder @@ -504,6 +507,7 @@ jobs: DEPTHAI_BUILD_BASALT: ON DEPTHAI_BUILD_PCL: ON DEPTHAI_BUILD_RTABMAP: ON + DEPTHAI_BUILD_KOMPUTE: ON VCPKG_MAX_CONCURRENCY: "2" steps: - name: Export GitHub Actions cache environment variables diff --git a/CMakeLists.txt b/CMakeLists.txt index ad1ed1797..6ec3c57b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ option(DEPTHAI_ENABLE_PROTOBUF "Enable Protobuf support" ON) option(DEPTHAI_BUILD_PYTHON "Build python bindings" OFF) option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) -OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" ON) +OPTION(DEPTHAI_ENABLE_KOMPUTE "Enable Kompute support" OFF) option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BOOTSTRAP_VCPKG "Automatically bootstrap VCPKG" ON) option(DEPTHAI_MERGED_TARGET "Enable merged target build" ON) diff --git a/bindings/python/setup.py b/bindings/python/setup.py index 2d3b76d3f..598e3955f 100644 --- a/bindings/python/setup.py +++ b/bindings/python/setup.py @@ -125,6 +125,8 @@ def build_extension(self, ext): cmake_args += ['-DDEPTHAI_PCL_SUPPORT=ON'] if env.get('DEPTHAI_BUILD_RTABMAP') == 'ON': cmake_args += ['-DDEPTHAI_RTABMAP_SUPPORT=ON'] + if env.get('DEPTHAI_BUILD_KOMPUTE') == 'ON': + cmake_args += ['-DDEPTHAI_KOMPUTE_SUPPORT=ON'] build_args += ['--target=depthai'] # Specify output directory and python executable From b11585f3779a2d58c8a507c5ba8cd8c31399f670 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 15 Jan 2025 17:44:12 +0100 Subject: [PATCH 47/51] try updating tests --- tests/src/ondevice_tests/rgbd_test.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp index 25f83be04..0ef7e407e 100644 --- a/tests/src/ondevice_tests/rgbd_test.cpp +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include "depthai/depthai.hpp" TEST_CASE("basic rgbd") { // Create pipeline @@ -11,18 +11,11 @@ TEST_CASE("basic rgbd") { auto stereo = pipeline.create(); auto rgbd = pipeline.create()->build(); auto color = pipeline.create(); - stereo->setExtendedDisparity(false); color->build(); left->build(dai::CameraBoardSocket::LEFT); right->build(dai::CameraBoardSocket::RIGHT); - stereo->setSubpixel(true); - stereo->setExtendedDisparity(false); - stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); - stereo->setLeftRightCheck(true); - stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout - stereo->enableDistortionCorrection(true); - stereo->initialConfig.setLeftRightCheckThreshold(10); + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); out->link(stereo->inputAlignTo); From 312b7a0917045ea8b0c367c09811b2474550bd95 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 16 Jan 2025 11:28:30 +0100 Subject: [PATCH 48/51] fix python tests --- .github/workflows/test.workflow.yml | 2 +- examples/python/RGBD/rgbd.py | 4 ++-- examples/python/RGBD/rgbd_o3d.py | 4 ++-- examples/python/RGBD/visualizer_rgbd.py | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.github/workflows/test.workflow.yml b/.github/workflows/test.workflow.yml index e48512e4c..873b6dd6e 100644 --- a/.github/workflows/test.workflow.yml +++ b/.github/workflows/test.workflow.yml @@ -80,7 +80,7 @@ jobs: export DISPLAY=:99 xdpyinfo -display $DISPLAY >/dev/null 2>&1 || (Xvfb $DISPLAY &) source venv/bin/activate # Activate virtual environment - python3 -m pip install jinja2 + python3 -m pip install jinja2 rerun-sdk open3d cmake -S . -B build -D CMAKE_BUILD_TYPE=Release -D HUNTER_ROOT=$HOME/.hun2_${{ matrix.flavor }} -D DEPTHAI_BUILD_EXAMPLES=ON -D DEPTHAI_BUILD_TESTS=ON -D DEPTHAI_TEST_EXAMPLES=ON -D DEPTHAI_BUILD_PYTHON=ON -D DEPTHAI_PYTHON_TEST_EXAMPLES=ON -D DEPTHAI_PYTHON_ENABLE_EXAMPLES=ON cmake --build build --parallel 4 --config Release cd tests diff --git a/examples/python/RGBD/rgbd.py b/examples/python/RGBD/rgbd.py index c62ab6368..e8f895c67 100644 --- a/examples/python/RGBD/rgbd.py +++ b/examples/python/RGBD/rgbd.py @@ -39,8 +39,8 @@ def run(self): rgbd = p.create(dai.node.RGBD).build() color.build() rerunViewer = RerunNode() - left.build(dai.CameraBoardSocket.LEFT) - right.build(dai.CameraBoardSocket.RIGHT) + left.build(dai.CameraBoardSocket.CAM_B) + right.build(dai.CameraBoardSocket.CAM_C) out = color.requestOutput((1280,720), dai.ImgFrame.Type.RGB888i) diff --git a/examples/python/RGBD/rgbd_o3d.py b/examples/python/RGBD/rgbd_o3d.py index 3e7bf3aa6..a6a415618 100644 --- a/examples/python/RGBD/rgbd_o3d.py +++ b/examples/python/RGBD/rgbd_o3d.py @@ -57,8 +57,8 @@ def key_callback(vis, action, mods): rgbd = p.create(dai.node.RGBD).build() color.build() o3dViewer = O3DNode() - left.build(dai.CameraBoardSocket.LEFT) - right.build(dai.CameraBoardSocket.RIGHT) + left.build(dai.CameraBoardSocket.CAM_B) + right.build(dai.CameraBoardSocket.CAM_C) out = color.requestOutput((1280,720), dai.ImgFrame.Type.RGB888i) diff --git a/examples/python/RGBD/visualizer_rgbd.py b/examples/python/RGBD/visualizer_rgbd.py index 1830c328a..72c087e4d 100644 --- a/examples/python/RGBD/visualizer_rgbd.py +++ b/examples/python/RGBD/visualizer_rgbd.py @@ -19,8 +19,8 @@ rgbd = p.create(dai.node.RGBD).build() color.build() - left.build(dai.CameraBoardSocket.LEFT) - right.build(dai.CameraBoardSocket.RIGHT) + left.build(dai.CameraBoardSocket.CAM_B) + right.build(dai.CameraBoardSocket.CAM_C) out = color.requestOutput((1280, 720)) From 8907adf5c95e689fad4174cf71ec0760a1ef088d Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 16 Jan 2025 11:45:08 +0100 Subject: [PATCH 49/51] update stereodepth build docs --- include/depthai/pipeline/node/StereoDepth.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 8bf607e88..2b1fa65f1 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -43,6 +43,11 @@ class StereoDepth : public DeviceNodeCRTP(shared_from_this()); } + /** + * Create StereoDepth node. Note that this API is global and if used autocreated cameras can't be reused. + * @param autoCreateCameras If true, will create left and right nodes if they don't exist + * @param presetMode Preset mode for stereo depth + */ std::shared_ptr build(bool autoCreateCameras, PresetMode presetMode = PresetMode::HIGH_ACCURACY); protected: From 0174f43f71be352155b0b3092cdb9069700bebfe Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 16 Jan 2025 18:05:17 +0100 Subject: [PATCH 50/51] update python tests, open3dcpu --- .github/workflows/test.workflow.yml | 2 +- examples/python/RGBD/visualizer_rgbd.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.workflow.yml b/.github/workflows/test.workflow.yml index 873b6dd6e..12ff519c6 100644 --- a/.github/workflows/test.workflow.yml +++ b/.github/workflows/test.workflow.yml @@ -80,7 +80,7 @@ jobs: export DISPLAY=:99 xdpyinfo -display $DISPLAY >/dev/null 2>&1 || (Xvfb $DISPLAY &) source venv/bin/activate # Activate virtual environment - python3 -m pip install jinja2 rerun-sdk open3d + python3 -m pip install jinja2 rerun-sdk open3d-cpu cmake -S . -B build -D CMAKE_BUILD_TYPE=Release -D HUNTER_ROOT=$HOME/.hun2_${{ matrix.flavor }} -D DEPTHAI_BUILD_EXAMPLES=ON -D DEPTHAI_BUILD_TESTS=ON -D DEPTHAI_TEST_EXAMPLES=ON -D DEPTHAI_BUILD_PYTHON=ON -D DEPTHAI_PYTHON_TEST_EXAMPLES=ON -D DEPTHAI_PYTHON_ENABLE_EXAMPLES=ON cmake --build build --parallel 4 --config Release cd tests diff --git a/examples/python/RGBD/visualizer_rgbd.py b/examples/python/RGBD/visualizer_rgbd.py index 72c087e4d..4627c4f5d 100644 --- a/examples/python/RGBD/visualizer_rgbd.py +++ b/examples/python/RGBD/visualizer_rgbd.py @@ -21,7 +21,7 @@ color.build() left.build(dai.CameraBoardSocket.CAM_B) right.build(dai.CameraBoardSocket.CAM_C) - out = color.requestOutput((1280, 720)) + out = color.requestOutput((1280, 720), dai.ImgFrame.Type.RGB888i) out.link(stereo.inputAlignTo) From 7126b7aa04c4831504a36814b609e7344a337c5a Mon Sep 17 00:00:00 2001 From: Serafadam Date: Thu, 16 Jan 2025 18:05:27 +0100 Subject: [PATCH 51/51] explicit align node for rvc4 --- src/pipeline/node/host/RGBD.cpp | 24 ++++++++++++++++++------ tests/src/ondevice_tests/rgbd_test.cpp | 26 +++++++++++++++++++------- 2 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/pipeline/node/host/RGBD.cpp b/src/pipeline/node/host/RGBD.cpp index f95aeb5b6..f7d59b8a2 100644 --- a/src/pipeline/node/host/RGBD.cpp +++ b/src/pipeline/node/host/RGBD.cpp @@ -9,6 +9,7 @@ #include "depthai/pipeline/datatype/MessageGroup.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/node/Camera.hpp" +#include "depthai/pipeline/node/ImageAlign.hpp" #include "depthai/pipeline/node/StereoDepth.hpp" #include "depthai/pipeline/node/Sync.hpp" #ifdef DEPTHAI_ENABLE_KOMPUTE @@ -40,9 +41,9 @@ class RGBD::Impl { break; } } - void setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit){ + void setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit) { // Default is millimeter - switch(depthUnit){ + switch(depthUnit) { case StereoDepthConfig::AlgorithmControl::DepthUnit::MILLIMETER: scaleFactor = 1.0f; break; @@ -253,11 +254,22 @@ std::shared_ptr RGBD::build(bool autocreate, std::pair size) { } auto pipeline = getParentPipeline(); auto colorCam = pipeline.create()->build(); - auto depth = pipeline.create()->build(true); + auto platform = pipeline.getDefaultDevice()->getPlatform(); + auto stereo = pipeline.create()->build(true); + std::shared_ptr align = nullptr; + if(platform == Platform::RVC4) { + auto align = pipeline.create(); + } auto* out = colorCam->requestOutput(size, dai::ImgFrame::Type::RGB888i); out->link(inColor); - out->link(depth->inputAlignTo); - depth->depth.link(inDepth); + if(platform == dai::Platform::RVC4) { + stereo->depth.link(align->input); + out->link(align->inputAlignTo); + align->outputAligned.link(inDepth); + } else { + out->link(stereo->inputAlignTo); + stereo->depth.link(inDepth); + } return build(); } @@ -327,7 +339,7 @@ void RGBD::run() { rgbd.send(rgbdData); } } -void RGBD::setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit){ +void RGBD::setDepthUnit(StereoDepthConfig::AlgorithmControl::DepthUnit depthUnit) { pimpl->setDepthUnit(depthUnit); } void RGBD::useCPU() { diff --git a/tests/src/ondevice_tests/rgbd_test.cpp b/tests/src/ondevice_tests/rgbd_test.cpp index 0ef7e407e..e6843b1aa 100644 --- a/tests/src/ondevice_tests/rgbd_test.cpp +++ b/tests/src/ondevice_tests/rgbd_test.cpp @@ -1,28 +1,40 @@ #include #include + #include "depthai/depthai.hpp" TEST_CASE("basic rgbd") { // Create pipeline dai::Pipeline pipeline; + auto platform = pipeline.getDefaultDevice()->getPlatform(); // Define sources and outputs auto left = pipeline.create(); auto right = pipeline.create(); auto stereo = pipeline.create(); auto rgbd = pipeline.create()->build(); auto color = pipeline.create(); + std::shared_ptr align = nullptr; + if(platform == dai::Platform::RVC4) { + align = pipeline.create(); + } color->build(); - left->build(dai::CameraBoardSocket::LEFT); - right->build(dai::CameraBoardSocket::RIGHT); + left->build(dai::CameraBoardSocket::CAM_B); + right->build(dai::CameraBoardSocket::CAM_C); stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT); - auto *out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); - out->link(stereo->inputAlignTo); - left->requestOutput(std::pair(1280, 720))->link(stereo->left); - right->requestOutput(std::pair(1280, 720))->link(stereo->right); + auto* out = color->requestOutput(std::pair(1280, 720), dai::ImgFrame::Type::RGB888i); + left->requestOutput(std::pair(1280, 720))->link(stereo->left); + right->requestOutput(std::pair(1280, 720))->link(stereo->right); - stereo->depth.link(rgbd->inDepth); + if(platform == dai::Platform::RVC4) { + stereo->depth.link(align->input); + out->link(align->inputAlignTo); + align->outputAligned.link(rgbd->inDepth); + } else { + out->link(stereo->inputAlignTo); + stereo->depth.link(rgbd->inDepth); + } out->link(rgbd->inColor); auto outQ = rgbd->pcl.createOutputQueue();