From 6be0199823c33e93609bdea59eeb4ae963f9378b Mon Sep 17 00:00:00 2001 From: Andrej Susnik Date: Tue, 20 Jun 2023 12:23:21 +0200 Subject: [PATCH 01/80] temp commit TODO: squash --- CMakeLists.txt | 2 + examples/CMakeLists.txt | 2 + examples/DepthAlign/rgb_depth_align.cpp | 143 ++++++++++++++++++ .../pipeline/datatype/DepthAlignConfig.hpp | 36 +++++ include/depthai/pipeline/node/DepthAlign.hpp | 75 +++++++++ include/depthai/pipeline/nodes.hpp | 1 + shared/depthai-shared | 2 +- src/pipeline/datatype/DepthAlignConfig.cpp | 21 +++ src/pipeline/datatype/StreamMessageParser.cpp | 10 ++ src/pipeline/node/DepthAlign.cpp | 32 ++++ tests/src/image_manip_node_test.cpp | 1 - 11 files changed, 323 insertions(+), 2 deletions(-) create mode 100644 examples/DepthAlign/rgb_depth_align.cpp create mode 100644 include/depthai/pipeline/datatype/DepthAlignConfig.hpp create mode 100644 include/depthai/pipeline/node/DepthAlign.hpp create mode 100644 src/pipeline/datatype/DepthAlignConfig.cpp create mode 100644 src/pipeline/node/DepthAlign.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index dc2972914..ca2818f85 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -214,6 +214,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/DetectionNetwork.cpp src/pipeline/node/Script.cpp src/pipeline/node/SpatialDetectionNetwork.cpp + src/pipeline/node/DepthAlign.cpp src/pipeline/node/SystemLogger.cpp src/pipeline/node/SpatialLocationCalculator.cpp src/pipeline/node/AprilTag.cpp @@ -243,6 +244,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/datatype/EdgeDetectorConfig.cpp src/pipeline/datatype/TrackedFeatures.cpp src/pipeline/datatype/FeatureTrackerConfig.cpp + src/pipeline/datatype/DepthAlignConfig.cpp src/pipeline/datatype/ToFConfig.cpp src/utility/Initialization.cpp src/utility/Resources.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 41272b289..c199f338c 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -236,6 +236,8 @@ dai_add_example(edge_detector EdgeDetector/edge_detector.cpp ON) dai_add_example(feature_detector FeatureTracker/feature_detector.cpp ON) dai_add_example(feature_tracker FeatureTracker/feature_tracker.cpp ON) +dai_add_example(depth_align DepthAlign/rgb_depth_align.cpp ON) + # host_side dai_add_example(device_queue_event host_side/device_queue_event.cpp ON) dai_add_example(opencv_support host_side/opencv_support.cpp ON) diff --git a/examples/DepthAlign/rgb_depth_align.cpp b/examples/DepthAlign/rgb_depth_align.cpp new file mode 100644 index 000000000..84c3ea704 --- /dev/null +++ b/examples/DepthAlign/rgb_depth_align.cpp @@ -0,0 +1,143 @@ +#include +#include + +#include "utility.hpp" + +// Includes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Optional. If set (true), the ColorCamera is downscaled from 1080p to 720p. +// Otherwise (false), the aligned depth is automatically upscaled to 1080p +static std::atomic downscaleColor{true}; +static constexpr int fps = 30; +// The disparity is computed at this resolution, then upscaled to RGB resolution +static constexpr auto monoRes = dai::MonoCameraProperties::SensorResolution::THE_720_P; + +static float rgbWeight = 0.4f; +static float depthWeight = 0.6f; + +static void updateBlendWeights(int percentRgb, void* ctx) { + rgbWeight = float(percentRgb) / 100.f; + depthWeight = 1.f - rgbWeight; +} + +int main() { + using namespace std; + + // Create pipeline + dai::Pipeline pipeline; + dai::Device device; + std::vector queueNames; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + + auto depthAlign = pipeline.create(); + + auto rgbOut = pipeline.create(); + auto depthOut = pipeline.create(); + auto alignedDepthOut = pipeline.create(); + + rgbOut->setStreamName("rgb"); + queueNames.push_back("rgb"); + depthOut->setStreamName("depth"); + queueNames.push_back("depth"); + alignedDepthOut->setStreamName("depth_aligned"); + queueNames.push_back("depth_aligned"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setFps(fps); + camRgb->setIspScale(2, 3); + // For now, RGB needs fixed focus to properly align with depth. + // This value was used during calibration + try { + auto calibData = device.readCalibration2(); + auto lensPosition = calibData.getLensPosition(dai::CameraBoardSocket::CAM_A); + if(lensPosition) { + camRgb->initialControl.setManualFocus(lensPosition); + } + } catch(const std::exception& ex) { + std::cout << ex.what() << std::endl; + return 1; + } + + left->setResolution(monoRes); + left->setCamera("left"); + left->setFps(fps); + right->setResolution(monoRes); + right->setCamera("right"); + right->setFps(fps); + + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + + // Linking + camRgb->isp.link(rgbOut->input); + left->out.link(stereo->left); + right->out.link(stereo->right); + stereo->depth.link(depthAlign->inputDepth); + + depthAlign->outputDepthAlign.link(alignedDepthOut->input); + depthAlign->properties.alignTo = dai::CameraBoardSocket::CAM_A; + stereo->depth.link(depthOut->input); + + // Connect to device and start pipeline + device.startPipeline(pipeline); + + // Sets queues size and behavior + for(const auto& name : queueNames) { + device.getOutputQueue(name, 4, false); + } + + std::unordered_map frame; + + auto rgbWindowName = "rgb"; + auto depthWindowName = "depth"; + auto blendedWindowName = "rgb-depth"; + auto alignedDepth = "depth_aligned"; + cv::namedWindow(rgbWindowName); + cv::namedWindow(depthWindowName); + cv::namedWindow(blendedWindowName); + cv::namedWindow(alignedDepth); + int defaultValue = (int)(rgbWeight * 100); + cv::createTrackbar("RGB Weight %", blendedWindowName, &defaultValue, 100, updateBlendWeights); + + while(true) { + std::unordered_map> latestPacket; + + auto queueEvents = device.getQueueEvents(queueNames); + for(const auto& name : queueEvents) { + auto packets = device.getOutputQueue(name)->tryGetAll(); + auto count = packets.size(); + if(count > 0) { + latestPacket[name] = packets[count - 1]; + } + } + + for(const auto& name : queueNames) { + if(latestPacket.find(name) != latestPacket.end()) { + if(name == depthWindowName) { + frame[name] = latestPacket[name]->getFrame(); + } else if (name == rgbWindowName) { + frame[name] = latestPacket[name]->getCvFrame(); + } + else { + frame[name] = latestPacket[name]->getFrame(); + } + + + cv::imshow(name, frame[name]); + } + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/include/depthai/pipeline/datatype/DepthAlignConfig.hpp b/include/depthai/pipeline/datatype/DepthAlignConfig.hpp new file mode 100644 index 000000000..2a19345f5 --- /dev/null +++ b/include/depthai/pipeline/datatype/DepthAlignConfig.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +#include "depthai-shared/datatype/RawDepthAlignConfig.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +/** + * DepthAlignConfig message + */ +class DepthAlignConfig : public Buffer { + std::shared_ptr serialize() const override; + RawDepthAlignConfig& cfg; + + public: + DepthAlignConfig(); + explicit DepthAlignConfig(std::shared_ptr ptr); + virtual ~DepthAlignConfig() = default; + + /** + * Set explicit configuration. + * @param config Explicit configuration + */ + DepthAlignConfig& set(dai::RawDepthAlignConfig config); + + /** + * Retrieve configuration data for SpatialLocationCalculator. + * @returns config for SpatialLocationCalculator + */ + dai::RawDepthAlignConfig get() const; +}; + +} // namespace dai \ No newline at end of file diff --git a/include/depthai/pipeline/node/DepthAlign.hpp b/include/depthai/pipeline/node/DepthAlign.hpp new file mode 100644 index 000000000..f42c7977f --- /dev/null +++ b/include/depthai/pipeline/node/DepthAlign.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include + +// standard +#include + +// shared +#include + +#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" + +namespace dai { +namespace node { + +/** + * @brief DepthAlign node. Calculates spatial location data on a set of ROIs on depth map. + */ +class DepthAlign : public NodeCRTP { + public: + constexpr static const char* NAME = "DepthAlign"; + + protected: + Properties& getProperties(); + + private: + std::shared_ptr rawConfig; + + public: + DepthAlign(const std::shared_ptr& par, int64_t nodeId); + DepthAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props); + + /** + * Initial config to use when calculating spatial location data. + */ + DepthAlignConfig initialConfig; + + /** + * Input PointCloudConfig message with ability to modify parameters in runtime. + * Default queue is non-blocking with size 4. + */ + Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::DepthAlignConfig, false}}}; + /** + * Input message with depth data used to retrieve spatial information about detected object. + * Default queue is non-blocking with size 4. + */ + Input inputDepth{*this, "inputDepth", Input::Type::SReceiver, false, 4, true, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Outputs ImgFrame message that carries aligned depth image. + */ + Output outputDepthAlign{*this, "outputAlignedDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Passthrough message on which the calculation was performed. + * Suitable for when input queue is set to non-blocking behavior. + */ + Output passthroughDepth{*this, "passthroughDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + // Functions to set properties + /** + * Specify whether or not wait until configuration message arrives to inputConfig Input. + * @param wait True to wait for configuration message, false otherwise. + */ + [[deprecated("Use 'inputConfig.setWaitForMessage()' instead")]] void setWaitForConfigInput(bool wait); + + /** + * @see setWaitForConfigInput + * @returns True if wait for inputConfig message, false otherwise + */ + [[deprecated("Use 'inputConfig.setWaitForMessage()' instead")]] bool getWaitForConfigInput() const; +}; + +} // namespace node +} // namespace dai \ No newline at end of file diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index bf0a3dc76..7c20fe5f8 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -18,6 +18,7 @@ #include "node/Script.hpp" #include "node/SpatialDetectionNetwork.hpp" #include "node/SpatialLocationCalculator.hpp" +#include "node/DepthAlign.hpp" #include "node/StereoDepth.hpp" #include "node/SystemLogger.hpp" #include "node/ToF.hpp" diff --git a/shared/depthai-shared b/shared/depthai-shared index f03e9d9b0..6251dfc31 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit f03e9d9b08df2c0a50b8a928901ce95b14f9c174 +Subproject commit 6251dfc3198e5966169df14b0896741ffe4d3552 diff --git a/src/pipeline/datatype/DepthAlignConfig.cpp b/src/pipeline/datatype/DepthAlignConfig.cpp new file mode 100644 index 000000000..5ffd291f8 --- /dev/null +++ b/src/pipeline/datatype/DepthAlignConfig.cpp @@ -0,0 +1,21 @@ +#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" + +namespace dai { + +std::shared_ptr DepthAlignConfig::serialize() const { + return raw; +} + +DepthAlignConfig::DepthAlignConfig() : Buffer(std::make_shared()), cfg(*dynamic_cast(raw.get())) {} +DepthAlignConfig::DepthAlignConfig(std::shared_ptr ptr) : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} + +dai::RawDepthAlignConfig DepthAlignConfig::get() const { + return cfg; +} + +DepthAlignConfig& DepthAlignConfig::set(dai::RawDepthAlignConfig config) { + cfg = config; + return *this; +} + +} // namespace dai \ No newline at end of file diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index a000b205d..c50bafd3b 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -22,6 +22,7 @@ #include "depthai/pipeline/datatype/ImgFrame.hpp" #include "depthai/pipeline/datatype/NNData.hpp" #include "depthai/pipeline/datatype/SpatialImgDetections.hpp" +#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp" #include "depthai/pipeline/datatype/StereoDepthConfig.hpp" @@ -43,6 +44,7 @@ #include "depthai-shared/datatype/RawImgDetections.hpp" #include "depthai-shared/datatype/RawImgFrame.hpp" #include "depthai-shared/datatype/RawNNData.hpp" +#include "depthai-shared/datatype/RawDepthAlignConfig.hpp" #include "depthai-shared/datatype/RawSpatialImgDetections.hpp" #include "depthai-shared/datatype/RawSpatialLocationCalculatorConfig.hpp" #include "depthai-shared/datatype/RawSpatialLocations.hpp" @@ -193,6 +195,10 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* case DatatypeEnum::ToFConfig: return parseDatatype(metadataStart, serializedObjectSize, data); break; + + case DatatypeEnum::DepthAlignConfig: + return parseDatatype(metadataStart, serializedObjectSize, data); + break; } throw std::runtime_error("Bad packet, couldn't parse"); @@ -288,6 +294,10 @@ std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPa case DatatypeEnum::ToFConfig: return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); break; + + case DatatypeEnum::DepthAlignConfig: + return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); + break; } throw std::runtime_error("Bad packet, couldn't parse (invalid message type)"); diff --git a/src/pipeline/node/DepthAlign.cpp b/src/pipeline/node/DepthAlign.cpp new file mode 100644 index 000000000..08aa0f339 --- /dev/null +++ b/src/pipeline/node/DepthAlign.cpp @@ -0,0 +1,32 @@ +#include "depthai/pipeline/node/DepthAlign.hpp" + +#include "spdlog/fmt/fmt.h" + +namespace dai { +namespace node { + +DepthAlign::DepthAlign(const std::shared_ptr& par, int64_t nodeId) : DepthAlign(par, nodeId, std::make_unique()) {} +DepthAlign::DepthAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) + : NodeCRTP(par, nodeId, std::move(props)), + rawConfig(std::make_shared()), + initialConfig(rawConfig) { + setInputRefs({&inputConfig, &inputDepth}); + setOutputRefs({&outputDepthAlign, &passthroughDepth}); +} + +DepthAlign::Properties& DepthAlign::getProperties() { + properties.initialConfig = *rawConfig; + return properties; +} + +// Node properties configuration +void DepthAlign::setWaitForConfigInput(bool wait) { + inputConfig.setWaitForMessage(wait); +} + +bool DepthAlign::getWaitForConfigInput() const { + return inputConfig.getWaitForMessage(); +} + +} // namespace node +} // namespace dai \ No newline at end of file diff --git a/tests/src/image_manip_node_test.cpp b/tests/src/image_manip_node_test.cpp index e47fea211..c0819dd54 100644 --- a/tests/src/image_manip_node_test.cpp +++ b/tests/src/image_manip_node_test.cpp @@ -13,7 +13,6 @@ int main() { // Create pipeline dai::Pipeline p; - auto xin = p.create(); auto imageManip = p.create(); auto xout = p.create(); From a5dcc5ae9b86bbb831d7f676976d97a32236beaf Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 21 Jul 2023 03:22:52 +0300 Subject: [PATCH 02/80] Add RPC `readCcmEepromRaw` for ToF --- include/depthai/device/DeviceBase.hpp | 12 ++++++++++++ src/device/DeviceBase.cpp | 11 +++++++++++ 2 files changed, 23 insertions(+) diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index 327f726fe..4ccfda10f 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -787,6 +787,18 @@ class DeviceBase { */ std::vector readFactoryCalibrationRaw(); + /** + * Fetches the raw EEPROM data from the specified CCM (compact camera module). + * Note: only certain CCMs (e.g. ToF) do have an EEPROM chip on-module + * + * @param socket CameraBoardSocket where the CCM is placed + * @param size Size in bytes to read + * @param offset Absolute offset in EEPROM memory to read from + * @throws std::runtime_exception if any error occurred + * @returns Binary dump of EEPROM data + */ + std::vector readCcmEepromRaw(CameraBoardSocket socket, int size, int offset = 0); + /** * Retrieves USB connection speed * diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 6994860ff..0121ae9b2 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1291,6 +1291,17 @@ std::vector DeviceBase::readFactoryCalibrationRaw() { return eepromDataRaw; } +std::vector DeviceBase::readCcmEepromRaw(CameraBoardSocket socket, int size, int offset) { + bool success; + std::string errorMsg; + std::vector eepromDataRaw; + std::tie(success, errorMsg, eepromDataRaw) = pimpl->rpcClient->call("readCcmEepromRaw", socket, size, offset).as>>(); + if(!success) { + throw EepromError(errorMsg); + } + return eepromDataRaw; +} + void DeviceBase::flashEepromClear() { bool factoryPermissions = false; bool protectedPermissions = false; From eca83d705ff9f199a792490f6ee47ca22d739f2a Mon Sep 17 00:00:00 2001 From: Erol444 Date: Wed, 7 Feb 2024 18:25:53 +0200 Subject: [PATCH 03/80] Updated videoEnc comments so docs will be updated. --- include/depthai/pipeline/node/VideoEncoder.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/include/depthai/pipeline/node/VideoEncoder.hpp b/include/depthai/pipeline/node/VideoEncoder.hpp index 59c186e52..904584d4c 100644 --- a/include/depthai/pipeline/node/VideoEncoder.hpp +++ b/include/depthai/pipeline/node/VideoEncoder.hpp @@ -78,7 +78,7 @@ class VideoEncoder : public NodeCRTP int getNumFramesPool() const; // encoder properties - /// Set rate control mode + /// Set rate control mode Applicable only to H264 and H265 profiles void setRateControlMode(Properties::RateControlMode mode); /// Set encoding profile void setProfile(Properties::Profile profile); @@ -89,9 +89,9 @@ class VideoEncoder : public NodeCRTP [[deprecated("Input width/height no longer needed, automatically determined from first frame")]] void setProfile(int width, int height, Properties::Profile profile); - /// Set output bitrate in bps, for CBR rate control mode. 0 for auto (based on frame size and FPS) + /// Set output bitrate in bps, for CBR rate control mode. 0 for auto (based on frame size and FPS). Applicable only to H264 and H265 profiles void setBitrate(int bitrate); - /// Set output bitrate in kbps, for CBR rate control mode. 0 for auto (based on frame size and FPS) + /// Set output bitrate in kbps, for CBR rate control mode. 0 for auto (based on frame size and FPS). Applicable only to H264 and H265 profiles void setBitrateKbps(int bitrateKbps); /** @@ -108,11 +108,11 @@ class VideoEncoder : public NodeCRTP */ void setKeyframeFrequency(int freq); - /// Set number of B frames to be inserted + /// Set number of B frames to be inserted. Applicable only to H264 and H265 profiles void setNumBFrames(int numBFrames); /** - * Set quality + * Set quality for [M]JPEG profile * @param quality Value between 0-100%. Approximates quality */ void setQuality(int quality); From c7384971c4c71c6091a8ed9f34d6484cbd271a63 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 21 Feb 2024 17:55:45 +0200 Subject: [PATCH 04/80] Add writeCcmEepromRaw RPC --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/device/DeviceBase.hpp | 11 +++++++++++ src/device/DeviceBase.cpp | 12 +++++++++++- 3 files changed, 23 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index add681a8e..659c3176c 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "7e834339d54869a99f7934aa952a6892323341e8") +set(DEPTHAI_DEVICE_SIDE_COMMIT "0a198915697ffb01046886ff38d71a8ee9d0d84c") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index 4ccfda10f..c830f78b1 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -799,6 +799,17 @@ class DeviceBase { */ std::vector readCcmEepromRaw(CameraBoardSocket socket, int size, int offset = 0); + /** + * Writes the raw EEPROM data from the specified CCM (compact camera module). + * Note: only certain CCMs (e.g. ToF) do have an EEPROM chip on-module + * + * @param socket CameraBoardSocket where the CCM is placed + * @param data Data buffer to write + * @param offset Absolute offset in EEPROM memory to read from + * @throws std::runtime_exception if any error occurred + */ + void writeCcmEepromRaw(CameraBoardSocket socket, std::vector data, int offset = 0); + /** * Retrieves USB connection speed * diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 0121ae9b2..1946ae9f8 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1295,13 +1295,23 @@ std::vector DeviceBase::readCcmEepromRaw(CameraBoardSocket socket, bool success; std::string errorMsg; std::vector eepromDataRaw; - std::tie(success, errorMsg, eepromDataRaw) = pimpl->rpcClient->call("readCcmEepromRaw", socket, size, offset).as>>(); + std::tie(success, errorMsg, eepromDataRaw) = + pimpl->rpcClient->call("readCcmEepromRaw", socket, size, offset).as>>(); if(!success) { throw EepromError(errorMsg); } return eepromDataRaw; } +void DeviceBase::writeCcmEepromRaw(CameraBoardSocket socket, std::vector data, int offset) { + bool success; + std::string errorMsg; + std::tie(success, errorMsg) = pimpl->rpcClient->call("writeCcmEepromRaw", socket, data, offset).as>(); + if(!success) { + throw EepromError(errorMsg); + } +} + void DeviceBase::flashEepromClear() { bool factoryPermissions = false; bool protectedPermissions = false; From 1a48df97e1a1bae9ef7eee798d3ebbaf99d01085 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 21 Feb 2024 22:08:46 +0200 Subject: [PATCH 05/80] DataQueue: catch packet parsing errors and return a null packet instead, with an `error` log --- src/device/DataQueue.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/device/DataQueue.cpp b/src/device/DataQueue.cpp index e16f2c8a1..bf22c9c3e 100644 --- a/src/device/DataQueue.cpp +++ b/src/device/DataQueue.cpp @@ -40,8 +40,13 @@ DataOutputQueue::DataOutputQueue(const std::shared_ptr conn, co // Blocking -- parse packet and gather timing information auto packet = stream.readMove(); DatatypeEnum type; + std::shared_ptr data{}; const auto t1Parse = std::chrono::steady_clock::now(); - const auto data = StreamMessageParser::parseMessageToADatatype(&packet, type); + try { + data = StreamMessageParser::parseMessageToADatatype(&packet, type); + } catch(const std::exception& ex) { + logger::error("Caught exception on stream '{}': {}", name, ex.what()); + } if(type == DatatypeEnum::MessageGroup) { auto msgGrp = std::static_pointer_cast(data); unsigned int size = msgGrp->getNumMessages(); From 90b92acc16c898f85b2b57745453377e8336dca6 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 21 Feb 2024 22:10:33 +0200 Subject: [PATCH 06/80] StreamMessageParser: add more details (size etc) to the 'bad packet' exceptions thrown --- src/pipeline/datatype/StreamMessageParser.cpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 0ce4ad476..b083a5acf 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -80,27 +80,29 @@ inline std::shared_ptr parseDatatype(std::uint8_t* metadata, size_t size, std static std::tuple parseHeader(streamPacketDesc_t* const packet) { if(packet->length < 8) { - throw std::runtime_error("Bad packet, couldn't parse (not enough data)"); + throw std::runtime_error(fmt::format("Bad packet, couldn't parse (not enough data), total size {}", packet->length)); } const int serializedObjectSize = readIntLE(packet->data + packet->length - 4); const auto objectType = static_cast(readIntLE(packet->data + packet->length - 8)); + const auto info = fmt::format(", total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize); + if(serializedObjectSize < 0) { - throw std::runtime_error("Bad packet, couldn't parse (metadata size negative)"); + throw std::runtime_error("Bad packet, couldn't parse (metadata size negative)" + info); } else if(serializedObjectSize > static_cast(packet->length)) { - throw std::runtime_error("Bad packet, couldn't parse (metadata size larger than packet length)"); + throw std::runtime_error("Bad packet, couldn't parse (metadata size larger than packet length)" + info); } if(static_cast(packet->length) - 8 - serializedObjectSize < 0) { - throw std::runtime_error("Bad packet, couldn't parse (data too small)"); + throw std::runtime_error("Bad packet, couldn't parse (data too small)" + info); } const std::uint32_t bufferLength = packet->length - 8 - serializedObjectSize; if(bufferLength > packet->length) { - throw std::runtime_error("Bad packet, couldn't parse (data too large)"); + throw std::runtime_error("Bad packet, couldn't parse (data too large)" + info); } auto* const metadataStart = packet->data + bufferLength; if(metadataStart < packet->data || metadataStart >= packet->data + packet->length) { - throw std::runtime_error("Bad packet, couldn't parse (metadata out of bounds)"); + throw std::runtime_error("Bad packet, couldn't parse (metadata out of bounds)" + info); } return {objectType, serializedObjectSize, bufferLength}; @@ -202,7 +204,8 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* break; } - throw std::runtime_error("Bad packet, couldn't parse"); + throw std::runtime_error( + fmt::format("Bad packet, couldn't parse, total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize)); } std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* const packet, DatatypeEnum& objectType) { @@ -300,8 +303,10 @@ std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPa break; } - throw std::runtime_error("Bad packet, couldn't parse (invalid message type)"); + throw std::runtime_error(fmt::format( + "Bad packet, couldn't parse (invalid message type), total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize)); } + std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* const packet) { DatatypeEnum objectType; return parseMessageToADatatype(packet, objectType); From 20e94ff3167913964771726321c24bb22b04e6e9 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 23 Feb 2024 19:33:49 +0200 Subject: [PATCH 07/80] ToF: add phase map output --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/ToF.hpp | 1 + src/pipeline/node/ToF.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 659c3176c..34f8d5b26 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "0a198915697ffb01046886ff38d71a8ee9d0d84c") +set(DEPTHAI_DEVICE_SIDE_COMMIT "d05c16c68b95c638cc88854292b257d0bafa466c") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/ToF.hpp b/include/depthai/pipeline/node/ToF.hpp index d4e01a7d2..75d47dff7 100644 --- a/include/depthai/pipeline/node/ToF.hpp +++ b/include/depthai/pipeline/node/ToF.hpp @@ -49,6 +49,7 @@ class ToF : public NodeCRTP { Output depth{*this, "depth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; Output amplitude{*this, "amplitude", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; Output intensity{*this, "intensity", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + Output phase{*this, "phase", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; Output error{*this, "error", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; }; diff --git a/src/pipeline/node/ToF.cpp b/src/pipeline/node/ToF.cpp index 86dddc0f4..872ddaabd 100644 --- a/src/pipeline/node/ToF.cpp +++ b/src/pipeline/node/ToF.cpp @@ -7,7 +7,7 @@ ToF::ToF(const std::shared_ptr& par, int64_t nodeId) : ToF(par, no ToF::ToF(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) : NodeCRTP(par, nodeId, std::move(props)), rawConfig(std::make_shared()), initialConfig(rawConfig) { setInputRefs({&inputConfig, &input}); - setOutputRefs({&depth, &litude, &intensity, &error}); + setOutputRefs({&depth, &litude, &intensity, &phase, &error}); } ToF::Properties& ToF::getProperties() { From c405422e063c062a6c14492a2dc2c570c362695a Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 4 Mar 2024 17:27:14 +0200 Subject: [PATCH 08/80] Rename functions, update shared --- examples/CMakeLists.txt | 2 +- include/depthai/pipeline/node/DepthAlign.hpp | 14 +------------- shared/depthai-shared | 2 +- src/pipeline/node/DepthAlign.cpp | 11 +---------- 4 files changed, 4 insertions(+), 25 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 2295de2e5..4d4243447 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -244,7 +244,7 @@ dai_add_example(edge_detector EdgeDetector/edge_detector.cpp ON OFF) dai_add_example(feature_detector FeatureTracker/feature_detector.cpp ON OFF) dai_add_example(feature_tracker FeatureTracker/feature_tracker.cpp ON OFF) -dai_add_example(depth_align DepthAlign/rgb_depth_align.cpp ON) +# dai_add_example(depth_align DepthAlign/rgb_depth_align.cpp ON) # host_side dai_add_example(device_queue_event host_side/device_queue_event.cpp ON OFF) diff --git a/include/depthai/pipeline/node/DepthAlign.hpp b/include/depthai/pipeline/node/DepthAlign.hpp index f42c7977f..bdf482351 100644 --- a/include/depthai/pipeline/node/DepthAlign.hpp +++ b/include/depthai/pipeline/node/DepthAlign.hpp @@ -49,7 +49,7 @@ class DepthAlign : public NodeCRTP { /** * Outputs ImgFrame message that carries aligned depth image. */ - Output outputDepthAlign{*this, "outputAlignedDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + Output outputAlignedDepth{*this, "outputAlignedDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; /** * Passthrough message on which the calculation was performed. @@ -57,18 +57,6 @@ class DepthAlign : public NodeCRTP { */ Output passthroughDepth{*this, "passthroughDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; - // Functions to set properties - /** - * Specify whether or not wait until configuration message arrives to inputConfig Input. - * @param wait True to wait for configuration message, false otherwise. - */ - [[deprecated("Use 'inputConfig.setWaitForMessage()' instead")]] void setWaitForConfigInput(bool wait); - - /** - * @see setWaitForConfigInput - * @returns True if wait for inputConfig message, false otherwise - */ - [[deprecated("Use 'inputConfig.setWaitForMessage()' instead")]] bool getWaitForConfigInput() const; }; } // namespace node diff --git a/shared/depthai-shared b/shared/depthai-shared index 207cedc5c..55a7f52db 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 207cedc5c3cff94a8c000e8fb129758e5fa6d039 +Subproject commit 55a7f52db413524a83057fbfc5a9081852324018 diff --git a/src/pipeline/node/DepthAlign.cpp b/src/pipeline/node/DepthAlign.cpp index 08aa0f339..a0f06e03f 100644 --- a/src/pipeline/node/DepthAlign.cpp +++ b/src/pipeline/node/DepthAlign.cpp @@ -11,7 +11,7 @@ DepthAlign::DepthAlign(const std::shared_ptr& par, int64_t nodeId, rawConfig(std::make_shared()), initialConfig(rawConfig) { setInputRefs({&inputConfig, &inputDepth}); - setOutputRefs({&outputDepthAlign, &passthroughDepth}); + setOutputRefs({&outputAlignedDepth, &passthroughDepth}); } DepthAlign::Properties& DepthAlign::getProperties() { @@ -19,14 +19,5 @@ DepthAlign::Properties& DepthAlign::getProperties() { return properties; } -// Node properties configuration -void DepthAlign::setWaitForConfigInput(bool wait) { - inputConfig.setWaitForMessage(wait); -} - -bool DepthAlign::getWaitForConfigInput() const { - return inputConfig.getWaitForMessage(); -} - } // namespace node } // namespace dai \ No newline at end of file From 2ef170739b7ebaa6658c3a95674fbbfa74f85279 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 4 Mar 2024 18:15:44 +0200 Subject: [PATCH 09/80] Update depth align node, fix crash --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/Camera/thermal_cam.cpp | 13 +++++++------ include/depthai/pipeline/node/DepthAlign.hpp | 10 ++++++++++ include/depthai/pipeline/nodes.hpp | 2 +- src/pipeline/datatype/StreamMessageParser.cpp | 4 ++-- src/pipeline/node/DepthAlign.cpp | 11 +++++++++++ 6 files changed, 32 insertions(+), 10 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index d1f8eb6a6..8101e12a2 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "cc5f1c3599470968a8dfecc47cca2689f73a0729") +set(DEPTHAI_DEVICE_SIDE_COMMIT "04d3dee6bed377ae5cc835dca226218d85ddde10") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/Camera/thermal_cam.cpp b/examples/Camera/thermal_cam.cpp index 8178655e8..2011dff5a 100644 --- a/examples/Camera/thermal_cam.cpp +++ b/examples/Camera/thermal_cam.cpp @@ -22,17 +22,18 @@ int main() { // Find the sensor width, height. int width, height; bool thermal_found = false; - for (auto &features : d.getConnectedCameraFeatures()) { - if (std::find_if(features.supportedTypes.begin(), features.supportedTypes.end(), [](const dai::CameraSensorType &type) { - return type == dai::CameraSensorType::THERMAL; - }) != features.supportedTypes.end()) { - thermal->setBoardSocket(features.socket); // Thermal will always be on CAM_E + for(auto& features : d.getConnectedCameraFeatures()) { + if(std::find_if(features.supportedTypes.begin(), + features.supportedTypes.end(), + [](const dai::CameraSensorType& type) { return type == dai::CameraSensorType::THERMAL; }) + != features.supportedTypes.end()) { + thermal->setBoardSocket(features.socket); // Thermal will always be on CAM_E width = features.width; height = features.height; thermal_found = true; } } - if (!thermal_found) { + if(!thermal_found) { throw std::runtime_error("Thermal camera not found!"); } thermal->setPreviewSize(width, height); diff --git a/include/depthai/pipeline/node/DepthAlign.hpp b/include/depthai/pipeline/node/DepthAlign.hpp index bdf482351..6fab755ed 100644 --- a/include/depthai/pipeline/node/DepthAlign.hpp +++ b/include/depthai/pipeline/node/DepthAlign.hpp @@ -57,6 +57,16 @@ class DepthAlign : public NodeCRTP { */ Output passthroughDepth{*this, "passthroughDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Specify to which camera socket the depth map is aligned to + */ + DepthAlign& setAlignTo(CameraBoardSocket alignTo); + + /** + * Specify the output size of the aligned depth map + */ + DepthAlign& setOutputSize(int width, int height); + }; } // namespace node diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 68ba9e50f..546a28dee 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -4,6 +4,7 @@ #include "node/AprilTag.hpp" #include "node/Camera.hpp" #include "node/ColorCamera.hpp" +#include "node/DepthAlign.hpp" #include "node/DetectionNetwork.hpp" #include "node/DetectionParser.hpp" #include "node/EdgeDetector.hpp" @@ -20,7 +21,6 @@ #include "node/Script.hpp" #include "node/SpatialDetectionNetwork.hpp" #include "node/SpatialLocationCalculator.hpp" -#include "node/DepthAlign.hpp" #include "node/StereoDepth.hpp" #include "node/Sync.hpp" #include "node/SystemLogger.hpp" diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index a046b101d..febe0ba34 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -14,6 +14,7 @@ #include "depthai/pipeline/datatype/AprilTags.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" +#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" #include "depthai/pipeline/datatype/EdgeDetectorConfig.hpp" #include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" @@ -26,7 +27,6 @@ #include "depthai/pipeline/datatype/PointCloudConfig.hpp" #include "depthai/pipeline/datatype/PointCloudData.hpp" #include "depthai/pipeline/datatype/SpatialImgDetections.hpp" -#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorConfig.hpp" #include "depthai/pipeline/datatype/SpatialLocationCalculatorData.hpp" #include "depthai/pipeline/datatype/StereoDepthConfig.hpp" @@ -41,6 +41,7 @@ #include "depthai-shared/datatype/RawAprilTags.hpp" #include "depthai-shared/datatype/RawBuffer.hpp" #include "depthai-shared/datatype/RawCameraControl.hpp" +#include "depthai-shared/datatype/RawDepthAlignConfig.hpp" #include "depthai-shared/datatype/RawEdgeDetectorConfig.hpp" #include "depthai-shared/datatype/RawEncodedFrame.hpp" #include "depthai-shared/datatype/RawFeatureTrackerConfig.hpp" @@ -52,7 +53,6 @@ #include "depthai-shared/datatype/RawNNData.hpp" #include "depthai-shared/datatype/RawPointCloudConfig.hpp" #include "depthai-shared/datatype/RawPointCloudData.hpp" -#include "depthai-shared/datatype/RawDepthAlignConfig.hpp" #include "depthai-shared/datatype/RawSpatialImgDetections.hpp" #include "depthai-shared/datatype/RawSpatialLocationCalculatorConfig.hpp" #include "depthai-shared/datatype/RawSpatialLocations.hpp" diff --git a/src/pipeline/node/DepthAlign.cpp b/src/pipeline/node/DepthAlign.cpp index a0f06e03f..46d1b373a 100644 --- a/src/pipeline/node/DepthAlign.cpp +++ b/src/pipeline/node/DepthAlign.cpp @@ -19,5 +19,16 @@ DepthAlign::Properties& DepthAlign::getProperties() { return properties; } +DepthAlign& DepthAlign::setAlignTo(CameraBoardSocket alignTo) { + properties.alignTo = alignTo; + return *this; +} + +DepthAlign& DepthAlign::setOutputSize(int width, int height) { + properties.alignWidth = alignWidth; + properties.alignHeight = alignHeight; + return *this; +} + } // namespace node } // namespace dai \ No newline at end of file From 272be36cd56101cb02719f6525615dc79d853e0e Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 8 Mar 2024 14:14:04 +0200 Subject: [PATCH 10/80] TMP FW with more logging for bad-packet debugging. Fix ImageManip potential OsDrvCpr crash --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 45508a27e..06dca192a 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "d86f2efa04753e0e47c153db678753afa9a630a0") +set(DEPTHAI_DEVICE_SIDE_COMMIT "00911c9808f8036c260149049010a29abc46d692") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 338bd26fdea08de75665b3cfd8b1580e4c789f8b Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 8 Mar 2024 20:41:24 +0200 Subject: [PATCH 11/80] Add decoding pipeline --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/datatype/ToFConfig.hpp | 7 +++++++ shared/depthai-shared | 2 +- src/pipeline/datatype/ToFConfig.cpp | 6 ++++++ 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 34f8d5b26..0d6f23015 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "d05c16c68b95c638cc88854292b257d0bafa466c") +set(DEPTHAI_DEVICE_SIDE_COMMIT "cc3f76a5b3b74bdcda426d6bd6068c9b3f7ed15b") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/datatype/ToFConfig.hpp b/include/depthai/pipeline/datatype/ToFConfig.hpp index 0d877ee99..df3a6757c 100644 --- a/include/depthai/pipeline/datatype/ToFConfig.hpp +++ b/include/depthai/pipeline/datatype/ToFConfig.hpp @@ -35,6 +35,13 @@ class ToFConfig : public Buffer { */ ToFConfig& setMedianFilter(MedianFilter median); + /** + * Set temperature coefficients for depth calculation + * @param A Temperature coefficient A + * @param B Temperature coefficient B + */ + ToFConfig& setTemperatureCoefficients(float A, float B); + /** * Set explicit configuration. * @param config Explicit configuration diff --git a/shared/depthai-shared b/shared/depthai-shared index 147468b12..2ce17f397 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 147468b124b3c30e40f69239e3560afca782ba6b +Subproject commit 2ce17f397454dd06242f066ccca2251003294e51 diff --git a/src/pipeline/datatype/ToFConfig.cpp b/src/pipeline/datatype/ToFConfig.cpp index da1ce41ed..2290e4f54 100644 --- a/src/pipeline/datatype/ToFConfig.cpp +++ b/src/pipeline/datatype/ToFConfig.cpp @@ -43,4 +43,10 @@ ToFConfig& ToFConfig::setMedianFilter(MedianFilter median) { return *this; } +ToFConfig& ToFConfig::setTemperatureCoefficients(float A, float B) { + cfg.depthParams.temperatureCoefficientA = A; + cfg.depthParams.temperatureCoefficientB = B; + return *this; +} + } // namespace dai From 981324aaf97f25cb21595cbe405f9559881e7150 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 12 Mar 2024 22:15:56 +0200 Subject: [PATCH 12/80] Try to fix a FW memory alignment issue --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- src/pipeline/datatype/StreamMessageParser.cpp | 17 +++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 06dca192a..504f5b060 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "00911c9808f8036c260149049010a29abc46d692") +set(DEPTHAI_DEVICE_SIDE_COMMIT "407c31132bbe0d29d991972995b1dc3740ea33ba") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index b083a5acf..109fe8f0b 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -79,29 +79,30 @@ inline std::shared_ptr parseDatatype(std::uint8_t* metadata, size_t size, std } static std::tuple parseHeader(streamPacketDesc_t* const packet) { - if(packet->length < 8) { + if(packet->length < 24) { throw std::runtime_error(fmt::format("Bad packet, couldn't parse (not enough data), total size {}", packet->length)); } - const int serializedObjectSize = readIntLE(packet->data + packet->length - 4); - const auto objectType = static_cast(readIntLE(packet->data + packet->length - 8)); + const std::uint32_t packetLength = packet->length - 16; + const int serializedObjectSize = readIntLE(packet->data + packetLength - 4); + const auto objectType = static_cast(readIntLE(packet->data + packetLength - 8)); const auto info = fmt::format(", total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize); if(serializedObjectSize < 0) { throw std::runtime_error("Bad packet, couldn't parse (metadata size negative)" + info); - } else if(serializedObjectSize > static_cast(packet->length)) { + } else if(serializedObjectSize > static_cast(packetLength)) { throw std::runtime_error("Bad packet, couldn't parse (metadata size larger than packet length)" + info); } - if(static_cast(packet->length) - 8 - serializedObjectSize < 0) { + if(static_cast(packetLength) - 8 - serializedObjectSize < 0) { throw std::runtime_error("Bad packet, couldn't parse (data too small)" + info); } - const std::uint32_t bufferLength = packet->length - 8 - serializedObjectSize; - if(bufferLength > packet->length) { + const std::uint32_t bufferLength = packetLength - 8 - serializedObjectSize; + if(bufferLength > packetLength) { throw std::runtime_error("Bad packet, couldn't parse (data too large)" + info); } auto* const metadataStart = packet->data + bufferLength; - if(metadataStart < packet->data || metadataStart >= packet->data + packet->length) { + if(metadataStart < packet->data || metadataStart >= packet->data + packetLength) { throw std::runtime_error("Bad packet, couldn't parse (metadata out of bounds)" + info); } From 181bc42d8cd1bdb49e91407305d0833f7fb38e08 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Wed, 13 Mar 2024 12:14:24 +0000 Subject: [PATCH 13/80] fix finding jsoncpp --- cmake/depthaiDependencies.cmake | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index abff1f6f3..e398023a8 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -76,8 +76,9 @@ endif() # OpenCV 4 - (optional, quiet always) find_package(OpenCV 4 QUIET CONFIG) - -find_package(jsoncpp QUIET) +if(NOT TARGET JsonCpp::JsonCpp) + find_package(jsoncpp QUIET) +endif() set(MODULE_TEMP ${CMAKE_MODULE_PATH}) set(PREFIX_TEMP ${CMAKE_PREFIX_PATH}) set(CMAKE_MODULE_PATH ${_DEPTHAI_MODULE_PATH_ORIGINAL}) From 12766a70b0e02bf599fa97096163996af06b5418 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 19 Mar 2024 19:13:52 +0200 Subject: [PATCH 14/80] FW: fix fsync for OAK-D-SR --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 1a61b82b6..283afc134 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "7665aed8712bb941958660a0103868908726eb56") +set(DEPTHAI_DEVICE_SIDE_COMMIT "e9f7c1cde3be3471cf7185af2bcb5a66eba57227") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 6cdc0125d748678d0950453fd38a629ad50b67ec Mon Sep 17 00:00:00 2001 From: asahtik Date: Wed, 20 Mar 2024 09:39:23 +0100 Subject: [PATCH 15/80] Change IMU fw update status to int --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/IMU/imu_firmware_update.cpp | 2 +- include/depthai/device/DeviceBase.hpp | 2 +- src/device/DeviceBase.cpp | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 283afc134..8bf5bc71b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "e9f7c1cde3be3471cf7185af2bcb5a66eba57227") +set(DEPTHAI_DEVICE_SIDE_COMMIT "daef726af020ee2dd9c84c6beee4a9567ff415aa") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/IMU/imu_firmware_update.cpp b/examples/IMU/imu_firmware_update.cpp index e95007bd6..49df1a673 100644 --- a/examples/IMU/imu_firmware_update.cpp +++ b/examples/IMU/imu_firmware_update.cpp @@ -36,7 +36,7 @@ int main() { while(true) { bool fwUpdateFinished; - float percentage; + unsigned int percentage; std::tie(fwUpdateFinished, percentage) = device.getIMUFirmwareUpdateStatus(); std::cout << "IMU FW update status: " << std::setprecision(1) << percentage << std::endl; if(fwUpdateFinished) { diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index b30f1c0fe..380812be2 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -673,7 +673,7 @@ class DeviceBase { * return value true and 100 means that the update was successful * return value true and other than 100 means that the update failed */ - std::tuple getIMUFirmwareUpdateStatus(); + std::tuple getIMUFirmwareUpdateStatus(); /** * Retrieves current DDR memory information from device diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 399f1f998..6f6dfb487 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1158,8 +1158,8 @@ bool DeviceBase::startIMUFirmwareUpdate(bool forceUpdate) { return pimpl->rpcClient->call("startIMUFirmwareUpdate", forceUpdate).as(); } -std::tuple DeviceBase::getIMUFirmwareUpdateStatus() { - return pimpl->rpcClient->call("getIMUFirmwareUpdateStatus").as>(); +std::tuple DeviceBase::getIMUFirmwareUpdateStatus() { + return pimpl->rpcClient->call("getIMUFirmwareUpdateStatus").as>(); } // Convenience functions for querying current system information From 21a2dc8c30b12817b6031623b69242123fcfdaee Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 23 Mar 2024 17:41:37 +0200 Subject: [PATCH 16/80] Add cast node --- CMakeLists.txt | 1 + cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/Cast.hpp | 61 +++++++++++++++++++++ shared/depthai-shared | 2 +- src/pipeline/node/Cast.cpp | 31 +++++++++++ 5 files changed, 95 insertions(+), 2 deletions(-) create mode 100644 include/depthai/pipeline/node/Cast.hpp create mode 100644 src/pipeline/node/Cast.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 184a680de..ec3c21e36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -230,6 +230,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/DetectionParser.cpp src/pipeline/node/UVC.cpp src/pipeline/node/PointCloud.cpp + src/pipeline/node/Cast.cpp src/pipeline/datatype/Buffer.cpp src/pipeline/datatype/ImgFrame.cpp src/pipeline/datatype/EncodedFrame.cpp diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 283afc134..731a879c7 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "e9f7c1cde3be3471cf7185af2bcb5a66eba57227") +set(DEPTHAI_DEVICE_SIDE_COMMIT "f8762f81bf92bec6aed5446b70eba133d385e45b") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/Cast.hpp b/include/depthai/pipeline/node/Cast.hpp new file mode 100644 index 000000000..ce5080518 --- /dev/null +++ b/include/depthai/pipeline/node/Cast.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include + +#include "depthai/pipeline/datatype/Tracklets.hpp" + +// standard +#include + +// shared +#include + +namespace dai { +namespace node { + +/** + * @brief Cast node. + */ +class Cast : public NodeCRTP { + public: + constexpr static const char* NAME = "Cast"; + + Cast(const std::shared_ptr& par, int64_t nodeId); + Cast(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props); + + /** + * Input NNData or ImgFrame message. + */ + Input input{*this, "input", Input::Type::SReceiver, false, 4, true, {{DatatypeEnum::ImgFrame, false}, {DatatypeEnum::NNData, false}}}; + + /** + * Output ImgFrame message. + */ + Output output{*this, "output", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Passthrough input message. + */ + Output passthroughInput{*this, "passthroughInput", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}, {DatatypeEnum::NNData, false}}}; + + /** + * Set number of frames in pool + * @param numFramesPool Number of frames in pool + */ + Cast& setNumFramesPool(int numFramesPool); + + /** + * Set output frame type + * @param outputType Output frame type + */ + Cast& setOutputFrameType(dai::RawImgFrame::Type outputType); + + /** + * Set scale + * @param scale Scale + */ + Cast& setScale(float scale); +}; + +} // namespace node +} // namespace dai diff --git a/shared/depthai-shared b/shared/depthai-shared index 4534104a5..148717798 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 4534104a58054357f38abb1618f9b82a059bfad1 +Subproject commit 148717798b117d48e294c5554bd4adaa8dbed93f diff --git a/src/pipeline/node/Cast.cpp b/src/pipeline/node/Cast.cpp new file mode 100644 index 000000000..a135b61b5 --- /dev/null +++ b/src/pipeline/node/Cast.cpp @@ -0,0 +1,31 @@ +#include "depthai/pipeline/node/Cast.hpp" + +#include "spdlog/fmt/fmt.h" + +namespace dai { +namespace node { + +Cast::Cast(const std::shared_ptr& par, int64_t nodeId) : Cast(par, nodeId, std::make_unique()) {} +Cast::Cast(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) + : NodeCRTP(par, nodeId, std::move(props)) { + setInputRefs({&input}); + setOutputRefs({&output, &passthroughInput}); +} + +Cast& Cast::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; + return *this; +} + +Cast& Cast::setOutputFrameType(dai::RawImgFrame::Type outputType) { + properties.outputType = outputType; + return *this; +} + +Cast& Cast::setScale(float scale) { + properties.scale = scale; + return *this; +} + +} // namespace node +} // namespace dai From a8ea8d25b2620bfa42a942cc6ad80d1cc9ecc21e Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 26 Mar 2024 19:55:08 +0100 Subject: [PATCH 17/80] Update Cast node with fixes --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 731a879c7..d141d0854 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "f8762f81bf92bec6aed5446b70eba133d385e45b") +set(DEPTHAI_DEVICE_SIDE_COMMIT "605494049fdc0660b900ea660e60753e24ddc8d6") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 10e6107cbfd61c205e0eb7820e020410ca29960a Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 27 Mar 2024 12:26:13 +0100 Subject: [PATCH 18/80] Handle multi input NN --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index d141d0854..656f7be0b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "605494049fdc0660b900ea660e60753e24ddc8d6") +set(DEPTHAI_DEVICE_SIDE_COMMIT "f134c967666e57000a11ba9dfffc6ddea3c5ef97") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 72af37bc860023b2b27f276044569a66ce54374f Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 28 Mar 2024 16:45:18 +0100 Subject: [PATCH 19/80] Update ToF decoding algorithm --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/datatype/ToFConfig.hpp | 7 ------- shared/depthai-shared | 2 +- src/pipeline/datatype/ToFConfig.cpp | 6 ------ 4 files changed, 2 insertions(+), 15 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 0d6f23015..96bdb4ecd 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "cc3f76a5b3b74bdcda426d6bd6068c9b3f7ed15b") +set(DEPTHAI_DEVICE_SIDE_COMMIT "a1f99767dcda742cfc580ea756ab58adfacf0fac") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/datatype/ToFConfig.hpp b/include/depthai/pipeline/datatype/ToFConfig.hpp index df3a6757c..0d877ee99 100644 --- a/include/depthai/pipeline/datatype/ToFConfig.hpp +++ b/include/depthai/pipeline/datatype/ToFConfig.hpp @@ -35,13 +35,6 @@ class ToFConfig : public Buffer { */ ToFConfig& setMedianFilter(MedianFilter median); - /** - * Set temperature coefficients for depth calculation - * @param A Temperature coefficient A - * @param B Temperature coefficient B - */ - ToFConfig& setTemperatureCoefficients(float A, float B); - /** * Set explicit configuration. * @param config Explicit configuration diff --git a/shared/depthai-shared b/shared/depthai-shared index 2ce17f397..77e1c2daa 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 2ce17f397454dd06242f066ccca2251003294e51 +Subproject commit 77e1c2daa1d6bc2253483a76b1667fd931f5f184 diff --git a/src/pipeline/datatype/ToFConfig.cpp b/src/pipeline/datatype/ToFConfig.cpp index 2290e4f54..da1ce41ed 100644 --- a/src/pipeline/datatype/ToFConfig.cpp +++ b/src/pipeline/datatype/ToFConfig.cpp @@ -43,10 +43,4 @@ ToFConfig& ToFConfig::setMedianFilter(MedianFilter median) { return *this; } -ToFConfig& ToFConfig::setTemperatureCoefficients(float A, float B) { - cfg.depthParams.temperatureCoefficientA = A; - cfg.depthParams.temperatureCoefficientB = B; - return *this; -} - } // namespace dai From 7f5eea57745611d1f2e09371f1bc4c800acb04fd Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 29 Mar 2024 16:52:01 +0100 Subject: [PATCH 20/80] Update FW; fixed optical correction --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 96bdb4ecd..aa4936a09 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "a1f99767dcda742cfc580ea756ab58adfacf0fac") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b8c593079de6b9059d48b6261523df874a16b748") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 92ad11d2e5492c7548059470157aede9c1e20bba Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 1 Apr 2024 15:47:00 +0300 Subject: [PATCH 21/80] Optimize wiggle correction --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index aa4936a09..44d48e24c 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "b8c593079de6b9059d48b6261523df874a16b748") +set(DEPTHAI_DEVICE_SIDE_COMMIT "bdc411045d1ca178466971c9624a5122870f16d0") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 1e57cb6ea83bd77d4efbd04a1b50b767199a1b46 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 1 Apr 2024 20:43:02 +0300 Subject: [PATCH 22/80] Handle edge cases; unsupported distortion coefficients --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 44d48e24c..89b3fef3a 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "bdc411045d1ca178466971c9624a5122870f16d0") +set(DEPTHAI_DEVICE_SIDE_COMMIT "8002bb03e3176d8f94f69aceb9e227804818d969") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 62d7b5a5b19cc8f7c47a444026612b83878cf1c9 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 3 Apr 2024 19:59:44 +0300 Subject: [PATCH 23/80] Add tilt projection support --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 283afc134..2724b0244 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "e9f7c1cde3be3471cf7185af2bcb5a66eba57227") +set(DEPTHAI_DEVICE_SIDE_COMMIT "7a11ef8a21cfd73ed890a146c44c6ab8206cca47") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 40721447193748b6214546e7e2429d98feffaaea Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 9 Apr 2024 22:56:15 +0300 Subject: [PATCH 24/80] Add phase unwrapping --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- .../depthai/pipeline/datatype/ToFConfig.hpp | 7 ------ shared/depthai-shared | 2 +- src/pipeline/datatype/ToFConfig.cpp | 22 +------------------ 4 files changed, 3 insertions(+), 30 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 5d6768707..77a2e3d1b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "76606927e89c7b6eaa317d9b7fc80bc26376aaa6") +set(DEPTHAI_DEVICE_SIDE_COMMIT "afc2923cc0d951335c56d1944a02a00daa41157e") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/datatype/ToFConfig.hpp b/include/depthai/pipeline/datatype/ToFConfig.hpp index 0d877ee99..dc19a9312 100644 --- a/include/depthai/pipeline/datatype/ToFConfig.hpp +++ b/include/depthai/pipeline/datatype/ToFConfig.hpp @@ -16,9 +16,6 @@ class ToFConfig : public Buffer { RawToFConfig& cfg; public: - // Raw* mirror - using DepthParams = RawToFConfig::DepthParams; - /** * Construct ToFConfig message. */ @@ -26,10 +23,6 @@ class ToFConfig : public Buffer { explicit ToFConfig(std::shared_ptr ptr); virtual ~ToFConfig() = default; - ToFConfig& setDepthParams(dai::ToFConfig::DepthParams config); - ToFConfig& setFreqModUsed(dai::ToFConfig::DepthParams::TypeFMod fmod); - ToFConfig& setAvgPhaseShuffle(bool enable); - ToFConfig& setMinAmplitude(float minamp); /** * @param median Set kernel size for median filtering, or disable */ diff --git a/shared/depthai-shared b/shared/depthai-shared index 1728a3bbe..f77ed0719 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 1728a3bbe29490078de1198214e3cb75d6c6aa95 +Subproject commit f77ed07190d60902728433a0d4afc93759720506 diff --git a/src/pipeline/datatype/ToFConfig.cpp b/src/pipeline/datatype/ToFConfig.cpp index da1ce41ed..bf530291e 100644 --- a/src/pipeline/datatype/ToFConfig.cpp +++ b/src/pipeline/datatype/ToFConfig.cpp @@ -13,33 +13,13 @@ dai::RawToFConfig ToFConfig::get() const { return cfg; } -ToFConfig& ToFConfig::setDepthParams(dai::ToFConfig::DepthParams config) { - cfg.depthParams = config; - return *this; -} - -ToFConfig& ToFConfig::setFreqModUsed(dai::ToFConfig::DepthParams::TypeFMod fmod) { - cfg.depthParams.freqModUsed = fmod; - return *this; -} - -ToFConfig& ToFConfig::setAvgPhaseShuffle(bool enable) { - cfg.depthParams.avgPhaseShuffle = enable; - return *this; -} - ToFConfig& ToFConfig::set(dai::RawToFConfig config) { cfg = config; return *this; } -ToFConfig& ToFConfig::setMinAmplitude(float minamp) { - cfg.depthParams.minimumAmplitude = minamp; - return *this; -} - ToFConfig& ToFConfig::setMedianFilter(MedianFilter median) { - cfg.depthParams.median = median; + cfg.median = median; return *this; } From 361d5d6f97b3d0a323c041cc6ca91ef378114562 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 10 Apr 2024 01:03:42 +0300 Subject: [PATCH 25/80] Update phase unwrapping, add configurable levels --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- shared/depthai-shared | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 77a2e3d1b..c25456501 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "afc2923cc0d951335c56d1944a02a00daa41157e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "e0739d876920b06c2d30f5f2d72e500eed8791e2") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index f77ed0719..810904080 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit f77ed07190d60902728433a0d4afc93759720506 +Subproject commit 81090408061e1fe3c241f889efb5ff4564138022 From 028115fcd79e1994509424843167e4bcd54294f9 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 10 Apr 2024 16:56:13 +0300 Subject: [PATCH 26/80] Device: add RPC setXLinkRateLimit for device transmit bandwidth throttling --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/device/DeviceBase.hpp | 10 ++++++++++ src/device/DeviceBase.cpp | 4 ++++ 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 504f5b060..d5dec56cf 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "407c31132bbe0d29d991972995b1dc3740ea33ba") +set(DEPTHAI_DEVICE_SIDE_COMMIT "bf8462d7859a8180ad83dd9deadc57a134e2ed61") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index b30f1c0fe..c9c1c363b 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -446,6 +446,16 @@ class DeviceBase { */ int getXLinkChunkSize(); + /** + * Sets the maximum transmission rate for the XLink connection on device side, + * using a simple token bucket algorithm. Useful for bandwidth throttling + * + * @param maxRateBytesPerSecond Rate limit in Bytes/second + * @param burstSize Size in Bytes for how much to attempt to send once, 0 = auto + * @param waitUs Time in microseconds to wait for replenishing tokens, 0 = auto + */ + void setXLinkRateLimit(int maxRateBytesPerSecond, int burstSize = 0, int waitUs = 0); + /** * Get the Device Info object o the device which is currently running * diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 91d24a553..db01f1ffc 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1221,6 +1221,10 @@ int DeviceBase::getXLinkChunkSize() { return pimpl->rpcClient->call("getXLinkChunkSize").as(); } +void DeviceBase::setXLinkRateLimit(int maxRateBytesPerSecond, int burstSize, int waitUs) { + pimpl->rpcClient->call("setXLinkRateLimit", maxRateBytesPerSecond, burstSize, waitUs); +} + DeviceInfo DeviceBase::getDeviceInfo() const { return deviceInfo; } From 33405d7e536d0aa7048a95fbacc664f46690960e Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Thu, 11 Apr 2024 12:44:05 +0200 Subject: [PATCH 27/80] Introduced `DeviceBootloader::getFlashedVersion` to retrieve the bootloader version flashed on the device. Fixed `isUserBootloaderSupported` - thereby fixed user bootloader flashing. (#997) * Introduced DeviceBootloader::getFlashedVersion to retrieve the bootloader version flashed on the device. Fixed isUserBootloaderSupported - thereby fixed user bootloader flashing. * Applied the suggestions * Fix sensor configs for ov9282/ov9782 (differentiate between color/mono when possible) --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/device/DeviceBootloader.hpp | 9 ++++++- src/device/DeviceBootloader.cpp | 26 +++++++++++++++++---- 3 files changed, 31 insertions(+), 6 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 2724b0244..19a80d226 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "7a11ef8a21cfd73ed890a146c44c6ab8206cca47") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c58546bf837efe2ffc1befb3dffc7d2c0a69fdec") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index 4b0282fb8..435bc1823 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -441,6 +441,12 @@ class DeviceBootloader { */ Version getVersion() const; + /** + * @return Version of the bootloader that is flashed on the device. + * Nullopt when the version could not be retrieved because the device was in X_LINK_UNBOOTED state before booting the bootloader. + */ + tl::optional getFlashedVersion() const; + /** * @returns True when bootloader was booted using latest bootloader integrated in the library. * False when bootloader is already running on the device and just connected to. @@ -504,6 +510,7 @@ class DeviceBootloader { bool isEmbedded = false; Type bootloaderType; + tl::optional flashedVersion; // closed std::atomic closed{false}; @@ -586,4 +593,4 @@ inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader:: inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Version& v) { return out << v.toString(); -} \ No newline at end of file +} diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index d81d45c78..7b06c9fa1 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -416,6 +416,7 @@ void DeviceBootloader::init(bool embeddedMvcmd, const dai::Path& pathToMvcmd, tl // Retrieve bootloader version version = requestVersion(); + flashedVersion = version; if(version >= Version(0, 0, 12)) { // If version is adequate, do an in memory boot. @@ -586,6 +587,10 @@ DeviceBootloader::Version DeviceBootloader::getVersion() const { return version; } +tl::optional DeviceBootloader::getFlashedVersion() const { + return flashedVersion; +} + DeviceBootloader::Version DeviceBootloader::requestVersion() { // Send request to retrieve bootloader version if(!sendRequest(Request::GetBootloaderVersion{})) { @@ -696,8 +701,12 @@ bool DeviceBootloader::isUserBootloaderSupported() { return false; } + if(!getFlashedVersion()) { + return false; + } + // Check if bootloader version is adequate - if(getVersion().getSemver() < Version(Request::IsUserBootloader::VERSION)) { + if(getFlashedVersion().value().getSemver() < Version(Request::IsUserBootloader::VERSION)) { return false; } @@ -725,7 +734,10 @@ std::tuple DeviceBootloader::flashDepthaiApplicationPackage(s std::vector package, Memory memory) { // Bug in NETWORK bootloader in version 0.0.12 < 0.0.14 - flashing can cause a soft brick - auto bootloaderVersion = getVersion(); + if(!getFlashedVersion()) { + return {false, "Can't flash DepthAI application package without knowing flashed bootloader version."}; + } + auto bootloaderVersion = *getFlashedVersion(); if(bootloaderType == Type::NETWORK && bootloaderVersion < Version(0, 0, 14)) { throw std::invalid_argument("Network bootloader requires version 0.0.14 or higher to flash applications. Current version: " + bootloaderVersion.toString()); @@ -925,8 +937,14 @@ std::tuple DeviceBootloader::flashUserBootloader(std::functio // } // Check if bootloader version is adequate - if(getVersion().getSemver() < Version(Request::IsUserBootloader::VERSION)) { - throw std::runtime_error("Current bootloader version doesn't support User Bootloader"); + if(!getFlashedVersion()) { + throw std::runtime_error( + "Couldn't retrieve version of the flashed bootloader. Make sure you have a factory bootloader flashed and the device is booted to bootloader."); + } + if(getFlashedVersion().value().getSemver() < Version(Request::IsUserBootloader::VERSION)) { + throw std::runtime_error(fmt::format("Current bootloader version doesn't support User Bootloader. Current version: {}, minimum required version: {}", + getFlashedVersion().value().toStringSemver(), + Version(Request::IsUserBootloader::VERSION).toStringSemver())); } // Retrieve bootloader From 00306cdb1329e63151609554239c02a2f56460e0 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Thu, 11 Apr 2024 17:23:38 +0300 Subject: [PATCH 28/80] FW: fixes for IMX378 and IMX582: fix concurrent run, fix scaling with IMX378 THE_1352X1012 resolution, change Camera node best sensor config select to prioritize matching aspect ratio --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 19a80d226..2201f55e7 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "c58546bf837efe2ffc1befb3dffc7d2c0a69fdec") +set(DEPTHAI_DEVICE_SIDE_COMMIT "72cee6bc0a4c984cfe7eede3f482269568f6fe2c") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From a9d80ac259bbd01d3786c476addaae6a722ec0ef Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 12 Apr 2024 20:54:59 +0300 Subject: [PATCH 29/80] Optimized decoding pipeline: 60->27 ms --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index c25456501..a94dde330 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "e0739d876920b06c2d30f5f2d72e500eed8791e2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "d9ff8cb9ba4031e2f54f789e363894c23c5804b1") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From e3ea20d436dd9d19323feec48ef9faee1fa1baf5 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 15 Apr 2024 12:07:51 +0200 Subject: [PATCH 30/80] Add jsoncpp with hunter --- cmake/depthaiDependencies.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index e398023a8..b6e26e558 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -22,6 +22,7 @@ else() hunter_add_package(Backward) endif() hunter_add_package(libnop) + hunter_add_package(jsoncpp) endif() # If library was build as static, find all dependencies From 58a0b81b71ae09f977c256dbabcf6c249feec5d8 Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 15 Apr 2024 13:07:30 +0200 Subject: [PATCH 31/80] Make PCL include optional --- CMakeLists.txt | 2 +- cmake/depthaiDependencies.cmake | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 184a680de..d3819db63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -302,7 +302,7 @@ option(DEPTHAI_BUILD_TESTS "Build tests" OFF) option(DEPTHAI_BUILD_EXAMPLES "Build examples - Requires OpenCV library to be installed" OFF) option(DEPTHAI_BUILD_DOCS "Build documentation - requires doxygen to be installed" OFF) option(DEPTHAI_OPENCV_SUPPORT "Enable optional OpenCV support" ON) -option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" ON) +option(DEPTHAI_PCL_SUPPORT "Enable optional PCL support" OFF) option(DEPTHAI_BINARIES_RESOURCE_COMPILE "Compile Depthai device side binaries into library" ON) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index b6e26e558..e150b05de 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -77,14 +77,16 @@ endif() # OpenCV 4 - (optional, quiet always) find_package(OpenCV 4 QUIET CONFIG) -if(NOT TARGET JsonCpp::JsonCpp) +if(DEPTHAI_PCL_SUPPORT AND NOT TARGET JsonCpp::JsonCpp) find_package(jsoncpp QUIET) endif() set(MODULE_TEMP ${CMAKE_MODULE_PATH}) set(PREFIX_TEMP ${CMAKE_PREFIX_PATH}) set(CMAKE_MODULE_PATH ${_DEPTHAI_MODULE_PATH_ORIGINAL}) set(CMAKE_PREFIX_PATH ${_DEPTHAI_PREFIX_PATH_ORIGINAL}) -find_package(PCL QUIET CONFIG COMPONENTS common visualization) +if(DEPTHAI_PCL_SUPPORT) + find_package(PCL QUIET CONFIG COMPONENTS common visualization) +endif() set(CMAKE_MODULE_PATH ${MODULE_TEMP}) set(CMAKE_PREFIX_PATH ${PREFIX_TEMP}) From f1351b8fb2eeb0dcfd3e4e95756ddd56d57be83a Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Mon, 15 Apr 2024 13:34:33 +0200 Subject: [PATCH 32/80] Fixed booting issue on OAK D SR POE and OAK T. (#1002) --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index b8c9e058d..33809726c 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -3,5 +3,5 @@ set(DEPTHAI_BOOTLOADER_MATURITY "release") # set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_BOOTLOADER_VERSION "0.0.26") +set(DEPTHAI_BOOTLOADER_VERSION "0.0.27") # set(DEPTHAI_BOOTLOADER_VERSION "0.0.24+57c26493754e2f00e57f6594b0b1a317f762d5f2") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 522bbede5..053ce1d4a 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "f9b85918529ab0ebe885e7fa16637df02022b95a") +set(DEPTHAI_DEVICE_SIDE_COMMIT "0e15bb23f924c195fd8a891043265a9b20ec4531") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 5a863070011ae2ceb50a1c7cf11cda9d661f584f Mon Sep 17 00:00:00 2001 From: asahtik Date: Mon, 15 Apr 2024 13:45:11 +0200 Subject: [PATCH 33/80] Fail loudly if PCL enabled, README changes --- README.md | 1 + cmake/depthaiDependencies.cmake | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 4087e2945..cfc74f3d9 100644 --- a/README.md +++ b/README.md @@ -194,6 +194,7 @@ The following environment variables can be set to alter default behavior of the | DEPTHAI_LIBUSB_ANDROID_JAVAVM | JavaVM pointer that is passed to libusb for rootless Android interaction with devices. Interpreted as decimal value of uintptr_t | | DEPTHAI_CRASHDUMP | Directory in which to save the crash dump. | | DEPTHAI_CRASHDUMP_TIMEOUT | Specifies the duration in seconds to wait for device reboot when obtaining a crash dump. Crash dump retrieval disabled if 0. | +| DEPTHAI_PCL_SUPPORT | Enables PCL support. | ## Running tests diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index e150b05de..75ea180d7 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -78,14 +78,14 @@ endif() # OpenCV 4 - (optional, quiet always) find_package(OpenCV 4 QUIET CONFIG) if(DEPTHAI_PCL_SUPPORT AND NOT TARGET JsonCpp::JsonCpp) - find_package(jsoncpp QUIET) + find_package(jsoncpp) endif() set(MODULE_TEMP ${CMAKE_MODULE_PATH}) set(PREFIX_TEMP ${CMAKE_PREFIX_PATH}) set(CMAKE_MODULE_PATH ${_DEPTHAI_MODULE_PATH_ORIGINAL}) set(CMAKE_PREFIX_PATH ${_DEPTHAI_PREFIX_PATH_ORIGINAL}) if(DEPTHAI_PCL_SUPPORT) - find_package(PCL QUIET CONFIG COMPONENTS common visualization) + find_package(PCL CONFIG COMPONENTS common visualization) endif() set(CMAKE_MODULE_PATH ${MODULE_TEMP}) set(CMAKE_PREFIX_PATH ${PREFIX_TEMP}) From 8020e0f736b3a2b26a165308828adcc7c8362443 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 15 Apr 2024 15:34:40 +0200 Subject: [PATCH 34/80] Conditionally add jsoncpp package --- cmake/depthaiDependencies.cmake | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index 75ea180d7..9be00b7f6 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -22,7 +22,9 @@ else() hunter_add_package(Backward) endif() hunter_add_package(libnop) - hunter_add_package(jsoncpp) + if(DEPTHAI_PCL_SUPPORT) + hunter_add_package(jsoncpp) + endif() endif() # If library was build as static, find all dependencies From 9a4892e8c27f0abd82f95e666e6b08ead1c9e621 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 15 Apr 2024 20:05:55 +0300 Subject: [PATCH 35/80] Add multi shave support for ToF decoding --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/common/CameraFeatures.hpp | 3 +-- include/depthai/pipeline/node/ToF.hpp | 27 +++++++++++++++++++-- shared/depthai-shared | 2 +- src/pipeline/node/ToF.cpp | 12 ++++++++- 5 files changed, 39 insertions(+), 7 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index a94dde330..a1b3d2574 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "d9ff8cb9ba4031e2f54f789e363894c23c5804b1") +set(DEPTHAI_DEVICE_SIDE_COMMIT "d48ee8b20f9372712cb945761ef9b522789e00fe") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/common/CameraFeatures.hpp b/include/depthai/common/CameraFeatures.hpp index c36569336..cf362d0ca 100644 --- a/include/depthai/common/CameraFeatures.hpp +++ b/include/depthai/common/CameraFeatures.hpp @@ -49,8 +49,7 @@ inline std::ostream& operator<<(std::ostream& out, const dai::CameraSensorConfig out << "minFps: " << config.minFps << ", "; out << "maxFps: " << config.maxFps << ", "; out << "type: " << config.type << ", "; - out << "fov: " - << "{x:" << config.fov.x << ", "; + out << "fov: " << "{x:" << config.fov.x << ", "; out << "y: " << config.fov.y << ", "; out << "width: " << config.fov.width << ", "; out << "height: " << config.fov.height << "}"; diff --git a/include/depthai/pipeline/node/ToF.hpp b/include/depthai/pipeline/node/ToF.hpp index 75d47dff7..59d1b1c7d 100644 --- a/include/depthai/pipeline/node/ToF.hpp +++ b/include/depthai/pipeline/node/ToF.hpp @@ -41,16 +41,39 @@ class ToF : public NodeCRTP { */ Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::ToFConfig, false}}}; + /** + * Input raw ToF data. + * Default queue is blocking with size 8. + */ Input input{*this, "input", Input::Type::SReceiver, true, 8, {{DatatypeEnum::ImgFrame, true}}}; /** - * Outputs ImgFrame message that carries modified image. + * Outputs ImgFrame message that carries decoded depth image. */ Output depth{*this, "depth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + /** + * Outputs ImgFrame message that carries amplitude image. + */ Output amplitude{*this, "amplitude", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + /** + * Outputs ImgFrame message that carries intensity image. + */ Output intensity{*this, "intensity", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + /** + * Outputs ImgFrame message that carries phase image, useful for debugging. float32 type. + */ Output phase{*this, "phase", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; - Output error{*this, "error", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + + /** + * Specify number of shaves reserved for ToF decoding. + */ + ToF& setNumShaves(int numShaves); + + /** + * Specify number of frames in output pool + * @param numFramesPool Number of frames in output pool + */ + ToF& setNumFramesPool(int numFramesPool); }; } // namespace node diff --git a/shared/depthai-shared b/shared/depthai-shared index 810904080..bd3cbe714 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 81090408061e1fe3c241f889efb5ff4564138022 +Subproject commit bd3cbe714700b247b345a1263f1ea960cd8cd3f4 diff --git a/src/pipeline/node/ToF.cpp b/src/pipeline/node/ToF.cpp index 872ddaabd..db5b04710 100644 --- a/src/pipeline/node/ToF.cpp +++ b/src/pipeline/node/ToF.cpp @@ -7,7 +7,7 @@ ToF::ToF(const std::shared_ptr& par, int64_t nodeId) : ToF(par, no ToF::ToF(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) : NodeCRTP(par, nodeId, std::move(props)), rawConfig(std::make_shared()), initialConfig(rawConfig) { setInputRefs({&inputConfig, &input}); - setOutputRefs({&depth, &litude, &intensity, &phase, &error}); + setOutputRefs({&depth, &litude, &intensity, &phase}); } ToF::Properties& ToF::getProperties() { @@ -15,5 +15,15 @@ ToF::Properties& ToF::getProperties() { return properties; } +ToF& ToF::setNumShaves(int numShaves) { + properties.numShaves = numShaves; + return *this; +} + +ToF& ToF::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; + return *this; +} + } // namespace node } // namespace dai From a09c3ea1391c0ec89e79aa7e21ef067e6a34c2e6 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 16 Apr 2024 18:07:50 +0300 Subject: [PATCH 36/80] Update ToF decoding: handle arbitrary sensor rotation --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 8bb8f513e..aa23816a6 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "4babf2323b442e69acc3941da04008c429fd5e33") +set(DEPTHAI_DEVICE_SIDE_COMMIT "a9d9eab89011c71321cc63667d042367b3933bee") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 5a2db83df063f8b12363f6b31d291cb4df77e293 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 16 Apr 2024 18:09:25 +0300 Subject: [PATCH 37/80] FW: fix default fsync GPIO state for OAK-FFC-4P R7, FSIN_4LANE GPIO42 = input, pull-down --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 053ce1d4a..fcfaf1931 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "0e15bb23f924c195fd8a891043265a9b20ec4531") +set(DEPTHAI_DEVICE_SIDE_COMMIT "729be280c478a321676b457aa8514179f66bb2e3") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From a588da9f9263b0a92498ba61b5fa97648b7a959c Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 18 Apr 2024 17:29:16 +0300 Subject: [PATCH 38/80] Add phaseUnwrappingErrorThreshold config --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- shared/depthai-shared | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index aa23816a6..34e2dd85b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "a9d9eab89011c71321cc63667d042367b3933bee") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c17f369d4abd81df2ec75c35ae28f21d995b9bae") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index bd3cbe714..224ef71db 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit bd3cbe714700b247b345a1263f1ea960cd8cd3f4 +Subproject commit 224ef71db2f14316dbbb86e26003217aecc5356b From 854b36db7d1238600d0736dba2d1eee3cd7850cc Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Mon, 22 Apr 2024 19:30:42 +0300 Subject: [PATCH 39/80] FW: fix S5K33D ToF streaming when enabled on both sockets B and C, shared I2C bus --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index ff900af8f..bce7a93a7 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "b8d6edd8d483d2d821060df04e34cfe55dea3e35") +set(DEPTHAI_DEVICE_SIDE_COMMIT "4daf300cb102ae50ee4b4e14b6e6998cf6c4a382") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From c8d642629f56833f10b85d7a1fc2867d6c0fcab7 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 25 Apr 2024 22:38:52 +0300 Subject: [PATCH 40/80] Fix feature tracker ID type deduction --- examples/FeatureTracker/feature_tracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/FeatureTracker/feature_tracker.cpp b/examples/FeatureTracker/feature_tracker.cpp index e7b688158..887923af4 100644 --- a/examples/FeatureTracker/feature_tracker.cpp +++ b/examples/FeatureTracker/feature_tracker.cpp @@ -16,7 +16,7 @@ class FeatureTrackerDrawer { // for how many frames the feature is tracked static int trackedFeaturesPathLength; - using featureIdType = decltype(dai::Point2f::x); + using featureIdType = decltype(dai::TrackedFeature::id); std::unordered_set trackedIDs; std::unordered_map> trackedFeaturesPath; From 8b326422b876d393bacfc6cb1aeb5183d99050c0 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 26 Apr 2024 12:58:56 +0300 Subject: [PATCH 41/80] FW: fsync shift on custom board --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index bce7a93a7..aed080989 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "4daf300cb102ae50ee4b4e14b6e6998cf6c4a382") +set(DEPTHAI_DEVICE_SIDE_COMMIT "2acb11cd583f02b48b84fa6b4db6a85f3171f82a") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From f0e426db99accbc4dbc9d511dd62a557ffe7dd59 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 2 May 2024 13:57:12 +0300 Subject: [PATCH 42/80] Add new eeprom content --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index aed080989..7ca57b0f5 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "2acb11cd583f02b48b84fa6b4db6a85f3171f82a") +set(DEPTHAI_DEVICE_SIDE_COMMIT "1ba40bb61b4aff6db210c9d0973b0f6e54ff9bdb") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From a1773216e42163a9eb050887dab43095e82d29fb Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 2 May 2024 22:09:32 +0300 Subject: [PATCH 43/80] Update FW: DepthAlign node implementeD --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index d1f8eb6a6..f902623a3 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "cc5f1c3599470968a8dfecc47cca2689f73a0729") +set(DEPTHAI_DEVICE_SIDE_COMMIT "4363aedd777aed9f28cd9b062a96e24c63ad823d") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 35274e7961c0f40be0ebe431b39a5215800cb9b6 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 3 May 2024 10:33:06 +0300 Subject: [PATCH 44/80] Update TOF decoding/eeprom content --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 7ca57b0f5..98d6dc2f1 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "1ba40bb61b4aff6db210c9d0973b0f6e54ff9bdb") +set(DEPTHAI_DEVICE_SIDE_COMMIT "d3d9488154b2031cf6832deb4b2bbe15f08772ad") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 241ec13300eb59a296c66ee8916612ea9164ec3f Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 7 May 2024 18:12:31 +0300 Subject: [PATCH 45/80] Added enableBurstMode=false and enablePhaseShuffleTemporalFilter=true options. --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- shared/depthai-shared | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 98d6dc2f1..e2c4fb11d 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "d3d9488154b2031cf6832deb4b2bbe15f08772ad") +set(DEPTHAI_DEVICE_SIDE_COMMIT "e31506a9b036ed7e863fdc62b4117f362381fac5") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index 224ef71db..f1726b038 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 224ef71db2f14316dbbb86e26003217aecc5356b +Subproject commit f1726b038f186e2b3bf4c72a01c7352cd8bf30e2 From 49789e569b7a67d1dee56baceab97dc4ab9c7ed8 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 7 May 2024 18:40:00 +0300 Subject: [PATCH 46/80] Fall back to sensor EEPROM if onboard intrinsics or distortion coefficients are not found --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index e2c4fb11d..1fa3b1e70 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "e31506a9b036ed7e863fdc62b4117f362381fac5") +set(DEPTHAI_DEVICE_SIDE_COMMIT "2952acac6cd8aa850579875e94343dbb25d83e65") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 1b58b3a19e772df0b836a7b9d450b8ff8e61c1f8 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 8 May 2024 19:19:13 +0300 Subject: [PATCH 47/80] Update temperature coefficients --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 1fa3b1e70..eb2c610bf 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "2952acac6cd8aa850579875e94343dbb25d83e65") +set(DEPTHAI_DEVICE_SIDE_COMMIT "75b99b2d6a429329eb3adebcb1e40f4ef9cc6957") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From fde473e3469efa2dfa6ac415ac06d39e7e0db6fc Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Thu, 9 May 2024 01:52:02 +0300 Subject: [PATCH 48/80] FW, BL: fix a potential bootup/reboot failure, that required power cycle to recover from --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 8 ++++---- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 33809726c..e0f9604c9 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -1,7 +1,7 @@ # Maturity level "snapshot" / "release" -set(DEPTHAI_BOOTLOADER_MATURITY "release") -# set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") +# set(DEPTHAI_BOOTLOADER_MATURITY "release") +set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_BOOTLOADER_VERSION "0.0.27") -# set(DEPTHAI_BOOTLOADER_VERSION "0.0.24+57c26493754e2f00e57f6594b0b1a317f762d5f2") +# set(DEPTHAI_BOOTLOADER_VERSION "0.0.27") +set(DEPTHAI_BOOTLOADER_VERSION "0.0.27+5fb331f993adceeeda72202c233a9e3939ab3dab") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 8375def30..bec6d1e22 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -5,4 +5,4 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") set(DEPTHAI_DEVICE_SIDE_COMMIT "c7127782f2da45aac89d5b5b816d04cc45ae40be") # "version if applicable" -set(DEPTHAI_DEVICE_SIDE_VERSION "") +set(DEPTHAI_DEVICE_SIDE_VERSION "4f7040e7f227ab11240d249a82596d25c5786000") From dc84019a334e1732bb7a81d4395626ac68b0b7f5 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 10 May 2024 16:03:53 +0300 Subject: [PATCH 49/80] Update FW; 10 bit amplitude, uint16; temperature calibration version check --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index eb2c610bf..8e9815497 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "75b99b2d6a429329eb3adebcb1e40f4ef9cc6957") +set(DEPTHAI_DEVICE_SIDE_COMMIT "3aa696f6491871aed67d3076446ddf7e44347898") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 98e885fa13d53f2d0a16be89af25dba7cb1afff8 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 10 May 2024 17:37:02 +0300 Subject: [PATCH 50/80] Update interface --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/DepthAlign.hpp | 23 ++++++++++---------- shared/depthai-shared | 2 +- src/pipeline/node/DepthAlign.cpp | 9 ++------ 4 files changed, 16 insertions(+), 20 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index f902623a3..1e9bd9b39 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "4363aedd777aed9f28cd9b062a96e24c63ad823d") +set(DEPTHAI_DEVICE_SIDE_COMMIT "3b11a9588b837cef296a0a7c956c21c2eca877f2") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/DepthAlign.hpp b/include/depthai/pipeline/node/DepthAlign.hpp index 7a0fe117f..2a8c8380c 100644 --- a/include/depthai/pipeline/node/DepthAlign.hpp +++ b/include/depthai/pipeline/node/DepthAlign.hpp @@ -36,37 +36,38 @@ class DepthAlign : public NodeCRTP { DepthAlignConfig initialConfig; /** - * Input PointCloudConfig message with ability to modify parameters in runtime. + * Input message with ability to modify parameters in runtime. * Default queue is non-blocking with size 4. */ Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::DepthAlignConfig, false}}}; + /** - * Input message with depth data used to retrieve spatial information about detected object. + * Input message. * Default queue is non-blocking with size 4. */ - Input inputDepth{*this, "inputDepth", Input::Type::SReceiver, false, 4, true, {{DatatypeEnum::ImgFrame, false}}}; + Input input{*this, "input", Input::Type::SReceiver, false, 4, true, {{DatatypeEnum::ImgFrame, false}}}; /** - * Outputs ImgFrame message that carries aligned depth image. + * Input align to message. + * Default queue is non-blocking with size 1. */ - Output outputAlignedDepth{*this, "outputAlignedDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + Input inputAlignTo{*this, "inputAlignTo", Input::Type::SReceiver, false, 1, true, {{DatatypeEnum::ImgFrame, false}}}; /** - * Passthrough message on which the calculation was performed. - * Suitable for when input queue is set to non-blocking behavior. + * Outputs ImgFrame message that is aligned to inputAlignTo. */ - Output passthroughDepth{*this, "passthroughDepth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + Output outputAligned{*this, "outputAligned", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; /** - * Specify to which camera socket the depth map is aligned to + * Passthrough message on which the calculation was performed. + * Suitable for when input queue is set to non-blocking behavior. */ - DepthAlign& setAlignTo(CameraBoardSocket alignTo); + Output passthroughInput{*this, "passthroughInput", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; /** * Specify the output size of the aligned depth map */ DepthAlign& setOutputSize(int alignWidth, int alignHeight); - }; } // namespace node diff --git a/shared/depthai-shared b/shared/depthai-shared index 573c4b085..7effb90e5 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 573c4b0857f04199c8809cfac9b5d78f709747fe +Subproject commit 7effb90e5a04722d8657117609f1d091cf4b1b5c diff --git a/src/pipeline/node/DepthAlign.cpp b/src/pipeline/node/DepthAlign.cpp index 7c9500647..9f27afee1 100644 --- a/src/pipeline/node/DepthAlign.cpp +++ b/src/pipeline/node/DepthAlign.cpp @@ -10,8 +10,8 @@ DepthAlign::DepthAlign(const std::shared_ptr& par, int64_t nodeId, : NodeCRTP(par, nodeId, std::move(props)), rawConfig(std::make_shared()), initialConfig(rawConfig) { - setInputRefs({&inputConfig, &inputDepth}); - setOutputRefs({&outputAlignedDepth, &passthroughDepth}); + setInputRefs({&inputConfig, &input, &inputAlignTo}); + setOutputRefs({&outputAligned, &passthroughInput}); } DepthAlign::Properties& DepthAlign::getProperties() { @@ -19,11 +19,6 @@ DepthAlign::Properties& DepthAlign::getProperties() { return properties; } -DepthAlign& DepthAlign::setAlignTo(CameraBoardSocket alignTo) { - properties.alignTo = alignTo; - return *this; -} - DepthAlign& DepthAlign::setOutputSize(int alignWidth, int alignHeight) { properties.alignWidth = alignWidth; properties.alignHeight = alignHeight; From 115f0d58ccb7315a9b3d3095f8eab977a345f9fd Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 9 May 2024 19:08:18 +0300 Subject: [PATCH 51/80] Fix stride size --- src/pipeline/datatype/ImgFrame.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/pipeline/datatype/ImgFrame.cpp b/src/pipeline/datatype/ImgFrame.cpp index 55c443bc9..a777c9925 100644 --- a/src/pipeline/datatype/ImgFrame.cpp +++ b/src/pipeline/datatype/ImgFrame.cpp @@ -99,6 +99,7 @@ ImgFrame& ImgFrame::setSequenceNum(int64_t sequenceNum) { ImgFrame& ImgFrame::setWidth(unsigned int width) { img.fb.width = width; img.fb.stride = width; + if(img.fb.bytesPP) img.fb.stride *= img.fb.bytesPP; return *this; } ImgFrame& ImgFrame::setHeight(unsigned int height) { @@ -117,6 +118,7 @@ ImgFrame& ImgFrame::setSize(std::tuple size) { ImgFrame& ImgFrame::setType(RawImgFrame::Type type) { img.fb.type = type; img.fb.bytesPP = RawImgFrame::typeToBpp(img.fb.type); + if(img.fb.width) img.fb.stride = img.fb.width * img.fb.bytesPP; return *this; } From a295f449e25f2d478bfb1ed8e402ce4a065cc7ec Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 10 May 2024 18:05:18 +0300 Subject: [PATCH 52/80] (fix mistake in previous commit, FW ver was put in the wrong place) --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index bec6d1e22..472c3d774 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "c7127782f2da45aac89d5b5b816d04cc45ae40be") +set(DEPTHAI_DEVICE_SIDE_COMMIT "4f7040e7f227ab11240d249a82596d25c5786000") # "version if applicable" -set(DEPTHAI_DEVICE_SIDE_VERSION "4f7040e7f227ab11240d249a82596d25c5786000") +set(DEPTHAI_DEVICE_SIDE_VERSION "") From cba9e06c27dd252e78a5eb664473f643fc15d636 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 10 May 2024 18:08:28 +0300 Subject: [PATCH 53/80] Revert "DataQueue: catch packet parsing errors and return a null packet instead, with an `error` log". Note: reason for revert was underlying cause likely fixed, we'll probably revisit in future and keep exception throwing but not closing the queues on these potential bad packets This reverts commit 1a48df97e1a1bae9ef7eee798d3ebbaf99d01085. --- src/device/DataQueue.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/device/DataQueue.cpp b/src/device/DataQueue.cpp index bf22c9c3e..e16f2c8a1 100644 --- a/src/device/DataQueue.cpp +++ b/src/device/DataQueue.cpp @@ -40,13 +40,8 @@ DataOutputQueue::DataOutputQueue(const std::shared_ptr conn, co // Blocking -- parse packet and gather timing information auto packet = stream.readMove(); DatatypeEnum type; - std::shared_ptr data{}; const auto t1Parse = std::chrono::steady_clock::now(); - try { - data = StreamMessageParser::parseMessageToADatatype(&packet, type); - } catch(const std::exception& ex) { - logger::error("Caught exception on stream '{}': {}", name, ex.what()); - } + const auto data = StreamMessageParser::parseMessageToADatatype(&packet, type); if(type == DatatypeEnum::MessageGroup) { auto msgGrp = std::static_pointer_cast(data); unsigned int size = msgGrp->getNumMessages(); From 07e826138b78124f46c40b56cd5771f463a8cd54 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 13 May 2024 18:11:31 +0300 Subject: [PATCH 54/80] Refactor DepthAlign -> ImageAlign --- CMakeLists.txt | 4 ++-- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/CMakeLists.txt | 2 +- ...thAlignConfig.hpp => ImageAlignConfig.hpp} | 18 +++++++-------- .../node/{DepthAlign.hpp => ImageAlign.hpp} | 22 +++++++++---------- include/depthai/pipeline/nodes.hpp | 2 +- shared/depthai-shared | 2 +- src/pipeline/datatype/DepthAlignConfig.cpp | 21 ------------------ src/pipeline/datatype/ImageAlignConfig.cpp | 21 ++++++++++++++++++ src/pipeline/datatype/StreamMessageParser.cpp | 12 +++++----- .../node/{DepthAlign.cpp => ImageAlign.cpp} | 14 ++++++------ 11 files changed, 60 insertions(+), 60 deletions(-) rename include/depthai/pipeline/datatype/{DepthAlignConfig.hpp => ImageAlignConfig.hpp} (54%) rename include/depthai/pipeline/node/{DepthAlign.hpp => ImageAlign.hpp} (71%) delete mode 100644 src/pipeline/datatype/DepthAlignConfig.cpp create mode 100644 src/pipeline/datatype/ImageAlignConfig.cpp rename src/pipeline/node/{DepthAlign.cpp => ImageAlign.cpp} (50%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7ce445302..ca509387b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -219,7 +219,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/DetectionNetwork.cpp src/pipeline/node/Script.cpp src/pipeline/node/SpatialDetectionNetwork.cpp - src/pipeline/node/DepthAlign.cpp + src/pipeline/node/ImageAlign.cpp src/pipeline/node/SystemLogger.cpp src/pipeline/node/SpatialLocationCalculator.cpp src/pipeline/node/AprilTag.cpp @@ -251,7 +251,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/datatype/EdgeDetectorConfig.cpp src/pipeline/datatype/TrackedFeatures.cpp src/pipeline/datatype/FeatureTrackerConfig.cpp - src/pipeline/datatype/DepthAlignConfig.cpp + src/pipeline/datatype/ImageAlignConfig.cpp src/pipeline/datatype/ToFConfig.cpp src/pipeline/datatype/PointCloudConfig.cpp src/pipeline/datatype/PointCloudData.cpp diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 1e9bd9b39..9c9f769f0 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "3b11a9588b837cef296a0a7c956c21c2eca877f2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "7e571685097f22808064919cafcee996e392a170") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 4d4243447..999befb73 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -244,7 +244,7 @@ dai_add_example(edge_detector EdgeDetector/edge_detector.cpp ON OFF) dai_add_example(feature_detector FeatureTracker/feature_detector.cpp ON OFF) dai_add_example(feature_tracker FeatureTracker/feature_tracker.cpp ON OFF) -# dai_add_example(depth_align DepthAlign/rgb_depth_align.cpp ON) +# dai_add_example(image_align ImageAlign/rgb_image_align.cpp ON) # host_side dai_add_example(device_queue_event host_side/device_queue_event.cpp ON OFF) diff --git a/include/depthai/pipeline/datatype/DepthAlignConfig.hpp b/include/depthai/pipeline/datatype/ImageAlignConfig.hpp similarity index 54% rename from include/depthai/pipeline/datatype/DepthAlignConfig.hpp rename to include/depthai/pipeline/datatype/ImageAlignConfig.hpp index 2a19345f5..f78f1f522 100644 --- a/include/depthai/pipeline/datatype/DepthAlignConfig.hpp +++ b/include/depthai/pipeline/datatype/ImageAlignConfig.hpp @@ -3,34 +3,34 @@ #include #include -#include "depthai-shared/datatype/RawDepthAlignConfig.hpp" +#include "depthai-shared/datatype/RawImageAlignConfig.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" namespace dai { /** - * DepthAlignConfig message + * ImageAlignConfig message */ -class DepthAlignConfig : public Buffer { +class ImageAlignConfig : public Buffer { std::shared_ptr serialize() const override; - RawDepthAlignConfig& cfg; + RawImageAlignConfig& cfg; public: - DepthAlignConfig(); - explicit DepthAlignConfig(std::shared_ptr ptr); - virtual ~DepthAlignConfig() = default; + ImageAlignConfig(); + explicit ImageAlignConfig(std::shared_ptr ptr); + virtual ~ImageAlignConfig() = default; /** * Set explicit configuration. * @param config Explicit configuration */ - DepthAlignConfig& set(dai::RawDepthAlignConfig config); + ImageAlignConfig& set(dai::RawImageAlignConfig config); /** * Retrieve configuration data for SpatialLocationCalculator. * @returns config for SpatialLocationCalculator */ - dai::RawDepthAlignConfig get() const; + dai::RawImageAlignConfig get() const; }; } // namespace dai \ No newline at end of file diff --git a/include/depthai/pipeline/node/DepthAlign.hpp b/include/depthai/pipeline/node/ImageAlign.hpp similarity index 71% rename from include/depthai/pipeline/node/DepthAlign.hpp rename to include/depthai/pipeline/node/ImageAlign.hpp index 2a8c8380c..f56c2447f 100644 --- a/include/depthai/pipeline/node/DepthAlign.hpp +++ b/include/depthai/pipeline/node/ImageAlign.hpp @@ -6,40 +6,40 @@ #include // shared -#include +#include -#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" namespace dai { namespace node { /** - * @brief DepthAlign node. Calculates spatial location data on a set of ROIs on depth map. + * @brief ImageAlign node. Calculates spatial location data on a set of ROIs on depth map. */ -class DepthAlign : public NodeCRTP { +class ImageAlign : public NodeCRTP { public: - constexpr static const char* NAME = "DepthAlign"; + constexpr static const char* NAME = "ImageAlign"; protected: Properties& getProperties(); private: - std::shared_ptr rawConfig; + std::shared_ptr rawConfig; public: - DepthAlign(const std::shared_ptr& par, int64_t nodeId); - DepthAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props); + ImageAlign(const std::shared_ptr& par, int64_t nodeId); + ImageAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props); /** * Initial config to use when calculating spatial location data. */ - DepthAlignConfig initialConfig; + ImageAlignConfig initialConfig; /** * Input message with ability to modify parameters in runtime. * Default queue is non-blocking with size 4. */ - Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::DepthAlignConfig, false}}}; + Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::ImageAlignConfig, false}}}; /** * Input message. @@ -67,7 +67,7 @@ class DepthAlign : public NodeCRTP { /** * Specify the output size of the aligned depth map */ - DepthAlign& setOutputSize(int alignWidth, int alignHeight); + ImageAlign& setOutputSize(int alignWidth, int alignHeight); }; } // namespace node diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 546a28dee..1107c52a3 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -4,12 +4,12 @@ #include "node/AprilTag.hpp" #include "node/Camera.hpp" #include "node/ColorCamera.hpp" -#include "node/DepthAlign.hpp" #include "node/DetectionNetwork.hpp" #include "node/DetectionParser.hpp" #include "node/EdgeDetector.hpp" #include "node/FeatureTracker.hpp" #include "node/IMU.hpp" +#include "node/ImageAlign.hpp" #include "node/ImageManip.hpp" #include "node/MessageDemux.hpp" #include "node/MonoCamera.hpp" diff --git a/shared/depthai-shared b/shared/depthai-shared index 7effb90e5..e1eb9bac1 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 7effb90e5a04722d8657117609f1d091cf4b1b5c +Subproject commit e1eb9bac1f7c106da9fa566760e7ee99abd0d58f diff --git a/src/pipeline/datatype/DepthAlignConfig.cpp b/src/pipeline/datatype/DepthAlignConfig.cpp deleted file mode 100644 index 5ffd291f8..000000000 --- a/src/pipeline/datatype/DepthAlignConfig.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" - -namespace dai { - -std::shared_ptr DepthAlignConfig::serialize() const { - return raw; -} - -DepthAlignConfig::DepthAlignConfig() : Buffer(std::make_shared()), cfg(*dynamic_cast(raw.get())) {} -DepthAlignConfig::DepthAlignConfig(std::shared_ptr ptr) : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} - -dai::RawDepthAlignConfig DepthAlignConfig::get() const { - return cfg; -} - -DepthAlignConfig& DepthAlignConfig::set(dai::RawDepthAlignConfig config) { - cfg = config; - return *this; -} - -} // namespace dai \ No newline at end of file diff --git a/src/pipeline/datatype/ImageAlignConfig.cpp b/src/pipeline/datatype/ImageAlignConfig.cpp new file mode 100644 index 000000000..8b6376eea --- /dev/null +++ b/src/pipeline/datatype/ImageAlignConfig.cpp @@ -0,0 +1,21 @@ +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" + +namespace dai { + +std::shared_ptr ImageAlignConfig::serialize() const { + return raw; +} + +ImageAlignConfig::ImageAlignConfig() : Buffer(std::make_shared()), cfg(*dynamic_cast(raw.get())) {} +ImageAlignConfig::ImageAlignConfig(std::shared_ptr ptr) : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} + +dai::RawImageAlignConfig ImageAlignConfig::get() const { + return cfg; +} + +ImageAlignConfig& ImageAlignConfig::set(dai::RawImageAlignConfig config) { + cfg = config; + return *this; +} + +} // namespace dai \ No newline at end of file diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index febe0ba34..0f06879d6 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -14,11 +14,11 @@ #include "depthai/pipeline/datatype/AprilTags.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" #include "depthai/pipeline/datatype/CameraControl.hpp" -#include "depthai/pipeline/datatype/DepthAlignConfig.hpp" #include "depthai/pipeline/datatype/EdgeDetectorConfig.hpp" #include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" #include "depthai/pipeline/datatype/ImageManipConfig.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" @@ -41,11 +41,11 @@ #include "depthai-shared/datatype/RawAprilTags.hpp" #include "depthai-shared/datatype/RawBuffer.hpp" #include "depthai-shared/datatype/RawCameraControl.hpp" -#include "depthai-shared/datatype/RawDepthAlignConfig.hpp" #include "depthai-shared/datatype/RawEdgeDetectorConfig.hpp" #include "depthai-shared/datatype/RawEncodedFrame.hpp" #include "depthai-shared/datatype/RawFeatureTrackerConfig.hpp" #include "depthai-shared/datatype/RawIMUData.hpp" +#include "depthai-shared/datatype/RawImageAlignConfig.hpp" #include "depthai-shared/datatype/RawImageManipConfig.hpp" #include "depthai-shared/datatype/RawImgDetections.hpp" #include "depthai-shared/datatype/RawImgFrame.hpp" @@ -212,8 +212,8 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* case DatatypeEnum::MessageGroup: return parseDatatype(metadataStart, serializedObjectSize, data); break; - case DatatypeEnum::DepthAlignConfig: - return parseDatatype(metadataStart, serializedObjectSize, data); + case DatatypeEnum::ImageAlignConfig: + return parseDatatype(metadataStart, serializedObjectSize, data); break; } @@ -319,8 +319,8 @@ std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPa case DatatypeEnum::MessageGroup: return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); break; - case DatatypeEnum::DepthAlignConfig: - return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); + case DatatypeEnum::ImageAlignConfig: + return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); break; } diff --git a/src/pipeline/node/DepthAlign.cpp b/src/pipeline/node/ImageAlign.cpp similarity index 50% rename from src/pipeline/node/DepthAlign.cpp rename to src/pipeline/node/ImageAlign.cpp index 9f27afee1..0ccbeaed5 100644 --- a/src/pipeline/node/DepthAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -1,25 +1,25 @@ -#include "depthai/pipeline/node/DepthAlign.hpp" +#include "depthai/pipeline/node/ImageAlign.hpp" #include "spdlog/fmt/fmt.h" namespace dai { namespace node { -DepthAlign::DepthAlign(const std::shared_ptr& par, int64_t nodeId) : DepthAlign(par, nodeId, std::make_unique()) {} -DepthAlign::DepthAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) - : NodeCRTP(par, nodeId, std::move(props)), - rawConfig(std::make_shared()), +ImageAlign::ImageAlign(const std::shared_ptr& par, int64_t nodeId) : ImageAlign(par, nodeId, std::make_unique()) {} +ImageAlign::ImageAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) + : NodeCRTP(par, nodeId, std::move(props)), + rawConfig(std::make_shared()), initialConfig(rawConfig) { setInputRefs({&inputConfig, &input, &inputAlignTo}); setOutputRefs({&outputAligned, &passthroughInput}); } -DepthAlign::Properties& DepthAlign::getProperties() { +ImageAlign::Properties& ImageAlign::getProperties() { properties.initialConfig = *rawConfig; return properties; } -DepthAlign& DepthAlign::setOutputSize(int alignWidth, int alignHeight) { +ImageAlign& ImageAlign::setOutputSize(int alignWidth, int alignHeight) { properties.alignWidth = alignWidth; properties.alignHeight = alignHeight; return *this; From 78e06425e447fc81f4656f0c5a332b98b740a711 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 14 May 2024 15:25:16 +0300 Subject: [PATCH 55/80] Remove readCcmEepromRaw, writeCcmEepromRaw --- include/depthai/device/DeviceBase.hpp | 23 ----------------------- src/device/DeviceBase.cpp | 21 --------------------- 2 files changed, 44 deletions(-) diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index 492e8263b..380812be2 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -837,29 +837,6 @@ class DeviceBase { */ std::vector readFactoryCalibrationRaw(); - /** - * Fetches the raw EEPROM data from the specified CCM (compact camera module). - * Note: only certain CCMs (e.g. ToF) do have an EEPROM chip on-module - * - * @param socket CameraBoardSocket where the CCM is placed - * @param size Size in bytes to read - * @param offset Absolute offset in EEPROM memory to read from - * @throws std::runtime_exception if any error occurred - * @returns Binary dump of EEPROM data - */ - std::vector readCcmEepromRaw(CameraBoardSocket socket, int size, int offset = 0); - - /** - * Writes the raw EEPROM data from the specified CCM (compact camera module). - * Note: only certain CCMs (e.g. ToF) do have an EEPROM chip on-module - * - * @param socket CameraBoardSocket where the CCM is placed - * @param data Data buffer to write - * @param offset Absolute offset in EEPROM memory to read from - * @throws std::runtime_exception if any error occurred - */ - void writeCcmEepromRaw(CameraBoardSocket socket, std::vector data, int offset = 0); - /** * Retrieves USB connection speed * diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 6fd31f421..6f6dfb487 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1458,27 +1458,6 @@ std::vector DeviceBase::readFactoryCalibrationRaw() { return eepromDataRaw; } -std::vector DeviceBase::readCcmEepromRaw(CameraBoardSocket socket, int size, int offset) { - bool success; - std::string errorMsg; - std::vector eepromDataRaw; - std::tie(success, errorMsg, eepromDataRaw) = - pimpl->rpcClient->call("readCcmEepromRaw", socket, size, offset).as>>(); - if(!success) { - throw EepromError(errorMsg); - } - return eepromDataRaw; -} - -void DeviceBase::writeCcmEepromRaw(CameraBoardSocket socket, std::vector data, int offset) { - bool success; - std::string errorMsg; - std::tie(success, errorMsg) = pimpl->rpcClient->call("writeCcmEepromRaw", socket, data, offset).as>(); - if(!success) { - throw EepromError(errorMsg); - } -} - void DeviceBase::flashEepromClear() { bool factoryPermissions = false; bool protectedPermissions = false; From 22141d7d6573916a021ada5f82fe6e0a9e39bee6 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 14 May 2024 19:54:36 +0300 Subject: [PATCH 56/80] BL: update to 0.0.28 release version (bootup/restart fixes) --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index e0f9604c9..3f1a15d1a 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -1,7 +1,7 @@ # Maturity level "snapshot" / "release" -# set(DEPTHAI_BOOTLOADER_MATURITY "release") -set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") +set(DEPTHAI_BOOTLOADER_MATURITY "release") +# set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" -# set(DEPTHAI_BOOTLOADER_VERSION "0.0.27") -set(DEPTHAI_BOOTLOADER_VERSION "0.0.27+5fb331f993adceeeda72202c233a9e3939ab3dab") +set(DEPTHAI_BOOTLOADER_VERSION "0.0.28") +# set(DEPTHAI_BOOTLOADER_VERSION "0.0.27+5fb331f993adceeeda72202c233a9e3939ab3dab") From 0b70c024e170faf4055b34cbeb22641ce101a5e5 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 14 May 2024 21:00:50 +0300 Subject: [PATCH 57/80] StreamMessageParser: check end-of-packet bytes, warn on mismatch, the warning usually can be ignored, but may indicate some issues --- src/pipeline/datatype/StreamMessageParser.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index c99b9022d..14e6892bc 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -7,6 +7,7 @@ // libraries #include #include +#include "utility/Logging.hpp" // project #include "depthai/pipeline/datatype/ADatatype.hpp" @@ -88,10 +89,21 @@ static std::tuple parseHeader(streamPacketDesc_t* if(packet->length < 24) { throw std::runtime_error(fmt::format("Bad packet, couldn't parse (not enough data), total size {}", packet->length)); } - const std::uint32_t packetLength = packet->length - 16; + const std::uint32_t markerLength = 16; + const std::uint32_t packetLength = packet->length - markerLength; const int serializedObjectSize = readIntLE(packet->data + packetLength - 4); const auto objectType = static_cast(readIntLE(packet->data + packetLength - 8)); + static const uint8_t expectedMarker[] = {0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0}; + uint8_t* marker = packet->data + packetLength; + if(memcmp(marker, expectedMarker, markerLength) != 0) { + std::string hex; + for(std::uint32_t i = 0; i < markerLength; i++) { + hex += fmt::format("{:02X}", marker[i]); + } + logger::warn("StreamMessageParser end-of-packet marker mismatch, got: " + hex); + } + const auto info = fmt::format(", total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize); if(serializedObjectSize < 0) { From 3b6b68869cd1893184f47832c5bb6c11fceb85e7 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 15 May 2024 16:38:47 +0300 Subject: [PATCH 58/80] FW: update after merge. Fix style - `make clangformat` --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/common/CameraFeatures.hpp | 3 ++- src/pipeline/datatype/StreamMessageParser.cpp | 1 + 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 36ca55d84..d26f01c6a 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "b653401c26c5d8f44508f4fa9b771f6b27f2627e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "1b395d76effe968b32b44a4019df4cf1892597be") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/common/CameraFeatures.hpp b/include/depthai/common/CameraFeatures.hpp index cf362d0ca..c36569336 100644 --- a/include/depthai/common/CameraFeatures.hpp +++ b/include/depthai/common/CameraFeatures.hpp @@ -49,7 +49,8 @@ inline std::ostream& operator<<(std::ostream& out, const dai::CameraSensorConfig out << "minFps: " << config.minFps << ", "; out << "maxFps: " << config.maxFps << ", "; out << "type: " << config.type << ", "; - out << "fov: " << "{x:" << config.fov.x << ", "; + out << "fov: " + << "{x:" << config.fov.x << ", "; out << "y: " << config.fov.y << ", "; out << "width: " << config.fov.width << ", "; out << "height: " << config.fov.height << "}"; diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index 14e6892bc..c7b0c9c1f 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -7,6 +7,7 @@ // libraries #include #include + #include "utility/Logging.hpp" // project From 3672f7806ece4206c2a295186c72fc5ffd958957 Mon Sep 17 00:00:00 2001 From: moratom <47612463+moratom@users.noreply.github.com> Date: Wed, 15 May 2024 19:25:57 +0200 Subject: [PATCH 59/80] Fix CI Remove CMake 3.10 test on macos-latest --- .github/workflows/main.workflow.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/main.workflow.yml b/.github/workflows/main.workflow.yml index eec36eba8..4560feb6b 100644 --- a/.github/workflows/main.workflow.yml +++ b/.github/workflows/main.workflow.yml @@ -67,6 +67,8 @@ jobs: exclude: - os: windows-latest cmake: '3.10.x' + - os: macos-latest # Skip the old cmake on latest macos - doesn't handle ARM64 aarch correctly + cmake: '3.10.x' steps: - name: Cache .hunter folder @@ -333,4 +335,4 @@ jobs: upload_url: ${{ steps.createRelease.outputs.upload_url }} asset_path: ${{ github.workspace }}/depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv.zip asset_name: depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv.zip - asset_content_type: application/octet-stream \ No newline at end of file + asset_content_type: application/octet-stream From 2ac4531016bd18bb4645d56c59b8f4e78c71a5a2 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Thu, 16 May 2024 04:48:25 +0300 Subject: [PATCH 60/80] DeviceBootloader: warning for potentially unstable flashed bootloader < 0.0.28 --- src/device/DeviceBootloader.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 7b06c9fa1..5a6ff0850 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -417,6 +417,19 @@ void DeviceBootloader::init(bool embeddedMvcmd, const dai::Path& pathToMvcmd, tl // Retrieve bootloader version version = requestVersion(); flashedVersion = version; + + auto recommendedMinVersion = DeviceBootloader::Version(0, 0, 28); + if(version < recommendedMinVersion) { + logger::warn( + "[{}] [{}] Flashed bootloader version {}, less than {} is susceptible to bootup/restart failure. Upgrading is advised, flashing " + "main/factory (not user) bootloader. Available: {}", + deviceInfo.mxid, + deviceInfo.name, + version.toString(), + recommendedMinVersion.toString(), + getEmbeddedBootloaderVersion().toString()); + } + if(version >= Version(0, 0, 12)) { // If version is adequate, do an in memory boot. From 5b4b8b8a6e68e764a9c0b765b238e9cfa5b52d94 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Thu, 16 May 2024 04:50:14 +0300 Subject: [PATCH 61/80] FW: fix for MessageGroup parsing after the added end-of-packet marker --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index d26f01c6a..d7f150198 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "1b395d76effe968b32b44a4019df4cf1892597be") +set(DEPTHAI_DEVICE_SIDE_COMMIT "90c8ad77377abed19ee1099a80ec3c00f8d7e9e4") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From f8d1976b068c770d4855d67193b8f3a217bc18e8 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 17 May 2024 00:57:25 +0300 Subject: [PATCH 62/80] StreamMessageParser: apply end-of-packet marker in both directions --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- src/pipeline/datatype/StreamMessageParser.cpp | 14 +++++---- tests/src/stream_message_parser_test.cpp | 29 +++++++++++++++---- 3 files changed, 33 insertions(+), 12 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index d7f150198..adb7b584f 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "90c8ad77377abed19ee1099a80ec3c00f8d7e9e4") +set(DEPTHAI_DEVICE_SIDE_COMMIT "50cac0c00e7dec39f7995fa2d39e612c9a777127") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index c7b0c9c1f..811cc62a0 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -69,6 +69,8 @@ namespace dai { +static constexpr std::array endOfPacketMarker = {0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0}; + // Reads int from little endian format inline int readIntLE(uint8_t* data) { return data[0] + data[1] * 256 + data[2] * 256 * 256 + data[3] * 256 * 256 * 256; @@ -90,16 +92,14 @@ static std::tuple parseHeader(streamPacketDesc_t* if(packet->length < 24) { throw std::runtime_error(fmt::format("Bad packet, couldn't parse (not enough data), total size {}", packet->length)); } - const std::uint32_t markerLength = 16; - const std::uint32_t packetLength = packet->length - markerLength; + const std::uint32_t packetLength = packet->length - endOfPacketMarker.size(); const int serializedObjectSize = readIntLE(packet->data + packetLength - 4); const auto objectType = static_cast(readIntLE(packet->data + packetLength - 8)); - static const uint8_t expectedMarker[] = {0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0}; uint8_t* marker = packet->data + packetLength; - if(memcmp(marker, expectedMarker, markerLength) != 0) { + if(memcmp(marker, endOfPacketMarker.data(), endOfPacketMarker.size()) != 0) { std::string hex; - for(std::uint32_t i = 0; i < markerLength; i++) { + for(std::uint32_t i = 0; i < endOfPacketMarker.size(); i++) { hex += fmt::format("{:02X}", marker[i]); } logger::warn("StreamMessageParser end-of-packet marker mismatch, got: " + hex); @@ -356,6 +356,7 @@ std::vector StreamMessageParser::serializeMessage(const RawBuffer& // 2. serialize and append metadata // 3. append datatype enum (4B LE) // 4. append size (4B LE) of serialized metadata + // 5. append 16-byte marker/canary DatatypeEnum datatype; std::vector metadata; @@ -369,11 +370,12 @@ std::vector StreamMessageParser::serializeMessage(const RawBuffer& for(int i = 0; i < 4; i++) leMetadataSize[i] = (metadataSize >> i * 8) & 0xFF; std::vector ser; - ser.reserve(data.data.size() + metadata.size() + leDatatype.size() + leMetadataSize.size()); + ser.reserve(data.data.size() + metadata.size() + leDatatype.size() + leMetadataSize.size() + endOfPacketMarker.size()); ser.insert(ser.end(), data.data.begin(), data.data.end()); ser.insert(ser.end(), metadata.begin(), metadata.end()); ser.insert(ser.end(), leDatatype.begin(), leDatatype.end()); ser.insert(ser.end(), leMetadataSize.begin(), leMetadataSize.end()); + ser.insert(ser.end(), endOfPacketMarker.begin(), endOfPacketMarker.end()); return ser; } diff --git a/tests/src/stream_message_parser_test.cpp b/tests/src/stream_message_parser_test.cpp index 07ef8839b..a0f64e2e6 100644 --- a/tests/src/stream_message_parser_test.cpp +++ b/tests/src/stream_message_parser_test.cpp @@ -6,6 +6,8 @@ // TODO(themarpe) - fuzz me instead +constexpr auto MARKER_SIZE = 16; + TEST_CASE("Correct message") { dai::ImgFrame frm; auto ser = dai::StreamMessageParser::serializeMessage(frm); @@ -20,12 +22,29 @@ TEST_CASE("Correct message") { REQUIRE(ser == ser2); } +TEST_CASE("Correct message, but padding corrupted, a warning should be printed") { + dai::ImgFrame frm; + auto ser = dai::StreamMessageParser::serializeMessage(frm); + ser[ser.size() - 1] = 0x55; + + streamPacketDesc_t packet; + packet.data = ser.data(); + packet.length = ser.size(); + + auto des = dai::StreamMessageParser::parseMessageToADatatype(&packet); + auto ser2 = dai::StreamMessageParser::serializeMessage(des); + // Just for the binary compare to match, cause the same corruption in re-serialized msg + ser2[ser.size() - 1] = 0x55; + + REQUIRE(ser == ser2); +} + TEST_CASE("Incorrect message bad size") { dai::ImgFrame frm; auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 100; + ser[ser.size() - 1 - MARKER_SIZE] = 100; streamPacketDesc_t packet; packet.data = ser.data(); @@ -39,7 +58,7 @@ TEST_CASE("Incorrect message negative size") { auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 200; + ser[ser.size() - 1 - MARKER_SIZE] = 200; streamPacketDesc_t packet; packet.data = ser.data(); @@ -87,7 +106,7 @@ TEST_CASE("Raw - Incorrect message bad size") { auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 100; + ser[ser.size() - 1 - MARKER_SIZE] = 100; streamPacketDesc_t packet; packet.data = ser.data(); @@ -101,7 +120,7 @@ TEST_CASE("Raw - Incorrect message negative size") { auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 200; + ser[ser.size() - 1 - MARKER_SIZE] = 200; streamPacketDesc_t packet; packet.data = ser.data(); @@ -128,4 +147,4 @@ TEST_CASE("Raw - Incorrect message too small size 2") { packet.length = ser.size(); REQUIRE_THROWS(dai::StreamMessageParser::parseMessage(&packet)); -} \ No newline at end of file +} From 0126c7a8f71846d92b5fa829d15b2d804750303c Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 17 May 2024 18:23:41 +0300 Subject: [PATCH 63/80] FW: stream parser marker merged, other changes: warp FP16 --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index adb7b584f..1183043de 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "50cac0c00e7dec39f7995fa2d39e612c9a777127") +set(DEPTHAI_DEVICE_SIDE_COMMIT "29e2341ec6fe1a64ea1d6996493aed2498aefa53") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 6f85d04363b004360e60c96c87d9282e87ef474e Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 17 May 2024 20:32:09 +0300 Subject: [PATCH 64/80] Update FW --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 491c665a4..ea00e1f4f 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "506abdf3f5e901f361801bb8cc155a7b3c214a1e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c4bd043693080aee33b56456de463c2deb7de1eb") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From acc1ad6f8c5cd4de2578ca4068ca9083f4b3c4f3 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Mon, 20 May 2024 04:17:48 +0300 Subject: [PATCH 65/80] FW: ToF double raw stream FPS internally to get depth at configured FPS, for the default case (non-burst mode). Other ToF changes: - fix timestamps and sequence numbers - reduce device CPU load when ToF streams, depending on FPS - set the proper ToF exposure time in ImgFrame metadata, 796us max - warnings for FPS capping, 80 max recommended, 88 max possible (with reduced depth quality far away) --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 1183043de..60f1288c3 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "29e2341ec6fe1a64ea1d6996493aed2498aefa53") +set(DEPTHAI_DEVICE_SIDE_COMMIT "ea5692bca917c3b09a4e4381663492b488297c33") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 7e50521acf56400fad3444b3db1774f918a674d9 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 20 May 2024 20:07:04 +0300 Subject: [PATCH 66/80] Update FW; handle edge cases --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/common/CameraFeatures.hpp | 3 +-- shared/depthai-shared | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 60f1288c3..d517f9ed1 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "ea5692bca917c3b09a4e4381663492b488297c33") +set(DEPTHAI_DEVICE_SIDE_COMMIT "0aec7e2c3c53a18677b339a89fa27233fc48b45d") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/common/CameraFeatures.hpp b/include/depthai/common/CameraFeatures.hpp index c36569336..cf362d0ca 100644 --- a/include/depthai/common/CameraFeatures.hpp +++ b/include/depthai/common/CameraFeatures.hpp @@ -49,8 +49,7 @@ inline std::ostream& operator<<(std::ostream& out, const dai::CameraSensorConfig out << "minFps: " << config.minFps << ", "; out << "maxFps: " << config.maxFps << ", "; out << "type: " << config.type << ", "; - out << "fov: " - << "{x:" << config.fov.x << ", "; + out << "fov: " << "{x:" << config.fov.x << ", "; out << "y: " << config.fov.y << ", "; out << "width: " << config.fov.width << ", "; out << "height: " << config.fov.height << "}"; diff --git a/shared/depthai-shared b/shared/depthai-shared index 705e7f390..80fcef1eb 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 705e7f39011edccf72827af3476a2204b7c85b95 +Subproject commit 80fcef1eb888fd5510c4662010e6b74f7fdd62d8 From 60f86a1758bf4a7e933d2038943d3b57564456f1 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 22 May 2024 17:59:10 +0300 Subject: [PATCH 67/80] ImageAlign: add multi shave support --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/ImageAlign.hpp | 20 ++++++++++++++++++++ shared/depthai-shared | 2 +- src/pipeline/node/ImageAlign.cpp | 19 +++++++++++++++++++ 4 files changed, 41 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index f3d729930..745096f45 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "045361e9d6601e4c20d016182b93dce4159ec38b") +set(DEPTHAI_DEVICE_SIDE_COMMIT "914b77d97821f131b9c34a286cb375bb9ba14a05") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/ImageAlign.hpp b/include/depthai/pipeline/node/ImageAlign.hpp index f56c2447f..6da49024d 100644 --- a/include/depthai/pipeline/node/ImageAlign.hpp +++ b/include/depthai/pipeline/node/ImageAlign.hpp @@ -68,6 +68,26 @@ class ImageAlign : public NodeCRTP { * Specify the output size of the aligned depth map */ ImageAlign& setOutputSize(int alignWidth, int alignHeight); + + /** + * Specify whether to keep aspect ratio when resizing + */ + ImageAlign& setOutKeepAspectRatio(bool keep); + + /** + * Specify interpolation method to use when resizing + */ + ImageAlign& setInterpolation(Interpolation interp); + + /** + * Specify number of shaves to use for this node + */ + ImageAlign& setNumShaves(int numShaves); + + /** + * Specify number of frames in the pool + */ + ImageAlign& setNumFramesPool(int numFramesPool); }; } // namespace node diff --git a/shared/depthai-shared b/shared/depthai-shared index 4aec2d4df..9762f334f 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 4aec2d4df20a388cb86f78f2de6e96fa35c3513f +Subproject commit 9762f334f39fc841a5bbea9b1114bbac933710e0 diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index 0ccbeaed5..e7070673a 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -24,6 +24,25 @@ ImageAlign& ImageAlign::setOutputSize(int alignWidth, int alignHeight) { properties.alignHeight = alignHeight; return *this; } +ImageAlign& ImageAlign::setOutKeepAspectRatio(bool keep) { + properties.outKeepAspectRatio = keep; + return *this; +} + +ImageAlign& ImageAlign::setInterpolation(Interpolation interp) { + properties.interpolation = interp; + return *this; +} + +ImageAlign& ImageAlign::setNumShaves(int numShaves) { + properties.numShaves = numShaves; + return *this; +} + +ImageAlign& ImageAlign::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; + return *this; +} } // namespace node } // namespace dai \ No newline at end of file From f73c640b894c5f6c546496c6f3b0f2a69a9e978a Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 23 May 2024 17:22:00 +0200 Subject: [PATCH 68/80] [FW] Update FW with better increased MIPI priority for cameras --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 745096f45..4c0284887 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "914b77d97821f131b9c34a286cb375bb9ba14a05") +set(DEPTHAI_DEVICE_SIDE_COMMIT "7c85f3188603913d24f6b1042d8ed85cc8af0d6f") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From eac1046a0c5e2f238804ee35747b952d115a70ee Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 23 May 2024 18:57:16 +0200 Subject: [PATCH 69/80] clangformat --- include/depthai/common/CameraFeatures.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/depthai/common/CameraFeatures.hpp b/include/depthai/common/CameraFeatures.hpp index cf362d0ca..c36569336 100644 --- a/include/depthai/common/CameraFeatures.hpp +++ b/include/depthai/common/CameraFeatures.hpp @@ -49,7 +49,8 @@ inline std::ostream& operator<<(std::ostream& out, const dai::CameraSensorConfig out << "minFps: " << config.minFps << ", "; out << "maxFps: " << config.maxFps << ", "; out << "type: " << config.type << ", "; - out << "fov: " << "{x:" << config.fov.x << ", "; + out << "fov: " + << "{x:" << config.fov.x << ", "; out << "y: " << config.fov.y << ", "; out << "width: " << config.fov.width << ", "; out << "height: " << config.fov.height << "}"; From 43b5a27f5d43771789325743d40796609c97d4ad Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Thu, 23 May 2024 22:19:14 +0200 Subject: [PATCH 70/80] Remove tau character from comments --- include/depthai/device/CalibrationHandler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index b1bb2b4f1..cc9e8f09c 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -166,7 +166,7 @@ class CalibrationHandler { * Get the Distortion Coefficients object * * @param cameraId Uses the cameraId to identify which distortion Coefficients to return. - * @return the distortion coefficients of the requested camera in this order: [k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,Ï„x,Ï„y] for CameraModel::Perspective + * @return the distortion coefficients of the requested camera in this order: [k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,tx,ty] for CameraModel::Perspective * or [k1, k2, k3, k4] for CameraModel::Fisheye * see https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html for Perspective model (Rational Polynomial Model) * see https://docs.opencv.org/4.5.4/db/d58/group__calib3d__fisheye.html for Fisheye model From 3565c7e523a4845298ee28a2024389bf7502600e Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 14:55:26 +0200 Subject: [PATCH 71/80] Add the align examples --- examples/CMakeLists.txt | 5 + examples/ImageAlign/depth_align.cpp | 148 +++++++++++++++++++++++ examples/ImageAlign/image_align.cpp | 135 +++++++++++++++++++++ examples/ImageAlign/thermal_align.cpp | 163 ++++++++++++++++++++++++++ examples/ImageAlign/tof_align.cpp | 135 +++++++++++++++++++++ 5 files changed, 586 insertions(+) create mode 100644 examples/ImageAlign/depth_align.cpp create mode 100644 examples/ImageAlign/image_align.cpp create mode 100644 examples/ImageAlign/thermal_align.cpp create mode 100644 examples/ImageAlign/tof_align.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 999befb73..83fc9c5e0 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -388,3 +388,8 @@ dai_add_example(imu_video_synced Sync/imu_video_synced.cpp ON OFF) if(DEPTHAI_HAVE_PCL_SUPPORT) dai_add_example(visualize_pointcloud PointCloud/visualize_pointcloud.cpp ON ON) endif() + +dai_add_example(tof_align ImageAlign/tof_align.cpp OFF OFF) +dai_add_example(image_align ImageAlign/image_align.cpp ON OFF) +dai_add_example(thermal_align ImageAlign/thermal_align.cpp OFF OFF) +dai_add_example(depth_align ImageAlign/depth_align.cpp ON OFF) diff --git a/examples/ImageAlign/depth_align.cpp b/examples/ImageAlign/depth_align.cpp new file mode 100644 index 000000000..3a94d5ad1 --- /dev/null +++ b/examples/ImageAlign/depth_align.cpp @@ -0,0 +1,148 @@ +#include +#include +#include + +#include "depthai/depthai.hpp" + +constexpr auto FPS = 30.0; +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto LEFT_SOCKET = dai::CameraBoardSocket::CAM_B; +constexpr auto RIGHT_SOCKET = dai::CameraBoardSocket::CAM_C; +constexpr auto ALIGN_SOCKET = LEFT_SOCKET; + +constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; +constexpr auto LEFT_RIGHT_RESOLUTION = dai::MonoCameraProperties::SensorResolution::THE_400_P; +constexpr auto ISP_SCALE = 3; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 10) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +double rgbWeight = 0.4; +double depthWeight = 0.6; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + depthWeight = 1.0 - rgbWeight; +} + +cv::Mat colorizeDepth(const cv::Mat& frameDepth) { + cv::Mat invalidMask = (frameDepth == 0); + cv::Mat depthFrameColor; + try { + double minDepth = 0.0; + double maxDepth = 0.0; + cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); + if(minDepth == maxDepth) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + return depthFrameColor; + } + cv::Mat logDepth; + frameDepth.convertTo(logDepth, CV_32F); + cv::log(logDepth, logDepth); + logDepth.setTo(log(minDepth), invalidMask); + cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); + depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); + } catch(const std::exception& e) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + } + return depthFrameColor; +} + +int main() { + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + auto sync = pipeline.create(); + auto out = pipeline.create(); + auto align = pipeline.create(); + + left->setResolution(LEFT_RIGHT_RESOLUTION); + left->setBoardSocket(LEFT_SOCKET); + left->setFps(FPS); + + right->setResolution(LEFT_RIGHT_RESOLUTION); + right->setBoardSocket(RIGHT_SOCKET); + right->setFps(FPS); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(COLOR_RESOLUTION); + camRgb->setFps(FPS); + camRgb->setIspScale(1, ISP_SCALE); + + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setDepthAlign(LEFT_SOCKET); + + out->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); + + // Linking + camRgb->isp.link(sync->inputs["rgb"]); + left->out.link(stereo->left); + right->out.link(stereo->right); + stereo->depth.link(align->input); + align->outputAligned.link(sync->inputs["depth_aligned"]); + camRgb->isp.link(align->inputAlignTo); + sync->out.link(out->input); + + dai::Device device(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + + FPSCounter fpsCounter; + + std::string windowName = "rgb-depth"; + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, 1280, 720); + cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto frameDepth = messageGroup->get("depth_aligned"); + + if(frameRgb && frameDepth) { + auto cvFrame = frameRgb->getCvFrame(); + + // Colorize the aligned depth + auto alignedDepthColorized = colorizeDepth(frameDepth->getCvFrame()); + + cv::imshow("Depth aligned", alignedDepthColorized); + + cv::Mat blended; + cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended); + cv::putText(blended, "FPS: " + std::to_string(fpsCounter.getFps()), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); + cv::imshow(windowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + + return 0; +} diff --git a/examples/ImageAlign/image_align.cpp b/examples/ImageAlign/image_align.cpp new file mode 100644 index 000000000..a676d13a0 --- /dev/null +++ b/examples/ImageAlign/image_align.cpp @@ -0,0 +1,135 @@ +#include +#include +#include + +#include "depthai/depthai.hpp" + +constexpr auto FPS = 30.0; + +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto LEFT_SOCKET = dai::CameraBoardSocket::CAM_B; +constexpr auto RIGHT_SOCKET = dai::CameraBoardSocket::CAM_C; +constexpr auto ALIGN_SOCKET = LEFT_SOCKET; + +constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; +constexpr auto LEFT_RIGHT_RESOLUTION = dai::MonoCameraProperties::SensorResolution::THE_720_P; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 100) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +double rgbWeight = 0.4; +double depthWeight = 0.6; +int staticDepthPlane = 0; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + depthWeight = 1.0 - rgbWeight; +} + +void updateDepthPlane(int depth, void*) { + staticDepthPlane = depth; +} + +int main() { + dai::Pipeline pipeline; + + auto camRgb = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); + auto sync = pipeline.create(); + auto out = pipeline.create(); + auto align = pipeline.create(); + auto cfgIn = pipeline.create(); + + left->setResolution(LEFT_RIGHT_RESOLUTION); + left->setBoardSocket(LEFT_SOCKET); + left->setFps(FPS); + + right->setResolution(LEFT_RIGHT_RESOLUTION); + right->setBoardSocket(RIGHT_SOCKET); + right->setFps(FPS); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(COLOR_RESOLUTION); + camRgb->setFps(FPS); + camRgb->setIspScale(1, 3); + + out->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); + + cfgIn->setStreamName("config"); + + align->outputAligned.link(sync->inputs["aligned"]); + camRgb->isp.link(sync->inputs["rgb"]); + camRgb->isp.link(align->inputAlignTo); + left->out.link(align->input); + sync->out.link(out->input); + cfgIn->out.link(align->inputConfig); + + dai::Device device(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + auto cfgQ = device.getInputQueue("config"); + + FPSCounter fpsCounter; + + std::string windowName = "rgb-left"; + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, 1280, 720); + cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); + cv::createTrackbar("Static Depth Plane [mm]", windowName, nullptr, 2000, updateDepthPlane); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto leftAligned = messageGroup->get("aligned"); + + if(frameRgb && leftAligned) { + auto frameRgbCv = frameRgb->getCvFrame(); + auto leftCv = leftAligned->getCvFrame(); + + if(leftCv.channels() == 1) { + cv::cvtColor(leftCv, leftCv, cv::COLOR_GRAY2BGR); + } + if(leftCv.size() != frameRgbCv.size()) { + cv::resize(leftCv, leftCv, frameRgbCv.size()); + } + + cv::Mat blended; + cv::addWeighted(frameRgbCv, rgbWeight, leftCv, depthWeight, 0, blended); + cv::imshow(windowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + + auto cfg = align->initialConfig.get(); + cfg.staticDepthPlane = staticDepthPlane; + auto alignConfig = std::make_shared(); + alignConfig->set(cfg); + cfgQ->send(alignConfig); + } + + return 0; +} diff --git a/examples/ImageAlign/thermal_align.cpp b/examples/ImageAlign/thermal_align.cpp new file mode 100644 index 000000000..a84a652f0 --- /dev/null +++ b/examples/ImageAlign/thermal_align.cpp @@ -0,0 +1,163 @@ +#include +#include +#include +#include "depthai/depthai.hpp" +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" + +constexpr auto FPS = 25.0; +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; + +class FPSCounter { +public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if (frameTimes.size() > 100) { + frameTimes.pop(); + } + } + + double getFps() { + if (frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + +private: + std::queue frameTimes; +}; + +double rgbWeight = 0.4; +double thermalWeight = 0.6; +int staticDepthPlane = 0; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + thermalWeight = 1.0 - rgbWeight; +} + +void updateDepthPlane(int depth, void*) { + staticDepthPlane = depth; +} + +cv::Mat createNaNMask(const cv::Mat& frame) { + cv::Mat nanMask = cv::Mat::zeros(frame.size(), CV_8UC1); + for (int r = 0; r < frame.rows; ++r) { + for (int c = 0; c < frame.cols; ++c) { + if (std::isnan(frame.at(r, c))) { + nanMask.at(r, c) = 255; + } + } + } + return nanMask; +} + +int main() { + dai::Device device; + int thermalWidth = -1, thermalHeight = -1; + bool thermalFound = false; + dai::CameraBoardSocket thermalSocket; + + for (const auto& features : device.getConnectedCameraFeatures()) { + if (std::find(features.supportedTypes.begin(), features.supportedTypes.end(), dai::CameraSensorType::THERMAL) != features.supportedTypes.end()) { + thermalFound = true; + thermalSocket = features.socket; + thermalWidth = features.width; + thermalHeight = features.height; + break; + } + } + + if (!thermalFound) { + throw std::runtime_error("No thermal camera found!"); + } + + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto thermalCam = pipeline.create(); + auto sync = pipeline.create(); + auto out = pipeline.create(); + auto align = pipeline.create(); + auto cfgIn = pipeline.create(); + + thermalCam->setBoardSocket(thermalSocket); + thermalCam->setFps(FPS); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(COLOR_RESOLUTION); + camRgb->setFps(FPS); + camRgb->setIspScale(1, 3); + + out->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); + + cfgIn->setStreamName("config"); + + align->outputAligned.link(sync->inputs["aligned"]); + camRgb->isp.link(sync->inputs["rgb"]); + camRgb->isp.link(align->inputAlignTo); + thermalCam->raw.link(align->input); + sync->out.link(out->input); + cfgIn->out.link(align->inputConfig); + + device.startPipeline(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + auto cfgQ = device.getInputQueue("config"); + + FPSCounter fpsCounter; + + std::string windowName = "rgb-thermal"; + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, 1280, 720); + cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); + cv::createTrackbar("Static Depth Plane [mm]", windowName, nullptr, 2000, updateDepthPlane); + + while (true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto thermalAligned = messageGroup->get("aligned"); + + if (frameRgb && thermalAligned) { + auto frameRgbCv = frameRgb->getCvFrame(); + auto thermalFrame = thermalAligned->getFrame(true); + + // Colorize the aligned depth + cv::Mat mask; + cv::Mat colormappedFrame; + cv::Mat thermalFrameFloat; + thermalFrame.convertTo(thermalFrameFloat, CV_32F); + // Get a mask for the nan values + mask = createNaNMask(thermalFrameFloat); + auto meanValue = cv::mean(thermalFrameFloat, ~mask); + thermalFrameFloat.setTo(meanValue, mask); + cv::normalize(thermalFrameFloat, thermalFrameFloat, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::applyColorMap(thermalFrameFloat, colormappedFrame, cv::COLORMAP_MAGMA); + + colormappedFrame.setTo(cv::Scalar(0, 0, 0), mask); + + cv::Mat blended; + cv::addWeighted(frameRgbCv, rgbWeight, colormappedFrame, thermalWeight, 0, blended); + cv::putText(blended, "FPS: " + std::to_string(fpsCounter.getFps()), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); + cv::imshow(windowName, blended); + } + + int key = cv::waitKey(1); + if (key == 'q' || key == 'Q') { + break; + } + + auto cfg = align->initialConfig.get(); + cfg.staticDepthPlane = staticDepthPlane; + auto alignConfig = std::make_shared(); + alignConfig->set(cfg); + cfgQ->send(alignConfig); + } + + return 0; +} \ No newline at end of file diff --git a/examples/ImageAlign/tof_align.cpp b/examples/ImageAlign/tof_align.cpp new file mode 100644 index 000000000..8d62006d4 --- /dev/null +++ b/examples/ImageAlign/tof_align.cpp @@ -0,0 +1,135 @@ +#include + +#include "depthai/depthai.hpp" + +constexpr auto FPS = 30.0; + +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_B; +constexpr auto TOF_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto ALIGN_SOCKET = RGB_SOCKET; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 100) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +cv::Mat colorizeDepth(const cv::Mat& frameDepth) { + cv::Mat invalidMask = (frameDepth == 0); + cv::Mat depthFrameColor; + try { + double minDepth = 0.0; + double maxDepth = 0.0; + cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); + if(minDepth == maxDepth) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + return depthFrameColor; + } + cv::Mat logDepth; + frameDepth.convertTo(logDepth, CV_32F); + cv::log(logDepth, logDepth); + logDepth.setTo(log(minDepth), invalidMask); + cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); + depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); + } catch(const std::exception& e) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + } + return depthFrameColor; +} + +double rgbWeight = 0.4; +double depthWeight = 0.6; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + depthWeight = 1.0 - rgbWeight; +} + +int main() { + dai::Pipeline pipeline; + + auto camRgb = pipeline.create(); + auto camTof = pipeline.create(); + auto tof = pipeline.create(); + auto sync = pipeline.create(); + auto align = pipeline.create(); + auto xout = pipeline.create(); + + camTof->setFps(FPS); + camTof->setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG); + camTof->setBoardSocket(TOF_SOCKET); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P); + camRgb->setFps(FPS); + camRgb->setIspScale(1, 2); + + xout->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast(1000 / FPS))); + + camRgb->isp.link(sync->inputs["rgb"]); + camTof->raw.link(tof->input); + tof->depth.link(align->input); + align->outputAligned.link(sync->inputs["depth_aligned"]); + camRgb->isp.link(align->inputAlignTo); + sync->out.link(xout->input); + + dai::Device device(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + + FPSCounter fpsCounter; + + std::string rgbDepthWindowName = "rgb-depth"; + cv::namedWindow(rgbDepthWindowName); + cv::createTrackbar("RGB Weight %", rgbDepthWindowName, nullptr, 100, updateBlendWeights); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto frameDepth = messageGroup->get("depth_aligned"); + + if(frameRgb && frameDepth) { + auto cvFrame = frameRgb->getCvFrame(); + auto alignedDepthColorized = colorizeDepth(frameDepth->getFrame()); + + cv::putText(alignedDepthColorized, + "FPS: " + std::to_string(fpsCounter.getFps()), + cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, + 1, + cv::Scalar(255, 255, 255), + 2); + + cv::imshow("depth", alignedDepthColorized); + + cv::Mat blended; + cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended); + cv::imshow(rgbDepthWindowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + + return 0; +} From 82e4a812f37181064a8d97e840cfb881992f0d9e Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 14:56:16 +0200 Subject: [PATCH 72/80] Remove unused example --- examples/DepthAlign/rgb_depth_align.cpp | 143 ------------------------ 1 file changed, 143 deletions(-) delete mode 100644 examples/DepthAlign/rgb_depth_align.cpp diff --git a/examples/DepthAlign/rgb_depth_align.cpp b/examples/DepthAlign/rgb_depth_align.cpp deleted file mode 100644 index 84c3ea704..000000000 --- a/examples/DepthAlign/rgb_depth_align.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include - -#include "utility.hpp" - -// Includes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -// Optional. If set (true), the ColorCamera is downscaled from 1080p to 720p. -// Otherwise (false), the aligned depth is automatically upscaled to 1080p -static std::atomic downscaleColor{true}; -static constexpr int fps = 30; -// The disparity is computed at this resolution, then upscaled to RGB resolution -static constexpr auto monoRes = dai::MonoCameraProperties::SensorResolution::THE_720_P; - -static float rgbWeight = 0.4f; -static float depthWeight = 0.6f; - -static void updateBlendWeights(int percentRgb, void* ctx) { - rgbWeight = float(percentRgb) / 100.f; - depthWeight = 1.f - rgbWeight; -} - -int main() { - using namespace std; - - // Create pipeline - dai::Pipeline pipeline; - dai::Device device; - std::vector queueNames; - - // Define sources and outputs - auto camRgb = pipeline.create(); - auto left = pipeline.create(); - auto right = pipeline.create(); - auto stereo = pipeline.create(); - - auto depthAlign = pipeline.create(); - - auto rgbOut = pipeline.create(); - auto depthOut = pipeline.create(); - auto alignedDepthOut = pipeline.create(); - - rgbOut->setStreamName("rgb"); - queueNames.push_back("rgb"); - depthOut->setStreamName("depth"); - queueNames.push_back("depth"); - alignedDepthOut->setStreamName("depth_aligned"); - queueNames.push_back("depth_aligned"); - - // Properties - camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A); - camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - camRgb->setFps(fps); - camRgb->setIspScale(2, 3); - // For now, RGB needs fixed focus to properly align with depth. - // This value was used during calibration - try { - auto calibData = device.readCalibration2(); - auto lensPosition = calibData.getLensPosition(dai::CameraBoardSocket::CAM_A); - if(lensPosition) { - camRgb->initialControl.setManualFocus(lensPosition); - } - } catch(const std::exception& ex) { - std::cout << ex.what() << std::endl; - return 1; - } - - left->setResolution(monoRes); - left->setCamera("left"); - left->setFps(fps); - right->setResolution(monoRes); - right->setCamera("right"); - right->setFps(fps); - - stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); - - // Linking - camRgb->isp.link(rgbOut->input); - left->out.link(stereo->left); - right->out.link(stereo->right); - stereo->depth.link(depthAlign->inputDepth); - - depthAlign->outputDepthAlign.link(alignedDepthOut->input); - depthAlign->properties.alignTo = dai::CameraBoardSocket::CAM_A; - stereo->depth.link(depthOut->input); - - // Connect to device and start pipeline - device.startPipeline(pipeline); - - // Sets queues size and behavior - for(const auto& name : queueNames) { - device.getOutputQueue(name, 4, false); - } - - std::unordered_map frame; - - auto rgbWindowName = "rgb"; - auto depthWindowName = "depth"; - auto blendedWindowName = "rgb-depth"; - auto alignedDepth = "depth_aligned"; - cv::namedWindow(rgbWindowName); - cv::namedWindow(depthWindowName); - cv::namedWindow(blendedWindowName); - cv::namedWindow(alignedDepth); - int defaultValue = (int)(rgbWeight * 100); - cv::createTrackbar("RGB Weight %", blendedWindowName, &defaultValue, 100, updateBlendWeights); - - while(true) { - std::unordered_map> latestPacket; - - auto queueEvents = device.getQueueEvents(queueNames); - for(const auto& name : queueEvents) { - auto packets = device.getOutputQueue(name)->tryGetAll(); - auto count = packets.size(); - if(count > 0) { - latestPacket[name] = packets[count - 1]; - } - } - - for(const auto& name : queueNames) { - if(latestPacket.find(name) != latestPacket.end()) { - if(name == depthWindowName) { - frame[name] = latestPacket[name]->getFrame(); - } else if (name == rgbWindowName) { - frame[name] = latestPacket[name]->getCvFrame(); - } - else { - frame[name] = latestPacket[name]->getFrame(); - } - - - cv::imshow(name, frame[name]); - } - } - - int key = cv::waitKey(1); - if(key == 'q' || key == 'Q') { - return 0; - } - } - return 0; -} From 76c21e21631383433fe7bf06e5d0809e7d988d18 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 16:43:19 +0200 Subject: [PATCH 73/80] Add the cast examples --- examples/CMakeLists.txt | 25 +++++++++++ examples/Cast/blur.cpp | 51 +++++++++++++++++++++++ examples/Cast/concat.cpp | 63 ++++++++++++++++++++++++++++ examples/Cast/diff.cpp | 66 ++++++++++++++++++++++++++++++ include/depthai/pipeline/nodes.hpp | 1 + 5 files changed, 206 insertions(+) create mode 100644 examples/Cast/blur.cpp create mode 100644 examples/Cast/concat.cpp create mode 100644 examples/Cast/diff.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 83fc9c5e0..79efec3a9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -196,6 +196,22 @@ hunter_private_data( LOCATION concat_model ) +# blur model +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/blur_simplified_openvino_2021.4_6shave.blob" + SHA1 "14d543bbaceffa438071f83be58ad22a07ce33ee" + FILE "blur_simplified_openvino_2021.4_6shave.blob" + LOCATION blur_model +) + +# diff model +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/diff_openvino_2022.1_6shave.blob" + SHA1 "fe9600e617d222f986a699f18e77e80ce2485000" + FILE "diff_openvino_2022.1_6shave.blob" + LOCATION diff_model +) + # normalization model hunter_private_data( URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/normalize_openvino_2021.4_4shave.blob" @@ -389,7 +405,16 @@ if(DEPTHAI_HAVE_PCL_SUPPORT) dai_add_example(visualize_pointcloud PointCloud/visualize_pointcloud.cpp ON ON) endif() +# ImageAlign dai_add_example(tof_align ImageAlign/tof_align.cpp OFF OFF) dai_add_example(image_align ImageAlign/image_align.cpp ON OFF) dai_add_example(thermal_align ImageAlign/thermal_align.cpp OFF OFF) dai_add_example(depth_align ImageAlign/depth_align.cpp ON OFF) + +# Cast +dai_add_example(blur Cast/blur.cpp ON OFF) +target_compile_definitions(blur PRIVATE BLOB_PATH="${blur_model}") +dai_add_example(concat Cast/concat.cpp ON OFF) +target_compile_definitions(concat PRIVATE BLOB_PATH="${concat_model}") +dai_add_example(diff Cast/diff.cpp ON OFF) +target_compile_definitions(diff PRIVATE BLOB_PATH="${diff_model}") \ No newline at end of file diff --git a/examples/Cast/blur.cpp b/examples/Cast/blur.cpp new file mode 100644 index 000000000..d8c2d256d --- /dev/null +++ b/examples/Cast/blur.cpp @@ -0,0 +1,51 @@ +#include +#include +#include + +constexpr int SHAPE = 300; + +int main() { + dai::Pipeline p; + + auto camRgb = p.create(); + auto nn = p.create(); + auto rgbOut = p.create(); + auto cast = p.create(); + auto castXout = p.create(); + + camRgb->setPreviewSize(SHAPE, SHAPE); + camRgb->setInterleaved(false); + + nn->setBlobPath(BLOB_PATH); + + rgbOut->setStreamName("rgb"); + castXout->setStreamName("cast"); + + cast->setOutputFrameType(dai::RawImgFrame::Type::BGR888p); + + // Linking + camRgb->preview.link(nn->input); + camRgb->preview.link(rgbOut->input); + nn->out.link(cast->input); + cast->output.link(castXout->input); + + dai::Device device(p); + auto qCam = device.getOutputQueue("rgb", 4, false); + auto qCast = device.getOutputQueue("cast", 4, false); + + while (true) { + auto inCast = qCast->get(); + auto inRgb = qCam->get(); + + if (inCast && inRgb) { + cv::imshow("Blur", inCast->getCvFrame()); + cv::imshow("Original", inRgb->getCvFrame()); + } + + if (cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/examples/Cast/concat.cpp b/examples/Cast/concat.cpp new file mode 100644 index 000000000..c1ad793a3 --- /dev/null +++ b/examples/Cast/concat.cpp @@ -0,0 +1,63 @@ +#include +#include + +constexpr int SHAPE = 300; + +int main() { + dai::Pipeline p; + + auto camRgb = p.create(); + auto left = p.create(); + auto right = p.create(); + auto manipLeft = p.create(); + auto manipRight = p.create(); + auto nn = p.create(); + auto cast = p.create(); + auto castXout = p.create(); + + camRgb->setPreviewSize(SHAPE, SHAPE); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + + left->setCamera("left"); + left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + manipLeft->initialConfig.setResize(SHAPE, SHAPE); + manipLeft->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + + right->setCamera("right"); + right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + manipRight->initialConfig.setResize(SHAPE, SHAPE); + manipRight->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + + nn->setBlobPath(BLOB_PATH); + nn->setNumInferenceThreads(2); + + castXout->setStreamName("cast"); + cast->setOutputFrameType(dai::ImgFrame::Type::BGR888p); + + // Linking + left->out.link(manipLeft->inputImage); + right->out.link(manipRight->inputImage); + manipLeft->out.link(nn->inputs["img1"]); + camRgb->preview.link(nn->inputs["img2"]); + manipRight->out.link(nn->inputs["img3"]); + nn->out.link(cast->input); + cast->output.link(castXout->input); + + // Pipeline is defined, now we can connect to the device + dai::Device device(p); + auto qCast = device.getOutputQueue("cast", 4, false); + + while (true) { + auto inCast = qCast->get(); + if (inCast) { + cv::imshow("Concated frames", inCast->getCvFrame()); + } + + if (cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/examples/Cast/diff.cpp b/examples/Cast/diff.cpp new file mode 100644 index 000000000..052c9d44f --- /dev/null +++ b/examples/Cast/diff.cpp @@ -0,0 +1,66 @@ +#include +#include +#include + +constexpr int SHAPE = 720; + +int main() { + dai::Pipeline p; + + auto camRgb = p.create(); + auto nn = p.create(); + auto script = p.create(); + auto rgbXout = p.create(); + auto cast = p.create(); + auto castXout = p.create(); + + camRgb->setVideoSize(SHAPE, SHAPE); + camRgb->setPreviewSize(SHAPE, SHAPE); + camRgb->setInterleaved(false); + + nn->setBlobPath(BLOB_PATH); + + script->setScript(R"( + old = node.io['in'].get() + while True: + frame = node.io['in'].get() + node.io['img1'].send(old) + node.io['img2'].send(frame) + old = frame + )"); + + rgbXout->setStreamName("rgb"); + castXout->setStreamName("cast"); + cast->setOutputFrameType(dai::RawImgFrame::Type::GRAY8); + + // Linking + camRgb->preview.link(script->inputs["in"]); + script->outputs["img1"].link(nn->inputs["img1"]); + script->outputs["img2"].link(nn->inputs["img2"]); + camRgb->video.link(rgbXout->input); + nn->out.link(cast->input); + cast->output.link(castXout->input); + + // Pipeline is defined, now we can connect to the device + dai::Device device(p); + auto qCam = device.getOutputQueue("rgb", 4, false); + auto qCast = device.getOutputQueue("cast", 4, false); + + while (true) { + auto colorFrame = qCam->get(); + if (colorFrame) { + cv::imshow("Color", colorFrame->getCvFrame()); + } + + auto inCast = qCast->get(); + if (inCast) { + cv::imshow("Diff", inCast->getCvFrame()); + } + + if (cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 1107c52a3..3dc939a3d 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -3,6 +3,7 @@ // all the nodes #include "node/AprilTag.hpp" #include "node/Camera.hpp" +#include "node/Cast.hpp" #include "node/ColorCamera.hpp" #include "node/DetectionNetwork.hpp" #include "node/DetectionParser.hpp" From ffea6abf767306383de962d8bd815ad3a0799a1b Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 17:38:45 +0200 Subject: [PATCH 74/80] Add the tof depth example --- examples/CMakeLists.txt | 5 +- examples/ToF/tof_depth.cpp | 140 +++++++++++++++++++++++++++++++++++++ 2 files changed, 144 insertions(+), 1 deletion(-) create mode 100644 examples/ToF/tof_depth.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 79efec3a9..11cfe8f93 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -417,4 +417,7 @@ target_compile_definitions(blur PRIVATE BLOB_PATH="${blur_model}") dai_add_example(concat Cast/concat.cpp ON OFF) target_compile_definitions(concat PRIVATE BLOB_PATH="${concat_model}") dai_add_example(diff Cast/diff.cpp ON OFF) -target_compile_definitions(diff PRIVATE BLOB_PATH="${diff_model}") \ No newline at end of file +target_compile_definitions(diff PRIVATE BLOB_PATH="${diff_model}") + +# ToF +dai_add_example(tof_depth ToF/tof_depth.cpp ON OFF) \ No newline at end of file diff --git a/examples/ToF/tof_depth.cpp b/examples/ToF/tof_depth.cpp new file mode 100644 index 000000000..9a8c9f8ae --- /dev/null +++ b/examples/ToF/tof_depth.cpp @@ -0,0 +1,140 @@ +#include +#include +#include +#include +#include +#include +#include "depthai-shared/datatype/RawToFConfig.hpp" +#include "depthai/pipeline/datatype/ToFConfig.hpp" + +constexpr auto SHAPE = 720; + +std::shared_ptr createConfig(dai::RawToFConfig configRaw) { + auto config = std::make_shared(); + config->set(std::move(configRaw)); + return config; +} + +cv::Mat colorizeDepth(const cv::Mat& frameDepth) { + cv::Mat invalidMask = (frameDepth == 0); + cv::Mat depthFrameColor; + try { + double minDepth = 0.0; + double maxDepth = 0.0; + cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); + if(minDepth == maxDepth) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + return depthFrameColor; + } + cv::Mat logDepth; + frameDepth.convertTo(logDepth, CV_32F); + cv::log(logDepth, logDepth); + logDepth.setTo(log(minDepth), invalidMask); + cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); + depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); + } catch(const std::exception& e) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + } + return depthFrameColor; +} + +int main() { + dai::Pipeline pipeline; + cv::Mat cvColorMap; + + auto tof = pipeline.create(); + + // Configure the ToF node + auto tofConfig = tof->initialConfig.get(); + + tofConfig.enableOpticalCorrection = true; + tofConfig.enablePhaseShuffleTemporalFilter = true; + tofConfig.phaseUnwrappingLevel = 4; + tofConfig.phaseUnwrapErrorThreshold = 300; + + tofConfig.enableTemperatureCorrection = false; // Not yet supported + + auto xinTofConfig = pipeline.create(); + xinTofConfig->setStreamName("tofConfig"); + xinTofConfig->out.link(tof->inputConfig); + + tof->initialConfig.set(tofConfig); + + auto camTof = pipeline.create(); + camTof->setFps(30); + camTof->setBoardSocket(dai::CameraBoardSocket::CAM_A); + camTof->raw.link(tof->input); + + auto xout = pipeline.create(); + xout->setStreamName("depth"); + tof->depth.link(xout->input); + + tofConfig = tof->initialConfig.get(); + + + dai::Device device(pipeline); + std::cout << "Connected cameras: " << device.getConnectedCameraFeatures().size() << std::endl; + auto qDepth = device.getOutputQueue("depth"); + + auto tofConfigInQueue = device.getInputQueue("tofConfig"); + + int counter = 0; + while (true) { + auto start = std::chrono::high_resolution_clock::now(); + int key = cv::waitKey(1); + if (key == 'f') { + tofConfig.enableFPPNCorrection = !tofConfig.enableFPPNCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'o') { + tofConfig.enableOpticalCorrection = !tofConfig.enableOpticalCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'w') { + tofConfig.enableWiggleCorrection = !tofConfig.enableWiggleCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 't') { + tofConfig.enableTemperatureCorrection = !tofConfig.enableTemperatureCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'q') { + break; + } else if (key == '0') { + tofConfig.enablePhaseUnwrapping = false; + tofConfig.phaseUnwrappingLevel = 0; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '1') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 1; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '2') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 2; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '3') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 3; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '4') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 4; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '5') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 5; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'm') { + std::vector medianSettings = { dai::MedianFilter::MEDIAN_OFF, dai::MedianFilter::KERNEL_3x3, dai::MedianFilter::KERNEL_5x5, dai::MedianFilter::KERNEL_7x7 }; + auto currentMedian = tofConfig.median; + auto nextMedian = medianSettings[(std::find(medianSettings.begin(), medianSettings.end(), currentMedian) - medianSettings.begin() + 1) % medianSettings.size()]; + tofConfig.median = nextMedian; + tofConfigInQueue->send(createConfig(tofConfig)); + } + + auto imgFrame = qDepth->get(); // blocking call, will wait until new data has arrived + auto depthColorized = colorizeDepth(imgFrame->getFrame()); + + cv::imshow("Colorized depth", depthColorized); + counter++; + } + + return 0; +} From 56e05d580d410cf386769aadecb6214901a18ea4 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 18:22:50 +0200 Subject: [PATCH 75/80] Add spatial examples for yolo --- examples/CMakeLists.txt | 5 +- .../spatial_tiny_yolo_tof.cpp | 171 ++++++++++++++++++ 2 files changed, 175 insertions(+), 1 deletion(-) create mode 100644 examples/SpatialDetection/spatial_tiny_yolo_tof.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 11cfe8f93..c41ad074a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -420,4 +420,7 @@ dai_add_example(diff Cast/diff.cpp ON OFF) target_compile_definitions(diff PRIVATE BLOB_PATH="${diff_model}") # ToF -dai_add_example(tof_depth ToF/tof_depth.cpp ON OFF) \ No newline at end of file +dai_add_example(spatial_tiny_yolo_tof_v3 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF) +dai_add_example(spatial_tiny_yolo_tof_v4 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF) +target_compile_definitions(spatial_tiny_yolo_tof_v3 PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}") +target_compile_definitions(spatial_tiny_yolo_tof_v4 PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}") \ No newline at end of file diff --git a/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp new file mode 100644 index 000000000..6bf8325d5 --- /dev/null +++ b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp @@ -0,0 +1,171 @@ +#include +#include +#include +#include +#include + +const int FPS = 15; + +static const std::vector labelMap = { + "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", + "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", + "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", + "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", + "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", + "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", + "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "sofa", "pottedplant", "bed", + "diningtable", "toilet", "tvmonitor", "laptop", "mouse", "remote", "keyboard", "cell phone", + "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", + "teddy bear", "hair drier", "toothbrush" +}; + +static std::atomic syncNN{true}; + +int main(int argc, char** argv) { + dai::Pipeline pipeline; + + auto camRgb = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto tof = pipeline.create(); + auto camTof = pipeline.create(); + auto imageAlign = pipeline.create(); + + auto xoutRgb = pipeline.create(); + auto xoutNN = pipeline.create(); + auto xoutDepth = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + xoutNN->setStreamName("detections"); + xoutDepth->setStreamName("depth"); + + camRgb->setPreviewSize(416, 416); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_B); + camRgb->setFps(FPS); + + camTof->setFps(FPS); + camTof->setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG); + camTof->setBoardSocket(dai::CameraBoardSocket::CAM_A); + + imageAlign->setOutputSize(640, 400); + + spatialDetectionNetwork->setBlobPath(BLOB_PATH); + spatialDetectionNetwork->setConfidenceThreshold(0.5f); + spatialDetectionNetwork->input.setBlocking(false); + spatialDetectionNetwork->setBoundingBoxScaleFactor(0.5); + spatialDetectionNetwork->setDepthLowerThreshold(100); + spatialDetectionNetwork->setDepthUpperThreshold(5000); + + spatialDetectionNetwork->setNumClasses(80); + spatialDetectionNetwork->setCoordinateSize(4); + spatialDetectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); + spatialDetectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}}); + spatialDetectionNetwork->setIouThreshold(0.5f); + + camTof->raw.link(tof->input); + tof->depth.link(imageAlign->input); + + camRgb->preview.link(spatialDetectionNetwork->input); + if(syncNN) { + spatialDetectionNetwork->passthrough.link(xoutRgb->input); + } else { + camRgb->preview.link(xoutRgb->input); + } + + spatialDetectionNetwork->out.link(xoutNN->input); + + camRgb->isp.link(imageAlign->inputAlignTo); + imageAlign->outputAligned.link(spatialDetectionNetwork->inputDepth); + spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); + + dai::Device device(pipeline); + + auto previewQueue = device.getOutputQueue("rgb", 4, false); + auto detectionNNQueue = device.getOutputQueue("detections", 4, false); + auto depthQueue = device.getOutputQueue("depth", 4, false); + + auto startTime = std::chrono::steady_clock::now(); + int counter = 0; + float fps = 0; + auto color = cv::Scalar(255, 255, 255); + + while(true) { + auto inPreview = previewQueue->get(); + auto inDet = detectionNNQueue->get(); + auto depth = depthQueue->get(); + + cv::Mat frame = inPreview->getCvFrame(); + cv::Mat depthFrame = depth->getFrame(); // depthFrame values are in millimeters + + cv::Mat depthFrameColor; + cv::normalize(depthFrame, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1); + cv::equalizeHist(depthFrameColor, depthFrameColor); + cv::applyColorMap(depthFrameColor, depthFrameColor, cv::COLORMAP_HOT); + + counter++; + auto currentTime = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast>(currentTime - startTime); + if(elapsed > std::chrono::seconds(1)) { + fps = counter / elapsed.count(); + counter = 0; + startTime = currentTime; + } + + auto detections = inDet->detections; + + for(const auto& detection : detections) { + auto roiData = detection.boundingBoxMapping; + auto roi = roiData.roi; + roi = roi.denormalize(depthFrameColor.cols, depthFrameColor.rows); + auto topLeft = roi.topLeft(); + auto bottomRight = roi.bottomRight(); + auto xmin = static_cast(topLeft.x); + auto ymin = static_cast(topLeft.y); + auto xmax = static_cast(bottomRight.x); + auto ymax = static_cast(bottomRight.y); + cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, 1); + + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + uint32_t labelIndex = detection.label; + std::string labelStr = std::to_string(labelIndex); + if(labelIndex < labelMap.size()) { + labelStr = labelMap[labelIndex]; + } + cv::putText(frame, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + + std::stringstream depthX; + depthX << "X: " << static_cast(detection.spatialCoordinates.x) << " mm"; + cv::putText(frame, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + std::stringstream depthY; + depthY << "Y: " << static_cast(detection.spatialCoordinates.y) << " mm"; + cv::putText(frame, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + std::stringstream depthZ; + depthZ << "Z: " << static_cast(detection.spatialCoordinates.z) << " mm"; + cv::putText(frame, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + + std::stringstream fpsStr; + fpsStr << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, frame.rows - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); + + cv::imshow("depth", depthFrameColor); + cv::imshow("rgb", frame); + + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} From e0e72e1d93e8a81175ae366b6a5fa81d3f7b4769 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 18:25:17 +0200 Subject: [PATCH 76/80] [FW] Update FW to develop --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 4c0284887..78aa2fa3f 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "7c85f3188603913d24f6b1042d8ed85cc8af0d6f") +set(DEPTHAI_DEVICE_SIDE_COMMIT "3fc15f7cb1c4b485db880630fff6df3d5f5cca5e") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 2bb244cc11918047fc115d15c0549afcef05ec33 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 19:19:01 +0200 Subject: [PATCH 77/80] Cleanup --- examples/CMakeLists.txt | 2 -- examples/Cast/blur.cpp | 3 +-- examples/SpatialDetection/spatial_tiny_yolo_tof.cpp | 2 +- include/depthai/pipeline/datatype/ImageAlignConfig.hpp | 3 --- include/depthai/pipeline/node/Cast.hpp | 5 ----- include/depthai/pipeline/node/ImageAlign.hpp | 5 +---- src/pipeline/node/Cast.cpp | 2 -- src/pipeline/node/ImageAlign.cpp | 2 -- 8 files changed, 3 insertions(+), 21 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index c41ad074a..f3c0762b8 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -260,8 +260,6 @@ dai_add_example(edge_detector EdgeDetector/edge_detector.cpp ON OFF) dai_add_example(feature_detector FeatureTracker/feature_detector.cpp ON OFF) dai_add_example(feature_tracker FeatureTracker/feature_tracker.cpp ON OFF) -# dai_add_example(image_align ImageAlign/rgb_image_align.cpp ON) - # host_side dai_add_example(device_queue_event host_side/device_queue_event.cpp ON OFF) dai_add_example(opencv_support host_side/opencv_support.cpp ON OFF) diff --git a/examples/Cast/blur.cpp b/examples/Cast/blur.cpp index d8c2d256d..85e57d955 100644 --- a/examples/Cast/blur.cpp +++ b/examples/Cast/blur.cpp @@ -1,6 +1,5 @@ #include #include -#include constexpr int SHAPE = 300; @@ -21,7 +20,7 @@ int main() { rgbOut->setStreamName("rgb"); castXout->setStreamName("cast"); - cast->setOutputFrameType(dai::RawImgFrame::Type::BGR888p); + cast->setOutputFrameType(dai::ImgFrame::Type::BGR888p); // Linking camRgb->preview.link(nn->input); diff --git a/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp index 6bf8325d5..84c68689c 100644 --- a/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp +++ b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp @@ -4,7 +4,7 @@ #include #include -const int FPS = 15; +constexpr auto FPS = 15; static const std::vector labelMap = { "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", diff --git a/include/depthai/pipeline/datatype/ImageAlignConfig.hpp b/include/depthai/pipeline/datatype/ImageAlignConfig.hpp index f78f1f522..404e28f3b 100644 --- a/include/depthai/pipeline/datatype/ImageAlignConfig.hpp +++ b/include/depthai/pipeline/datatype/ImageAlignConfig.hpp @@ -1,8 +1,5 @@ #pragma once -#include -#include - #include "depthai-shared/datatype/RawImageAlignConfig.hpp" #include "depthai/pipeline/datatype/Buffer.hpp" diff --git a/include/depthai/pipeline/node/Cast.hpp b/include/depthai/pipeline/node/Cast.hpp index f6bba4b37..d4d9b8c00 100644 --- a/include/depthai/pipeline/node/Cast.hpp +++ b/include/depthai/pipeline/node/Cast.hpp @@ -2,11 +2,6 @@ #include -#include "depthai/pipeline/datatype/Tracklets.hpp" - -// standard -#include - // shared #include diff --git a/include/depthai/pipeline/node/ImageAlign.hpp b/include/depthai/pipeline/node/ImageAlign.hpp index 6da49024d..d6684df7f 100644 --- a/include/depthai/pipeline/node/ImageAlign.hpp +++ b/include/depthai/pipeline/node/ImageAlign.hpp @@ -2,9 +2,6 @@ #include -// standard -#include - // shared #include @@ -65,7 +62,7 @@ class ImageAlign : public NodeCRTP { Output passthroughInput{*this, "passthroughInput", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; /** - * Specify the output size of the aligned depth map + * Specify the output size of the aligned image */ ImageAlign& setOutputSize(int alignWidth, int alignHeight); diff --git a/src/pipeline/node/Cast.cpp b/src/pipeline/node/Cast.cpp index 2a95d23d4..dfb3ff451 100644 --- a/src/pipeline/node/Cast.cpp +++ b/src/pipeline/node/Cast.cpp @@ -1,7 +1,5 @@ #include "depthai/pipeline/node/Cast.hpp" -#include "spdlog/fmt/fmt.h" - namespace dai { namespace node { diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index e7070673a..8f0ceb73b 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -1,7 +1,5 @@ #include "depthai/pipeline/node/ImageAlign.hpp" -#include "spdlog/fmt/fmt.h" - namespace dai { namespace node { From d5a51493f57fc6baf2fee7463a41a92529e6faab Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 19:23:04 +0200 Subject: [PATCH 78/80] Clangformat --- examples/Cast/blur.cpp | 6 ++--- examples/Cast/concat.cpp | 6 ++--- examples/Cast/diff.cpp | 10 +++---- examples/ImageAlign/thermal_align.cpp | 27 ++++++++++--------- .../spatial_tiny_yolo_tof.cpp | 24 ++++++++--------- 5 files changed, 36 insertions(+), 37 deletions(-) diff --git a/examples/Cast/blur.cpp b/examples/Cast/blur.cpp index 85e57d955..8c762a2d0 100644 --- a/examples/Cast/blur.cpp +++ b/examples/Cast/blur.cpp @@ -32,16 +32,16 @@ int main() { auto qCam = device.getOutputQueue("rgb", 4, false); auto qCast = device.getOutputQueue("cast", 4, false); - while (true) { + while(true) { auto inCast = qCast->get(); auto inRgb = qCam->get(); - if (inCast && inRgb) { + if(inCast && inRgb) { cv::imshow("Blur", inCast->getCvFrame()); cv::imshow("Original", inRgb->getCvFrame()); } - if (cv::waitKey(1) == 'q') { + if(cv::waitKey(1) == 'q') { break; } } diff --git a/examples/Cast/concat.cpp b/examples/Cast/concat.cpp index c1ad793a3..7168ac4c3 100644 --- a/examples/Cast/concat.cpp +++ b/examples/Cast/concat.cpp @@ -48,13 +48,13 @@ int main() { dai::Device device(p); auto qCast = device.getOutputQueue("cast", 4, false); - while (true) { + while(true) { auto inCast = qCast->get(); - if (inCast) { + if(inCast) { cv::imshow("Concated frames", inCast->getCvFrame()); } - if (cv::waitKey(1) == 'q') { + if(cv::waitKey(1) == 'q') { break; } } diff --git a/examples/Cast/diff.cpp b/examples/Cast/diff.cpp index 052c9d44f..78960beaf 100644 --- a/examples/Cast/diff.cpp +++ b/examples/Cast/diff.cpp @@ -1,6 +1,6 @@ #include -#include #include +#include constexpr int SHAPE = 720; @@ -46,18 +46,18 @@ int main() { auto qCam = device.getOutputQueue("rgb", 4, false); auto qCast = device.getOutputQueue("cast", 4, false); - while (true) { + while(true) { auto colorFrame = qCam->get(); - if (colorFrame) { + if(colorFrame) { cv::imshow("Color", colorFrame->getCvFrame()); } auto inCast = qCast->get(); - if (inCast) { + if(inCast) { cv::imshow("Diff", inCast->getCvFrame()); } - if (cv::waitKey(1) == 'q') { + if(cv::waitKey(1) == 'q') { break; } } diff --git a/examples/ImageAlign/thermal_align.cpp b/examples/ImageAlign/thermal_align.cpp index a84a652f0..e30d2651e 100644 --- a/examples/ImageAlign/thermal_align.cpp +++ b/examples/ImageAlign/thermal_align.cpp @@ -1,6 +1,7 @@ #include #include #include + #include "depthai/depthai.hpp" #include "depthai/pipeline/datatype/ImageAlignConfig.hpp" @@ -9,22 +10,22 @@ constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; class FPSCounter { -public: + public: void tick() { auto now = std::chrono::steady_clock::now(); frameTimes.push(now); - if (frameTimes.size() > 100) { + if(frameTimes.size() > 100) { frameTimes.pop(); } } double getFps() { - if (frameTimes.size() <= 1) return 0; + if(frameTimes.size() <= 1) return 0; auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); return (frameTimes.size() - 1) * 1000.0 / duration; } -private: + private: std::queue frameTimes; }; @@ -43,9 +44,9 @@ void updateDepthPlane(int depth, void*) { cv::Mat createNaNMask(const cv::Mat& frame) { cv::Mat nanMask = cv::Mat::zeros(frame.size(), CV_8UC1); - for (int r = 0; r < frame.rows; ++r) { - for (int c = 0; c < frame.cols; ++c) { - if (std::isnan(frame.at(r, c))) { + for(int r = 0; r < frame.rows; ++r) { + for(int c = 0; c < frame.cols; ++c) { + if(std::isnan(frame.at(r, c))) { nanMask.at(r, c) = 255; } } @@ -59,8 +60,8 @@ int main() { bool thermalFound = false; dai::CameraBoardSocket thermalSocket; - for (const auto& features : device.getConnectedCameraFeatures()) { - if (std::find(features.supportedTypes.begin(), features.supportedTypes.end(), dai::CameraSensorType::THERMAL) != features.supportedTypes.end()) { + for(const auto& features : device.getConnectedCameraFeatures()) { + if(std::find(features.supportedTypes.begin(), features.supportedTypes.end(), dai::CameraSensorType::THERMAL) != features.supportedTypes.end()) { thermalFound = true; thermalSocket = features.socket; thermalWidth = features.width; @@ -69,7 +70,7 @@ int main() { } } - if (!thermalFound) { + if(!thermalFound) { throw std::runtime_error("No thermal camera found!"); } @@ -116,14 +117,14 @@ int main() { cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); cv::createTrackbar("Static Depth Plane [mm]", windowName, nullptr, 2000, updateDepthPlane); - while (true) { + while(true) { auto messageGroup = queue->get(); fpsCounter.tick(); auto frameRgb = messageGroup->get("rgb"); auto thermalAligned = messageGroup->get("aligned"); - if (frameRgb && thermalAligned) { + if(frameRgb && thermalAligned) { auto frameRgbCv = frameRgb->getCvFrame(); auto thermalFrame = thermalAligned->getFrame(true); @@ -148,7 +149,7 @@ int main() { } int key = cv::waitKey(1); - if (key == 'q' || key == 'Q') { + if(key == 'q' || key == 'Q') { break; } diff --git a/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp index 84c68689c..c97f8adf3 100644 --- a/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp +++ b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp @@ -1,23 +1,21 @@ +#include #include +#include #include #include -#include -#include constexpr auto FPS = 15; static const std::vector labelMap = { - "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", - "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", - "horse", "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", - "handbag", "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", - "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", - "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", "orange", - "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "sofa", "pottedplant", "bed", - "diningtable", "toilet", "tvmonitor", "laptop", "mouse", "remote", "keyboard", "cell phone", - "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", - "teddy bear", "hair drier", "toothbrush" -}; + "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", + "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", + "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", + "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", + "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", + "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", + "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", + "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", + "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; static std::atomic syncNN{true}; From b6318d048c9c4f4a86b7871f435413dd590fc678 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 May 2024 19:53:14 +0200 Subject: [PATCH 79/80] Bump version to v2.26.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2cc51767f..c68a04c96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ if(WIN32) endif() # Create depthai project -project(depthai VERSION "2.25.1" LANGUAGES CXX C) +project(depthai VERSION "2.26.0" LANGUAGES CXX C) get_directory_property(has_parent PARENT_DIRECTORY) if(has_parent) set(DEPTHAI_VERSION ${PROJECT_VERSION} PARENT_SCOPE) From b9e25844a26c6f0f5c7c5968689d6869ae5193c8 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Sat, 25 May 2024 05:45:06 +0300 Subject: [PATCH 80/80] FW: fix timings for OV9282 fsync at 400p on CAM_A --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 78aa2fa3f..ddf90cb8b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "3fc15f7cb1c4b485db880630fff6df3d5f5cca5e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "24a3b465b979de3f69410cd225914d8bd029f3ba") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "")