From 3634db32aeffb0a92ddb5572b2238db2ade429a8 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sun, 16 May 2021 01:44:39 +0200 Subject: [PATCH 001/101] WIP: Device configuration --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/device/Device.hpp | 61 +++-- include/depthai/device/DeviceBootloader.hpp | 15 +- include/depthai/pipeline/Pipeline.hpp | 28 ++- shared/depthai-bootloader-shared | 2 +- shared/depthai-shared | 2 +- src/device/Device.cpp | 108 ++++++-- src/device/DeviceBootloader.cpp | 46 ++-- src/pipeline/Pipeline.cpp | 36 ++- src/utility/Resources.cpp | 265 ++++++++++++-------- src/utility/Resources.hpp | 9 +- 11 files changed, 375 insertions(+), 199 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 88d2756dd..2bb81b1e3 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 "fddbe1b7868bdb67945b50ddc9dfaeb15bba2d60") +set(DEPTHAI_DEVICE_SIDE_COMMIT "8641723d7f46be11969c96d5346636cba2f2334b") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/device/Device.hpp b/include/depthai/device/Device.hpp index 9d575bc17..aa79d241d 100644 --- a/include/depthai/device/Device.hpp +++ b/include/depthai/device/Device.hpp @@ -20,6 +20,7 @@ #include "depthai-shared/common/ChipTemperature.hpp" #include "depthai-shared/common/CpuUsage.hpp" #include "depthai-shared/common/MemoryInfo.hpp" +#include "depthai-shared/device/PrebootConfig.hpp" #include "depthai-shared/log/LogLevel.hpp" #include "depthai-shared/log/LogMessage.hpp" @@ -44,6 +45,18 @@ class Device { static constexpr std::size_t EVENT_QUEUE_MAXIMUM_SIZE{2048}; /// Default rate at which system information is logged static constexpr float DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ{1.0f}; + /// Default UsbSpeed for device connection + static constexpr UsbSpeed DEFAULT_USB_SPEED{UsbSpeed::SUPER}; + + // Structures + + /** + * Device specific configuration + */ + struct Config { + OpenVINO::Version version; + PrebootConfig preboot; + }; // static API @@ -106,15 +119,16 @@ class Device { /** * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. * @param pipeline Pipeline to be executed on the device - * @param pathToCmd Path to custom device firmware + * @param maxUsbSpeed Maximum allowed USB speed */ - Device(const Pipeline& pipeline, const char* pathToCmd); + Device(const Pipeline& pipeline, UsbSpeed maxUsbSpeed); /** * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. * @param pipeline Pipeline to be executed on the device * @param pathToCmd Path to custom device firmware */ + Device(const Pipeline& pipeline, const char* pathToCmd); Device(const Pipeline& pipeline, const std::string& pathToCmd); /** @@ -129,16 +143,17 @@ class Device { * Connects to device specified by devInfo. * @param pipeline Pipeline to be executed on the device * @param devInfo DeviceInfo which specifies which device to connect to - * @param pathToCmd Path to custom device firmware + * @param maxUsbSpeed Maximum allowed USB speed */ - Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd); + Device(const Pipeline& pipeline, const DeviceInfo& devInfo, UsbSpeed maxUsbSpeed); /** * Connects to device specified by devInfo. * @param pipeline Pipeline to be executed on the device * @param devInfo DeviceInfo which specifies which device to connect to - * @param usb2Mode Path to custom device firmware + * @param pathToCmd Path to custom device firmware */ + Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd); Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd); /** @@ -157,15 +172,16 @@ class Device { /** * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. * @param version OpenVINO version which the device will be booted with - * @param pathToCmd Path to custom device firmware + * @param maxUsbSpeed Maximum allowed USB speed */ - Device(OpenVINO::Version version, const char* pathToCmd); + Device(OpenVINO::Version version, UsbSpeed maxUsbSpeed); /** * Connects to any available device with a DEFAULT_SEARCH_TIME timeout. * @param version OpenVINO version which the device will be booted with * @param pathToCmd Path to custom device firmware */ + Device(OpenVINO::Version version, const char* pathToCmd); Device(OpenVINO::Version version, const std::string& pathToCmd); /** @@ -180,18 +196,26 @@ class Device { * Connects to device specified by devInfo. * @param version OpenVINO version which the device will be booted with * @param devInfo DeviceInfo which specifies which device to connect to - * @param pathToCmd Path to custom device firmware + * @param maxUsbSpeed Maximum USB speed */ - Device(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd); + Device(OpenVINO::Version version, const DeviceInfo& devInfo, UsbSpeed maxUsbSpeed); /** * Connects to device specified by devInfo. * @param version OpenVINO version which the device will be booted with * @param devInfo DeviceInfo which specifies which device to connect to - * @param usb2Mode Path to custom device firmware + * @param pathToCmd Path to custom device firmware */ + Device(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd); Device(OpenVINO::Version version, const DeviceInfo& devInfo, const std::string& pathToCmd); + /** + * Connects to device 'devInfo' with custom config. + * @param devInfo DeviceInfo which specifies which device to connect to + * @param config Device custom configuration to boot with + */ + Device(const DeviceInfo& devInfo, Config config); + /** * Device destructor. Closes the connection and data queues. */ @@ -451,6 +475,13 @@ class Device { */ CpuUsage getLeonMssCpuUsage(); + /** + * Retrieves USB connection speed + * + * @returns USB connection speed of connected device if applicable. Unknown otherwise. + */ + UsbSpeed getUsbSpeed(); + /** * Explicitly closes connection to device. * @note This function does not need to be explicitly called @@ -464,10 +495,12 @@ class Device { bool isClosed() const; private: - // private static + // private void init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); void init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); - void init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd, tl::optional pipeline); + void init(OpenVINO::Version version, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd); + void init(const Pipeline& pipeline, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd); + void init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional pipeline); void checkClosed() const; std::shared_ptr connection; @@ -514,8 +547,8 @@ class Device { class Impl; Pimpl pimpl; - // OpenVINO version device was booted with - OpenVINO::Version openvinoVersion; + // Device config + Config config; }; } // namespace dai diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index f4e5d04b4..c9f5827c0 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -1,6 +1,7 @@ #pragma once // std +#include #include #include #include @@ -8,6 +9,7 @@ // project #include "CallbackHandler.hpp" #include "DataQueue.hpp" +#include "Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/xlink/XLinkConnection.hpp" #include "depthai/xlink/XLinkStream.hpp" @@ -41,6 +43,11 @@ class DeviceBootloader { unsigned versionMajor, versionMinor, versionPatch; }; + struct Config { + Config() : timeout(3) {} + std::chrono::milliseconds timeout; + }; + // Static API /** * Searches for connected devices in either UNBOOTED or BOOTLOADER states and returns first available. @@ -60,7 +67,7 @@ class DeviceBootloader { * @param pathToCmd Optional path to custom device firmware * @returns Depthai application package */ - static std::vector createDepthaiApplicationPackage(Pipeline& pipeline, std::string pathToCmd = ""); + static std::vector createDepthaiApplicationPackage(Pipeline& pipeline, UsbSpeed maxUsbSpeed = Device::DEFAULT_USB_SPEED); /** * Saves application package to a file which can be flashed to depthai device. @@ -68,7 +75,7 @@ class DeviceBootloader { * @param pipeline Pipeline from which to create the application package * @param pathToCmd Optional path to custom device firmware */ - static void saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, std::string pathToCmd = ""); + static void saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, UsbSpeed maxUsbSpeed = Device::DEFAULT_USB_SPEED); /** * @returns Embedded bootloader version @@ -78,7 +85,7 @@ class DeviceBootloader { /** * @returns Embedded bootloader binary */ - static std::vector getEmbeddedBootloaderBinary(); + static std::vector getEmbeddedBootloaderBinary(Config config); // DeviceBootloader() = delete; @@ -121,7 +128,7 @@ class DeviceBootloader { * @param progressCallback Callback that sends back a value between 0..1 which signifies current flashing progress * @param path Optional parameter to custom bootloader to flash */ - std::tuple flashBootloader(std::function progressCallback, std::string path = ""); + std::tuple flashBootloader(std::function progressCallback, Config config = {}); /** * @returns Version of current running bootloader diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index 31a937bea..a45545fee 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -12,6 +12,7 @@ #include "depthai/openvino/OpenVINO.hpp" // shared +#include "depthai-shared/device/PrebootConfig.hpp" #include "depthai-shared/pipeline/PipelineSchema.hpp" #include "depthai-shared/properties/GlobalProperties.hpp" @@ -33,9 +34,11 @@ class PipelineImpl { // Functions Node::Id getNextUniqueId(); PipelineSchema getPipelineSchema() const; - OpenVINO::Version getPipelineOpenVINOVersion() const; + tl::optional getPipelineOpenVINOVersion() const; + bool isOpenVINOVersionCompatible(OpenVINO::Version version) const; AssetManager getAllAssets() const; void setCameraTuningBlobPath(const std::string& path); + PrebootConfig getDevicePrebootConfig() const; // Access to nodes std::vector> getAllNodes() const; @@ -43,7 +46,7 @@ class PipelineImpl { std::shared_ptr getNode(Node::Id id) const; std::shared_ptr getNode(Node::Id id); - void serialize(PipelineSchema& schema, Assets& assets, std::vector& assetStorage, OpenVINO::Version& version) const; + void serialize(PipelineSchema& schema, Assets& assets, std::vector& assetStorage) const; void remove(std::shared_ptr node); std::vector getConnections() const; @@ -118,8 +121,8 @@ class Pipeline { PipelineSchema getPipelineSchema(); // void loadAssets(AssetManager& assetManager); - void serialize(PipelineSchema& schema, Assets& assets, std::vector& assetStorage, OpenVINO::Version& version) const { - impl()->serialize(schema, assets, assetStorage, version); + void serialize(PipelineSchema& schema, Assets& assets, std::vector& assetStorage) const { + impl()->serialize(schema, assets, assetStorage); } /** @@ -216,8 +219,13 @@ class Pipeline { impl()->forceRequiredOpenVINOVersion = version; } - /// Get required OpenVINO version to run this pipeline + /// Get possible OpenVINO version to run this pipeline OpenVINO::Version getOpenVINOVersion() const { + return impl()->getPipelineOpenVINOVersion().value_or(Pipeline::DEFAULT_OPENVINO_VERSION); + } + + /// Get required OpenVINO version to run this pipeline. Can be none + tl::optional getRequiredOpenVINOVersion() const { return impl()->getPipelineOpenVINOVersion(); } @@ -225,6 +233,16 @@ class Pipeline { void setCameraTuningBlobPath(const std::string& path) { impl()->setCameraTuningBlobPath(path); } + + /// Checks whether a given OpenVINO version is compatible with the pipeline + bool isOpenVINOVersionCompatible(OpenVINO::Version version) const { + return impl()->isOpenVINOVersionCompatible(version); + } + + /// Checks whether a given OpenVINO version is compatible with the pipeline + PrebootConfig getDevicePrebootConfig() const { + return impl()->getDevicePrebootConfig(); + } }; } // namespace dai diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index c8779ca58..8e9965a34 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit c8779ca581659803b6f3e465cc2bde7b22203be0 +Subproject commit 8e9965a34a2c8de02a151f111e2397657e2ddf52 diff --git a/shared/depthai-shared b/shared/depthai-shared index cd7a857f4..e23302538 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit cd7a857f4bc4a4d0e5348a6cc6a536c0a3abc7b7 +Subproject commit e23302538d867042e1ec6c2eb6ff723e7c4d538a diff --git a/src/device/Device.cpp b/src/device/Device.cpp index ed3c425c5..641618b31 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -84,6 +84,7 @@ template std::tuple Device::getAnyAvailableDevice(std::chrono: constexpr std::chrono::seconds Device::DEFAULT_SEARCH_TIME; constexpr std::size_t Device::EVENT_QUEUE_MAXIMUM_SIZE; constexpr float Device::DEFAULT_SYSTEM_INFORMATION_LOGGING_RATE_HZ; +constexpr UsbSpeed Device::DEFAULT_USB_SPEED; template std::tuple Device::getAnyAvailableDevice(std::chrono::duration timeout) { @@ -214,6 +215,10 @@ Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mod init(pipeline, true, usb2Mode, ""); } +Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, UsbSpeed maxUsbSpeed) : deviceInfo(devInfo) { + init(pipeline, true, maxUsbSpeed, ""); +} + Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { init(pipeline, false, false, std::string(pathToCmd)); } @@ -254,6 +259,16 @@ Device::Device(const Pipeline& pipeline, const std::string& pathToCmd) { init(pipeline, false, false, pathToCmd); } +Device::Device(const Pipeline& pipeline, UsbSpeed maxUsbSpeed) { + // Searches for any available device for 'default' timeout + bool found = false; + std::tie(found, deviceInfo) = getAnyAvailableDevice(); + + // If no device found, throw + if(!found) throw std::runtime_error("No available devices"); + init(pipeline, true, maxUsbSpeed, ""); +} + Device::Device(const Pipeline& pipeline, bool usb2Mode) { // Searches for any available device for 'default' timeout bool found = false; @@ -268,6 +283,10 @@ Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, bool usb2Mo init(version, true, usb2Mode, ""); } +Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, UsbSpeed maxUsbSpeed) : deviceInfo(devInfo) { + init(version, true, maxUsbSpeed, ""); +} + Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { init(version, false, false, std::string(pathToCmd)); } @@ -318,6 +337,20 @@ Device::Device(OpenVINO::Version version, bool usb2Mode) { init(version, true, usb2Mode, ""); } +Device::Device(OpenVINO::Version version, UsbSpeed maxUsbSpeed) { + // Searches for any available device for 'default' timeout + bool found = false; + std::tie(found, deviceInfo) = getAnyAvailableDevice(); + + // If no device found, throw + if(!found) throw std::runtime_error("No available devices"); + init(version, true, maxUsbSpeed, ""); +} + +Device::Device(const DeviceInfo& devInfo, Config config) { + init2(config, false, {}, {}); +} + void Device::close() { // Only allow to close once if(closed.exchange(true)) return; @@ -372,35 +405,56 @@ Device::~Device() { } void Device::init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { - // Initalize depthai library if not already - initialize(); - + Config cfg; + // Specify usb speed + cfg.preboot.maxUsbSpeed = usb2Mode ? UsbSpeed::HIGH : Device::DEFAULT_USB_SPEED; // Specify the OpenVINO version - openvinoVersion = version; - - spdlog::debug("Device - OpenVINO version: {}", OpenVINO::getVersionName(openvinoVersion)); - - init2(embeddedMvcmd, usb2Mode, pathToMvcmd, tl::nullopt); + cfg.version = version; + init2(cfg, embeddedMvcmd, pathToMvcmd, {}); } - void Device::init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { + Config cfg; + // Specify usb speed + cfg.preboot.maxUsbSpeed = usb2Mode ? UsbSpeed::HIGH : Device::DEFAULT_USB_SPEED; + // Specify the OpenVINO version + cfg.version = pipeline.getOpenVINOVersion(); + init2(cfg, embeddedMvcmd, pathToMvcmd, pipeline); +} +void Device::init(OpenVINO::Version version, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd) { + Config cfg; + // Specify usb speed + cfg.preboot.maxUsbSpeed = maxUsbSpeed; + // Specify the OpenVINO version + cfg.version = version; + init2(cfg, embeddedMvcmd, pathToMvcmd, {}); +} +void Device::init(const Pipeline& pipeline, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd) { + Config cfg; + // Specify usb speed + cfg.preboot.maxUsbSpeed = maxUsbSpeed; + // Specify the OpenVINO version + cfg.version = pipeline.getOpenVINOVersion(); + init2(cfg, embeddedMvcmd, pathToMvcmd, pipeline); +} + +void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional pipeline) { // Initalize depthai library if not already initialize(); - // Mark the OpenVINO version - openvinoVersion = pipeline.getOpenVINOVersion(); - - spdlog::debug("Device - pipeline serialized, OpenVINO version: {}", OpenVINO::getVersionName(openvinoVersion)); + // Specify cfg + config = cfg; - init2(embeddedMvcmd, usb2Mode, pathToMvcmd, pipeline); -} + if(pipeline) { + spdlog::debug("Device - pipeline serialized, OpenVINO version: {}", OpenVINO::getVersionName(config.version)); + } else { + spdlog::debug("Device - OpenVINO version: {}", OpenVINO::getVersionName(config.version)); + } -void Device::init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd, tl::optional pipeline) { // Set logging pattern of device (device id + shared pattern) pimpl->setPattern(fmt::format("[{}] {}", deviceInfo.getMxId(), LOG_DEFAULT_PATTERN)); // Get embedded mvcmd - std::vector embeddedFw = Resources::getInstance().getDeviceFirmware(usb2Mode, openvinoVersion); + std::vector embeddedFw = Resources::getInstance().getDeviceFirmware(config); // Init device (if bootloader, handle correctly - issue USB boot command) if(deviceInfo.state == X_LINK_UNBOOTED) { @@ -838,6 +892,12 @@ CpuUsage Device::getLeonMssCpuUsage() { return client->call("getLeonMssCpuUsage").as(); } +UsbSpeed Device::getUsbSpeed() { + checkClosed(); + + return client->call("getUsbSpeed").as(); +} + bool Device::isPipelineRunning() { checkClosed(); @@ -923,16 +983,16 @@ bool Device::startPipeline(const Pipeline& pipeline) { throw std::runtime_error("Pipeline is already running"); } + // Check openvino version + if(!pipeline.isOpenVINOVersionCompatible(config.version)) { + throw std::runtime_error("Device booted with different OpenVINO version that pipeline requires"); + } + + // Serialize the pipeline PipelineSchema schema; Assets assets; std::vector assetStorage; - - // Mark the OpenVINO version and serialize the pipeline - OpenVINO::Version pipelineOpenvinoVersion; - pipeline.serialize(schema, assets, assetStorage, pipelineOpenvinoVersion); - if(openvinoVersion != pipelineOpenvinoVersion) { - throw std::runtime_error("Device booted with different OpenVINO version that pipeline requires"); - } + pipeline.serialize(schema, assets, assetStorage); // Open queues upfront, let queues know about data sizes (input queues) // Go through Pipeline and check for 'XLinkIn' and 'XLinkOut' nodes diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index c578afd00..c9872fc16 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -51,24 +51,21 @@ std::vector DeviceBootloader::getAllAvailableDevices() { return availableDevices; } -std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& pipeline, std::string pathToCmd) { +std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& pipeline, UsbSpeed maxUsbSpeed) { // Serialize the pipeline PipelineSchema schema; Assets assets; std::vector assetStorage; - OpenVINO::Version version; - pipeline.serialize(schema, assets, assetStorage, version); + pipeline.serialize(schema, assets, assetStorage); + + // Prepare Device::Config + Device::Config cfg; + cfg.version = pipeline.getOpenVINOVersion(); + cfg.preboot = pipeline.getDevicePrebootConfig(); + cfg.preboot.maxUsbSpeed = maxUsbSpeed; // Prepare device firmware - std::vector deviceFirmware; - if(pathToCmd != "") { - std::ifstream fwStream(pathToCmd, std::ios::binary); - if(!fwStream.is_open()) throw std::runtime_error("Cannot create application package, device firmware at path: " + pathToCmd + " doesn't exist"); - deviceFirmware = std::vector(std::istreambuf_iterator(fwStream), {}); - } else { - // TODO(themarpe) - specify OpenVINO version - deviceFirmware = Resources::getInstance().getDeviceFirmware(false, version); - } + std::vector deviceFirmware = Resources::getInstance().getDeviceFirmware(cfg); // Create msgpacks std::vector pipelineBinary, assetsBinary; @@ -155,7 +152,11 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd) if(deviceInfo.state == X_LINK_UNBOOTED) { // Unbooted device found, boot to BOOTLOADER and connect with XLinkConnection constructor if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(), X_LINK_BOOTLOADER); + Config cfg; + // Never timeout + cfg.timeout = std::chrono::milliseconds(-1); + + connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(cfg), X_LINK_BOOTLOADER); } else { connection = std::make_shared(deviceInfo, pathToMvcmd, X_LINK_BOOTLOADER); } @@ -272,8 +273,8 @@ std::tuple DeviceBootloader::flash(std::function return flashDepthaiApplicationPackage(progressCb, createDepthaiApplicationPackage(pipeline)); } -void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, std::string pathToCmd) { - auto dap = createDepthaiApplicationPackage(pipeline, pathToCmd); +void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, UsbSpeed maxUsbSpeed) { + auto dap = createDepthaiApplicationPackage(pipeline, maxUsbSpeed); std::ofstream outfile(path, std::ios::binary); outfile.write(reinterpret_cast(dap.data()), dap.size()); } @@ -317,15 +318,8 @@ std::tuple DeviceBootloader::flashDepthaiApplicationPackage(s return {result.success, result.errorMsg}; } -std::tuple DeviceBootloader::flashBootloader(std::function progressCb, std::string path) { - std::vector package; - if(path != "") { - std::ifstream fwStream(path, std::ios::binary); - if(!fwStream.is_open()) throw std::runtime_error("Cannot flash bootloader, binary at path: " + path + " doesn't exist"); - package = std::vector(std::istreambuf_iterator(fwStream), {}); - } else { - package = getEmbeddedBootloaderBinary(); - } +std::tuple DeviceBootloader::flashBootloader(std::function progressCb, DeviceBootloader::Config config) { + std::vector package = getEmbeddedBootloaderBinary(config); // get streamId streamId_t streamId = stream->getStreamId(); @@ -371,8 +365,8 @@ bool DeviceBootloader::isEmbeddedVersion() { return isEmbedded; } -std::vector DeviceBootloader::getEmbeddedBootloaderBinary() { - return Resources::getInstance().getBootloaderFirmware(); +std::vector DeviceBootloader::getEmbeddedBootloaderBinary(Config config) { + return Resources::getInstance().getBootloaderFirmware(config); } DeviceBootloader::Version::Version(const std::string& v) : versionMajor(0), versionMinor(0), versionPatch(0) { diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 853dd2fc1..984b7773b 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -101,15 +101,12 @@ std::vector> PipelineImpl::getAllNodes() { return nodes; } -void PipelineImpl::serialize(PipelineSchema& schema, Assets& assets, std::vector& assetStorage, OpenVINO::Version& version) const { +void PipelineImpl::serialize(PipelineSchema& schema, Assets& assets, std::vector& assetStorage) const { // Set schema schema = getPipelineSchema(); // set assets and generate asset storage getAllAssets().serialize(assets, assetStorage); - - // detect and set openvino version - version = getPipelineOpenVINOVersion(); } AssetManager PipelineImpl::getAllAssets() const { @@ -200,7 +197,16 @@ PipelineSchema PipelineImpl::getPipelineSchema() const { return schema; } -OpenVINO::Version PipelineImpl::getPipelineOpenVINOVersion() const { +bool PipelineImpl::isOpenVINOVersionCompatible(OpenVINO::Version version) const { + auto ver = getPipelineOpenVINOVersion(); + if(ver) { + return OpenVINO::areVersionsBlobCompatible(version, *ver); + } else { + return true; + } +} + +tl::optional PipelineImpl::getPipelineOpenVINOVersion() const { // Loop over nodes, and get the required information tl::optional version; std::string lastNodeNameWithRequiredVersion = ""; @@ -240,17 +246,23 @@ OpenVINO::Version PipelineImpl::getPipelineOpenVINOVersion() const { } } - // After iterating over, set openvinoVersion - OpenVINO::Version openvinoVersion = DEFAULT_OPENVINO_VERSION; + // After iterating over, return appropriate version if(forceRequiredOpenVINOVersion) { - // set to forced version - openvinoVersion = *forceRequiredOpenVINOVersion; + // Return forced version + return forceRequiredOpenVINOVersion; } else if(version) { - // set to detected version - openvinoVersion = *version; + // Return detected version + return version; + } else { + // Return null + return tl::nullopt; } +} - return openvinoVersion; +PrebootConfig PipelineImpl::getDevicePrebootConfig() const { + PrebootConfig cfg{}; + // TODO - rest of preboot config + return cfg; } void PipelineImpl::setCameraTuningBlobPath(const std::string& path) { diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 62d1227b8..071daaa53 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -16,6 +16,9 @@ #include "spdlog/fmt/chrono.h" #include "spdlog/spdlog.h" +// shared +#include "depthai-bootloader-shared/PrebootConfig.hpp" + extern "C" { #include "bspatch/bspatch.h" } @@ -28,46 +31,42 @@ CMRC_DECLARE(depthai); namespace dai { -static std::vector getEmbeddedBootloaderBinary(); +static std::vector createPrebootHeader(const std::vector& payload); #ifdef DEPTHAI_RESOURCES_TAR_XZ constexpr static auto CMRC_DEPTHAI_DEVICE_TAR_XZ = "depthai-device-fwp-" DEPTHAI_DEVICE_VERSION ".tar.xz"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_1_PATH = "depthai-device-openvino-2020.1-" DEPTHAI_DEVICE_VERSION ".cmd"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_3_PATH = "depthai-device-openvino-2020.3-" DEPTHAI_DEVICE_VERSION ".cmd"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_2_PATH = DEPTHAI_CMD_OPENVINO_2020_3_PATH; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_4_PATH = "depthai-device-openvino-2020.4-" DEPTHAI_DEVICE_VERSION ".cmd"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2021_1_PATH = "depthai-device-openvino-2021.1-" DEPTHAI_DEVICE_VERSION ".cmd"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2021_2_PATH = "depthai-device-openvino-2021.2-" DEPTHAI_DEVICE_VERSION ".cmd"; + +// Main FW constexpr static auto DEPTHAI_CMD_OPENVINO_2021_3_PATH = "depthai-device-openvino-2021.3-" DEPTHAI_DEVICE_VERSION ".cmd"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_1_USB2_PATCH_PATH = "depthai-device-usb2-patch-openvino-2020.1-" DEPTHAI_DEVICE_VERSION ".patch"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_3_USB2_PATCH_PATH = "depthai-device-usb2-patch-openvino-2020.3-" DEPTHAI_DEVICE_VERSION ".patch"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_2_USB2_PATCH_PATH = DEPTHAI_CMD_OPENVINO_2020_3_USB2_PATCH_PATH; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_4_USB2_PATCH_PATH = "depthai-device-usb2-patch-openvino-2020.4-" DEPTHAI_DEVICE_VERSION ".patch"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2021_1_USB2_PATCH_PATH = "depthai-device-usb2-patch-openvino-2021.1-" DEPTHAI_DEVICE_VERSION ".patch"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2021_2_USB2_PATCH_PATH = "depthai-device-usb2-patch-openvino-2021.2-" DEPTHAI_DEVICE_VERSION ".patch"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2021_3_USB2_PATCH_PATH = "depthai-device-usb2-patch-openvino-2021.3-" DEPTHAI_DEVICE_VERSION ".patch"; - -constexpr static std::array resourcesListTarXz = { - DEPTHAI_CMD_OPENVINO_2020_1_PATH, - DEPTHAI_CMD_OPENVINO_2020_2_PATH, - DEPTHAI_CMD_OPENVINO_2020_3_PATH, - DEPTHAI_CMD_OPENVINO_2020_4_PATH, - DEPTHAI_CMD_OPENVINO_2021_1_PATH, - DEPTHAI_CMD_OPENVINO_2021_2_PATH, - DEPTHAI_CMD_OPENVINO_2021_3_PATH, - DEPTHAI_CMD_OPENVINO_2020_1_USB2_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2020_2_USB2_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2020_3_USB2_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2020_4_USB2_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2021_1_USB2_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2021_2_USB2_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2021_3_USB2_PATCH_PATH, -}; - -std::vector Resources::getDeviceBinary(OpenVINO::Version version, bool usb2Mode) { - std::vector finalCmd; +// Patches from Main FW +constexpr static auto DEPTHAI_CMD_OPENVINO_2020_1_PATCH_PATH = "depthai-device-openvino-2020.1-" DEPTHAI_DEVICE_VERSION ".patch"; +constexpr static auto DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH = "depthai-device-openvino-2020.3-" DEPTHAI_DEVICE_VERSION ".patch"; +constexpr static auto DEPTHAI_CMD_OPENVINO_2020_2_PATCH_PATH = DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH; +constexpr static auto DEPTHAI_CMD_OPENVINO_2020_4_PATCH_PATH = "depthai-device-openvino-2020.4-" DEPTHAI_DEVICE_VERSION ".patch"; +constexpr static auto DEPTHAI_CMD_OPENVINO_2021_1_PATCH_PATH = "depthai-device-openvino-2021.1-" DEPTHAI_DEVICE_VERSION ".patch"; +constexpr static auto DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH = "depthai-device-openvino-2021.2-" DEPTHAI_DEVICE_VERSION ".patch"; + +// Creates std::array without explicitly needing to state the size +template +static constexpr auto array_of(T&&... t) -> std::array { + return {{std::forward(t)...}}; +} + +constexpr static auto resourcesListTarXz = array_of(DEPTHAI_CMD_OPENVINO_2021_3_PATH, + DEPTHAI_CMD_OPENVINO_2020_1_PATCH_PATH, + DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH, + DEPTHAI_CMD_OPENVINO_2020_2_PATCH_PATH, + DEPTHAI_CMD_OPENVINO_2020_4_PATCH_PATH, + DEPTHAI_CMD_OPENVINO_2021_1_PATCH_PATH, + DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH); + +std::vector Resources::getDeviceBinary(Device::Config config) { + std::vector depthaiBinary; + + // Get OpenVINO version + auto& version = config.version; // Check if env variable DEPTHAI_DEVICE_BINARY is set auto fwBinaryPath = spdlog::details::os::getenv("DEPTHAI_DEVICE_BINARY"); @@ -80,83 +79,82 @@ std::vector Resources::getDeviceBinary(OpenVINO::Version version, throw std::runtime_error(fmt::format("File at path {} pointed to by DEPTHAI_DEVICE_BINARY doesn't exist.", fwBinaryPath)); } - return std::vector(std::istreambuf_iterator(stream), {}); - } - + depthaiBinary = std::vector(std::istreambuf_iterator(stream), {}); + } else { // Binaries are resource compiled #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES - std::vector& depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_PATH]; - std::vector& depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_USB2_PATCH_PATH]; - - switch(version) { - case OpenVINO::VERSION_2020_1: - spdlog::warn("OpenVino version 2020.1 is deprecated and will be removed in the next release!"); - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2020_1_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_1_USB2_PATCH_PATH]; - break; - - case OpenVINO::VERSION_2020_2: - spdlog::warn("OpenVino version 2020.2 is deprecated and will be removed in the next release!"); - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2020_2_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_2_USB2_PATCH_PATH]; - break; - - case OpenVINO::VERSION_2020_3: - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2020_3_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_3_USB2_PATCH_PATH]; - break; - - case OpenVINO::VERSION_2020_4: - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2020_4_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_4_USB2_PATCH_PATH]; - break; - - case OpenVINO::VERSION_2021_1: - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_1_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_1_USB2_PATCH_PATH]; - break; - case OpenVINO::VERSION_2021_2: - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_2_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_2_USB2_PATCH_PATH]; - break; - case OpenVINO::VERSION_2021_3: - depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_PATH]; - depthaiUsb2Patch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_USB2_PATCH_PATH]; - break; - } - - if(usb2Mode) { - #ifdef DEPTHAI_PATCH_ONLY_MODE - - // Get new size - int64_t patchedSize = bspatch_mem_get_newsize(depthaiUsb2Patch.data(), depthaiUsb2Patch.size()); - - // Reserve space for patched binary - finalCmd.resize(patchedSize); + // Temporary binary + std::vector tmpDepthaiBinary; + // Main FW + depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_PATH]; + // Patch from main to specified + std::vector& depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH]; + + switch(version) { + case OpenVINO::VERSION_2020_1: + spdlog::warn("OpenVino version 2020.1 is deprecated and will be removed in the next release!"); + depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_1_PATCH_PATH]; + break; + + case OpenVINO::VERSION_2020_2: + spdlog::warn("OpenVino version 2020.2 is deprecated and will be removed in the next release!"); + depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_2_PATCH_PATH]; + break; + + case OpenVINO::VERSION_2020_3: + depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH]; + break; + + case OpenVINO::VERSION_2020_4: + depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_4_PATCH_PATH]; + break; + + case OpenVINO::VERSION_2021_1: + depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_1_PATCH_PATH]; + break; + + case OpenVINO::VERSION_2021_2: + depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH]; + break; + + case OpenVINO::VERSION_2021_3: + depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_PATH]; + break; + } - // Patch - int error = bspatch_mem(depthaiBinary.data(), depthaiBinary.size(), depthaiUsb2Patch.data(), depthaiUsb2Patch.size(), finalCmd.data()); + // is patching required? + if(version != OpenVINO::VERSION_2021_3) { + spdlog::debug("Patching OpenVINO FW version from {} to {}", OpenVINO::getVersionName(OpenVINO::VERSION_2021_3), OpenVINO::getVersionName(version)); - // if patch not successful - if(error > 0) throw std::runtime_error("Error while patching cmd for usb2 mode"); + // Get new size + int64_t patchedSize = bspatch_mem_get_newsize(depthaiPatch.data(), depthaiPatch.size()); - #else + // Reserve space for patched binary + tmpDepthaiBinary.resize(patchedSize); - static_assert("Unsupported currently"); + // Patch + int error = bspatch_mem(depthaiBinary.data(), depthaiBinary.size(), depthaiPatch.data(), depthaiPatch.size(), tmpDepthaiBinary.data()); - #endif + // if patch not successful + if(error > 0) throw std::runtime_error("Error while patching cmd for usb2 mode"); - } else { - return depthaiBinary; - } + // Change depthaiBinary to tmpDepthaiBinary + depthaiBinary = tmpDepthaiBinary; + } #else - // Binaries from default path (TODO) + // Binaries from default path (TODO) #endif + } - return finalCmd; + // Prepend preboot config + auto prebootHeader = createPrebootHeader(nlohmann::json::to_msgpack(config.preboot)); + depthaiBinary.insert(depthaiBinary.begin(), prebootHeader.begin(), prebootHeader.end()); + + // Return created firmware + return depthaiBinary; } #else @@ -343,8 +341,7 @@ Resources::~Resources() { if(lazyThread.joinable()) lazyThread.join(); } -// Get device firmware -std::vector Resources::getDeviceFirmware(bool usb2Mode, OpenVINO::Version version) { +std::vector Resources::getDeviceFirmware(Device::Config config) { // Acquire mutex (this mutex signifies that lazy load is complete) // It is necessary when accessing resourceMap variable std::unique_lock lock(mtx); @@ -352,25 +349,34 @@ std::vector Resources::getDeviceFirmware(bool usb2Mode, OpenVINO:: #ifdef DEPTHAI_RESOURCES_TAR_XZ // Return device firmware - return getDeviceBinary(version, usb2Mode); + return getDeviceBinary(config); #else // Return device firmware - return getEmbeddedDeviceBinary(usb2Mode); + return getEmbeddedDeviceBinary(config); #endif } -std::vector Resources::getBootloaderFirmware() { - // Acquire mutex (this mutex signifies that lazy load is complete) - // It is necessary when accessing resourceMap variable - std::unique_lock lock(mtx); +// Get device firmware +std::vector Resources::getDeviceFirmware(bool usb2Mode, OpenVINO::Version version) { + Device::Config cfg; + if(usb2Mode) { + cfg.preboot.maxUsbSpeed = UsbSpeed::HIGH; + } else { + cfg.preboot.maxUsbSpeed = Device::DEFAULT_USB_SPEED; + } + cfg.version = version; - return getEmbeddedBootloaderBinary(); + return getDeviceFirmware(cfg); } -std::vector getEmbeddedBootloaderBinary() { +std::vector Resources::getBootloaderFirmware(DeviceBootloader::Config config) { + // Convert Config to PrebootConfig + bootloader::PrebootConfig prebootCfg; + prebootCfg.timeoutMs = config.timeout.count(); + // Binaries are resource compiled #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES @@ -379,8 +385,17 @@ std::vector getEmbeddedBootloaderBinary() { // Get binaries from internal sources auto fs = cmrc::depthai::get_filesystem(); + // Load the bootloader FW auto bootloaderBinary = fs.open(CMRC_DEPTHAI_BOOTLOADER_PATH); - return std::vector(bootloaderBinary.begin(), bootloaderBinary.end()); + std::vector bootloaderFw{bootloaderBinary.begin(), bootloaderBinary.end()}; + + // Create prebootCfg header + const uint8_t* pCfg = reinterpret_cast(&prebootCfg); + auto header = createPrebootHeader({pCfg, pCfg + sizeof(prebootCfg)}); + bootloaderFw.insert(bootloaderFw.begin(), header.begin(), header.end()); + + // Return final bootloader fw + return bootloaderFw; #else static_assert(0 && "Unsupported"); @@ -388,4 +403,38 @@ std::vector getEmbeddedBootloaderBinary() { #endif } +std::vector createPrebootHeader(const std::vector& payload) { + constexpr const std::uint8_t HEADER[] = {77, 65, 50, 120, 0x8A, 0x00, 0x00, 0x00, 0x70}; + + // Store the constructed preboot information + std::vector prebootHeader; + + // Store initial header + prebootHeader.insert(prebootHeader.begin(), std::begin(HEADER), std::end(HEADER)); + + // Calculate size + std::size_t totalPayloadSize = payload.size() + sizeof(std::uint32_t); + std::size_t toAddBytes = 4 - (totalPayloadSize % 4); + std::size_t totalSize = totalPayloadSize + toAddBytes; + std::size_t totalSizeWord = totalSize / 4; + + // Write size in words in little endian + prebootHeader.push_back((totalSizeWord >> 0) & 0xFF); + prebootHeader.push_back((totalSizeWord >> 8) & 0xFF); + + // Write payload size (uint32_t LE) + for(int i = 0; i < 4; i++) { + prebootHeader.push_back((payload.size() >> (i * 8)) & 0xFF); + } + + // Copy payload + prebootHeader.insert(prebootHeader.end(), payload.begin(), payload.end()); + // Add missing bytes + for(std::size_t i = 0; i < toAddBytes; i++) { + prebootHeader.push_back(0x00); + } + + return prebootHeader; +} + } // namespace dai diff --git a/src/utility/Resources.hpp b/src/utility/Resources.hpp index a4b841d23..b1e1e3756 100644 --- a/src/utility/Resources.hpp +++ b/src/utility/Resources.hpp @@ -7,11 +7,13 @@ #include #include +// project #include +#include +#include namespace dai { - class Resources { // private constructor Resources(); @@ -21,7 +23,7 @@ class Resources { std::thread lazyThread; std::unordered_map> resourceMap; - std::vector getDeviceBinary(OpenVINO::Version version, bool usb2Mode); + std::vector getDeviceBinary(Device::Config config); public: static Resources& getInstance(); @@ -30,7 +32,8 @@ class Resources { // Available resources std::vector getDeviceFirmware(bool usb2Mode, OpenVINO::Version version = OpenVINO::VERSION_2020_1); - std::vector getBootloaderFirmware(); + std::vector getDeviceFirmware(Device::Config config); + std::vector getBootloaderFirmware(DeviceBootloader::Config config); }; From 00d4dc4526c1fab101aa1d42d0d0186fbceca49e Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Fri, 28 May 2021 13:02:32 +0200 Subject: [PATCH 002/101] Refactored and added preboot config --- CMakeLists.txt | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- cmake/Hunter/config.cmake | 60 ++--- examples/CMakeLists.txt | 162 ++++++++----- examples/src/camera_mobilenet_example.cpp | 131 ----------- .../src/camera_mobilenet_sync_example.cpp | 18 +- examples/src/camera_preview_example.cpp | 63 ----- examples/src/camera_video_example.cpp | 63 ----- examples/src/depth_crop_control.cpp | 110 +++++++++ examples/src/depth_preview.cpp | 68 ++++++ examples/src/device_queue_event.cpp | 50 ++++ examples/src/device_queue_event_example.cpp | 64 ------ examples/src/encoding_max_limit.cpp | 86 +++++++ examples/src/h264_encoding_example.cpp | 70 ------ examples/src/image_manip_example.cpp | 32 ++- examples/src/mjpeg_encoding_example.cpp | 45 ++-- ...mobilenet_device_side_decoding_example.cpp | 124 ---------- examples/src/mono_camera_control.cpp | 149 ++++++++++++ examples/src/mono_camera_example.cpp | 57 ----- examples/src/mono_depth_mobilenetssd.cpp | 153 +++++++++++++ examples/src/mono_full_resolution_saver.cpp | 53 +++++ examples/src/mono_mobilenet.cpp | 116 ++++++++++ examples/src/mono_preview.cpp | 55 +++++ ...tracker_example.cpp => object_tracker.cpp} | 122 +++++----- examples/src/opencv_support.cpp | 52 +++++ examples/src/opencv_support_example.cpp | 56 ----- ...rol_example.cpp => rgb_camera_control.cpp} | 144 ++++++------ ...gned_example.cpp => rgb_depth_aligned.cpp} | 60 ++--- examples/src/rgb_encoding.cpp | 55 +++++ examples/src/rgb_encoding_mobilenet.cpp | 127 ++++++++++ examples/src/rgb_encoding_mono_mobilenet.cpp | 162 +++++++++++++ .../src/rgb_encoding_mono_mobilenet_depth.cpp | 216 ++++++++++++++++++ examples/src/rgb_full_resolution_saver.cpp | 64 ++++++ examples/src/rgb_mobilenet.cpp | 145 ++++++++++++ examples/src/rgb_mobilenet_4k.cpp | 130 +++++++++++ examples/src/rgb_mono_encoding.cpp | 82 +++++++ examples/src/rgb_preview.cpp | 55 +++++ ...p_warp_example.cpp => rgb_rotate_warp.cpp} | 94 ++++---- examples/src/rgb_video.cpp | 45 ++++ ...le.cpp => spatial_location_calculator.cpp} | 74 +++--- ...enet_example.cpp => spatial_mobilenet.cpp} | 164 +++++++------ ...example.cpp => spatial_mobilenet_mono.cpp} | 165 +++++++------ ...example.cpp => spatial_object_tracker.cpp} | 127 +++++----- ...yolo_example.cpp => spatial_tiny_yolo.cpp} | 163 +++++++------ ...reo_example.cpp => stereo_depth_video.cpp} | 72 +++--- ...ion_example.cpp => system_information.cpp} | 71 +++--- ... => tiny_yolo_v3_device_side_decoding.cpp} | 173 +++++++------- ...y_yolo_v3_device_side_decoding_example.cpp | 137 ----------- .../src/tiny_yolo_v4_device_side_decoding.cpp | 159 +++++++++++++ examples/src/utility.cpp | 38 ++- examples/src/utility.hpp | 1 + examples/src/video_mobilenet.cpp | 112 +++++++++ examples/src/webcam_mobilenet_example.cpp | 32 +-- include/depthai/common/CameraBoardSocket.hpp | 25 ++ include/depthai/common/UsbSpeed.hpp | 31 +++ include/depthai/device/Device.hpp | 3 +- include/depthai/device/DeviceBootloader.hpp | 15 +- include/depthai/pipeline/node/MonoCamera.hpp | 11 +- include/depthai/pipeline/node/StereoDepth.hpp | 5 +- shared/depthai-bootloader-shared | 2 +- shared/depthai-shared | 2 +- shared/depthai-shared.cmake | 5 +- src/device/CallbackHandler.cpp | 2 +- src/device/DataQueue.cpp | 2 +- src/device/Device.cpp | 12 +- src/device/DeviceBootloader.cpp | 44 ++-- src/opencv/ImgFrame.cpp | 11 +- src/pipeline/node/MonoCamera.cpp | 2 +- src/utility/Resources.cpp | 49 ++-- src/utility/Resources.hpp | 2 +- tests/src/image_manip_node_test.cpp | 2 +- tests/src/neural_network_test.cpp | 2 +- 72 files changed, 3329 insertions(+), 1728 deletions(-) delete mode 100644 examples/src/camera_mobilenet_example.cpp delete mode 100644 examples/src/camera_preview_example.cpp delete mode 100644 examples/src/camera_video_example.cpp create mode 100644 examples/src/depth_crop_control.cpp create mode 100644 examples/src/depth_preview.cpp create mode 100644 examples/src/device_queue_event.cpp delete mode 100644 examples/src/device_queue_event_example.cpp create mode 100644 examples/src/encoding_max_limit.cpp delete mode 100644 examples/src/h264_encoding_example.cpp delete mode 100644 examples/src/mobilenet_device_side_decoding_example.cpp create mode 100644 examples/src/mono_camera_control.cpp delete mode 100644 examples/src/mono_camera_example.cpp create mode 100644 examples/src/mono_depth_mobilenetssd.cpp create mode 100644 examples/src/mono_full_resolution_saver.cpp create mode 100644 examples/src/mono_mobilenet.cpp create mode 100644 examples/src/mono_preview.cpp rename examples/src/{object_tracker_example.cpp => object_tracker.cpp} (55%) create mode 100644 examples/src/opencv_support.cpp delete mode 100644 examples/src/opencv_support_example.cpp rename examples/src/{color_camera_control_example.cpp => rgb_camera_control.cpp} (57%) rename examples/src/{rgb_depth_aligned_example.cpp => rgb_depth_aligned.cpp} (71%) create mode 100644 examples/src/rgb_encoding.cpp create mode 100644 examples/src/rgb_encoding_mobilenet.cpp create mode 100644 examples/src/rgb_encoding_mono_mobilenet.cpp create mode 100644 examples/src/rgb_encoding_mono_mobilenet_depth.cpp create mode 100644 examples/src/rgb_full_resolution_saver.cpp create mode 100644 examples/src/rgb_mobilenet.cpp create mode 100644 examples/src/rgb_mobilenet_4k.cpp create mode 100644 examples/src/rgb_mono_encoding.cpp create mode 100644 examples/src/rgb_preview.cpp rename examples/src/{image_manip_warp_example.cpp => rgb_rotate_warp.cpp} (72%) create mode 100644 examples/src/rgb_video.cpp rename examples/src/{spatial_location_calculator_example.cpp => spatial_location_calculator.cpp} (79%) rename examples/src/{spatial_mobilenet_example.cpp => spatial_mobilenet.cpp} (61%) rename examples/src/{spatial_mobilenet_mono_example.cpp => spatial_mobilenet_mono.cpp} (60%) rename examples/src/{spatial_object_tracker_example.cpp => spatial_object_tracker.cpp} (67%) rename examples/src/{spatial_tiny_yolo_example.cpp => spatial_tiny_yolo.cpp} (68%) rename examples/src/{stereo_example.cpp => stereo_depth_video.cpp} (70%) rename examples/src/{system_information_example.cpp => system_information.cpp} (65%) rename examples/src/{tiny_yolo_v4_device_side_decoding_example.cpp => tiny_yolo_v3_device_side_decoding.cpp} (51%) delete mode 100644 examples/src/tiny_yolo_v3_device_side_decoding_example.cpp create mode 100644 examples/src/tiny_yolo_v4_device_side_decoding.cpp create mode 100644 examples/src/video_mobilenet.cpp create mode 100644 include/depthai/common/CameraBoardSocket.hpp create mode 100644 include/depthai/common/UsbSpeed.hpp mode change 100755 => 100644 src/opencv/ImgFrame.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ea0ae3be7..669541110 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,7 @@ if(WIN32) endif() # Create depthai project -project(depthai VERSION "2.3.0" LANGUAGES CXX C) +project(depthai VERSION "2.4.0" LANGUAGES CXX C) get_directory_property(has_parent PARENT_DIRECTORY) if(has_parent) set(DEPTHAI_VERSION ${PROJECT_VERSION} PARENT_SCOPE) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 2bb81b1e3..ef84e670f 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 "8641723d7f46be11969c96d5346636cba2f2334b") +set(DEPTHAI_DEVICE_SIDE_COMMIT "67acb9bdda92ca767d26ce5a264740bb72524372") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/cmake/Hunter/config.cmake b/cmake/Hunter/config.cmake index 3b9641829..4aaf3f142 100644 --- a/cmake/Hunter/config.cmake +++ b/cmake/Hunter/config.cmake @@ -8,8 +8,8 @@ hunter_config( hunter_config( XLink VERSION "luxonis-2021.2-develop" - URL "https://github.com/luxonis/XLink/archive/ee361ecba950335390ad539e509f8ab96313b6b4.tar.gz" - SHA1 "72108319bf2289d91157a3933663ed5fb2b6eb18" + URL "https://github.com/luxonis/XLink/archive/48549564792b32cfff9292b3f3cd89710151775e.tar.gz" + SHA1 "c8893d59051da5abf99bc09cf818877d9e4fa88f" ) hunter_config( @@ -34,33 +34,33 @@ hunter_config( URL "https://github.com/luxonis/libarchive/archive/cf2caf0588fc5e2af22cae37027d3ff6902e096f.tar.gz" SHA1 "e99477d32ce14292fe652dc5f4f460d3af8fbc93" CMAKE_ARGS - ENABLE_ACL=OFF - ENABLE_BZip2=OFF - ENABLE_CAT=OFF - ENABLE_CAT_SHARED=OFF - ENABLE_CNG=OFF - ENABLE_COVERAGE=OFF - ENABLE_CPIO=OFF - ENABLE_CPIO_SHARED=OFF - ENABLE_EXPAT=OFF - ENABLE_ICONV=OFF - ENABLE_INSTALL=ON - ENABLE_LIBB2=OFF - ENABLE_LIBXML2=OFF - ENABLE_LZ4=OFF - ENABLE_LZMA=ON - ENABLE_LZO=OFF - ENABLE_LibGCC=OFF - ENABLE_MBEDTLS=OFF - ENABLE_NETTLE=OFF - ENABLE_OPENSSL=OFF - ENABLE_PCREPOSIX=OFF - ENABLE_SAFESEH=AUTO - ENABLE_TAR=OFF - ENABLE_TAR_SHARED=OFF - ENABLE_TEST=OFF - ENABLE_WERROR=OFF - ENABLE_XATTR=OFF - ENABLE_ZLIB=OFF + ENABLE_ACL=OFF + ENABLE_BZip2=OFF + ENABLE_CAT=OFF + ENABLE_CAT_SHARED=OFF + ENABLE_CNG=OFF + ENABLE_COVERAGE=OFF + ENABLE_CPIO=OFF + ENABLE_CPIO_SHARED=OFF + ENABLE_EXPAT=OFF + ENABLE_ICONV=OFF + ENABLE_INSTALL=ON + ENABLE_LIBB2=OFF + ENABLE_LIBXML2=OFF + ENABLE_LZ4=OFF + ENABLE_LZMA=ON + ENABLE_LZO=OFF + ENABLE_LibGCC=OFF + ENABLE_MBEDTLS=OFF + ENABLE_NETTLE=OFF + ENABLE_OPENSSL=OFF + ENABLE_PCREPOSIX=OFF + ENABLE_SAFESEH=AUTO + ENABLE_TAR=OFF + ENABLE_TAR_SHARED=OFF + ENABLE_TEST=OFF + ENABLE_WERROR=OFF + ENABLE_XATTR=OFF + ENABLE_ZLIB=OFF ENABLE_ZSTD=OFF ) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index aa63e340d..d20fb20d1 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -56,17 +56,6 @@ endmacro() # Hunter test data download -## message(STATUS "Location of test1.data: ${test1_data}") - -# Color camera preview output example -dai_add_example(camera_preview src/camera_preview_example.cpp) - -# Color camera video output example -dai_add_example(camera_video src/camera_video_example.cpp) - -# Mono camera video output example -dai_add_example(mono_camera src/mono_camera_example.cpp) - # NeuralNetwork node, mobilenet example hunter_private_data( URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/mobilenet-ssd_openvino_2021.2_6shave.blob" @@ -91,6 +80,14 @@ hunter_private_data( LOCATION tiny_yolo_v4_blob ) +# NeuralNetwork node, mobilenet example, 5 shaves +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/mobilenet-ssd_openvino_2021.2_5shave.blob" + SHA1 "d715f85e474609cf3f696d7a2e3750804ed6c726" + FILE "mobilenet-ssd_openvino_2021.2_5shave.blob" + LOCATION mobilenet_5shaves_blob +) + # NeuralNetwork node, mobilenet example, 8 shaves hunter_private_data( URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/mobilenet-ssd_openvino_2021.2_8shave.blob" @@ -99,80 +96,133 @@ hunter_private_data( LOCATION mobilenet_8shaves_blob ) -dai_add_example(camera_mobilenet src/camera_mobilenet_example.cpp) -target_compile_definitions(camera_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") +# Video file with objects to detect +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/construction_vest.mp4" + SHA1 "271d8d0b702e683ce02957db7c100843de5ceaec" + FILE "construction_vest.mp4" + LOCATION construction_vest +) -# NeuralNetwork node, webcam input (from host) -dai_add_example(webcam_mobilenet src/webcam_mobilenet_example.cpp) -target_compile_definitions(webcam_mobilenet PRIVATE BLOB_PATH="${mobilenet_8shaves_blob}") +## message(STATUS "Location of test1.data: ${test1_data}") -# MJPEG encoding -dai_add_example(mjpeg_encoding src/mjpeg_encoding_example.cpp) +# RGB camera preview output example +dai_add_example(rgb_preview src/rgb_preview.cpp) -# h264 encoding -dai_add_example(h264_encoding src/h264_encoding_example.cpp) +# Mono camera preview output example +dai_add_example(mono_preview src/mono_preview.cpp) -# StereoDepth example -dai_add_example(stereo src/stereo_example.cpp) +# Depth preview output example +dai_add_example(depth_preview src/depth_preview.cpp) -# Image Manip node examples -dai_add_example(image_manip src/image_manip_example.cpp) -dai_add_example(image_manip_warp src/image_manip_warp_example.cpp) +# rgb encoding +dai_add_example(rgb_encoding src/rgb_encoding.cpp) -# Color Camera config example -dai_add_example(color_camera_control src/color_camera_control_example.cpp) +# rgb+mono encoding +dai_add_example(rgb_mono_encoding src/rgb_mono_encoding.cpp) -# System information example -dai_add_example(system_information src/system_information_example.cpp) +dai_add_example(rgb_full_resolution_saver src/rgb_full_resolution_saver.cpp) -# Device getQueueEvent example -dai_add_example(device_queue_event src/device_queue_event_example.cpp) +dai_add_example(mono_full_resolution_saver src/mono_full_resolution_saver.cpp) -# OpenCV support example -dai_add_example(opencv_support src/opencv_support_example.cpp) +dai_add_example(rgb_mobilenet src/rgb_mobilenet.cpp) +target_compile_definitions(rgb_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") -# RGB-depth alignment example -dai_add_example(rgb_depth_aligned src/rgb_depth_aligned_example.cpp) +dai_add_example(mono_mobilenet src/mono_mobilenet.cpp) +target_compile_definitions(mono_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") -# Device side decoding example for mobilenet-ssd -dai_add_example(mobilenet_device_side_decoding src/mobilenet_device_side_decoding_example.cpp) -target_compile_definitions(mobilenet_device_side_decoding PRIVATE BLOB_PATH="${mobilenet_blob}") +dai_add_example(mono_depth_mobilenetssd src/mono_depth_mobilenetssd.cpp) +target_compile_definitions(mono_depth_mobilenetssd PRIVATE BLOB_PATH="${mobilenet_blob}") -# Device side decoding example for mobilenet-ssd with 3d coordinates on RGB camera -dai_add_example(spatial_mobilenet src/spatial_mobilenet_example.cpp) -target_compile_definitions(spatial_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") +dai_add_example(rgb_encoding_mono_mobilenet src/rgb_encoding_mono_mobilenet.cpp) +target_compile_definitions(rgb_encoding_mono_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") -# Device side decoding example for mobilenet-ssd with 3d coordinates on right camera -dai_add_example(spatial_mobilenet_mono src/spatial_mobilenet_mono_example.cpp) -target_compile_definitions(spatial_mobilenet_mono PRIVATE BLOB_PATH="${mobilenet_blob}") +dai_add_example(rgb_encoding_mono_mobilenet_depth src/rgb_encoding_mono_mobilenet_depth.cpp) +target_compile_definitions(rgb_encoding_mono_mobilenet_depth PRIVATE BLOB_PATH="${mobilenet_blob}") + +dai_add_example(encoding_max_limit src/encoding_max_limit.cpp) + +# RGB Camera config example +dai_add_example(rgb_camera_control src/rgb_camera_control.cpp) + +dai_add_example(mono_camera_control src/mono_camera_control.cpp) + +dai_add_example(depth_crop_control src/depth_crop_control.cpp) + +dai_add_example(rgb_mobilenet_4k src/rgb_mobilenet_4k.cpp) +target_compile_definitions(rgb_mobilenet_4k PRIVATE BLOB_PATH="${mobilenet_5shaves_blob}") + +# Device getQueueEvent example +dai_add_example(device_queue_event src/device_queue_event.cpp) + +dai_add_example(video_mobilenet src/video_mobilenet.cpp) +target_compile_definitions(video_mobilenet PRIVATE BLOB_PATH="${mobilenet_8shaves_blob}" VIDEO_PATH="${construction_vest}") + +dai_add_example(rgb_encoding_mobilenet src/rgb_encoding_mobilenet.cpp) +target_compile_definitions(rgb_encoding_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") + +# Image Manip node examples +dai_add_example(image_manip src/image_manip_example.cpp) + +# Imagae manip node exapmle with warping +dai_add_example(rgb_rotate_warp src/rgb_rotate_warp.cpp) # Device side decoding example for tiny-yolo-v3 -dai_add_example(tiny_yolo_v3_device_side_decoding src/tiny_yolo_v3_device_side_decoding_example.cpp) +dai_add_example(tiny_yolo_v3_device_side_decoding src/tiny_yolo_v3_device_side_decoding.cpp) target_compile_definitions(tiny_yolo_v3_device_side_decoding PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}") # Device side decoding example for tiny-yolo-v4 -dai_add_example(tiny_yolo_v4_device_side_decoding src/tiny_yolo_v4_device_side_decoding_example.cpp) +dai_add_example(tiny_yolo_v4_device_side_decoding src/tiny_yolo_v4_device_side_decoding.cpp) target_compile_definitions(tiny_yolo_v4_device_side_decoding PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}") -# Sync example between NN and camera frames -dai_add_example(camera_mobilenet_sync src/camera_mobilenet_sync_example.cpp) -target_compile_definitions(camera_mobilenet_sync PRIVATE BLOB_PATH="${mobilenet_blob}") +# OpenCV support example +dai_add_example(opencv_support src/opencv_support.cpp) + +# System information example +dai_add_example(system_information src/system_information.cpp) -dai_add_example(spatial_location_calculator src/spatial_location_calculator_example.cpp) +# Device side decoding example for mobilenet-ssd with 3d coordinates on RGB camera +dai_add_example(spatial_mobilenet src/spatial_mobilenet.cpp) +target_compile_definitions(spatial_mobilenet PRIVATE BLOB_PATH="${mobilenet_blob}") + +# Device side decoding example for mobilenet-ssd with 3d coordinates on right camera +dai_add_example(spatial_mobilenet_mono src/spatial_mobilenet_mono.cpp) +target_compile_definitions(spatial_mobilenet_mono PRIVATE BLOB_PATH="${mobilenet_blob}") # Device side decoding example for tiny-yolo-v3 with 3d coordinates on RGB camera -dai_add_example(spatial_tiny_yolo_v3 src/spatial_tiny_yolo_example.cpp) +dai_add_example(spatial_tiny_yolo_v3 src/spatial_tiny_yolo.cpp) target_compile_definitions(spatial_tiny_yolo_v3 PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}") # Device side decoding example for tiny-yolo-v4 with 3d coordinates on RGB camera -dai_add_example(spatial_tiny_yolo_v4 src/spatial_tiny_yolo_example.cpp) +dai_add_example(spatial_tiny_yolo_v4 src/spatial_tiny_yolo.cpp) target_compile_definitions(spatial_tiny_yolo_v4 PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}") +dai_add_example(spatial_location_calculator src/spatial_location_calculator.cpp) + +# RGB camera video output example +dai_add_example(rgb_video src/rgb_video.cpp) + # Object tracker example on mobilenet SSD output -dai_add_example(object_tracker_example src/object_tracker_example.cpp) -target_compile_definitions(object_tracker_example PRIVATE BLOB_PATH="${mobilenet_blob}") +dai_add_example(object_tracker src/object_tracker.cpp) +target_compile_definitions(object_tracker PRIVATE BLOB_PATH="${mobilenet_blob}") # Spatial Object tracker example on mobilenet SSD output -dai_add_example(spatial_object_tracker_example src/spatial_object_tracker_example.cpp) -target_compile_definitions(spatial_object_tracker_example PRIVATE BLOB_PATH="${mobilenet_blob}") +dai_add_example(spatial_object_tracker src/spatial_object_tracker.cpp) +target_compile_definitions(spatial_object_tracker PRIVATE BLOB_PATH="${mobilenet_blob}") + +# Stereo Depth example +dai_add_example(stereo_depth_video src/stereo_depth_video.cpp) + +# NeuralNetwork node, webcam input (from host) +dai_add_example(webcam_mobilenet src/webcam_mobilenet_example.cpp) +target_compile_definitions(webcam_mobilenet PRIVATE BLOB_PATH="${mobilenet_8shaves_blob}") +# MJPEG encoding +dai_add_example(mjpeg_encoding src/mjpeg_encoding_example.cpp) + +# RGB-depth alignment example +dai_add_example(rgb_depth_aligned src/rgb_depth_aligned.cpp) + +# Sync example between NN and camera frames +dai_add_example(camera_mobilenet_sync src/camera_mobilenet_sync_example.cpp) +target_compile_definitions(camera_mobilenet_sync PRIVATE BLOB_PATH="${mobilenet_blob}") diff --git a/examples/src/camera_mobilenet_example.cpp b/examples/src/camera_mobilenet_example.cpp deleted file mode 100644 index 5f094058d..000000000 --- a/examples/src/camera_mobilenet_example.cpp +++ /dev/null @@ -1,131 +0,0 @@ - -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -static bool syncNN = true; - -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; - - auto colorCam = p.create(); - auto xlinkOut = p.create(); - auto nn1 = p.create(); - auto nnOut = p.create(); - - nn1->setBlobPath(nnPath); - - xlinkOut->setStreamName("preview"); - nnOut->setStreamName("detections"); - - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); - - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(nn1->input); - if(syncNN) { - nn1->passthrough.link(xlinkOut->input); - } else { - colorCam->preview.link(xlinkOut->input); - } - - nn1->out.link(nnOut->input); - - return p; -} - -int main(int argc, char** argv) { - using namespace std; - - // Default blob path provided by Hunter private data download - // Applicable for easier example usage only - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); - - cv::Mat frame; - auto preview = d.getOutputQueue("preview"); - auto detections = d.getOutputQueue("detections"); - - while(1) { - auto imgFrame = preview->get(); - if(imgFrame) { - printf("Frame - w: %d, h: %d\n", imgFrame->getWidth(), imgFrame->getHeight()); - frame = toMat(imgFrame->getData(), imgFrame->getWidth(), imgFrame->getHeight(), 3, 1); - } - - struct Detection { - unsigned int label; - float score; - float x_min; - float y_min; - float x_max; - float y_max; - }; - - vector dets; - - auto det = detections->get(); - std::vector detData = det->getFirstLayerFp16(); - if(detData.size() > 0) { - int i = 0; - while(detData[i * 7] != -1.0f) { - Detection d; - d.label = detData[i * 7 + 1]; - d.score = detData[i * 7 + 2]; - d.x_min = detData[i * 7 + 3]; - d.y_min = detData[i * 7 + 4]; - d.x_max = detData[i * 7 + 5]; - d.y_max = detData[i * 7 + 6]; - i++; - dets.push_back(d); - } - } - - for(const auto& d : dets) { - int x1 = d.x_min * frame.cols; - int y1 = d.y_min * frame.rows; - int x2 = d.x_max * frame.cols; - int y2 = d.y_max * frame.rows; - - cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), cv::Scalar(255, 255, 255)); - } - - printf("===================== %lu detection(s) =======================\n", dets.size()); - for(unsigned det = 0; det < dets.size(); ++det) { - printf("%5d | %6.4f | %7.4f | %7.4f | %7.4f | %7.4f\n", - dets[det].label, - dets[det].score, - dets[det].x_min, - dets[det].y_min, - dets[det].x_max, - dets[det].y_max); - } - - cv::imshow("preview", frame); - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } - - return 0; -} diff --git a/examples/src/camera_mobilenet_sync_example.cpp b/examples/src/camera_mobilenet_sync_example.cpp index e09d5fbe8..e6220f0a8 100644 --- a/examples/src/camera_mobilenet_sync_example.cpp +++ b/examples/src/camera_mobilenet_sync_example.cpp @@ -21,9 +21,11 @@ int main(int argc, char** argv) { nnPath = std::string(argv[1]); } + // Create pipeline dai::Pipeline pipeline; - auto colorCam = pipeline.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); auto nn = pipeline.create(); auto camOut = pipeline.create(); auto passthroughMeta = pipeline.create(); @@ -35,11 +37,11 @@ int main(int argc, char** argv) { resultOut->setStreamName("resultOut"); // ColorCamera options - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); - colorCam->setFps(CAMERA_FPS); + camRgb->setPreviewSize(300, 300); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + camRgb->setFps(CAMERA_FPS); // NN input options nn->input.setBlocking(false); @@ -49,8 +51,8 @@ int main(int argc, char** argv) { nn->setNumInferenceThreads(2); // Link nodes CAM -> XLINK - colorCam->preview.link(nn->input); - colorCam->preview.link(camOut->input); + camRgb->preview.link(nn->input); + camRgb->preview.link(camOut->input); nn->passthrough.link(passthroughMeta->input); nn->out.link(resultOut->input); diff --git a/examples/src/camera_preview_example.cpp b/examples/src/camera_preview_example.cpp deleted file mode 100644 index 82b7c99c7..000000000 --- a/examples/src/camera_preview_example.cpp +++ /dev/null @@ -1,63 +0,0 @@ - -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -dai::Pipeline createCameraPipeline() { - dai::Pipeline p; - - auto colorCam = p.create(); - auto xlinkOut = p.create(); - xlinkOut->setStreamName("preview"); - - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(true); - - // Link plugins CAM -> XLINK - colorCam->preview.link(xlinkOut->input); - - return p; -} - -int main() { - using namespace std; - - dai::Pipeline p = createCameraPipeline(); - - // Start the pipeline - dai::Device d; - - cout << "Connected cameras: "; - for(const auto& cam : d.getConnectedCameras()) { - cout << static_cast(cam) << " "; - } - cout << endl; - - // Start the pipeline - d.startPipeline(p); - - cv::Mat frame; - auto preview = d.getOutputQueue("preview"); - - while(1) { - auto imgFrame = preview->get(); - if(imgFrame) { - printf("Frame - w: %d, h: %d\n", imgFrame->getWidth(), imgFrame->getHeight()); - frame = cv::Mat(imgFrame->getHeight(), imgFrame->getWidth(), CV_8UC3, imgFrame->getData().data()); - cv::imshow("preview", frame); - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } else { - std::cout << "Not ImgFrame" << std::endl; - } - } - - return 0; -} diff --git a/examples/src/camera_video_example.cpp b/examples/src/camera_video_example.cpp deleted file mode 100644 index c610446b9..000000000 --- a/examples/src/camera_video_example.cpp +++ /dev/null @@ -1,63 +0,0 @@ - -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -dai::Pipeline createCameraFullPipeline() { - dai::Pipeline p; - - auto colorCam = p.create(); - auto xlinkOut = p.create(); - xlinkOut->setStreamName("video"); - - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(true); - - // Link plugins CAM -> XLINK - colorCam->video.link(xlinkOut->input); - - return p; -} - -int main() { - using namespace std; - using namespace std::chrono; - - // Create pipeline - dai::Pipeline p = createCameraFullPipeline(); - - // Connect and start the pipeline - dai::Device d(p); - - cv::Mat frame; - auto video = d.getOutputQueue("video"); - - while(1) { - auto imgFrame = video->get(); - if(imgFrame) { - auto dur = steady_clock::now() - imgFrame->getTimestamp(); - - printf("Frame - w: %d, h: %d, latency: %ldms\n", imgFrame->getWidth(), imgFrame->getHeight(), duration_cast(dur).count()); - - frame = cv::Mat(imgFrame->getHeight() * 3 / 2, imgFrame->getWidth(), CV_8UC1, imgFrame->getData().data()); - - cv::Mat rgb(imgFrame->getHeight(), imgFrame->getWidth(), CV_8UC3); - - cv::cvtColor(frame, rgb, cv::COLOR_YUV2BGR_NV12); - - cv::imshow("video", rgb); - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } else { - std::cout << "Not ImgFrame" << std::endl; - } - } - return 0; -} \ No newline at end of file diff --git a/examples/src/depth_crop_control.cpp b/examples/src/depth_crop_control.cpp new file mode 100644 index 000000000..4265bffbd --- /dev/null +++ b/examples/src/depth_crop_control.cpp @@ -0,0 +1,110 @@ +/** + * This example shows usage of depth camera in crop mode with the possibility to move the crop. + * Use 'WASD' in order to do it. + */ +#include + +// Includes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Step size ('W','A','S','D' controls) +static constexpr float stepSize = 0.02; + +static std::atomic sendCamConfig{false}; + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoRight = pipeline.create(); + auto monoLeft = pipeline.create(); + auto manip = pipeline.create(); + auto stereo = pipeline.create(); + + auto configIn = pipeline.create(); + auto xout = pipeline.create(); + + configIn->setStreamName("config"); + xout->setStreamName("depth"); + + // Crop range + dai::Point2f topLeft(0.2, 0.2); + dai::Point2f bottomRight(0.8, 0.8); + + // Properties + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + + manip->initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y); + manip->setMaxOutputFrameSize(monoRight->getResolutionHeight() * monoRight->getResolutionWidth() * 3); + stereo->setConfidenceThreshold(200); + + // Linking + configIn->out.link(manip->inputConfig); + stereo->depth.link(manip->inputImage); + manip->out.link(xout->input); + monoRight->out.link(stereo->left); + monoLeft->out.link(stereo->right); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Queues + auto q = device.getOutputQueue(xout->getStreamName(), 4, false); + auto configQueue = device.getInputQueue(configIn->getStreamName()); + + while(true) { + auto inDepth = q->get(); + cv::Mat depthFrame = inDepth->getFrame(); + // Frame is transformed, the color map will be applied to highlight the depth info + 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); + + // Frame is ready to be shown + cv::imshow("depth", depthFrameColor); + + // Update screen (10ms pooling rate) + int key = cv::waitKey(10); + if(key == 'q') { + break; + } else if(key == 'w') { + if(topLeft.y - stepSize >= 0) { + topLeft.y -= stepSize; + bottomRight.y -= stepSize; + sendCamConfig = true; + } + } else if(key == 'a') { + if(topLeft.x - stepSize >= 0) { + topLeft.x -= stepSize; + bottomRight.x -= stepSize; + sendCamConfig = true; + } + } else if(key == 's') { + if(bottomRight.y + stepSize <= 1) { + topLeft.y += stepSize; + bottomRight.y += stepSize; + sendCamConfig = true; + } + } else if(key == 'd') { + if(bottomRight.x + stepSize <= 1) { + topLeft.x += stepSize; + bottomRight.x += stepSize; + sendCamConfig = true; + } + } + + // Send new config to camera + if(sendCamConfig) { + dai::ImageManipConfig cfg; + cfg.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y); + configQueue->send(cfg); + sendCamConfig = false; + } + } + return 0; +} diff --git a/examples/src/depth_preview.cpp b/examples/src/depth_preview.cpp new file mode 100644 index 000000000..12054b080 --- /dev/null +++ b/examples/src/depth_preview.cpp @@ -0,0 +1,68 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Closer-in minimum depth, disparity range is doubled (from 95 to 190): +static std::atomic extended_disparity{false}; +// Better accuracy for longer distance, fractional disparity 32-levels: +static std::atomic subpixel{false}; +// Better handling for occlusions: +static std::atomic lr_check{false}; + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto depth = pipeline.create(); + auto xout = pipeline.create(); + + xout->setStreamName("disparity"); + + // Properties + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + + // Create a node that will produce the depth map (using disparity output as it's easier to visualize depth this way) + depth->setConfidenceThreshold(200); + // Options: MEDIAN_OFF, KERNEL_3x3, KERNEL_5x5, KERNEL_7x7 (default) + depth->setMedianFilter(dai::StereoDepthProperties::MedianFilter::KERNEL_7x7); + depth->setLeftRightCheck(lr_check); + depth->setExtendedDisparity(extended_disparity); + depth->setSubpixel(subpixel); + + // Linking + monoLeft->out.link(depth->left); + monoRight->out.link(depth->right); + depth->disparity.link(xout->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queue will be used to get the disparity frames from the outputs defined above + auto q = device.getOutputQueue("disparity", 4, false); + + while(true) { + auto inDepth = q->get(); + auto frame = inDepth->getFrame(); + // Normalization for better visualization + frame.convertTo(frame, CV_8UC1, 255 / depth->getMaxDisparity()); + + cv::imshow("disparity", frame); + + // Available color maps: https://docs.opencv.org/3.4/d3/d50/group__imgproc__colormap.html + cv::applyColorMap(frame, frame, cv::COLORMAP_JET); + cv::imshow("disparity_color", frame); + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/device_queue_event.cpp b/examples/src/device_queue_event.cpp new file mode 100644 index 000000000..056cb54cb --- /dev/null +++ b/examples/src/device_queue_event.cpp @@ -0,0 +1,50 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto camMono = pipeline.create(); + auto xoutRgb = pipeline.create(); + auto xoutMono = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + xoutMono->setStreamName("mono"); + + // Properties + camRgb->setInterleaved(true); + camRgb->setPreviewSize(300, 300); + + // Linking + camRgb->preview.link(xoutRgb->input); + camMono->out.link(xoutMono->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Clear queue events + device.getQueueEvents(); + + while(true) { + auto ev = device.getQueueEvent(); + + if(ev == "rgb") { + auto rgb = device.getOutputQueue(ev)->get(); + cv::imshow("rgb", rgb->getFrame()); + } else if(ev == "mono") { + auto mono = device.getOutputQueue(ev)->get(); + cv::imshow("mono", mono->getFrame()); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/device_queue_event_example.cpp b/examples/src/device_queue_event_example.cpp deleted file mode 100644 index 4bfaf70b2..000000000 --- a/examples/src/device_queue_event_example.cpp +++ /dev/null @@ -1,64 +0,0 @@ - -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - - dai::Pipeline p; - - auto colorCam = p.create(); - auto xout = p.create(); - auto xout2 = p.create(); - auto videnc = p.create(); - - // XLinkOut - xout->setStreamName("mjpeg"); - xout2->setStreamName("preview"); - - // ColorCamera - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - // colorCam->setFps(5.0); - colorCam->setInterleaved(true); - - // VideoEncoder - videnc->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::MJPEG); - - // Link plugins CAM -> XLINK - colorCam->video.link(videnc->input); - colorCam->preview.link(xout2->input); - videnc->bitstream.link(xout->input); - - // Connect and start the pipeline - dai::Device d(p); - - // Sets queues size and behaviour - d.getOutputQueue("mjpeg", 8, false); - d.getOutputQueue("preview", 8, false); - - while(1) { - auto ev = d.getQueueEvent(); - if(ev == "preview") { - auto preview = d.getOutputQueue(ev)->get(); - cv::imshow("preview", cv::Mat(preview->getHeight(), preview->getWidth(), CV_8UC3, preview->getData().data())); - } else if(ev == "mjpeg") { - auto mjpeg = d.getOutputQueue(ev)->get(); - cv::Mat decodedFrame = cv::imdecode(cv::Mat(mjpeg->getData()), cv::IMREAD_COLOR); - cv::imshow("mjpeg", decodedFrame); - } - - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } - - return 0; -} diff --git a/examples/src/encoding_max_limit.cpp b/examples/src/encoding_max_limit.cpp new file mode 100644 index 000000000..8d29a5392 --- /dev/null +++ b/examples/src/encoding_max_limit.cpp @@ -0,0 +1,86 @@ +#include +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Keyboard interrupt (Ctrl + C) detected +static std::atomic alive{true}; +static void sigintHandler(int signum) { + alive = false; +} + +int main() { + using namespace std; + using namespace std::chrono; + std::signal(SIGINT, &sigintHandler); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto ve1 = pipeline.create(); + auto ve2 = pipeline.create(); + auto ve3 = pipeline.create(); + + auto ve1Out = pipeline.create(); + auto ve2Out = pipeline.create(); + auto ve3Out = pipeline.create(); + + ve1Out->setStreamName("ve1Out"); + ve2Out->setStreamName("ve2Out"); + ve3Out->setStreamName("ve3Out"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_4_K); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + + // Setting to 26fps will trigger error + ve1->setDefaultProfilePreset(1280, 720, 25, dai::VideoEncoderProperties::Profile::H264_MAIN); + ve2->setDefaultProfilePreset(3840, 2160, 25, dai::VideoEncoderProperties::Profile::H265_MAIN); + ve3->setDefaultProfilePreset(1280, 720, 25, dai::VideoEncoderProperties::Profile::H264_MAIN); + + // Linking + monoLeft->out.link(ve1->input); + camRgb->video.link(ve2->input); + monoRight->out.link(ve3->input); + + ve1->bitstream.link(ve1Out->input); + ve2->bitstream.link(ve2Out->input); + ve3->bitstream.link(ve3Out->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the encoded data from the output defined above + auto outQ1 = device.getOutputQueue("ve1Out", 30, true); + auto outQ2 = device.getOutputQueue("ve2Out", 30, true); + auto outQ3 = device.getOutputQueue("ve3Out", 30, true); + + // The .h264 / .h265 files are raw stream files (not playable yet) + auto videoFile1 = ofstream("mono1.h264", ios::binary); + auto videoFile2 = ofstream("color.h265", ios::binary); + auto videoFile3 = ofstream("mono2.h264", ios::binary); + cout << "Press Ctrl+C to stop encoding..." << endl; + + while(alive) { + auto out1 = outQ1->get(); + videoFile1.write((char*)out1->getData().data(), out1->getData().size()); + auto out2 = outQ2->get(); + videoFile2.write((char*)out2->getData().data(), out2->getData().size()); + auto out3 = outQ3->get(); + videoFile3.write((char*)out3->getData().data(), out3->getData().size()); + } + + cout << "To view the encoded data, convert the stream file (.h264/.h265) into a video file (.mp4), using a command below:" << endl; + cout << "ffmpeg -framerate 25 -i mono1.h264 -c copy mono1.mp4" << endl; + cout << "ffmpeg -framerate 25 -i mono2.h264 -c copy mono2.mp4" << endl; + cout << "ffmpeg -framerate 25 -i color.h265 -c copy color.mp4" << endl; + + return 0; +} diff --git a/examples/src/h264_encoding_example.cpp b/examples/src/h264_encoding_example.cpp deleted file mode 100644 index ee1659226..000000000 --- a/examples/src/h264_encoding_example.cpp +++ /dev/null @@ -1,70 +0,0 @@ - -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - - std::string h264Path("video.h264"); - - // If path specified, use that - if(argc > 1) { - h264Path = std::string(argv[1]); - } - - dai::Pipeline p; - - auto colorCam = p.create(); - auto xout = p.create(); - auto xout2 = p.create(); - auto videnc = p.create(); - - // XLinkOut - xout->setStreamName("h264"); - xout2->setStreamName("preview"); - - // ColorCamera - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - // colorCam->setFps(5.0); - colorCam->setInterleaved(true); - - // VideoEncoder - videnc->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::H264_MAIN); - - // Link plugins CAM -> XLINK - colorCam->video.link(videnc->input); - colorCam->preview.link(xout2->input); - videnc->bitstream.link(xout->input); - - // Connect and start the pipeline - dai::Device d(p); - - auto myfile = std::fstream(h264Path, std::ios::out | std::ios::binary); - - auto h264Queue = d.getOutputQueue("h264", 8, false); - auto previewQueue = d.getOutputQueue("preview", 8, false); - while(1) { - auto preview = previewQueue->get(); - cv::imshow("preview", cv::Mat(preview->getHeight(), preview->getWidth(), CV_8UC3, preview->getData().data())); - auto h264 = h264Queue->get(); - myfile.write((char*)h264->getData().data(), h264->getData().size()); - - int key = cv::waitKey(1); - if(key == 'q') { - break; - } - } - myfile.close(); - - std::cout << "To view the encoded data, convert the stream file " << h264Path << " into a video file (.mp4) using a command below:" << std::endl; - std::cout << "ffmpeg -framerate " << colorCam->getFps() << " -i " << h264Path << " -c copy video.mp4" << std::endl; - - return 0; -} diff --git a/examples/src/image_manip_example.cpp b/examples/src/image_manip_example.cpp index a3707b7b0..c6278529a 100644 --- a/examples/src/image_manip_example.cpp +++ b/examples/src/image_manip_example.cpp @@ -1,6 +1,3 @@ - - -#include #include #include "utility.hpp" @@ -9,9 +6,11 @@ #include "depthai/depthai.hpp" int main() { + // Create pipeline dai::Pipeline pipeline; - auto colorCam = pipeline.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); auto imageManip = pipeline.create(); auto imageManip2 = pipeline.create(); auto camOut = pipeline.create(); @@ -24,10 +23,11 @@ int main() { manipOut2->setStreamName("manip2"); manip2In->setStreamName("manip2In"); - colorCam->setPreviewSize(304, 304); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + // Properties + camRgb->setPreviewSize(304, 304); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); // Create a center crop image manipulation imageManip->initialConfig.setCenterCrop(0.7f); @@ -37,20 +37,12 @@ int main() { imageManip2->initialConfig.setCropRect(0.1, 0.1, 0.3, 0.3); imageManip2->setWaitForConfigInput(true); - // Link nodes CAM -> XLINK - colorCam->preview.link(camOut->input); - - // Link nodes CAM -> imageManip -> XLINK - colorCam->preview.link(imageManip->inputImage); + // Linking + camRgb->preview.link(camOut->input); + camRgb->preview.link(imageManip->inputImage); imageManip->out.link(manipOut->input); - - // ImageManip -> ImageManip 2 imageManip->out.link(imageManip2->inputImage); - - // ImageManip2 -> XLinkOut imageManip2->out.link(manipOut2->input); - - // Host config -> image manip 2 manip2In->out.link(imageManip2->inputConfig); // Connect to device and start pipeline @@ -66,6 +58,7 @@ int main() { int frameCounter = 0; float xmin = 0.1f; float xmax = 0.3f; + while(true) { xmin += 0.003f; xmax += 0.003f; @@ -99,4 +92,5 @@ int main() { frameCounter++; } + return 0; } diff --git a/examples/src/mjpeg_encoding_example.cpp b/examples/src/mjpeg_encoding_example.cpp index 075978fee..8e1daacc1 100644 --- a/examples/src/mjpeg_encoding_example.cpp +++ b/examples/src/mjpeg_encoding_example.cpp @@ -1,47 +1,43 @@ - -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" -int main(int argc, char** argv) { - using namespace std; +int main() { using namespace std::chrono; - dai::Pipeline p; + // Create pipeline + dai::Pipeline pipeline; - auto colorCam = p.create(); - auto xout = p.create(); - auto xout2 = p.create(); - auto videnc = p.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); + auto xout = pipeline.create(); + auto xout2 = pipeline.create(); + auto videnc = pipeline.create(); - // XLinkOut xout->setStreamName("mjpeg"); xout2->setStreamName("preview"); // ColorCamera - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - // colorCam->setFps(5.0); - colorCam->setInterleaved(true); + camRgb->setPreviewSize(300, 300); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + // camRgb->setFps(5.0); + camRgb->setInterleaved(true); // VideoEncoder videnc->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::MJPEG); - // Link plugins CAM -> XLINK - colorCam->video.link(videnc->input); - colorCam->preview.link(xout2->input); + // Linking + camRgb->video.link(videnc->input); + camRgb->preview.link(xout2->input); videnc->bitstream.link(xout->input); - // Connect and start the pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); - auto mjpegQueue = d.getOutputQueue("mjpeg", 8, false); - auto previewQueue = d.getOutputQueue("preview", 8, false); - while(1) { + auto mjpegQueue = device.getOutputQueue("mjpeg", 8, false); + auto previewQueue = device.getOutputQueue("preview", 8, false); + while(true) { auto t1 = steady_clock::now(); auto preview = previewQueue->get(); auto t2 = steady_clock::now(); @@ -69,6 +65,5 @@ int main(int argc, char** argv) { return 0; } } - return 0; } diff --git a/examples/src/mobilenet_device_side_decoding_example.cpp b/examples/src/mobilenet_device_side_decoding_example.cpp deleted file mode 100644 index 29347768f..000000000 --- a/examples/src/mobilenet_device_side_decoding_example.cpp +++ /dev/null @@ -1,124 +0,0 @@ -#include -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", - "car", "cat", "chair", "cow", "diningtable", "dog", "horse", - "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; - -static bool syncNN = true; - -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; - - auto colorCam = p.create(); - auto xlinkOut = p.create(); - auto detectionNetwork = p.create(); - auto nnOut = p.create(); - - xlinkOut->setStreamName("preview"); - nnOut->setStreamName("detections"); - - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); - colorCam->setFps(40); - - // testing MobileNet DetectionNetwork - detectionNetwork->setConfidenceThreshold(0.5f); - detectionNetwork->setBlobPath(nnPath); - - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(detectionNetwork->input); - if(syncNN) { - detectionNetwork->passthrough.link(xlinkOut->input); - } else { - colorCam->preview.link(xlinkOut->input); - } - - detectionNetwork->out.link(nnOut->input); - - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); - - auto preview = d.getOutputQueue("preview", 4, false); - auto detections = d.getOutputQueue("detections", 4, false); - - auto startTime = steady_clock::now(); - int counter = 0; - float fps = 0; - while(1) { - auto imgFrame = preview->get(); - auto det = detections->get(); - - counter++; - auto currentTime = steady_clock::now(); - auto elapsed = duration_cast>(currentTime - startTime); - if(elapsed > seconds(1)) { - fps = counter / elapsed.count(); - counter = 0; - startTime = currentTime; - } - - cv::Mat frame = imgFrame->getCvFrame(); - - auto color = cv::Scalar(255, 255, 255); - auto dets = det->detections; - for(const auto& d : dets) { - int x1 = d.xmin * frame.cols; - int y1 = d.ymin * frame.rows; - int x2 = d.xmax * frame.cols; - int y2 = d.ymax * frame.rows; - - int labelIndex = d.label; - std::string labelStr = 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, color); - - std::stringstream confStr; - confStr << std::fixed << std::setprecision(2) << d.confidence * 100; - cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); - - 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, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); - - cv::imshow("preview", frame); - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } - - return 0; -} \ No newline at end of file diff --git a/examples/src/mono_camera_control.cpp b/examples/src/mono_camera_control.cpp new file mode 100644 index 000000000..8636c0f43 --- /dev/null +++ b/examples/src/mono_camera_control.cpp @@ -0,0 +1,149 @@ +/** + * This example shows usage of mono camera in crop mode with the possibility to move the crop. + * Uses 'WASD' controls to move the crop window, 'T' to trigger autofocus, 'IOKL,.' for manual exposure/focus: + * Control: key[dec/inc] min..max + * exposure time: I O 1..33000 [us] + * sensitivity iso: K L 100..1600 + * To go back to auto controls: + * 'E' - autoexposure + */ +#include + +// Includes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Step size ('W','A','S','D' controls) +static constexpr float stepSize = 0.02; + +// Manual exposure/focus set step +static constexpr int EXP_STEP = 500; // us +static constexpr int ISO_STEP = 50; + +static int clamp(int num, int v0, int v1) { + return std::max(v0, std::min(num, v1)); +} + +static std::atomic sendCamConfig{false}; + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoRight = pipeline.create(); + auto monoLeft = pipeline.create(); + auto manipRight = pipeline.create(); + auto manipLeft = pipeline.create(); + + auto controlIn = pipeline.create(); + auto configIn = pipeline.create(); + auto manipOutRight = pipeline.create(); + auto manipOutLeft = pipeline.create(); + + controlIn->setStreamName("control"); + configIn->setStreamName("config"); + manipOutRight->setStreamName("right"); + manipOutLeft->setStreamName("left"); + + // Crop range + dai::Point2f topLeft(0.2, 0.2); + dai::Point2f bottomRight(0.8, 0.8); + + // Properties + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + manipRight->initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y); + manipLeft->initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y); + + // Linking + monoRight->out.link(manipRight->inputImage); + monoLeft->out.link(manipLeft->inputImage); + controlIn->out.link(monoRight->inputControl); + controlIn->out.link(monoLeft->inputControl); + configIn->out.link(manipRight->inputConfig); + configIn->out.link(manipLeft->inputConfig); + manipRight->out.link(manipOutRight->input); + manipLeft->out.link(manipOutLeft->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the grayscale frames + auto qRight = device.getOutputQueue(manipOutRight->getStreamName(), 4, false); + auto qLeft = device.getOutputQueue(manipOutLeft->getStreamName(), 4, false); + auto controlQueue = device.getInputQueue(controlIn->getStreamName()); + auto configQueue = device.getInputQueue(configIn->getStreamName()); + + // Defaults and limits for manual focus/exposure controls + int exp_time = 20000; + int exp_min = 1; + int exp_max = 33000; + + int sens_iso = 800; + int sens_min = 100; + int sens_max = 1600; + + while(true) { + auto inRight = qRight->get(); + auto inLeft = qLeft->get(); + cv::imshow("right", inRight->getCvFrame()); + cv::imshow("left", inLeft->getCvFrame()); + + // Update screen (1ms pooling rate) + int key = cv::waitKey(1); + if(key == 'q') { + break; + } else if(key == 'e') { + printf("Autoexposure enable\n"); + dai::CameraControl ctrl; + ctrl.setAutoExposureEnable(); + controlQueue->send(ctrl); + } else if(key == 'i' || key == 'o' || key == 'k' || key == 'l') { + if(key == 'i') exp_time -= EXP_STEP; + if(key == 'o') exp_time += EXP_STEP; + if(key == 'k') sens_iso -= ISO_STEP; + if(key == 'l') sens_iso += ISO_STEP; + exp_time = clamp(exp_time, exp_min, exp_max); + sens_iso = clamp(sens_iso, sens_min, sens_max); + printf("Setting manual exposure, time: %d, iso: %d\n", exp_time, sens_iso); + dai::CameraControl ctrl; + ctrl.setManualExposure(exp_time, sens_iso); + controlQueue->send(ctrl); + } else if(key == 'w') { + if(topLeft.y - stepSize >= 0) { + topLeft.y -= stepSize; + bottomRight.y -= stepSize; + sendCamConfig = true; + } + } else if(key == 'a') { + if(topLeft.x - stepSize >= 0) { + topLeft.x -= stepSize; + bottomRight.x -= stepSize; + sendCamConfig = true; + } + } else if(key == 's') { + if(bottomRight.y + stepSize <= 1) { + topLeft.y += stepSize; + bottomRight.y += stepSize; + sendCamConfig = true; + } + } else if(key == 'd') { + if(bottomRight.x + stepSize <= 1) { + topLeft.x += stepSize; + bottomRight.x += stepSize; + sendCamConfig = true; + } + } + + // Send new config to camera + if(sendCamConfig) { + dai::ImageManipConfig cfg; + cfg.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y); + configQueue->send(cfg); + sendCamConfig = false; + } + } + return 0; +} diff --git a/examples/src/mono_camera_example.cpp b/examples/src/mono_camera_example.cpp deleted file mode 100644 index 730b736d9..000000000 --- a/examples/src/mono_camera_example.cpp +++ /dev/null @@ -1,57 +0,0 @@ - -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -dai::Pipeline createMonoPipeline() { - dai::Pipeline p; - - auto monoCam = p.create(); - auto xlinkOut = p.create(); - xlinkOut->setStreamName("mono"); - - // Set camera socket - monoCam->setBoardSocket(dai::CameraBoardSocket::RIGHT); - - // Link plugins CAM -> XLINK - monoCam->out.link(xlinkOut->input); - - return p; -} - -int main() { - using namespace std; - using namespace std::chrono; - - // Create pipeline - dai::Pipeline p = createMonoPipeline(); - - // Connect and start the pipeline - dai::Device d(p); - - cv::Mat frame; - auto monoQueue = d.getOutputQueue("mono"); - - while(1) { - auto imgFrame = monoQueue->get(); - if(imgFrame) { - int latencyMs = duration_cast(steady_clock::now() - imgFrame->getTimestamp()).count(); - printf("Frame - w: %d, h: %d, latency %dms\n", imgFrame->getWidth(), imgFrame->getHeight(), latencyMs); - - frame = cv::Mat(imgFrame->getHeight(), imgFrame->getWidth(), CV_8UC1, imgFrame->getData().data()); - - cv::imshow("video", frame); - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } else { - std::cout << "Not ImgFrame" << std::endl; - } - } - return 0; -} \ No newline at end of file diff --git a/examples/src/mono_depth_mobilenetssd.cpp b/examples/src/mono_depth_mobilenetssd.cpp new file mode 100644 index 000000000..cfa5b5812 --- /dev/null +++ b/examples/src/mono_depth_mobilenetssd.cpp @@ -0,0 +1,153 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +static std::atomic flipRectified{true}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoRight = pipeline.create(); + auto monoLeft = pipeline.create(); + auto stereo = pipeline.create(); + auto manip = pipeline.create(); + auto nn = pipeline.create(); + + auto disparityOut = pipeline.create(); + auto xoutRight = pipeline.create(); + auto nnOut = pipeline.create(); + + disparityOut->setStreamName("disparity"); + xoutRight->setStreamName("rectifiedRight"); + nnOut->setStreamName("nn"); + + // Properties + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + // Produce the depth map (using disparity output as it's easier to visualize depth this way) + stereo->setConfidenceThreshold(255); + stereo->setRectifyEdgeFillColor(0); // Black, to better see the cutout from rectification (black stripe on the edges) + // Convert the grayscale frame into the nn-acceptable form + manip->initialConfig.setResize(300, 300); + // The NN model expects BGR input. By default ImageManip output type would be same as input (gray in this case) + manip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + + // Define a neural network that will make predictions based on the source frames + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // Linking + monoRight->out.link(stereo->right); + monoLeft->out.link(stereo->left); + stereo->rectifiedRight.link(manip->inputImage); + stereo->disparity.link(disparityOut->input); + manip->out.link(nn->input); + manip->out.link(xoutRight->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the grayscale / depth frames and nn data from the outputs defined above + auto qRight = device.getOutputQueue("rectifiedRight", 4, false); + auto qDisparity = device.getOutputQueue("disparity", 4, false); + auto qDet = device.getOutputQueue("nn", 4, false); + + cv::Mat rightFrame; + cv::Mat disparityFrame; + std::vector detections; + + // Add bounding boxes and text to the frame and show it to the user + auto show = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 192, 203); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + float disparityMultiplier = 255 / stereo->getMaxDisparity(); + + while(true) { + // Instead of get (blocking), we use tryGet (nonblocking) which will return the available data or None otherwise + auto inRight = qRight->tryGet(); + auto inDet = qDet->tryGet(); + auto inDisparity = qDisparity->tryGet(); + + if(inRight) { + rightFrame = inRight->getCvFrame(); + if(flipRectified) { + cv::flip(rightFrame, rightFrame, 1); + } + } + + if(inDet) { + detections = inDet->detections; + if(flipRectified) { + for(auto& detection : detections) { + auto swap = detection.xmin; + detection.xmin = 1 - detection.xmax; + detection.xmax = 1 - swap; + } + } + } + + if(inDisparity) { + // Frame is transformed, normalized, and color map will be applied to highlight the depth info + disparityFrame = inDisparity->getFrame(); + disparityFrame.convertTo(disparityFrame, CV_8UC1, disparityMultiplier); + // Available color maps: https://docs.opencv.org/3.4/d3/d50/group__imgproc__colormap.html + cv::applyColorMap(disparityFrame, disparityFrame, cv::COLORMAP_JET); + show("disparity", disparityFrame, detections); + } + + if(!rightFrame.empty()) { + show("rectified right", rightFrame, detections); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') return 0; + } + return 0; +} diff --git a/examples/src/mono_full_resolution_saver.cpp b/examples/src/mono_full_resolution_saver.cpp new file mode 100644 index 000000000..a3e98346a --- /dev/null +++ b/examples/src/mono_full_resolution_saver.cpp @@ -0,0 +1,53 @@ +#include +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" +#include "utility.hpp" + +int main() { + using namespace std::chrono; + // Create pipeline + dai::Pipeline pipeline; + + // Define source and output + auto monoRight = pipeline.create(); + auto xoutRight = pipeline.create(); + + xoutRight->setStreamName("right"); + + // Properties + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + + // Linking + monoRight->out.link(xoutRight->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queue will be used to get the grayscale frames from the output defined above + auto qRight = device.getOutputQueue("right", 4, false); + + std::string dirName = "mono_data"; + createDirectory(dirName); + + while(true) { + auto inRight = qRight->get(); + // Data is originally represented as a flat 1D array, it needs to be converted into HxW form + // Frame is transformed and ready to be shown + cv::imshow("right", inRight->getCvFrame()); + + uint64_t time = duration_cast(system_clock::now().time_since_epoch()).count(); + std::stringstream videoStr; + videoStr << dirName << "/" << time << ".png"; + // After showing the frame, it's being stored inside a target directory as a PNG image + cv::imwrite(videoStr.str(), inRight->getCvFrame()); + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/mono_mobilenet.cpp b/examples/src/mono_mobilenet.cpp new file mode 100644 index 000000000..bb16d5603 --- /dev/null +++ b/examples/src/mono_mobilenet.cpp @@ -0,0 +1,116 @@ +#include + +#include "utility.hpp" + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoRight = pipeline.create(); + auto manip = pipeline.create(); + auto nn = pipeline.create(); + auto manipOut = pipeline.create(); + auto nnOut = pipeline.create(); + + manipOut->setStreamName("right"); + nnOut->setStreamName("nn"); + + // Properties + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + + // Convert the grayscale frame into the nn-acceptable form + manip->initialConfig.setResize(300, 300); + // The NN model expects BGR input. By default ImageManip output type would be same as input (gray in this case) + manip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // Linking + monoRight->out.link(manip->inputImage); + manip->out.link(nn->input); + manip->out.link(manipOut->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the grayscale frames and nn data from the outputs defined above + auto qRight = device.getOutputQueue("right", 4, false); + auto qDet = device.getOutputQueue("nn", 4, false); + + cv::Mat frame; + std::vector detections; + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + while(true) { + // Instead of get (blocking), we use tryGet (nonblocking) which will return the available data or None otherwise + auto inRight = qRight->tryGet(); + auto inDet = qDet->tryGet(); + + if(inRight) { + frame = inRight->getCvFrame(); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!frame.empty()) { + displayFrame("right", frame, detections); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') return 0; + } + return 0; +} diff --git a/examples/src/mono_preview.cpp b/examples/src/mono_preview.cpp new file mode 100644 index 000000000..44558145e --- /dev/null +++ b/examples/src/mono_preview.cpp @@ -0,0 +1,55 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto xoutLeft = pipeline.create(); + auto xoutRight = pipeline.create(); + + xoutLeft->setStreamName("left"); + xoutRight->setStreamName("right"); + + // Properties + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + + // Linking + monoRight->out.link(xoutRight->input); + monoLeft->out.link(xoutLeft->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the grayscale frames from the outputs defined above + auto qLeft = device.getOutputQueue("left", 4, false); + auto qRight = device.getOutputQueue("right", 4, false); + + while(true) { + // Instead of get (blocking), we use tryGet (nonblocking) which will return the available data or None otherwise + auto inLeft = qLeft->tryGet(); + auto inRight = qRight->tryGet(); + + if(inLeft) { + cv::imshow("left", inLeft->getCvFrame()); + } + + if(inRight) { + cv::imshow("right", inRight->getCvFrame()); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/object_tracker_example.cpp b/examples/src/object_tracker.cpp similarity index 55% rename from examples/src/object_tracker_example.cpp rename to examples/src/object_tracker.cpp index c691642ad..dc43559c2 100644 --- a/examples/src/object_tracker_example.cpp +++ b/examples/src/object_tracker.cpp @@ -1,9 +1,6 @@ #include -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" @@ -11,76 +8,78 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; -static bool syncNN = true; +static std::atomic fullFrameTracking{false}; + +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } - auto colorCam = p.create(); - auto xlinkOut = p.create(); - auto detectionNetwork = p.create(); - auto objectTracker = p.create(); - auto trackerOut = p.create(); + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto detectionNetwork = pipeline.create(); + auto objectTracker = pipeline.create(); + + auto xlinkOut = pipeline.create(); + auto trackerOut = pipeline.create(); xlinkOut->setStreamName("preview"); trackerOut->setStreamName("tracklets"); - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); - colorCam->setFps(40); + // Properties + camRgb->setPreviewSize(300, 300); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + camRgb->setFps(40); // testing MobileNet DetectionNetwork - detectionNetwork->setConfidenceThreshold(0.5f); detectionNetwork->setBlobPath(nnPath); - - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(detectionNetwork->input); - if(syncNN) { - objectTracker->passthroughTrackerFrame.link(xlinkOut->input); - } else { - colorCam->preview.link(xlinkOut->input); - } + detectionNetwork->setConfidenceThreshold(0.5f); + detectionNetwork->input.setBlocking(false); objectTracker->setDetectionLabelsToTrack({15}); // track only person + // possible tracking types: ZERO_TERM_COLOR_HISTOGRAM, ZERO_TERM_IMAGELESS objectTracker->setTrackerType(dai::TrackerType::ZERO_TERM_COLOR_HISTOGRAM); + // take the smallest ID when new object is tracked, possible options: SMALLEST_ID, UNIQUE_ID objectTracker->setTrackerIdAssigmentPolicy(dai::TrackerIdAssigmentPolicy::SMALLEST_ID); - detectionNetwork->passthrough.link(objectTracker->inputTrackerFrame); - detectionNetwork->passthrough.link(objectTracker->inputDetectionFrame); - detectionNetwork->out.link(objectTracker->inputDetections); - objectTracker->out.link(trackerOut->input); - - return p; -} + // Linking + camRgb->preview.link(detectionNetwork->input); + objectTracker->passthroughTrackerFrame.link(xlinkOut->input); -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); + if(fullFrameTracking) { + camRgb->video.link(objectTracker->inputTrackerFrame); + } else { + detectionNetwork->passthrough.link(objectTracker->inputTrackerFrame); } - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); + detectionNetwork->passthrough.link(objectTracker->inputDetectionFrame); + detectionNetwork->out.link(objectTracker->inputDetections); + objectTracker->out.link(trackerOut->input); - // Connect and start the pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); - auto preview = d.getOutputQueue("preview", 4, false); - auto tracklets = d.getOutputQueue("tracklets", 4, false); + auto preview = device.getOutputQueue("preview", 4, false); + auto tracklets = device.getOutputQueue("tracklets", 4, false); auto startTime = steady_clock::now(); int counter = 0; float fps = 0; - while(1) { + + while(true) { auto imgFrame = preview->get(); auto track = tracklets->get(); @@ -94,10 +93,10 @@ int main(int argc, char** argv) { } auto color = cv::Scalar(255, 0, 0); - cv::Mat trackletFrame = imgFrame->getCvFrame(); + cv::Mat frame = imgFrame->getCvFrame(); auto trackletsData = track->tracklets; for(auto& t : trackletsData) { - auto roi = t.roi.denormalize(trackletFrame.cols, trackletFrame.rows); + auto roi = t.roi.denormalize(frame.cols, frame.rows); int x1 = roi.topLeft().x; int y1 = roi.topLeft().y; int x2 = roi.bottomRight().x; @@ -108,29 +107,28 @@ int main(int argc, char** argv) { if(labelIndex < labelMap.size()) { labelStr = labelMap[labelIndex]; } - cv::putText(trackletFrame, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); std::stringstream idStr; idStr << "ID: " << t.id; - cv::putText(trackletFrame, idStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, idStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); std::stringstream statusStr; statusStr << "Status: " << t.status; - cv::putText(trackletFrame, statusStr.str(), cv::Point(x1 + 10, y1 + 60), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, statusStr.str(), cv::Point(x1 + 10, y1 + 60), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); - cv::rectangle(trackletFrame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + 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(trackletFrame, fpsStr.str(), cv::Point(2, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); + fpsStr << "NN fps:" << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); - cv::imshow("tracker", trackletFrame); + cv::imshow("tracker", frame); int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } - return 0; -} \ No newline at end of file +} diff --git a/examples/src/opencv_support.cpp b/examples/src/opencv_support.cpp new file mode 100644 index 000000000..e55af8c38 --- /dev/null +++ b/examples/src/opencv_support.cpp @@ -0,0 +1,52 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Include OpenCV +#include + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define source and outputs + auto camRgb = pipeline.create(); + auto xoutVideo = pipeline.create(); + auto xoutPreview = pipeline.create(); + + xoutVideo->setStreamName("video"); + xoutPreview->setStreamName("preview"); + + // Properties + camRgb->setPreviewSize(300, 300); + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(true); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + + // Linking + camRgb->video.link(xoutVideo->input); + camRgb->preview.link(xoutPreview->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + auto video = device.getOutputQueue("video"); + auto preview = device.getOutputQueue("preview"); + + while(true) { + auto videoFrame = video->get(); + auto previewFrame = preview->get(); + + // Get BGR frame from NV12 encoded video frame to show with opencv + cv::imshow("video", videoFrame->getCvFrame()); + + // Show 'preview' frame as is (already in correct format, no copy is made) + cv::imshow("preview", previewFrame->getFrame()); + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') return 0; + } + return 0; +} diff --git a/examples/src/opencv_support_example.cpp b/examples/src/opencv_support_example.cpp deleted file mode 100644 index 4b8ca5541..000000000 --- a/examples/src/opencv_support_example.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include -#include - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -// Include OpenCV -#include - -dai::Pipeline createCameraFullPipeline() { - dai::Pipeline p; - - auto colorCam = p.create(); - auto xlinkOut = p.create(); - auto xlinkOut2 = p.create(); - xlinkOut->setStreamName("video"); - xlinkOut2->setStreamName("preview"); - - colorCam->setPreviewSize(320, 180); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(true); - - // Link plugins CAM -> XLINK - colorCam->video.link(xlinkOut->input); - colorCam->preview.link(xlinkOut2->input); - - return p; -} - -int main() { - using namespace std; - using namespace std::chrono; - - // Create pipeline - dai::Pipeline p = createCameraFullPipeline(); - - // Connect and start the pipeline - dai::Device d(p); - - auto video = d.getOutputQueue("video"); - auto preview = d.getOutputQueue("preview"); - - while(1) { - // Retrieves video ImgFrame and converts a cv::Mat copy in BGR format (suitable for opencv usage) - auto videoFrame = video->get(); - cv::imshow("video", videoFrame->getCvFrame()); - - // Retrieves preview ImgFrame and returns (without copying deepCopy = false) cv::Mat - auto previewFrame = preview->get(); - cv::imshow("preview", previewFrame->getFrame()); - - // Waits a bit and updates windows - if(cv::waitKey(1) == 'q') break; - } - return 0; -} \ No newline at end of file diff --git a/examples/src/color_camera_control_example.cpp b/examples/src/rgb_camera_control.cpp similarity index 57% rename from examples/src/color_camera_control_example.cpp rename to examples/src/rgb_camera_control.cpp index 5bd33ade4..791b4b900 100644 --- a/examples/src/color_camera_control_example.cpp +++ b/examples/src/rgb_camera_control.cpp @@ -10,7 +10,6 @@ * 'E' - autoexposure * 'F' - autofocus (continuous) */ -#include #include #include "utility.hpp" @@ -19,7 +18,7 @@ #include "depthai/depthai.hpp" // Step size ('W','A','S','D' controls) -static constexpr int STEP_SIZE = 16; +static constexpr int STEP_SIZE = 8; // Manual exposure/focus set step static constexpr int EXP_STEP = 500; // us @@ -31,68 +30,72 @@ static int clamp(int num, int v0, int v1) { } int main() { + // Create pipeline dai::Pipeline pipeline; - // Nodes - auto colorCam = pipeline.create(); - auto controlIn = pipeline.create(); - auto configIn = pipeline.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); auto videoEncoder = pipeline.create(); auto stillEncoder = pipeline.create(); + + auto controlIn = pipeline.create(); + auto configIn = pipeline.create(); auto videoMjpegOut = pipeline.create(); auto stillMjpegOut = pipeline.create(); auto previewOut = pipeline.create(); - // Properties - colorCam->setVideoSize(640, 360); - colorCam->setPreviewSize(300, 300); controlIn->setStreamName("control"); configIn->setStreamName("config"); - videoEncoder->setDefaultProfilePreset(colorCam->getVideoSize(), colorCam->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); - stillEncoder->setDefaultProfilePreset(colorCam->getStillSize(), 1, dai::VideoEncoderProperties::Profile::MJPEG); videoMjpegOut->setStreamName("video"); stillMjpegOut->setStreamName("still"); previewOut->setStreamName("preview"); - // Link nodes - colorCam->video.link(videoEncoder->input); - colorCam->still.link(stillEncoder->input); - colorCam->preview.link(previewOut->input); - controlIn->out.link(colorCam->inputControl); - configIn->out.link(colorCam->inputConfig); + // Properties + camRgb->setVideoSize(640, 360); + camRgb->setPreviewSize(300, 300); + videoEncoder->setDefaultProfilePreset(camRgb->getVideoSize(), camRgb->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + stillEncoder->setDefaultProfilePreset(camRgb->getStillSize(), 1, dai::VideoEncoderProperties::Profile::MJPEG); + + // Linking + camRgb->video.link(videoEncoder->input); + camRgb->still.link(stillEncoder->input); + camRgb->preview.link(previewOut->input); + controlIn->out.link(camRgb->inputControl); + configIn->out.link(camRgb->inputConfig); videoEncoder->bitstream.link(videoMjpegOut->input); stillEncoder->bitstream.link(stillMjpegOut->input); - // Connect and start the pipeline - dai::Device dev(pipeline); + // Connect to device and start pipeline + dai::Device device(pipeline); - // Create data queues - auto controlQueue = dev.getInputQueue("control"); - auto configQueue = dev.getInputQueue("config"); - auto previewQueue = dev.getOutputQueue("preview"); - auto videoQueue = dev.getOutputQueue("video"); - auto stillQueue = dev.getOutputQueue("still"); + // Get data queues + auto controlQueue = device.getInputQueue("control"); + auto configQueue = device.getInputQueue("config"); + auto previewQueue = device.getOutputQueue("preview"); + auto videoQueue = device.getOutputQueue("video"); + auto stillQueue = device.getOutputQueue("still"); - // Max crop_x & crop_y - float max_crop_x = (colorCam->getResolutionWidth() - colorCam->getVideoWidth()) / (float)colorCam->getResolutionWidth(); - float max_crop_y = (colorCam->getResolutionHeight() - colorCam->getVideoHeight()) / (float)colorCam->getResolutionHeight(); + // Max cropX & cropY + float maxCropX = (camRgb->getResolutionWidth() - camRgb->getVideoWidth()) / (float)camRgb->getResolutionWidth(); + float maxCropY = (camRgb->getResolutionHeight() - camRgb->getVideoHeight()) / (float)camRgb->getResolutionHeight(); // Default crop - float crop_x = 0; - float crop_y = 0; + float cropX = 0; + float cropY = 0; + bool sendCamConfig = true; // Defaults and limits for manual focus/exposure controls - int lens_pos = 150; - int lens_min = 0; - int lens_max = 255; + int lensPos = 150; + int lensMin = 0; + int lensMax = 255; - int exp_time = 20000; - int exp_min = 1; - int exp_max = 33000; + int expTime = 20000; + int expMin = 1; + int expMax = 33000; - int sens_iso = 800; - int sens_min = 100; - int sens_max = 1600; + int sensIso = 800; + int sensMin = 100; + int sensMax = 1600; while(true) { auto previewFrames = previewQueue->tryGetAll(); @@ -107,6 +110,15 @@ int main() { auto frame = cv::imdecode(videoFrame->getData(), cv::IMREAD_UNCHANGED); // Display cv::imshow("video", frame); + + // Send new cfg to camera + if(sendCamConfig) { + dai::ImageManipConfig cfg; + cfg.setCropRect(cropX, cropY, 0, 0); + configQueue->send(cfg); + printf("Sending new crop - x: %f, y: %f\n", cropX, cropY); + sendCamConfig = false; + } } auto stillFrames = stillQueue->tryGetAll(); @@ -117,8 +129,8 @@ int main() { cv::imshow("still", frame); } - // Update screen (10ms pooling rate) - int key = cv::waitKey(10); + // Update screen (1ms pooling rate) + int key = cv::waitKey(1); if(key == 'q') { break; } else if(key == 'c') { @@ -142,44 +154,40 @@ int main() { ctrl.setAutoExposureEnable(); controlQueue->send(ctrl); } else if(key == ',' || key == '.') { - if(key == ',') lens_pos -= LENS_STEP; - if(key == '.') lens_pos += LENS_STEP; - lens_pos = clamp(lens_pos, lens_min, lens_max); - printf("Setting manual focus, lens position: %d\n", lens_pos); + if(key == ',') lensPos -= LENS_STEP; + if(key == '.') lensPos += LENS_STEP; + lensPos = clamp(lensPos, lensMin, lensMax); + printf("Setting manual focus, lens position: %d\n", lensPos); dai::CameraControl ctrl; - ctrl.setManualFocus(lens_pos); + ctrl.setManualFocus(lensPos); controlQueue->send(ctrl); } else if(key == 'i' || key == 'o' || key == 'k' || key == 'l') { - if(key == 'i') exp_time -= EXP_STEP; - if(key == 'o') exp_time += EXP_STEP; - if(key == 'k') sens_iso -= ISO_STEP; - if(key == 'l') sens_iso += ISO_STEP; - exp_time = clamp(exp_time, exp_min, exp_max); - sens_iso = clamp(sens_iso, sens_min, sens_max); - printf("Setting manual exposure, time %d us, iso %d\n", exp_time, sens_iso); + if(key == 'i') expTime -= EXP_STEP; + if(key == 'o') expTime += EXP_STEP; + if(key == 'k') sensIso -= ISO_STEP; + if(key == 'l') sensIso += ISO_STEP; + expTime = clamp(expTime, expMin, expMax); + sensIso = clamp(sensIso, sensMin, sensMax); + printf("Setting manual exposure, time: %d, iso: %d\n", expTime, sensIso); dai::CameraControl ctrl; - ctrl.setManualExposure(exp_time, sens_iso); + ctrl.setManualExposure(expTime, sensIso); controlQueue->send(ctrl); } else if(key == 'w' || key == 'a' || key == 's' || key == 'd') { if(key == 'a') { - crop_x -= (max_crop_x / colorCam->getResolutionWidth()) * STEP_SIZE; - if(crop_x < 0) crop_x = max_crop_x; + cropX -= (maxCropX / camRgb->getResolutionWidth()) * STEP_SIZE; + if(cropX < 0) cropX = maxCropX; } else if(key == 'd') { - crop_x += (max_crop_x / colorCam->getResolutionWidth()) * STEP_SIZE; - if(crop_x > max_crop_x) crop_x = 0.0f; + cropX += (maxCropX / camRgb->getResolutionWidth()) * STEP_SIZE; + if(cropX > maxCropX) cropX = 0.0f; } else if(key == 'w') { - crop_y -= (max_crop_y / colorCam->getResolutionHeight()) * STEP_SIZE; - if(crop_y < 0) crop_y = max_crop_y; + cropY -= (maxCropY / camRgb->getResolutionHeight()) * STEP_SIZE; + if(cropY < 0) cropY = maxCropY; } else if(key == 's') { - crop_y += (max_crop_y / colorCam->getResolutionHeight()) * STEP_SIZE; - if(crop_y > max_crop_y) crop_y = 0.0f; + cropY += (maxCropY / camRgb->getResolutionHeight()) * STEP_SIZE; + if(cropY > maxCropY) cropY = 0.0f; } - - // Send new cfg to camera - dai::ImageManipConfig cfg; - cfg.setCropRect(crop_x, crop_y, 0, 0); - configQueue->send(cfg); - printf("Sending new crop - x: %f, y: %f\n", crop_x, crop_y); + sendCamConfig = true; } } + return 0; } diff --git a/examples/src/rgb_depth_aligned_example.cpp b/examples/src/rgb_depth_aligned.cpp similarity index 71% rename from examples/src/rgb_depth_aligned_example.cpp rename to examples/src/rgb_depth_aligned.cpp index 851d18584..98c0bffcd 100644 --- a/examples/src/rgb_depth_aligned_example.cpp +++ b/examples/src/rgb_depth_aligned.cpp @@ -8,65 +8,69 @@ // Optional. If set (true), the ColorCamera is downscaled from 1080p to 720p. // Otherwise (false), the aligned depth is automatically upscaled to 1080p -static constexpr bool downscaleColor = true; +static std::atomic downscaleColor{true}; int main() { using namespace std; - dai::Pipeline p; + // Create pipeline + dai::Pipeline pipeline; std::vector queueNames; - auto cam = p.create(); - cam->setBoardSocket(dai::CameraBoardSocket::RGB); - cam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - if(downscaleColor) cam->setIspScale(2, 3); - // For now, RGB needs fixed focus to properly align with depth. - // This value was used during calibration - cam->initialControl.setManualFocus(130); + // Define sources and outputs + auto camRgb = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + + auto rgbOut = pipeline.create(); + auto depthOut = pipeline.create(); - auto rgbOut = p.create(); rgbOut->setStreamName("rgb"); queueNames.push_back("rgb"); - cam->isp.link(rgbOut->input); + depthOut->setStreamName("depth"); + queueNames.push_back("depth"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + if(downscaleColor) camRgb->setIspScale(2, 3); + // For now, RGB needs fixed focus to properly align with depth. + // This value was used during calibration + camRgb->initialControl.setManualFocus(130); - auto left = p.create(); left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); left->setBoardSocket(dai::CameraBoardSocket::LEFT); - - auto right = p.create(); right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); right->setBoardSocket(dai::CameraBoardSocket::RIGHT); - auto stereo = p.create(); stereo->setConfidenceThreshold(200); // LR-check is required for depth alignment stereo->setLeftRightCheck(true); stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + + // Linking + camRgb->isp.link(rgbOut->input); left->out.link(stereo->left); right->out.link(stereo->right); - - auto depthOut = p.create(); - depthOut->setStreamName("depth"); - queueNames.push_back("depth"); - // Currently we use the 'disparity' output. TODO 'depth' stereo->disparity.link(depthOut->input); // Connect to device and start pipeline - dai::Device d(p); + dai::Device device(pipeline); // Sets queues size and behavior for(const auto& name : queueNames) { - d.getOutputQueue(name, 4, false); + device.getOutputQueue(name, 4, false); } std::unordered_map frame; - while(1) { + while(true) { std::unordered_map> latestPacket; - auto queueEvents = d.getQueueEvents(queueNames); + auto queueEvents = device.getQueueEvents(queueNames); for(const auto& name : queueEvents) { - auto packets = d.getOutputQueue(name)->tryGetAll(); + auto packets = device.getOutputQueue(name)->tryGetAll(); auto count = packets.size(); if(count > 0) { latestPacket[name] = packets[count - 1]; @@ -77,8 +81,9 @@ int main() { if(latestPacket.find(name) != latestPacket.end()) { if(name == "depth") { frame[name] = latestPacket[name]->getFrame(); + auto maxDisparity = stereo->getMaxDisparity(); // Optional, extend range 0..95 -> 0..255, for a better visualisation - if(1) frame[name].convertTo(frame[name], CV_8UC1, 255. / 95); + if(1) frame[name].convertTo(frame[name], CV_8UC1, 255. / maxDisparity); // Optional, apply false colorization if(1) cv::applyColorMap(frame[name], frame[name], cv::COLORMAP_HOT); } else { @@ -102,8 +107,9 @@ int main() { } int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } + return 0; } diff --git a/examples/src/rgb_encoding.cpp b/examples/src/rgb_encoding.cpp new file mode 100644 index 000000000..52a192003 --- /dev/null +++ b/examples/src/rgb_encoding.cpp @@ -0,0 +1,55 @@ +#include +#include + +// Includes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Keyboard interrupt (Ctrl + C) detected +static std::atomic alive{true}; +static void sigintHandler(int signum) { + alive = false; +} + +int main(int argc, char** argv) { + using namespace std; + std::signal(SIGINT, &sigintHandler); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto videoEnc = pipeline.create(); + auto xout = pipeline.create(); + + xout->setStreamName("h265"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_4_K); + videoEnc->setDefaultProfilePreset(3840, 2160, 30, dai::VideoEncoderProperties::Profile::H265_MAIN); + + // Linking + camRgb->video.link(videoEnc->input); + videoEnc->bitstream.link(xout->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queue will be used to get the encoded data from the output defined above + auto q = device.getOutputQueue("h265", 30, true); + + // The .h265 file is a raw stream file (not playable yet) + auto videoFile = std::ofstream("video.h265", std::ios::binary); + cout << "Press Ctrl+C to stop encoding..." << endl; + + while(alive) { + auto h265Packet = q->get(); + videoFile.write((char*)(h265Packet->getData().data()), h265Packet->getData().size()); + } + + cout << "To view the encoded data, convert the stream file (.h265) into a video file (.mp4) using a command below:" << endl; + cout << "ffmpeg -framerate 30 -i video.h265 -c copy video.mp4" << endl; + + return 0; +} diff --git a/examples/src/rgb_encoding_mobilenet.cpp b/examples/src/rgb_encoding_mobilenet.cpp new file mode 100644 index 000000000..991e13189 --- /dev/null +++ b/examples/src/rgb_encoding_mobilenet.cpp @@ -0,0 +1,127 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto videoEncoder = pipeline.create(); + auto nn = pipeline.create(); + + auto xoutRgb = pipeline.create(); + auto videoOut = pipeline.create(); + auto nnOut = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + videoOut->setStreamName("h265"); + nnOut->setStreamName("nn"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setPreviewSize(300, 300); + camRgb->setInterleaved(false); + + videoEncoder->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::H265_MAIN); + + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // Linking + camRgb->video.link(videoEncoder->input); + camRgb->preview.link(xoutRgb->input); + camRgb->preview.link(nn->input); + videoEncoder->bitstream.link(videoOut->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Queues + int queueSize = 8; + auto qRgb = device.getOutputQueue("rgb", queueSize); + auto qDet = device.getOutputQueue("nn", queueSize); + auto qRgbEnc = device.getOutputQueue("h265", 30, true); + + cv::Mat frame; + std::vector detections; + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + auto videoFile = std::ofstream("video.h264", std::ios::binary); + + while(true) { + auto inRgb = qRgb->tryGet(); + auto inDet = qDet->tryGet(); + + auto out = qRgbEnc->get(); + videoFile.write((char*)out->getData().data(), out->getData().size()); + + if(inRgb) { + frame = inRgb->getCvFrame(); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!frame.empty()) { + displayFrame("rgb", frame, detections); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + cout << "To view the encoded data, convert the stream file (.h265) into a video file (.mp4), using a command below:" << endl; + cout << "ffmpeg -framerate 30 -i video.h264 -c copy video.mp4" << endl; + return 0; +} diff --git a/examples/src/rgb_encoding_mono_mobilenet.cpp b/examples/src/rgb_encoding_mono_mobilenet.cpp new file mode 100644 index 000000000..249459ec9 --- /dev/null +++ b/examples/src/rgb_encoding_mono_mobilenet.cpp @@ -0,0 +1,162 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto monoRight = pipeline.create(); + auto videoEncoder = pipeline.create(); + auto nn = pipeline.create(); + auto manip = pipeline.create(); + + auto videoOut = pipeline.create(); + auto xoutRight = pipeline.create(); + auto manipOut = pipeline.create(); + auto nnOut = pipeline.create(); + + videoOut->setStreamName("h265"); + xoutRight->setStreamName("right"); + manipOut->setStreamName("manip"); + nnOut->setStreamName("nn"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P); + videoEncoder->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::H265_MAIN); + + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // The NN model expects BGR input. By default ImageManip output type would be same as input (gray in this case) + manip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + manip->initialConfig.setResize(300, 300); + + // Linking + camRgb->video.link(videoEncoder->input); + videoEncoder->bitstream.link(videoOut->input); + monoRight->out.link(manip->inputImage); + manip->out.link(nn->input); + monoRight->out.link(xoutRight->input); + manip->out.link(manipOut->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Queues + int queueSize = 8; + auto qRight = device.getOutputQueue("right", queueSize); + auto qManip = device.getOutputQueue("manip", queueSize); + auto qDet = device.getOutputQueue("nn", queueSize); + auto qRgbEnc = device.getOutputQueue("h265", 30, true); + + cv::Mat frame; + cv::Mat frameManip; + std::vector detections; + int offsetX = (monoRight->getResolutionWidth() - monoRight->getResolutionHeight()) / 2; + auto color = cv::Scalar(255, 0, 0); + + auto videoFile = std::ofstream("video.h265", std::ios::binary); + cv::namedWindow("right", cv::WINDOW_NORMAL); + cv::namedWindow("manip", cv::WINDOW_NORMAL); + + while(true) { + auto inRight = qRight->tryGet(); + auto inManip = qManip->tryGet(); + auto inDet = qDet->tryGet(); + + auto out1 = qRgbEnc->get(); + videoFile.write((char*)out1->getData().data(), out1->getData().size()); + + if(inRight) { + frame = inRight->getCvFrame(); + } + + if(inManip) { + frameManip = inManip->getCvFrame(); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!frame.empty()) { + for(auto& detection : detections) { + int x1 = detection.xmin * monoRight->getResolutionHeight() + offsetX; + int y1 = detection.ymin * monoRight->getResolutionHeight(); + int x2 = detection.xmax * monoRight->getResolutionHeight() + offsetX; + int y2 = detection.ymax * monoRight->getResolutionHeight(); + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow("right", frame); + } + + if(!frameManip.empty()) { + for(auto& detection : detections) { + int x1 = detection.xmin * frameManip.cols; + int y1 = detection.ymin * frameManip.rows; + int x2 = detection.xmax * frameManip.cols; + int y2 = detection.ymax * frameManip.rows; + + int labelIndex = detection.label; + std::string labelStr = to_string(labelIndex); + if(labelIndex < labelMap.size()) { + labelStr = labelMap[labelIndex]; + } + cv::putText(frameManip, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frameManip, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frameManip, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow("manip", frameManip); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + cout << "To view the encoded data, convert the stream file (.h265) into a video file (.mp4), using a command below:" << endl; + cout << "ffmpeg -framerate 30 -i video.h265 -c copy video.mp4" << endl; + return 0; +} diff --git a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp new file mode 100644 index 000000000..139fa6d00 --- /dev/null +++ b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp @@ -0,0 +1,216 @@ +#include +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +static std::atomic flipRectified{true}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto videoEncoder = pipeline.create(); + auto monoRight = pipeline.create(); + auto monoLeft = pipeline.create(); + auto depth = pipeline.create(); + auto manip = pipeline.create(); + auto nn = pipeline.create(); + + auto videoOut = pipeline.create(); + auto xoutRight = pipeline.create(); + auto disparityOut = pipeline.create(); + auto manipOut = pipeline.create(); + auto nnOut = pipeline.create(); + + videoOut->setStreamName("h265"); + xoutRight->setStreamName("right"); + disparityOut->setStreamName("disparity"); + manipOut->setStreamName("manip"); + nnOut->setStreamName("nn"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + videoEncoder->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::H265_MAIN); + + // Note: the rectified streams are horizontally mirrored by default + depth->setConfidenceThreshold(255); + depth->setRectifyMirrorFrame(false); + depth->setRectifyEdgeFillColor(0); // Black, to better see the cutout + + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // The NN model expects BGR input-> By default ImageManip output type would be same as input (gray in this case) + manip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + manip->initialConfig.setResize(300, 300); + + // Linking + camRgb->video.link(videoEncoder->input); + videoEncoder->bitstream.link(videoOut->input); + monoRight->out.link(xoutRight->input); + monoRight->out.link(depth->right); + monoLeft->out.link(depth->left); + depth->disparity.link(disparityOut->input); + depth->rectifiedRight.link(manip->inputImage); + manip->out.link(nn->input); + manip->out.link(manipOut->input); + nn->out.link(nnOut->input); + + // Disparity range is used for normalization + float disparityMultiplier = 255 / depth->getMaxDisparity(); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Queues + int queueSize = 8; + auto qRight = device.getOutputQueue("right", queueSize); + auto qDisparity = device.getOutputQueue("disparity", queueSize); + auto qManip = device.getOutputQueue("manip", queueSize); + auto qDet = device.getOutputQueue("nn", queueSize); + auto qRgbEnc = device.getOutputQueue("h265", 30, true); + + cv::Mat frame; + cv::Mat frameManip; + cv::Mat frameDisparity; + std::vector detections; + int offsetX = (monoRight->getResolutionWidth() - monoRight->getResolutionHeight()) / 2; + auto color = cv::Scalar(255, 0, 0); + + auto videoFile = std::ofstream("video.h265", std::ios::binary); + cv::namedWindow("right", cv::WINDOW_NORMAL); + cv::namedWindow("manip", cv::WINDOW_NORMAL); + + while(true) { + auto inRight = qRight->tryGet(); + auto inManip = qManip->tryGet(); + auto inDet = qDet->tryGet(); + auto inDisparity = qDisparity->tryGet(); + + auto out1 = qRgbEnc->get(); + videoFile.write((char*)out1->getData().data(), out1->getData().size()); + + if(inRight) { + frame = inRight->getCvFrame(); + } + + if(inManip) { + frameManip = inManip->getCvFrame(); + } + + if(inDisparity) { + frameDisparity = inDisparity->getCvFrame(); + if(flipRectified) { + cv::flip(frameDisparity, frameDisparity, 1); + } + frameDisparity.convertTo(frameDisparity, CV_8UC1, disparityMultiplier); + cv::applyColorMap(frameDisparity, frameDisparity, cv::COLORMAP_JET); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!frame.empty()) { + for(auto& detection : detections) { + int x1 = detection.xmin * monoRight->getResolutionHeight() + offsetX; + int y1 = detection.ymin * monoRight->getResolutionHeight(); + int x2 = detection.xmax * monoRight->getResolutionHeight() + offsetX; + int y2 = detection.ymax * monoRight->getResolutionHeight(); + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the right cam frame + cv::imshow("right", frame); + } + + if(!frameDisparity.empty()) { + for(auto& detection : detections) { + int x1 = detection.xmin * monoRight->getResolutionHeight() + offsetX; + int y1 = detection.ymin * monoRight->getResolutionHeight(); + int x2 = detection.xmax * monoRight->getResolutionHeight() + offsetX; + int y2 = detection.ymax * monoRight->getResolutionHeight(); + + int labelIndex = detection.label; + std::string labelStr = to_string(labelIndex); + if(labelIndex < labelMap.size()) { + labelStr = labelMap[labelIndex]; + } + cv::putText(frameDisparity, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frameDisparity, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frameDisparity, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the disparity frame + cv::imshow("disparity", frameDisparity); + } + + if(!frameManip.empty()) { + for(auto& detection : detections) { + int x1 = detection.xmin * frameManip.cols; + int y1 = detection.ymin * frameManip.rows; + int x2 = detection.xmax * frameManip.cols; + int y2 = detection.ymax * frameManip.rows; + + int labelIndex = detection.label; + std::string labelStr = to_string(labelIndex); + if(labelIndex < labelMap.size()) { + labelStr = labelMap[labelIndex]; + } + cv::putText(frameManip, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frameManip, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frameManip, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the manip frame + cv::imshow("manip", frameManip); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + cout << "To view the encoded data, convert the stream file (.h265) into a video file (.mp4), using a command below:" << endl; + cout << "ffmpeg -framerate 30 -i video.h265 -c copy video.mp4" << endl; + return 0; +} diff --git a/examples/src/rgb_full_resolution_saver.cpp b/examples/src/rgb_full_resolution_saver.cpp new file mode 100644 index 000000000..69b6eaf8f --- /dev/null +++ b/examples/src/rgb_full_resolution_saver.cpp @@ -0,0 +1,64 @@ +#include +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" +#include "utility.hpp" + +int main() { + using namespace std::chrono; + + // Create pipeline + dai::Pipeline pipeline; + + // Define source and outputs + auto camRgb = pipeline.create(); + auto videoEnc = pipeline.create(); + auto xoutJpeg = pipeline.create(); + auto xoutRgb = pipeline.create(); + + xoutJpeg->setStreamName("jpeg"); + xoutRgb->setStreamName("rgb"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_4_K); + videoEnc->setDefaultProfilePreset(camRgb->getVideoSize(), camRgb->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + + // Linking + camRgb->video.link(xoutRgb->input); + camRgb->video.link(videoEnc->input); + videoEnc->bitstream.link(xoutJpeg->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Queues + auto qRgb = device.getOutputQueue("rgb", 30, false); + auto qJpeg = device.getOutputQueue("jpeg", 30, true); + + std::string dirName = "rgb_data"; + createDirectory(dirName); + + while(true) { + auto inRgb = qRgb->tryGet(); + if(inRgb != NULL) { + cv::imshow("rgb", inRgb->getCvFrame()); + } + + auto encFrames = qJpeg->tryGetAll(); + for(const auto& encFrame : encFrames) { + uint64_t time = duration_cast(system_clock::now().time_since_epoch()).count(); + std::stringstream videoStr; + videoStr << dirName << "/" << time << ".jpeg"; + auto videoFile = std::ofstream(videoStr.str(), std::ios::binary); + videoFile.write((char*)encFrame->getData().data(), encFrame->getData().size()); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/rgb_mobilenet.cpp b/examples/src/rgb_mobilenet.cpp new file mode 100644 index 000000000..4186a9446 --- /dev/null +++ b/examples/src/rgb_mobilenet.cpp @@ -0,0 +1,145 @@ +#include +#include +#include + +#include "utility.hpp" + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +static std::atomic syncNN{true}; + +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto nn = pipeline.create(); + auto xoutRgb = pipeline.create(); + auto nnOut = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + nnOut->setStreamName("nn"); + + // Properties + camRgb->setPreviewSize(300, 300); // NN input + camRgb->setInterleaved(false); + camRgb->setFps(40); + // Define a neural network that will make predictions based on the source frames + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // Linking + if(syncNN) { + nn->passthrough.link(xoutRgb->input); + } else { + camRgb->preview.link(xoutRgb->input); + } + + camRgb->preview.link(nn->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the rgb frames and nn data from the outputs defined above + auto qRgb = device.getOutputQueue("rgb", 4, false); + auto qDet = device.getOutputQueue("nn", 4, false); + + cv::Mat frame; + std::vector detections; + auto startTime = steady_clock::now(); + int counter = 0; + float fps = 0; + auto color2 = cv::Scalar(255, 255, 255); + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + while(true) { + std::shared_ptr inRgb; + std::shared_ptr inDet; + + if(syncNN) { + inRgb = qRgb->get(); + inDet = qDet->get(); + } else { + inRgb = qRgb->tryGet(); + inDet = qDet->tryGet(); + } + + counter++; + auto currentTime = steady_clock::now(); + auto elapsed = duration_cast>(currentTime - startTime); + if(elapsed > seconds(1)) { + fps = counter / elapsed.count(); + counter = 0; + startTime = currentTime; + } + + if(inRgb) { + frame = inRgb->getCvFrame(); + std::stringstream fpsStr; + fpsStr << "NN fps: " << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, inRgb->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color2); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!frame.empty()) { + displayFrame("video", frame, detections); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/rgb_mobilenet_4k.cpp b/examples/src/rgb_mobilenet_4k.cpp new file mode 100644 index 000000000..449b79332 --- /dev/null +++ b/examples/src/rgb_mobilenet_4k.cpp @@ -0,0 +1,130 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto nn = pipeline.create(); + + auto xoutVideo = pipeline.create(); + auto xoutPreview = pipeline.create(); + auto nnOut = pipeline.create(); + + xoutVideo->setStreamName("video"); + xoutPreview->setStreamName("preview"); + nnOut->setStreamName("nn"); + + // Properties + camRgb->setPreviewSize(300, 300); // NN input + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_4_K); + camRgb->setInterleaved(false); + camRgb->setPreviewKeepAspectRatio(false); + // Define a neural network that will make predictions based on the source frames + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // Linking + camRgb->video.link(xoutVideo->input); + camRgb->preview.link(xoutPreview->input); + camRgb->preview.link(nn->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the frames and nn data from the outputs defined above + auto qVideo = device.getOutputQueue("video", 4, false); + auto qPreview = device.getOutputQueue("preview", 4, false); + auto qDet = device.getOutputQueue("nn", 4, false); + + cv::Mat previewFrame; + cv::Mat videoFrame; + std::vector detections; + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + cv::namedWindow("video", cv::WINDOW_NORMAL); + cv::resizeWindow("video", 1280, 720); + cout << "Resize video window with mouse drag!" << endl; + + while(true) { + // Instead of get (blocking), we use tryGet (nonblocking) which will return the available data or None otherwise + auto inVideo = qVideo->tryGet(); + auto inPreview = qPreview->tryGet(); + auto inDet = qDet->tryGet(); + + if(inVideo) { + videoFrame = inVideo->getCvFrame(); + } + + if(inPreview) { + previewFrame = inPreview->getCvFrame(); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!videoFrame.empty()) { + displayFrame("video", videoFrame, detections); + } + + if(!previewFrame.empty()) { + displayFrame("preview", previewFrame, detections); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/rgb_mono_encoding.cpp b/examples/src/rgb_mono_encoding.cpp new file mode 100644 index 000000000..b99efe5a0 --- /dev/null +++ b/examples/src/rgb_mono_encoding.cpp @@ -0,0 +1,82 @@ +#include +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// Keyboard interrupt (Ctrl + C) detected +static std::atomic alive{true}; +static void sigintHandler(int signum) { + alive = false; +} + +int main() { + using namespace std; + std::signal(SIGINT, &sigintHandler); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto ve1 = pipeline.create(); + auto ve2 = pipeline.create(); + auto ve3 = pipeline.create(); + + auto ve1Out = pipeline.create(); + auto ve2Out = pipeline.create(); + auto ve3Out = pipeline.create(); + + ve1Out->setStreamName("ve1Out"); + ve2Out->setStreamName("ve2Out"); + ve3Out->setStreamName("ve3Out"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + // Create encoders, one for each camera, consuming the frames and encoding them using H.264 / H.265 encoding + ve1->setDefaultProfilePreset(1280, 720, 30, dai::VideoEncoderProperties::Profile::H264_MAIN); + ve2->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::H265_MAIN); + ve3->setDefaultProfilePreset(1280, 720, 30, dai::VideoEncoderProperties::Profile::H264_MAIN); + + // Linking + monoLeft->out.link(ve1->input); + camRgb->video.link(ve2->input); + monoRight->out.link(ve3->input); + ve1->bitstream.link(ve1Out->input); + ve2->bitstream.link(ve2Out->input); + ve3->bitstream.link(ve3Out->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the encoded data from the output defined above + auto outQ1 = device.getOutputQueue("ve1Out", 30, true); + auto outQ2 = device.getOutputQueue("ve2Out", 30, true); + auto outQ3 = device.getOutputQueue("ve3Out", 30, true); + + // The .h264 / .h265 files are raw stream files (not playable yet) + auto videoFile1 = std::ofstream("mono1.h264", std::ios::binary); + auto videoFile2 = std::ofstream("color.h265", std::ios::binary); + auto videoFile3 = std::ofstream("mono2.h264", std::ios::binary); + cout << "Press Ctrl+C to stop encoding..." << endl; + + while(alive) { + auto out1 = outQ1->get(); + videoFile1.write((char*)out1->getData().data(), out1->getData().size()); + auto out2 = outQ2->get(); + videoFile2.write((char*)out2->getData().data(), out2->getData().size()); + auto out3 = outQ3->get(); + videoFile3.write((char*)out3->getData().data(), out3->getData().size()); + } + + cout << "To view the encoded data, convert the stream file (.h264/.h265) into a video file (.mp4), using a command below:" << endl; + cout << "ffmpeg -framerate 30 -i mono1.h264 -c copy mono1.mp4" << endl; + cout << "ffmpeg -framerate 30 -i mono2.h264 -c copy mono2.mp4" << endl; + cout << "ffmpeg -framerate 30 -i color.h265 -c copy color.mp4" << endl; + + return 0; +} diff --git a/examples/src/rgb_preview.cpp b/examples/src/rgb_preview.cpp new file mode 100644 index 000000000..911f7d1d9 --- /dev/null +++ b/examples/src/rgb_preview.cpp @@ -0,0 +1,55 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +int main() { + using namespace std; + // Create pipeline + dai::Pipeline pipeline; + + // Define source and output + auto camRgb = pipeline.create(); + auto xoutRgb = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + + // Properties + camRgb->setPreviewSize(300, 300); + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::RGB); + + // Linking + camRgb->preview.link(xoutRgb->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + cout << "Connected cameras: "; + for(const auto& cam : device.getConnectedCameras()) { + cout << static_cast(cam) << " "; + cout << cam << " "; + } + cout << endl; + + // Print USB speed + cout << "Usb speed: " << device.getUsbSpeed() << endl; + + // Output queue will be used to get the rgb frames from the output defined above + auto qRgb = device.getOutputQueue("rgb", 4, false); + + while(true) { + auto inRgb = qRgb->get(); + + // Retrieve 'bgr' (opencv format) frame + cv::imshow("rgb", inRgb->getCvFrame()); + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/image_manip_warp_example.cpp b/examples/src/rgb_rotate_warp.cpp similarity index 72% rename from examples/src/image_manip_warp_example.cpp rename to examples/src/rgb_rotate_warp.cpp index 48595fabe..610cff4f8 100644 --- a/examples/src/image_manip_warp_example.cpp +++ b/examples/src/rgb_rotate_warp.cpp @@ -1,24 +1,28 @@ -#include #include #include "depthai/depthai.hpp" #include "utility.hpp" -struct warpFourPointTest { - std::vector points; - bool normalizedCoords; - const char* description; -}; +static constexpr auto keyRotateDecr = 'z'; +static constexpr auto keyRotateIncr = 'x'; +static constexpr auto keyResizeInc = 'v'; +static constexpr auto keyWarpTestCycle = 'c'; + +void printControls() { + printf("\n=== Controls:\n"); + printf(" %c -rotated rectangle crop, decrease rate\n", keyRotateDecr); + printf(" %c -rotated rectangle crop, increase rate\n", keyRotateIncr); + printf(" %c -warp 4-point transform, cycle through modes\n", keyWarpTestCycle); + printf(" %c -resize cropped region, or disable resize\n", keyResizeInc); + printf(" h -print controls (help)\n"); +} static constexpr auto ROTATE_RATE_MAX = 5.0; static constexpr auto ROTATE_RATE_INC = 0.1; -static constexpr auto KEY_ROTATE_DECR = 'z'; -static constexpr auto KEY_ROTATE_INCR = 'x'; static constexpr auto RESIZE_MAX_W = 800; static constexpr auto RESIZE_MAX_H = 600; static constexpr auto RESIZE_FACTOR_MAX = 5; -static constexpr auto KEY_RESIZE_INC = 'v'; /* The crop points are specified in clockwise order, * with first point mapped to output top-left, as: @@ -30,6 +34,11 @@ static const dai::Point2f P0 = {0, 0}; // top-left static const dai::Point2f P1 = {1, 0}; // top-right static const dai::Point2f P2 = {1, 1}; // bottom-right static const dai::Point2f P3 = {0, 1}; // bottom-left +struct warpFourPointTest { + std::vector points; + bool normalizedCoords; + const char* description; +}; std::vector warpList = { //{{{ 0, 0},{ 1, 0},{ 1, 1},{ 0, 1}}, true, "passthrough"}, @@ -44,42 +53,33 @@ std::vector warpList = { {{{-0.3, 0}, {1, 0}, {1.3, 1}, {0, 1}}, true, "8. parallelogram transform"}, {{{-0.2, 0}, {1.8, 0}, {1, 1}, {0, 1}}, true, "9. trapezoid transform"}, }; -static constexpr auto KEY_WARP_TEST_CYCLE = 'c'; - -void printControls() { - printf("\n=== Controls:\n"); - printf(" %c -rotated rectangle crop, decrease rate\n", KEY_ROTATE_DECR); - printf(" %c -rotated rectangle crop, increase rate\n", KEY_ROTATE_INCR); - printf(" %c -warp 4-point transform, cycle through modes\n", KEY_WARP_TEST_CYCLE); - printf(" %c -resize cropped region, or disable resize\n", KEY_RESIZE_INC); - printf(" h -print controls (help)\n"); -} int main() { + // Create pipeline dai::Pipeline pipeline; - auto colorCam = pipeline.create(); - auto camOut = pipeline.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); auto manip = pipeline.create(); - auto manipCfg = pipeline.create(); + + auto camOut = pipeline.create(); auto manipOut = pipeline.create(); + auto manipCfg = pipeline.create(); camOut->setStreamName("preview"); manipOut->setStreamName("manip"); manipCfg->setStreamName("manipCfg"); - colorCam->setPreviewSize(640, 480); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + // Properties + camRgb->setPreviewSize(640, 480); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); manip->setMaxOutputFrameSize(2000 * 1500 * 3); - /* Link nodes: CAM --> XLINK(preview) - * \--> manip -> XLINK(manipOut) - * manipCfg ---/ - */ - colorCam->preview.link(camOut->input); - colorCam->preview.link(manip->inputImage); + // Linking + camRgb->preview.link(camOut->input); + camRgb->preview.link(manip->inputImage); manip->out.link(manipOut->input); manipCfg->out.link(manip->inputConfig); @@ -87,17 +87,19 @@ int main() { dai::Device device(pipeline); // Create input & output queues - auto previewQueue = device.getOutputQueue("preview", 8, false); - auto manipQueue = device.getOutputQueue("manip", 8, false); - auto manipInQueue = device.getInputQueue("manipCfg"); + auto qPreview = device.getOutputQueue("preview", 8, false); + auto qManip = device.getOutputQueue("manip", 8, false); + auto qManipCfg = device.getInputQueue("manipCfg"); - std::vector frameQueues{previewQueue, manipQueue}; + std::vector frameQueues{qPreview, qManip}; // keep processing data int key = -1; float angleDeg = 0; float rotateRate = 1.0; - int resizeFactor = 0, resizeX, resizeY; + int resizeFactor = 0; + int resizeX = 0; + int resizeY = 0; bool testFourPt = false; int warpIdx = -1; @@ -106,15 +108,15 @@ int main() { while(key != 'q') { if(key >= 0) { printf("Pressed: %c | ", key); - if(key == KEY_ROTATE_DECR || key == KEY_ROTATE_INCR) { - if(key == KEY_ROTATE_DECR) { + if(key == keyRotateDecr || key == keyRotateIncr) { + if(key == keyRotateDecr) { if(rotateRate > -ROTATE_RATE_MAX) rotateRate -= ROTATE_RATE_INC; - } else if(key == KEY_ROTATE_INCR) { + } else if(key == keyRotateIncr) { if(rotateRate < ROTATE_RATE_MAX) rotateRate += ROTATE_RATE_INC; } testFourPt = false; printf("Crop rotated rectangle, rate: %g degrees", rotateRate); - } else if(key == KEY_RESIZE_INC) { + } else if(key == keyResizeInc) { resizeFactor++; if(resizeFactor > RESIZE_FACTOR_MAX) { resizeFactor = 0; @@ -124,10 +126,10 @@ int main() { resizeY = RESIZE_MAX_H / resizeFactor; printf("Crop region resized to: %d x %d", resizeX, resizeY); } - } else if(key == KEY_WARP_TEST_CYCLE) { + } else if(key == keyWarpTestCycle) { resizeFactor = 0; // Disable resizing initially warpIdx = (warpIdx + 1) % warpList.size(); - printf("Warp 4-point transform, %s", warpList[warpIdx].description); + printf("Warp 4-point transform: %s", warpList[warpIdx].description); testFourPt = true; } else if(key == 'h') { printControls(); @@ -145,15 +147,14 @@ int main() { dai::RotatedRect rr = {{320, 240}, // center {640, 480}, //{400, 400}, // size angleDeg}; - bool normalized = false; - cfg.setCropRotatedRect(rr, normalized); + cfg.setCropRotatedRect(rr, false); } if(resizeFactor > 0) { cfg.setResize(resizeX, resizeY); } // cfg.setWarpBorderFillColor(255, 0, 0); // cfg.setWarpBorderReplicatePixels(); - manipInQueue->send(cfg); + qManipCfg->send(cfg); } for(const auto& q : frameQueues) { @@ -163,4 +164,5 @@ int main() { } key = cv::waitKey(1); } + return 0; } diff --git a/examples/src/rgb_video.cpp b/examples/src/rgb_video.cpp new file mode 100644 index 000000000..ecb349e4d --- /dev/null +++ b/examples/src/rgb_video.cpp @@ -0,0 +1,45 @@ +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +int main() { + // Create pipeline + dai::Pipeline pipeline; + + // Define source and output + auto camRgb = pipeline.create(); + auto xoutVideo = pipeline.create(); + + xoutVideo->setStreamName("video"); + + // Properties + camRgb->setBoardSocket(dai::CameraBoardSocket::RGB); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setVideoSize(1920, 1080); + + xoutVideo->input.setBlocking(false); + xoutVideo->input.setQueueSize(1); + + // Linking + camRgb->video.link(xoutVideo->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + auto video = device.getOutputQueue("video"); + + while(true) { + auto videoIn = video->get(); + + // Get BGR frame from NV12 encoded video frame to show with opencv + // Visualizing the frame on slower hosts might have overhead + cv::imshow("video", videoIn->getCvFrame()); + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/spatial_location_calculator_example.cpp b/examples/src/spatial_location_calculator.cpp similarity index 79% rename from examples/src/spatial_location_calculator_example.cpp rename to examples/src/spatial_location_calculator.cpp index dc0ba33ac..05137bfef 100644 --- a/examples/src/spatial_location_calculator_example.cpp +++ b/examples/src/spatial_location_calculator.cpp @@ -1,6 +1,3 @@ - - -#include #include #include "utility.hpp" @@ -10,26 +7,29 @@ static constexpr float stepSize = 0.05; +static std::atomic newConfig{false}; + int main() { using namespace std; - dai::Pipeline p; + // Create pipeline + dai::Pipeline pipeline; - auto monoLeft = p.create(); - auto monoRight = p.create(); - auto stereo = p.create(); - auto spatialDataCalculator = p.create(); + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto stereo = pipeline.create(); + auto spatialDataCalculator = pipeline.create(); - auto xoutDepth = p.create(); - auto xoutSpatialData = p.create(); - auto xinSpatialCalcConfig = p.create(); + auto xoutDepth = pipeline.create(); + auto xoutSpatialData = pipeline.create(); + auto xinSpatialCalcConfig = pipeline.create(); - // XLinkOut xoutDepth->setStreamName("depth"); xoutSpatialData->setStreamName("spatialData"); xinSpatialCalcConfig->setStreamName("spatialCalcConfig"); - // MonoCamera + // Properties monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); @@ -38,47 +38,48 @@ int main() { bool lrcheck = false; bool subpixel = false; - // StereoDepth stereo->setConfidenceThreshold(255); - - // stereo->setMedianFilter(dai::StereoDepthProperties::MedianFilter::MEDIAN_OFF); stereo->setLeftRightCheck(lrcheck); stereo->setSubpixel(subpixel); - // Link plugins CAM -> STEREO -> XLINK - monoLeft->out.link(stereo->left); - monoRight->out.link(stereo->right); - - spatialDataCalculator->passthroughDepth.link(xoutDepth->input); - stereo->depth.link(spatialDataCalculator->inputDepth); - + // Config dai::Point2f topLeft(0.4f, 0.4f); dai::Point2f bottomRight(0.6f, 0.6f); - spatialDataCalculator->setWaitForConfigInput(false); dai::SpatialLocationCalculatorConfigData config; config.depthThresholds.lowerThreshold = 100; config.depthThresholds.upperThreshold = 10000; config.roi = dai::Rect(topLeft, bottomRight); + + spatialDataCalculator->setWaitForConfigInput(false); spatialDataCalculator->initialConfig.addROI(config); + + // Linking + monoLeft->out.link(stereo->left); + monoRight->out.link(stereo->right); + + spatialDataCalculator->passthroughDepth.link(xoutDepth->input); + stereo->depth.link(spatialDataCalculator->inputDepth); + spatialDataCalculator->out.link(xoutSpatialData->input); xinSpatialCalcConfig->out.link(spatialDataCalculator->inputConfig); - // Connect and start the pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); - auto depthQueue = d.getOutputQueue("depth", 8, false); - auto spatialCalcQueue = d.getOutputQueue("spatialData", 8, false); - auto spatialCalcConfigInQueue = d.getInputQueue("spatialCalcConfig"); + // Output queue will be used to get the depth frames from the outputs defined above + auto depthQueue = device.getOutputQueue("depth", 8, false); + auto spatialCalcQueue = device.getOutputQueue("spatialData", 8, false); + auto spatialCalcConfigInQueue = device.getInputQueue("spatialCalcConfig"); - cv::Mat depthFrame; auto color = cv::Scalar(255, 255, 255); + std::cout << "Use WASD keys to move ROI!" << std::endl; - while(1) { - auto depth = depthQueue->get(); + while(true) { + auto inDepth = depthQueue->get(); - cv::Mat depthFrame = depth->getFrame(); + cv::Mat depthFrame = inDepth->getFrame(); cv::Mat depthFrameColor; cv::normalize(depthFrame, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1); cv::equalizeHist(depthFrameColor, depthFrameColor); @@ -104,14 +105,12 @@ int main() { depthZ << "Z: " << (int)depthData.spatialCoordinates.z << " mm"; cv::putText(depthFrameColor, depthZ.str(), cv::Point(xmin + 10, ymin + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); } - + // Show the frame cv::imshow("depth", depthFrameColor); - bool newConfig = false; int key = cv::waitKey(1); switch(key) { case 'q': - return 0; break; case 'w': if(topLeft.y - stepSize >= 0) { @@ -144,11 +143,14 @@ int main() { default: break; } + if(newConfig) { config.roi = dai::Rect(topLeft, bottomRight); dai::SpatialLocationCalculatorConfig cfg; cfg.addROI(config); spatialCalcConfigInQueue->send(cfg); + newConfig = false; } } + return 0; } diff --git a/examples/src/spatial_mobilenet_example.cpp b/examples/src/spatial_mobilenet.cpp similarity index 61% rename from examples/src/spatial_mobilenet_example.cpp rename to examples/src/spatial_mobilenet.cpp index fd88e3b22..ba72f1ad2 100644 --- a/examples/src/spatial_mobilenet_example.cpp +++ b/examples/src/spatial_mobilenet.cpp @@ -1,9 +1,6 @@ #include -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" @@ -11,40 +8,53 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; -static bool syncNN = true; +static std::atomic syncNN{true}; + +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; + // Create pipeline + dai::Pipeline pipeline; - // create nodes - auto colorCam = p.create(); - auto spatialDetectionNetwork = p.create(); - auto monoLeft = p.create(); - auto monoRight = p.create(); - auto stereo = p.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto stereo = pipeline.create(); - // create xlink connections - auto xoutRgb = p.create(); - auto xoutNN = p.create(); - auto xoutBoundingBoxDepthMapping = p.create(); - auto xoutDepth = p.create(); + auto xoutRgb = pipeline.create(); + auto xoutNN = pipeline.create(); + auto xoutBoundingBoxDepthMapping = pipeline.create(); + auto xoutDepth = pipeline.create(); - xoutRgb->setStreamName("preview"); + xoutRgb->setStreamName("rgb"); xoutNN->setStreamName("detections"); xoutBoundingBoxDepthMapping->setStreamName("boundingBoxDepthMapping"); xoutDepth->setStreamName("depth"); - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + // Properties + camRgb->setPreviewSize(300, 300); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); - /// setting node configs + // Setting node configs stereo->setConfidenceThreshold(255); spatialDetectionNetwork->setBlobPath(nnPath); @@ -54,16 +64,15 @@ dai::Pipeline createNNPipeline(std::string nnPath) { spatialDetectionNetwork->setDepthLowerThreshold(100); spatialDetectionNetwork->setDepthUpperThreshold(5000); - // Link plugins CAM -> STEREO -> XLINK + // Linking monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(spatialDetectionNetwork->input); + camRgb->preview.link(spatialDetectionNetwork->input); if(syncNN) { spatialDetectionNetwork->passthrough.link(xoutRgb->input); } else { - colorCam->preview.link(xoutRgb->input); + camRgb->preview.link(xoutRgb->input); } spatialDetectionNetwork->out.link(xoutNN->input); @@ -72,53 +81,44 @@ dai::Pipeline createNNPipeline(std::string nnPath) { stereo->depth.link(spatialDetectionNetwork->inputDepth); spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); + // Connect to device and start pipeline + dai::Device device(pipeline); - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); - - auto preview = d.getOutputQueue("preview", 4, false); - auto detections = d.getOutputQueue("detections", 4, false); - auto xoutBoundingBoxDepthMapping = d.getOutputQueue("boundingBoxDepthMapping", 4, false); - auto depthQueue = d.getOutputQueue("depth", 4, false); + auto previewQueue = device.getOutputQueue("rgb", 4, false); + auto detectionNNQueue = device.getOutputQueue("detections", 4, false); + auto xoutBoundingBoxDepthMappingQueue = device.getOutputQueue("boundingBoxDepthMapping", 4, false); + auto depthQueue = device.getOutputQueue("depth", 4, false); auto startTime = steady_clock::now(); int counter = 0; float fps = 0; auto color = cv::Scalar(255, 255, 255); - while(1) { - auto imgFrame = preview->get(); - auto det = detections->get(); + while(true) { + auto inPreview = previewQueue->get(); + auto inDet = detectionNNQueue->get(); auto depth = depthQueue->get(); - auto dets = det->detections; + counter++; + auto currentTime = steady_clock::now(); + auto elapsed = duration_cast>(currentTime - startTime); + if(elapsed > seconds(1)) { + fps = counter / elapsed.count(); + counter = 0; + startTime = currentTime; + } + cv::Mat frame = inPreview->getCvFrame(); cv::Mat depthFrame = depth->getFrame(); + 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); - if(!dets.empty()) { - auto boundingBoxMapping = xoutBoundingBoxDepthMapping->get(); + auto detections = inDet->detections; + if(!detections.empty()) { + auto boundingBoxMapping = xoutBoundingBoxDepthMappingQueue->get(); auto roiDatas = boundingBoxMapping->getConfigData(); for(auto roiData : roiDatas) { @@ -134,57 +134,47 @@ int main(int argc, char** argv) { cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, cv::FONT_HERSHEY_SIMPLEX); } } - counter++; - auto currentTime = steady_clock::now(); - auto elapsed = duration_cast>(currentTime - startTime); - if(elapsed > seconds(1)) { - fps = counter / elapsed.count(); - counter = 0; - startTime = currentTime; - } - cv::Mat frame = imgFrame->getCvFrame(); + for(const auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; - for(const auto& d : dets) { - int x1 = d.xmin * frame.cols; - int y1 = d.ymin * frame.rows; - int x2 = d.xmax * frame.cols; - int y2 = d.ymax * frame.rows; - - int labelIndex = d.label; + int labelIndex = detection.label; std::string labelStr = 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, color); + 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) << d.confidence * 100; - cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + 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: " << (int)d.spatialCoordinates.x << " mm"; - cv::putText(frame, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthX << "X: " << (int)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: " << (int)d.spatialCoordinates.y << " mm"; - cv::putText(frame, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthY << "Y: " << (int)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: " << (int)d.spatialCoordinates.z << " mm"; - cv::putText(frame, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthZ << "Z: " << (int)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, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); + fpsStr << "NN fps: " << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, inPreview->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); cv::imshow("depth", depthFrameColor); cv::imshow("preview", frame); + int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } - return 0; } diff --git a/examples/src/spatial_mobilenet_mono_example.cpp b/examples/src/spatial_mobilenet_mono.cpp similarity index 60% rename from examples/src/spatial_mobilenet_mono_example.cpp rename to examples/src/spatial_mobilenet_mono.cpp index 3eaa53581..960d14207 100644 --- a/examples/src/spatial_mobilenet_mono_example.cpp +++ b/examples/src/spatial_mobilenet_mono.cpp @@ -1,9 +1,6 @@ #include -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" @@ -11,32 +8,47 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; -static bool syncNN = true; -static bool flipRectified = true; +static std::atomic syncNN{true}; +static std::atomic flipRectified{true}; -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); - auto xlinkOut = p.create(); - auto spatialDetectionNetwork = p.create(); - auto imageManip = p.create(); + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); - auto nnOut = p.create(); - auto depthRoiMap = p.create(); - auto xoutDepth = p.create(); + // Create pipeline + dai::Pipeline pipeline; - xlinkOut->setStreamName("preview"); + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto stereo = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto imageManip = pipeline.create(); + + auto xoutManip = pipeline.create(); + auto nnOut = pipeline.create(); + auto depthRoiMap = pipeline.create(); + auto xoutDepth = pipeline.create(); + + xoutManip->setStreamName("right"); nnOut->setStreamName("detections"); depthRoiMap->setStreamName("boundingBoxDepthMapping"); xoutDepth->setStreamName("depth"); + // Properties imageManip->initialConfig.setResize(300, 300); // The NN model expects BGR input. By default ImageManip output type would be same as input (gray in this case) - imageManip->initialConfig.setFrameType(dai::RawImgFrame::Type::BGR888p); + imageManip->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); - auto monoLeft = p.create(); - auto monoRight = p.create(); - auto stereo = p.create(); monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); @@ -45,70 +57,50 @@ dai::Pipeline createNNPipeline(std::string nnPath) { // StereoDepth stereo->setConfidenceThreshold(255); - // Link plugins CAM -> STEREO -> XLINK - monoLeft->out.link(stereo->left); - monoRight->out.link(stereo->right); - - stereo->rectifiedRight.link(imageManip->inputImage); - - // testing MobileNet DetectionNetwork + // Define a neural network that will make predictions based on the source frames spatialDetectionNetwork->setConfidenceThreshold(0.5f); - spatialDetectionNetwork->setBoundingBoxScaleFactor(0.7); + spatialDetectionNetwork->setBlobPath(nnPath); + spatialDetectionNetwork->input.setBlocking(false); + spatialDetectionNetwork->setBoundingBoxScaleFactor(0.5); spatialDetectionNetwork->setDepthLowerThreshold(100); spatialDetectionNetwork->setDepthUpperThreshold(5000); - spatialDetectionNetwork->setBlobPath(nnPath); + // Linking + monoLeft->out.link(stereo->left); + monoRight->out.link(stereo->right); - // Link plugins CAM -> NN -> XLINK imageManip->out.link(spatialDetectionNetwork->input); if(syncNN) { - spatialDetectionNetwork->passthrough.link(xlinkOut->input); + spatialDetectionNetwork->passthrough.link(xoutManip->input); } else { - imageManip->out.link(xlinkOut->input); + imageManip->out.link(xoutManip->input); } spatialDetectionNetwork->out.link(nnOut->input); spatialDetectionNetwork->boundingBoxMapping.link(depthRoiMap->input); + stereo->rectifiedRight.link(imageManip->inputImage); stereo->depth.link(spatialDetectionNetwork->inputDepth); spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); - auto preview = d.getOutputQueue("preview", 4, false); - auto detections = d.getOutputQueue("detections", 4, false); - auto depthRoiMap = d.getOutputQueue("boundingBoxDepthMapping", 4, false); - auto depthQueue = d.getOutputQueue("depth", 4, false); + // Output queues will be used to get the rgb frames and nn data from the outputs defined above + auto previewQueue = device.getOutputQueue("right", 4, false); + auto detectionNNQueue = device.getOutputQueue("detections", 4, false); + auto depthRoiMapQueue = device.getOutputQueue("boundingBoxDepthMapping", 4, false); + auto depthQueue = device.getOutputQueue("depth", 4, false); auto startTime = steady_clock::now(); int counter = 0; float fps = 0; auto color = cv::Scalar(255, 255, 255); - while(1) { - auto inRectifiedRight = preview->get(); - auto det = detections->get(); - auto depth = depthQueue->get(); + while(true) { + auto inRectified = previewQueue->get(); + auto inDet = detectionNNQueue->get(); + auto inDepth = depthQueue->get(); counter++; auto currentTime = steady_clock::now(); @@ -119,16 +111,18 @@ int main(int argc, char** argv) { startTime = currentTime; } - auto dets = det->detections; + cv::Mat rectifiedRight = inRectified->getCvFrame(); + if(flipRectified) cv::flip(rectifiedRight, rectifiedRight, 1); - cv::Mat depthFrame = depth->getFrame(); + cv::Mat depthFrame = inDepth->getFrame(); 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); - if(!dets.empty()) { - auto boundingBoxMapping = depthRoiMap->get(); + auto detections = inDet->detections; + if(!detections.empty()) { + auto boundingBoxMapping = depthRoiMapQueue->get(); auto roiDatas = boundingBoxMapping->getConfigData(); for(auto roiData : roiDatas) { @@ -140,45 +134,40 @@ int main(int argc, char** argv) { auto ymin = (int)topLeft.y; auto xmax = (int)bottomRight.x; auto ymax = (int)bottomRight.y; - cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, cv::FONT_HERSHEY_SIMPLEX); } } - cv::Mat rectifiedRight = inRectifiedRight->getCvFrame(); - - if(flipRectified) cv::flip(rectifiedRight, rectifiedRight, 1); - - for(auto& d : dets) { + for(auto& detection : detections) { if(flipRectified) { - auto swap = d.xmin; - d.xmin = 1 - d.xmax; - d.xmax = 1 - swap; + auto swap = detection.xmin; + detection.xmin = 1 - detection.xmax; + detection.xmax = 1 - swap; } - int x1 = d.xmin * rectifiedRight.cols; - int y1 = d.ymin * rectifiedRight.rows; - int x2 = d.xmax * rectifiedRight.cols; - int y2 = d.ymax * rectifiedRight.rows; + int x1 = detection.xmin * rectifiedRight.cols; + int y1 = detection.ymin * rectifiedRight.rows; + int x2 = detection.xmax * rectifiedRight.cols; + int y2 = detection.ymax * rectifiedRight.rows; - int labelIndex = d.label; + int labelIndex = detection.label; std::string labelStr = to_string(labelIndex); if(labelIndex < labelMap.size()) { labelStr = labelMap[labelIndex]; } - cv::putText(rectifiedRight, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(rectifiedRight, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream confStr; - confStr << std::fixed << std::setprecision(2) << d.confidence * 100; - cv::putText(rectifiedRight, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(rectifiedRight, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream depthX; - depthX << "X: " << (int)d.spatialCoordinates.x << " mm"; - cv::putText(rectifiedRight, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthX << "X: " << (int)detection.spatialCoordinates.x << " mm"; + cv::putText(rectifiedRight, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream depthY; - depthY << "Y: " << (int)d.spatialCoordinates.y << " mm"; - cv::putText(rectifiedRight, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthY << "Y: " << (int)detection.spatialCoordinates.y << " mm"; + cv::putText(rectifiedRight, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream depthZ; - depthZ << "Z: " << (int)d.spatialCoordinates.z << " mm"; - cv::putText(rectifiedRight, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthZ << "Z: " << (int)detection.spatialCoordinates.z << " mm"; + cv::putText(rectifiedRight, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); cv::rectangle(rectifiedRight, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); } @@ -189,11 +178,11 @@ int main(int argc, char** argv) { cv::imshow("depth", depthFrameColor); cv::imshow("rectified right", rectifiedRight); + int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } - return 0; } diff --git a/examples/src/spatial_object_tracker_example.cpp b/examples/src/spatial_object_tracker.cpp similarity index 67% rename from examples/src/spatial_object_tracker_example.cpp rename to examples/src/spatial_object_tracker.cpp index b7195495f..b54ceca87 100644 --- a/examples/src/spatial_object_tracker_example.cpp +++ b/examples/src/spatial_object_tracker.cpp @@ -1,6 +1,4 @@ #include -#include -#include #include "utility.hpp" @@ -11,30 +9,43 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; -static bool syncNN = true; +static std::atomic fullFrameTracking{false}; -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } - // create nodes - auto colorCam = p.create(); - auto spatialDetectionNetwork = p.create(); - auto monoLeft = p.create(); - auto monoRight = p.create(); - auto stereo = p.create(); - auto objectTracker = p.create(); + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); - // create xlink connections - auto xoutRgb = p.create(); - auto trackerOut = p.create(); + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto stereo = pipeline.create(); + auto objectTracker = pipeline.create(); + + auto xoutRgb = pipeline.create(); + auto trackerOut = pipeline.create(); xoutRgb->setStreamName("preview"); trackerOut->setStreamName("tracklets"); - colorCam->setPreviewSize(300, 300); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + // Properties + camRgb->setPreviewSize(300, 300); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); @@ -51,60 +62,46 @@ dai::Pipeline createNNPipeline(std::string nnPath) { spatialDetectionNetwork->setDepthLowerThreshold(100); spatialDetectionNetwork->setDepthUpperThreshold(5000); - // Link plugins CAM -> STEREO -> XLINK + objectTracker->setDetectionLabelsToTrack({15}); // track only person + // possible tracking types: ZERO_TERM_COLOR_HISTOGRAM, ZERO_TERM_IMAGELESS + objectTracker->setTrackerType(dai::TrackerType::ZERO_TERM_COLOR_HISTOGRAM); + // take the smallest ID when new object is tracked, possible options: SMALLEST_ID, UNIQUE_ID + objectTracker->setTrackerIdAssigmentPolicy(dai::TrackerIdAssigmentPolicy::SMALLEST_ID); + + // Linking monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(spatialDetectionNetwork->input); - if(syncNN) { - objectTracker->passthroughTrackerFrame.link(xoutRgb->input); + camRgb->preview.link(spatialDetectionNetwork->input); + objectTracker->passthroughTrackerFrame.link(xoutRgb->input); + objectTracker->out.link(trackerOut->input); + + if(fullFrameTracking) { + camRgb->setPreviewKeepAspectRatio(false); + camRgb->video.link(objectTracker->inputTrackerFrame); + objectTracker->inputTrackerFrame.setBlocking(false); + // do not block the pipeline if it's too slow on full frame + objectTracker->inputTrackerFrame.setQueueSize(2); } else { - colorCam->preview.link(xoutRgb->input); + spatialDetectionNetwork->passthrough.link(objectTracker->inputTrackerFrame); } - objectTracker->setDetectionLabelsToTrack({15}); // track only person - objectTracker->setTrackerType(dai::TrackerType::ZERO_TERM_COLOR_HISTOGRAM); - objectTracker->setTrackerIdAssigmentPolicy(dai::TrackerIdAssigmentPolicy::SMALLEST_ID); - objectTracker->out.link(trackerOut->input); - - spatialDetectionNetwork->passthrough.link(objectTracker->inputTrackerFrame); spatialDetectionNetwork->passthrough.link(objectTracker->inputDetectionFrame); spatialDetectionNetwork->out.link(objectTracker->inputDetections); - stereo->depth.link(spatialDetectionNetwork->inputDepth); - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); + // Connect to device and start pipeline + dai::Device device(pipeline); - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); - - auto preview = d.getOutputQueue("preview", 4, false); - auto tracklets = d.getOutputQueue("tracklets", 4, false); + auto preview = device.getOutputQueue("preview", 4, false); + auto tracklets = device.getOutputQueue("tracklets", 4, false); auto startTime = steady_clock::now(); int counter = 0; float fps = 0; - auto color = cv::Scalar(255, 0, 0); + auto color = cv::Scalar(255, 255, 255); - while(1) { + while(true) { auto imgFrame = preview->get(); auto track = tracklets->get(); @@ -131,38 +128,38 @@ int main(int argc, char** argv) { if(labelIndex < labelMap.size()) { labelStr = labelMap[labelIndex]; } - cv::putText(frame, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream idStr; idStr << "ID: " << t.id; - cv::putText(frame, idStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, idStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream statusStr; statusStr << "Status: " << t.status; - cv::putText(frame, statusStr.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, statusStr.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream depthX; depthX << "X: " << (int)t.spatialCoordinates.x << " mm"; - cv::putText(frame, depthX.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, depthX.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream depthY; depthY << "Y: " << (int)t.spatialCoordinates.y << " mm"; - cv::putText(frame, depthY.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, depthY.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); std::stringstream depthZ; depthZ << "Z: " << (int)t.spatialCoordinates.z << " mm"; - cv::putText(frame, depthZ.str(), cv::Point(x1 + 10, y1 + 95), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::putText(frame, depthZ.str(), cv::Point(x1 + 10, y1 + 95), 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; + fpsStr << "NN fps: " << std::fixed << std::setprecision(2) << fps; cv::putText(frame, fpsStr.str(), cv::Point(2, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); cv::imshow("tracker", frame); + int key = cv::waitKey(1); if(key == 'q') { return 0; } } - return 0; } diff --git a/examples/src/spatial_tiny_yolo_example.cpp b/examples/src/spatial_tiny_yolo.cpp similarity index 68% rename from examples/src/spatial_tiny_yolo_example.cpp rename to examples/src/spatial_tiny_yolo.cpp index d320662dd..5371c5442 100644 --- a/examples/src/spatial_tiny_yolo_example.cpp +++ b/examples/src/spatial_tiny_yolo.cpp @@ -1,6 +1,4 @@ #include -#include -#include #include "utility.hpp" @@ -18,33 +16,46 @@ static const std::vector labelMap = { "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; -static bool syncNN = true; +static std::atomic syncNN{true}; -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; - // create nodes - auto colorCam = p.create(); - auto spatialDetectionNetwork = p.create(); - auto monoLeft = p.create(); - auto monoRight = p.create(); - auto stereo = p.create(); + // Define sources and outputs + auto camRgb = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto stereo = pipeline.create(); - // create xlink connections - auto xoutRgb = p.create(); - auto xoutNN = p.create(); - auto xoutBoundingBoxDepthMapping = p.create(); - auto xoutDepth = p.create(); + auto xoutRgb = pipeline.create(); + auto xoutNN = pipeline.create(); + auto xoutBoundingBoxDepthMapping = pipeline.create(); + auto xoutDepth = pipeline.create(); - xoutRgb->setStreamName("preview"); + xoutRgb->setStreamName("rgb"); xoutNN->setStreamName("detections"); xoutBoundingBoxDepthMapping->setStreamName("boundingBoxDepthMapping"); xoutDepth->setStreamName("depth"); - colorCam->setPreviewSize(416, 416); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + // Properties + camRgb->setPreviewSize(416, 416); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); @@ -65,19 +76,18 @@ dai::Pipeline createNNPipeline(std::string nnPath) { spatialDetectionNetwork->setNumClasses(80); spatialDetectionNetwork->setCoordinateSize(4); spatialDetectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); - spatialDetectionNetwork->setAnchorMasks({{"side13", {3, 4, 5}}, {"side26", {1, 2, 3}}}); + spatialDetectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}}); spatialDetectionNetwork->setIouThreshold(0.5f); - // Link plugins CAM -> STEREO -> XLINK + // Linking monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(spatialDetectionNetwork->input); + camRgb->preview.link(spatialDetectionNetwork->input); if(syncNN) { spatialDetectionNetwork->passthrough.link(xoutRgb->input); } else { - colorCam->preview.link(xoutRgb->input); + camRgb->preview.link(xoutRgb->input); } spatialDetectionNetwork->out.link(xoutNN->input); @@ -86,53 +96,44 @@ dai::Pipeline createNNPipeline(std::string nnPath) { stereo->depth.link(spatialDetectionNetwork->inputDepth); spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); - auto preview = d.getOutputQueue("preview", 4, false); - auto detections = d.getOutputQueue("detections", 4, false); - auto xoutBoundingBoxDepthMapping = d.getOutputQueue("boundingBoxDepthMapping", 4, false); - auto depthQueue = d.getOutputQueue("depth", 4, false); + // Output queues will be used to get the rgb frames and nn data from the outputs defined above + auto previewQueue = device.getOutputQueue("rgb", 4, false); + auto detectionNNQueue = device.getOutputQueue("detections", 4, false); + auto xoutBoundingBoxDepthMappingQueue = device.getOutputQueue("boundingBoxDepthMapping", 4, false); + auto depthQueue = device.getOutputQueue("depth", 4, false); auto startTime = steady_clock::now(); int counter = 0; float fps = 0; auto color = cv::Scalar(255, 255, 255); - while(1) { - auto imgFrame = preview->get(); - auto det = detections->get(); + while(true) { + auto imgFrame = previewQueue->get(); + auto inDet = detectionNNQueue->get(); auto depth = depthQueue->get(); - auto dets = det->detections; - + cv::Mat frame = imgFrame->getCvFrame(); cv::Mat depthFrame = depth->getFrame(); 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); - if(!dets.empty()) { - auto boundingBoxMapping = xoutBoundingBoxDepthMapping->get(); + counter++; + auto currentTime = steady_clock::now(); + auto elapsed = duration_cast>(currentTime - startTime); + if(elapsed > seconds(1)) { + fps = counter / elapsed.count(); + counter = 0; + startTime = currentTime; + } + + auto detections = inDet->detections; + if(!detections.empty()) { + auto boundingBoxMapping = xoutBoundingBoxDepthMappingQueue->get(); auto roiDatas = boundingBoxMapping->getConfigData(); for(auto roiData : roiDatas) { @@ -148,42 +149,32 @@ int main(int argc, char** argv) { cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, cv::FONT_HERSHEY_SIMPLEX); } } - counter++; - auto currentTime = steady_clock::now(); - auto elapsed = duration_cast>(currentTime - startTime); - if(elapsed > seconds(1)) { - fps = counter / elapsed.count(); - counter = 0; - startTime = currentTime; - } - - cv::Mat frame = imgFrame->getCvFrame(); - for(const auto& d : dets) { - int x1 = d.xmin * frame.cols; - int y1 = d.ymin * frame.rows; - int x2 = d.xmax * frame.cols; - int y2 = d.ymax * frame.rows; + for(const auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; - int labelIndex = d.label; + int labelIndex = detection.label; std::string labelStr = 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, color); + 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) << d.confidence * 100; - cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + 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: " << (int)d.spatialCoordinates.x << " mm"; - cv::putText(frame, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthX << "X: " << (int)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: " << (int)d.spatialCoordinates.y << " mm"; - cv::putText(frame, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthY << "Y: " << (int)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: " << (int)d.spatialCoordinates.z << " mm"; - cv::putText(frame, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + depthZ << "Z: " << (int)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); } @@ -193,12 +184,12 @@ int main(int argc, char** argv) { cv::putText(frame, fpsStr.str(), cv::Point(2, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); cv::imshow("depth", depthFrameColor); - cv::imshow("preview", frame); + cv::imshow("rgb", frame); + int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } - return 0; } diff --git a/examples/src/stereo_example.cpp b/examples/src/stereo_depth_video.cpp similarity index 70% rename from examples/src/stereo_example.cpp rename to examples/src/stereo_depth_video.cpp index 10a4ed60b..712a18b0a 100644 --- a/examples/src/stereo_example.cpp +++ b/examples/src/stereo_depth_video.cpp @@ -1,30 +1,27 @@ - - -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" +static std::atomic withDepth{true}; + int main() { using namespace std; - using namespace std::chrono; - // TODO - split this example into two separate examples - bool withDepth = true; - - dai::Pipeline p; - - auto monoLeft = p.create(); - auto monoRight = p.create(); - auto xoutLeft = p.create(); - auto xoutRight = p.create(); - auto stereo = withDepth ? p.create() : nullptr; - auto xoutDisp = p.create(); - auto xoutDepth = p.create(); - auto xoutRectifL = p.create(); - auto xoutRectifR = p.create(); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto stereo = withDepth ? pipeline.create() : nullptr; + + auto xoutLeft = pipeline.create(); + auto xoutRight = pipeline.create(); + auto xoutDisp = pipeline.create(); + auto xoutDepth = pipeline.create(); + auto xoutRectifL = pipeline.create(); + auto xoutRectifR = pipeline.create(); // XLinkOut xoutLeft->setStreamName("left"); @@ -44,11 +41,11 @@ int main() { monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); // monoRight->setFps(5.0); - bool outputDepth = false; - bool outputRectified = true; - bool lrcheck = true; - bool extended = false; - bool subpixel = true; + std::atomic outputDepth{false}; + std::atomic outputRectified{true}; + std::atomic lrcheck{true}; + std::atomic extended{false}; + std::atomic subpixel{false}; if(withDepth) { // StereoDepth @@ -57,12 +54,12 @@ int main() { // stereo->loadCalibrationFile("../../../../depthai/resources/depthai.calib"); // stereo->setInputResolution(1280, 720); // TODO: median filtering is disabled on device with (lrcheck || extended || subpixel) - // stereo->setMedianFilter(dai::StereoDepthProperties::MedianFilter::MEDIAN_OFF); + stereo->setMedianFilter(dai::StereoDepthProperties::MedianFilter::MEDIAN_OFF); stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); stereo->setSubpixel(subpixel); - // Link plugins CAM -> STEREO -> XLINK + // Linking monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); @@ -83,17 +80,17 @@ int main() { monoRight->out.link(xoutRight->input); } - // Connect and start the pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); - auto leftQueue = d.getOutputQueue("left", 8, false); - auto rightQueue = d.getOutputQueue("right", 8, false); - auto dispQueue = withDepth ? d.getOutputQueue("disparity", 8, false) : nullptr; - auto depthQueue = withDepth ? d.getOutputQueue("depth", 8, false) : nullptr; - auto rectifLeftQueue = withDepth ? d.getOutputQueue("rectified_left", 8, false) : nullptr; - auto rectifRightQueue = withDepth ? d.getOutputQueue("rectified_right", 8, false) : nullptr; + auto leftQueue = device.getOutputQueue("left", 8, false); + auto rightQueue = device.getOutputQueue("right", 8, false); + auto dispQueue = withDepth ? device.getOutputQueue("disparity", 8, false) : nullptr; + auto depthQueue = withDepth ? device.getOutputQueue("depth", 8, false) : nullptr; + auto rectifLeftQueue = withDepth ? device.getOutputQueue("rectified_left", 8, false) : nullptr; + auto rectifRightQueue = withDepth ? device.getOutputQueue("rectified_right", 8, false) : nullptr; - while(1) { + while(true) { auto left = leftQueue->get(); cv::imshow("left", cv::Mat(left->getHeight(), left->getWidth(), CV_8UC1, left->getData().data())); auto right = rightQueue->get(); @@ -129,8 +126,9 @@ int main() { } int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } + return 0; } diff --git a/examples/src/system_information_example.cpp b/examples/src/system_information.cpp similarity index 65% rename from examples/src/system_information_example.cpp rename to examples/src/system_information.cpp index 5a4e67524..be4a11713 100644 --- a/examples/src/system_information_example.cpp +++ b/examples/src/system_information.cpp @@ -1,62 +1,47 @@ - - -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" - -void printSystemInformation(dai::SystemInformation); +void printSystemInformation(dai::SystemInformation info) { + printf("Ddr used / total - %.2f / %.2f MiB\n", info.ddrMemoryUsage.used / (1024.0f * 1024.0f), info.ddrMemoryUsage.total / (1024.0f * 1024.0f)); + printf("Cmx used / total - %.2f / %.2f MiB\n", info.cmxMemoryUsage.used / (1024.0f * 1024.0f), info.cmxMemoryUsage.total / (1024.0f * 1024.0f)); + printf("LeonCss heap used / total - %.2f / %.2f MiB\n", + info.leonCssMemoryUsage.used / (1024.0f * 1024.0f), + info.leonCssMemoryUsage.total / (1024.0f * 1024.0f)); + printf("LeonMss heap used / total - %.2f / %.2f MiB\n", + info.leonMssMemoryUsage.used / (1024.0f * 1024.0f), + info.leonMssMemoryUsage.total / (1024.0f * 1024.0f)); + const auto& t = info.chipTemperature; + printf("Chip temperature - average: %.2f, css: %.2f, mss: %.2f, upa0: %.2f, upa1: %.2f\n", t.average, t.css, t.mss, t.upa, t.dss); + printf("Cpu usage - Leon OS: %.2f %%, Leon RT: %.2f %%\n", info.leonCssCpuUsage.average * 100, info.leonMssCpuUsage.average * 100); + printf("----------------------------------------\n"); +} int main() { - using namespace std; - + // Create pipeline dai::Pipeline pipeline; + + // Define source and output auto sysLog = pipeline.create(); auto xout = pipeline.create(); - // properties - sysLog->setRate(1.0f); // 1 hz updates xout->setStreamName("sysinfo"); - // links - sysLog->out.link(xout->input); - - // Connect to device - dai::Device device; - - // Query device (before pipeline starts) - dai::MemoryInfo ddr = device.getDdrMemoryUsage(); - printf("Ddr used / total - %.2f / %.2f MiB\n", ddr.used / (1024.0f * 1024.0f), ddr.total / (1024.0f * 1024.0f)); + // Properties + sysLog->setRate(1.0f); // 1 hz updates - dai::MemoryInfo cmx = device.getCmxMemoryUsage(); - printf("Cmx used / total - %.2f / %.2f MiB\n", cmx.used / (1024.0f * 1024.0f), cmx.total / (1024.0f * 1024.0f)); + // Linking + sysLog->out.link(xout->input); - // Start pipeline - device.startPipeline(pipeline); + // Connect to device and start pipeline + dai::Device device(pipeline); - // Create 'sysinfo' queue - auto queue = device.getOutputQueue("sysinfo"); + // Output queue will be used to get the system info + auto qSysInfo = device.getOutputQueue("sysinfo", 4, false); - while(1) { - auto sysInfo = queue->get(); + while(true) { + auto sysInfo = qSysInfo->get(); printSystemInformation(*sysInfo); } + return 0; } - -void printSystemInformation(dai::SystemInformation info) { - printf("Ddr used / total - %.2f / %.2f MiB\n", info.ddrMemoryUsage.used / (1024.0f * 1024.0f), info.ddrMemoryUsage.total / (1024.0f * 1024.0f)); - printf("Cmx used / total - %.2f / %.2f MiB\n", info.cmxMemoryUsage.used / (1024.0f * 1024.0f), info.cmxMemoryUsage.total / (1024.0f * 1024.0f)); - - printf("LeonCss heap used / total - %.2f / %.2f MiB\n", - info.leonCssMemoryUsage.used / (1024.0f * 1024.0f), - info.leonCssMemoryUsage.total / (1024.0f * 1024.0f)); - printf("LeonMss heap used / total - %.2f / %.2f MiB\n", - info.leonMssMemoryUsage.used / (1024.0f * 1024.0f), - info.leonMssMemoryUsage.total / (1024.0f * 1024.0f)); - const auto& t = info.chipTemperature; - printf("Chip temperature - average: %.2f, css: %.2f, mss: %.2f, upa0: %.2f, upa1: %.2f\n", t.average, t.css, t.mss, t.upa, t.dss); - printf("Cpu usage - Leon OS: %.2f %%, Leon RT: %.2f %%\n", info.leonCssCpuUsage.average * 100, info.leonMssCpuUsage.average * 100); -} \ No newline at end of file diff --git a/examples/src/tiny_yolo_v4_device_side_decoding_example.cpp b/examples/src/tiny_yolo_v3_device_side_decoding.cpp similarity index 51% rename from examples/src/tiny_yolo_v4_device_side_decoding_example.cpp rename to examples/src/tiny_yolo_v3_device_side_decoding.cpp index 9fcb2ea31..2968fcacf 100644 --- a/examples/src/tiny_yolo_v4_device_side_decoding_example.cpp +++ b/examples/src/tiny_yolo_v3_device_side_decoding.cpp @@ -1,17 +1,9 @@ #include -#include #include -#include "utility.hpp" - // Inludes common necessary includes for development using depthai library #include "depthai/depthai.hpp" -/* -The code is the same as for Tiny-yolo-V3, the only difference is the blob file. -The blob was compiled following this tutorial: https://github.com/TNTWEN/OpenVINO-YOLOV4 -*/ - 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", @@ -23,76 +15,110 @@ static const std::vector labelMap = { "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; -static bool syncNN = true; +static std::atomic syncNN{true}; + +int main(int argc, char** argv) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); - auto colorCam = p.create(); - auto xlinkOut = p.create(); - auto nnOut = p.create(); + // Create pipeline + dai::Pipeline pipeline; - xlinkOut->setStreamName("preview"); - nnOut->setStreamName("detections"); + // Define sources and outputs + auto camRgb = pipeline.create(); + auto detectionNetwork = pipeline.create(); + auto xoutRgb = pipeline.create(); + auto nnOut = pipeline.create(); - colorCam->setPreviewSize(416, 416); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); - colorCam->setFps(40); + xoutRgb->setStreamName("rgb"); + nnOut->setStreamName("nn"); - auto detectionNetwork = p.create(); + // Properties + camRgb->setPreviewSize(416, 416); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + camRgb->setFps(40); - // network specific!! + // Network specific settings detectionNetwork->setConfidenceThreshold(0.5f); detectionNetwork->setNumClasses(80); detectionNetwork->setCoordinateSize(4); detectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); - detectionNetwork->setAnchorMasks({{"side13", {3, 4, 5}}, {"side26", {1, 2, 3}}}); + detectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}}); detectionNetwork->setIouThreshold(0.5f); detectionNetwork->setBlobPath(nnPath); + detectionNetwork->setNumInferenceThreads(2); + detectionNetwork->input.setBlocking(false); - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(detectionNetwork->input); + // Linking + camRgb->preview.link(detectionNetwork->input); if(syncNN) { - detectionNetwork->passthrough.link(xlinkOut->input); + detectionNetwork->passthrough.link(xoutRgb->input); } else { - colorCam->preview.link(xlinkOut->input); + camRgb->preview.link(xoutRgb->input); } detectionNetwork->out.link(nnOut->input); - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } + // Connect to device and start pipeline + dai::Device device(pipeline); - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); - - auto preview = d.getOutputQueue("preview", 4, false); - auto detections = d.getOutputQueue("detections", 4, false); + auto qRgb = device.getOutputQueue("rgb", 4, false); + auto qDet = device.getOutputQueue("nn", 4, false); + cv::Mat frame; + std::vector detections; auto startTime = steady_clock::now(); int counter = 0; float fps = 0; - while(1) { - auto imgFrame = preview->get(); - auto det = detections->get(); + auto color2 = cv::Scalar(255, 255, 255); + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + while(true) { + std::shared_ptr inRgb; + std::shared_ptr inDet; + + if(syncNN) { + inRgb = qRgb->get(); + inDet = qDet->get(); + } else { + inRgb = qRgb->tryGet(); + inDet = qDet->tryGet(); + } counter++; auto currentTime = steady_clock::now(); @@ -103,40 +129,25 @@ int main(int argc, char** argv) { startTime = currentTime; } - cv::Mat frame = imgFrame->getCvFrame(); - - auto color = cv::Scalar(255, 255, 255); - auto dets = det->detections; - for(const auto& d : dets) { - int x1 = d.xmin * frame.cols; - int y1 = d.ymin * frame.rows; - int x2 = d.xmax * frame.cols; - int y2 = d.ymax * frame.rows; - - int labelIndex = d.label; - std::string labelStr = 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, color); - - std::stringstream confStr; - confStr << std::fixed << std::setprecision(2) << d.confidence * 100; - cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + if(inRgb) { + frame = inRgb->getCvFrame(); + std::stringstream fpsStr; + fpsStr << "NN fps: " << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, inRgb->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color2); + } - cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + if(inDet) { + detections = inDet->detections; } - std::stringstream fpsStr; - fpsStr << std::fixed << std::setprecision(2) << fps; - cv::putText(frame, fpsStr.str(), cv::Point(2, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); + if(!frame.empty()) { + displayFrame("rgb", frame, detections); + } - cv::imshow("preview", frame); int key = cv::waitKey(1); - if(key == 'q') { + if(key == 'q' || key == 'Q') { return 0; } } - return 0; } \ No newline at end of file diff --git a/examples/src/tiny_yolo_v3_device_side_decoding_example.cpp b/examples/src/tiny_yolo_v3_device_side_decoding_example.cpp deleted file mode 100644 index 18b2fd1db..000000000 --- a/examples/src/tiny_yolo_v3_device_side_decoding_example.cpp +++ /dev/null @@ -1,137 +0,0 @@ -#include -#include -#include - -#include "utility.hpp" - -// Inludes common necessary includes for development using depthai library -#include "depthai/depthai.hpp" - -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 bool syncNN = true; - -dai::Pipeline createNNPipeline(std::string nnPath) { - dai::Pipeline p; - - auto colorCam = p.create(); - auto xlinkOut = p.create(); - auto nnOut = p.create(); - - xlinkOut->setStreamName("preview"); - nnOut->setStreamName("detections"); - - colorCam->setPreviewSize(416, 416); - colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); - colorCam->setInterleaved(false); - colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); - colorCam->setFps(40); - - auto detectionNetwork = p.create(); - - // network specific!! - detectionNetwork->setConfidenceThreshold(0.5f); - detectionNetwork->setNumClasses(80); - detectionNetwork->setCoordinateSize(4); - detectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); - detectionNetwork->setAnchorMasks({{"side13", {3, 4, 5}}, {"side26", {1, 2, 3}}}); - detectionNetwork->setIouThreshold(0.5f); - detectionNetwork->setBlobPath(nnPath); - - // Link plugins CAM -> NN -> XLINK - colorCam->preview.link(detectionNetwork->input); - if(syncNN) { - detectionNetwork->passthrough.link(xlinkOut->input); - } else { - colorCam->preview.link(xlinkOut->input); - } - - detectionNetwork->out.link(nnOut->input); - - return p; -} - -int main(int argc, char** argv) { - using namespace std; - using namespace std::chrono; - std::string nnPath(BLOB_PATH); - - // If path to blob specified, use that - if(argc > 1) { - nnPath = std::string(argv[1]); - } - - // Print which blob we are using - printf("Using blob at path: %s\n", nnPath.c_str()); - - // Create pipeline - dai::Pipeline p = createNNPipeline(nnPath); - - // Connect and start the pipeline - dai::Device d(p); - - auto preview = d.getOutputQueue("preview", 4, false); - auto detections = d.getOutputQueue("detections", 4, false); - - auto startTime = steady_clock::now(); - int counter = 0; - float fps = 0; - while(1) { - auto imgFrame = preview->get(); - auto det = detections->get(); - - counter++; - auto currentTime = steady_clock::now(); - auto elapsed = duration_cast>(currentTime - startTime); - if(elapsed > seconds(1)) { - fps = counter / elapsed.count(); - counter = 0; - startTime = currentTime; - } - - cv::Mat frame = imgFrame->getCvFrame(); - - auto color = cv::Scalar(255, 255, 255); - auto dets = det->detections; - for(const auto& d : dets) { - int x1 = d.xmin * frame.cols; - int y1 = d.ymin * frame.rows; - int x2 = d.xmax * frame.cols; - int y2 = d.ymax * frame.rows; - - int labelIndex = d.label; - std::string labelStr = 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, color); - - std::stringstream confStr; - confStr << std::fixed << std::setprecision(2) << d.confidence * 100; - cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); - - 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, imgFrame->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); - - cv::imshow("preview", frame); - int key = cv::waitKey(1); - if(key == 'q') { - return 0; - } - } - - return 0; -} \ No newline at end of file diff --git a/examples/src/tiny_yolo_v4_device_side_decoding.cpp b/examples/src/tiny_yolo_v4_device_side_decoding.cpp new file mode 100644 index 000000000..0221f1c7d --- /dev/null +++ b/examples/src/tiny_yolo_v4_device_side_decoding.cpp @@ -0,0 +1,159 @@ +#include +#include + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +/* +The code is the same as for Tiny-yolo-V3, the only difference is the blob file. +The blob was compiled following this tutorial: https://github.com/TNTWEN/OpenVINO-YOLOV4 +*/ + +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) { + using namespace std; + using namespace std::chrono; + std::string nnPath(BLOB_PATH); + + // If path to blob specified, use that + if(argc > 1) { + nnPath = std::string(argv[1]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto detectionNetwork = pipeline.create(); + auto xoutRgb = pipeline.create(); + auto nnOut = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + nnOut->setStreamName("detections"); + + // Properties + camRgb->setPreviewSize(416, 416); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + camRgb->setFps(40); + + // Network specific settings + detectionNetwork->setConfidenceThreshold(0.5f); + detectionNetwork->setNumClasses(80); + detectionNetwork->setCoordinateSize(4); + detectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); + detectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}}); + detectionNetwork->setIouThreshold(0.5f); + detectionNetwork->setBlobPath(nnPath); + detectionNetwork->setNumInferenceThreads(2); + detectionNetwork->input.setBlocking(false); + + // Linking + camRgb->preview.link(detectionNetwork->input); + if(syncNN) { + detectionNetwork->passthrough.link(xoutRgb->input); + } else { + camRgb->preview.link(xoutRgb->input); + } + + detectionNetwork->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Output queues will be used to get the rgb frames and nn data from the outputs defined above + auto qRgb = device.getOutputQueue("rgb", 4, false); + auto qDet = device.getOutputQueue("detections", 4, false); + + cv::Mat frame; + std::vector detections; + auto startTime = steady_clock::now(); + int counter = 0; + float fps = 0; + auto color2 = cv::Scalar(255, 255, 255); + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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 + 40), 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); + } + // Show the frame + cv::imshow(name, frame); + }; + + while(true) { + std::shared_ptr inRgb; + std::shared_ptr inDet; + + if(syncNN) { + inRgb = qRgb->get(); + inDet = qDet->get(); + } else { + inRgb = qRgb->tryGet(); + inDet = qDet->tryGet(); + } + + counter++; + auto currentTime = steady_clock::now(); + auto elapsed = duration_cast>(currentTime - startTime); + if(elapsed > seconds(1)) { + fps = counter / elapsed.count(); + counter = 0; + startTime = currentTime; + } + + if(inRgb) { + frame = inRgb->getCvFrame(); + std::stringstream fpsStr; + fpsStr << "NN fps: " << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, inRgb->getHeight() - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color2); + } + + if(inDet) { + detections = inDet->detections; + } + + if(!frame.empty()) { + displayFrame("rgb", frame, detections); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + return 0; + } + } + return 0; +} diff --git a/examples/src/utility.cpp b/examples/src/utility.cpp index 64cfe80df..16fd91d74 100644 --- a/examples/src/utility.cpp +++ b/examples/src/utility.cpp @@ -3,8 +3,30 @@ // libraries #include "fp16/fp16.h" +#include "errno.h" + +#if (defined(_WIN32) || defined(_WIN64)) +#include +#include +#else +#include +#endif + +int createDirectory(std::string directory) +{ + int ret = 0; +#if (defined(_WIN32) || defined(_WIN64)) + ret = _mkdir(directory.c_str()); +#else + ret = mkdir(directory.c_str(), 0777); +#endif + if (ret == EEXIST) ret = 0; + return ret; +} + + cv::Mat toMat(const std::vector& data, int w, int h , int numPlanes, int bpp){ - + cv::Mat frame; if(numPlanes == 3){ @@ -15,15 +37,15 @@ cv::Mat toMat(const std::vector& data, int w, int h , int numPlanes, in uint8_t b = data.data()[i + w*h * 0]; frame.data[i*3+0] = b; } - for(int i = 0; i < w*h; i++) { - uint8_t g = data.data()[i + w*h * 1]; + for(int i = 0; i < w*h; i++) { + uint8_t g = data.data()[i + w*h * 1]; frame.data[i*3+1] = g; } for(int i = 0; i < w*h; i++) { uint8_t r = data.data()[i + w*h * 2]; frame.data[i*3+2] = r; } - + } else { if(bpp == 3){ frame = cv::Mat(h, w, CV_8UC3); @@ -31,26 +53,26 @@ cv::Mat toMat(const std::vector& data, int w, int h , int numPlanes, in uint8_t b,g,r; b = data.data()[i + 2]; g = data.data()[i + 1]; - r = data.data()[i + 0]; + r = data.data()[i + 0]; frame.at( (i/bpp) / w, (i/bpp) % w) = cv::Vec3b(b,g,r); } } else if(bpp == 6) { //first denormalize //dump - + frame = cv::Mat(h, w, CV_8UC3); for(int y = 0; y < h; y++){ for(int x = 0; x < w; x++){ - const uint16_t* fp16 = (const uint16_t*) (data.data() + (y*w+x)*bpp); + const uint16_t* fp16 = (const uint16_t*) (data.data() + (y*w+x)*bpp); uint8_t r = (uint8_t) (fp16_ieee_to_fp32_value(fp16[0]) * 255.0f); uint8_t g = (uint8_t) (fp16_ieee_to_fp32_value(fp16[1]) * 255.0f); uint8_t b = (uint8_t) (fp16_ieee_to_fp32_value(fp16[2]) * 255.0f); frame.at(y, x) = cv::Vec3b(b,g,r); } } - + } } diff --git a/examples/src/utility.hpp b/examples/src/utility.hpp index 9f0d75807..10f6facde 100644 --- a/examples/src/utility.hpp +++ b/examples/src/utility.hpp @@ -5,3 +5,4 @@ cv::Mat toMat(const std::vector& data, int w, int h , int numPlanes, int bpp); void toPlanar(cv::Mat& bgr, std::vector& data); cv::Mat resizeKeepAspectRatio(const cv::Mat &input, const cv::Size &dstSize, const cv::Scalar &bgcolor); +int createDirectory(std::string directory); diff --git a/examples/src/video_mobilenet.cpp b/examples/src/video_mobilenet.cpp new file mode 100644 index 000000000..a4ce91420 --- /dev/null +++ b/examples/src/video_mobilenet.cpp @@ -0,0 +1,112 @@ +#include + +#include "utility.hpp" + +// Inludes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +// MobilenetSSD label texts +static const std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", + "car", "cat", "chair", "cow", "diningtable", "dog", "horse", + "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + +int main(int argc, char** argv) { + using namespace std; + // Default blob path provided by Hunter private data download + // Applicable for easier example usage only + std::string nnPath(BLOB_PATH); + std::string videoPath(VIDEO_PATH); + + // If path to blob specified, use that + if(argc > 2) { + nnPath = std::string(argv[1]); + videoPath = std::string(argv[2]); + } + + // Print which blob we are using + printf("Using blob at path: %s\n", nnPath.c_str()); + printf("Using video at path: %s\n", videoPath.c_str()); + + // Create pipeline + dai::Pipeline pipeline; + + // Define source and outputs + auto nn = pipeline.create(); + + auto xinFrame = pipeline.create(); + auto nnOut = pipeline.create(); + + xinFrame->setStreamName("inFrame"); + nnOut->setStreamName("nn"); + + // Properties + nn->setConfidenceThreshold(0.5); + nn->setBlobPath(nnPath); + nn->setNumInferenceThreads(2); + nn->input.setBlocking(false); + + // Linking + xinFrame->out.link(nn->input); + nn->out.link(nnOut->input); + + // Connect to device and start pipeline + dai::Device device(pipeline); + + // Input queue will be used to send video frames to the device. + auto qIn = device.getInputQueue("inFrame"); + // Output queue will be used to get nn data from the video frames. + auto qDet = device.getOutputQueue("nn", 4, false); + + // Add bounding boxes and text to the frame and show it to the user + auto displayFrame = [](std::string name, cv::Mat frame, std::vector& detections) { + auto color = cv::Scalar(255, 0, 0); + // nn data, being the bounding box locations, are in <0..1> range - they need to be normalized with frame width/height + for(auto& detection : detections) { + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + int labelIndex = detection.label; + std::string labelStr = 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, color); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 40), cv::FONT_HERSHEY_TRIPLEX, 0.5, color); + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + // Show the frame + cv::imshow(name, frame); + }; + + cv::Mat frame; + cv::VideoCapture cap(videoPath); + + cv::namedWindow("inFrame", cv::WINDOW_NORMAL); + cv::resizeWindow("inFrame", 1280, 720); + std::cout << "Resize video window with mouse drag!" << std::endl; + + while(cap.isOpened()) { + // Read frame from video + cap >> frame; + auto tensor = std::make_shared(); + + frame = resizeKeepAspectRatio(frame, cv::Size(300, 300), cv::Scalar(0)); + + toPlanar(frame, tensor->data); + + qIn->send(tensor); + + auto inDet = qDet->get(); + auto detections = inDet->detections; + + displayFrame("inFrame", frame, detections); + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') return 0; + } + return 0; +} diff --git a/examples/src/webcam_mobilenet_example.cpp b/examples/src/webcam_mobilenet_example.cpp index 3a517ce7a..a32dd4c1f 100644 --- a/examples/src/webcam_mobilenet_example.cpp +++ b/examples/src/webcam_mobilenet_example.cpp @@ -1,5 +1,4 @@ -#include #include #include "utility.hpp" @@ -25,37 +24,38 @@ int main(int argc, char** argv) { using namespace std; - // CREATE PIPELINE - dai::Pipeline p; + // Create pipeline + dai::Pipeline pipeline; - auto xin = p.create(); - auto nn = p.create(); - auto xout = p.create(); + // Define sources and outputs + auto nn = pipeline.create(); + auto xin = pipeline.create(); + auto xout = pipeline.create(); + + xin->setStreamName("nn_in"); + xout->setStreamName("nn_out"); // Properties nn->setBlobPath(nnPath); - xin->setStreamName("nn_in"); xin->setMaxDataSize(300 * 300 * 3); xin->setNumFrames(4); - xout->setStreamName("nn_out"); - - // Link plugins XLINK -> NN -> XLINK + // Linking xin->out.link(nn->input); nn->out.link(xout->input); // Open Webcam cv::VideoCapture webcam(camId); - // Connect to device with above created pipeline - dai::Device d(p); + // Connect to device and start pipeline + dai::Device device(pipeline); cv::Mat frame; - auto in = d.getInputQueue("nn_in"); - auto detections = d.getOutputQueue("nn_out"); + auto in = device.getInputQueue("nn_in"); + auto detections = device.getOutputQueue("nn_out"); - while(1) { + while(true) { // data to send further auto tensor = std::make_shared(); @@ -120,11 +120,11 @@ int main(int argc, char** argv) { } cv::imshow("preview", frame); + int key = cv::waitKey(1); if(key == 'q') { return 0; } } - return 0; } diff --git a/include/depthai/common/CameraBoardSocket.hpp b/include/depthai/common/CameraBoardSocket.hpp new file mode 100644 index 000000000..b27bedd0b --- /dev/null +++ b/include/depthai/common/CameraBoardSocket.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include "depthai-shared/common/CameraBoardSocket.hpp" + +namespace dai { + +inline std::ostream& operator<<(std::ostream& out, const CameraBoardSocket& socket) { + switch(socket) { + case CameraBoardSocket::AUTO: + out << "AUTO"; + break; + case CameraBoardSocket::RGB: + out << "RGB"; + break; + case CameraBoardSocket::LEFT: + out << "LEFT"; + break; + case CameraBoardSocket::RIGHT: + out << "RIGHT"; + break; + } + return out; +} + +} // namespace dai \ No newline at end of file diff --git a/include/depthai/common/UsbSpeed.hpp b/include/depthai/common/UsbSpeed.hpp new file mode 100644 index 000000000..59ccdcb5e --- /dev/null +++ b/include/depthai/common/UsbSpeed.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "depthai-shared/common/UsbSpeed.hpp" + +namespace dai { + +inline std::ostream& operator<<(std::ostream& out, const UsbSpeed& speed) { + switch(speed) { + case UsbSpeed::UNKNOWN: + out << "UNKNOWN"; + break; + case UsbSpeed::LOW: + out << "LOW"; + break; + case UsbSpeed::FULL: + out << "FULL"; + break; + case UsbSpeed::HIGH: + out << "HIGH"; + break; + case UsbSpeed::SUPER: + out << "SUPER"; + break; + case UsbSpeed::SUPER_PLUS: + out << "SUPER_PLUS"; + break; + } + return out; +} + +} // namespace dai diff --git a/include/depthai/device/Device.hpp b/include/depthai/device/Device.hpp index aa79d241d..e63a95f59 100644 --- a/include/depthai/device/Device.hpp +++ b/include/depthai/device/Device.hpp @@ -10,13 +10,14 @@ // project #include "CallbackHandler.hpp" #include "DataQueue.hpp" +#include "depthai/common/CameraBoardSocket.hpp" +#include "depthai/common/UsbSpeed.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/utility/Pimpl.hpp" #include "depthai/xlink/XLinkConnection.hpp" #include "depthai/xlink/XLinkStream.hpp" // shared -#include "depthai-shared/common/CameraBoardSocket.hpp" #include "depthai-shared/common/ChipTemperature.hpp" #include "depthai-shared/common/CpuUsage.hpp" #include "depthai-shared/common/MemoryInfo.hpp" diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index c9f5827c0..f4e5d04b4 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -1,7 +1,6 @@ #pragma once // std -#include #include #include #include @@ -9,7 +8,6 @@ // project #include "CallbackHandler.hpp" #include "DataQueue.hpp" -#include "Device.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/xlink/XLinkConnection.hpp" #include "depthai/xlink/XLinkStream.hpp" @@ -43,11 +41,6 @@ class DeviceBootloader { unsigned versionMajor, versionMinor, versionPatch; }; - struct Config { - Config() : timeout(3) {} - std::chrono::milliseconds timeout; - }; - // Static API /** * Searches for connected devices in either UNBOOTED or BOOTLOADER states and returns first available. @@ -67,7 +60,7 @@ class DeviceBootloader { * @param pathToCmd Optional path to custom device firmware * @returns Depthai application package */ - static std::vector createDepthaiApplicationPackage(Pipeline& pipeline, UsbSpeed maxUsbSpeed = Device::DEFAULT_USB_SPEED); + static std::vector createDepthaiApplicationPackage(Pipeline& pipeline, std::string pathToCmd = ""); /** * Saves application package to a file which can be flashed to depthai device. @@ -75,7 +68,7 @@ class DeviceBootloader { * @param pipeline Pipeline from which to create the application package * @param pathToCmd Optional path to custom device firmware */ - static void saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, UsbSpeed maxUsbSpeed = Device::DEFAULT_USB_SPEED); + static void saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, std::string pathToCmd = ""); /** * @returns Embedded bootloader version @@ -85,7 +78,7 @@ class DeviceBootloader { /** * @returns Embedded bootloader binary */ - static std::vector getEmbeddedBootloaderBinary(Config config); + static std::vector getEmbeddedBootloaderBinary(); // DeviceBootloader() = delete; @@ -128,7 +121,7 @@ class DeviceBootloader { * @param progressCallback Callback that sends back a value between 0..1 which signifies current flashing progress * @param path Optional parameter to custom bootloader to flash */ - std::tuple flashBootloader(std::function progressCallback, Config config = {}); + std::tuple flashBootloader(std::function progressCallback, std::string path = ""); /** * @returns Version of current running bootloader diff --git a/include/depthai/pipeline/node/MonoCamera.hpp b/include/depthai/pipeline/node/MonoCamera.hpp index 0978e55bf..fe3b81314 100644 --- a/include/depthai/pipeline/node/MonoCamera.hpp +++ b/include/depthai/pipeline/node/MonoCamera.hpp @@ -2,10 +2,10 @@ #include +#include "depthai/common/CameraBoardSocket.hpp" #include "depthai/pipeline/Node.hpp" // shared -#include #include namespace dai { @@ -46,10 +46,17 @@ class MonoCamera : public Node { /** * Outputs ImgFrame message that carries RAW8 encoded (grayscale) frame data. * - * Suitable for use StereoDepth node + * Suitable for use StereoDepth node. Processed by ISP */ Output out{*this, "out", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Outputs ImgFrame message that carries RAW10-packed (MIPI CSI-2 format) frame data. + * + * Captured directly from the camera sensor + */ + Output raw{*this, "raw", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** * Specify which board socket to use * @param boardSocket Board socket to use diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 407226092..926139131 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -155,8 +155,9 @@ class StereoDepth : public Node { * The mirroring is required to have a normal non-mirrored disparity/depth output. * * A side effect of this option is disparity alignment to the perspective of left or right input: - * - LR-check disabled: `false`: mapped to left and mirrored, `true`: mapped to right; - * - LR-check enabled: `false`: mapped to right, `true`: mapped to left, never mirrored. + * `false`: mapped to left and mirrored, `true`: mapped to right. + * With LR-check enabled, this option is ignored, none of the outputs are mirrored, + * and disparity is mapped to right. * * @param enable True for normal disparity/depth, otherwise mirrored */ diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 8e9965a34..c8779ca58 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 8e9965a34a2c8de02a151f111e2397657e2ddf52 +Subproject commit c8779ca581659803b6f3e465cc2bde7b22203be0 diff --git a/shared/depthai-shared b/shared/depthai-shared index e23302538..d9c639076 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit e23302538d867042e1ec6c2eb6ff723e7c4d538a +Subproject commit d9c6390769efa691ae9707f493fccc3247287428 diff --git a/shared/depthai-shared.cmake b/shared/depthai-shared.cmake index 3e2f95285..39722c9b1 100644 --- a/shared/depthai-shared.cmake +++ b/shared/depthai-shared.cmake @@ -4,6 +4,7 @@ set(DEPTHAI_SHARED_3RDPARTY_HEADERS_PATH "depthai-shared/3rdparty") set(DEPTHAI_SHARED_SOURCES ${DEPTHAI_SHARED_FOLDER}/src/datatype/DatatypeEnum.cpp + ${DEPTHAI_SHARED_FOLDER}/src/utility/Checksum.cpp ) set(DEPTHAI_SHARED_PUBLIC_INCLUDE @@ -21,7 +22,7 @@ set(DEPTHAI_SHARED_INCLUDE # Try retriving depthai-shared commit hash (if cloned and not sources only) find_package(Git) if(GIT_FOUND AND NOT DEPTHAI_DOWNLOADED_SOURCES) - + # Check that submodule is initialized and updated execute_process( COMMAND ${GIT_EXECUTABLE} submodule status ${DEPTHAI_SHARED_FOLDER} @@ -32,7 +33,7 @@ if(GIT_FOUND AND NOT DEPTHAI_DOWNLOADED_SOURCES) string(SUBSTRING ${statusCommit} 0 1 status) if(${status} STREQUAL "-") message(FATAL_ERROR "Submodule 'depthai-shared' not initialized/updated. Run 'git submodule update --init --recursive' first") - endif() + endif() # Get depthai-shared current commit execute_process( diff --git a/src/device/CallbackHandler.cpp b/src/device/CallbackHandler.cpp index cf2176929..1e4fe3003 100644 --- a/src/device/CallbackHandler.cpp +++ b/src/device/CallbackHandler.cpp @@ -18,7 +18,7 @@ CallbackHandler::CallbackHandler(std::shared_ptr conn, t = std::thread([this, streamName]() { try { // open stream with 1B write size (no writing will happen here) - XLinkStream stream(*connection, streamName, XLINK_USB_BUFFER_MAX_SIZE); + XLinkStream stream(*connection, streamName, device::XLINK_USB_BUFFER_MAX_SIZE); while(running) { // read packet diff --git a/src/device/DataQueue.cpp b/src/device/DataQueue.cpp index eb648c116..caa80dde5 100644 --- a/src/device/DataQueue.cpp +++ b/src/device/DataQueue.cpp @@ -162,7 +162,7 @@ bool DataOutputQueue::removeCallback(int callbackId) { DataInputQueue::DataInputQueue(const std::shared_ptr& conn, const std::string& streamName, unsigned int maxSize, bool blocking) : queue(maxSize, blocking), name(streamName) { // open stream with default XLINK_USB_BUFFER_MAX_SIZE write size - XLinkStream stream(*conn, name, dai::XLINK_USB_BUFFER_MAX_SIZE); + XLinkStream stream(*conn, name, device::XLINK_USB_BUFFER_MAX_SIZE); writingThread = std::thread(std::bind( [this, conn](XLinkStream& stream) { diff --git a/src/device/Device.cpp b/src/device/Device.cpp index 641618b31..74e80eba2 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -518,7 +518,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm deviceInfo.state = X_LINK_BOOTED; // prepare rpc for both attached and host controlled mode - rpcStream = std::unique_ptr(new XLinkStream(*connection, dai::XLINK_CHANNEL_MAIN_RPC, dai::XLINK_USB_BUFFER_MAX_SIZE)); + rpcStream = std::unique_ptr(new XLinkStream(*connection, device::XLINK_CHANNEL_MAIN_RPC, device::XLINK_USB_BUFFER_MAX_SIZE)); client = std::unique_ptr>( new nanorpc::core::client([this](nanorpc::core::type::buffer request) { @@ -548,7 +548,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm break; } // Ping with a period half of that of the watchdog timeout - std::this_thread::sleep_for(XLINK_WATCHDOG_TIMEOUT / 2); + std::this_thread::sleep_for(device::XLINK_WATCHDOG_TIMEOUT / 2); } // Watchdog ended. Useful for checking disconnects @@ -560,7 +560,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm using namespace std::chrono; try { - XLinkStream stream(*this->connection, XLINK_CHANNEL_TIMESYNC, 128); + XLinkStream stream(*this->connection, device::XLINK_CHANNEL_TIMESYNC, 128); Timestamp timestamp = {}; while(timesyncRunning) { // Block @@ -587,7 +587,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm using namespace std::chrono; std::vector messages; try { - XLinkStream stream(*this->connection, XLINK_CHANNEL_LOG, 128); + XLinkStream stream(*this->connection, device::XLINK_CHANNEL_LOG, 128); while(loggingRunning) { // Block auto log = stream.read(); @@ -1059,10 +1059,10 @@ bool Device::startPipeline(const Pipeline& pipeline) { // Transfer the whole assetStorage in a separate thread const std::string streamAssetStorage = "__stream_asset_storage"; std::thread t1([this, &streamAssetStorage, &assetStorage]() { - XLinkStream stream(*connection, streamAssetStorage, XLINK_USB_BUFFER_MAX_SIZE); + XLinkStream stream(*connection, streamAssetStorage, device::XLINK_USB_BUFFER_MAX_SIZE); int64_t offset = 0; do { - int64_t toTransfer = std::min(static_cast(XLINK_USB_BUFFER_MAX_SIZE), static_cast(assetStorage.size() - offset)); + int64_t toTransfer = std::min(static_cast(device::XLINK_USB_BUFFER_MAX_SIZE), static_cast(assetStorage.size() - offset)); stream.write(&assetStorage[offset], toTransfer); offset += toTransfer; } while(offset < static_cast(assetStorage.size())); diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index c9872fc16..bdd44f139 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -51,21 +51,26 @@ std::vector DeviceBootloader::getAllAvailableDevices() { return availableDevices; } -std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& pipeline, UsbSpeed maxUsbSpeed) { +std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& pipeline, std::string pathToCmd) { // Serialize the pipeline PipelineSchema schema; Assets assets; std::vector assetStorage; pipeline.serialize(schema, assets, assetStorage); - // Prepare Device::Config - Device::Config cfg; - cfg.version = pipeline.getOpenVINOVersion(); - cfg.preboot = pipeline.getDevicePrebootConfig(); - cfg.preboot.maxUsbSpeed = maxUsbSpeed; + // Get openvino version + OpenVINO::Version version = pipeline.getOpenVINOVersion(); // Prepare device firmware - std::vector deviceFirmware = Resources::getInstance().getDeviceFirmware(cfg); + std::vector deviceFirmware; + if(pathToCmd != "") { + std::ifstream fwStream(pathToCmd, std::ios::binary); + if(!fwStream.is_open()) throw std::runtime_error("Cannot create application package, device firmware at path: " + pathToCmd + " doesn't exist"); + deviceFirmware = std::vector(std::istreambuf_iterator(fwStream), {}); + } else { + // TODO(themarpe) - specify OpenVINO version + deviceFirmware = Resources::getInstance().getDeviceFirmware(false, version); + } // Create msgpacks std::vector pipelineBinary, assetsBinary; @@ -152,11 +157,7 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd) if(deviceInfo.state == X_LINK_UNBOOTED) { // Unbooted device found, boot to BOOTLOADER and connect with XLinkConnection constructor if(embeddedMvcmd) { - Config cfg; - // Never timeout - cfg.timeout = std::chrono::milliseconds(-1); - - connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(cfg), X_LINK_BOOTLOADER); + connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(), X_LINK_BOOTLOADER); } else { connection = std::make_shared(deviceInfo, pathToMvcmd, X_LINK_BOOTLOADER); } @@ -273,8 +274,8 @@ std::tuple DeviceBootloader::flash(std::function return flashDepthaiApplicationPackage(progressCb, createDepthaiApplicationPackage(pipeline)); } -void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, UsbSpeed maxUsbSpeed) { - auto dap = createDepthaiApplicationPackage(pipeline, maxUsbSpeed); +void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, std::string pathToCmd) { + auto dap = createDepthaiApplicationPackage(pipeline, pathToCmd); std::ofstream outfile(path, std::ios::binary); outfile.write(reinterpret_cast(dap.data()), dap.size()); } @@ -318,8 +319,15 @@ std::tuple DeviceBootloader::flashDepthaiApplicationPackage(s return {result.success, result.errorMsg}; } -std::tuple DeviceBootloader::flashBootloader(std::function progressCb, DeviceBootloader::Config config) { - std::vector package = getEmbeddedBootloaderBinary(config); +std::tuple DeviceBootloader::flashBootloader(std::function progressCb, std::string path) { + std::vector package; + if(path != "") { + std::ifstream fwStream(path, std::ios::binary); + if(!fwStream.is_open()) throw std::runtime_error("Cannot flash bootloader, binary at path: " + path + " doesn't exist"); + package = std::vector(std::istreambuf_iterator(fwStream), {}); + } else { + package = getEmbeddedBootloaderBinary(); + } // get streamId streamId_t streamId = stream->getStreamId(); @@ -365,8 +373,8 @@ bool DeviceBootloader::isEmbeddedVersion() { return isEmbedded; } -std::vector DeviceBootloader::getEmbeddedBootloaderBinary(Config config) { - return Resources::getInstance().getBootloaderFirmware(config); +std::vector DeviceBootloader::getEmbeddedBootloaderBinary() { + return Resources::getInstance().getBootloaderFirmware(); } DeviceBootloader::Version::Version(const std::string& v) : versionMajor(0), versionMinor(0), versionPatch(0) { diff --git a/src/opencv/ImgFrame.cpp b/src/opencv/ImgFrame.cpp old mode 100755 new mode 100644 index 658f8aa30..0dfe6d562 --- a/src/opencv/ImgFrame.cpp +++ b/src/opencv/ImgFrame.cpp @@ -2,6 +2,8 @@ #include +// #include "spdlog/spdlog.h" + namespace dai { void ImgFrame::setFrame(cv::Mat frame) { @@ -64,8 +66,13 @@ cv::Mat ImgFrame::getFrame(bool deepCopy) { // Check if enough data long requiredSize = CV_ELEM_SIZE(type) * size.area(); - if(static_cast(img.data.size()) < requiredSize) { - throw std::runtime_error("ImgFrame doesn't have enough data to encode specified frame. Maybe metadataOnly transfer was made?"); + long actualSize = static_cast(img.data.size()); + if(actualSize < requiredSize) { + throw std::runtime_error("ImgFrame doesn't have enough data to encode specified frame, required " + std::to_string(requiredSize) + ", actual " + + std::to_string(actualSize) + ". Maybe metadataOnly transfer was made?"); + } else if(actualSize > requiredSize) { + // FIXME doesn't build on Windows (multiple definitions during link) + // spdlog::warn("ImgFrame has excess data: actual {}, expected {}", actualSize, requiredSize); } if(getWidth() <= 0 || getHeight() <= 0) { throw std::runtime_error("ImgFrame metadata not valid (width or height = 0)"); diff --git a/src/pipeline/node/MonoCamera.cpp b/src/pipeline/node/MonoCamera.cpp index 9191e117c..32ee64483 100644 --- a/src/pipeline/node/MonoCamera.cpp +++ b/src/pipeline/node/MonoCamera.cpp @@ -17,7 +17,7 @@ std::string MonoCamera::getName() const { } std::vector MonoCamera::getOutputs() { - return {out}; + return {out, raw}; } std::vector MonoCamera::getInputs() { diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 071daaa53..be12cdf57 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -17,7 +17,8 @@ #include "spdlog/spdlog.h" // shared -#include "depthai-bootloader-shared/PrebootConfig.hpp" +#include "depthai-shared/device/PrebootConfig.hpp" +#include "depthai-shared/utility/Checksum.hpp" extern "C" { #include "bspatch/bspatch.h" @@ -31,7 +32,7 @@ CMRC_DECLARE(depthai); namespace dai { -static std::vector createPrebootHeader(const std::vector& payload); +static std::vector createPrebootHeader(const std::vector& payload, uint32_t magic1, uint32_t magic2); #ifdef DEPTHAI_RESOURCES_TAR_XZ @@ -150,7 +151,7 @@ std::vector Resources::getDeviceBinary(Device::Config config) { } // Prepend preboot config - auto prebootHeader = createPrebootHeader(nlohmann::json::to_msgpack(config.preboot)); + auto prebootHeader = createPrebootHeader(nlohmann::json::to_msgpack(config.preboot), PREBOOT_CONFIG_MAGIC1, PREBOOT_CONFIG_MAGIC2); depthaiBinary.insert(depthaiBinary.begin(), prebootHeader.begin(), prebootHeader.end()); // Return created firmware @@ -372,11 +373,7 @@ std::vector Resources::getDeviceFirmware(bool usb2Mode, OpenVINO:: return getDeviceFirmware(cfg); } -std::vector Resources::getBootloaderFirmware(DeviceBootloader::Config config) { - // Convert Config to PrebootConfig - bootloader::PrebootConfig prebootCfg; - prebootCfg.timeoutMs = config.timeout.count(); - +std::vector Resources::getBootloaderFirmware() { // Binaries are resource compiled #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES @@ -389,12 +386,6 @@ std::vector Resources::getBootloaderFirmware(DeviceBootloader::Con auto bootloaderBinary = fs.open(CMRC_DEPTHAI_BOOTLOADER_PATH); std::vector bootloaderFw{bootloaderBinary.begin(), bootloaderBinary.end()}; - // Create prebootCfg header - const uint8_t* pCfg = reinterpret_cast(&prebootCfg); - auto header = createPrebootHeader({pCfg, pCfg + sizeof(prebootCfg)}); - bootloaderFw.insert(bootloaderFw.begin(), header.begin(), header.end()); - - // Return final bootloader fw return bootloaderFw; #else @@ -403,8 +394,16 @@ std::vector Resources::getBootloaderFirmware(DeviceBootloader::Con #endif } -std::vector createPrebootHeader(const std::vector& payload) { - constexpr const std::uint8_t HEADER[] = {77, 65, 50, 120, 0x8A, 0x00, 0x00, 0x00, 0x70}; +std::vector createPrebootHeader(const std::vector& payload, uint32_t magic1, uint32_t magic2) { + const std::uint8_t HEADER[] = {77, + 65, + 50, + 120, + 0x8A, + static_cast((magic1 >> 0) & 0xFF), + static_cast((magic1 >> 8) & 0xFF), + static_cast((magic1 >> 16) & 0xFF), + static_cast((magic1 >> 24) & 0xFF)}; // Store the constructed preboot information std::vector prebootHeader; @@ -413,8 +412,11 @@ std::vector createPrebootHeader(const std::vector& payloa prebootHeader.insert(prebootHeader.begin(), std::begin(HEADER), std::end(HEADER)); // Calculate size - std::size_t totalPayloadSize = payload.size() + sizeof(std::uint32_t); - std::size_t toAddBytes = 4 - (totalPayloadSize % 4); + std::size_t totalPayloadSize = payload.size() + sizeof(magic2) + sizeof(uint32_t) + sizeof(uint32_t); + std::size_t toAddBytes = 0; + if(totalPayloadSize % 4 != 0) { + toAddBytes = 4 - (totalPayloadSize % 4); + } std::size_t totalSize = totalPayloadSize + toAddBytes; std::size_t totalSizeWord = totalSize / 4; @@ -422,9 +424,14 @@ std::vector createPrebootHeader(const std::vector& payloa prebootHeader.push_back((totalSizeWord >> 0) & 0xFF); prebootHeader.push_back((totalSizeWord >> 8) & 0xFF); - // Write payload size (uint32_t LE) - for(int i = 0; i < 4; i++) { - prebootHeader.push_back((payload.size() >> (i * 8)) & 0xFF); + // Compute payload checksum + auto checksum = utility::checksum(payload.data(), payload.size()); + + // Write checksum & payload size as uint32_t LE + for(const auto& field : {magic2, checksum, static_cast(payload.size())}) { + for(int i = 0; i < 4; i++) { + prebootHeader.push_back((field >> (i * 8)) & 0xFF); + } } // Copy payload diff --git a/src/utility/Resources.hpp b/src/utility/Resources.hpp index b1e1e3756..027028346 100644 --- a/src/utility/Resources.hpp +++ b/src/utility/Resources.hpp @@ -33,7 +33,7 @@ class Resources { // Available resources std::vector getDeviceFirmware(bool usb2Mode, OpenVINO::Version version = OpenVINO::VERSION_2020_1); std::vector getDeviceFirmware(Device::Config config); - std::vector getBootloaderFirmware(DeviceBootloader::Config config); + std::vector getBootloaderFirmware(); }; diff --git a/tests/src/image_manip_node_test.cpp b/tests/src/image_manip_node_test.cpp index e5a303e3f..2665ff332 100644 --- a/tests/src/image_manip_node_test.cpp +++ b/tests/src/image_manip_node_test.cpp @@ -52,7 +52,7 @@ int main() { inFrame.getData().resize(originalWidth * originalHeight * 3); inFrame.setWidth(originalWidth); inFrame.setHeight(originalHeight); - inFrame.setType(dai::RawImgFrame::Type::RGB888p); + inFrame.setType(dai::ImgFrame::Type::RGB888p); // Send the frame in->send(inFrame); diff --git a/tests/src/neural_network_test.cpp b/tests/src/neural_network_test.cpp index 2f4bfbec1..8a3879f66 100644 --- a/tests/src/neural_network_test.cpp +++ b/tests/src/neural_network_test.cpp @@ -72,7 +72,7 @@ TEST_CASE("Neural network node data checks") { dai::ImgFrame frame; frame.setWidth(MOBILENET_WIDTH); frame.setHeight(MOBILENET_HEIGHT); - frame.setType(dai::RawImgFrame::Type::BGR888p); + frame.setType(dai::ImgFrame::Type::BGR888p); frame.setData(std::vector(MOBILENET_DATA_SIZE + i * 1024 * 10)); int msgIndex = 0; From 7dd586a30d337bf0041bb30d0add37d31e550961 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Fri, 28 May 2021 14:48:02 +0200 Subject: [PATCH 003/101] Updated example --- examples/src/rgb_preview.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/src/rgb_preview.cpp b/examples/src/rgb_preview.cpp index 911f7d1d9..eba6edca3 100644 --- a/examples/src/rgb_preview.cpp +++ b/examples/src/rgb_preview.cpp @@ -25,11 +25,10 @@ int main() { camRgb->preview.link(xoutRgb->input); // Connect to device and start pipeline - dai::Device device(pipeline); + dai::Device device(pipeline, dai::UsbSpeed::SUPER); cout << "Connected cameras: "; for(const auto& cam : device.getConnectedCameras()) { - cout << static_cast(cam) << " "; cout << cam << " "; } cout << endl; From 0df668c7cfc04059e8997f5c9afa786ca0b57354 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 31 May 2021 22:06:06 +0200 Subject: [PATCH 004/101] Removed deprecated OpenVINO versions --- CMakeLists.txt | 2 - cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- src/utility/Resources.cpp | 113 +++----------------- src/utility/Resources.hpp | 2 +- 4 files changed, 14 insertions(+), 105 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 135f2c6b8..b39c8df8f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -358,8 +358,6 @@ target_compile_definitions(${TARGET_CORE_NAME} __PC__ # Add depthai-device version DEPTHAI_DEVICE_VERSION="${DEPTHAI_DEVICE_SIDE_COMMIT}" - # Add if using depthai device FWP - DEPTHAI_RESOURCES_TAR_XZ # Add depthai-bootloader version DEPTHAI_BOOTLOADER_VERSION="${DEPTHAI_BOOTLOADER_VERSION}" ) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index ef84e670f..d288ac9a6 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 "67acb9bdda92ca767d26ce5a264740bb72524372") +set(DEPTHAI_DEVICE_SIDE_COMMIT "f5e364878a118dcd3b9e8a08878a96923a29ee18") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index be12cdf57..a1e9d2a33 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -34,17 +34,13 @@ namespace dai { static std::vector createPrebootHeader(const std::vector& payload, uint32_t magic1, uint32_t magic2); -#ifdef DEPTHAI_RESOURCES_TAR_XZ - constexpr static auto CMRC_DEPTHAI_DEVICE_TAR_XZ = "depthai-device-fwp-" DEPTHAI_DEVICE_VERSION ".tar.xz"; // Main FW constexpr static auto DEPTHAI_CMD_OPENVINO_2021_3_PATH = "depthai-device-openvino-2021.3-" DEPTHAI_DEVICE_VERSION ".cmd"; // Patches from Main FW -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_1_PATCH_PATH = "depthai-device-openvino-2020.1-" DEPTHAI_DEVICE_VERSION ".patch"; constexpr static auto DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH = "depthai-device-openvino-2020.3-" DEPTHAI_DEVICE_VERSION ".patch"; -constexpr static auto DEPTHAI_CMD_OPENVINO_2020_2_PATCH_PATH = DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH; constexpr static auto DEPTHAI_CMD_OPENVINO_2020_4_PATCH_PATH = "depthai-device-openvino-2020.4-" DEPTHAI_DEVICE_VERSION ".patch"; constexpr static auto DEPTHAI_CMD_OPENVINO_2021_1_PATCH_PATH = "depthai-device-openvino-2021.1-" DEPTHAI_DEVICE_VERSION ".patch"; constexpr static auto DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH = "depthai-device-openvino-2021.2-" DEPTHAI_DEVICE_VERSION ".patch"; @@ -56,14 +52,16 @@ static constexpr auto array_of(T&&... t) -> std::array { } constexpr static auto resourcesListTarXz = array_of(DEPTHAI_CMD_OPENVINO_2021_3_PATH, - DEPTHAI_CMD_OPENVINO_2020_1_PATCH_PATH, DEPTHAI_CMD_OPENVINO_2020_3_PATCH_PATH, - DEPTHAI_CMD_OPENVINO_2020_2_PATCH_PATH, DEPTHAI_CMD_OPENVINO_2020_4_PATCH_PATH, DEPTHAI_CMD_OPENVINO_2021_1_PATCH_PATH, DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH); std::vector Resources::getDeviceBinary(Device::Config config) { + // Acquire mutex (this mutex signifies that lazy load is complete) + // It is necessary when accessing resourceMap variable + std::unique_lock lock(mtx); + std::vector depthaiBinary; // Get OpenVINO version @@ -82,25 +80,21 @@ std::vector Resources::getDeviceBinary(Device::Config config) { depthaiBinary = std::vector(std::istreambuf_iterator(stream), {}); } else { - // Binaries are resource compiled - #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES +// Binaries are resource compiled +#ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES // Temporary binary std::vector tmpDepthaiBinary; // Main FW depthaiBinary = resourceMap[DEPTHAI_CMD_OPENVINO_2021_3_PATH]; // Patch from main to specified - std::vector& depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH]; + std::vector depthaiPatch; switch(version) { case OpenVINO::VERSION_2020_1: - spdlog::warn("OpenVino version 2020.1 is deprecated and will be removed in the next release!"); - depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_1_PATCH_PATH]; - break; - case OpenVINO::VERSION_2020_2: - spdlog::warn("OpenVino version 2020.2 is deprecated and will be removed in the next release!"); - depthaiPatch = resourceMap[DEPTHAI_CMD_OPENVINO_2020_2_PATCH_PATH]; + // Deprecated + throw std::invalid_argument("Selected OpenVINO version is deprecated"); break; case OpenVINO::VERSION_2020_3: @@ -144,10 +138,10 @@ std::vector Resources::getDeviceBinary(Device::Config config) { depthaiBinary = tmpDepthaiBinary; } - #else - // Binaries from default path (TODO) +#else + // Binaries from default path (TODO) - #endif +#endif } // Prepend preboot config @@ -158,80 +152,12 @@ std::vector Resources::getDeviceBinary(Device::Config config) { return depthaiBinary; } -#else -// TODO - DEPRECATE - -constexpr static auto CMRC_DEPTHAI_CMD_PATH = "depthai-" DEPTHAI_DEVICE_VERSION ".cmd"; - #ifdef DEPTHAI_PATCH_ONLY_MODE -constexpr static auto CMRC_DEPTHAI_USB2_PATCH_PATH = "depthai-usb2-patch-" DEPTHAI_DEVICE_VERSION ".patch"; - #else -constexpr static auto CMRC_DEPTHAI_USB2_CMD_PATH = "depthai-usb2-" DEPTHAI_DEVICE_VERSION ".cmd"; - #endif - -static std::vector getEmbeddedDeviceBinary(bool usb2Mode) { - std::vector finalCmd; - - // Binaries are resource compiled - #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES - - // Get binaries from internal sources - auto fs = cmrc::depthai::get_filesystem(); - - if(usb2Mode) { - #ifdef DEPTHAI_PATCH_ONLY_MODE - - // Get size of original - auto depthaiBinary = fs.open(CMRC_DEPTHAI_CMD_PATH); - - // Open patch - auto depthaiUsb2Patch = fs.open(CMRC_DEPTHAI_USB2_PATCH_PATH); - - // Get new size - int64_t patchedSize = bspatch_mem_get_newsize(reinterpret_cast(depthaiUsb2Patch.begin()), depthaiUsb2Patch.size()); - - // Reserve space for patched binary - finalCmd.resize(patchedSize); - - // Patch - int error = bspatch_mem(reinterpret_cast(depthaiBinary.begin()), - depthaiBinary.size(), - reinterpret_cast(depthaiUsb2Patch.begin()), - depthaiUsb2Patch.size(), - finalCmd.data()); - - // if patch not successful - if(error > 0) throw std::runtime_error("Error while patching cmd for usb2 mode"); - - #else - - auto depthaiUsb2Binary = fs.open(CMRC_DEPTHAI_USB2_CMD_PATH); - finalCmd = std::vector(depthaiUsb2Binary.begin(), depthaiUsb2Binary.end()); - - #endif - - } else { - auto depthaiBinary = fs.open(CMRC_DEPTHAI_CMD_PATH); - finalCmd = std::vector(depthaiBinary.begin(), depthaiBinary.end()); - } - - #else - // Binaries from default path (TODO) - - #endif - - return finalCmd; -} - -#endif - Resources& Resources::getInstance() { static Resources instance; // Guaranteed to be destroyed, instantiated on first use. return instance; } Resources::Resources() { -#ifdef DEPTHAI_RESOURCES_TAR_XZ - // condition variable to let this thread know when the mutex was acquired std::mutex mtxCv; std::condition_variable cv; @@ -333,8 +259,6 @@ Resources::Resources() { // Wait for 'cv' to signal std::unique_lock l(mtxCv); cv.wait(l, [&mutexAcquired]() { return mutexAcquired; }); - -#endif } Resources::~Resources() { @@ -343,21 +267,8 @@ Resources::~Resources() { } std::vector Resources::getDeviceFirmware(Device::Config config) { - // Acquire mutex (this mutex signifies that lazy load is complete) - // It is necessary when accessing resourceMap variable - std::unique_lock lock(mtx); - -#ifdef DEPTHAI_RESOURCES_TAR_XZ - // Return device firmware return getDeviceBinary(config); - -#else - - // Return device firmware - return getEmbeddedDeviceBinary(config); - -#endif } // Get device firmware diff --git a/src/utility/Resources.hpp b/src/utility/Resources.hpp index 027028346..fc5d705b4 100644 --- a/src/utility/Resources.hpp +++ b/src/utility/Resources.hpp @@ -31,7 +31,7 @@ class Resources { void operator=(Resources const&) = delete; // Available resources - std::vector getDeviceFirmware(bool usb2Mode, OpenVINO::Version version = OpenVINO::VERSION_2020_1); + std::vector getDeviceFirmware(bool usb2Mode, OpenVINO::Version version = Pipeline::DEFAULT_OPENVINO_VERSION); std::vector getDeviceFirmware(Device::Config config); std::vector getBootloaderFirmware(); From e37fd321bc617b4d48629f7525db5c76bd115efc Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 22 Jul 2021 12:25:48 +0200 Subject: [PATCH 005/101] Updated preboot and added watchdog configuration --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/device/Device.hpp | 10 +- shared/depthai-shared | 2 +- src/device/Device.cpp | 113 ++++++++++---------- src/utility/Resources.cpp | 33 +++--- src/utility/Resources.hpp | 2 +- 6 files changed, 86 insertions(+), 76 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index ca6442a07..abe061b71 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 "9cee56147e74ce7f0e070f938be91063c219786a") +set(DEPTHAI_DEVICE_SIDE_COMMIT "f7fedcfc1f8af380c6181f6daca5b569688bc508") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/device/Device.hpp b/include/depthai/device/Device.hpp index 4189adad6..f482bbe51 100644 --- a/include/depthai/device/Device.hpp +++ b/include/depthai/device/Device.hpp @@ -524,11 +524,11 @@ class Device { private: // private - void init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); - void init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd); - void init(OpenVINO::Version version, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd); - void init(const Pipeline& pipeline, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd); - void init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional pipeline); + void init(OpenVINO::Version version, bool usb2Mode, const std::string& pathToMvcmd); + void init(const Pipeline& pipeline, bool usb2Mode, const std::string& pathToMvcmd); + void init(OpenVINO::Version version, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd); + void init(const Pipeline& pipeline, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd); + void init2(Config cfg, const std::string& pathToMvcmd, tl::optional pipeline); void checkClosed() const; std::shared_ptr connection; diff --git a/shared/depthai-shared b/shared/depthai-shared index f3b2c82f2..47be3e59e 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit f3b2c82f24908931de1e403ae754c2238c35dfdf +Subproject commit 47be3e59e62e3c37ea2421892dc02c0a9349fdea diff --git a/src/device/Device.cpp b/src/device/Device.cpp index a6ea80129..0c7f4d2a9 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -27,6 +27,7 @@ // libraries #include "nanorpc/core/client.h" #include "nanorpc/packer/nlohmann_msgpack.h" +#include "spdlog/details/os.h" #include "spdlog/fmt/chrono.h" #include "spdlog/sinks/stdout_color_sinks.h" #include "spdlog/spdlog.h" @@ -221,19 +222,19 @@ LogLevel Device::Impl::getLogLevel() { /////////////////////////////////////////////// Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, bool usb2Mode) : deviceInfo(devInfo) { - init(pipeline, true, usb2Mode, ""); + init(pipeline, usb2Mode, ""); } Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, UsbSpeed maxUsbSpeed) : deviceInfo(devInfo) { - init(pipeline, true, maxUsbSpeed, ""); + init(pipeline, maxUsbSpeed, ""); } Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { - init(pipeline, false, false, std::string(pathToCmd)); + init(pipeline, false, std::string(pathToCmd)); } Device::Device(const Pipeline& pipeline, const DeviceInfo& devInfo, const std::string& pathToCmd) : deviceInfo(devInfo) { - init(pipeline, false, false, pathToCmd); + init(pipeline, false, pathToCmd); } Device::Device(const Pipeline& pipeline) { @@ -244,7 +245,7 @@ Device::Device(const Pipeline& pipeline) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(pipeline, true, false, ""); + init(pipeline, false, ""); } Device::Device(const Pipeline& pipeline, const char* pathToCmd) { @@ -255,7 +256,7 @@ Device::Device(const Pipeline& pipeline, const char* pathToCmd) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(pipeline, false, false, std::string(pathToCmd)); + init(pipeline, false, std::string(pathToCmd)); } Device::Device(const Pipeline& pipeline, const std::string& pathToCmd) { @@ -265,7 +266,7 @@ Device::Device(const Pipeline& pipeline, const std::string& pathToCmd) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(pipeline, false, false, pathToCmd); + init(pipeline, false, pathToCmd); } Device::Device(const Pipeline& pipeline, UsbSpeed maxUsbSpeed) { @@ -275,7 +276,7 @@ Device::Device(const Pipeline& pipeline, UsbSpeed maxUsbSpeed) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(pipeline, true, maxUsbSpeed, ""); + init(pipeline, maxUsbSpeed, ""); } Device::Device(const Pipeline& pipeline, bool usb2Mode) { @@ -285,23 +286,23 @@ Device::Device(const Pipeline& pipeline, bool usb2Mode) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(pipeline, true, usb2Mode, ""); + init(pipeline, usb2Mode, ""); } Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, bool usb2Mode) : deviceInfo(devInfo) { - init(version, true, usb2Mode, ""); + init(version, usb2Mode, ""); } Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, UsbSpeed maxUsbSpeed) : deviceInfo(devInfo) { - init(version, true, maxUsbSpeed, ""); + init(version, maxUsbSpeed, ""); } Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, const char* pathToCmd) : deviceInfo(devInfo) { - init(version, false, false, std::string(pathToCmd)); + init(version, false, std::string(pathToCmd)); } Device::Device(OpenVINO::Version version, const DeviceInfo& devInfo, const std::string& pathToCmd) : deviceInfo(devInfo) { - init(version, false, false, pathToCmd); + init(version, false, pathToCmd); } Device::Device(OpenVINO::Version version) { @@ -312,7 +313,7 @@ Device::Device(OpenVINO::Version version) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(version, true, false, ""); + init(version, false, ""); } Device::Device(OpenVINO::Version version, const char* pathToCmd) { @@ -323,7 +324,7 @@ Device::Device(OpenVINO::Version version, const char* pathToCmd) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(version, false, false, std::string(pathToCmd)); + init(version, false, std::string(pathToCmd)); } Device::Device(OpenVINO::Version version, const std::string& pathToCmd) { @@ -333,7 +334,7 @@ Device::Device(OpenVINO::Version version, const std::string& pathToCmd) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(version, false, false, pathToCmd); + init(version, false, pathToCmd); } Device::Device(OpenVINO::Version version, bool usb2Mode) { @@ -343,7 +344,7 @@ Device::Device(OpenVINO::Version version, bool usb2Mode) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(version, true, usb2Mode, ""); + init(version, usb2Mode, ""); } Device::Device(OpenVINO::Version version, UsbSpeed maxUsbSpeed) { @@ -353,11 +354,12 @@ Device::Device(OpenVINO::Version version, UsbSpeed maxUsbSpeed) { // If no device found, throw if(!found) throw std::runtime_error("No available devices"); - init(version, true, maxUsbSpeed, ""); + init(version, maxUsbSpeed, ""); } Device::Device(const DeviceInfo& devInfo, Config config) { - init2(config, false, {}, {}); + deviceInfo = devInfo; + init2(config, {}, {}); } void Device::close() { @@ -415,40 +417,40 @@ Device::~Device() { close(); } -void Device::init(OpenVINO::Version version, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { +void Device::init(OpenVINO::Version version, bool usb2Mode, const std::string& pathToMvcmd) { Config cfg; // Specify usb speed - cfg.preboot.maxUsbSpeed = usb2Mode ? UsbSpeed::HIGH : Device::DEFAULT_USB_SPEED; + cfg.preboot.usb.maxSpeed = usb2Mode ? UsbSpeed::HIGH : Device::DEFAULT_USB_SPEED; // Specify the OpenVINO version cfg.version = version; - init2(cfg, embeddedMvcmd, pathToMvcmd, {}); + init2(cfg, pathToMvcmd, {}); } -void Device::init(const Pipeline& pipeline, bool embeddedMvcmd, bool usb2Mode, const std::string& pathToMvcmd) { +void Device::init(const Pipeline& pipeline, bool usb2Mode, const std::string& pathToMvcmd) { Config cfg; // Specify usb speed - cfg.preboot.maxUsbSpeed = usb2Mode ? UsbSpeed::HIGH : Device::DEFAULT_USB_SPEED; + cfg.preboot.usb.maxSpeed = usb2Mode ? UsbSpeed::HIGH : Device::DEFAULT_USB_SPEED; // Specify the OpenVINO version cfg.version = pipeline.getOpenVINOVersion(); - init2(cfg, embeddedMvcmd, pathToMvcmd, pipeline); + init2(cfg, pathToMvcmd, pipeline); } -void Device::init(OpenVINO::Version version, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd) { +void Device::init(OpenVINO::Version version, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd) { Config cfg; // Specify usb speed - cfg.preboot.maxUsbSpeed = maxUsbSpeed; + cfg.preboot.usb.maxSpeed = maxUsbSpeed; // Specify the OpenVINO version cfg.version = version; - init2(cfg, embeddedMvcmd, pathToMvcmd, {}); + init2(cfg, pathToMvcmd, {}); } -void Device::init(const Pipeline& pipeline, bool embeddedMvcmd, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd) { +void Device::init(const Pipeline& pipeline, UsbSpeed maxUsbSpeed, const std::string& pathToMvcmd) { Config cfg; // Specify usb speed - cfg.preboot.maxUsbSpeed = maxUsbSpeed; + cfg.preboot.usb.maxSpeed = maxUsbSpeed; // Specify the OpenVINO version cfg.version = pipeline.getOpenVINOVersion(); - init2(cfg, embeddedMvcmd, pathToMvcmd, pipeline); + init2(cfg, pathToMvcmd, pipeline); } -void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional pipeline) { +void Device::init2(Config cfg, const std::string& pathToMvcmd, tl::optional pipeline) { // Initalize depthai library if not already initialize(); @@ -464,18 +466,27 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm // Set logging pattern of device (device id + shared pattern) pimpl->setPattern(fmt::format("[{}] {}", deviceInfo.getMxId(), LOG_DEFAULT_PATTERN)); - // Get embedded mvcmd - std::vector embeddedFw = Resources::getInstance().getDeviceFirmware(config); + // Check if WD env var is set + std::chrono::milliseconds watchdogTimeout = device::XLINK_WATCHDOG_TIMEOUT; + auto watchdogMsStr = spdlog::details::os::getenv("DEPTHAI_WATCHDOG"); + if(!watchdogMsStr.empty()) { + // Try parsing the string as a number + try { + std::chrono::milliseconds watchdog{std::stoi(watchdogMsStr)}; + config.preboot.watchdogTimeoutMs = watchdog.count(); + watchdogTimeout = watchdog; + } catch(const std::invalid_argument e) { + spdlog::warn("DEPTHAI_WATCHDOG value invalid: {}", e.what()); + } + } + + // Get embedded mvcmd or external with applied config + std::vector fwWithConfig = Resources::getInstance().getDeviceFirmware(config, pathToMvcmd); // Init device (if bootloader, handle correctly - issue USB boot command) if(deviceInfo.state == X_LINK_UNBOOTED) { // Unbooted device found, boot and connect with XLinkConnection constructor - if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, embeddedFw); - } else { - connection = std::make_shared(deviceInfo, pathToMvcmd); - } - + connection = std::make_shared(deviceInfo, fwWithConfig); } else if(deviceInfo.state == X_LINK_BOOTLOADER) { // Scope so bootloaderConnection is desctructed and XLink cleans its state { @@ -518,8 +529,8 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm if(version >= DeviceBootloader::Version(0, 0, 12)) { // Send request to boot firmware directly from bootloader dai::bootloader::request::BootMemory bootMemory; - bootMemory.totalSize = static_cast(embeddedFw.size()); - bootMemory.numPackets = ((static_cast(embeddedFw.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; + bootMemory.totalSize = static_cast(fwWithConfig.size()); + bootMemory.numPackets = ((static_cast(fwWithConfig.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; if(!sendBootloaderRequest(stream.getStreamId(), bootMemory)) { throw std::runtime_error("Error trying to connect to device"); } @@ -527,7 +538,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm using namespace std::chrono; auto t1 = steady_clock::now(); // After that send numPackets of data - stream.writeSplit(embeddedFw.data(), embeddedFw.size(), bootloader::XLINK_STREAM_MAX_SIZE); + stream.writeSplit(fwWithConfig.data(), fwWithConfig.size(), bootloader::XLINK_STREAM_MAX_SIZE); spdlog::debug( "Booting FW with Bootloader. Version {}, Time taken: {}", version.toString(), duration_cast(steady_clock::now() - t1)); @@ -559,19 +570,11 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm } // Boot and connect with XLinkConnection constructor - if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, embeddedFw); - } else { - connection = std::make_shared(deviceInfo, pathToMvcmd); - } + connection = std::make_shared(deviceInfo, fwWithConfig); } else if(deviceInfo.state == X_LINK_BOOTED) { // Connect without booting - if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, embeddedFw); - } else { - connection = std::make_shared(deviceInfo, pathToMvcmd); - } + connection = std::make_shared(deviceInfo, fwWithConfig); } else { throw std::runtime_error("Cannot find any device with given deviceInfo"); } @@ -600,7 +603,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm }); // prepare watchdog thread, which will keep device alive - watchdogThread = std::thread([this]() { + watchdogThread = std::thread([this, watchdogTimeout]() { std::shared_ptr conn = this->connection; while(watchdogRunning) { try { @@ -609,7 +612,7 @@ void Device::init2(Config cfg, bool embeddedMvcmd, const std::string& pathToMvcm break; } // Ping with a period half of that of the watchdog timeout - std::this_thread::sleep_for(device::XLINK_WATCHDOG_TIMEOUT / 2); + std::this_thread::sleep_for(watchdogTimeout / 2); } // Watchdog ended. Useful for checking disconnects diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 42502d236..94e39c792 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -61,7 +61,7 @@ constexpr static auto RESOURCE_LIST_DEVICE = array_of(DEPTHAI_CMD_O DEPTHAI_CMD_OPENVINO_2021_2_PATCH_PATH, DEPTHAI_CMD_OPENVINO_2021_3_PATCH_PATH); -std::vector Resources::getDeviceBinary(Device::Config config) { +std::vector Resources::getDeviceFirmware(Device::Config config, std::string pathToMvcmd) { // Acquire mutex (this mutex signifies that lazy load is complete) // It is necessary when accessing resourceMap variable std::unique_lock lock(mtxDevice); @@ -71,11 +71,20 @@ std::vector Resources::getDeviceBinary(Device::Config config) { // Get OpenVINO version auto& version = config.version; - // Check if env variable DEPTHAI_DEVICE_BINARY is set + // Check if pathToMvcmd variable is set + std::string finalFwBinaryPath = ""; + if(!pathToMvcmd.empty()) { + finalFwBinaryPath = pathToMvcmd; + } + // Override if env variable DEPTHAI_DEVICE_BINARY is set auto fwBinaryPath = spdlog::details::os::getenv("DEPTHAI_DEVICE_BINARY"); if(!fwBinaryPath.empty()) { + finalFwBinaryPath = fwBinaryPath; + } + // Return binary from file if any of above paths are present + if(!finalFwBinaryPath.empty()) { // Load binary file at path - std::ifstream stream(fwBinaryPath, std::ios::binary); + std::ifstream stream(finalFwBinaryPath, std::ios::binary); if(!stream.is_open()) { // Throw an error // TODO(themarpe) - Unify exceptions into meaningful groups @@ -88,9 +97,9 @@ std::vector Resources::getDeviceBinary(Device::Config config) { #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES // Main FW - std::vector depthaiBinary{}; + std::vector depthaiBinary; // Patch from main to specified - std::vector depthaiPatch{}; + std::vector depthaiPatch; switch(version) { case OpenVINO::VERSION_2020_3: @@ -134,7 +143,10 @@ std::vector Resources::getDeviceBinary(Device::Config config) { int error = bspatch_mem(depthaiBinary.data(), depthaiBinary.size(), depthaiPatch.data(), depthaiPatch.size(), tmpDepthaiBinary.data()); // if patch not successful - if(error > 0) throw std::runtime_error("Error while patching cmd for usb2 mode"); + if(error > 0) { + throw std::runtime_error(fmt::format( + "Error while patching OpenVINO FW version from {} to {}", OpenVINO::getVersionName(MAIN_FW_VERSION), OpenVINO::getVersionName(version))); + } // Change depthaiBinary to tmpDepthaiBinary depthaiBinary = std::move(tmpDepthaiBinary); @@ -326,18 +338,13 @@ Resources::~Resources() { if(lazyThreadBootloader.joinable()) lazyThreadBootloader.join(); } -std::vector Resources::getDeviceFirmware(Device::Config config) { - // Return device firmware - return getDeviceBinary(config); -} - // Get device firmware std::vector Resources::getDeviceFirmware(bool usb2Mode, OpenVINO::Version version) { Device::Config cfg; if(usb2Mode) { - cfg.preboot.maxUsbSpeed = UsbSpeed::HIGH; + cfg.preboot.usb.maxSpeed = UsbSpeed::HIGH; } else { - cfg.preboot.maxUsbSpeed = Device::DEFAULT_USB_SPEED; + cfg.preboot.usb.maxSpeed = Device::DEFAULT_USB_SPEED; } cfg.version = version; diff --git a/src/utility/Resources.hpp b/src/utility/Resources.hpp index a19c10431..77e8d888b 100644 --- a/src/utility/Resources.hpp +++ b/src/utility/Resources.hpp @@ -36,7 +36,7 @@ class Resources { // Available resources std::vector getDeviceFirmware(bool usb2Mode, OpenVINO::Version version = Pipeline::DEFAULT_OPENVINO_VERSION); - std::vector getDeviceFirmware(Device::Config config); + std::vector getDeviceFirmware(Device::Config config, std::string pathToMvcmd = ""); std::vector getBootloaderFirmware(DeviceBootloader::Type type = DeviceBootloader::Type::USB); }; From 64f1aba871829c1cb49ae78e887f793c360b417a Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 22 Jul 2021 13:14:36 +0200 Subject: [PATCH 006/101] Modified watchdog to use a separate stream --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- shared/depthai-shared | 2 +- src/device/Device.cpp | 19 +++++++++++-------- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index abe061b71..85a6c69c6 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 "f7fedcfc1f8af380c6181f6daca5b569688bc508") +set(DEPTHAI_DEVICE_SIDE_COMMIT "08bcc9966ca8df97aa4b0ccb45d03951fd4f1fbd") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index 47be3e59e..07ecb24a7 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 47be3e59e62e3c37ea2421892dc02c0a9349fdea +Subproject commit 07ecb24a7f57c64f82e43ed295712d7113d06eea diff --git a/src/device/Device.cpp b/src/device/Device.cpp index 0c7f4d2a9..7ad83b684 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -603,16 +603,19 @@ void Device::init2(Config cfg, const std::string& pathToMvcmd, tl::optional conn = this->connection; - while(watchdogRunning) { - try { - pimpl->rpcClient->call("watchdogKeepalive"); - } catch(const std::exception&) { - break; + try { + XLinkStream stream(*this->connection, device::XLINK_CHANNEL_WATCHDOG, 128); + std::vector watchdogKeepalive = {0, 0, 0, 0}; + while(watchdogRunning) { + stream.write(watchdogKeepalive); + // Ping with a period half of that of the watchdog timeout + std::this_thread::sleep_for(watchdogTimeout / 2); } - // Ping with a period half of that of the watchdog timeout - std::this_thread::sleep_for(watchdogTimeout / 2); + } catch(const std::exception& ex) { + // ignore + spdlog::debug("Watchdog thread exception caught: {}", ex.what()); } // Watchdog ended. Useful for checking disconnects From d6d95e25d0188c1029be97cc59725fdcd455d021 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 22 Jul 2021 13:43:49 +0200 Subject: [PATCH 007/101] Fixed patching --- src/utility/Resources.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 94e39c792..912e1295e 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -123,15 +123,17 @@ std::vector Resources::getDeviceFirmware(Device::Config config, st break; case MAIN_FW_VERSION: - spdlog::debug("resourceMapDevice[{}] size: {}", MAIN_FW_PATH, resourceMapDevice[MAIN_FW_PATH].size()); depthaiBinary = resourceMapDevice[MAIN_FW_PATH]; break; } // is patching required? - if(version != MAIN_FW_VERSION) { + if(!depthaiPatch.empty()) { spdlog::debug("Patching OpenVINO FW version from {} to {}", OpenVINO::getVersionName(MAIN_FW_VERSION), OpenVINO::getVersionName(version)); + // Load full binary for patch + depthaiBinary = resourceMapDevice[MAIN_FW_PATH]; + // Get new size int64_t patchedSize = bspatch_mem_get_newsize(depthaiPatch.data(), depthaiPatch.size()); From 23319c3a25cd7d5cc43541b81d41b0c1922d429d Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sat, 24 Jul 2021 22:35:27 +0200 Subject: [PATCH 008/101] Updated FW and a catch clause --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/Pipeline.hpp | 2 +- src/device/Device.cpp | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 98420a121..c9b2679cc 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 "ba0f81ce8fa357040a6bac4435468ec14f31c358") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b5d2239124f9b66d056222676e9e845824329928") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/Pipeline.hpp b/include/depthai/pipeline/Pipeline.hpp index 5e055012e..153db114a 100644 --- a/include/depthai/pipeline/Pipeline.hpp +++ b/include/depthai/pipeline/Pipeline.hpp @@ -250,7 +250,7 @@ class Pipeline { return impl()->isOpenVINOVersionCompatible(version); } - /// Checks whether a given OpenVINO version is compatible with the pipeline + /// Get device configuration needed for this pipeline Device::Config getDeviceConfig() const { return impl()->getDeviceConfig(); } diff --git a/src/device/Device.cpp b/src/device/Device.cpp index a1fc064ed..ee5060472 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -481,7 +481,8 @@ void Device::init2(Config cfg, const std::string& pathToMvcmd, tl::optional Date: Fri, 23 Jul 2021 18:52:35 +0300 Subject: [PATCH 009/101] Revert "Removed flash_bootloader example temporarily" This reverts commit ee2a04e58b995e1bfa0cb03b91f83a45d446ca7f. --- examples/src/flash_bootloader.cpp | 40 +++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 examples/src/flash_bootloader.cpp diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp new file mode 100644 index 000000000..0579e89eb --- /dev/null +++ b/examples/src/flash_bootloader.cpp @@ -0,0 +1,40 @@ +#include +#include + +#include "depthai/depthai.hpp" + +int main(int argc, char** argv) { + using namespace std::chrono; + + dai::DeviceBootloader::Type blType = dai::DeviceBootloader::Type::USB; + if(argc > 1) { + if(std::string(argv[1]) == "usb") { + blType = dai::DeviceBootloader::Type::USB; + } else if(std::string(argv[1]) == "eth") { + blType = dai::DeviceBootloader::Type::NETWORK; + } else { + std::cout << "Specify either 'usb' or 'eth' bootloader type\n"; + return 0; + } + } else { + std::cout << "Usage: " << argv[0] << " \n"; + return 0; + } + + bool res = false; + dai::DeviceInfo info; + std::tie(res, info) = dai::DeviceBootloader::getFirstAvailableDevice(); + + dai::DeviceBootloader bl(info); + auto progress = [](float p) { std::cout << "Flashing Progress..." << p * 100 << "%" << std::endl; }; + + std::string message; + auto t1 = steady_clock::now(); + std::tie(res, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, blType, progress); + if(res) { + std::cout << "Flashing successful. Took " << duration_cast(steady_clock::now() - t1).count() << "ms" << std::endl; + } else { + std::cout << "Flashing failed: " << message << std::endl; + } + return 0; +} \ No newline at end of file From 7ce7614ece441d1f03b64d85976fc946b7bc51fa Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Fri, 23 Jul 2021 18:52:42 +0300 Subject: [PATCH 010/101] Revert "Removed flash_bootloader" This reverts commit f1f03bcefde92b518fe5a1534b83c3fa919e30e6. --- examples/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 75327436e..6573918e6 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -268,7 +268,7 @@ target_compile_definitions(object_tracker_video PRIVATE BLOB_PATH="${person_dete dai_add_example(queue_add_callback src/queue_add_callback.cpp ON) dai_add_example(bootloader_version src/bootloader_version.cpp ON) -#dai_add_example(flash_bootloader src/flash_bootloader.cpp OFF) +dai_add_example(flash_bootloader src/flash_bootloader.cpp OFF) dai_add_example(edge_detector src/edge_detector.cpp ON) # Calibration Read and write samples From eb3defc3b3cc40880d6e24f5ceb5fa37919c2ca0 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 28 Jul 2021 20:14:11 +0300 Subject: [PATCH 011/101] Update bootloader: support for more NOR flash chips, fixes issues with flash erasing --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 6 ++++-- shared/depthai-bootloader-shared | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 780c2313f..7441ed04b 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -1,5 +1,7 @@ # Maturity level "snapshot" / "release" -set(DEPTHAI_BOOTLOADER_MATURITY "release") +#set(DEPTHAI_BOOTLOADER_MATURITY "release") +set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_BOOTLOADER_VERSION "0.0.12") +#set(DEPTHAI_BOOTLOADER_VERSION "0.0.12") +set(DEPTHAI_BOOTLOADER_VERSION "68b296bf866b11174127290b46f9a5f4966102c1") diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 7efb3e188..391e3431f 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 7efb3e188715844d7b2b0e50861e4bfc36087370 +Subproject commit 391e3431fcc6f271f60584603697edf664bbb59c From c1e4180f364711655251ac5b7e0d2b5d412416c8 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 28 Jul 2021 20:17:14 +0300 Subject: [PATCH 012/101] Optional env var DEPTHAI_BOOTLOADER_BINARY to override bootloader FW path, mostly for development --- src/utility/Resources.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 3f8e00f19..da6697b5b 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -191,6 +191,20 @@ constexpr static std::array RESOURCE_LIST_BOOTLOADER = { }; std::vector Resources::getBootloaderFirmware(dai::bootloader::Type type) { + // Check if env variable DEPTHAI_BOOTLOADER_BINARY is set + auto blBinaryPath = spdlog::details::os::getenv("DEPTHAI_BOOTLOADER_BINARY"); + if(!blBinaryPath.empty()) { + // Load binary file at path + std::ifstream stream(blBinaryPath, std::ios::binary); + if(!stream.is_open()) { + // Throw an error + // TODO(themarpe) - Unify exceptions into meaningful groups + throw std::runtime_error(fmt::format("File at path {} pointed to by DEPTHAI_BOOTLOADER_BINARY doesn't exist.", blBinaryPath)); + } + // Read the file and return its content + return std::vector(std::istreambuf_iterator(stream), {}); + } + // Acquire mutex (this mutex signifies that lazy load is complete) // It is necessary when accessing resourceMap variable std::unique_lock lock(mtxBootloader); From 92a5c0dca19a24619f9513e29bb625be29951844 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Wed, 28 Jul 2021 20:19:11 +0300 Subject: [PATCH 013/101] Warn when firmware or bootloader binaries are overriden - to confirm it's picked up, or to notice when forgotten exported --- src/utility/Resources.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index da6697b5b..8eaba3e3b 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -78,6 +78,7 @@ std::vector Resources::getDeviceBinary(OpenVINO::Version version, // TODO(themarpe) - Unify exceptions into meaningful groups throw std::runtime_error(fmt::format("File at path {} pointed to by DEPTHAI_DEVICE_BINARY doesn't exist.", fwBinaryPath)); } + spdlog::warn("Overriding firmware: {}", fwBinaryPath); // Read the file and return its contents return std::vector(std::istreambuf_iterator(stream), {}); } @@ -201,6 +202,7 @@ std::vector Resources::getBootloaderFirmware(dai::bootloader::Type // TODO(themarpe) - Unify exceptions into meaningful groups throw std::runtime_error(fmt::format("File at path {} pointed to by DEPTHAI_BOOTLOADER_BINARY doesn't exist.", blBinaryPath)); } + spdlog::warn("Overriding bootloader: {}", blBinaryPath); // Read the file and return its content return std::vector(std::istreambuf_iterator(stream), {}); } From d2946aa84d2d3dfb2a9839dc2aea111c0920ccc1 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 29 Jul 2021 03:38:08 +0200 Subject: [PATCH 014/101] Moved operator<< overloads to global namespace --- include/depthai/common/CameraBoardSocket.hpp | 15 ++- include/depthai/common/UsbSpeed.hpp | 19 ++-- .../depthai/pipeline/datatype/Tracklets.hpp | 17 ++-- src/utility/BootloaderHelper.hpp | 98 +++++++++---------- 4 files changed, 72 insertions(+), 77 deletions(-) diff --git a/include/depthai/common/CameraBoardSocket.hpp b/include/depthai/common/CameraBoardSocket.hpp index c9871c918..9a2586c6b 100644 --- a/include/depthai/common/CameraBoardSocket.hpp +++ b/include/depthai/common/CameraBoardSocket.hpp @@ -4,24 +4,21 @@ #include "depthai-shared/common/CameraBoardSocket.hpp" -namespace dai { - -inline std::ostream& operator<<(std::ostream& out, const CameraBoardSocket& socket) { +// Global namespace +inline std::ostream& operator<<(std::ostream& out, const dai::CameraBoardSocket& socket) { switch(socket) { - case CameraBoardSocket::AUTO: + case dai::CameraBoardSocket::AUTO: out << "AUTO"; break; - case CameraBoardSocket::RGB: + case dai::CameraBoardSocket::RGB: out << "RGB"; break; - case CameraBoardSocket::LEFT: + case dai::CameraBoardSocket::LEFT: out << "LEFT"; break; - case CameraBoardSocket::RIGHT: + case dai::CameraBoardSocket::RIGHT: out << "RIGHT"; break; } return out; } - -} // namespace dai \ No newline at end of file diff --git a/include/depthai/common/UsbSpeed.hpp b/include/depthai/common/UsbSpeed.hpp index e53346bb8..aef2dc45e 100644 --- a/include/depthai/common/UsbSpeed.hpp +++ b/include/depthai/common/UsbSpeed.hpp @@ -4,30 +4,27 @@ #include "depthai-shared/common/UsbSpeed.hpp" -namespace dai { - -inline std::ostream& operator<<(std::ostream& out, const UsbSpeed& speed) { +// Global namespace +inline std::ostream& operator<<(std::ostream& out, const dai::UsbSpeed& speed) { switch(speed) { - case UsbSpeed::UNKNOWN: + case dai::UsbSpeed::UNKNOWN: out << "UNKNOWN"; break; - case UsbSpeed::LOW: + case dai::UsbSpeed::LOW: out << "LOW"; break; - case UsbSpeed::FULL: + case dai::UsbSpeed::FULL: out << "FULL"; break; - case UsbSpeed::HIGH: + case dai::UsbSpeed::HIGH: out << "HIGH"; break; - case UsbSpeed::SUPER: + case dai::UsbSpeed::SUPER: out << "SUPER"; break; - case UsbSpeed::SUPER_PLUS: + case dai::UsbSpeed::SUPER_PLUS: out << "SUPER_PLUS"; break; } return out; } - -} // namespace dai diff --git a/include/depthai/pipeline/datatype/Tracklets.hpp b/include/depthai/pipeline/datatype/Tracklets.hpp index 81d77bdec..8dd483ecc 100644 --- a/include/depthai/pipeline/datatype/Tracklets.hpp +++ b/include/depthai/pipeline/datatype/Tracklets.hpp @@ -31,22 +31,23 @@ class Tracklets : public Buffer { std::vector& tracklets; }; -inline std::ostream& operator<<(std::ostream& out, const Tracklet::TrackingStatus& status) { +} // namespace dai + +// Global namespace +inline std::ostream& operator<<(std::ostream& out, const dai::Tracklet::TrackingStatus& status) { switch(status) { - case Tracklet::TrackingStatus::NEW: + case dai::Tracklet::TrackingStatus::NEW: out << "NEW"; break; - case Tracklet::TrackingStatus::TRACKED: + case dai::Tracklet::TrackingStatus::TRACKED: out << "TRACKED"; break; - case Tracklet::TrackingStatus::LOST: + case dai::Tracklet::TrackingStatus::LOST: out << "LOST"; break; - case Tracklet::TrackingStatus::REMOVED: + case dai::Tracklet::TrackingStatus::REMOVED: out << "REMOVED"; break; } return out; -} - -} // namespace dai +} \ No newline at end of file diff --git a/src/utility/BootloaderHelper.hpp b/src/utility/BootloaderHelper.hpp index 8fd11b3f4..79e6678eb 100644 --- a/src/utility/BootloaderHelper.hpp +++ b/src/utility/BootloaderHelper.hpp @@ -13,55 +13,55 @@ namespace dai { - template - bool sendBootloaderRequest(streamId_t streamId, T request){ - if(XLinkWriteData(streamId, (uint8_t*) &request, sizeof(T)) != X_LINK_SUCCESS) return false; - return true; - } - - inline bool receiveBootloaderResponseData(streamId_t streamId, std::vector& data){ - data = std::vector(); - - streamPacketDesc_t* pPacket; - if(XLinkReadData(streamId, &pPacket) != X_LINK_SUCCESS) return false; - - // Resize vector - data.resize(pPacket->length); - - // copy data - memcpy(data.data(), pPacket->data, pPacket->length); - - // release data - if(XLinkReleaseData(streamId) != X_LINK_SUCCESS) return false; - - return true; - } - - template - bool parseBootloaderResponse(const std::vector& data, T& response){ - // Checks that 'data' is type T - dai::bootloader::response::Command command; - if(data.size() < sizeof(command)) return false; - memcpy(&command, data.data(), sizeof(command)); - if(response.cmd != command) return false; - if(data.size() < sizeof(response)) return false; - - // If yes, memcpy to response - memcpy(&response, data.data(), sizeof(response)); - return true; - } - - template - bool receiveBootloaderResponse(streamId_t streamId, T& response){ - // Receive data first - std::vector data; - if(!receiveBootloaderResponseData(streamId, data)) return false; - - // Then try to parse - if(!parseBootloaderResponse(data, response)) return false; - - return true; - } +template +bool sendBootloaderRequest(streamId_t streamId, T request){ + if(XLinkWriteData(streamId, (uint8_t*) &request, sizeof(T)) != X_LINK_SUCCESS) return false; + return true; +} + +inline bool receiveBootloaderResponseData(streamId_t streamId, std::vector& data){ + data = std::vector(); + + streamPacketDesc_t* pPacket; + if(XLinkReadData(streamId, &pPacket) != X_LINK_SUCCESS) return false; + + // Resize vector + data.resize(pPacket->length); + + // copy data + memcpy(data.data(), pPacket->data, pPacket->length); + + // release data + if(XLinkReleaseData(streamId) != X_LINK_SUCCESS) return false; + + return true; +} + +template +bool parseBootloaderResponse(const std::vector& data, T& response){ + // Checks that 'data' is type T + dai::bootloader::response::Command command; + if(data.size() < sizeof(command)) return false; + memcpy(&command, data.data(), sizeof(command)); + if(response.cmd != command) return false; + if(data.size() < sizeof(response)) return false; + + // If yes, memcpy to response + memcpy(&response, data.data(), sizeof(response)); + return true; +} + +template +bool receiveBootloaderResponse(streamId_t streamId, T& response){ + // Receive data first + std::vector data; + if(!receiveBootloaderResponseData(streamId, data)) return false; + + // Then try to parse + if(!parseBootloaderResponse(data, response)) return false; + + return true; +} } // namespace dai From 78fdd289dd39c377f6a52ad2a09c01e3ba3dee87 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 29 Jul 2021 03:40:31 +0200 Subject: [PATCH 015/101] Added an explicit flag to allow flashing bootloader --- include/depthai/device/DeviceBootloader.hpp | 80 ++++++++++++++++++--- src/device/DeviceBootloader.cpp | 67 +++++++++++------ 2 files changed, 117 insertions(+), 30 deletions(-) diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index fb5fcb40d..8cc03cf9d 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -39,16 +39,16 @@ class DeviceBootloader { Version(unsigned major, unsigned minor, unsigned patch); bool operator==(const Version& other) const; bool operator<(const Version& other) const; - inline bool operator!=(const Version& rhs) { + inline bool operator!=(const Version& rhs) const { return !(*this == rhs); } - inline bool operator>(const Version& rhs) { + inline bool operator>(const Version& rhs) const { return rhs < *this; } - inline bool operator<=(const Version& rhs) { + inline bool operator<=(const Version& rhs) const { return !(*this > rhs); } - inline bool operator>=(const Version& rhs) { + inline bool operator>=(const Version& rhs) const { return !(*this < rhs); } /// Convert Version to string @@ -107,28 +107,31 @@ class DeviceBootloader { /** * Connects to or boots device in bootloader mode depending on devInfo state. * @param devInfo DeviceInfo of which to boot or connect to + * @param allowFlashingBootloader Set to true to allow flashing the devices bootloader. Defaults to false */ - explicit DeviceBootloader(const DeviceInfo& devInfo); + explicit DeviceBootloader(const DeviceInfo& devInfo, bool allowFlashingBootloader = false); /** * Connects to device in bootloader of specified type. Throws if it wasn't possible. * This constructor will automatically boot into specified bootloader type if not already running * @param devInfo DeviceInfo of which to boot or connect to * @param type Type of bootloader to boot/connect to. + * @param allowFlashingBootloader Set to true to allow flashing the devices bootloader. Defaults to false */ - DeviceBootloader(const DeviceInfo& devInfo, Type type); + DeviceBootloader(const DeviceInfo& devInfo, Type type, bool allowFlashingBootloader = false); /** * Connects to or boots device in bootloader mode depending on devInfo state with a custom bootloader firmware. * @param devInfo DeviceInfo of which to boot or connect to * @param pathToBootloader Custom bootloader firmware to boot + * @param allowFlashingBootloader Set to true to allow flashing the devices bootloader. Defaults to false */ - DeviceBootloader(const DeviceInfo& devInfo, const std::string& pathToBootloader); + DeviceBootloader(const DeviceInfo& devInfo, const std::string& pathToBootloader, bool allowFlashingBootloader = false); /** * @overload */ - DeviceBootloader(const DeviceInfo& devInfo, const char* pathToBootloader); + DeviceBootloader(const DeviceInfo& devInfo, const char* pathToBootloader, bool allowFlashingBootloader = false); ~DeviceBootloader(); /** @@ -176,10 +179,21 @@ class DeviceBootloader { Version getVersion(); /** - * @returns True whether the bootloader running is flashed or booted by library + * @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. */ bool isEmbeddedVersion() const; + /** + * @returns Type of currently connected bootloader + */ + Type getType(); + + /** + * @returns True if allowed to flash bootloader + */ + bool isAllowedFlashingBootloader(); + /** * Explicitly closes connection to device. * @note This function does not need to be explicitly called @@ -196,7 +210,7 @@ class DeviceBootloader { // private static // private variables - void init(bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional type); + void init(bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional type, bool allowBlFlash); void checkClosed() const; std::shared_ptr connection; @@ -214,6 +228,52 @@ class DeviceBootloader { // bootloader stream std::unique_ptr stream; + + // Allow flashing bootloader flag + bool allowFlashingBootloader = false; }; } // namespace dai + +// Global namespace +inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Type& type) { + switch(type) { + case dai::DeviceBootloader::Type::USB: + out << "USB"; + break; + case dai::DeviceBootloader::Type::NETWORK: + out << "NETWORK"; + break; + } + return out; +} + +inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Memory& memory) { + switch(memory) { + case dai::DeviceBootloader::Memory::FLASH: + out << "FLASH"; + break; + case dai::DeviceBootloader::Memory::EMMC: + out << "EMMC"; + break; + } + return out; +} + +inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Section& type) { + switch(type) { + case dai::DeviceBootloader::Section::HEADER: + out << "HEADER"; + break; + case dai::DeviceBootloader::Section::BOOTLOADER: + out << "BOOTLOADER"; + break; + case dai::DeviceBootloader::Section::BOOTLOADER_CONFIG: + out << "BOOTLOADER_CONFIG"; + break; + case dai::DeviceBootloader::Section::APPLICATION: + out << "APPLICATION"; + break; + } + return out; +} \ No newline at end of file diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 43ebc7667..223673cc3 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -141,24 +141,25 @@ std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& return fwPackage; } -DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo) : deviceInfo(devInfo) { - init(true, "", tl::nullopt); +DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, bool allowFlashingBootloader) : deviceInfo(devInfo) { + init(true, "", tl::nullopt, allowFlashingBootloader); } -DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, Type type) : deviceInfo(devInfo) { - init(true, "", type); +DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, Type type, bool allowFlashingBootloader) : deviceInfo(devInfo) { + init(true, "", type, allowFlashingBootloader); } -DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, const char* pathToBootloader) : deviceInfo(devInfo) { - init(false, std::string(pathToBootloader), tl::nullopt); +DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, const char* pathToBootloader, bool allowFlashingBootloader) : deviceInfo(devInfo) { + init(false, std::string(pathToBootloader), tl::nullopt, allowFlashingBootloader); } -DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, const std::string& pathToBootloader) : deviceInfo(devInfo) { - init(false, pathToBootloader, tl::nullopt); +DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, const std::string& pathToBootloader, bool allowFlashingBootloader) : deviceInfo(devInfo) { + init(false, pathToBootloader, tl::nullopt, allowFlashingBootloader); } -void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional type) { +void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional type, bool allowBlFlash) { stream = nullptr; + allowFlashingBootloader = allowBlFlash; bootloaderType = type.value_or(DEFAULT_TYPE); @@ -194,8 +195,9 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, if(!receiveBootloaderResponse(stream->getStreamId(), ver)) throw std::runtime_error("Error trying to connect to device"); DeviceBootloader::Version version(ver.major, ver.minor, ver.patch); - // If version is adequite if(version >= Version(0, 0, 12)) { + // If version is adequate, do an in memory boot. + // Send request for bootloader type if(!sendBootloaderRequest(stream->getStreamId(), bootloader::request::GetBootloaderType{})) { throw std::runtime_error("Error trying to connect to device"); @@ -207,8 +209,10 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // Modify actual bootloader type bootloaderType = runningBootloaderType.type; - // Boot memory correct type of BL - if(type && runningBootloaderType.type != *type) { + Type desiredBootloaderType = type.value_or(bootloaderType); + + // If not correct type OR if allowFlashingBootloader is set, then boot internal (latest) bootloader of correct type + if((desiredBootloaderType != bootloaderType) || allowFlashingBootloader) { // prepare watchdog thread, which will keep device alive std::atomic wdRunning{true}; std::thread wd = std::thread([&]() { @@ -228,7 +232,7 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // Send request to boot firmware directly from bootloader dai::bootloader::request::BootMemory bootMemory; - auto binary = getEmbeddedBootloaderBinary(*type); + auto binary = getEmbeddedBootloaderBinary(desiredBootloaderType); bootMemory.totalSize = static_cast(binary.size()); bootMemory.numPackets = ((static_cast(binary.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; if(!sendBootloaderRequest(stream->getStreamId(), bootMemory)) { @@ -249,13 +253,22 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // Now connect connection = std::make_shared(deviceInfo, X_LINK_BOOTLOADER); - isEmbedded = false; - } else { + // The type of bootloader is now 'desiredBootloaderType' + bootloaderType = desiredBootloaderType; + + // Embedded bootloader was used to boot, set to true isEmbedded = true; + } else { + // Just connected to existing bootloader on device. Set embedded to false + isEmbedded = false; } } else { - if(type && *type != Type::USB) { + // If version isn't adequate to do an in memory boot - do regular Bootloader -> USB ROM -> Boot transition. + Type desiredBootloaderType = type.value_or(Type::USB); + + // If not correct type OR if allowFlashingBootloader is set, then boot internal (latest) bootloader of correct type + if((desiredBootloaderType != Type::USB) || allowFlashingBootloader) { // Send request to jump to USB bootloader // Boot into USB ROM BOOTLOADER NOW if(!sendBootloaderRequest(stream->getStreamId(), dai::bootloader::request::UsbRomBoot{})) { @@ -268,19 +281,20 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // Unbooted device found, boot to BOOTLOADER and connect with XLinkConnection constructor if(embeddedMvcmd) { - connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(*type), X_LINK_BOOTLOADER); + connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(desiredBootloaderType), X_LINK_BOOTLOADER); } else { connection = std::make_shared(deviceInfo, pathToMvcmd, X_LINK_BOOTLOADER); } - bootloaderType = *type; + bootloaderType = desiredBootloaderType; - // Device wasn't already in bootloader, that means that embedded bootloader is booted + // Embedded bootloader was used to boot, set to true isEmbedded = true; } else { bootloaderType = dai::bootloader::Type::USB; - // Device was already in bootloader, that means that embedded isn't running + + // Just connected to existing bootloader on device. Set embedded to false isEmbedded = false; } } @@ -385,6 +399,14 @@ DeviceBootloader::Version DeviceBootloader::getVersion() { return DeviceBootloader::Version(ver.major, ver.minor, ver.patch); } +DeviceBootloader::Type DeviceBootloader::getType() { + return bootloaderType; +} + +bool DeviceBootloader::isAllowedFlashingBootloader() { + return allowFlashingBootloader; +} + std::tuple DeviceBootloader::flash(std::function progressCb, Pipeline& pipeline) { return flashDepthaiApplicationPackage(progressCb, createDepthaiApplicationPackage(pipeline)); } @@ -439,6 +461,11 @@ std::tuple DeviceBootloader::flashBootloader(std::function DeviceBootloader::flashBootloader(Memory memory, Type type, std::function progressCb, std::string path) { + // Check if 'allowFlashingBootloader' is set to true. + if(!allowFlashingBootloader) { + throw std::invalid_argument("DeviceBootloader wasn't initialized to allow flashing bootloader. Set 'allowFlashingBootloader' in constructor"); + } + // Only flash memory is supported for now if(memory != Memory::FLASH) { throw std::invalid_argument("Only FLASH memory is supported for now"); From 4cac8173dc49dd844d71c860e49562d74dc8978f Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 29 Jul 2021 04:31:56 +0200 Subject: [PATCH 016/101] Updated flash_bootloader to be a bit more verbose --- examples/src/flash_bootloader.cpp | 55 +++++++++++++++++++++++++------ 1 file changed, 45 insertions(+), 10 deletions(-) diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index 0579e89eb..30559955f 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -6,32 +6,67 @@ int main(int argc, char** argv) { using namespace std::chrono; + char confirmation; + dai::DeviceBootloader::Type blType = dai::DeviceBootloader::Type::USB; + std::string blTypeStr; if(argc > 1) { - if(std::string(argv[1]) == "usb") { + blTypeStr = std::string(argv[1]); + if(blTypeStr == "usb") { blType = dai::DeviceBootloader::Type::USB; - } else if(std::string(argv[1]) == "eth") { + } else if(blTypeStr == "network") { blType = dai::DeviceBootloader::Type::NETWORK; } else { - std::cout << "Specify either 'usb' or 'eth' bootloader type\n"; + std::cout << "Specify either 'usb' or 'network' bootloader type\n"; return 0; } } else { - std::cout << "Usage: " << argv[0] << " \n"; + std::cout << "Usage: " << argv[0] << " \n"; return 0; } - bool res = false; + std::cout << "Warning! Flashing bootloader can potentially soft brick your device and should be done with caution." << std::endl; + std::cout << "Do not unplug your device in while the bootloader is flashing." << std::endl; + std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; + confirmation = std::cin.get(); + if(confirmation != 'y') { + std::cout << "Prompt declined, exiting..." << std::endl; + return -1; + } + + bool found = false; dai::DeviceInfo info; - std::tie(res, info) = dai::DeviceBootloader::getFirstAvailableDevice(); + std::tie(found, info) = dai::DeviceBootloader::getFirstAvailableDevice(); + if(!found) { + std::cout << "No device found to flash. Exiting." << std::endl; + return -1; + } + + // Open DeviceBootloader and allow flashing bootloader + dai::DeviceBootloader bl(info, true); + auto currentBlType = bl.getType(); + std::cout << "Connected to already running bootloader: " << !bl.isEmbeddedVersion() << std::endl; - dai::DeviceBootloader bl(info); + // Check if bootloader type is the same + if(currentBlType != blType) { + std::cout << "Are you sure you want to flash '" << blType << "' bootloader over current '" << currentBlType << "' bootloader?" << std::endl; + std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; + std::cin.ignore(); + confirmation = std::cin.get(); + if(confirmation != 'y') { + std::cout << "Prompt declined, exiting..." << std::endl; + return -1; + } + } + + // Create a progress callback lambda auto progress = [](float p) { std::cout << "Flashing Progress..." << p * 100 << "%" << std::endl; }; - std::string message; auto t1 = steady_clock::now(); - std::tie(res, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, blType, progress); - if(res) { + bool success = false; + std::string message; + std::tie(success, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, blType, progress); + if(success) { std::cout << "Flashing successful. Took " << duration_cast(steady_clock::now() - t1).count() << "ms" << std::endl; } else { std::cout << "Flashing failed: " << message << std::endl; From 9eedc3531d14832e98d8c1ea336e5f38dad819b9 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 29 Jul 2021 04:59:03 +0200 Subject: [PATCH 017/101] Improved the flash_bootloader example a bit --- examples/src/flash_bootloader.cpp | 10 +++------- include/depthai/device/DeviceBootloader.hpp | 4 ++-- src/device/DeviceBootloader.cpp | 4 ++-- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index 30559955f..a7aafda27 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -6,12 +6,9 @@ int main(int argc, char** argv) { using namespace std::chrono; - char confirmation; - dai::DeviceBootloader::Type blType = dai::DeviceBootloader::Type::USB; - std::string blTypeStr; if(argc > 1) { - blTypeStr = std::string(argv[1]); + std::string blTypeStr(argv[1]); if(blTypeStr == "usb") { blType = dai::DeviceBootloader::Type::USB; } else if(blTypeStr == "network") { @@ -28,8 +25,7 @@ int main(int argc, char** argv) { std::cout << "Warning! Flashing bootloader can potentially soft brick your device and should be done with caution." << std::endl; std::cout << "Do not unplug your device in while the bootloader is flashing." << std::endl; std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; - confirmation = std::cin.get(); - if(confirmation != 'y') { + if(std::cin.get() != 'y') { std::cout << "Prompt declined, exiting..." << std::endl; return -1; } @@ -45,7 +41,6 @@ int main(int argc, char** argv) { // Open DeviceBootloader and allow flashing bootloader dai::DeviceBootloader bl(info, true); auto currentBlType = bl.getType(); - std::cout << "Connected to already running bootloader: " << !bl.isEmbeddedVersion() << std::endl; // Check if bootloader type is the same if(currentBlType != blType) { @@ -62,6 +57,7 @@ int main(int argc, char** argv) { // Create a progress callback lambda auto progress = [](float p) { std::cout << "Flashing Progress..." << p * 100 << "%" << std::endl; }; + std::cout << "Flashing " << blType << " bootloader..." << std::endl; auto t1 = steady_clock::now(); bool success = false; std::string message; diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index 8cc03cf9d..ec9436cce 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -187,12 +187,12 @@ class DeviceBootloader { /** * @returns Type of currently connected bootloader */ - Type getType(); + Type getType() const; /** * @returns True if allowed to flash bootloader */ - bool isAllowedFlashingBootloader(); + bool isAllowedFlashingBootloader() const; /** * Explicitly closes connection to device. diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 223673cc3..3e6a93671 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -399,11 +399,11 @@ DeviceBootloader::Version DeviceBootloader::getVersion() { return DeviceBootloader::Version(ver.major, ver.minor, ver.patch); } -DeviceBootloader::Type DeviceBootloader::getType() { +DeviceBootloader::Type DeviceBootloader::getType() const { return bootloaderType; } -bool DeviceBootloader::isAllowedFlashingBootloader() { +bool DeviceBootloader::isAllowedFlashingBootloader() const { return allowFlashingBootloader; } From 08e6ad7f0765340e07a0bbb846a72b4b02cb2ca7 Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Thu, 29 Jul 2021 15:16:11 +0200 Subject: [PATCH 018/101] Updated flash_bootloader example --- examples/src/flash_bootloader.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index a7aafda27..5886b0199 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -47,8 +47,7 @@ int main(int argc, char** argv) { std::cout << "Are you sure you want to flash '" << blType << "' bootloader over current '" << currentBlType << "' bootloader?" << std::endl; std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; std::cin.ignore(); - confirmation = std::cin.get(); - if(confirmation != 'y') { + if(std::cin.get() != 'y') { std::cout << "Prompt declined, exiting..." << std::endl; return -1; } From 29a3590556b45259676db81ee6bfc9a79ce98fc8 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 3 Aug 2021 18:36:27 +0300 Subject: [PATCH 019/101] Allow to specify which bootloader is overridden by the env var: `DEPTHAI_BOOTLOADER_BINARY_USB` `DEPTHAI_BOOTLOADER_BINARY_ETH` (both can be set) --- src/utility/Resources.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 8eaba3e3b..8804be82a 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -192,17 +192,23 @@ constexpr static std::array RESOURCE_LIST_BOOTLOADER = { }; std::vector Resources::getBootloaderFirmware(dai::bootloader::Type type) { - // Check if env variable DEPTHAI_BOOTLOADER_BINARY is set - auto blBinaryPath = spdlog::details::os::getenv("DEPTHAI_BOOTLOADER_BINARY"); + // Check if env variable DEPTHAI_BOOTLOADER_BINARY_USB/_ETH is set + std::string blEnvVar; + if(type == dai::bootloader::Type::USB) { + blEnvVar = "DEPTHAI_BOOTLOADER_BINARY_USB"; + } else if(type == dai::bootloader::Type::NETWORK) { + blEnvVar = "DEPTHAI_BOOTLOADER_BINARY_ETH"; + } + auto blBinaryPath = spdlog::details::os::getenv(blEnvVar.c_str()); if(!blBinaryPath.empty()) { // Load binary file at path std::ifstream stream(blBinaryPath, std::ios::binary); if(!stream.is_open()) { // Throw an error // TODO(themarpe) - Unify exceptions into meaningful groups - throw std::runtime_error(fmt::format("File at path {} pointed to by DEPTHAI_BOOTLOADER_BINARY doesn't exist.", blBinaryPath)); + throw std::runtime_error(fmt::format("File at path {} pointed to by {} doesn't exist.", blBinaryPath, blEnvVar)); } - spdlog::warn("Overriding bootloader: {}", blBinaryPath); + spdlog::warn("Overriding bootloader {}: {}", blEnvVar, blBinaryPath); // Read the file and return its content return std::vector(std::istreambuf_iterator(stream), {}); } From 819e9e2211041113ed8f30f203d9ca12ba03b2cd Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Wed, 4 Aug 2021 19:52:10 +0200 Subject: [PATCH 020/101] Fixed boot_memory bootloader upgrade routine --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 2 +- examples/src/flash_bootloader.cpp | 2 +- src/device/DeviceBootloader.cpp | 32 ++++++++++----------- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 7441ed04b..8195330bb 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -4,4 +4,4 @@ set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" #set(DEPTHAI_BOOTLOADER_VERSION "0.0.12") -set(DEPTHAI_BOOTLOADER_VERSION "68b296bf866b11174127290b46f9a5f4966102c1") +set(DEPTHAI_BOOTLOADER_VERSION "d3119797ed0cc301e934be12fcbda283f307e4e1") diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index 5886b0199..e1847c918 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -23,7 +23,7 @@ int main(int argc, char** argv) { } std::cout << "Warning! Flashing bootloader can potentially soft brick your device and should be done with caution." << std::endl; - std::cout << "Do not unplug your device in while the bootloader is flashing." << std::endl; + std::cout << "Do not unplug your device while the bootloader is flashing." << std::endl; std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; if(std::cin.get() != 'y') { std::cout << "Prompt declined, exiting..." << std::endl; diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 3e6a93671..933a260a5 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -183,7 +183,7 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, connection = std::make_shared(deviceInfo, X_LINK_BOOTLOADER); // If type is specified, try to boot into that BL type - stream = std::unique_ptr(new XLinkStream(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE)); + stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); // Send request for bootloader version if(!sendBootloaderRequest(stream->getStreamId(), bootloader::request::GetBootloaderVersion{})) { @@ -241,17 +241,18 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // After that send numPackets of data stream->writeSplit(binary.data(), binary.size(), bootloader::XLINK_STREAM_MAX_SIZE); - + // Close existing stream first + stream = nullptr; // Stop watchdog wdRunning = false; wd.join(); + // Close connection + connection->close(); - // Dummy read, until link falls down and it returns an error code - streamPacketDesc_t* pPacket; - XLinkReadData(stream->getStreamId(), &pPacket); - - // Now connect + // Now reconnect connection = std::make_shared(deviceInfo, X_LINK_BOOTLOADER); + // prepare new bootloader stream + stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); // The type of bootloader is now 'desiredBootloaderType' bootloaderType = desiredBootloaderType; @@ -274,11 +275,12 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, if(!sendBootloaderRequest(stream->getStreamId(), dai::bootloader::request::UsbRomBoot{})) { throw std::runtime_error("Error trying to connect to device"); } + // Close existing stream first + stream = nullptr; + // Close connection + connection->close(); - // Dummy read, until link falls down and it returns an error code - streamPacketDesc_t* pPacket; - XLinkReadData(stream->getStreamId(), &pPacket); - + // Now reconnect // Unbooted device found, boot to BOOTLOADER and connect with XLinkConnection constructor if(embeddedMvcmd) { connection = std::make_shared(deviceInfo, getEmbeddedBootloaderBinary(desiredBootloaderType), X_LINK_BOOTLOADER); @@ -286,6 +288,9 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, connection = std::make_shared(deviceInfo, pathToMvcmd, X_LINK_BOOTLOADER); } + // prepare bootloader stream + stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); + bootloaderType = desiredBootloaderType; // Embedded bootloader was used to boot, set to true @@ -334,11 +339,6 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // Sleep a bit, so device isn't available anymore std::this_thread::sleep_for(std::chrono::milliseconds(500)); }); - - // prepare bootloader stream - if(stream == nullptr) { - stream = std::unique_ptr(new XLinkStream(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE)); - } } void DeviceBootloader::close() { From f7711f9091998871e4c07d2efe7f13fab9a0e94f Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Mon, 16 Aug 2021 13:26:17 +0200 Subject: [PATCH 021/101] WIP: Bootloader configuration --- examples/CMakeLists.txt | 5 +- examples/src/bootloader_configuration.cpp | 60 ++++++++++++++ include/depthai/device/DeviceBootloader.hpp | 52 ++++++++++++ shared/depthai-bootloader-shared | 2 +- src/device/DeviceBootloader.cpp | 87 +++++++++++++++++++++ 5 files changed, 204 insertions(+), 2 deletions(-) create mode 100644 examples/src/bootloader_configuration.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 94f414351..7c726f8b5 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -289,4 +289,7 @@ dai_add_example(image_manip_tiling src/image_manip_tiling.cpp ON) target_compile_definitions(calibration_flash PRIVATE CALIB_PATH="${calib_v6}") target_compile_definitions(calibration_flash_version5 PRIVATE CALIB_PATH="${calib_v5}" BOARD_PATH="${device_config}") -target_compile_definitions(calibration_load PRIVATE CALIB_PATH="${calib_v6}") \ No newline at end of file +target_compile_definitions(calibration_load PRIVATE CALIB_PATH="${calib_v6}") + +# Bootloader configuration example +dai_add_example(bootloader_configuration src/bootloader_configuration.cpp OFF) diff --git a/examples/src/bootloader_configuration.cpp b/examples/src/bootloader_configuration.cpp new file mode 100644 index 000000000..a956224a7 --- /dev/null +++ b/examples/src/bootloader_configuration.cpp @@ -0,0 +1,60 @@ +#include "depthai/depthai.hpp" + +int main(int argc, char** argv) { + bool read = true, clear = false; + std::string path = ""; + if(argc >= 2) { + std::string op = argv[1]; + if(op == "read") { + read = true; + } else if(op == "flash") { + read = false; + if(argc >= 3) { + path = argv[2]; + } else if(op == "clear") { + clear = true; + read = false; + } else { + std::cout << "Usage: " << argv[0] << " " << argv[1] << " [path/to/config/json]" << std::endl; + return -1; + } + } else { + std::cout << "Usage: " << argv[0] << " [read/flash] [flash: path/to/config/json]" << std::endl; + return -1; + } + } else { + std::cout << "Usage: " << argv[0] << " [read/flash] [flash: path/to/config/json]" << std::endl; + return -1; + } + + bool res = false; + dai::DeviceInfo info; + std::tie(res, info) = dai::DeviceBootloader::getFirstAvailableDevice(); + + if(res) { + std::cout << "Found device with name: " << info.desc.name << std::endl; + dai::DeviceBootloader bl(info); + + if(read) { + std::cout << "Current flashed configuration\n" << bl.readConfigurationData().dump(4) << std::endl; + } else { + bool success; + std::string error; + if(clear) { + std::tie(success, error) = bl.flashConfigurationClear(); + } else { + std::tie(success, error) = bl.flashConfigurationFile(path); + } + if(success) { + std::cout << "Successfully flashed bootloader configuration\n"; + } else { + std::cout << "Error flashing bootloader configuration: " << error; + } + } + + } else { + std::cout << "No devices found" << std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index ec9436cce..c0a65b7cb 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -13,6 +13,7 @@ #include "depthai/xlink/XLinkStream.hpp" // shared +#include "depthai-bootloader-shared/Config.hpp" #include "depthai-bootloader-shared/Memory.hpp" #include "depthai-bootloader-shared/Section.hpp" #include "depthai-bootloader-shared/Type.hpp" @@ -30,6 +31,9 @@ class DeviceBootloader { using Type = dai::bootloader::Type; using Memory = dai::bootloader::Memory; using Section = dai::bootloader::Section; + using UsbConfig = dai::bootloader::UsbConfig; + using NetworkConfig = dai::bootloader::NetworkConfig; + using Config = dai::bootloader::Config; /// Bootloader version structure struct Version { @@ -173,6 +177,48 @@ class DeviceBootloader { */ // std::tuple flashCustom(Memory memory, uint32_t offset, std::function progressCb, std::vector data); + /** + * Reads configuration data from bootloader + * @returns Unstructured configuration data + */ + nlohmann::json readConfigurationData(Memory memory = Memory::AUTO); + + /** + * Flashes configuration data to bootloader + * @param configData Unstructured configuration data + */ + std::tuple flashConfigurationData(nlohmann::json configData, Memory memory = Memory::AUTO); + + /** + * Flashes JSON configuration data to bootloader + * @param configJson JSON configuration data + */ + std::tuple flashConfigurationData(std::string configJson, Memory memory = Memory::AUTO); + + /** + * Flashes configuration data to bootloader + * @param configPath Unstructured configuration data + */ + std::tuple flashConfigurationFile(std::string configPath, Memory memory = Memory::AUTO); + + /** + * Flashes configuration data to bootloader + * @param configData Unstructured configuration data + */ + std::tuple flashConfigurationClear(Memory memory = Memory::AUTO); + + /** + * Reads configuration from bootloader + * @returns Configuration + */ + Config readConfiguration(Memory memory = Memory::AUTO); + + /** + * Flashes configuration to bootloader + * @param configData Configuration + */ + std::tuple flashConfiguration(const Config& config); + /** * @returns Version of current running bootloader */ @@ -250,6 +296,9 @@ inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader:: inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Memory& memory) { switch(memory) { + case dai::DeviceBootloader::Memory::AUTO: + out << "AUTO"; + break; case dai::DeviceBootloader::Memory::FLASH: out << "FLASH"; break; @@ -262,6 +311,9 @@ inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader:: inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Section& type) { switch(type) { + case dai::DeviceBootloader::Section::AUTO: + out << "AUTO"; + break; case dai::DeviceBootloader::Section::HEADER: out << "HEADER"; break; diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 391e3431f..ff0388376 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 391e3431fcc6f271f60584603697edf664bbb59c +Subproject commit ff0388376d272cabdb79bf68f21fd7130d8212a6 diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 34b18c981..5cbef6486 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -6,6 +6,7 @@ // shared #include "depthai-bootloader-shared/Bootloader.hpp" #include "depthai-bootloader-shared/SBR.h" +#include "depthai-bootloader-shared/Structure.hpp" #include "depthai-bootloader-shared/XLinkConstants.hpp" #include "depthai-shared/datatype/RawImgFrame.hpp" #include "depthai-shared/pipeline/Assets.hpp" @@ -172,6 +173,9 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, connection = std::make_shared(deviceInfo, pathToMvcmd, X_LINK_BOOTLOADER); } + // prepare bootloader stream + stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); + // Device wasn't already in bootloader, that means that embedded bootloader is booted isEmbedded = true; } else if(deviceInfo.state == X_LINK_BOOTLOADER) { @@ -596,6 +600,89 @@ std::tuple DeviceBootloader::flashCustom(Memory memory, uint3 } */ +nlohmann::json DeviceBootloader::readConfigurationData(Memory memory) { + // Send request to GET_BOOTLOADER_CONFIG + dai::bootloader::request::GetBootloaderConfig getConfigReq; + getConfigReq.memory = memory; + if(!sendBootloaderRequest(stream->getStreamId(), getConfigReq)) return {false, "Couldn't send request to get configuration data"}; + + // Get response + dai::bootloader::response::GetBootloaderConfig resp; + receiveBootloaderResponse(stream->getStreamId(), resp); + + if(resp.success) { + // Read back bootloader config (1 packet max) + auto bsonConfig = stream->read(); + // Parse from BSON + return nlohmann::json::from_bson(bsonConfig); + } else { + return {}; + } +} + +std::tuple DeviceBootloader::flashConfigurationClear(Memory memory) { + // send request to SET_BOOTLOADER_CONFIG + dai::bootloader::request::SetBootloaderConfig setConfigReq; + setConfigReq.memory = memory; + setConfigReq.numPackets = 0; + setConfigReq.totalSize = 0; + setConfigReq.clearConfig = 1; + if(!sendBootloaderRequest(stream->getStreamId(), setConfigReq)) return {false, "Couldn't send request to flash configuration data"}; + + // Read back response + dai::bootloader::response::FlashComplete result; + receiveBootloaderResponse(stream->getStreamId(), result); + + // Return if flashing was successful + return {result.success, result.errorMsg}; +} + +std::tuple DeviceBootloader::flashConfigurationData(nlohmann::json configData, Memory memory) { + // Parse to BSON + auto bson = nlohmann::json::to_bson(configData); + + // Send request to SET_BOOTLOADER_CONFIG + dai::bootloader::request::SetBootloaderConfig setConfigReq; + setConfigReq.memory = memory; + setConfigReq.numPackets = 1; + setConfigReq.totalSize = bson.size(); + setConfigReq.clearConfig = 0; + if(!sendBootloaderRequest(stream->getStreamId(), setConfigReq)) return {false, "Couldn't send request to flash configuration data"}; + + // Send 1 packet, of bson config data + stream->write(bson); + + // Read back response + dai::bootloader::response::FlashComplete result; + receiveBootloaderResponse(stream->getStreamId(), result); + + // Return if flashing was successful + return {result.success, result.errorMsg}; +} + +std::tuple DeviceBootloader::flashConfigurationData(std::string configJson, Memory memory) { + return flashConfigurationData(nlohmann::json::parse(configJson), memory); +} + +std::tuple DeviceBootloader::flashConfigurationFile(std::string configPath, Memory memory) { + // read a JSON file + std::ifstream configInputStream(configPath); + nlohmann::json configJson; + configInputStream >> configJson; + return flashConfigurationData(configJson, memory); +} + +DeviceBootloader::Config DeviceBootloader::readConfiguration(Memory memory) { + auto json = readConfigurationData(memory); + // Implicit parse from json to Config + return json; +} + +std::tuple DeviceBootloader::flashConfiguration(const Config& config) { + // Implicit parse from Config to json + return flashConfigurationData(config); +} + bool DeviceBootloader::isEmbeddedVersion() const { return isEmbedded; } From 497a4df17aa367cb16018c75056fef3850382501 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Mon, 16 Aug 2021 22:28:50 +0900 Subject: [PATCH 022/101] Throw XLink specific errors for read/write errors --- include/depthai/xlink/XLinkStream.hpp | 10 ++++++++++ src/xlink/XLinkStream.cpp | 14 +++++++------- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index fdd8d0751..134772aaf 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -60,4 +60,14 @@ class XLinkStream { streamId_t getStreamId() const; }; +struct XLinkError : public std::runtime_error { + using std::runtime_error::runtime_error; +}; +struct XLinkReadError : public XLinkError { + using XLinkError::XLinkError; +}; +struct XLinkWriteError : public XLinkError { + using XLinkError::XLinkError; +}; + } // namespace dai diff --git a/src/xlink/XLinkStream.cpp b/src/xlink/XLinkStream.cpp index 6c44805a4..36754d6e1 100644 --- a/src/xlink/XLinkStream.cpp +++ b/src/xlink/XLinkStream.cpp @@ -58,7 +58,7 @@ XLinkStream::~XLinkStream() { void XLinkStream::write(const std::uint8_t* data, std::size_t size) { auto status = XLinkWriteData(streamId, data, static_cast(size)); if(status != X_LINK_SUCCESS) { - throw std::runtime_error(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkWriteError(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); } } void XLinkStream::write(const void* data, std::size_t size) { @@ -73,7 +73,7 @@ void XLinkStream::read(std::vector& data) { streamPacketDesc_t* pPacket = nullptr; auto status = XLinkReadData(streamId, &pPacket); if(status != X_LINK_SUCCESS) { - throw std::runtime_error(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); } data = std::vector(pPacket->data, pPacket->data + pPacket->length); XLinkReleaseData(streamId); @@ -90,7 +90,7 @@ streamPacketDesc_t* XLinkStream::readRaw() { streamPacketDesc_t* pPacket = nullptr; auto status = XLinkReadData(streamId, &pPacket); if(status != X_LINK_SUCCESS) { - throw std::runtime_error(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); } return pPacket; } @@ -111,7 +111,7 @@ void XLinkStream::writeSplit(const void* d, std::size_t size, std::size_t split) sizeToTransmit = remaining > split ? split : remaining; ret = XLinkWriteData(streamId, data + currentOffset, static_cast(sizeToTransmit)); if(ret != X_LINK_SUCCESS) { - throw std::runtime_error(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(ret))); + throw XLinkWriteError(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(ret))); } currentOffset += sizeToTransmit; remaining = size - currentOffset; @@ -133,7 +133,7 @@ bool XLinkStream::write(const std::uint8_t* data, std::size_t size, std::chrono: } else if(status == X_LINK_TIMEOUT) { return false; } else { - throw std::runtime_error(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkWriteError(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); } } @@ -155,7 +155,7 @@ bool XLinkStream::read(std::vector& data, std::chrono::millisecond } else if(status == X_LINK_TIMEOUT) { return false; } else { - throw std::runtime_error(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); } return false; } @@ -167,7 +167,7 @@ bool XLinkStream::readRaw(streamPacketDesc_t*& pPacket, std::chrono::millisecond } else if(status == X_LINK_TIMEOUT) { return false; } else { - throw std::runtime_error(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); } } From 4d311a5df451450cb43670608be46cb3104f0361 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Tue, 17 Aug 2021 01:32:26 +0900 Subject: [PATCH 023/101] Add pertinent info to XLinkError struct --- include/depthai/xlink/XLinkConnection.hpp | 3 ++- include/depthai/xlink/XLinkStream.hpp | 10 ++++++++-- src/xlink/XLinkStream.cpp | 22 +++++++++++++++------- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/include/depthai/xlink/XLinkConnection.hpp b/include/depthai/xlink/XLinkConnection.hpp index 4b2d16696..076089903 100644 --- a/include/depthai/xlink/XLinkConnection.hpp +++ b/include/depthai/xlink/XLinkConnection.hpp @@ -68,7 +68,8 @@ class XLinkConnection { bool isClosed() const; private: - friend class XLinkStream; + friend class XLinkReadError; + friend class XLinkWriteError; // static static bool bootAvailableDevice(const deviceDesc_t& deviceToBoot, const std::string& pathToMvcmd); static bool bootAvailableDevice(const deviceDesc_t& deviceToBoot, std::vector& mvcmd); diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index 134772aaf..6643f1910 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -61,13 +61,19 @@ class XLinkStream { }; struct XLinkError : public std::runtime_error { + XLinkError_t status = X_LINK_ERROR; + std::string streamName; + using std::runtime_error::runtime_error; + + XLinkError(XLinkError_t status_, const std::string& streamName_, const std::string& message_) + : runtime_error(message_), status(status_), streamName(streamName_) {} }; struct XLinkReadError : public XLinkError { - using XLinkError::XLinkError; + XLinkReadError(XLinkError_t status_, const std::string& streamName_); }; struct XLinkWriteError : public XLinkError { - using XLinkError::XLinkError; + XLinkWriteError(XLinkError_t status_, const std::string& streamName_); }; } // namespace dai diff --git a/src/xlink/XLinkStream.cpp b/src/xlink/XLinkStream.cpp index 36754d6e1..ecf467f67 100644 --- a/src/xlink/XLinkStream.cpp +++ b/src/xlink/XLinkStream.cpp @@ -58,7 +58,7 @@ XLinkStream::~XLinkStream() { void XLinkStream::write(const std::uint8_t* data, std::size_t size) { auto status = XLinkWriteData(streamId, data, static_cast(size)); if(status != X_LINK_SUCCESS) { - throw XLinkWriteError(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkWriteError(status, streamName); } } void XLinkStream::write(const void* data, std::size_t size) { @@ -73,7 +73,7 @@ void XLinkStream::read(std::vector& data) { streamPacketDesc_t* pPacket = nullptr; auto status = XLinkReadData(streamId, &pPacket); if(status != X_LINK_SUCCESS) { - throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(status, streamName); } data = std::vector(pPacket->data, pPacket->data + pPacket->length); XLinkReleaseData(streamId); @@ -90,7 +90,7 @@ streamPacketDesc_t* XLinkStream::readRaw() { streamPacketDesc_t* pPacket = nullptr; auto status = XLinkReadData(streamId, &pPacket); if(status != X_LINK_SUCCESS) { - throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(status, streamName); } return pPacket; } @@ -111,7 +111,7 @@ void XLinkStream::writeSplit(const void* d, std::size_t size, std::size_t split) sizeToTransmit = remaining > split ? split : remaining; ret = XLinkWriteData(streamId, data + currentOffset, static_cast(sizeToTransmit)); if(ret != X_LINK_SUCCESS) { - throw XLinkWriteError(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(ret))); + throw XLinkWriteError(ret, streamName); } currentOffset += sizeToTransmit; remaining = size - currentOffset; @@ -133,7 +133,7 @@ bool XLinkStream::write(const std::uint8_t* data, std::size_t size, std::chrono: } else if(status == X_LINK_TIMEOUT) { return false; } else { - throw XLinkWriteError(fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkWriteError(status, streamName); } } @@ -155,7 +155,7 @@ bool XLinkStream::read(std::vector& data, std::chrono::millisecond } else if(status == X_LINK_TIMEOUT) { return false; } else { - throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(status, streamName); } return false; } @@ -167,7 +167,7 @@ bool XLinkStream::readRaw(streamPacketDesc_t*& pPacket, std::chrono::millisecond } else if(status == X_LINK_TIMEOUT) { return false; } else { - throw XLinkReadError(fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))); + throw XLinkReadError(status, streamName); } } @@ -175,4 +175,12 @@ streamId_t XLinkStream::getStreamId() const { return streamId; } +XLinkReadError::XLinkReadError(XLinkError_t status_, const std::string& streamName_) + : XLinkError( + status_, streamName_, fmt::format("Couldn't write data to stream: '{}' ({})", streamName_, XLinkConnection::convertErrorCodeToString(status_))) {} + +XLinkWriteError::XLinkWriteError(XLinkError_t status_, const std::string& streamName_) + : XLinkError( + status_, streamName_, fmt::format("Couldn't read data from stream: '{}' ({})", streamName_, XLinkConnection::convertErrorCodeToString(status_))) {} + } // namespace dai From e12436c9a91a075efd30be137299c11ec387d479 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Tue, 17 Aug 2021 01:50:24 +0900 Subject: [PATCH 024/101] Make data members const --- include/depthai/xlink/XLinkStream.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index 6643f1910..0ddfd6e1a 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -61,8 +61,8 @@ class XLinkStream { }; struct XLinkError : public std::runtime_error { - XLinkError_t status = X_LINK_ERROR; - std::string streamName; + const XLinkError_t status = X_LINK_ERROR; + const std::string streamName; using std::runtime_error::runtime_error; From 02f51a8dd4f363d6a2d3d67d58c73667602c5d46 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 16 Aug 2021 20:42:26 +0200 Subject: [PATCH 025/101] Updated bootloader_configuration example --- examples/src/bootloader_configuration.cpp | 29 +++++++++++++++-------- shared/depthai-bootloader-shared | 2 +- src/device/DeviceBootloader.cpp | 1 + 3 files changed, 21 insertions(+), 11 deletions(-) diff --git a/examples/src/bootloader_configuration.cpp b/examples/src/bootloader_configuration.cpp index a956224a7..546b812a3 100644 --- a/examples/src/bootloader_configuration.cpp +++ b/examples/src/bootloader_configuration.cpp @@ -1,6 +1,8 @@ #include "depthai/depthai.hpp" int main(int argc, char** argv) { + // Options + bool usage = false; bool read = true, clear = false; std::string path = ""; if(argc >= 2) { @@ -11,22 +13,25 @@ int main(int argc, char** argv) { read = false; if(argc >= 3) { path = argv[2]; - } else if(op == "clear") { - clear = true; - read = false; - } else { - std::cout << "Usage: " << argv[0] << " " << argv[1] << " [path/to/config/json]" << std::endl; - return -1; } + } else if(op == "clear") { + clear = true; + read = false; + } else if(op == "clear") { + clear = true; + read = false; } else { - std::cout << "Usage: " << argv[0] << " [read/flash] [flash: path/to/config/json]" << std::endl; - return -1; + usage = true; } } else { - std::cout << "Usage: " << argv[0] << " [read/flash] [flash: path/to/config/json]" << std::endl; + usage = true; + } + if(usage) { + std::cout << "Usage: " << argv[0] << " [read/flash/clear] [flash: path/to/config/json]" << std::endl; return -1; } + // DeviceBootloader configuration bool res = false; dai::DeviceInfo info; std::tie(res, info) = dai::DeviceBootloader::getFirstAvailableDevice(); @@ -43,7 +48,11 @@ int main(int argc, char** argv) { if(clear) { std::tie(success, error) = bl.flashConfigurationClear(); } else { - std::tie(success, error) = bl.flashConfigurationFile(path); + if(path.empty()) { + std::tie(success, error) = bl.flashConfiguration(dai::DeviceBootloader::Config{}); + } else { + std::tie(success, error) = bl.flashConfigurationFile(path); + } } if(success) { std::cout << "Successfully flashed bootloader configuration\n"; diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index ff0388376..2771f0243 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit ff0388376d272cabdb79bf68f21fd7130d8212a6 +Subproject commit 2771f02436c80c211b17295754966be48123ebbf diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 5cbef6486..8be491ef7 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -667,6 +667,7 @@ std::tuple DeviceBootloader::flashConfigurationData(std::stri std::tuple DeviceBootloader::flashConfigurationFile(std::string configPath, Memory memory) { // read a JSON file std::ifstream configInputStream(configPath); + if(!configInputStream.is_open()) throw std::runtime_error("Cannot flash configuration, JSON at path: " + configPath + " doesn't exist"); nlohmann::json configJson; configInputStream >> configJson; return flashConfigurationData(configJson, memory); From 9403c52c3a37a9bd881eb07198c81ad4ce7b134f Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Tue, 17 Aug 2021 20:20:26 +0900 Subject: [PATCH 026/101] Bring the 3 variable ctor into visibility --- include/depthai/xlink/XLinkStream.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index 0ddfd6e1a..abce480ef 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -70,9 +70,11 @@ struct XLinkError : public std::runtime_error { : runtime_error(message_), status(status_), streamName(streamName_) {} }; struct XLinkReadError : public XLinkError { + using XLinkError = XLinkError; XLinkReadError(XLinkError_t status_, const std::string& streamName_); }; struct XLinkWriteError : public XLinkError { + using XLinkError = XLinkError; XLinkWriteError(XLinkError_t status_, const std::string& streamName_); }; From 5c5ff19c19944284c0e3e5cea005d8e8996a11cf Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Tue, 17 Aug 2021 20:23:04 +0900 Subject: [PATCH 027/101] Rename vars as requested --- include/depthai/xlink/XLinkStream.hpp | 7 +++---- src/xlink/XLinkStream.cpp | 10 ++++------ 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index abce480ef..b1ad12123 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -66,16 +66,15 @@ struct XLinkError : public std::runtime_error { using std::runtime_error::runtime_error; - XLinkError(XLinkError_t status_, const std::string& streamName_, const std::string& message_) - : runtime_error(message_), status(status_), streamName(streamName_) {} + XLinkError(XLinkError_t statusID, const std::string& stream, const std::string& message) : runtime_error(message), status(statusID), streamName(stream) {} }; struct XLinkReadError : public XLinkError { using XLinkError = XLinkError; - XLinkReadError(XLinkError_t status_, const std::string& streamName_); + XLinkReadError(XLinkError_t status, const std::string& stream); }; struct XLinkWriteError : public XLinkError { using XLinkError = XLinkError; - XLinkWriteError(XLinkError_t status_, const std::string& streamName_); + XLinkWriteError(XLinkError_t status, const std::string& stream); }; } // namespace dai diff --git a/src/xlink/XLinkStream.cpp b/src/xlink/XLinkStream.cpp index ecf467f67..6f039f6af 100644 --- a/src/xlink/XLinkStream.cpp +++ b/src/xlink/XLinkStream.cpp @@ -175,12 +175,10 @@ streamId_t XLinkStream::getStreamId() const { return streamId; } -XLinkReadError::XLinkReadError(XLinkError_t status_, const std::string& streamName_) - : XLinkError( - status_, streamName_, fmt::format("Couldn't write data to stream: '{}' ({})", streamName_, XLinkConnection::convertErrorCodeToString(status_))) {} +XLinkReadError::XLinkReadError(XLinkError_t status, const std::string& streamName) + : XLinkError(status, streamName, fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))) {} -XLinkWriteError::XLinkWriteError(XLinkError_t status_, const std::string& streamName_) - : XLinkError( - status_, streamName_, fmt::format("Couldn't read data from stream: '{}' ({})", streamName_, XLinkConnection::convertErrorCodeToString(status_))) {} +XLinkWriteError::XLinkWriteError(XLinkError_t status, const std::string& streamName) + : XLinkError(status, streamName, fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))) {} } // namespace dai From 25e43b70494b97c43e72de8e6cdec0b30f4f2838 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Tue, 17 Aug 2021 20:37:46 +0900 Subject: [PATCH 028/101] Apply suggestions by clang-tidy --- include/depthai/xlink/XLinkConnection.hpp | 4 ++-- include/depthai/xlink/XLinkStream.hpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/depthai/xlink/XLinkConnection.hpp b/include/depthai/xlink/XLinkConnection.hpp index 076089903..b3ff8a71a 100644 --- a/include/depthai/xlink/XLinkConnection.hpp +++ b/include/depthai/xlink/XLinkConnection.hpp @@ -68,8 +68,8 @@ class XLinkConnection { bool isClosed() const; private: - friend class XLinkReadError; - friend class XLinkWriteError; + friend struct XLinkReadError; + friend struct XLinkWriteError; // static static bool bootAvailableDevice(const deviceDesc_t& deviceToBoot, const std::string& pathToMvcmd); static bool bootAvailableDevice(const deviceDesc_t& deviceToBoot, std::vector& mvcmd); diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index b1ad12123..05baeb85c 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -66,7 +66,8 @@ struct XLinkError : public std::runtime_error { using std::runtime_error::runtime_error; - XLinkError(XLinkError_t statusID, const std::string& stream, const std::string& message) : runtime_error(message), status(statusID), streamName(stream) {} + XLinkError(XLinkError_t statusID, std::string stream, const std::string& message) + : runtime_error(message), status(statusID), streamName(std::move(stream)) {} }; struct XLinkReadError : public XLinkError { using XLinkError = XLinkError; From aaf66c57a4dee656f910f100d08949f18668f58e Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 19 Aug 2021 03:34:41 +0300 Subject: [PATCH 029/101] Update FW with stereo LR-check, subpixel fixes; extended mode is not available --- 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 afe158d89..2890e6122 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 "eb3b3cfd94b306ae38f114c1c98b090cb3fe2671") +set(DEPTHAI_DEVICE_SIDE_COMMIT "06a2d8b03e7903024ed58e3a74161440438a6da3") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 5dc961ab5eb8ec4feb6e351760c6f647c69f235b Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 19 Aug 2021 10:27:52 +0200 Subject: [PATCH 030/101] Reduced BL check to 0.0.14 and updated FW and BL --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- shared/depthai-bootloader-shared | 2 +- src/device/DeviceBase.cpp | 5 +++-- src/device/DeviceBootloader.cpp | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 8195330bb..2bef38972 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -4,4 +4,4 @@ set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" #set(DEPTHAI_BOOTLOADER_VERSION "0.0.12") -set(DEPTHAI_BOOTLOADER_VERSION "d3119797ed0cc301e934be12fcbda283f307e4e1") +set(DEPTHAI_BOOTLOADER_VERSION "d404136d0b8d871f738258d8bfa29b6b6b00b98f") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 78e0ce9fc..ced128ae6 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 "a2baa42ba41f7b79dcbee987b34f24da075de196") +set(DEPTHAI_DEVICE_SIDE_COMMIT "a0a53662404cb8cf126bbefa6365f3a4635a2a16") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 2771f0243..678ff6801 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 2771f02436c80c211b17295754966be48123ebbf +Subproject commit 678ff68015661137137c454383cc2ff7bf82f016 diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index fd797a624..452740aa9 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -318,11 +318,12 @@ void DeviceBase::closeImpl() { auto t1 = steady_clock::now(); spdlog::debug("Device about to be closed..."); - // Close connection first (so queues unblock) + // Close connection first + // Resets device as well and queues unblock connection->close(); connection = nullptr; - // Stop watchdog + // Stop various threads watchdogRunning = false; timesyncRunning = false; loggingRunning = false; diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 8be491ef7..ea75b69d9 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -426,7 +426,7 @@ std::tuple DeviceBootloader::flashDepthaiApplicationPackage(s // Bug in NETWORK bootloader in version 0.0.12 < 0.1.0 - flashing can cause a soft brick auto version = getVersion(); - if(bootloaderType == Type::NETWORK && version < Version(0, 1, 0)) { + if(bootloaderType == Type::NETWORK && version < Version(0, 0, 14)) { throw std::invalid_argument("Network bootloader requires version 0.1.0 or higher to flash applications. Current version: " + version.toString()); } From 93df9978e301265229ca8a613a47397cf5f3dc47 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sat, 21 Aug 2021 22:34:21 +0200 Subject: [PATCH 031/101] Added capability to compress FW and additional BL config helper --- CMakeLists.txt | 2 + cmake/Depthai/DepthaiBootloaderConfig.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- cmake/depthaiDependencies.cmake | 7 +- examples/src/bootloader_configuration.cpp | 11 +- include/depthai/device/DeviceBootloader.hpp | 122 ++++++++++-- shared/depthai-bootloader-shared | 2 +- src/device/DeviceBootloader.cpp | 208 +++++++++++++++++--- src/utility/Platform.cpp | 47 +++++ src/utility/Platform.hpp | 13 ++ src/utility/Resources.cpp | 4 + tests/CMakeLists.txt | 2 + tests/src/bootloader_config_test.cpp | 39 ++++ 13 files changed, 405 insertions(+), 56 deletions(-) create mode 100644 src/utility/Platform.cpp create mode 100644 src/utility/Platform.hpp create mode 100644 tests/src/bootloader_config_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e0984ef2..980ad5055 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -181,6 +181,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/datatype/FeatureTrackerConfig.cpp src/utility/Initialization.cpp src/utility/Resources.cpp + src/utility/Platform.cpp src/xlink/XLinkConnection.cpp src/xlink/XLinkStream.cpp src/openvino/OpenVINO.cpp @@ -366,6 +367,7 @@ target_link_libraries(${TARGET_CORE_NAME} FP16::fp16 archive_static spdlog::spdlog + ZLIB::zlib ) # Add compile definitions diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 2bef38972..e91bab56b 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -4,4 +4,4 @@ set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" #set(DEPTHAI_BOOTLOADER_VERSION "0.0.12") -set(DEPTHAI_BOOTLOADER_VERSION "d404136d0b8d871f738258d8bfa29b6b6b00b98f") +set(DEPTHAI_BOOTLOADER_VERSION "a243221305bd0351cc2c9b98037e931068f4afef") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index ced128ae6..6c439da18 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 "a0a53662404cb8cf126bbefa6365f3a4635a2a16") +set(DEPTHAI_DEVICE_SIDE_COMMIT "9b857aedf34a19c7738a2f7f2f16a39f4e3831b3") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index 3a9ba9d18..d74abb410 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -12,11 +12,12 @@ else() hunter_add_package(FP16) hunter_add_package(libarchive-luxonis) hunter_add_package(spdlog) + hunter_add_package(ZLIB) endif() # If library was build as static, find all dependencies if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT depthai_SHARED_LIBS)) - + # BZip2 (for bspatch) find_package(BZip2 ${_QUIET} CONFIG REQUIRED) @@ -26,10 +27,12 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT depthai_SHARED_LIBS)) # libarchive for firmware packages find_package(archive_static ${_QUIET} CONFIG REQUIRED) find_package(lzma ${_QUIET} CONFIG REQUIRED) + # ZLIB for compressing Apps + find_package(ZLIB CONFIG REQUIRED) # spdlog for library and device logging find_package(spdlog ${_QUIET} CONFIG REQUIRED) - + endif() # Add threads (c++) diff --git a/examples/src/bootloader_configuration.cpp b/examples/src/bootloader_configuration.cpp index 546b812a3..b2eb9564a 100644 --- a/examples/src/bootloader_configuration.cpp +++ b/examples/src/bootloader_configuration.cpp @@ -2,8 +2,7 @@ int main(int argc, char** argv) { // Options - bool usage = false; - bool read = true, clear = false; + bool usage = false, read = true, clear = false; std::string path = ""; if(argc >= 2) { std::string op = argv[1]; @@ -41,17 +40,17 @@ int main(int argc, char** argv) { dai::DeviceBootloader bl(info); if(read) { - std::cout << "Current flashed configuration\n" << bl.readConfigurationData().dump(4) << std::endl; + std::cout << "Current flashed configuration\n" << bl.readConfigData().dump(4) << std::endl; } else { bool success; std::string error; if(clear) { - std::tie(success, error) = bl.flashConfigurationClear(); + std::tie(success, error) = bl.flashConfigClear(); } else { if(path.empty()) { - std::tie(success, error) = bl.flashConfiguration(dai::DeviceBootloader::Config{}); + std::tie(success, error) = bl.flashConfig(dai::DeviceBootloader::Config{}); } else { - std::tie(success, error) = bl.flashConfigurationFile(path); + std::tie(success, error) = bl.flashConfigFile(path); } } if(success) { diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index c0a65b7cb..ae290dd56 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -8,6 +8,7 @@ // project #include "CallbackHandler.hpp" #include "DataQueue.hpp" +#include "depthai/common/UsbSpeed.hpp" #include "depthai/pipeline/Pipeline.hpp" #include "depthai/xlink/XLinkConnection.hpp" #include "depthai/xlink/XLinkStream.hpp" @@ -33,7 +34,48 @@ class DeviceBootloader { using Section = dai::bootloader::Section; using UsbConfig = dai::bootloader::UsbConfig; using NetworkConfig = dai::bootloader::NetworkConfig; - using Config = dai::bootloader::Config; + + // Derive and extend bootloader::Config for easier usage + struct Config : public bootloader::Config { + /// Setting a static IPv4 won't start DHCP client + void setStaticIPv4(std::string ip, std::string mask, std::string gateway); + /// Setting a dynamic IPv4 will set that IP as well as start DHCP client + void setDynamicIPv4(std::string ip, std::string mask, std::string gateway); + /// Get if static IPv4 configuration is set + bool isStaticIPV4(); + /// Get IPv4 + std::string getIPv4(); + /// Get IPv4 mask + std::string getIPv4Mask(); + /// Get IPv4 gateway + std::string getIPv4Gateway(); + /// Set IPv4 DNS options + void setDnsIPv4(std::string dns, std::string dnsAlt = ""); + /// Get primary IPv4 DNS server + std::string getDnsIPv4(); + /// Get alternate IPv4 DNS server + std::string getDnsAltIPv4(); + + /// Set USB timeout + void setUsbTimeout(std::chrono::milliseconds ms); + /// Get USB timeout + std::chrono::milliseconds getUsbTimeout(); + + /// Set NETWOR timeout + void setNetworkTimeout(std::chrono::milliseconds ms); + /// Get NETWORK timeout + std::chrono::milliseconds getNetworkTimeout(); + + /// Set MAC address if not flashed on controller + void setMacAddress(std::string mac); + /// Get MAC address if not flashed on controller + std::string getMacAddress(); + + /// Set maxUsbSpeed + void setUsbMaxSpeed(UsbSpeed speed); + /// Get maxUsbSpeed + UsbSpeed getUsbMaxSpeed(); + }; /// Bootloader version structure struct Version { @@ -84,17 +126,35 @@ class DeviceBootloader { * Creates application package which can be flashed to depthai device. * @param pipeline Pipeline from which to create the application package * @param pathToCmd Optional path to custom device firmware + * @param compress Optional boolean which specifies if contents should be compressed + * @returns Depthai application package + */ + static std::vector createDepthaiApplicationPackage(const Pipeline& pipeline, std::string pathToCmd = "", bool compress = false); + + /** + * Creates application package which can be flashed to depthai device. + * @param pipeline Pipeline from which to create the application package + * @param compress Specifies if contents should be compressed * @returns Depthai application package */ - static std::vector createDepthaiApplicationPackage(Pipeline& pipeline, std::string pathToCmd = ""); + static std::vector createDepthaiApplicationPackage(const Pipeline& pipeline, bool compress); /** * Saves application package to a file which can be flashed to depthai device. * @param path Path where to save the application package * @param pipeline Pipeline from which to create the application package * @param pathToCmd Optional path to custom device firmware + * @param compress Optional boolean which specifies if contents should be compressed */ - static void saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, std::string pathToCmd = ""); + static void saveDepthaiApplicationPackage(std::string path, const Pipeline& pipeline, std::string pathToCmd = "", bool compress = false); + + /** + * Saves application package to a file which can be flashed to depthai device. + * @param path Path where to save the application package + * @param pipeline Pipeline from which to create the application package + * @param compress Specifies if contents should be compressed + */ + static void saveDepthaiApplicationPackage(std::string path, const Pipeline& pipeline, bool compress); /** * @returns Embedded bootloader version @@ -139,11 +199,17 @@ class DeviceBootloader { ~DeviceBootloader(); /** - * Flashes a give pipeline to the board. + * Flashes a given pipeline to the device. * @param progressCallback Callback that sends back a value between 0..1 which signifies current flashing progress * @param pipeline Pipeline to flash to the board */ - std::tuple flash(std::function progressCallback, Pipeline& pipeline); + std::tuple flash(std::function progressCallback, const Pipeline& pipeline, bool compress = false); + + /** + * Flashes a given pipeline to the device. + * @param pipeline Pipeline to flash to the board + */ + std::tuple flash(const Pipeline& pipeline, bool compress = false); /** * Flashes a specific depthai application package that was generated using createDepthaiApplicationPackage or saveDepthaiApplicationPackage @@ -152,6 +218,12 @@ class DeviceBootloader { */ std::tuple flashDepthaiApplicationPackage(std::function progressCallback, std::vector package); + /** + * Flashes a specific depthai application package that was generated using createDepthaiApplicationPackage or saveDepthaiApplicationPackage + * @param package Depthai application package to flash to the board + */ + std::tuple flashDepthaiApplicationPackage(std::vector package); + /** * Flashes bootloader to the current board * @param progressCallback Callback that sends back a value between 0..1 which signifies current flashing progress @@ -180,44 +252,49 @@ class DeviceBootloader { /** * Reads configuration data from bootloader * @returns Unstructured configuration data + * @param memory Optional - from which memory to read configuration data + * @param type Optional - from which type of bootloader to read configuration data */ - nlohmann::json readConfigurationData(Memory memory = Memory::AUTO); + nlohmann::json readConfigData(Memory memory = Memory::AUTO, Type type = Type::AUTO); /** * Flashes configuration data to bootloader * @param configData Unstructured configuration data + * @param memory Optional - to which memory flash configuration + * @param type Optional - for which type of bootloader to flash configuration */ - std::tuple flashConfigurationData(nlohmann::json configData, Memory memory = Memory::AUTO); - - /** - * Flashes JSON configuration data to bootloader - * @param configJson JSON configuration data - */ - std::tuple flashConfigurationData(std::string configJson, Memory memory = Memory::AUTO); + std::tuple flashConfigData(nlohmann::json configData, Memory memory = Memory::AUTO, Type type = Type::AUTO); /** * Flashes configuration data to bootloader * @param configPath Unstructured configuration data + * @param memory Optional - to which memory flash configuration + * @param type Optional - for which type of bootloader to flash configuration */ - std::tuple flashConfigurationFile(std::string configPath, Memory memory = Memory::AUTO); + std::tuple flashConfigFile(std::string configPath, Memory memory = Memory::AUTO, Type type = Type::AUTO); /** - * Flashes configuration data to bootloader - * @param configData Unstructured configuration data + * Clears configuration data + * @param memory Optional - on which memory to clear configuration data + * @param type Optional - for which type of bootloader to clear configuration data */ - std::tuple flashConfigurationClear(Memory memory = Memory::AUTO); + std::tuple flashConfigClear(Memory memory = Memory::AUTO, Type type = Type::AUTO); /** * Reads configuration from bootloader - * @returns Configuration + * @param memory Optional - from which memory to read configuration + * @param type Optional - from which type of bootloader to read configuration + * @returns Configuration structure */ - Config readConfiguration(Memory memory = Memory::AUTO); + Config readConfig(Memory memory = Memory::AUTO, Type type = Type::AUTO); /** * Flashes configuration to bootloader - * @param configData Configuration + * @param configData Configuration structure + * @param memory Optional - to which memory flash configuration + * @param type Optional - for which type of bootloader to flash configuration */ - std::tuple flashConfiguration(const Config& config); + std::tuple flashConfig(const Config& config, Memory memory = Memory::AUTO, Type type = Type::AUTO); /** * @returns Version of current running bootloader @@ -284,6 +361,9 @@ class DeviceBootloader { // Global namespace inline std::ostream& operator<<(std::ostream& out, const dai::DeviceBootloader::Type& type) { switch(type) { + case dai::DeviceBootloader::Type::AUTO: + out << "AUTO"; + break; case dai::DeviceBootloader::Type::USB: out << "USB"; break; diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 678ff6801..7b2c4ab14 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 678ff68015661137137c454383cc2ff7bf82f016 +Subproject commit 7b2c4ab141df199b3d08e06bda20a9326576c345 diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index ea75b69d9..24d9a8abc 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -16,10 +16,13 @@ #include "device/Device.hpp" #include "pipeline/Pipeline.hpp" #include "utility/BootloaderHelper.hpp" +#include "utility/Platform.hpp" #include "utility/Resources.hpp" // libraries +#include "spdlog/fmt/chrono.h" #include "spdlog/spdlog.h" +#include "zlib.h" // Resource compiled assets (cmds) #ifdef DEPTHAI_RESOURCE_COMPILED_BINARIES @@ -55,7 +58,7 @@ std::vector DeviceBootloader::getAllAvailableDevices() { return availableDevices; } -std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& pipeline, std::string pathToCmd) { +std::vector DeviceBootloader::createDepthaiApplicationPackage(const Pipeline& pipeline, std::string pathToCmd, bool compress) { // Serialize the pipeline PipelineSchema schema; Assets assets; @@ -99,12 +102,47 @@ std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& return ((((S) + (SECTION_ALIGNMENT_SIZE)-1)) & ~((SECTION_ALIGNMENT_SIZE)-1)); }; + // Should compress firmware? + if(compress) { + using namespace std::chrono; + + auto t1 = steady_clock::now(); + auto compressBufferSize = compressBound(deviceFirmware.size()); + std::vector compressBuffer(compressBufferSize); + // Chosen impirically + constexpr int COMPRESSION_LEVEL = 9; + if(compress2(compressBuffer.data(), &compressBufferSize, deviceFirmware.data(), deviceFirmware.size(), COMPRESSION_LEVEL) != Z_OK) { + throw std::runtime_error("Error while compressing device firmware\n"); + } + + // Resize output buffer + compressBuffer.resize(compressBufferSize); + + // Set the compressed firmware + auto prevSize = deviceFirmware.size(); + deviceFirmware = std::move(compressBuffer); + + auto diff = duration_cast(steady_clock::now() - t1); + spdlog::debug("Compressed firmware for Dephai Application Package. Took {}, size reduced from {:.2f}MiB to {:.2f}MiB", + diff, + prevSize / (1024.0f * 1024.0f), + deviceFirmware.size() / (1024.0f * 1024.0f)); + } + // First section, MVCMD, name '__firmware' sbr_section_set_name(fwSection, "__firmware"); sbr_section_set_bootable(fwSection, true); sbr_section_set_size(fwSection, static_cast(deviceFirmware.size())); sbr_section_set_checksum(fwSection, sbr_compute_checksum(deviceFirmware.data(), static_cast(deviceFirmware.size()))); sbr_section_set_offset(fwSection, SBR_RAW_SIZE); + // Ignore checksum to allow faster booting (images are verified after flashing, low risk) + sbr_section_set_ignore_checksum(fwSection, true); + // Set compression flags + if(compress) { + sbr_section_set_compression(fwSection, SBR_COMPRESSION_ZLIB); + } else { + sbr_section_set_compression(fwSection, SBR_NO_COMPRESSION); + } // Second section, pipeline schema, name 'pipeline' sbr_section_set_name(pipelineSection, "pipeline"); @@ -134,14 +172,30 @@ std::vector DeviceBootloader::createDepthaiApplicationPackage(Pipeline& sbr_serialize(&sbr, fwPackage.data(), static_cast(fwPackage.size())); // Write to fwPackage - for(unsigned i = 0; i < deviceFirmware.size(); i++) fwPackage[fwSection->offset + i] = deviceFirmware[i]; - for(unsigned i = 0; i < pipelineBinary.size(); i++) fwPackage[pipelineSection->offset + i] = pipelineBinary[i]; - for(unsigned i = 0; i < assetsBinary.size(); i++) fwPackage[assetsSection->offset + i] = assetsBinary[i]; - for(unsigned i = 0; i < assetStorage.size(); i++) fwPackage[assetStorageSection->offset + i] = assetStorage[i]; + for(std::size_t i = 0; i < deviceFirmware.size(); i++) fwPackage[fwSection->offset + i] = deviceFirmware[i]; + for(std::size_t i = 0; i < pipelineBinary.size(); i++) fwPackage[pipelineSection->offset + i] = pipelineBinary[i]; + for(std::size_t i = 0; i < assetsBinary.size(); i++) fwPackage[assetsSection->offset + i] = assetsBinary[i]; + for(std::size_t i = 0; i < assetStorage.size(); i++) fwPackage[assetStorageSection->offset + i] = assetStorage[i]; return fwPackage; } +std::vector DeviceBootloader::createDepthaiApplicationPackage(const Pipeline& pipeline, bool compress) { + return createDepthaiApplicationPackage(pipeline, "", compress); +} + +void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, const Pipeline& pipeline, std::string pathToCmd, bool compress) { + auto dap = createDepthaiApplicationPackage(pipeline, pathToCmd, compress); + std::ofstream outfile(path, std::ios::binary); + outfile.write(reinterpret_cast(dap.data()), dap.size()); +} + +void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, const Pipeline& pipeline, bool compress) { + auto dap = createDepthaiApplicationPackage(pipeline, compress); + std::ofstream outfile(path, std::ios::binary); + outfile.write(reinterpret_cast(dap.data()), dap.size()); +} + DeviceBootloader::DeviceBootloader(const DeviceInfo& devInfo, bool allowFlashingBootloader) : deviceInfo(devInfo) { init(true, "", tl::nullopt, allowFlashingBootloader); } @@ -411,14 +465,12 @@ bool DeviceBootloader::isAllowedFlashingBootloader() const { return allowFlashingBootloader; } -std::tuple DeviceBootloader::flash(std::function progressCb, Pipeline& pipeline) { - return flashDepthaiApplicationPackage(progressCb, createDepthaiApplicationPackage(pipeline)); +std::tuple DeviceBootloader::flash(std::function progressCb, const Pipeline& pipeline, bool compress) { + return flashDepthaiApplicationPackage(progressCb, createDepthaiApplicationPackage(pipeline, compress)); } -void DeviceBootloader::saveDepthaiApplicationPackage(std::string path, Pipeline& pipeline, std::string pathToCmd) { - auto dap = createDepthaiApplicationPackage(pipeline, pathToCmd); - std::ofstream outfile(path, std::ios::binary); - outfile.write(reinterpret_cast(dap.data()), dap.size()); +std::tuple DeviceBootloader::flash(const Pipeline& pipeline, bool compress) { + return flashDepthaiApplicationPackage(createDepthaiApplicationPackage(pipeline, compress)); } std::tuple DeviceBootloader::flashDepthaiApplicationPackage(std::function progressCb, std::vector package) { @@ -466,6 +518,10 @@ std::tuple DeviceBootloader::flashDepthaiApplicationPackage(s return {result.success, result.errorMsg}; } +std::tuple DeviceBootloader::flashDepthaiApplicationPackage(std::vector package) { + return flashDepthaiApplicationPackage(nullptr, package); +} + std::tuple DeviceBootloader::flashBootloader(std::function progressCb, std::string path) { return flashBootloader(Memory::FLASH, bootloaderType, progressCb, path); } @@ -600,10 +656,19 @@ std::tuple DeviceBootloader::flashCustom(Memory memory, uint3 } */ -nlohmann::json DeviceBootloader::readConfigurationData(Memory memory) { +nlohmann::json DeviceBootloader::readConfigData(Memory memory, Type type) { // Send request to GET_BOOTLOADER_CONFIG dai::bootloader::request::GetBootloaderConfig getConfigReq; getConfigReq.memory = memory; + + if(type != Type::AUTO) { + const auto confStructure = bootloader::getStructure(type); + getConfigReq.offset = confStructure.offset.at(bootloader::Section::BOOTLOADER_CONFIG); + getConfigReq.maxSize = confStructure.size.at(bootloader::Section::BOOTLOADER_CONFIG); + } else { + // leaves as default values, which correspond to AUTO + } + if(!sendBootloaderRequest(stream->getStreamId(), getConfigReq)) return {false, "Couldn't send request to get configuration data"}; // Get response @@ -620,10 +685,14 @@ nlohmann::json DeviceBootloader::readConfigurationData(Memory memory) { } } -std::tuple DeviceBootloader::flashConfigurationClear(Memory memory) { +std::tuple DeviceBootloader::flashConfigClear(Memory memory, Type type) { // send request to SET_BOOTLOADER_CONFIG dai::bootloader::request::SetBootloaderConfig setConfigReq; setConfigReq.memory = memory; + if(type != Type::AUTO) { + setConfigReq.offset = bootloader::getStructure(type).offset.at(bootloader::Section::BOOTLOADER_CONFIG); + } + setConfigReq.numPackets = 0; setConfigReq.totalSize = 0; setConfigReq.clearConfig = 1; @@ -637,13 +706,16 @@ std::tuple DeviceBootloader::flashConfigurationClear(Memory m return {result.success, result.errorMsg}; } -std::tuple DeviceBootloader::flashConfigurationData(nlohmann::json configData, Memory memory) { +std::tuple DeviceBootloader::flashConfigData(nlohmann::json configData, Memory memory, Type type) { // Parse to BSON auto bson = nlohmann::json::to_bson(configData); // Send request to SET_BOOTLOADER_CONFIG dai::bootloader::request::SetBootloaderConfig setConfigReq; setConfigReq.memory = memory; + if(type != Type::AUTO) { + setConfigReq.offset = bootloader::getStructure(type).offset.at(bootloader::Section::BOOTLOADER_CONFIG); + } setConfigReq.numPackets = 1; setConfigReq.totalSize = bson.size(); setConfigReq.clearConfig = 0; @@ -660,28 +732,24 @@ std::tuple DeviceBootloader::flashConfigurationData(nlohmann: return {result.success, result.errorMsg}; } -std::tuple DeviceBootloader::flashConfigurationData(std::string configJson, Memory memory) { - return flashConfigurationData(nlohmann::json::parse(configJson), memory); -} - -std::tuple DeviceBootloader::flashConfigurationFile(std::string configPath, Memory memory) { +std::tuple DeviceBootloader::flashConfigFile(std::string configPath, Memory memory, Type type) { // read a JSON file std::ifstream configInputStream(configPath); if(!configInputStream.is_open()) throw std::runtime_error("Cannot flash configuration, JSON at path: " + configPath + " doesn't exist"); nlohmann::json configJson; configInputStream >> configJson; - return flashConfigurationData(configJson, memory); + return flashConfigData(configJson, memory, type); } -DeviceBootloader::Config DeviceBootloader::readConfiguration(Memory memory) { - auto json = readConfigurationData(memory); +DeviceBootloader::Config DeviceBootloader::readConfig(Memory memory, Type type) { + auto json = readConfigData(memory, type); // Implicit parse from json to Config return json; } -std::tuple DeviceBootloader::flashConfiguration(const Config& config) { +std::tuple DeviceBootloader::flashConfig(const Config& config, Memory memory, Type type) { // Implicit parse from Config to json - return flashConfigurationData(config); + return flashConfigData(config, memory, type); } bool DeviceBootloader::isEmbeddedVersion() const { @@ -727,4 +795,96 @@ std::string DeviceBootloader::Version::toString() const { return std::to_string(versionMajor) + "." + std::to_string(versionMinor) + "." + std::to_string(versionPatch); } +// Config functions +void DeviceBootloader::Config::setStaticIPv4(std::string ip, std::string mask, std::string gateway) { + network.ipv4 = platform::getIPv4AddressAsBinary(ip); + network.ipv4Mask = platform::getIPv4AddressAsBinary(mask); + network.ipv4Gateway = platform::getIPv4AddressAsBinary(gateway); + network.staticIpv4 = true; +} +void DeviceBootloader::Config::setDynamicIPv4(std::string ip, std::string mask, std::string gateway) { + network.ipv4 = platform::getIPv4AddressAsBinary(ip); + network.ipv4Mask = platform::getIPv4AddressAsBinary(mask); + network.ipv4Gateway = platform::getIPv4AddressAsBinary(gateway); + network.staticIpv4 = false; +} + +bool DeviceBootloader::Config::isStaticIPV4() { + return network.staticIpv4; +} + +std::string DeviceBootloader::Config::getIPv4() { + return platform::getIPv4AddressAsString(network.ipv4); +} +std::string DeviceBootloader::Config::getIPv4Mask() { + return platform::getIPv4AddressAsString(network.ipv4Mask); +} +std::string DeviceBootloader::Config::getIPv4Gateway() { + return platform::getIPv4AddressAsString(network.ipv4Gateway); +} + +void DeviceBootloader::Config::setDnsIPv4(std::string dns, std::string dnsAlt) { + network.ipv4Dns = platform::getIPv4AddressAsBinary(dns); + network.ipv4DnsAlt = platform::getIPv4AddressAsBinary(dnsAlt); +} + +std::string DeviceBootloader::Config::getDnsIPv4() { + return platform::getIPv4AddressAsString(network.ipv4Dns); +} + +std::string DeviceBootloader::Config::getDnsAltIPv4() { + return platform::getIPv4AddressAsString(network.ipv4DnsAlt); +} + +void DeviceBootloader::Config::setUsbTimeout(std::chrono::milliseconds ms) { + usb.timeoutMs = ms.count(); +} + +std::chrono::milliseconds DeviceBootloader::Config::getUsbTimeout() { + return std::chrono::milliseconds(usb.timeoutMs); +} + +void DeviceBootloader::Config::setNetworkTimeout(std::chrono::milliseconds ms) { + network.timeoutMs = ms.count(); +} + +std::chrono::milliseconds DeviceBootloader::Config::getNetworkTimeout() { + return std::chrono::milliseconds(network.timeoutMs); +} + +void DeviceBootloader::Config::setUsbMaxSpeed(UsbSpeed speed) { + usb.maxUsbSpeed = static_cast(speed); +} + +UsbSpeed DeviceBootloader::Config::getUsbMaxSpeed() { + return static_cast(usb.maxUsbSpeed); +} + +void DeviceBootloader::Config::setMacAddress(std::string mac) { + std::array a; + int last = -1; + int rc = std::sscanf(mac.c_str(), "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx%n", &a[0], &a[1], &a[2], &a[3], &a[4], &a[5], &last); + if(rc != 6 || static_cast(mac.size()) != last) { + throw std::invalid_argument("Invalid MAC address format " + mac); + } + + // Set the parsed mac address + network.mac = a; +} +std::string DeviceBootloader::Config::getMacAddress() { + // 32 characters is adequite for MAC address representation + std::array macStr = {}; + std::snprintf(macStr.data(), + macStr.size(), + "%02X:%02X:%02X:%02X:%02X:%02X", + network.mac[0], + network.mac[1], + network.mac[2], + network.mac[3], + network.mac[4], + network.mac[5]); + + return std::string(macStr.data()); +} + } // namespace dai diff --git a/src/utility/Platform.cpp b/src/utility/Platform.cpp new file mode 100644 index 000000000..be60c10da --- /dev/null +++ b/src/utility/Platform.cpp @@ -0,0 +1,47 @@ +#include "Platform.hpp" + +// Platform specific +#if defined(_WIN32) || defined(__USE_W32_SOCKETS) + #include + #ifdef _MSC_VER + #pragma comment(lib, "Ws2_32.lib") + #endif +#else + #if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__) + #include + #endif + #include +#endif + +namespace dai { +namespace platform { + +uint32_t getIPv4AddressAsBinary(std::string address) { + uint32_t binary = 0; + +#if defined(_WIN32) || defined(__USE_W32_SOCKETS) + #ifdef _WIN32_WINNT 0x0501 + binary = inet_addr(address.c_str()); // for XP + #endif + inet_pton(AF_INET, address.c_str(), &binary); // for Vista or higher +#else + inet_pton(AF_INET, address.c_str(), &binary); +#endif + + return binary; +} + +std::string getIPv4AddressAsString(std::uint32_t binary) { + char address[INET_ADDRSTRLEN] = {0}; + +#if defined(_WIN32) || defined(__USE_W32_SOCKETS) + InetNtopA(AF_INET, &binary, address, sizeof(address)); +#else + inet_ntop(AF_INET, &binary, address, sizeof(address)); +#endif + + return {address}; +} + +} // namespace platform +} // namespace dai diff --git a/src/utility/Platform.hpp b/src/utility/Platform.hpp new file mode 100644 index 000000000..ef299164b --- /dev/null +++ b/src/utility/Platform.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +namespace dai { +namespace platform { + +uint32_t getIPv4AddressAsBinary(std::string address); +std::string getIPv4AddressAsString(std::uint32_t binary); + +} +} diff --git a/src/utility/Resources.cpp b/src/utility/Resources.cpp index 8804be82a..9608cf90b 100644 --- a/src/utility/Resources.cpp +++ b/src/utility/Resources.cpp @@ -218,6 +218,10 @@ std::vector Resources::getBootloaderFirmware(dai::bootloader::Type std::unique_lock lock(mtxBootloader); switch(type) { + case dai::bootloader::Type::AUTO: + throw std::invalid_argument("DeviceBootloader::Type::AUTO not allowed, when getting bootloader firmware."); + break; + case dai::bootloader::Type::USB: return resourceMapBootloader[DEVICE_BOOTLOADER_USB_PATH]; break; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 6b258c53b..6b3963b4f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -99,5 +99,7 @@ target_compile_definitions(openvino_blob PRIVATE OPENVINO_2021_4_BLOB_PATH="${openvino_2021_4_blob}" ) +# Bootloader configuration tests +dai_add_test(bootloader_config_test src/bootloader_config_test.cpp) diff --git a/tests/src/bootloader_config_test.cpp b/tests/src/bootloader_config_test.cpp new file mode 100644 index 000000000..278665eb8 --- /dev/null +++ b/tests/src/bootloader_config_test.cpp @@ -0,0 +1,39 @@ +#include + +#include "depthai/depthai.hpp" + +int main() { + dai::DeviceBootloader::Config config; + + std::string ipv4 = "192.168.1.150"; + std::string ipv4Mask = "255.255.255.0"; + std::string ipv4Gateway = "192.168.1.1"; + + config.setStaticIPv4(ipv4, ipv4Mask, ipv4Gateway); + + assert(ipv4 == config.getIPv4()); + assert(ipv4Mask == config.getIPv4Mask()); + assert(ipv4Gateway == config.getIPv4Gateway()); + + std::string dns = "1.1.1.1"; + std::string dnsAlt = "8.8.8.8"; + + config.setDnsIPv4(dns); + + assert(config.getDnsIPv4() == dns); + assert(config.network.ipv4DnsAlt == 0); + + config.setDnsIPv4(dns, dnsAlt); + + assert(config.getDnsIPv4() == dns); + assert(config.getDnsAltIPv4() == dnsAlt); + + // MAC address + std::string mac = "FF:AA:BB:CC:00:11"; + config.setMacAddress(mac); + // std::cout << "Orig mac address: " << mac << " len: " << mac.length() << std::endl; + // std::cout << "Get mac address: " << config.getMacAddress() << " len: " << config.getMacAddress().length() << std::endl; + assert(config.getMacAddress() == mac); + + return 0; +} \ No newline at end of file From 6a414d4c081903a748a3941aed9b6f1c400cb31f Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sat, 21 Aug 2021 22:35:40 +0200 Subject: [PATCH 032/101] Updated flash_bootloader example --- examples/src/flash_bootloader.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index e1847c918..001145797 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -6,7 +6,7 @@ int main(int argc, char** argv) { using namespace std::chrono; - dai::DeviceBootloader::Type blType = dai::DeviceBootloader::Type::USB; + dai::DeviceBootloader::Type blType = dai::DeviceBootloader::Type::AUTO; if(argc > 1) { std::string blTypeStr(argv[1]); if(blTypeStr == "usb") { @@ -17,9 +17,6 @@ int main(int argc, char** argv) { std::cout << "Specify either 'usb' or 'network' bootloader type\n"; return 0; } - } else { - std::cout << "Usage: " << argv[0] << " \n"; - return 0; } std::cout << "Warning! Flashing bootloader can potentially soft brick your device and should be done with caution." << std::endl; @@ -39,11 +36,12 @@ int main(int argc, char** argv) { } // Open DeviceBootloader and allow flashing bootloader + std::cout << "Booting latest bootloader first, will take a tad longer..." << std::endl; dai::DeviceBootloader bl(info, true); auto currentBlType = bl.getType(); // Check if bootloader type is the same - if(currentBlType != blType) { + if(blType != dai::DeviceBootloader::Type::AUTO && currentBlType != blType) { std::cout << "Are you sure you want to flash '" << blType << "' bootloader over current '" << currentBlType << "' bootloader?" << std::endl; std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; std::cin.ignore(); @@ -56,11 +54,11 @@ int main(int argc, char** argv) { // Create a progress callback lambda auto progress = [](float p) { std::cout << "Flashing Progress..." << p * 100 << "%" << std::endl; }; - std::cout << "Flashing " << blType << " bootloader..." << std::endl; + std::cout << "Flashing " << currentBlType << " bootloader..." << std::endl; auto t1 = steady_clock::now(); bool success = false; std::string message; - std::tie(success, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, blType, progress); + std::tie(success, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, currentBlType, progress); if(success) { std::cout << "Flashing successful. Took " << duration_cast(steady_clock::now() - t1).count() << "ms" << std::endl; } else { From fb57b897826fb7feeb99522b5c63a2350764dde5 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 23 Aug 2021 05:28:13 +0200 Subject: [PATCH 033/101] Added versioning to BL requests and refactored --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 2 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/CMakeLists.txt | 2 +- ...onfiguration.cpp => bootloader_config.cpp} | 0 examples/src/rgb_preview.cpp | 2 +- include/depthai/device/DeviceBootloader.hpp | 29 ++- include/depthai/pipeline/node/Script.hpp | 2 + shared/depthai-bootloader-shared | 2 +- shared/depthai-bootloader-shared.cmake | 5 +- src/device/Device.cpp | 1 - src/device/DeviceBase.cpp | 73 +----- src/device/DeviceBootloader.cpp | 222 ++++++++++++------ src/utility/BootloaderHelper.hpp | 68 ------ 13 files changed, 200 insertions(+), 210 deletions(-) rename examples/src/{bootloader_configuration.cpp => bootloader_config.cpp} (100%) delete mode 100644 src/utility/BootloaderHelper.hpp diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index e91bab56b..cc3090bfc 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -4,4 +4,4 @@ set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" #set(DEPTHAI_BOOTLOADER_VERSION "0.0.12") -set(DEPTHAI_BOOTLOADER_VERSION "a243221305bd0351cc2c9b98037e931068f4afef") +set(DEPTHAI_BOOTLOADER_VERSION "ad1e3d8c3a335c42d17e3b968e5d26e69885d706") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 6c439da18..014898834 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 "9b857aedf34a19c7738a2f7f2f16a39f4e3831b3") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b595df452a0366d44b5b921cba31cc87f3596ad4") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 7c726f8b5..e0300553b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -292,4 +292,4 @@ target_compile_definitions(calibration_flash_version5 PRIVATE CALIB_PATH="${cali target_compile_definitions(calibration_load PRIVATE CALIB_PATH="${calib_v6}") # Bootloader configuration example -dai_add_example(bootloader_configuration src/bootloader_configuration.cpp OFF) +dai_add_example(bootloader_config src/bootloader_config.cpp OFF) diff --git a/examples/src/bootloader_configuration.cpp b/examples/src/bootloader_config.cpp similarity index 100% rename from examples/src/bootloader_configuration.cpp rename to examples/src/bootloader_config.cpp diff --git a/examples/src/rgb_preview.cpp b/examples/src/rgb_preview.cpp index 911f7d1d9..70e41867d 100644 --- a/examples/src/rgb_preview.cpp +++ b/examples/src/rgb_preview.cpp @@ -48,7 +48,7 @@ int main() { int key = cv::waitKey(1); if(key == 'q' || key == 'Q') { - return 0; + break; } } return 0; diff --git a/include/depthai/device/DeviceBootloader.hpp b/include/depthai/device/DeviceBootloader.hpp index ae290dd56..5e07fccbf 100644 --- a/include/depthai/device/DeviceBootloader.hpp +++ b/include/depthai/device/DeviceBootloader.hpp @@ -296,10 +296,23 @@ class DeviceBootloader { */ std::tuple flashConfig(const Config& config, Memory memory = Memory::AUTO, Type type = Type::AUTO); + /** + * Boots a custom FW in memory + * @param fw + * @throws A runtime exception if there are any communication issues + */ + void bootMemory(const std::vector& fw); + + /** + * Boots into integrated ROM bootloader in USB mode + * @throws A runtime exception if there are any communication issues + */ + void bootUsbRomBootloader(); + /** * @returns Version of current running bootloader */ - Version getVersion(); + Version getVersion() const; /** * @returns True when bootloader was booted using latest bootloader integrated in the library. @@ -332,10 +345,19 @@ class DeviceBootloader { private: // private static - // private variables + // private methods void init(bool embeddedMvcmd, const std::string& pathToMvcmd, tl::optional type, bool allowBlFlash); void checkClosed() const; + template + bool sendRequest(const T& request); + bool receiveResponseData(std::vector& data); + template + bool parseResponse(const std::vector& data, T& response); + template + bool receiveResponse(T& response); + Version requestVersion(); + // private variables std::shared_ptr connection; DeviceInfo deviceInfo = {}; @@ -354,6 +376,9 @@ class DeviceBootloader { // Allow flashing bootloader flag bool allowFlashingBootloader = false; + + // Current connected bootloader version + Version version{0, 0, 2}; }; } // namespace dai diff --git a/include/depthai/pipeline/node/Script.hpp b/include/depthai/pipeline/node/Script.hpp index b20c37686..2640bfc17 100644 --- a/include/depthai/pipeline/node/Script.hpp +++ b/include/depthai/pipeline/node/Script.hpp @@ -48,12 +48,14 @@ class Script : public Node { /** * Sets script data to be interpreted * @param script Script string to be interpreted + * @param name Optionally set a name of this script */ void setScript(const std::string& script, const std::string& name = ""); /** * Sets script data to be interpreted * @param data Binary data that represents the script to be interpreted + * @param name Optionally set a name of this script */ void setScript(const std::vector& data, const std::string& name = ""); diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 7b2c4ab14..1d97cf463 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 7b2c4ab141df199b3d08e06bda20a9326576c345 +Subproject commit 1d97cf4630268c1292dfe9104abc19c7fe486da5 diff --git a/shared/depthai-bootloader-shared.cmake b/shared/depthai-bootloader-shared.cmake index 2f1c56ed4..18435fb8b 100644 --- a/shared/depthai-bootloader-shared.cmake +++ b/shared/depthai-bootloader-shared.cmake @@ -2,6 +2,7 @@ set(DEPTHAI_BOOTLOADER_SHARED_FOLDER ${CMAKE_CURRENT_LIST_DIR}/depthai-bootloade set(DEPTHAI_BOOTLOADER_SHARED_SOURCES ${DEPTHAI_BOOTLOADER_SHARED_FOLDER}/src/SBR.c + ${DEPTHAI_BOOTLOADER_SHARED_FOLDER}/src/Bootloader.cpp ) set(DEPTHAI_BOOTLOADER_SHARED_PUBLIC_INCLUDE @@ -26,8 +27,8 @@ if(GIT_FOUND AND NOT DEPTHAI_DOWNLOADED_SOURCES) string(SUBSTRING ${statusCommit} 0 1 status) if(${status} STREQUAL "-") message(FATAL_ERROR "Submodule 'depthai-bootloader-shared' not initialized/updated. Run 'git submodule update --init --recursive' first") - endif() - + endif() + # Get depthai-bootloader-shared current commit execute_process( COMMAND ${GIT_EXECUTABLE} rev-parse HEAD diff --git a/src/device/Device.cpp b/src/device/Device.cpp index 8810a0fdc..d07e229f7 100644 --- a/src/device/Device.cpp +++ b/src/device/Device.cpp @@ -15,7 +15,6 @@ #include "depthai/pipeline/node/XLinkIn.hpp" #include "depthai/pipeline/node/XLinkOut.hpp" #include "pipeline/Pipeline.hpp" -#include "utility/BootloaderHelper.hpp" #include "utility/Initialization.hpp" #include "utility/Resources.hpp" diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 452740aa9..8ea371980 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -18,7 +18,6 @@ #include "depthai/pipeline/node/XLinkIn.hpp" #include "depthai/pipeline/node/XLinkOut.hpp" #include "pipeline/Pipeline.hpp" -#include "utility/BootloaderHelper.hpp" #include "utility/Initialization.hpp" #include "utility/PimplImpl.hpp" #include "utility/Resources.hpp" @@ -408,85 +407,31 @@ void DeviceBase::init2(bool embeddedMvcmd, bool usb2Mode, const std::string& pat } } else if(deviceInfo.state == X_LINK_BOOTLOADER) { - // Scope so bootloaderConnection is desctructed and XLink cleans its state + // Scope so DeviceBootloader is disconnected { - // TODO(themarpe) - move this logic to DeviceBootloader - - // Bootloader state, proceed by issuing a command to bootloader - XLinkConnection bootloaderConnection(deviceInfo, X_LINK_BOOTLOADER); - - // Open stream - XLinkStream stream(bootloaderConnection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); - - // prepare watchdog thread, which will keep device alive - std::atomic watchdogRunning{true}; - watchdogThread = std::thread([&]() { - // prepare watchdog thread - XLinkStream stream(bootloaderConnection, bootloader::XLINK_CHANNEL_WATCHDOG, 64); - std::vector watchdogKeepalive = {0, 0, 0, 0}; - while(watchdogRunning) { - try { - stream.write(watchdogKeepalive); - } catch(const std::exception&) { - break; - } - // Ping with a period half of that of the watchdog timeout - std::this_thread::sleep_for(bootloader::XLINK_WATCHDOG_TIMEOUT / 2); - } - }); - - // Send request for bootloader version - if(!sendBootloaderRequest(stream.getStreamId(), bootloader::request::GetBootloaderVersion{})) { - throw std::runtime_error("Error trying to connect to device"); - } - // Receive response - bootloader::response::BootloaderVersion ver; - if(!receiveBootloaderResponse(stream.getStreamId(), ver)) throw std::runtime_error("Error trying to connect to device"); - DeviceBootloader::Version version(ver.major, ver.minor, ver.patch); + DeviceBootloader bl(deviceInfo); + auto version = bl.getVersion(); // If version is >= 0.0.12 then boot directly, otherwise jump to USB ROM bootloader // Check if version is recent enough for this operation if(version >= DeviceBootloader::Version(0, 0, 12)) { - // Send request to boot firmware directly from bootloader - dai::bootloader::request::BootMemory bootMemory; - bootMemory.totalSize = static_cast(embeddedFw.size()); - bootMemory.numPackets = ((static_cast(embeddedFw.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(stream.getStreamId(), bootMemory)) { - throw std::runtime_error("Error trying to connect to device"); - } - using namespace std::chrono; + // Boot the given FW auto t1 = steady_clock::now(); - // After that send numPackets of data - stream.writeSplit(embeddedFw.data(), embeddedFw.size(), bootloader::XLINK_STREAM_MAX_SIZE); - spdlog::debug( - "Booting FW with Bootloader. Version {}, Time taken: {}", version.toString(), duration_cast(steady_clock::now() - t1)); + bl.bootMemory(embeddedFw); + auto t2 = steady_clock::now(); + spdlog::debug("Booting FW with Bootloader. Version {}, Time taken: {}", version.toString(), duration_cast(t2 - t1)); // After that the state will be BOOTED deviceInfo.state = X_LINK_BOOTED; - - // TODO(themarpe) - Possibility of switching to another port for cleaner transitions - // strcat(deviceInfo.desc.name, ":11492"); - } else { + // Boot into USB ROM BOOTLOADER + bl.bootUsbRomBootloader(); spdlog::debug("Booting FW by jumping to USB ROM Bootloader first. Bootloader Version {}", version.toString()); - // Send request to jump to USB bootloader - // Boot into USB ROM BOOTLOADER NOW - if(!sendBootloaderRequest(stream.getStreamId(), dai::bootloader::request::UsbRomBoot{})) { - throw std::runtime_error("Error trying to connect to device"); - } - // After that the state will be UNBOOTED deviceInfo.state = X_LINK_UNBOOTED; } - - watchdogRunning = false; - watchdogThread.join(); - - // Dummy read, until link falls down and it returns an error code - streamPacketDesc_t* pPacket; - XLinkReadData(stream.getStreamId(), &pPacket); } // Boot and connect with XLinkConnection constructor diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 24d9a8abc..d82ec156f 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -15,7 +15,6 @@ // project #include "device/Device.hpp" #include "pipeline/Pipeline.hpp" -#include "utility/BootloaderHelper.hpp" #include "utility/Platform.hpp" #include "utility/Resources.hpp" @@ -32,6 +31,10 @@ CMRC_DECLARE(depthai); namespace dai { +// Using +namespace Request = bootloader::request; +namespace Response = bootloader::response; + // constants constexpr const DeviceBootloader::Type DeviceBootloader::DEFAULT_TYPE; @@ -230,6 +233,9 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // prepare bootloader stream stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); + // Retrieve bootloader version + version = requestVersion(); + // Device wasn't already in bootloader, that means that embedded bootloader is booted isEmbedded = true; } else if(deviceInfo.state == X_LINK_BOOTLOADER) { @@ -243,26 +249,18 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // If type is specified, try to boot into that BL type stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); - // Send request for bootloader version - if(!sendBootloaderRequest(stream->getStreamId(), bootloader::request::GetBootloaderVersion{})) { - throw std::runtime_error("Error trying to connect to device"); - } - - // Receive response - bootloader::response::BootloaderVersion ver; - if(!receiveBootloaderResponse(stream->getStreamId(), ver)) throw std::runtime_error("Error trying to connect to device"); - DeviceBootloader::Version version(ver.major, ver.minor, ver.patch); - + // Retrieve bootloader version + version = requestVersion(); if(version >= Version(0, 0, 12)) { // If version is adequate, do an in memory boot. // Send request for bootloader type - if(!sendBootloaderRequest(stream->getStreamId(), bootloader::request::GetBootloaderType{})) { + if(!sendRequest(Request::GetBootloaderType{})) { throw std::runtime_error("Error trying to connect to device"); } // Receive response - bootloader::response::BootloaderType runningBootloaderType; - if(!receiveBootloaderResponse(stream->getStreamId(), runningBootloaderType)) throw std::runtime_error("Error trying to connect to device"); + Response::BootloaderType runningBootloaderType; + if(!receiveResponse(runningBootloaderType)) throw std::runtime_error("Error trying to connect to device"); // Modify actual bootloader type bootloaderType = runningBootloaderType.type; @@ -289,11 +287,11 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, }); // Send request to boot firmware directly from bootloader - dai::bootloader::request::BootMemory bootMemory; + Request::BootMemory bootMemory; auto binary = getEmbeddedBootloaderBinary(desiredBootloaderType); bootMemory.totalSize = static_cast(binary.size()); bootMemory.numPackets = ((static_cast(binary.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(stream->getStreamId(), bootMemory)) { + if(!sendRequest(bootMemory)) { throw std::runtime_error("Error trying to connect to device"); } @@ -312,6 +310,9 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // prepare new bootloader stream stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); + // Retrieve bootloader version + version = requestVersion(); + // The type of bootloader is now 'desiredBootloaderType' bootloaderType = desiredBootloaderType; @@ -330,7 +331,7 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, if((desiredBootloaderType != Type::USB) || allowFlashingBootloader) { // Send request to jump to USB bootloader // Boot into USB ROM BOOTLOADER NOW - if(!sendBootloaderRequest(stream->getStreamId(), dai::bootloader::request::UsbRomBoot{})) { + if(!sendRequest(Request::UsbRomBoot{})) { throw std::runtime_error("Error trying to connect to device"); } // Close existing stream first @@ -349,6 +350,10 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // prepare bootloader stream stream = std::make_unique(*connection, bootloader::XLINK_CHANNEL_BOOTLOADER, bootloader::XLINK_STREAM_MAX_SIZE); + // Retrieve bootloader version + version = requestVersion(); + + // The type of bootloader is now 'desiredBootloaderType' bootloaderType = desiredBootloaderType; // Embedded bootloader was used to boot, set to true @@ -439,17 +444,19 @@ DeviceBootloader::Version DeviceBootloader::getEmbeddedBootloaderVersion() { return DeviceBootloader::Version(DEPTHAI_BOOTLOADER_VERSION); } -DeviceBootloader::Version DeviceBootloader::getVersion() { - streamId_t streamId = stream->getStreamId(); +DeviceBootloader::Version DeviceBootloader::getVersion() const { + return version; +} +DeviceBootloader::Version DeviceBootloader::requestVersion() { // Send request to jump to USB bootloader - if(!sendBootloaderRequest(streamId, bootloader::request::GetBootloaderVersion{})) { + if(!sendRequest(Request::GetBootloaderVersion{})) { throw std::runtime_error("Couldn't get bootloader version"); } // Receive response - dai::bootloader::response::BootloaderVersion ver; - if(!receiveBootloaderResponse(streamId, ver)) { + Response::BootloaderVersion ver; + if(!receiveResponse(ver)) { throw std::runtime_error("Couldn't get bootloader version"); } @@ -474,8 +481,6 @@ std::tuple DeviceBootloader::flash(const Pipeline& pipeline, } std::tuple DeviceBootloader::flashDepthaiApplicationPackage(std::function progressCb, std::vector package) { - streamId_t streamId = stream->getStreamId(); - // Bug in NETWORK bootloader in version 0.0.12 < 0.1.0 - flashing can cause a soft brick auto version = getVersion(); if(bootloaderType == Type::NETWORK && version < Version(0, 0, 14)) { @@ -483,29 +488,29 @@ std::tuple DeviceBootloader::flashDepthaiApplicationPackage(s } // send request to FLASH BOOTLOADER - dai::bootloader::request::UpdateFlash updateFlash; - updateFlash.storage = dai::bootloader::request::UpdateFlash::SBR; + Request::UpdateFlash updateFlash; + updateFlash.storage = Request::UpdateFlash::SBR; updateFlash.totalSize = static_cast(package.size()); updateFlash.numPackets = ((static_cast(package.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(streamId, updateFlash)) return {false, "Couldn't send bootloader flash request"}; + if(!sendRequest(updateFlash)) return {false, "Couldn't send bootloader flash request"}; // After that send numPackets of data stream->writeSplit(package.data(), package.size(), bootloader::XLINK_STREAM_MAX_SIZE); // Then wait for response by bootloader // Wait till FLASH_COMPLETE response - dai::bootloader::response::FlashComplete result; + Response::FlashComplete result; do { std::vector data; - if(!receiveBootloaderResponseData(streamId, data)) return {false, "Couldn't receive bootloader response"}; + if(!receiveResponseData(data)) return {false, "Couldn't receive bootloader response"}; - dai::bootloader::response::FlashStatusUpdate update; - if(parseBootloaderResponse(data, update)) { + Response::FlashStatusUpdate update; + if(parseResponse(data, update)) { // if progress callback is set if(progressCb != nullptr) { progressCb(update.progress); } - } else if(parseBootloaderResponse(data, result)) { + } else if(parseResponse(data, result)) { break; } else { // Unknown response, shouldn't happen @@ -536,7 +541,7 @@ std::tuple DeviceBootloader::flashBootloader(Memory memory, T if(memory != Memory::FLASH) { throw std::invalid_argument("Only FLASH memory is supported for now"); } - if(bootloaderType != type && getVersion() < Version(0, 0, 12)) { + if(bootloaderType != type && getVersion() < Version(Request::UpdateFlashEx2::VERSION)) { std::runtime_error("Current bootloader version doesn't support flashing different type of bootloader"); } @@ -549,29 +554,26 @@ std::tuple DeviceBootloader::flashBootloader(Memory memory, T package = getEmbeddedBootloaderBinary(type); } - // get streamId - streamId_t streamId = stream->getStreamId(); - // If booted and desired bootloader types don't match // Use UpdateFlashEx2 instead to properly flash if(bootloaderType == type) { // Old command // send request to FLASH BOOTLOADER - dai::bootloader::request::UpdateFlash updateFlash; - updateFlash.storage = dai::bootloader::request::UpdateFlash::BOOTLOADER; + Request::UpdateFlash updateFlash; + updateFlash.storage = Request::UpdateFlash::BOOTLOADER; updateFlash.totalSize = static_cast(package.size()); updateFlash.numPackets = ((static_cast(package.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(streamId, updateFlash)) return {false, "Couldn't send bootloader flash request"}; + if(!sendRequest(updateFlash)) return {false, "Couldn't send bootloader flash request"}; } else { // send request to FLASH BOOTLOADER - dai::bootloader::request::UpdateFlashEx2 updateFlashEx2; + Request::UpdateFlashEx2 updateFlashEx2; updateFlashEx2.memory = memory; updateFlashEx2.offset = dai::bootloader::getStructure(type).offset.at(Section::BOOTLOADER); updateFlashEx2.totalSize = static_cast(package.size()); updateFlashEx2.numPackets = ((static_cast(package.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(streamId, updateFlashEx2)) return {false, "Couldn't send bootloader flash request"}; + if(!sendRequest(updateFlashEx2)) return {false, "Couldn't send bootloader flash request"}; } // After that send numPackets of data @@ -579,19 +581,19 @@ std::tuple DeviceBootloader::flashBootloader(Memory memory, T // Then wait for response by bootloader // Wait till FLASH_COMPLETE response - dai::bootloader::response::FlashComplete result; + Response::FlashComplete result; do { std::vector data; - if(!receiveBootloaderResponseData(streamId, data)) return {false, "Couldn't receive bootloader response"}; + if(!receiveResponseData(data)) return {false, "Couldn't receive bootloader response"}; - dai::bootloader::response::FlashStatusUpdate update; - if(parseBootloaderResponse(data, update)) { + Response::FlashStatusUpdate update; + if(parseResponse(data, update)) { // if progress callback is set if(progressCb != nullptr) { progressCb(update.progress); } // if flash complete response arrived, break from while loop - } else if(parseBootloaderResponse(data, result)) { + } else if(parseResponse(data, result)) { break; } else { // Unknown response, shouldn't happen @@ -618,24 +620,24 @@ std::tuple DeviceBootloader::flashCustom(Memory memory, uint3 streamId_t streamId = stream->getStreamId(); // send request to FLASH BOOTLOADER - dai::bootloader::request::UpdateFlashEx2 updateFlashEx2; + Request::UpdateFlashEx2 updateFlashEx2; updateFlashEx2.memory = memory; updateFlashEx2.offset = offset; updateFlashEx2.totalSize = static_cast(data.size()); updateFlashEx2.numPackets = ((static_cast(data.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; - if(!sendBootloaderRequest(streamId, updateFlashEx2)) return {false, "Couldn't send bootloader flash request"}; + if(!sendRequest(updateFlashEx2)) return {false, "Couldn't send bootloader flash request"}; // After that send numPackets of data stream->writeSplit(data.data(), data.size(), bootloader::XLINK_STREAM_MAX_SIZE); // Then wait for response by bootloader // Wait till FLASH_COMPLETE response - dai::bootloader::response::FlashComplete result; + Response::FlashComplete result; do { std::vector data; - if(!receiveBootloaderResponseData(streamId, data)) return {false, "Couldn't receive bootloader response"}; + if(!receiveResponseData(data)) return {false, "Couldn't receive bootloader response"}; - dai::bootloader::response::FlashStatusUpdate update; + Response::FlashStatusUpdate update; if(parseBootloaderResponse(data, update)) { // if progress callback is set if(progressCb != nullptr) { @@ -658,7 +660,7 @@ std::tuple DeviceBootloader::flashCustom(Memory memory, uint3 nlohmann::json DeviceBootloader::readConfigData(Memory memory, Type type) { // Send request to GET_BOOTLOADER_CONFIG - dai::bootloader::request::GetBootloaderConfig getConfigReq; + Request::GetBootloaderConfig getConfigReq; getConfigReq.memory = memory; if(type != Type::AUTO) { @@ -669,11 +671,11 @@ nlohmann::json DeviceBootloader::readConfigData(Memory memory, Type type) { // leaves as default values, which correspond to AUTO } - if(!sendBootloaderRequest(stream->getStreamId(), getConfigReq)) return {false, "Couldn't send request to get configuration data"}; + if(!sendRequest(getConfigReq)) return {false, "Couldn't send request to get configuration data"}; // Get response - dai::bootloader::response::GetBootloaderConfig resp; - receiveBootloaderResponse(stream->getStreamId(), resp); + Response::GetBootloaderConfig resp; + receiveResponse(resp); if(resp.success) { // Read back bootloader config (1 packet max) @@ -687,7 +689,7 @@ nlohmann::json DeviceBootloader::readConfigData(Memory memory, Type type) { std::tuple DeviceBootloader::flashConfigClear(Memory memory, Type type) { // send request to SET_BOOTLOADER_CONFIG - dai::bootloader::request::SetBootloaderConfig setConfigReq; + Request::SetBootloaderConfig setConfigReq; setConfigReq.memory = memory; if(type != Type::AUTO) { setConfigReq.offset = bootloader::getStructure(type).offset.at(bootloader::Section::BOOTLOADER_CONFIG); @@ -696,11 +698,11 @@ std::tuple DeviceBootloader::flashConfigClear(Memory memory, setConfigReq.numPackets = 0; setConfigReq.totalSize = 0; setConfigReq.clearConfig = 1; - if(!sendBootloaderRequest(stream->getStreamId(), setConfigReq)) return {false, "Couldn't send request to flash configuration data"}; + if(!sendRequest(setConfigReq)) return {false, "Couldn't send request to flash configuration data"}; // Read back response - dai::bootloader::response::FlashComplete result; - receiveBootloaderResponse(stream->getStreamId(), result); + Response::FlashComplete result; + receiveResponse(result); // Return if flashing was successful return {result.success, result.errorMsg}; @@ -711,7 +713,7 @@ std::tuple DeviceBootloader::flashConfigData(nlohmann::json c auto bson = nlohmann::json::to_bson(configData); // Send request to SET_BOOTLOADER_CONFIG - dai::bootloader::request::SetBootloaderConfig setConfigReq; + Request::SetBootloaderConfig setConfigReq; setConfigReq.memory = memory; if(type != Type::AUTO) { setConfigReq.offset = bootloader::getStructure(type).offset.at(bootloader::Section::BOOTLOADER_CONFIG); @@ -719,14 +721,14 @@ std::tuple DeviceBootloader::flashConfigData(nlohmann::json c setConfigReq.numPackets = 1; setConfigReq.totalSize = bson.size(); setConfigReq.clearConfig = 0; - if(!sendBootloaderRequest(stream->getStreamId(), setConfigReq)) return {false, "Couldn't send request to flash configuration data"}; + if(!sendRequest(setConfigReq)) return {false, "Couldn't send request to flash configuration data"}; // Send 1 packet, of bson config data stream->write(bson); // Read back response - dai::bootloader::response::FlashComplete result; - receiveBootloaderResponse(stream->getStreamId(), result); + Response::FlashComplete result; + receiveResponse(result); // Return if flashing was successful return {result.success, result.errorMsg}; @@ -752,6 +754,41 @@ std::tuple DeviceBootloader::flashConfig(const Config& config return flashConfigData(config, memory, type); } +// Boot memory +void DeviceBootloader::bootMemory(const std::vector& embeddedFw) { + // Send request to boot firmware directly from bootloader + Request::BootMemory bootMemory; + bootMemory.totalSize = static_cast(embeddedFw.size()); + bootMemory.numPackets = ((static_cast(embeddedFw.size()) - 1) / bootloader::XLINK_STREAM_MAX_SIZE) + 1; + if(!sendRequest(bootMemory)) { + throw std::runtime_error("Error trying to connect to device"); + } + + // After that send numPackets of data + stream->writeSplit(embeddedFw.data(), embeddedFw.size(), bootloader::XLINK_STREAM_MAX_SIZE); + + // Then wait for the link to fall down + try { + stream->read(); + } catch (const std::exception& ex){ + // ignore + } +} + +void DeviceBootloader::bootUsbRomBootloader() { + // Boot into USB ROM BOOTLOADER now + if(!sendRequest(Request::UsbRomBoot{})) { + throw std::runtime_error("Error trying to connect to device"); + } + + // Then wait for the link to fall down + try { + stream->read(); + } catch (const std::exception& ex){ + // ignore + } +} + bool DeviceBootloader::isEmbeddedVersion() const { return isEmbedded; } @@ -765,11 +802,7 @@ DeviceBootloader::Version::Version(const std::string& v) : versionMajor(0), vers if(std::sscanf(v.c_str(), "%u.%u.%u", &versionMajor, &versionMinor, &versionPatch) != 3) throw std::runtime_error("Cannot parse version: " + v); } -DeviceBootloader::Version::Version(unsigned vmajor, unsigned vminor, unsigned vpatch) { - this->versionMajor = vmajor; - this->versionMinor = vminor; - this->versionPatch = vpatch; -} +DeviceBootloader::Version::Version(unsigned vmajor, unsigned vminor, unsigned vpatch) : versionMajor(vmajor), versionMinor(vminor), versionPatch(vpatch) {} bool DeviceBootloader::Version::operator==(const Version& other) const { if(versionMajor == other.versionMajor && versionMinor == other.versionMinor && versionPatch == other.versionPatch) return true; @@ -795,6 +828,59 @@ std::string DeviceBootloader::Version::toString() const { return std::to_string(versionMajor) + "." + std::to_string(versionMinor) + "." + std::to_string(versionPatch); } +template +bool DeviceBootloader::sendRequest(const T& request) { + if(stream == nullptr) return false; + + // Do a version check beforehand + if(getVersion() < Version(T::VERSION)) { + throw std::runtime_error( + fmt::format("Bootloader version {} required to send request '{}'. Current version {}", T::VERSION, T::NAME, getVersion().toString())); + } + + try { + stream->write((uint8_t*)&request, sizeof(T)); + } catch(const std::exception& ex) { + return false; + } + + return true; +} + +bool DeviceBootloader::receiveResponseData(std::vector& data) { + if(stream == nullptr) return false; + + data = stream->read(); + return true; +} + +template +bool DeviceBootloader::parseResponse(const std::vector& data, T& response) { + // Checks that 'data' is type T + Response::Command command; + if(data.size() < sizeof(command)) return false; + memcpy(&command, data.data(), sizeof(command)); + if(response.cmd != command) return false; + if(data.size() < sizeof(response)) return false; + + // If yes, memcpy to response + memcpy(&response, data.data(), sizeof(response)); + return true; +} + +template +bool DeviceBootloader::receiveResponse(T& response) { + if(stream == nullptr) return false; + // Receive data first + std::vector data; + if(!receiveResponseData(data)) return false; + + // Then try to parse + if(!parseResponse(data, response)) return false; + + return true; +} + // Config functions void DeviceBootloader::Config::setStaticIPv4(std::string ip, std::string mask, std::string gateway) { network.ipv4 = platform::getIPv4AddressAsBinary(ip); diff --git a/src/utility/BootloaderHelper.hpp b/src/utility/BootloaderHelper.hpp deleted file mode 100644 index 79e6678eb..000000000 --- a/src/utility/BootloaderHelper.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -// std -#include -#include - -// libraries -#include - -// shared -#include "depthai-bootloader-shared/Bootloader.hpp" - -namespace dai -{ - -template -bool sendBootloaderRequest(streamId_t streamId, T request){ - if(XLinkWriteData(streamId, (uint8_t*) &request, sizeof(T)) != X_LINK_SUCCESS) return false; - return true; -} - -inline bool receiveBootloaderResponseData(streamId_t streamId, std::vector& data){ - data = std::vector(); - - streamPacketDesc_t* pPacket; - if(XLinkReadData(streamId, &pPacket) != X_LINK_SUCCESS) return false; - - // Resize vector - data.resize(pPacket->length); - - // copy data - memcpy(data.data(), pPacket->data, pPacket->length); - - // release data - if(XLinkReleaseData(streamId) != X_LINK_SUCCESS) return false; - - return true; -} - -template -bool parseBootloaderResponse(const std::vector& data, T& response){ - // Checks that 'data' is type T - dai::bootloader::response::Command command; - if(data.size() < sizeof(command)) return false; - memcpy(&command, data.data(), sizeof(command)); - if(response.cmd != command) return false; - if(data.size() < sizeof(response)) return false; - - // If yes, memcpy to response - memcpy(&response, data.data(), sizeof(response)); - return true; -} - -template -bool receiveBootloaderResponse(streamId_t streamId, T& response){ - // Receive data first - std::vector data; - if(!receiveBootloaderResponseData(streamId, data)) return false; - - // Then try to parse - if(!parseBootloaderResponse(data, response)) return false; - - return true; -} - -} // namespace dai - - From 5dd1ec3e8d06c2857bcf03c09650a758467a4599 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 23 Aug 2021 20:37:54 +0200 Subject: [PATCH 034/101] Updated FW for UsbSpeed handling --- 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 c9b2679cc..74430323a 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 "b5d2239124f9b66d056222676e9e845824329928") +set(DEPTHAI_DEVICE_SIDE_COMMIT "02668d20837db9d90cd66a9f47b5a4a8f359f9e7") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 8071ddb6c4b2228c48c6eca11d0c400c3b3081d4 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 23 Aug 2021 22:36:06 +0200 Subject: [PATCH 035/101] Fixed Super Speed mode and added a test --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- tests/CMakeLists.txt | 4 ++-- tests/src/device_usbspeed_test.cpp | 24 +++++++++++++++++++++ 3 files changed, 27 insertions(+), 3 deletions(-) create mode 100644 tests/src/device_usbspeed_test.cpp diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 74430323a..708a9b022 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 "02668d20837db9d90cd66a9f47b5a4a8f359f9e7") +set(DEPTHAI_DEVICE_SIDE_COMMIT "3f22c4cf67ca3ec8dcf5e01fffdf9137f49aefbf") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 6b258c53b..0dfd3c627 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -99,5 +99,5 @@ target_compile_definitions(openvino_blob PRIVATE OPENVINO_2021_4_BLOB_PATH="${openvino_2021_4_blob}" ) - - +# Device USB Speed test +dai_add_test(device_usbspeed_test src/device_usbspeed_test.cpp) diff --git a/tests/src/device_usbspeed_test.cpp b/tests/src/device_usbspeed_test.cpp new file mode 100644 index 000000000..7df9e61e2 --- /dev/null +++ b/tests/src/device_usbspeed_test.cpp @@ -0,0 +1,24 @@ +#define CATCH_CONFIG_MAIN +#include + +// std +#include +#include + +// Include depthai library +#include + +TEST_CASE("UsbSpeed::HIGH") { + dai::Pipeline p; + dai::Device d(p, dai::UsbSpeed::HIGH); +} + +TEST_CASE("UsbSpeed::SUPER") { + dai::Pipeline p; + dai::Device d(p, dai::UsbSpeed::SUPER); +} + +TEST_CASE("UsbSpeed::SUPER_PLUS") { + dai::Pipeline p; + dai::Device d(p, dai::UsbSpeed::SUPER_PLUS); +} From 2fe25a198cc4e5872f0b372a96c69da60f697dfe Mon Sep 17 00:00:00 2001 From: Martin Peterlin Date: Mon, 23 Aug 2021 23:38:10 +0200 Subject: [PATCH 036/101] Fixed Windows Platform specific code --- src/utility/Platform.cpp | 9 +++++-- tests/src/bootloader_config_test.cpp | 38 +++++++++++++++++++--------- 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/src/utility/Platform.cpp b/src/utility/Platform.cpp index be60c10da..2749c6054 100644 --- a/src/utility/Platform.cpp +++ b/src/utility/Platform.cpp @@ -18,12 +18,17 @@ namespace platform { uint32_t getIPv4AddressAsBinary(std::string address) { uint32_t binary = 0; + if(address == "") { + // inet_addr returns 0xFFFFFFFF if addr is "" + return 0; + } #if defined(_WIN32) || defined(__USE_W32_SOCKETS) - #ifdef _WIN32_WINNT 0x0501 + #if (_WIN32_WINNT <= 0x0501) binary = inet_addr(address.c_str()); // for XP - #endif + #else inet_pton(AF_INET, address.c_str(), &binary); // for Vista or higher + #endif #else inet_pton(AF_INET, address.c_str(), &binary); #endif diff --git a/tests/src/bootloader_config_test.cpp b/tests/src/bootloader_config_test.cpp index 278665eb8..0dd9fea84 100644 --- a/tests/src/bootloader_config_test.cpp +++ b/tests/src/bootloader_config_test.cpp @@ -1,39 +1,53 @@ -#include +#define CATCH_CONFIG_MAIN +#include -#include "depthai/depthai.hpp" +// std +#include + +// Include depthai library +#include + +TEST_CASE("Bootloader Config") { -int main() { dai::DeviceBootloader::Config config; + // By default IPv4 is 0.0.0.0 (invalid) + REQUIRE("0.0.0.0" == config.getIPv4()); + std::string ipv4 = "192.168.1.150"; std::string ipv4Mask = "255.255.255.0"; std::string ipv4Gateway = "192.168.1.1"; config.setStaticIPv4(ipv4, ipv4Mask, ipv4Gateway); - assert(ipv4 == config.getIPv4()); - assert(ipv4Mask == config.getIPv4Mask()); - assert(ipv4Gateway == config.getIPv4Gateway()); + std::array ipv4InMemory = {192, 168, 1, 150}; + for(int i = 0; i < ipv4InMemory.size(); i++){ + REQUIRE(ipv4InMemory[i] == reinterpret_cast(&config.network.ipv4)[i]); + } + + REQUIRE(ipv4 == config.getIPv4()); + REQUIRE(ipv4Mask == config.getIPv4Mask()); + REQUIRE(ipv4Gateway == config.getIPv4Gateway()); + REQUIRE(config.isStaticIPV4() == true); std::string dns = "1.1.1.1"; std::string dnsAlt = "8.8.8.8"; config.setDnsIPv4(dns); - assert(config.getDnsIPv4() == dns); - assert(config.network.ipv4DnsAlt == 0); + REQUIRE(config.getDnsIPv4() == dns); + REQUIRE(config.network.ipv4DnsAlt == 0); config.setDnsIPv4(dns, dnsAlt); - assert(config.getDnsIPv4() == dns); - assert(config.getDnsAltIPv4() == dnsAlt); + REQUIRE(config.getDnsIPv4() == dns); + REQUIRE(config.getDnsAltIPv4() == dnsAlt); // MAC address std::string mac = "FF:AA:BB:CC:00:11"; config.setMacAddress(mac); // std::cout << "Orig mac address: " << mac << " len: " << mac.length() << std::endl; // std::cout << "Get mac address: " << config.getMacAddress() << " len: " << config.getMacAddress().length() << std::endl; - assert(config.getMacAddress() == mac); + REQUIRE(config.getMacAddress() == mac); - return 0; } \ No newline at end of file From fc5c1d0717ae275ad91cf2c76311b33d12876423 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Tue, 24 Aug 2021 12:49:47 +0200 Subject: [PATCH 037/101] Fixed incorrect exception message --- src/xlink/XLinkStream.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/xlink/XLinkStream.cpp b/src/xlink/XLinkStream.cpp index 6f039f6af..e7467e550 100644 --- a/src/xlink/XLinkStream.cpp +++ b/src/xlink/XLinkStream.cpp @@ -176,9 +176,9 @@ streamId_t XLinkStream::getStreamId() const { } XLinkReadError::XLinkReadError(XLinkError_t status, const std::string& streamName) - : XLinkError(status, streamName, fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))) {} + : XLinkError(status, streamName, fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))) {} XLinkWriteError::XLinkWriteError(XLinkError_t status, const std::string& streamName) - : XLinkError(status, streamName, fmt::format("Couldn't read data from stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))) {} + : XLinkError(status, streamName, fmt::format("Couldn't write data to stream: '{}' ({})", streamName, XLinkConnection::convertErrorCodeToString(status))) {} } // namespace dai From 24499d0c5e69ec3941a4fb7ea08116b058bc5508 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Wed, 25 Aug 2021 01:50:49 +0200 Subject: [PATCH 038/101] Added flash booted state and handling --- cmake/Hunter/config.cmake | 6 ++-- include/depthai/xlink/XLinkConnection.hpp | 2 +- include/depthai/xlink/XLinkStream.hpp | 1 - src/device/DeviceBase.cpp | 14 +++++++-- src/device/DeviceBootloader.cpp | 13 ++++++-- src/xlink/XLinkConnection.cpp | 38 ++++++++++++++++++++++- src/xlink/XLinkStream.cpp | 1 - 7 files changed, 64 insertions(+), 11 deletions(-) diff --git a/cmake/Hunter/config.cmake b/cmake/Hunter/config.cmake index af32b675d..fde95f5ce 100644 --- a/cmake/Hunter/config.cmake +++ b/cmake/Hunter/config.cmake @@ -7,9 +7,9 @@ hunter_config( hunter_config( XLink - VERSION "luxonis-2021.3-master" - URL "https://github.com/luxonis/XLink/archive/2c6cdb857f3d21088b34ec172a0ea8df16528d00.tar.gz" - SHA1 "736da6528515d9c969008e2334f1387428f91a3b" + VERSION "luxonis-2021.3-develop" + URL "https://github.com/luxonis/XLink/archive/4c149080d22c35a17ce285f5bca99f2b2fe05e46.tar.gz" + SHA1 "64b0a8bfeb1a91f909df88ea8b1d0b17885b92ff" ) hunter_config( diff --git a/include/depthai/xlink/XLinkConnection.hpp b/include/depthai/xlink/XLinkConnection.hpp index 4b2d16696..ff6512d58 100644 --- a/include/depthai/xlink/XLinkConnection.hpp +++ b/include/depthai/xlink/XLinkConnection.hpp @@ -36,13 +36,13 @@ class XLinkConnection { static std::atomic xlinkGlobalInitialized; static XLinkGlobalHandler_t xlinkGlobalHandler; static void initXLinkGlobal(); - static std::mutex xlinkStreamOperationMutex; public: // static API static std::vector getAllConnectedDevices(XLinkDeviceState_t state = X_LINK_ANY_STATE); static std::tuple getFirstDevice(XLinkDeviceState_t state = X_LINK_ANY_STATE); static std::tuple getDeviceByMxId(std::string, XLinkDeviceState_t state = X_LINK_ANY_STATE); + static DeviceInfo bootBootloader(const DeviceInfo& devInfo); XLinkConnection(const DeviceInfo& deviceDesc, std::vector mvcmdBinary, XLinkDeviceState_t expectedState = X_LINK_BOOTED); XLinkConnection(const DeviceInfo& deviceDesc, std::string pathToMvcmd, XLinkDeviceState_t expectedState = X_LINK_BOOTED); diff --git a/include/depthai/xlink/XLinkStream.hpp b/include/depthai/xlink/XLinkStream.hpp index fdd8d0751..e26e20f89 100644 --- a/include/depthai/xlink/XLinkStream.hpp +++ b/include/depthai/xlink/XLinkStream.hpp @@ -24,7 +24,6 @@ class XLinkStream { // static constexpr static int STREAM_OPEN_RETRIES = 5; constexpr static std::chrono::milliseconds WAIT_FOR_STREAM_RETRY{50}; - static std::mutex xlinkStreamOperationMutex; std::string streamName; streamId_t streamId{INVALID_STREAM_ID}; diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index e7e92b869..82f3b66c1 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -23,6 +23,7 @@ #include "utility/Resources.hpp" // libraries +#include "XLink/XLink.h" #include "nanorpc/core/client.h" #include "nanorpc/packer/nlohmann_msgpack.h" #include "spdlog/details/os.h" @@ -98,7 +99,7 @@ std::tuple DeviceBase::getAnyAvailableDevice(std::chrono::dura bool found = false; DeviceInfo deviceInfo; do { - for(auto searchState : {X_LINK_UNBOOTED, X_LINK_BOOTLOADER}) { + for(auto searchState : {X_LINK_UNBOOTED, X_LINK_BOOTLOADER, X_LINK_FLASH_BOOTED}) { std::tie(found, deviceInfo) = XLinkConnection::getFirstDevice(searchState); if(found) break; } @@ -135,6 +136,9 @@ std::tuple DeviceBase::getFirstAvailableDevice() { if(!found) { std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_BOOTLOADER); } + if(!found) { + std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_FLASH_BOOTED); + } return {found, dev}; } @@ -476,7 +480,13 @@ void DeviceBase::init2(Config cfg, const std::string& pathToMvcmd, tl::optional< if(deviceInfo.state == X_LINK_UNBOOTED) { // Unbooted device found, boot and connect with XLinkConnection constructor connection = std::make_shared(deviceInfo, fwWithConfig); - } else if(deviceInfo.state == X_LINK_BOOTLOADER) { + } else if(deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + // If device is in flash booted state, reset to bootloader and then continue by booting appropriate FW + if(deviceInfo.state == X_LINK_FLASH_BOOTED) { + // Boot bootloader and set current deviceInfo to new device state + deviceInfo = XLinkConnection::bootBootloader(deviceInfo); + } + // Scope so DeviceBootloader is disconnected { DeviceBootloader bl(deviceInfo); diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 7476837ec..02fa3784e 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -48,6 +48,9 @@ std::tuple DeviceBootloader::getFirstAvailableDevice() { if(!found) { std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_BOOTLOADER); } + if(!found) { + std::tie(found, dev) = XLinkConnection::getFirstDevice(X_LINK_FLASH_BOOTED); + } return {found, dev}; } @@ -240,7 +243,13 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, // Device wasn't already in bootloader, that means that embedded bootloader is booted isEmbedded = true; - } else if(deviceInfo.state == X_LINK_BOOTLOADER) { + } else if(deviceInfo.state == X_LINK_BOOTLOADER || deviceInfo.state == X_LINK_FLASH_BOOTED) { + // If device is in flash booted state, reset to bootloader and then continue by booting appropriate FW + if(deviceInfo.state == X_LINK_FLASH_BOOTED) { + // Boot bootloader and set current deviceInfo to new device state + deviceInfo = XLinkConnection::bootBootloader(deviceInfo); + } + // In this case boot specified bootloader only if current bootloader isn't of correct type // Check version first, if >= 0.0.12 then check type and then either bootmemory to correct BL or continue as is @@ -370,7 +379,7 @@ void DeviceBootloader::init(bool embeddedMvcmd, const std::string& pathToMvcmd, } } else { - throw std::runtime_error("Device not in UNBOOTED or BOOTLOADER state"); + throw std::runtime_error("Device not in UNBOOTED, BOOTLOADER or FLASH_BOOTED state"); } deviceInfo.state = X_LINK_BOOTLOADER; diff --git a/src/xlink/XLinkConnection.cpp b/src/xlink/XLinkConnection.cpp index 2fb435e78..eea0aa693 100644 --- a/src/xlink/XLinkConnection.cpp +++ b/src/xlink/XLinkConnection.cpp @@ -76,7 +76,7 @@ std::vector XLinkConnection::getAllConnectedDevices(XLinkDeviceState std::vector states; if(state == X_LINK_ANY_STATE) { - states = {X_LINK_UNBOOTED, X_LINK_BOOTLOADER, X_LINK_BOOTED}; + states = {X_LINK_UNBOOTED, X_LINK_BOOTLOADER, X_LINK_BOOTED, X_LINK_FLASH_BOOTED}; } else { states = {state}; } @@ -123,6 +123,37 @@ std::tuple XLinkConnection::getDeviceByMxId(std::string mxId, return {false, DeviceInfo()}; } +DeviceInfo XLinkConnection::bootBootloader(const DeviceInfo& deviceInfo) { + using namespace std::chrono; + + // Device is in flash booted state. Boot to bootloader first + XLinkBootBootloader(&deviceInfo.desc); + + // Fix deviceInfo for BOOTLOADER state + DeviceInfo deviceToWait = deviceInfoFix(deviceInfo, X_LINK_BOOTLOADER); + + // Device desc if found + deviceDesc_t foundDeviceDesc = {}; + + // Wait for device to get to bootloader state + XLinkError_t rc; + auto tstart = steady_clock::now(); + do { + rc = XLinkFindFirstSuitableDevice(X_LINK_BOOTLOADER, deviceToWait.desc, &foundDeviceDesc); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if(rc == X_LINK_SUCCESS) break; + } while(steady_clock::now() - tstart < WAIT_FOR_BOOTUP_TIMEOUT); + + // If device not found + if(rc != X_LINK_SUCCESS) { + throw std::runtime_error("Failed to find device (" + std::string(deviceToWait.desc.name) + "), error message: " + convertErrorCodeToString(rc)); + } + + deviceToWait.state = X_LINK_BOOTLOADER; + deviceToWait.desc = foundDeviceDesc; + return deviceToWait; +} + XLinkConnection::XLinkConnection(const DeviceInfo& deviceDesc, std::vector mvcmdBinary, XLinkDeviceState_t expectedState) { bootDevice = true; bootWithPath = false; @@ -319,6 +350,11 @@ std::string XLinkConnection::convertErrorCodeToString(XLinkError_t errorCode) { } DeviceInfo deviceInfoFix(const DeviceInfo& dev, XLinkDeviceState_t state) { + if(dev.desc.protocol == X_LINK_TCP_IP) { + // X_LINK_TCP_IP doesn't need a fix + return dev; + } + DeviceInfo fixed(dev); // Remove everything after dash diff --git a/src/xlink/XLinkStream.cpp b/src/xlink/XLinkStream.cpp index 6c44805a4..70161f258 100644 --- a/src/xlink/XLinkStream.cpp +++ b/src/xlink/XLinkStream.cpp @@ -12,7 +12,6 @@ namespace dai { // static constexpr std::chrono::milliseconds XLinkStream::WAIT_FOR_STREAM_RETRY; constexpr int XLinkStream::STREAM_OPEN_RETRIES; -std::mutex XLinkStream::xlinkStreamOperationMutex; XLinkStream::XLinkStream(const XLinkConnection& conn, const std::string& name, std::size_t maxWriteSize) : streamName(name) { if(name.empty()) throw std::invalid_argument("Cannot create XLinkStream using empty stream name"); From 5a361217d81bad0af2ac170078f23c64890164af Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 26 Aug 2021 00:31:22 +0200 Subject: [PATCH 039/101] Updated FW with GPIO and SPI improvements --- 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 2ed0023a4..766beda4e 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 "2d05346034bd91fccc22974d70aa05651513624b") +set(DEPTHAI_DEVICE_SIDE_COMMIT "231cdecf84a2f1977a0d00bcee2ce6fa2ae8ce85") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 03bde90ec81b9770ad6a66004633b6ebac4b804a Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 26 Aug 2021 02:30:15 +0200 Subject: [PATCH 040/101] Added Backward library to print stacktraces on crash --- CMakeLists.txt | 5 +++++ cmake/Hunter/config.cmake | 10 ++++++++++ cmake/depthaiDependencies.cmake | 4 ++++ src/utility/Initialization.cpp | 8 ++++++++ 4 files changed, 27 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 980ad5055..40d28946c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,10 @@ HunterGate( LOCAL # Local config for dependencies ) +# TODO(themarpe) - might be more applicable to set as a target property +# Additional function information for 'backward' stacktrace +set(CMAKE_ENABLE_EXPORTS ON) + # Move binary dir if windows, to shorten the path if(WIN32) set(HUNTER_BINARY_DIR "${HUNTER_GATE_ROOT}/_bin" CACHE STRING "Hunter binary directory") @@ -368,6 +372,7 @@ target_link_libraries(${TARGET_CORE_NAME} archive_static spdlog::spdlog ZLIB::zlib + Backward::Backward ) # Add compile definitions diff --git a/cmake/Hunter/config.cmake b/cmake/Hunter/config.cmake index fde95f5ce..a2cf20dd2 100644 --- a/cmake/Hunter/config.cmake +++ b/cmake/Hunter/config.cmake @@ -74,4 +74,14 @@ hunter_config( CMAKE_ARGS FP16_BUILD_BENCHMARKS=OFF FP16_BUILD_TESTS=OFF +) + +# Backward - Stacktrace printer +hunter_config( + Backward + VERSION "1.6" + URL "https://github.com/bombela/backward-cpp/archive/refs/tags/v1.6.tar.gz" + SHA1 "4ecb711eabfd15bc88ff9dd9342907fc5da46b62" + CMAKE_ARGS + BACKWARD_TESTS=OFF ) \ No newline at end of file diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index d74abb410..4e303cbba 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -13,6 +13,7 @@ else() hunter_add_package(libarchive-luxonis) hunter_add_package(spdlog) hunter_add_package(ZLIB) + hunter_add_package(Backward) endif() # If library was build as static, find all dependencies @@ -33,6 +34,9 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT depthai_SHARED_LIBS)) # spdlog for library and device logging find_package(spdlog ${_QUIET} CONFIG REQUIRED) + # Backward + find_package(Backward ${_QUIET} CONFIG REQUIRED) + endif() # Add threads (c++) diff --git a/src/utility/Initialization.cpp b/src/utility/Initialization.cpp index 7060f2d82..6406262b1 100644 --- a/src/utility/Initialization.cpp +++ b/src/utility/Initialization.cpp @@ -9,6 +9,14 @@ #include "spdlog/cfg/helpers.h" #include "spdlog/details/os.h" #include "spdlog/spdlog.h" +#include "backward.hpp" + +// Backward library stacktrace handling +namespace backward { + +backward::SignalHandling sh; + +} // namespace backward // For easier access to dai namespaced symbols namespace dai { From e9d9eeade9133230c7e073e2edb7dbde51dc19f8 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 26 Aug 2021 17:14:20 +0200 Subject: [PATCH 041/101] Fixed style --- src/utility/Initialization.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/utility/Initialization.cpp b/src/utility/Initialization.cpp index 6406262b1..f511835cb 100644 --- a/src/utility/Initialization.cpp +++ b/src/utility/Initialization.cpp @@ -5,18 +5,18 @@ #include "utility/Resources.hpp" // libraries +#include "backward.hpp" #include "spdlog/cfg/env.h" #include "spdlog/cfg/helpers.h" #include "spdlog/details/os.h" #include "spdlog/spdlog.h" -#include "backward.hpp" // Backward library stacktrace handling namespace backward { backward::SignalHandling sh; -} // namespace backward +} // namespace backward // For easier access to dai namespaced symbols namespace dai { From 1cd00e6c96ff15fec183fff8141e1ab67111f958 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Tue, 31 Aug 2021 00:16:17 +0300 Subject: [PATCH 042/101] Added some function overloads --- .../pipeline/datatype/ImageManipConfig.hpp | 23 +++++++++++++++++++ .../depthai/pipeline/datatype/ImgFrame.hpp | 19 +++++++++++++-- include/depthai/pipeline/node/StereoDepth.hpp | 7 ++++++ src/pipeline/datatype/ImageManipConfig.cpp | 12 ++++++++++ src/pipeline/datatype/ImgFrame.cpp | 7 ++++++ src/pipeline/node/StereoDepth.cpp | 3 +++ 6 files changed, 69 insertions(+), 2 deletions(-) diff --git a/include/depthai/pipeline/datatype/ImageManipConfig.hpp b/include/depthai/pipeline/datatype/ImageManipConfig.hpp index 5f51527fe..67948d225 100644 --- a/include/depthai/pipeline/datatype/ImageManipConfig.hpp +++ b/include/depthai/pipeline/datatype/ImageManipConfig.hpp @@ -40,6 +40,12 @@ class ImageManipConfig : public Buffer { */ void setCropRect(float xmin, float ymin, float xmax, float ymax); + /** + * Specifies crop with rectangle with normalized values (0..1) + * @param coordinates Coordinate of rectangle + */ + void setCropRect(std::tuple coordinates); + /** * Specifies crop with rotated rectangle. Optionally as non normalized coordinates * @param rr Rotated rectangle which specifies crop @@ -104,6 +110,12 @@ class ImageManipConfig : public Buffer { */ void setResize(int w, int h); + /** + * Specifies output image size. After crop stage the image will be streched to fit. + * @param size Size in pixels + */ + void setResize(std::tuple size); + /** * Specifies output image size. After crop stage the image will be resized by preserving aspect ration. * Optionally background can be specified. @@ -116,6 +128,17 @@ class ImageManipConfig : public Buffer { */ void setResizeThumbnail(int w, int h, int bgRed = 0, int bgGreen = 0, int bgBlue = 0); + /** + * Specifies output image size. After crop stage the image will be resized by preserving aspect ration. + * Optionally background can be specified. + * + * @param size Size in pixels + * @param bgRed Red component + * @param bgGreen Green component + * @param bgBlue Blue component + */ + void setResizeThumbnail(std::tuple size, int bgRed = 0, int bgGreen = 0, int bgBlue = 0); + /** * Specify output frame type. * @param name Frame type diff --git a/include/depthai/pipeline/datatype/ImgFrame.hpp b/include/depthai/pipeline/datatype/ImgFrame.hpp index c75f181bd..b8dc2300d 100644 --- a/include/depthai/pipeline/datatype/ImgFrame.hpp +++ b/include/depthai/pipeline/datatype/ImgFrame.hpp @@ -115,9 +115,24 @@ class ImgFrame : public Buffer { /** * Specifies frame height * - * @param width frame height + * @param height frame height */ - void setHeight(unsigned int); + void setHeight(unsigned int height); + + /** + * Specifies frame size + * + * @param height frame height + * @param width frame width + */ + void setSize(unsigned int width, unsigned int height); + + /** + * Specifies frame size + * + * @param size frame size + */ + void setSize(std::tuple size); /** * Specifies frame type, RGB, BGR, ... diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 58e48d603..0b5d0a13a 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -139,6 +139,13 @@ class StereoDepth : public Node { */ void setInputResolution(int width, int height); + /** + * Specify input resolution size + * + * Optional if MonoCamera exists, otherwise necessary + */ + void setInputResolution(std::tuple resolution); + /** * Specify disparity/depth output resolution size, implemented by scaling. * diff --git a/src/pipeline/datatype/ImageManipConfig.cpp b/src/pipeline/datatype/ImageManipConfig.cpp index f3faa8a0d..6d1578705 100644 --- a/src/pipeline/datatype/ImageManipConfig.cpp +++ b/src/pipeline/datatype/ImageManipConfig.cpp @@ -29,6 +29,10 @@ void ImageManipConfig::setCropRect(float xmin, float ymin, float xmax, float yma cfg.cropConfig.cropRect.ymax = std::min(ymax, 1.0f); } +void ImageManipConfig::setCropRect(std::tuple coordinates) { + setCropRect(std::get<0>(coordinates), std::get<1>(coordinates), std::get<2>(coordinates), std::get<3>(coordinates)); +} + void ImageManipConfig::setCropRotatedRect(RotatedRect rr, bool normalizedCoords) { // Enable crop stage and extended flags cfg.enableCrop = true; @@ -108,6 +112,10 @@ void ImageManipConfig::setResize(int w, int h) { cfg.resizeConfig.height = h; } +void ImageManipConfig::setResize(std::tuple size) { + setResize(std::get<0>(size), std::get<1>(size)); +} + void ImageManipConfig::setResizeThumbnail(int w, int h, int bgRed, int bgGreen, int bgBlue) { // Enable resize stage cfg.enableResize = true; @@ -125,6 +133,10 @@ void ImageManipConfig::setResizeThumbnail(int w, int h, int bgRed, int bgGreen, cfg.resizeConfig.bgBlue = bgBlue; } +void ImageManipConfig::setResizeThumbnail(std::tuple size, int bgRed, int bgGreen, int bgBlue) { + setResizeThumbnail(std::get<0>(size), std::get<1>(size), bgRed, bgGreen, bgBlue); +} + void ImageManipConfig::setFrameType(dai::RawImgFrame::Type type) { // Enable format stage cfg.enableFormat = true; diff --git a/src/pipeline/datatype/ImgFrame.cpp b/src/pipeline/datatype/ImgFrame.cpp index 25d017139..28f0689da 100644 --- a/src/pipeline/datatype/ImgFrame.cpp +++ b/src/pipeline/datatype/ImgFrame.cpp @@ -68,6 +68,13 @@ void ImgFrame::setWidth(unsigned int width) { void ImgFrame::setHeight(unsigned int height) { img.fb.height = height; } +void ImgFrame::setSize(unsigned int width, unsigned int height) { + setWidth(width); + setHeight(height); +} +void ImgFrame::setSize(std::tuple size) { + setSize(std::get<0>(size), std::get<1>(size)) +} void ImgFrame::setType(RawImgFrame::Type type) { img.fb.type = type; } diff --git a/src/pipeline/node/StereoDepth.cpp b/src/pipeline/node/StereoDepth.cpp index 53b133fe8..820c93e6b 100644 --- a/src/pipeline/node/StereoDepth.cpp +++ b/src/pipeline/node/StereoDepth.cpp @@ -90,6 +90,9 @@ void StereoDepth::setInputResolution(int width, int height) { properties.width = width; properties.height = height; } +void StereoDepth::setInputResolution(std::tuple resolution) { + setInputResolution(std::get<0>(resolution), std::get<1>(resolution)); +} void StereoDepth::setOutputSize(int width, int height) { properties.outWidth = width; properties.outHeight = height; From 852d1d968ee1e962a6f9f5ad26b88ecb82c75c94 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Tue, 31 Aug 2021 01:45:21 +0300 Subject: [PATCH 043/101] fix compiling error --- src/pipeline/datatype/ImgFrame.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pipeline/datatype/ImgFrame.cpp b/src/pipeline/datatype/ImgFrame.cpp index 28f0689da..59c26462b 100644 --- a/src/pipeline/datatype/ImgFrame.cpp +++ b/src/pipeline/datatype/ImgFrame.cpp @@ -73,7 +73,7 @@ void ImgFrame::setSize(unsigned int width, unsigned int height) { setHeight(height); } void ImgFrame::setSize(std::tuple size) { - setSize(std::get<0>(size), std::get<1>(size)) + setSize(std::get<0>(size), std::get<1>(size)); } void ImgFrame::setType(RawImgFrame::Type type) { img.fb.type = type; From 8d8874691e193fc43b09d58c251049b6e611f249 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 2 Sep 2021 02:00:21 +0300 Subject: [PATCH 044/101] Added helper functions to get translation vector and baseline distance --- examples/src/calibration_reader.cpp | 4 ++- include/depthai/device/CalibrationHandler.hpp | 25 +++++++++++++++++-- src/device/CalibrationHandler.cpp | 19 ++++++++++++++ 3 files changed, 45 insertions(+), 3 deletions(-) diff --git a/examples/src/calibration_reader.cpp b/examples/src/calibration_reader.cpp index 907d3cd97..169307943 100644 --- a/examples/src/calibration_reader.cpp +++ b/examples/src/calibration_reader.cpp @@ -29,6 +29,8 @@ int main(int argc, char** argv) { cout << "Width -> " << width << endl; cout << "Height -> " << height << endl; + cout << "Stereo baseline distance -> " << calibData.getBaselineDistance() << " cm" << endl; + cout << "Intrinsics from getCameraIntrinsics function full resolution ->" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT); @@ -37,7 +39,7 @@ int main(int argc, char** argv) { cout << endl; } - cout << "Intrinsics from getCameraIntrinsics function 1280 x 720 ->" << endl; + cout << "Intrinsics from getCameraIntrinsicscalibData function 1280 x 720 ->" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT, 1280, 720); for(auto row : intrinsics) { diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index 4000aa896..29c8cca10 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -158,8 +158,8 @@ class CalibrationHandler { uint8_t getLensPosition(CameraBoardSocket cameraId); /** - * Get the Camera Extrinsics object between two cameras from the data loaded if there is a linked connection - * between any two cameras then there relative rotation and translation (in centimeters) is returned by this function. + * Get the Camera Extrinsics object between two cameras from the calibration data if there is a linked connection + * between any two cameras then the relative rotation and translation (in centimeters) is returned by this function. * * @param srcCamera Camera Id of the camera which will be considerd as origin. * @param dstCamera Camera Id of the destination camera to which we are fetching the rotation and translation from the SrcCamera @@ -177,6 +177,27 @@ class CalibrationHandler { */ std::vector> getCameraExtrinsics(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation = false); + /** + * Get the Camera translation vector between two cameras from the calibration data. + * + * @param srcCamera Camera Id of the camera which will be considerd as origin. + * @param dstCamera Camera Id of the destination camera to which we are fetching the translation vector from the SrcCamera + * @param useSpecTranslation Disabling this bool uses the translation information from the calibration data (not the board design data) + * @return a translation vector like [x, y, z] in centimeters + */ + std::vector getCameraTranslationVector(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation = true); + + /** + * Get the baseline distance between two specified cameras. By default it will get the baseline between CameraBoardSocket.RIGHT + * and CameraBoardSocket.LEFT. + * + * @param cam1 First camera + * @param cam2 Second camera + * @param useSpecTranslation Enabling this bool uses the translation information from the board design data (not the calibration data) + * @return baseline distance in centimeters + */ + float getBaselineDistance(CameraBoardSocket cam1 = CameraBoardSocket::RIGHT, CameraBoardSocket cam2 = CameraBoardSocket::LEFT, bool useSpecTranslation = true); + /** * Get the Camera To Imu Extrinsics object * From the data loaded if there is a linked connection between IMU and the given camera then there relative rotation and translation from the camera to IMU diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 1e4891b31..c50e845fe 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -313,6 +313,25 @@ std::vector> CalibrationHandler::getCameraExtrinsics(CameraBo return extrinsics; } +std::vector CalibrationHandler::getCameraTranslationVector(CameraBoardSocket srcCamera, CameraBoardSocket dstCamera, bool useSpecTranslation) { + std::vector> extrinsics = getCameraExtrinsics(srcCamera, dstCamera, useSpecTranslation); + + std::vector translationVector = {0, 0, 0}; + for(auto i = 0; i < 3; i++) { + translationVector[i] = extrinsics[i][3]; + } + return translationVector; +} + +float CalibrationHandler::getBaselineDistance(CameraBoardSocket cam1, CameraBoardSocket cam2, bool useSpecTranslation) { + std::vector translationVector = getCameraTranslationVector(cam1, cam2, useSpecTranslation); + float sum = 0; + for(auto val : translationVector) { + sum += val * val; + } + return std::sqrt(sum); +} + std::vector> CalibrationHandler::getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation) { std::vector> transformationMatrix = getImuToCameraExtrinsics(cameraId, useSpecTranslation); float temp = transformationMatrix[0][1]; From 8c13909dd1f9798c2ea0ad467bd9bb0cbd7ec34b Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 2 Sep 2021 12:42:40 +0200 Subject: [PATCH 045/101] Updated FW to allow for graceful resets --- 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 766beda4e..82d7746d9 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 "231cdecf84a2f1977a0d00bcee2ce6fa2ae8ce85") +set(DEPTHAI_DEVICE_SIDE_COMMIT "6a2abd52e513272993a6f8140702407e5c6ea251") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From f0c226c89777981803d73d85d89a0c011060cfb2 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 2 Sep 2021 13:46:26 +0300 Subject: [PATCH 046/101] Style check fix --- include/depthai/device/CalibrationHandler.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index 29c8cca10..089b4700e 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -196,7 +196,9 @@ class CalibrationHandler { * @param useSpecTranslation Enabling this bool uses the translation information from the board design data (not the calibration data) * @return baseline distance in centimeters */ - float getBaselineDistance(CameraBoardSocket cam1 = CameraBoardSocket::RIGHT, CameraBoardSocket cam2 = CameraBoardSocket::LEFT, bool useSpecTranslation = true); + float getBaselineDistance(CameraBoardSocket cam1 = CameraBoardSocket::RIGHT, + CameraBoardSocket cam2 = CameraBoardSocket::LEFT, + bool useSpecTranslation = true); /** * Get the Camera To Imu Extrinsics object From b11dc037f624159b742806c7f2a7d236a2d44c91 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 2 Sep 2021 13:52:36 +0300 Subject: [PATCH 047/101] Fixed typo --- examples/src/calibration_reader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/src/calibration_reader.cpp b/examples/src/calibration_reader.cpp index 169307943..4ed924045 100644 --- a/examples/src/calibration_reader.cpp +++ b/examples/src/calibration_reader.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) { cout << endl; } - cout << "Intrinsics from getCameraIntrinsicscalibData function 1280 x 720 ->" << endl; + cout << "Intrinsics from getCameraIntrinsics function 1280 x 720 ->" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT, 1280, 720); for(auto row : intrinsics) { From 54efe3de007a3bc9045ca9b642c8df8a5cbb1323 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 2 Sep 2021 17:41:55 +0300 Subject: [PATCH 048/101] Added option to calculate FOV based on camera intrinsics. Added this function to calibration_reader and also refactored it so matricies are more readable --- examples/src/calibration_reader.cpp | 97 +++++++------------ include/depthai/device/CalibrationHandler.hpp | 3 +- src/device/CalibrationHandler.cpp | 17 +++- 3 files changed, 50 insertions(+), 67 deletions(-) diff --git a/examples/src/calibration_reader.cpp b/examples/src/calibration_reader.cpp index 4ed924045..1ecaeb86a 100644 --- a/examples/src/calibration_reader.cpp +++ b/examples/src/calibration_reader.cpp @@ -7,6 +7,17 @@ #include "depthai-shared/common/EepromData.hpp" #include "depthai/depthai.hpp" +void printMatrix(std::vector> matrix) { + using namespace std; + std::string out = "["; + for(auto row : matrix) { + out += "["; + for(auto val : row) out += to_string(val) + ", "; + out = out.substr(0, out.size()-2) + "]\n"; + } + out = out.substr(0, out.size()-1) + "]\n\n"; + cout << out; +} int main(int argc, char** argv) { using namespace std; @@ -18,92 +29,54 @@ int main(int argc, char** argv) { std::vector> intrinsics; int width, height; - cout << "Intrinsics from defaultIntrinsics function" << endl; + cout << "Intrinsics from defaultIntrinsics function:" << endl; std::tie(intrinsics, width, height) = calibData.getDefaultIntrinsics(dai::CameraBoardSocket::RIGHT); + printMatrix(intrinsics); - for(auto row : intrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } + cout << "Width: " << width << endl; + cout << "Height: " << height << endl; - cout << "Width -> " << width << endl; - cout << "Height -> " << height << endl; + cout << "Stereo baseline distance: " << calibData.getBaselineDistance() << " cm" << endl; - cout << "Stereo baseline distance -> " << calibData.getBaselineDistance() << " cm" << endl; + cout << "Mono FOV from camera specs: " << calibData.getFov(dai::CameraBoardSocket::LEFT) << ", calculated FOV: " << calibData.getFov(dai::CameraBoardSocket::LEFT, false) << endl; - cout << "Intrinsics from getCameraIntrinsics function full resolution ->" << endl; + cout << "Intrinsics from getCameraIntrinsics function full resolution:" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT); + printMatrix(intrinsics); - for(auto row : intrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Intrinsics from getCameraIntrinsics function 1280 x 720 ->" << endl; + cout << "Intrinsics from getCameraIntrinsics function 1280 x 720:" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT, 1280, 720); + printMatrix(intrinsics); - for(auto row : intrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Intrinsics from getCameraIntrinsics function 720 x 450 ->" << endl; + cout << "Intrinsics from getCameraIntrinsics function 720 x 450:" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT, 720); + printMatrix(intrinsics); - for(auto row : intrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Intrinsics from getCameraIntrinsics function 600 x 1280 ->" << endl; + cout << "Intrinsics from getCameraIntrinsics function 600 x 1280:" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT, 600, 1280); - - for(auto row : intrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } + printMatrix(intrinsics); std::vector> extrinsics; - cout << "Extrinsics from left->right test ->" << endl; + cout << "Extrinsics from left->right test:" << endl; extrinsics = calibData.getCameraExtrinsics(dai::CameraBoardSocket::LEFT, dai::CameraBoardSocket::RIGHT); + printMatrix(extrinsics); - for(auto row : extrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Extrinsics from right->left test ->" << endl; + cout << "Extrinsics from right->left test:" << endl; extrinsics = calibData.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT); + printMatrix(extrinsics); - for(auto row : extrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Extrinsics from right->rgb test ->" << endl; + cout << "Extrinsics from right->rgb test:" << endl; extrinsics = calibData.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::RGB); + printMatrix(extrinsics); - for(auto row : extrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Extrinsics from rgb->right test ->" << endl; + cout << "Extrinsics from rgb->right test:" << endl; extrinsics = calibData.getCameraExtrinsics(dai::CameraBoardSocket::RGB, dai::CameraBoardSocket::RIGHT); + printMatrix(extrinsics); - for(auto row : extrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } - - cout << "Extrinsics from left->rgb test ->" << endl; + cout << "Extrinsics from left->rgb test:" << endl; extrinsics = calibData.getCameraExtrinsics(dai::CameraBoardSocket::LEFT, dai::CameraBoardSocket::RGB); - - for(auto row : extrinsics) { - for(auto val : row) cout << val << " "; - cout << endl; - } + printMatrix(extrinsics); return 0; } diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index 089b4700e..6b5f79f0c 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -145,9 +145,10 @@ class CalibrationHandler { * Get the Fov of the camera * * @param cameraId of the camera of which we are fetching fov. + * @param useSpec Disabling this bool will calculate the fov based on intrinsics (focal length, image width), instead of getting it from the camera specs * @return field of view of the camera with given cameraId. */ - float getFov(CameraBoardSocket cameraId); + float getFov(CameraBoardSocket cameraId, bool useSpec = true); /** * Get the lens position of the given camera diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index c50e845fe..235192cde 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -13,6 +13,7 @@ #include "nlohmann/json.hpp" #include "spdlog/spdlog.h" #include "utility/matrixOps.hpp" +#include namespace dai { @@ -252,11 +253,19 @@ std::vector CalibrationHandler::getDistortionCoefficients(CameraBoardSock return eepromData.cameraData[cameraId].distortionCoeff; } -float CalibrationHandler::getFov(CameraBoardSocket cameraId) { - if(eepromData.cameraData.find(cameraId) == eepromData.cameraData.end()) - throw std::runtime_error("There is no Camera data available corresponding to the the requested cameraID"); +float CalibrationHandler::getFov(CameraBoardSocket cameraId, bool useSpec) { + if (useSpec) { + if(eepromData.cameraData.find(cameraId) == eepromData.cameraData.end()) + throw std::runtime_error("There is no Camera data available corresponding to the the requested cameraID"); - return eepromData.cameraData[cameraId].specHfovDeg; + return eepromData.cameraData[cameraId].specHfovDeg; + } + // Calculate fov from intrinsics + std::vector> intrinsics; + int width, height; + std::tie(intrinsics, width, height) = CalibrationHandler::getDefaultIntrinsics(dai::CameraBoardSocket::LEFT); + auto focalLength = intrinsics[0][0]; + return 2 * 180 / M_PI * atan(width * 0.5 / focalLength); } uint8_t CalibrationHandler::getLensPosition(CameraBoardSocket cameraId) { From f375e951e0ce21998bc326845553f191a0013ac5 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 2 Sep 2021 17:24:34 +0200 Subject: [PATCH 049/101] Added capability to not install signal handlers --- include/depthai/utility/Initialization.hpp | 2 +- include/depthai/xlink/XLinkConnection.hpp | 4 --- src/utility/Initialization.cpp | 38 ++++++++++++++-------- src/xlink/XLinkConnection.cpp | 23 +++---------- 4 files changed, 30 insertions(+), 37 deletions(-) diff --git a/include/depthai/utility/Initialization.hpp b/include/depthai/utility/Initialization.hpp index 348d7f34a..d9c420c68 100644 --- a/include/depthai/utility/Initialization.hpp +++ b/include/depthai/utility/Initialization.hpp @@ -4,6 +4,6 @@ namespace dai { -bool initialize(std::string additionalInfo = ""); +bool initialize(std::string additionalInfo = "", bool installSignalHandler = true); } // namespace dai diff --git a/include/depthai/xlink/XLinkConnection.hpp b/include/depthai/xlink/XLinkConnection.hpp index ff6512d58..d845a6f6c 100644 --- a/include/depthai/xlink/XLinkConnection.hpp +++ b/include/depthai/xlink/XLinkConnection.hpp @@ -33,10 +33,6 @@ struct DeviceInfo { * Represents connection between host and device over XLink protocol */ class XLinkConnection { - static std::atomic xlinkGlobalInitialized; - static XLinkGlobalHandler_t xlinkGlobalHandler; - static void initXLinkGlobal(); - public: // static API static std::vector getAllConnectedDevices(XLinkDeviceState_t state = X_LINK_ANY_STATE); diff --git a/src/utility/Initialization.cpp b/src/utility/Initialization.cpp index f511835cb..5b7930acb 100644 --- a/src/utility/Initialization.cpp +++ b/src/utility/Initialization.cpp @@ -5,18 +5,15 @@ #include "utility/Resources.hpp" // libraries +#include "XLink/XLink.h" #include "backward.hpp" #include "spdlog/cfg/env.h" #include "spdlog/cfg/helpers.h" #include "spdlog/details/os.h" #include "spdlog/spdlog.h" - -// Backward library stacktrace handling -namespace backward { - -backward::SignalHandling sh; - -} // namespace backward +extern "C" { +#include "XLink/XLinkLog.h" +} // For easier access to dai namespaced symbols namespace dai { @@ -37,12 +34,20 @@ namespace { } // namespace -bool initialize(std::string additionalInfo) { +// Backward library stacktrace handling +static backward::SignalHandling* pSignalHandler; + +bool initialize(std::string additionalInfo, bool installSignalHandler) { // atomic bool for checking whether depthai was already initialized static std::atomic initialized{false}; - if(initialized.exchange(true)) return true; + // install backward if specified + auto envSignalHandler = spdlog::details::os::getenv("DEPTHAI_INSTALL_SIGNAL_HANDLER"); + if(installSignalHandler && envSignalHandler != "0") { + pSignalHandler = new backward::SignalHandling; + } + // Set global logging level from ENV variable 'DEPTHAI_LEVEL' // Taken from spdlog, to replace with DEPTHAI_LEVEL instead of SPDLOG_LEVEL // spdlog::cfg::load_env_levels(); @@ -54,11 +59,6 @@ bool initialize(std::string additionalInfo) { spdlog::set_level(spdlog::level::warn); } - // auto debugger_val = spdlog::details::os::getenv("DEPTHAI_DEBUGGER"); - // if(!debugger_val.empty()){ - // // TODO(themarpe) - instruct Device class that first available device is also a booted device - // } - // Print core commit and build datetime if(!additionalInfo.empty()) { spdlog::debug("{}", additionalInfo); @@ -71,6 +71,16 @@ bool initialize(std::string additionalInfo) { // Preload Resources (getting instance causes some internal lazy loading to start) Resources::getInstance(); + // Static global handler + static XLinkGlobalHandler_t xlinkGlobalHandler = {}; + xlinkGlobalHandler.protocol = X_LINK_USB_VSC; + auto status = XLinkInitialize(&xlinkGlobalHandler); + if(X_LINK_SUCCESS != status) { + throw std::runtime_error("Couldn't initialize XLink"); + } + // Suppress XLink related errors + mvLogDefaultLevelSet(MVLOG_LAST); + spdlog::debug("Initialize - finished"); return true; diff --git a/src/xlink/XLinkConnection.cpp b/src/xlink/XLinkConnection.cpp index eea0aa693..cd34b3f81 100644 --- a/src/xlink/XLinkConnection.cpp +++ b/src/xlink/XLinkConnection.cpp @@ -9,6 +9,9 @@ #include #include +// project +#include "depthai/utility/Initialization.hpp" + // libraries #include extern "C" { @@ -53,24 +56,8 @@ static DeviceInfo deviceInfoFix(const DeviceInfo& d, XLinkDeviceState_t state); constexpr std::chrono::milliseconds XLinkConnection::WAIT_FOR_BOOTUP_TIMEOUT; constexpr std::chrono::milliseconds XLinkConnection::WAIT_FOR_CONNECT_TIMEOUT; -void XLinkConnection::initXLinkGlobal() { - if(xlinkGlobalInitialized.exchange(true)) return; - - xlinkGlobalHandler.protocol = X_LINK_USB_VSC; - auto status = XLinkInitialize(&xlinkGlobalHandler); - if(X_LINK_SUCCESS != status) { - throw std::runtime_error("Couldn't initialize XLink"); - } - - // Suppress XLink related errors - mvLogDefaultLevelSet(MVLOG_LAST); -} - -std::atomic XLinkConnection::xlinkGlobalInitialized{false}; -XLinkGlobalHandler_t XLinkConnection::xlinkGlobalHandler = {}; - std::vector XLinkConnection::getAllConnectedDevices(XLinkDeviceState_t state) { - initXLinkGlobal(); + initialize(); std::vector devices; @@ -239,7 +226,7 @@ bool XLinkConnection::bootAvailableDevice(const deviceDesc_t& deviceToBoot, std: } void XLinkConnection::initDevice(const DeviceInfo& deviceToInit, XLinkDeviceState_t expectedState) { - initXLinkGlobal(); + initialize(); assert(deviceLinkId == -1); XLinkError_t rc = X_LINK_ERROR; From 3f2c17625f2571c86190ccba968d1696ff6e5e10 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 2 Sep 2021 23:34:59 +0300 Subject: [PATCH 050/101] Fixed issues for the PR --- examples/src/calibration_reader.cpp | 7 ++++--- src/device/CalibrationHandler.cpp | 12 ++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/examples/src/calibration_reader.cpp b/examples/src/calibration_reader.cpp index 1ecaeb86a..3ce0bc93e 100644 --- a/examples/src/calibration_reader.cpp +++ b/examples/src/calibration_reader.cpp @@ -13,9 +13,9 @@ void printMatrix(std::vector> matrix) { for(auto row : matrix) { out += "["; for(auto val : row) out += to_string(val) + ", "; - out = out.substr(0, out.size()-2) + "]\n"; + out = out.substr(0, out.size() - 2) + "]\n"; } - out = out.substr(0, out.size()-1) + "]\n\n"; + out = out.substr(0, out.size() - 1) + "]\n\n"; cout << out; } int main(int argc, char** argv) { @@ -38,7 +38,8 @@ int main(int argc, char** argv) { cout << "Stereo baseline distance: " << calibData.getBaselineDistance() << " cm" << endl; - cout << "Mono FOV from camera specs: " << calibData.getFov(dai::CameraBoardSocket::LEFT) << ", calculated FOV: " << calibData.getFov(dai::CameraBoardSocket::LEFT, false) << endl; + cout << "Mono FOV from camera specs: " << calibData.getFov(dai::CameraBoardSocket::LEFT) + << ", calculated FOV: " << calibData.getFov(dai::CameraBoardSocket::LEFT, false) << endl; cout << "Intrinsics from getCameraIntrinsics function full resolution:" << endl; intrinsics = calibData.getCameraIntrinsics(dai::CameraBoardSocket::RIGHT); diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 235192cde..9429995d3 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -1,5 +1,6 @@ #include "device/CalibrationHandler.hpp" +#include #include #include #include @@ -13,7 +14,6 @@ #include "nlohmann/json.hpp" #include "spdlog/spdlog.h" #include "utility/matrixOps.hpp" -#include namespace dai { @@ -254,18 +254,18 @@ std::vector CalibrationHandler::getDistortionCoefficients(CameraBoardSock } float CalibrationHandler::getFov(CameraBoardSocket cameraId, bool useSpec) { - if (useSpec) { - if(eepromData.cameraData.find(cameraId) == eepromData.cameraData.end()) - throw std::runtime_error("There is no Camera data available corresponding to the the requested cameraID"); + if(eepromData.cameraData.find(cameraId) == eepromData.cameraData.end()) + throw std::runtime_error("There is no Camera data available corresponding to the the requested cameraID"); + if(useSpec) { return eepromData.cameraData[cameraId].specHfovDeg; } // Calculate fov from intrinsics std::vector> intrinsics; int width, height; - std::tie(intrinsics, width, height) = CalibrationHandler::getDefaultIntrinsics(dai::CameraBoardSocket::LEFT); + std::tie(intrinsics, width, height) = CalibrationHandler::getDefaultIntrinsics(cameraId); auto focalLength = intrinsics[0][0]; - return 2 * 180 / M_PI * atan(width * 0.5 / focalLength); + return 2 * 180 / M_PI * atan(width * 0.5f / focalLength); } uint8_t CalibrationHandler::getLensPosition(CameraBoardSocket cameraId) { From 1c7eddf43efd5a84df4f5277d0921cff788a921f Mon Sep 17 00:00:00 2001 From: Erol444 Date: Sat, 4 Sep 2021 22:34:16 +0300 Subject: [PATCH 051/101] Add ImageManipConfig helper functions --- include/depthai/pipeline/datatype/ImageManipConfig.hpp | 10 ++++++++++ src/pipeline/datatype/ImageManipConfig.cpp | 8 ++++++++ 2 files changed, 18 insertions(+) diff --git a/include/depthai/pipeline/datatype/ImageManipConfig.hpp b/include/depthai/pipeline/datatype/ImageManipConfig.hpp index 67948d225..ab3213384 100644 --- a/include/depthai/pipeline/datatype/ImageManipConfig.hpp +++ b/include/depthai/pipeline/datatype/ImageManipConfig.hpp @@ -199,6 +199,16 @@ class ImageManipConfig : public Buffer { */ int getResizeHeight() const; + /** + * @returns Crop configuration + */ + RawImageManipConfig::CropConfig getCropConfig() const; + + /** + * @returns Resize configuration + */ + RawImageManipConfig::ResizeConfig getResizeConfig() const; + /** * @returns True if resize thumbnail mode is set, false otherwise */ diff --git a/src/pipeline/datatype/ImageManipConfig.cpp b/src/pipeline/datatype/ImageManipConfig.cpp index 6d1578705..5e5c54bca 100644 --- a/src/pipeline/datatype/ImageManipConfig.cpp +++ b/src/pipeline/datatype/ImageManipConfig.cpp @@ -191,6 +191,14 @@ int ImageManipConfig::getResizeHeight() const { return cfg.resizeConfig.height; } +RawImageManipConfig::CropConfig ImageManipConfig::getCropConfig() const { + return cfg.cropConfig; +} + +RawImageManipConfig::ResizeConfig ImageManipConfig::getResizeConfig() const { + return cfg.resizeConfig; +} + bool ImageManipConfig::isResizeThumbnail() const { return cfg.resizeConfig.lockAspectRatioFill; } From f7ee029b657a41a0e1b70a5b777d709177879e32 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sat, 4 Sep 2021 21:40:21 +0200 Subject: [PATCH 052/101] Added alias --- include/depthai/pipeline/datatype/ImageManipConfig.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/include/depthai/pipeline/datatype/ImageManipConfig.hpp b/include/depthai/pipeline/datatype/ImageManipConfig.hpp index ab3213384..b7df8068d 100644 --- a/include/depthai/pipeline/datatype/ImageManipConfig.hpp +++ b/include/depthai/pipeline/datatype/ImageManipConfig.hpp @@ -25,6 +25,11 @@ class ImageManipConfig : public Buffer { RawImageManipConfig& cfg; public: + // Alias + using CropConfig = RawImageManipConfig::CropConfig; + using ResizeConfig = RawImageManipConfig::ResizeConfig; + using FormatConfig = RawImageManipConfig::FormatConfig; + /// Construct ImageManipConfig message ImageManipConfig(); explicit ImageManipConfig(std::shared_ptr ptr); @@ -202,12 +207,12 @@ class ImageManipConfig : public Buffer { /** * @returns Crop configuration */ - RawImageManipConfig::CropConfig getCropConfig() const; + CropConfig getCropConfig() const; /** * @returns Resize configuration */ - RawImageManipConfig::ResizeConfig getResizeConfig() const; + ResizeConfig getResizeConfig() const; /** * @returns True if resize thumbnail mode is set, false otherwise From ca42d9d2ba1bc87a5c9b6a79f9d1bea37cbc21ed Mon Sep 17 00:00:00 2001 From: Erol444 Date: Sat, 4 Sep 2021 22:46:49 +0300 Subject: [PATCH 053/101] Fixed style checks, added FormatConfig --- include/depthai/pipeline/datatype/ImageManipConfig.hpp | 7 ++++++- src/pipeline/datatype/ImageManipConfig.cpp | 8 ++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/include/depthai/pipeline/datatype/ImageManipConfig.hpp b/include/depthai/pipeline/datatype/ImageManipConfig.hpp index b7df8068d..8d8e6c855 100644 --- a/include/depthai/pipeline/datatype/ImageManipConfig.hpp +++ b/include/depthai/pipeline/datatype/ImageManipConfig.hpp @@ -29,7 +29,7 @@ class ImageManipConfig : public Buffer { using CropConfig = RawImageManipConfig::CropConfig; using ResizeConfig = RawImageManipConfig::ResizeConfig; using FormatConfig = RawImageManipConfig::FormatConfig; - + /// Construct ImageManipConfig message ImageManipConfig(); explicit ImageManipConfig(std::shared_ptr ptr); @@ -214,6 +214,11 @@ class ImageManipConfig : public Buffer { */ ResizeConfig getResizeConfig() const; + /** + * @returns Format configuration + */ + FormatConfig getFormatConfig() const; + /** * @returns True if resize thumbnail mode is set, false otherwise */ diff --git a/src/pipeline/datatype/ImageManipConfig.cpp b/src/pipeline/datatype/ImageManipConfig.cpp index 5e5c54bca..31ea98de5 100644 --- a/src/pipeline/datatype/ImageManipConfig.cpp +++ b/src/pipeline/datatype/ImageManipConfig.cpp @@ -191,14 +191,18 @@ int ImageManipConfig::getResizeHeight() const { return cfg.resizeConfig.height; } -RawImageManipConfig::CropConfig ImageManipConfig::getCropConfig() const { +ImageManipConfig::CropConfig ImageManipConfig::getCropConfig() const { return cfg.cropConfig; } -RawImageManipConfig::ResizeConfig ImageManipConfig::getResizeConfig() const { +ImageManipConfig::ResizeConfig ImageManipConfig::getResizeConfig() const { return cfg.resizeConfig; } +ImageManipConfig::FormatConfig ImageManipConfig::getFormatConfig() const { + return cfg.formatConfig; +} + bool ImageManipConfig::isResizeThumbnail() const { return cfg.resizeConfig.lockAspectRatioFill; } From a324364736ab816be9c9da7d6e243364c9f7f7ed Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sun, 5 Sep 2021 02:23:53 +0200 Subject: [PATCH 054/101] Fixed exception rethrow in DeviceBase --- src/device/DeviceBase.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 695ac40e3..067a466e8 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -361,8 +361,10 @@ void DeviceBase::tryStartPipeline(const Pipeline& pipeline) { throw std::runtime_error("Couldn't start the pipeline"); } } catch(const std::exception& e) { + // close device (cleanup) close(); - throw e; + // Rethrow original exception + throw; } } @@ -878,6 +880,7 @@ bool DeviceBase::startPipelineImpl(const Pipeline& pipeline) { if(success) { pimpl->rpcClient->call("startPipeline"); } else { + printf("errorMsg: %s\n", errorMsg.c_str()); throw std::runtime_error(errorMsg); return false; } From 8dcdd217fe696e837af63bf64877fb90afb2cee6 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sun, 5 Sep 2021 02:28:23 +0200 Subject: [PATCH 055/101] Pipeline - number of connections improvement --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- tests/CMakeLists.txt | 3 +- tests/src/unlimited_io_connection_test.cpp | 136 ++++++++++++++++++++ 3 files changed, 138 insertions(+), 3 deletions(-) create mode 100644 tests/src/unlimited_io_connection_test.cpp diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index db1778ddd..db715c791 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 "7131affa2c01ecd34506e9c3dd8ea9198ed874f1") +set(DEPTHAI_DEVICE_SIDE_COMMIT "a78b518e8a08f2a6b55b7b2e926685b6539d7a71") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 6b258c53b..ce0edebd6 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -99,5 +99,4 @@ target_compile_definitions(openvino_blob PRIVATE OPENVINO_2021_4_BLOB_PATH="${openvino_2021_4_blob}" ) - - +dai_add_test(unlimited_io_connection_test src/unlimited_io_connection_test.cpp) diff --git a/tests/src/unlimited_io_connection_test.cpp b/tests/src/unlimited_io_connection_test.cpp new file mode 100644 index 000000000..b5ec12e75 --- /dev/null +++ b/tests/src/unlimited_io_connection_test.cpp @@ -0,0 +1,136 @@ +#define CATCH_CONFIG_MAIN +#include + +// Include depthai library +#include + +// Number of IO to test +constexpr auto NUM_CONN = 16; + +// Using +using namespace std; +using namespace dai; +using namespace std::chrono; +using namespace std::chrono_literals; + +TEST_CASE("Test many IO connections after crossing Leon processors") { + Pipeline pipeline; + auto camRgb = pipeline.create(); + camRgb->setInterleaved(true); + + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + auto xout = pipeline.create(); + xout->setStreamName(out); + camRgb->preview.link(xout->input); + } + + Device device(pipeline); + + this_thread::sleep_for(5s); + + // Set queues to non-blocking + std::unordered_map numFrames; + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + device.getOutputQueue(out, 9, false); + numFrames[out] = 0; + } + + // Retrieve atleast 10 frames of each, without errors + constexpr auto NUM_FRAMES_REQUIRED = 10; + + bool receivedEnoughFrames = true; + auto t1 = steady_clock::now(); + do { + receivedEnoughFrames = true; + // Set queues to non-blocking + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + + auto frame = device.getOutputQueue(out, 9, false)->tryGet(); + if(frame != nullptr) { + numFrames[out]++; + } + + // Check + if(numFrames[out] < NUM_FRAMES_REQUIRED) { + receivedEnoughFrames = false; + } + } + + if(receivedEnoughFrames) { + break; + } + } while(steady_clock::now() - t1 < 5s); + + cout << "numFrames: "; + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + cout << numFrames[out] << ", "; + } + cout << "\n"; + + REQUIRE(receivedEnoughFrames == true); +} + +TEST_CASE("Test many IO connections before crossing Leon processors") { + Pipeline pipeline; + auto camRgb = pipeline.create(); + camRgb->setInterleaved(false); + + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + auto manip = pipeline.create(); + camRgb->preview.link(manip->inputImage); + auto xout = pipeline.create(); + xout->setStreamName(out); + manip->out.link(xout->input); + } + + Device device(pipeline); + + // Set queues to non-blocking + std::unordered_map numFrames; + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + device.getOutputQueue(out, 9, false); + numFrames[out] = 0; + } + + // Retrieve atleast 10 frames of each, without errors + constexpr auto NUM_FRAMES_REQUIRED = 10; + + bool receivedEnoughFrames = false; + auto t1 = steady_clock::now(); + do { + receivedEnoughFrames = true; + // Set queues to non-blocking + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + auto frame = device.getOutputQueue(out, 9, false)->tryGet(); + if(frame != nullptr) { + numFrames[out]++; + } + + // Check + if(numFrames[out] < NUM_FRAMES_REQUIRED) { + receivedEnoughFrames = false; + } + } + + if(receivedEnoughFrames) { + break; + } + + } while(steady_clock::now() - t1 < 5s); + + cout << "numFrames: "; + for(int i = 0; i < NUM_CONN; i++) { + string out = "out" + to_string(i); + cout << numFrames[out] << ", "; + } + cout << "\n"; + + REQUIRE(receivedEnoughFrames == true); +} From 823e39e6deb8041f79d062a9f47a4f888c78a165 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Mon, 6 Sep 2021 02:39:37 +0300 Subject: [PATCH 056/101] Update FW: fix default camera orientation for OAK-1-PoE, was rotated --- 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 db715c791..3e103b792 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 "a78b518e8a08f2a6b55b7b2e926685b6539d7a71") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c5303ecec4c30a752a7f362589239b9d1784a0d3") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 32bde360729f684de9e54ac543693d9ec6f9b094 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Mon, 6 Sep 2021 17:57:21 +0300 Subject: [PATCH 057/101] Update FW, for depthai-shared to match with depthai-core --- 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 3e103b792..dd050fd49 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 "c5303ecec4c30a752a7f362589239b9d1784a0d3") +set(DEPTHAI_DEVICE_SIDE_COMMIT "a61df0cda79ec82a665dbf236de74b9aa2c096e4") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 35e163a9951badae16541f012f37bc5e2fe2c02c Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 6 Sep 2021 18:57:01 +0200 Subject: [PATCH 058/101] Updated FW - fixed cache coherency issue --- 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 82d7746d9..b86aa6d3e 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 "6a2abd52e513272993a6f8140702407e5c6ea251") +set(DEPTHAI_DEVICE_SIDE_COMMIT "6793bd877ae334ac468b884ad1c9db22667da5b8") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 8f1223647cf9af620991540fb800933164e056ba Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 6 Sep 2021 21:27:12 +0300 Subject: [PATCH 059/101] Remove rectification flipping on host, it was resolved in firmware --- examples/src/mono_depth_mobilenetssd.cpp | 20 ------------------- .../src/rgb_encoding_mono_mobilenet_depth.cpp | 5 ----- examples/src/spatial_mobilenet_mono.cpp | 7 ------- 3 files changed, 32 deletions(-) diff --git a/examples/src/mono_depth_mobilenetssd.cpp b/examples/src/mono_depth_mobilenetssd.cpp index f07c49ed1..595ebfef9 100644 --- a/examples/src/mono_depth_mobilenetssd.cpp +++ b/examples/src/mono_depth_mobilenetssd.cpp @@ -8,8 +8,6 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; -static std::atomic flipRectified{true}; - int main(int argc, char** argv) { using namespace std; // Default blob path provided by Hunter private data download @@ -115,24 +113,6 @@ int main(int argc, char** argv) { auto inDet = qDet->tryGet(); auto inDisparity = qDisparity->tryGet(); - if(inRight) { - rightFrame = inRight->getCvFrame(); - if(flipRectified) { - cv::flip(rightFrame, rightFrame, 1); - } - } - - if(inDet) { - detections = inDet->detections; - if(flipRectified) { - for(auto& detection : detections) { - auto swap = detection.xmin; - detection.xmin = 1 - detection.xmax; - detection.xmax = 1 - swap; - } - } - } - if(inDisparity) { // Frame is transformed, normalized, and color map will be applied to highlight the depth info disparityFrame = inDisparity->getFrame(); diff --git a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp index 79fbfed03..8dd660384 100644 --- a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp +++ b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp @@ -9,8 +9,6 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; -static std::atomic flipRectified{true}; - int main(int argc, char** argv) { using namespace std; // Default blob path provided by Hunter private data download @@ -128,9 +126,6 @@ int main(int argc, char** argv) { if(inDisparity) { frameDisparity = inDisparity->getCvFrame(); - if(flipRectified) { - cv::flip(frameDisparity, frameDisparity, 1); - } frameDisparity.convertTo(frameDisparity, CV_8UC1, disparityMultiplier); cv::applyColorMap(frameDisparity, frameDisparity, cv::COLORMAP_JET); } diff --git a/examples/src/spatial_mobilenet_mono.cpp b/examples/src/spatial_mobilenet_mono.cpp index 692c1192e..12c58da03 100644 --- a/examples/src/spatial_mobilenet_mono.cpp +++ b/examples/src/spatial_mobilenet_mono.cpp @@ -9,7 +9,6 @@ static const std::vector labelMap = {"background", "aeroplane", "bi "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; static std::atomic syncNN{true}; -static std::atomic flipRectified{true}; int main(int argc, char** argv) { using namespace std; @@ -112,7 +111,6 @@ int main(int argc, char** argv) { } cv::Mat rectifiedRight = inRectified->getCvFrame(); - if(flipRectified) cv::flip(rectifiedRight, rectifiedRight, 1); cv::Mat depthFrame = inDepth->getFrame(); cv::Mat depthFrameColor; @@ -139,11 +137,6 @@ int main(int argc, char** argv) { } for(auto& detection : detections) { - if(flipRectified) { - auto swap = detection.xmin; - detection.xmin = 1 - detection.xmax; - detection.xmax = 1 - swap; - } int x1 = detection.xmin * rectifiedRight.cols; int y1 = detection.ymin * rectifiedRight.rows; int x2 = detection.xmax * rectifiedRight.cols; From 48d90876b676d0a62325f1a7fd07312120b53c82 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Tue, 7 Sep 2021 17:13:17 +0300 Subject: [PATCH 060/101] Fixed integration issues --- cmake/depthaiConfig.cmake.in | 5 ++++- cmake/depthaiDependencies.cmake | 6 +++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/cmake/depthaiConfig.cmake.in b/cmake/depthaiConfig.cmake.in index fd6a2cd4c..98309545b 100644 --- a/cmake/depthaiConfig.cmake.in +++ b/cmake/depthaiConfig.cmake.in @@ -1,5 +1,8 @@ # Get whether library was build as shared or not -set(depthai_SHARED_LIBS @BUILD_SHARED_LIBS@) +set(DEPTHAI_SHARED_LIBS @BUILD_SHARED_LIBS@) + +# Get whether library was build with Backward or not +set(DEPTHAI_ENABLE_BACKWARD @DEPTHAI_ENABLE_BACKWARD@) # Specify that this is config mode (Called by find_package) set(CONFIG_MODE TRUE) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index 91be07460..8eb6744d8 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -3,7 +3,7 @@ if(CONFIG_MODE) set(CMAKE_PREFIX_PATH "${CMAKE_CURRENT_LIST_DIR}/${_IMPORT_PREFIX}" ${CMAKE_PREFIX_PATH}) set(_QUIET "QUIET") else() - set(depthai_SHARED_LIBS ${BUILD_SHARED_LIBS}) + set(DEPTHAI_SHARED_LIBS ${BUILD_SHARED_LIBS}) hunter_add_package(nlohmann_json) if(NOT DEPTHAI_XLINK_LOCAL) hunter_add_package(XLink) @@ -19,7 +19,7 @@ else() endif() # If library was build as static, find all dependencies -if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT depthai_SHARED_LIBS)) +if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT DEPTHAI_SHARED_LIBS)) # BZip2 (for bspatch) find_package(BZip2 ${_QUIET} CONFIG REQUIRED) @@ -65,5 +65,5 @@ if(CONFIG_MODE) set(_CMAKE_PREFIX_PATH_ORIGINAL) set(_QUIET) else() - set(depthai_SHARED_LIBS) + set(DEPTHAI_SHARED_LIBS) endif() From fe5fce7703bf0ef797b2090d3567de4b5fd170cd Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 9 Sep 2021 01:07:02 +0300 Subject: [PATCH 061/101] FW - Updated ColorCamera 1080P resolution config --- 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 e5098dba8..da94dfe7a 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 "c2462c39ab83d97b673c19b37c3e957ed13da0fb") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b8692a26d1806d778713b8f01caf07554c8f50a8") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 706bce77fd0ecc2136026a1dafc8a95ef33f5623 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Thu, 9 Sep 2021 19:19:59 +0300 Subject: [PATCH 062/101] Update FW with stereo performance improvements --- 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 676a69db2..e8266dd77 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 "ff3310c9e34043ddaf1f7c18bcfc64a22b9305c9") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c4b3d00943fc95edac6e51602e74ce506f72b191") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 480c52fe43a2ea46a26fca4497562f7a9782c66e Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Thu, 9 Sep 2021 23:07:57 +0300 Subject: [PATCH 063/101] Updated Bootloader to 0.0.15 --- cmake/Depthai/DepthaiBootloaderConfig.cmake | 8 ++++---- shared/depthai-bootloader-shared | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 70e4f91e9..71dea6a0e 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.12") -set(DEPTHAI_BOOTLOADER_VERSION "870bec4ca0ef026ddb9c490a38dfe9d097614554") +set(DEPTHAI_BOOTLOADER_VERSION "0.0.15") +#set(DEPTHAI_BOOTLOADER_VERSION "870bec4ca0ef026ddb9c490a38dfe9d097614554") diff --git a/shared/depthai-bootloader-shared b/shared/depthai-bootloader-shared index 1a4da6776..126f34415 160000 --- a/shared/depthai-bootloader-shared +++ b/shared/depthai-bootloader-shared @@ -1 +1 @@ -Subproject commit 1a4da6776a1c019ee2698ab7587a5c7e81f89d60 +Subproject commit 126f344155346836a6a2d67b0993c161b6b48729 From 0e44c3893ba8dafbfa238de5aaa3b3531990782c Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 10 Sep 2021 00:19:24 +0300 Subject: [PATCH 064/101] Update FW with stereo confidence runtime config fix --- 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 e8266dd77..f55e000f9 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 "c4b3d00943fc95edac6e51602e74ce506f72b191") +set(DEPTHAI_DEVICE_SIDE_COMMIT "998500e9b6cd25679bfacf54c085e4d6c494d944") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 10897a7e19952af14e1c39cf7310270452032aa3 Mon Sep 17 00:00:00 2001 From: Sachin Guruswamy <43363595+saching13@users.noreply.github.com> Date: Thu, 16 Sep 2021 10:48:42 -0700 Subject: [PATCH 065/101] Imu extrinsics (#211) * Updated IMU extrinsics --- src/device/CalibrationHandler.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index c50e845fe..fa5f1b0e0 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -362,13 +362,13 @@ std::vector> CalibrationHandler::getImuToCameraExtrinsics(Cam std::vector> transformationMatrix = eepromData.imuExtrinsics.rotationMatrix; if(useSpecTranslation) { - transformationMatrix[0].push_back(eepromData.cameraData[cameraId].extrinsics.specTranslation.x); - transformationMatrix[1].push_back(eepromData.cameraData[cameraId].extrinsics.specTranslation.y); - transformationMatrix[2].push_back(eepromData.cameraData[cameraId].extrinsics.specTranslation.z); + transformationMatrix[0].push_back(eepromData.imuExtrinsics.specTranslation.x); + transformationMatrix[1].push_back(eepromData.imuExtrinsics.specTranslation.y); + transformationMatrix[2].push_back(eepromData.imuExtrinsics.specTranslation.z); } else { - transformationMatrix[0].push_back(eepromData.cameraData[cameraId].extrinsics.translation.x); - transformationMatrix[1].push_back(eepromData.cameraData[cameraId].extrinsics.translation.y); - transformationMatrix[2].push_back(eepromData.cameraData[cameraId].extrinsics.translation.z); + transformationMatrix[0].push_back(eepromData.imuExtrinsics.translation.x); + transformationMatrix[1].push_back(eepromData.imuExtrinsics.translation.y); + transformationMatrix[2].push_back(eepromData.imuExtrinsics.translation.z); } std::vector homogeneous_vector = {0, 0, 0, 1}; transformationMatrix.push_back(homogeneous_vector); From 75cc2d48685985eaa74a792600cc5e5214886b36 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 17 Sep 2021 18:55:57 +0300 Subject: [PATCH 066/101] Update FW, enable runtime configuration of Stereo node --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- .../src/rgb_encoding_mono_mobilenet_depth.cpp | 2 -- .../pipeline/datatype/StereoDepthConfig.hpp | 26 +++++++++++++++ include/depthai/pipeline/node/StereoDepth.hpp | 9 +++++- shared/depthai-shared | 2 +- src/pipeline/datatype/StereoDepthConfig.cpp | 32 ++++++++++++++----- src/pipeline/node/StereoDepth.cpp | 15 ++++++--- 7 files changed, 71 insertions(+), 17 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index f55e000f9..6add2b741 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 "998500e9b6cd25679bfacf54c085e4d6c494d944") +set(DEPTHAI_DEVICE_SIDE_COMMIT "4fe26550e101a25d552eed78850da6b558b48585") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp index 8dd660384..79957134d 100644 --- a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp +++ b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp @@ -56,9 +56,7 @@ int main(int argc, char** argv) { monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); videoEncoder->setDefaultProfilePreset(1920, 1080, 30, dai::VideoEncoderProperties::Profile::H265_MAIN); - // Note: the rectified streams are horizontally mirrored by default depth->initialConfig.setConfidenceThreshold(255); - depth->setRectifyMirrorFrame(false); depth->setRectifyEdgeFillColor(0); // Black, to better see the cutout nn->setConfidenceThreshold(0.5); diff --git a/include/depthai/pipeline/datatype/StereoDepthConfig.hpp b/include/depthai/pipeline/datatype/StereoDepthConfig.hpp index f1bc8507b..d1193ed08 100644 --- a/include/depthai/pipeline/datatype/StereoDepthConfig.hpp +++ b/include/depthai/pipeline/datatype/StereoDepthConfig.hpp @@ -61,6 +61,32 @@ class StereoDepthConfig : public Buffer { * Get threshold for left-right check combine */ int getLeftRightCheckThreshold() const; + + /** + * Computes and combines disparities in both L-R and R-L directions, and combine them. + * + * For better occlusion handling, discarding invalid disparity values + */ + void setLeftRightCheck(bool enable); + + /** + * Computes disparity with sub-pixel interpolation (5 fractional bits). + * + * Suitable for long range. Currently incompatible with extended disparity + */ + void setSubpixel(bool enable); + + /** + * Set explicit configuration. + * @param config Explicit configuration + */ + void set(dai::RawStereoDepthConfig config); + + /** + * Retrieve configuration data for StereoDepth. + * @returns config for stereo depth algorithm + */ + dai::RawStereoDepthConfig get() const; }; } // namespace dai diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 0b5d0a13a..04f969b17 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -214,6 +214,7 @@ class StereoDepth : public Node { void setRectifyEdgeFillColor(int color); /** + * DEPRECATED function. It was removed, since rectified images are not flipped anymore. * Mirror rectified frames, only when LR-check mode is disabled. Default `true`. * The mirroring is required to have a normal non-mirrored disparity/depth output. * @@ -224,7 +225,7 @@ class StereoDepth : public Node { * * @param enable True for normal disparity/depth, otherwise mirrored */ - void setRectifyMirrorFrame(bool enable); + [[deprecated("Function call should be removed")]] void setRectifyMirrorFrame(bool enable); /** * Enable outputting rectified frames. Optimizes computation on device side when disabled. @@ -239,6 +240,12 @@ class StereoDepth : public Node { */ [[deprecated("Function call should be removed")]] void setOutputDepth(bool enable); + /** + * Enable runtime stereo mode switch, e.g. from standard to LR-check. + * Note: when enabled resources allocated for worst case to enable switching to any mode. + */ + void setRuntimeModeSwitch(bool enable); + /** * Useful for normalization of the disparity map. * @returns Maximum disparity value that the node can return diff --git a/shared/depthai-shared b/shared/depthai-shared index ec7b5fc53..d6ef8123a 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit ec7b5fc53261955411d29a01790a2fe683c26a63 +Subproject commit d6ef8123a2367178c1a9041ab3a56b24e6494619 diff --git a/src/pipeline/datatype/StereoDepthConfig.cpp b/src/pipeline/datatype/StereoDepthConfig.cpp index b9bba6f37..376e5f155 100644 --- a/src/pipeline/datatype/StereoDepthConfig.cpp +++ b/src/pipeline/datatype/StereoDepthConfig.cpp @@ -11,35 +11,51 @@ StereoDepthConfig::StereoDepthConfig(std::shared_ptr ptr) : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} void StereoDepthConfig::setConfidenceThreshold(int confThr) { - cfg.config.confidenceThreshold = confThr; + cfg.config.costMatching.confidenceThreshold = confThr; } int StereoDepthConfig::getConfidenceThreshold() const { - return cfg.config.confidenceThreshold; + return cfg.config.costMatching.confidenceThreshold; } void StereoDepthConfig::setMedianFilter(dai::MedianFilter median) { - cfg.config.median = median; + cfg.config.postProcessing.median = median; } dai::MedianFilter StereoDepthConfig::getMedianFilter() const { - return cfg.config.median; + return cfg.config.postProcessing.median; } void StereoDepthConfig::setBilateralFilterSigma(uint16_t sigma) { - cfg.config.bilateralSigmaValue = sigma; + cfg.config.postProcessing.bilateralSigmaValue = sigma; } uint16_t StereoDepthConfig::getBilateralFilterSigma() const { - return cfg.config.bilateralSigmaValue; + return cfg.config.postProcessing.bilateralSigmaValue; } void StereoDepthConfig::setLeftRightCheckThreshold(int threshold) { - cfg.config.leftRightCheckThreshold = threshold; + cfg.config.algorithmControl.leftRightCheckThreshold = threshold; } int StereoDepthConfig::getLeftRightCheckThreshold() const { - return cfg.config.leftRightCheckThreshold; + return cfg.config.algorithmControl.leftRightCheckThreshold; +} + +void StereoDepthConfig::setLeftRightCheck(bool enable) { + cfg.config.algorithmControl.enableLeftRightCheck = enable; +} + +void StereoDepthConfig::setSubpixel(bool enable) { + cfg.config.algorithmControl.enableSubpixel = enable; +} + +dai::RawStereoDepthConfig StereoDepthConfig::get() const { + return cfg; +} + +void StereoDepthConfig::set(dai::RawStereoDepthConfig config) { + cfg = config; } } // namespace dai diff --git a/src/pipeline/node/StereoDepth.cpp b/src/pipeline/node/StereoDepth.cpp index 820c93e6b..b8b879ec2 100644 --- a/src/pipeline/node/StereoDepth.cpp +++ b/src/pipeline/node/StereoDepth.cpp @@ -120,10 +120,12 @@ void StereoDepth::setRectification(bool enable) { properties.enableRectification = enable; } void StereoDepth::setLeftRightCheck(bool enable) { - properties.enableLeftRightCheck = enable; + initialConfig.setLeftRightCheck(enable); + properties.initialConfig = *rawConfig; } void StereoDepth::setSubpixel(bool enable) { - properties.enableSubpixel = enable; + initialConfig.setSubpixel(enable); + properties.initialConfig = *rawConfig; } void StereoDepth::setExtendedDisparity(bool enable) { properties.enableExtendedDisparity = enable; @@ -132,7 +134,8 @@ void StereoDepth::setRectifyEdgeFillColor(int color) { properties.rectifyEdgeFillColor = color; } void StereoDepth::setRectifyMirrorFrame(bool enable) { - properties.rectifyMirrorFrame = enable; + (void)enable; + spdlog::warn("{} is deprecated.", __func__); } void StereoDepth::setOutputRectified(bool enable) { (void)enable; @@ -143,10 +146,14 @@ void StereoDepth::setOutputDepth(bool enable) { spdlog::warn("{} is deprecated. The output is auto-enabled if used", __func__); } +void StereoDepth::setRuntimeModeSwitch(bool enable) { + properties.enableRuntimeStereoModeSwitch = enable; +} + float StereoDepth::getMaxDisparity() const { float maxDisp = 95.0; if(properties.enableExtendedDisparity) maxDisp *= 2; - if(properties.enableSubpixel) maxDisp *= 32; + if(rawConfig->config.algorithmControl.enableSubpixel) maxDisp *= 32; return maxDisp; } From 3e7fe456cb80fae5e42d4e03c1b65bb382140e04 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 17 Sep 2021 19:46:42 +0300 Subject: [PATCH 067/101] Refactor stereo depth config structure --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- shared/depthai-shared | 2 +- src/pipeline/datatype/StereoDepthConfig.cpp | 20 ++++++++++---------- src/pipeline/node/StereoDepth.cpp | 2 +- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 6add2b741..517d5f7d7 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 "4fe26550e101a25d552eed78850da6b558b48585") +set(DEPTHAI_DEVICE_SIDE_COMMIT "71bfc84bfc06df57a826b0924b2ec0d388565b09") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index d6ef8123a..ba4cf26e9 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit d6ef8123a2367178c1a9041ab3a56b24e6494619 +Subproject commit ba4cf26e9cea62baa09c52790162134dc11f34c4 diff --git a/src/pipeline/datatype/StereoDepthConfig.cpp b/src/pipeline/datatype/StereoDepthConfig.cpp index 376e5f155..f5ec788b1 100644 --- a/src/pipeline/datatype/StereoDepthConfig.cpp +++ b/src/pipeline/datatype/StereoDepthConfig.cpp @@ -11,43 +11,43 @@ StereoDepthConfig::StereoDepthConfig(std::shared_ptr ptr) : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} void StereoDepthConfig::setConfidenceThreshold(int confThr) { - cfg.config.costMatching.confidenceThreshold = confThr; + cfg.costMatching.confidenceThreshold = confThr; } int StereoDepthConfig::getConfidenceThreshold() const { - return cfg.config.costMatching.confidenceThreshold; + return cfg.costMatching.confidenceThreshold; } void StereoDepthConfig::setMedianFilter(dai::MedianFilter median) { - cfg.config.postProcessing.median = median; + cfg.postProcessing.median = median; } dai::MedianFilter StereoDepthConfig::getMedianFilter() const { - return cfg.config.postProcessing.median; + return cfg.postProcessing.median; } void StereoDepthConfig::setBilateralFilterSigma(uint16_t sigma) { - cfg.config.postProcessing.bilateralSigmaValue = sigma; + cfg.postProcessing.bilateralSigmaValue = sigma; } uint16_t StereoDepthConfig::getBilateralFilterSigma() const { - return cfg.config.postProcessing.bilateralSigmaValue; + return cfg.postProcessing.bilateralSigmaValue; } void StereoDepthConfig::setLeftRightCheckThreshold(int threshold) { - cfg.config.algorithmControl.leftRightCheckThreshold = threshold; + cfg.algorithmControl.leftRightCheckThreshold = threshold; } int StereoDepthConfig::getLeftRightCheckThreshold() const { - return cfg.config.algorithmControl.leftRightCheckThreshold; + return cfg.algorithmControl.leftRightCheckThreshold; } void StereoDepthConfig::setLeftRightCheck(bool enable) { - cfg.config.algorithmControl.enableLeftRightCheck = enable; + cfg.algorithmControl.enableLeftRightCheck = enable; } void StereoDepthConfig::setSubpixel(bool enable) { - cfg.config.algorithmControl.enableSubpixel = enable; + cfg.algorithmControl.enableSubpixel = enable; } dai::RawStereoDepthConfig StereoDepthConfig::get() const { diff --git a/src/pipeline/node/StereoDepth.cpp b/src/pipeline/node/StereoDepth.cpp index b8b879ec2..57d5cd59c 100644 --- a/src/pipeline/node/StereoDepth.cpp +++ b/src/pipeline/node/StereoDepth.cpp @@ -153,7 +153,7 @@ void StereoDepth::setRuntimeModeSwitch(bool enable) { float StereoDepth::getMaxDisparity() const { float maxDisp = 95.0; if(properties.enableExtendedDisparity) maxDisp *= 2; - if(rawConfig->config.algorithmControl.enableSubpixel) maxDisp *= 32; + if(rawConfig->algorithmControl.enableSubpixel) maxDisp *= 32; return maxDisp; } From 2a87322ff0e55ac0f63e92a72004b8795c3d4df2 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 17 Sep 2021 20:50:38 +0300 Subject: [PATCH 068/101] Update FW with subpixel fix --- 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 517d5f7d7..6629ab7d0 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 "71bfc84bfc06df57a826b0924b2ec0d388565b09") +set(DEPTHAI_DEVICE_SIDE_COMMIT "f1e86083efeaf88f9dc6342e51b88cc3eb41a14b") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From b8ccbad529957921da62de6bebf765edc9c21f03 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 18 Sep 2021 02:51:40 +0300 Subject: [PATCH 069/101] Add option to pick spatial calculation algorithm : average,min,max of selected ROI --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/SpatialDetectionNetwork.hpp | 6 ++++++ shared/depthai-shared | 2 +- src/pipeline/node/SpatialDetectionNetwork.cpp | 4 ++++ 4 files changed, 12 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index da94dfe7a..a1840b168 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 "b8692a26d1806d778713b8f01caf07554c8f50a8") +set(DEPTHAI_DEVICE_SIDE_COMMIT "3068e8bd9bba9e46d565c92fe2c2d2d663a20bd2") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp b/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp index 6da36d5cd..8bdd9638e 100644 --- a/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp +++ b/include/depthai/pipeline/node/SpatialDetectionNetwork.hpp @@ -87,6 +87,12 @@ class SpatialDetectionNetwork : public DetectionNetwork { * @param upperThreshold UpperThreshold must be in the interval (lowerThreshold,65535]. */ void setDepthUpperThreshold(uint32_t upperThreshold); + + /** + * Specifies spatial location calculator algorithm: Average/Min/Max + * @param calculationAlgorithm Calculation algorithm. + */ + void setSpatialCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm calculationAlgorithm); }; /** diff --git a/shared/depthai-shared b/shared/depthai-shared index ec7b5fc53..b8dc63068 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit ec7b5fc53261955411d29a01790a2fe683c26a63 +Subproject commit b8dc6306861720acb7a444bfa8fb3d0b0db875de diff --git a/src/pipeline/node/SpatialDetectionNetwork.cpp b/src/pipeline/node/SpatialDetectionNetwork.cpp index 8ad188517..700074aa4 100644 --- a/src/pipeline/node/SpatialDetectionNetwork.cpp +++ b/src/pipeline/node/SpatialDetectionNetwork.cpp @@ -46,6 +46,10 @@ void SpatialDetectionNetwork::setDepthUpperThreshold(uint32_t upperThreshold) { getPropertiesRef().depthThresholds.upperThreshold = upperThreshold; } +void SpatialDetectionNetwork::setSpatialCalculationAlgorithm(dai::SpatialLocationCalculatorAlgorithm calculationAlgorithm) { + getPropertiesRef().calculationAlgorithm = calculationAlgorithm; +} + //-------------------------------------------------------------------- // MobileNet //-------------------------------------------------------------------- From 66a969a4cd471e8cdc5ee490b9869582cbb92924 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 18 Sep 2021 18:17:22 +0300 Subject: [PATCH 070/101] Set bytes per pixel for ImgFrame --- shared/depthai-shared | 2 +- src/pipeline/datatype/ImgFrame.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/shared/depthai-shared b/shared/depthai-shared index ba4cf26e9..405dce90a 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit ba4cf26e9cea62baa09c52790162134dc11f34c4 +Subproject commit 405dce90a0cda008b79fefbfb25fc5d9fee32daf diff --git a/src/pipeline/datatype/ImgFrame.cpp b/src/pipeline/datatype/ImgFrame.cpp index 59c26462b..a2ced7ef7 100644 --- a/src/pipeline/datatype/ImgFrame.cpp +++ b/src/pipeline/datatype/ImgFrame.cpp @@ -77,6 +77,7 @@ void ImgFrame::setSize(std::tuple size) { } void ImgFrame::setType(RawImgFrame::Type type) { img.fb.type = type; + img.fb.bytesPP = RawImgFrame::typeToBpp(img.fb.type); } } // namespace dai From 7fc9a13cf858e6ce3fdc04da6149a82840cc28e6 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Sat, 18 Sep 2021 18:27:57 +0300 Subject: [PATCH 071/101] flash_bootloader: fix flashing NETWORK bootloader (when booted over USB), or flashing a different bootloader type --- examples/src/flash_bootloader.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index 001145797..a60dd7dc6 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -40,8 +40,12 @@ int main(int argc, char** argv) { dai::DeviceBootloader bl(info, true); auto currentBlType = bl.getType(); + if(blType == dai::DeviceBootloader::Type::AUTO) { + blType = currentBlType; + } + // Check if bootloader type is the same - if(blType != dai::DeviceBootloader::Type::AUTO && currentBlType != blType) { + if(currentBlType != blType) { std::cout << "Are you sure you want to flash '" << blType << "' bootloader over current '" << currentBlType << "' bootloader?" << std::endl; std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; std::cin.ignore(); @@ -54,11 +58,11 @@ int main(int argc, char** argv) { // Create a progress callback lambda auto progress = [](float p) { std::cout << "Flashing Progress..." << p * 100 << "%" << std::endl; }; - std::cout << "Flashing " << currentBlType << " bootloader..." << std::endl; + std::cout << "Flashing " << blType << " bootloader..." << std::endl; auto t1 = steady_clock::now(); bool success = false; std::string message; - std::tie(success, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, currentBlType, progress); + std::tie(success, message) = bl.flashBootloader(dai::DeviceBootloader::Memory::FLASH, blType, progress); if(success) { std::cout << "Flashing successful. Took " << duration_cast(steady_clock::now() - t1).count() << "ms" << std::endl; } else { From 8c85d31c14e03462df267221884de541a0f8866b Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 18 Sep 2021 18:34:10 +0300 Subject: [PATCH 072/101] Update FW to match depthai-shared --- 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 6629ab7d0..e20fcd2ff 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 "f1e86083efeaf88f9dc6342e51b88cc3eb41a14b") +set(DEPTHAI_DEVICE_SIDE_COMMIT "484b9952039f243d7fe57ee2d58d62a797ca784e") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From f077c3e258e4fb244b02e61c718a3ddd4491e97c Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Sat, 18 Sep 2021 18:35:39 +0300 Subject: [PATCH 073/101] flash_bootloader: improve user prompts, when booted over USB / recovery mode: don't ask for confirmations, as if flashing is interrupted, recovery mode should still be accessible. Also it was a bit confusing asking to replace USB bootloader (booted as a flasher helper) with NETWORK --- examples/src/flash_bootloader.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/examples/src/flash_bootloader.cpp b/examples/src/flash_bootloader.cpp index a60dd7dc6..84681c543 100644 --- a/examples/src/flash_bootloader.cpp +++ b/examples/src/flash_bootloader.cpp @@ -2,6 +2,7 @@ #include #include "depthai/depthai.hpp" +#include "depthai/xlink/XLinkConnection.hpp" int main(int argc, char** argv) { using namespace std::chrono; @@ -19,14 +20,6 @@ int main(int argc, char** argv) { } } - std::cout << "Warning! Flashing bootloader can potentially soft brick your device and should be done with caution." << std::endl; - std::cout << "Do not unplug your device while the bootloader is flashing." << std::endl; - std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; - if(std::cin.get() != 'y') { - std::cout << "Prompt declined, exiting..." << std::endl; - return -1; - } - bool found = false; dai::DeviceInfo info; std::tie(found, info) = dai::DeviceBootloader::getFirstAvailableDevice(); @@ -35,6 +28,17 @@ int main(int argc, char** argv) { return -1; } + bool hasBootloader = (info.state == X_LINK_BOOTLOADER); + if(hasBootloader) { + std::cout << "Warning! Flashing bootloader can potentially soft brick your device and should be done with caution." << std::endl; + std::cout << "Do not unplug your device while the bootloader is flashing." << std::endl; + std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; + if(std::cin.get() != 'y') { + std::cout << "Prompt declined, exiting..." << std::endl; + return -1; + } + } + // Open DeviceBootloader and allow flashing bootloader std::cout << "Booting latest bootloader first, will take a tad longer..." << std::endl; dai::DeviceBootloader bl(info, true); @@ -44,8 +48,8 @@ int main(int argc, char** argv) { blType = currentBlType; } - // Check if bootloader type is the same - if(currentBlType != blType) { + // Check if bootloader type is the same, if already booted by bootloader (not in USB recovery mode) + if(currentBlType != blType && hasBootloader) { std::cout << "Are you sure you want to flash '" << blType << "' bootloader over current '" << currentBlType << "' bootloader?" << std::endl; std::cout << "Type 'y' and press enter to proceed, otherwise exits: "; std::cin.ignore(); From b93190cb7ae4489fccbb9f9c9ffeb795a330b9b7 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 18 Sep 2021 18:47:54 +0300 Subject: [PATCH 074/101] Update shared w/ CI fixes --- 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 e20fcd2ff..c7eee3333 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 "484b9952039f243d7fe57ee2d58d62a797ca784e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "5812c91f0993597c990ae589032469af7c55c410") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index 405dce90a..7b5db4f26 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 405dce90a0cda008b79fefbfb25fc5d9fee32daf +Subproject commit 7b5db4f266413d181c049daa9c401bb9e0259914 From 4d70dc23f8380159f97034295a1e0a705740845c Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 18 Sep 2021 23:18:48 +0300 Subject: [PATCH 075/101] Update FW/shared to latest develop --- 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 a1840b168..14e6ed745 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 "3068e8bd9bba9e46d565c92fe2c2d2d663a20bd2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "c33a06e6de71f4bfb0dc168e6bf3c41c5b73edca") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index b8dc63068..ffe1f467e 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit b8dc6306861720acb7a444bfa8fb3d0b0db875de +Subproject commit ffe1f467ee98c4c1128611413ccdca9220876fc7 From 80cf830cc69eff7b4479dd263c605a8f3c0ebb6d Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sun, 19 Sep 2021 04:06:25 +0300 Subject: [PATCH 076/101] Add debug outputs to stereo node; expose number of frame pools --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/StereoDepth.hpp | 18 ++++++++++++++++++ shared/depthai-shared | 2 +- src/pipeline/node/StereoDepth.cpp | 4 ++++ 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index ae792a7b1..e24e07523 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 "301b2f40a756d635c69778fc800aa4e914f46e2e") +set(DEPTHAI_DEVICE_SIDE_COMMIT "afe13fd7f5c1f986b1701bdd06996142b0673037") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 04f969b17..14c6c144c 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -88,6 +88,18 @@ class StereoDepth : public Node { */ Output rectifiedRight{*this, "rectifiedRight", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Outputs ImgFrame message that carries left-right check first iteration (before combining with second iteration) disparity map. + * Useful for debugging. + */ + Output debugDispLrCheckIt1{*this, "debugDispLrCheckIt1", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Outputs ImgFrame message that carries cost dump of disparity map. + * Useful for debugging. + */ + Output debugDispCostDump{*this, "debugDispCostDump", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** * Specify local filesystem path to the calibration file * @param path Path to calibration file. If empty use EEPROM @@ -246,6 +258,12 @@ class StereoDepth : public Node { */ void setRuntimeModeSwitch(bool enable); + /** + * Specify number of frames in pool. + * @param numFramesPool How many frames should the pool have + */ + void setNumFramesPool(int numFramesPool); + /** * Useful for normalization of the disparity map. * @returns Maximum disparity value that the node can return diff --git a/shared/depthai-shared b/shared/depthai-shared index b7ee4af56..16e236e0e 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit b7ee4af5631559c92dd31f8c8dbf0b4434e36ad6 +Subproject commit 16e236e0ee7398a8eb04567cfb37acbe5e72f546 diff --git a/src/pipeline/node/StereoDepth.cpp b/src/pipeline/node/StereoDepth.cpp index 57d5cd59c..653c7f5ec 100644 --- a/src/pipeline/node/StereoDepth.cpp +++ b/src/pipeline/node/StereoDepth.cpp @@ -150,6 +150,10 @@ void StereoDepth::setRuntimeModeSwitch(bool enable) { properties.enableRuntimeStereoModeSwitch = enable; } +void StereoDepth::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; +} + float StereoDepth::getMaxDisparity() const { float maxDisp = 95.0; if(properties.enableExtendedDisparity) maxDisp *= 2; From 19fc7be9954c7c5467b1c1d1b61d36666442c395 Mon Sep 17 00:00:00 2001 From: Erik Date: Mon, 20 Sep 2021 22:41:09 +0300 Subject: [PATCH 077/101] Update calibration_reader.cpp --- examples/src/calibration_reader.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/src/calibration_reader.cpp b/examples/src/calibration_reader.cpp index 3ce0bc93e..d4551dae5 100644 --- a/examples/src/calibration_reader.cpp +++ b/examples/src/calibration_reader.cpp @@ -18,6 +18,7 @@ void printMatrix(std::vector> matrix) { out = out.substr(0, out.size() - 1) + "]\n\n"; cout << out; } + int main(int argc, char** argv) { using namespace std; From 9a0147fbf06cf5713bf1fa97c4f44bd3beeb2ba2 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 21 Sep 2021 00:10:30 +0300 Subject: [PATCH 078/101] Add stereo node output config --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/StereoDepth.hpp | 15 +++++++++++++-- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index e24e07523..21b1b229c 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 "afe13fd7f5c1f986b1701bdd06996142b0673037") +set(DEPTHAI_DEVICE_SIDE_COMMIT "238b43b7fbd0af171e301e764699776538594b8d") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 14c6c144c..841751726 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -88,15 +88,26 @@ class StereoDepth : public Node { */ Output rectifiedRight{*this, "rectifiedRight", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Outputs StereoDepthConfig message that contains current stereo configuration. + */ + Output outConfig{*this, "outConfig", Output::Type::MSender, {{DatatypeEnum::StereoDepthConfig, false}}}; + /** * Outputs ImgFrame message that carries left-right check first iteration (before combining with second iteration) disparity map. - * Useful for debugging. + * Useful for debugging/fine tuning. */ Output debugDispLrCheckIt1{*this, "debugDispLrCheckIt1", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Outputs ImgFrame message that carries left-right check second iteration (before combining with first iteration) disparity map. + * Useful for debugging/fine tuning. + */ + Output debugDispLrCheckIt2{*this, "debugDispLrCheckIt2", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** * Outputs ImgFrame message that carries cost dump of disparity map. - * Useful for debugging. + * Useful for debugging/fine tuning. */ Output debugDispCostDump{*this, "debugDispCostDump", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; From 3d6a21c149051b5639d8aa019d041c32fbccf0b3 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 21 Sep 2021 00:59:50 +0300 Subject: [PATCH 079/101] Add getMaxDisparity() based on subpixel bits --- include/depthai/pipeline/datatype/StereoDepthConfig.hpp | 6 ++++++ include/depthai/pipeline/node/StereoDepth.hpp | 2 +- src/pipeline/datatype/StereoDepthConfig.cpp | 7 +++++++ src/pipeline/node/StereoDepth.cpp | 5 +---- 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/include/depthai/pipeline/datatype/StereoDepthConfig.hpp b/include/depthai/pipeline/datatype/StereoDepthConfig.hpp index d1193ed08..dabc87f3a 100644 --- a/include/depthai/pipeline/datatype/StereoDepthConfig.hpp +++ b/include/depthai/pipeline/datatype/StereoDepthConfig.hpp @@ -76,6 +76,12 @@ class StereoDepthConfig : public Buffer { */ void setSubpixel(bool enable); + /** + * Useful for normalization of the disparity map. + * @returns Maximum disparity value that the node can return + */ + float getMaxDisparity() const; + /** * Set explicit configuration. * @param config Explicit configuration diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 841751726..f1f8658ff 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -279,7 +279,7 @@ class StereoDepth : public Node { * Useful for normalization of the disparity map. * @returns Maximum disparity value that the node can return */ - float getMaxDisparity() const; + [[deprecated("Use 'initialConfig.getMaxDisparity()' instead")]] float getMaxDisparity() const; }; } // namespace node diff --git a/src/pipeline/datatype/StereoDepthConfig.cpp b/src/pipeline/datatype/StereoDepthConfig.cpp index f5ec788b1..de3fe6a6e 100644 --- a/src/pipeline/datatype/StereoDepthConfig.cpp +++ b/src/pipeline/datatype/StereoDepthConfig.cpp @@ -50,6 +50,13 @@ void StereoDepthConfig::setSubpixel(bool enable) { cfg.algorithmControl.enableSubpixel = enable; } +float StereoDepthConfig::getMaxDisparity() const { + float maxDisp = 95.0; + if(false) maxDisp *= 2; // TODO re-enable with extended + if(cfg.algorithmControl.enableSubpixel) maxDisp *= (1 << cfg.algorithmControl.subpixelFractionalBits); + return maxDisp; +} + dai::RawStereoDepthConfig StereoDepthConfig::get() const { return cfg; } diff --git a/src/pipeline/node/StereoDepth.cpp b/src/pipeline/node/StereoDepth.cpp index 653c7f5ec..9d7e95f47 100644 --- a/src/pipeline/node/StereoDepth.cpp +++ b/src/pipeline/node/StereoDepth.cpp @@ -155,10 +155,7 @@ void StereoDepth::setNumFramesPool(int numFramesPool) { } float StereoDepth::getMaxDisparity() const { - float maxDisp = 95.0; - if(properties.enableExtendedDisparity) maxDisp *= 2; - if(rawConfig->algorithmControl.enableSubpixel) maxDisp *= 32; - return maxDisp; + return initialConfig.getMaxDisparity(); } } // namespace node From 0776684af309df781d4e76a2ed150bef3667c31c Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 21 Sep 2021 22:23:48 +0300 Subject: [PATCH 080/101] Update FW with runtime disparity range fix --- 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 21b1b229c..6cee38ed0 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 "238b43b7fbd0af171e301e764699776538594b8d") +set(DEPTHAI_DEVICE_SIDE_COMMIT "db62f875ea8aca53e22b4095daff67a26e1cfdd2") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 562e98159c87e46d05cdfd240e6fd6cb3f567c52 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 22 Sep 2021 00:26:01 +0300 Subject: [PATCH 081/101] Handle disparity companding in getMaxDisparity --- src/pipeline/datatype/StereoDepthConfig.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/pipeline/datatype/StereoDepthConfig.cpp b/src/pipeline/datatype/StereoDepthConfig.cpp index de3fe6a6e..5ac088aeb 100644 --- a/src/pipeline/datatype/StereoDepthConfig.cpp +++ b/src/pipeline/datatype/StereoDepthConfig.cpp @@ -52,6 +52,10 @@ void StereoDepthConfig::setSubpixel(bool enable) { float StereoDepthConfig::getMaxDisparity() const { float maxDisp = 95.0; + if(cfg.costMatching.disparityWidth == RawStereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64) { + maxDisp = 63; + } + if(cfg.costMatching.enableCompanding) maxDisp = 175; if(false) maxDisp *= 2; // TODO re-enable with extended if(cfg.algorithmControl.enableSubpixel) maxDisp *= (1 << cfg.algorithmControl.subpixelFractionalBits); return maxDisp; From e68a496ca994ae328969e99a7711d0b209315c02 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 22 Sep 2021 01:12:58 +0300 Subject: [PATCH 082/101] Replace deprecated getMaxDisparity() function --- examples/src/depth_preview.cpp | 2 +- examples/src/mono_depth_mobilenetssd.cpp | 2 +- examples/src/rgb_depth_aligned.cpp | 2 +- examples/src/rgb_encoding_mono_mobilenet_depth.cpp | 2 +- examples/src/stereo_depth_video.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/src/depth_preview.cpp b/examples/src/depth_preview.cpp index 7da9df484..52c0d42cf 100644 --- a/examples/src/depth_preview.cpp +++ b/examples/src/depth_preview.cpp @@ -51,7 +51,7 @@ int main() { auto inDepth = q->get(); auto frame = inDepth->getFrame(); // Normalization for better visualization - frame.convertTo(frame, CV_8UC1, 255 / depth->getMaxDisparity()); + frame.convertTo(frame, CV_8UC1, 255 / depth->initialConfig.getMaxDisparity()); cv::imshow("disparity", frame); diff --git a/examples/src/mono_depth_mobilenetssd.cpp b/examples/src/mono_depth_mobilenetssd.cpp index 595ebfef9..4a7aa9db9 100644 --- a/examples/src/mono_depth_mobilenetssd.cpp +++ b/examples/src/mono_depth_mobilenetssd.cpp @@ -105,7 +105,7 @@ int main(int argc, char** argv) { cv::imshow(name, frame); }; - float disparityMultiplier = 255 / stereo->getMaxDisparity(); + float disparityMultiplier = 255 / stereo->initialConfig.getMaxDisparity(); while(true) { // Instead of get (blocking), we use tryGet (nonblocking) which will return the available data or None otherwise diff --git a/examples/src/rgb_depth_aligned.cpp b/examples/src/rgb_depth_aligned.cpp index 452a0e446..bc998e6c9 100644 --- a/examples/src/rgb_depth_aligned.cpp +++ b/examples/src/rgb_depth_aligned.cpp @@ -87,7 +87,7 @@ int main() { if(latestPacket.find(name) != latestPacket.end()) { if(name == "depth") { frame[name] = latestPacket[name]->getFrame(); - auto maxDisparity = stereo->getMaxDisparity(); + auto maxDisparity = stereo->initialConfig.getMaxDisparity(); // Optional, extend range 0..95 -> 0..255, for a better visualisation if(1) frame[name].convertTo(frame[name], CV_8UC1, 255. / maxDisparity); // Optional, apply false colorization diff --git a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp index 79957134d..716fed39d 100644 --- a/examples/src/rgb_encoding_mono_mobilenet_depth.cpp +++ b/examples/src/rgb_encoding_mono_mobilenet_depth.cpp @@ -81,7 +81,7 @@ int main(int argc, char** argv) { nn->out.link(nnOut->input); // Disparity range is used for normalization - float disparityMultiplier = 255 / depth->getMaxDisparity(); + float disparityMultiplier = 255 / depth->initialConfig.getMaxDisparity(); // Connect to device and start pipeline dai::Device device(pipeline); diff --git a/examples/src/stereo_depth_video.cpp b/examples/src/stereo_depth_video.cpp index a0b522771..7fc28f044 100644 --- a/examples/src/stereo_depth_video.cpp +++ b/examples/src/stereo_depth_video.cpp @@ -91,7 +91,7 @@ int main() { auto rectifRightQueue = withDepth ? device.getOutputQueue("rectified_right", 8, false) : nullptr; // Disparity range is used for normalization - float disparityMultiplier = withDepth ? 255 / stereo->getMaxDisparity() : 0; + float disparityMultiplier = withDepth ? 255 / stereo->initialConfig.getMaxDisparity() : 0; while(true) { auto left = leftQueue->get(); From 7f9dd0c33a7c52bb5dab4c893b3afc2859f7e56f Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 22 Sep 2021 18:16:00 +0300 Subject: [PATCH 083/101] Update FW/shared to latest develop --- 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 6cee38ed0..88242f1a8 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 "db62f875ea8aca53e22b4095daff67a26e1cfdd2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "30d8539b9a7b6b056cdb9d3deab5036542204dd2") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/shared/depthai-shared b/shared/depthai-shared index 16e236e0e..d00ddff16 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 16e236e0ee7398a8eb04567cfb37acbe5e72f546 +Subproject commit d00ddff163b7ef902b2a70b6366c7f2e37697522 From b276b1864df0135e1ca759081ae3cef0621f6500 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 22 Sep 2021 18:16:14 +0300 Subject: [PATCH 084/101] Sync stereo_depth_video example --- examples/src/stereo_depth_video.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/examples/src/stereo_depth_video.cpp b/examples/src/stereo_depth_video.cpp index 7fc28f044..5a105cec4 100644 --- a/examples/src/stereo_depth_video.cpp +++ b/examples/src/stereo_depth_video.cpp @@ -47,12 +47,10 @@ int main() { if(withDepth) { // StereoDepth - stereo->initialConfig.setConfidenceThreshold(200); + stereo->initialConfig.setConfidenceThreshold(230); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout - // stereo->loadCalibrationFile("../../../../depthai/resources/depthai.calib"); // stereo->setInputResolution(1280, 720); - // TODO: median filtering is disabled on device with (lrcheck || extended || subpixel) - stereo->initialConfig.setMedianFilter(dai::MedianFilter::MEDIAN_OFF); + stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5); stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); stereo->setSubpixel(subpixel); @@ -111,16 +109,14 @@ int main() { if(outputDepth) { auto depth = depthQueue->get(); - cv::imshow("depth", cv::Mat(depth->getHeight(), depth->getWidth(), CV_16UC1, depth->getData().data())); + cv::imshow("depth", depth->getCvFrame()); } if(outputRectified) { auto rectifL = rectifLeftQueue->get(); - // cv::flip(rectifiedLeftFrame, rectifiedLeftFrame, 1); cv::imshow("rectified_left", rectifL->getFrame()); auto rectifR = rectifRightQueue->get(); - // cv::flip(rectifiedRightFrame, rectifiedRightFrame, 1); cv::imshow("rectified_right", rectifR->getFrame()); } } From c282b5e479297ecae5e0154db9d712fa5d81e98d Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Wed, 22 Sep 2021 18:51:38 +0300 Subject: [PATCH 085/101] Fix bootloader version example --- examples/src/bootloader_version.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/src/bootloader_version.cpp b/examples/src/bootloader_version.cpp index d7d8ac460..8b3056173 100644 --- a/examples/src/bootloader_version.cpp +++ b/examples/src/bootloader_version.cpp @@ -4,11 +4,10 @@ int main(int argc, char** argv) { bool res = false; dai::DeviceInfo info; std::tie(res, info) = dai::DeviceBootloader::getFirstAvailableDevice(); - std::string path; if(res) { std::cout << "Found device with name: " << info.desc.name << std::endl; - dai::DeviceBootloader bl(info, path); + dai::DeviceBootloader bl(info); std::cout << "Version: " << bl.getVersion().toString() << std::endl; } else { std::cout << "No devices found" << std::endl; From 2c98b9923c951c932b22f0b1f94d38693b77c530 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Fri, 24 Sep 2021 21:05:48 +0300 Subject: [PATCH 086/101] Fix still image output in RGB postprocessing --- 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 88242f1a8..3fa3eef8c 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 "30d8539b9a7b6b056cdb9d3deab5036542204dd2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "4edb4923933e4cf2750f1958e4dee77de2cdca3f") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 177c47c8e66aee053c94fac4217a8c9845909052 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sat, 25 Sep 2021 02:26:44 +0300 Subject: [PATCH 087/101] Update FW to latest 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 3fa3eef8c..8dc309c24 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 "4edb4923933e4cf2750f1958e4dee77de2cdca3f") +set(DEPTHAI_DEVICE_SIDE_COMMIT "ebd8f842de48304a24a07bb62e7f9b8854e78601") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From b574b787e3e7b864bd0c6ca5277a21f76ec0c016 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Sat, 25 Sep 2021 06:55:54 +0200 Subject: [PATCH 088/101] Updated FW with SPI improvements --- 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 88242f1a8..8d2aae768 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 "30d8539b9a7b6b056cdb9d3deab5036542204dd2") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b0ea2353a4a75cd9960e765c8c414101e22bfcad") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 16fe59a5f659ce0c4fd97441e55256579d4d64ea Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 28 Sep 2021 03:18:43 +0300 Subject: [PATCH 089/101] Add confidence map output to stereo node --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/StereoDepth.hpp | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 8dc309c24..99733d7dc 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 "ebd8f842de48304a24a07bb62e7f9b8854e78601") +set(DEPTHAI_DEVICE_SIDE_COMMIT "ab8156a8bc044f10beb36fbcc924b36b82346e69") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index f1f8658ff..567d7a896 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -111,6 +111,13 @@ class StereoDepth : public Node { */ Output debugDispCostDump{*this, "debugDispCostDump", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** + * Outputs ImgFrame message that carries RAW8 confidence map. + * Lower values means higher confidence of the calculated disparity value. + * RGB aligment or left-right check is not performed on confidence map. + */ + Output confidenceMap{*this, "confidenceMap", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + /** * Specify local filesystem path to the calibration file * @param path Path to calibration file. If empty use EEPROM From 05f8e9d8dc2e4809becce0c87ea3f64e913c196b Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 28 Sep 2021 03:22:08 +0300 Subject: [PATCH 090/101] Update confidence map output docs --- include/depthai/pipeline/node/StereoDepth.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 567d7a896..66de407a5 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -114,7 +114,7 @@ class StereoDepth : public Node { /** * Outputs ImgFrame message that carries RAW8 confidence map. * Lower values means higher confidence of the calculated disparity value. - * RGB aligment or left-right check is not performed on confidence map. + * RGB aligment, left-right check or any postproccessing (e.g. median filter) is not performed on confidence map. */ Output confidenceMap{*this, "confidenceMap", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; From 7a8a522d0e78940cb1d319a686801c8224206e32 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 28 Sep 2021 17:33:46 +0300 Subject: [PATCH 091/101] Update FW to latest develop --- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- include/depthai/pipeline/node/StereoDepth.hpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 99733d7dc..8e6dcce5d 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 "ab8156a8bc044f10beb36fbcc924b36b82346e69") +set(DEPTHAI_DEVICE_SIDE_COMMIT "b81c795238f5ccf667829ae270c1c35876cdd206") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 66de407a5..201e496b3 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -118,6 +118,20 @@ class StereoDepth : public Node { */ Output confidenceMap{*this, "confidenceMap", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; +#if 0 //will be enabled when confidence map RGB aligment/LR-check support will be added + /** + * Outputs ImgFrame message that carries left-right check first iteration (before combining with second iteration) confidence map. + * Useful for debugging/fine tuning. + */ + Output debugConfMapLrCheckIt1{*this, "debugConfMapLrCheckIt1", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Outputs ImgFrame message that carries left-right check second iteration (before combining with first iteration) confidence map. + * Useful for debugging/fine tuning. + */ + Output debugConfMapLrCheckIt2{*this, "debugConfMapLrCheckIt2", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; +#endif + /** * Specify local filesystem path to the calibration file * @param path Path to calibration file. If empty use EEPROM From fafd1ddc2aab1563e4cab530ee56c78ed547796d Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Tue, 28 Sep 2021 18:08:16 +0300 Subject: [PATCH 092/101] Fix styling --- include/depthai/pipeline/node/StereoDepth.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 201e496b3..83b48da56 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -118,7 +118,7 @@ class StereoDepth : public Node { */ Output confidenceMap{*this, "confidenceMap", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; -#if 0 //will be enabled when confidence map RGB aligment/LR-check support will be added +#if 0 // will be enabled when confidence map RGB aligment/LR-check support will be added /** * Outputs ImgFrame message that carries left-right check first iteration (before combining with second iteration) confidence map. * Useful for debugging/fine tuning. From dc72ec50fc648d23a0902119a54e237882604898 Mon Sep 17 00:00:00 2001 From: Otto Seiskari Date: Thu, 16 Sep 2021 23:20:14 +0300 Subject: [PATCH 093/101] Fix inversion formula for extrinsic matrices in CalibrationHandler --- src/device/CalibrationHandler.cpp | 64 +++++++++++++++---------------- 1 file changed, 30 insertions(+), 34 deletions(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 1e4891b31..b8c7a0f3e 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -18,6 +18,33 @@ namespace dai { using namespace matrix; +namespace { +void invertSe3Matrix4x4InPlace(std::vector>& mat) { + // Transpose in-place + float temp = mat[0][1]; + mat[0][1] = mat[1][0]; + mat[1][0] = temp; + + temp = mat[0][2]; + mat[0][2] = mat[2][0]; + mat[2][0] = temp; + + temp = mat[1][2]; + mat[1][2] = mat[2][1]; + mat[2][1] = temp; + + // The inverse of an SE(3) transformation (R, t) is (R^T, -R^T t) + float newTrans[3]; + for(int i = 0; i < 3; ++i) { + newTrans[i] = 0; + for(int j = 0; j < 3; ++j) { + newTrans[i] -= mat[i][j] * mat[j][3]; + } + } + for(int i = 0; i < 3; ++i) mat[i][3] = newTrans[i]; +} +} // namespace + CalibrationHandler::CalibrationHandler(std::string eepromDataPath) { std::ifstream jsonStream(eepromDataPath); // TODO(sachin): Check if the file exists first. @@ -289,23 +316,7 @@ std::vector> CalibrationHandler::getCameraExtrinsics(CameraBo return computeExtrinsicMatrix(srcCamera, dstCamera, useSpecTranslation); } else if(checkExtrinsicsLink(dstCamera, srcCamera)) { extrinsics = computeExtrinsicMatrix(dstCamera, srcCamera, useSpecTranslation); - - float temp = extrinsics[0][1]; - extrinsics[0][1] = extrinsics[1][0]; - extrinsics[1][0] = temp; - - temp = extrinsics[0][2]; - extrinsics[0][2] = extrinsics[2][0]; - extrinsics[2][0] = temp; - - temp = extrinsics[1][2]; - extrinsics[1][2] = extrinsics[2][1]; - extrinsics[2][1] = temp; - - extrinsics[0][3] = -extrinsics[0][3]; - extrinsics[1][3] = -extrinsics[1][3]; - extrinsics[2][3] = -extrinsics[2][3]; - + invertSe3Matrix4x4InPlace(extrinsics); return extrinsics; } else { throw std::runtime_error("Extrinsic connection between the requested cameraId's doesn't exist. Please recalibrate or modify your calibration data"); @@ -315,22 +326,7 @@ std::vector> CalibrationHandler::getCameraExtrinsics(CameraBo std::vector> CalibrationHandler::getCameraToImuExtrinsics(CameraBoardSocket cameraId, bool useSpecTranslation) { std::vector> transformationMatrix = getImuToCameraExtrinsics(cameraId, useSpecTranslation); - float temp = transformationMatrix[0][1]; - transformationMatrix[0][1] = transformationMatrix[1][0]; - transformationMatrix[1][0] = temp; - - temp = transformationMatrix[0][2]; - transformationMatrix[0][2] = transformationMatrix[2][0]; - transformationMatrix[2][0] = temp; - - temp = transformationMatrix[1][2]; - transformationMatrix[1][2] = transformationMatrix[2][1]; - transformationMatrix[2][1] = temp; - - transformationMatrix[0][3] = -transformationMatrix[0][3]; - transformationMatrix[1][3] = -transformationMatrix[1][3]; - transformationMatrix[2][3] = -transformationMatrix[2][3]; - + invertSe3Matrix4x4InPlace(transformationMatrix); return transformationMatrix; } @@ -661,4 +657,4 @@ bool CalibrationHandler::checkSrcLinks(CameraBoardSocket headSocket) { return isConnectionValidated; } -} // namespace dai \ No newline at end of file +} // namespace dai From 78f0051f2abacaa4cdd8746a22240198bcbc2c60 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 30 Sep 2021 18:40:16 +0200 Subject: [PATCH 094/101] Fixed for Windows --- src/device/CalibrationHandler.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 9429995d3..88452ddb1 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -1,5 +1,6 @@ #include "device/CalibrationHandler.hpp" +#define _USE_MATH_DEFINES #include #include #include From 9056d786e4553a108e57f30e4d8674206763fc5a Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Sun, 3 Oct 2021 18:24:21 +0300 Subject: [PATCH 095/101] Hotfix for FW message cache coherency --- 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 8e6dcce5d..dec9991fe 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 "b81c795238f5ccf667829ae270c1c35876cdd206") +set(DEPTHAI_DEVICE_SIDE_COMMIT "8e63c65cbb74103573a7bc5dbfcf765fd391ad0b") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From a5d1646418fe9eed0d81d32b9015b9383775c6fc Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Mon, 11 Oct 2021 12:59:04 +0200 Subject: [PATCH 096/101] Windows prebuilt libraries (#220) * Added CI to build Win64 & Win32 prebuilt libraries and upload along the release --- .github/workflows/main.workflow.yml | 65 ++++++++++++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/.github/workflows/main.workflow.yml b/.github/workflows/main.workflow.yml index f3da8a52b..b2b6bc74e 100644 --- a/.github/workflows/main.workflow.yml +++ b/.github/workflows/main.workflow.yml @@ -115,6 +115,16 @@ jobs: os: [macos-latest, windows-latest, ubuntu-latest] build-type: [Debug, Release] shared: [Shared, Static] + platform: [x64] + include: + - platform: Win32 + os: windows-latest + shared: Shared + build-type: Debug + - platform: Win32 + os: windows-latest + shared: Shared + build-type: Release steps: - name: Cache .hunter folder if: matrix.os != 'windows-latest' @@ -153,8 +163,13 @@ jobs: echo "C:\tools\opencv\build\x64\vc15\bin" | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append - name: Configure ${{ matrix.build-type }}, ${{ matrix.shared }} - if: matrix.shared == 'Shared' + if: matrix.shared == 'Shared' && matrix.platform != 'Win32' run: cmake -S . -B build -D BUILD_SHARED_LIBS=ON -D CMAKE_BUILD_TYPE=${{ matrix.build-type }} -D CMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/depthai_install + + - name: Configure ${{ matrix.build-type }}, ${{ matrix.shared }}, Win32 + if: matrix.shared == 'Shared' && matrix.platform == 'Win32' + run: cmake -A Win32 -S . -B build -D BUILD_SHARED_LIBS=ON -D CMAKE_BUILD_TYPE=${{ matrix.build-type }} -D CMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/depthai_install + - name: Configure ${{ matrix.build-type }}, ${{ matrix.shared }} if: matrix.shared == 'Static' run: cmake -S . -B build -D BUILD_SHARED_LIBS=OFF -D CMAKE_BUILD_TYPE=${{ matrix.build-type }} -D CMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/depthai_install @@ -166,6 +181,20 @@ jobs: - name: Build and install run: cmake --build build --config ${{ matrix.build-type }} --target install --parallel 8 + - name: Upload Win64 shared library + if: matrix.os == 'windows-latest' && matrix.shared == 'Shared' + uses: actions/upload-artifact@v2 + with: + name: windows-prebuilt-win64 + path: ${{ env.GITHUB_WORKSPACE }}/depthai_install/ + + - name: Upload Win32 shared library + if: matrix.os == 'windows-latest' && matrix.shared == 'Shared' && matrix.platform == 'Win32' + uses: actions/upload-artifact@v2 + with: + name: windows-prebuilt-win32-no-opencv + path: ${{ env.GITHUB_WORKSPACE }}/depthai_install/ + - name: Build and test add_subdirectory run: | cmake -S tests/integration/ -B tests/integration/build_add_subdirectory -D TEST_FIND_PACKAGE=OFF @@ -214,10 +243,24 @@ jobs: - name: Install dependencies run: python3.8 -m pip install git-archive-all + - name: Download Win64 artifacts + uses: actions/download-artifact@v2 + with: + name: 'windows-prebuilt-win64' + path: depthai-core-${{ steps.tag.outputs.version }}-win64 + + - name: Download Win32 artifacts + uses: actions/download-artifact@v2 + with: + name: 'windows-prebuilt-win32-no-opencv' + path: depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv + - name: Create release assets run: | git-archive-all depthai-core-${{ steps.tag.outputs.version }}.tar.gz git-archive-all depthai-core-${{ steps.tag.outputs.version }}.zip + zip -r depthai-core-${{ steps.tag.outputs.version }}-win64.zip depthai-core-${{ steps.tag.outputs.version }}-win64 + zip -r depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv.zip depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv # Create GitHub release - uses: actions/create-release@master @@ -257,3 +300,23 @@ jobs: asset_path: ${{ github.workspace }}/depthai-core-${{ steps.tag.outputs.version }}.tar.gz asset_name: depthai-core-${{ steps.tag.outputs.version }}.tar.gz asset_content_type: application/octet-stream + + - name: Upload Win64 package (zip) + uses: actions/upload-release-asset@v1.0.1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + upload_url: ${{ steps.createRelease.outputs.upload_url }} + asset_path: ${{ github.workspace }}/depthai-core-${{ steps.tag.outputs.version }}-win64.zip + asset_name: depthai-core-${{ steps.tag.outputs.version }}-win64.zip + asset_content_type: application/octet-stream + + - name: Upload Win32 package (zip) + uses: actions/upload-release-asset@v1.0.1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + 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 From d9d82cd1164be7e6077d0a4c5e00e2ac8f39264c Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 12 Oct 2021 17:19:14 +0300 Subject: [PATCH 097/101] Update FW: increase ImageManip warp max out height: 1520 -> 2560 --- 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 b65103fc7..1a9c47303 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 "dffab55fd7e6d6ed24f054d59e18a48b57868560") +set(DEPTHAI_DEVICE_SIDE_COMMIT "9820d2403580961a0e8457ee722f748e3bddec18") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") From 2f73c307f24f3c7513f6d30930bb029328a31e07 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Tue, 12 Oct 2021 23:23:13 +0200 Subject: [PATCH 098/101] Backward - Disables use of additional stack unwinding libs --- cmake/depthaiDependencies.cmake | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cmake/depthaiDependencies.cmake b/cmake/depthaiDependencies.cmake index 8eb6744d8..070c57cf4 100644 --- a/cmake/depthaiDependencies.cmake +++ b/cmake/depthaiDependencies.cmake @@ -38,7 +38,11 @@ if(NOT CONFIG_MODE OR (CONFIG_MODE AND NOT DEPTHAI_SHARED_LIBS)) # Backward if(DEPTHAI_ENABLE_BACKWARD) + # Disable automatic check for additional stack unwinding libraries + # Just use the default compiler one + set(STACK_DETAILS_AUTO_DETECT FALSE) find_package(Backward ${_QUIET} CONFIG REQUIRED) + set(STACK_DETAILS_AUTO_DETECT) endif() endif() From 07f9847e736d0d846f481aefaa9c1ce71cd89b11 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Tue, 12 Oct 2021 23:49:39 +0200 Subject: [PATCH 099/101] Bumped to v2.11.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26eb7cff5..d514b3552 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,7 @@ if(WIN32) endif() # Create depthai project -project(depthai VERSION "2.10.0" LANGUAGES CXX C) +project(depthai VERSION "2.11.0" LANGUAGES CXX C) get_directory_property(has_parent PARENT_DIRECTORY) if(has_parent) set(DEPTHAI_VERSION ${PROJECT_VERSION} PARENT_SCOPE) From cb400ab5cfbe979a8da4d15420467ef7e202cdb3 Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Wed, 13 Oct 2021 11:43:19 +0200 Subject: [PATCH 100/101] Fixed double promotion warning --- src/device/CalibrationHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index ff510fee9..fcd4bc699 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -293,7 +293,7 @@ float CalibrationHandler::getFov(CameraBoardSocket cameraId, bool useSpec) { int width, height; std::tie(intrinsics, width, height) = CalibrationHandler::getDefaultIntrinsics(cameraId); auto focalLength = intrinsics[0][0]; - return 2 * 180 / M_PI * atan(width * 0.5f / focalLength); + return 2 * 180 / ((float) M_PI) * std::atan(width * 0.5f / focalLength); } uint8_t CalibrationHandler::getLensPosition(CameraBoardSocket cameraId) { From ce8ae6dfd4db0399d038109df9abe0582e84d70c Mon Sep 17 00:00:00 2001 From: TheMarpe Date: Wed, 13 Oct 2021 12:04:16 +0200 Subject: [PATCH 101/101] Updated formatting --- src/device/CalibrationHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index fcd4bc699..28478935a 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -293,7 +293,7 @@ float CalibrationHandler::getFov(CameraBoardSocket cameraId, bool useSpec) { int width, height; std::tie(intrinsics, width, height) = CalibrationHandler::getDefaultIntrinsics(cameraId); auto focalLength = intrinsics[0][0]; - return 2 * 180 / ((float) M_PI) * std::atan(width * 0.5f / focalLength); + return 2 * 180 / ((float)M_PI) * std::atan(width * 0.5f / focalLength); } uint8_t CalibrationHandler::getLensPosition(CameraBoardSocket cameraId) {