From 6e079d0bf0b69634536f52daf3fe7693d92a01f6 Mon Sep 17 00:00:00 2001 From: Serafadam Date: Tue, 31 Dec 2024 13:12:42 +0100 Subject: [PATCH] Rebase from 'upstream' --- CHANGELOG.rst | 24 +++ CMakeLists.txt | 5 +- cmake/Depthai/DepthaiDeviceSideConfig.cmake | 2 +- examples/CMakeLists.txt | 12 ++ examples/Yolo/yolov8_nano.cpp | 157 ++++++++++++++++++ include/depthai/device/DeviceBase.hpp | 17 ++ .../pipeline/datatype/EncodedFrame.hpp | 23 +++ include/depthai/pipeline/node/StereoDepth.hpp | 9 +- package.xml | 2 +- .../datatype/RawCameraControl.hpp | 4 +- .../datatype/RawEncodedFrame.hpp | 3 + .../datatype/RawStereoDepthConfig.hpp | 29 +++- src/device/DeviceBase.cpp | 20 +++ src/pipeline/datatype/EncodedFrame.cpp | 14 ++ src/pipeline/datatype/StereoDepthConfig.cpp | 42 +++++ src/pipeline/node/StereoDepth.cpp | 156 +++++++++++++++++ tests/src/stability_stress_test.cpp | 4 +- 17 files changed, 507 insertions(+), 16 deletions(-) create mode 100644 examples/Yolo/yolov8_nano.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst index afa13dd..3023bde 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,30 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package depthai ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.29.0 (2024-11-26) +* Features +* Add the ability to change the calibration on the device in runtime with the new `dai::Device.setCalibration()` method and to retrieve it with the `dai::Device.getCalibration()`. +* New `StereoDepth` presets: + * `DEFAULT` + * `FACE` + * `HIGH_DETAIL` + * `ROBOTICS` +* Multiple camera improvements (more details in https://github.com/luxonis/depthai-core/pull/972): + * Expose more downsampling modes when picking a lower than native resolutions + * Expose more binning modes when binning is picked on IMX582/586 (`sum` and `avg`) + * HDR on IMX582/586 + * Option to bypass 3A for having manual expose/ISO take effect faster + * Initial support for new Sony 4K Starvis sensors: IMX678 and IMX715 + * Option to set the main camera to drive auto-exposure and auto-white-balance in multi-camera configurations +* Improved StereoDepth filtering and an option to use a set a custom order of filters + * Disparity is first scaled to 13 bit before going through filtering, which results in filters being more effective. + + +* Misc +* Remove false reports on crashes that happened on device destruction +* Add `getWidth()` and `getHeight()` API to `EncodedFrame` + 2.28.0 (2024-08-21) ----------- Features diff --git a/CMakeLists.txt b/CMakeLists.txt index be08685..9f92bd8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,9 +13,6 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Early options option(DEPTHAI_ENABLE_LIBUSB "Enable usage of libusb and interaction with USB devices" ON) -# Early options -option(DEPTHAI_ENABLE_LIBUSB "Enable usage of libusb and interaction with USB devices" ON) - # Set to use native tls for windows before including Hunter (used for Curl) if(WIN32) set(DEPTHAI_CURL_USE_SCHANNEL ON) @@ -62,7 +59,7 @@ if(WIN32) endif() # Create depthai project -project(depthai VERSION "2.28.0" LANGUAGES CXX C) +project(depthai VERSION "2.29.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 a493b37..e88448c 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 "9ed7c9ae4c232ff93a3500a585a6b1c00650e22c") +set(DEPTHAI_DEVICE_SIDE_COMMIT "4d360b5c56225f23e9a3d3a3999ce46c90cfdeaf") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index f1455ea..9b4649a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -134,6 +134,14 @@ hunter_private_data( LOCATION tiny_yolo_v4_blob ) +# YoloV8 resource +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/yolov8n_coco_640x352.blob" + SHA1 "6b88a80ffb4267e253a89cb2b3e4e186453431b0" + FILE "yolov8n_coco_640x352.blob" + LOCATION yolov8n_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.4_5shave.blob" @@ -220,6 +228,7 @@ hunter_private_data( LOCATION normalization_model ) + ## message(STATUS "Location of test1.data: ${test1_data}") # bootloader @@ -380,8 +389,10 @@ dai_add_example(mjpeg_encoding VideoEncoder/mjpeg_encoding_example.cpp ON OFF) # Yolo dai_add_example(tiny_yolo_v4_device_side_decoding Yolo/tiny_yolo.cpp ON OFF) dai_add_example(tiny_yolo_v3_device_side_decoding Yolo/tiny_yolo.cpp ON OFF) +dai_add_example(yolo_v8_nano_device_side_decoding Yolo/yolov8_nano.cpp ON OFF) target_compile_definitions(tiny_yolo_v4_device_side_decoding PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}") target_compile_definitions(tiny_yolo_v3_device_side_decoding PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}") +target_compile_definitions(yolo_v8_nano_device_side_decoding PRIVATE BLOB_PATH="${yolov8n_blob}") dai_add_example(apriltag AprilTag/apriltag.cpp ON OFF) dai_add_example(apriltag_rgb AprilTag/apriltag_rgb.cpp ON OFF) @@ -421,3 +432,4 @@ target_compile_definitions(cast_diff PRIVATE BLOB_PATH="${diff_model}") dai_add_example(spatial_tiny_yolo_tof_v3 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF) dai_add_example(spatial_tiny_yolo_tof_v4 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF) target_compile_definitions(spatial_tiny_yolo_tof_v3 PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}") +target_compile_definitions(spatial_tiny_yolo_tof_v4 PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}") diff --git a/examples/Yolo/yolov8_nano.cpp b/examples/Yolo/yolov8_nano.cpp new file mode 100644 index 0000000..1268e1a --- /dev/null +++ b/examples/Yolo/yolov8_nano.cpp @@ -0,0 +1,157 @@ +#include +#include + +// Includes common necessary includes for development using depthai library +#include "depthai/depthai.hpp" + +/* +The code is the same as for Tiny-yolo-V3 and V4, the only difference is the blob file and anchors that are not needed for V8. +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(640, 352); + 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->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; + + uint32_t 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/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index b9a9355..3e44e63 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -758,6 +758,23 @@ class DeviceBase { */ void flashCalibration2(CalibrationHandler calibrationDataHandler); + /** + * Sets the Calibration at runtime. This is not persistent and will be lost after device reset. + * + * @throws std::runtime_exception if failed to set the calibration + * @param calibrationObj CalibrationHandler object which is loaded with calibration information. + * + */ + void setCalibration(CalibrationHandler calibrationDataHandler); + + /** + * Retrieves the CalibrationHandler object containing the non-persistent calibration + * + * @throws std::runtime_exception if failed to get the calibration + * @returns The CalibrationHandler object containing the non-persistent calibration + */ + CalibrationHandler getCalibration(); + /** * Fetches the EEPROM data from the device and loads it into CalibrationHandler object * If no calibration is flashed, it returns default diff --git a/include/depthai/pipeline/datatype/EncodedFrame.hpp b/include/depthai/pipeline/datatype/EncodedFrame.hpp index 13d5b10..0244d12 100644 --- a/include/depthai/pipeline/datatype/EncodedFrame.hpp +++ b/include/depthai/pipeline/datatype/EncodedFrame.hpp @@ -36,6 +36,15 @@ class EncodedFrame : public Buffer { * Retrieves instance number */ unsigned int getInstanceNum() const; + /** + * Retrieves image width in pixels + */ + unsigned int getWidth() const; + + /** + * Retrieves image height in pixels + */ + unsigned int getHeight() const; /** * Retrieves exposure time */ @@ -111,6 +120,20 @@ class EncodedFrame : public Buffer { */ EncodedFrame& setInstanceNum(unsigned int instance); + /** + * Specifies frame width + * + * @param width frame width + */ + EncodedFrame& setWidth(unsigned int width); + + /** + * Specifies frame height + * + * @param height frame height + */ + EncodedFrame& setHeight(unsigned int height); + /** * Specifies the encoding quality * diff --git a/include/depthai/pipeline/node/StereoDepth.hpp b/include/depthai/pipeline/node/StereoDepth.hpp index 8976e2b..2b64c9e 100644 --- a/include/depthai/pipeline/node/StereoDepth.hpp +++ b/include/depthai/pipeline/node/StereoDepth.hpp @@ -23,11 +23,16 @@ class StereoDepth : public NodeCRTP { /** * Prefers accuracy over density. More invalid depth values, but less outliers. */ - HIGH_ACCURACY, + HIGH_ACCURACY [[deprecated("Will be removed in future releases and replaced with DEFAULT")]], /** * Prefers density over accuracy. Less invalid depth values, but more outliers. */ - HIGH_DENSITY + HIGH_DENSITY [[deprecated("Will be removed in future releases and replaced with DEFAULT")]], + + DEFAULT, + FACE, + HIGH_DETAIL, + ROBOTICS }; protected: diff --git a/package.xml b/package.xml index 88017ec..87f54bf 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ depthai - 2.28.0 + 2.29.0 DepthAI core is a C++ library which comes with firmware and an API to interact with OAK Platform Adam Serafin diff --git a/shared/depthai-shared/include/depthai-shared/datatype/RawCameraControl.hpp b/shared/depthai-shared/include/depthai-shared/datatype/RawCameraControl.hpp index 68febb5..37d5243 100644 --- a/shared/depthai-shared/include/depthai-shared/datatype/RawCameraControl.hpp +++ b/shared/depthai-shared/include/depthai-shared/datatype/RawCameraControl.hpp @@ -437,6 +437,7 @@ struct RawCameraControl : public RawBuffer { uint16_t wbColorTemp; // 1000 .. 12000 uint8_t lowPowerNumFramesBurst; uint8_t lowPowerNumFramesDiscard; + std::vector> miscControls; void setCommand(Command cmd, bool value = true) { uint64_t mask = 1ull << (uint8_t)cmd; @@ -493,7 +494,8 @@ struct RawCameraControl : public RawBuffer { chromaDenoise, wbColorTemp, lowPowerNumFramesBurst, - lowPowerNumFramesDiscard); + lowPowerNumFramesDiscard, + miscControls); }; } // namespace dai diff --git a/shared/depthai-shared/include/depthai-shared/datatype/RawEncodedFrame.hpp b/shared/depthai-shared/include/depthai-shared/datatype/RawEncodedFrame.hpp index d3a934e..59e84f0 100644 --- a/shared/depthai-shared/include/depthai-shared/datatype/RawEncodedFrame.hpp +++ b/shared/depthai-shared/include/depthai-shared/datatype/RawEncodedFrame.hpp @@ -23,6 +23,9 @@ struct RawEncodedFrame : public RawBuffer { CameraSettings cam; uint32_t instanceNum = 0; // Which source created this frame (color, mono, ...) + unsigned int width; // width in pixels + unsigned int height; // height in pixels + uint32_t quality; uint32_t bitrate; Profile profile; diff --git a/shared/depthai-shared/include/depthai-shared/datatype/RawStereoDepthConfig.hpp b/shared/depthai-shared/include/depthai-shared/datatype/RawStereoDepthConfig.hpp index e8a9e36..06168aa 100644 --- a/shared/depthai-shared/include/depthai-shared/datatype/RawStereoDepthConfig.hpp +++ b/shared/depthai-shared/include/depthai-shared/datatype/RawStereoDepthConfig.hpp @@ -1,7 +1,7 @@ #pragma once +#include #include #include -#include #include "depthai-shared/common/MedianFilter.hpp" #include "depthai-shared/datatype/DatatypeEnum.hpp" @@ -132,6 +132,13 @@ struct RawStereoDepthConfig : public RawBuffer { * Post-processing filters, all the filters are applied in disparity domain. */ struct PostProcessing { + enum class Filter : int32_t { NONE = 0, DECIMATION, SPECKLE, MEDIAN, SPATIAL, TEMPORAL, FILTER_COUNT = TEMPORAL }; + + /** + * Order of filters to be applied if filtering is enabled. + */ + std::array filteringOrder = {Filter::MEDIAN, Filter::DECIMATION, Filter::SPECKLE, Filter::SPATIAL, Filter::TEMPORAL}; + /** * Set kernel size for disparity/depth median filtering, or disable */ @@ -312,7 +319,13 @@ struct RawStereoDepthConfig : public RawBuffer { */ std::uint32_t speckleRange = 50; - DEPTHAI_SERIALIZE(SpeckleFilter, enable, speckleRange); + /** + * Maximum difference between neighbor disparity pixels to put them into the same blob. + * Units in disparity integer levels. + */ + std::uint32_t differenceThreshold = 2; + + DEPTHAI_SERIALIZE(SpeckleFilter, enable, speckleRange, differenceThreshold); }; /** @@ -354,8 +367,16 @@ struct RawStereoDepthConfig : public RawBuffer { */ DecimationFilter decimationFilter; - DEPTHAI_SERIALIZE( - PostProcessing, median, bilateralSigmaValue, spatialFilter, temporalFilter, thresholdFilter, brightnessFilter, speckleFilter, decimationFilter); + DEPTHAI_SERIALIZE(PostProcessing, + filteringOrder, + median, + bilateralSigmaValue, + spatialFilter, + temporalFilter, + thresholdFilter, + brightnessFilter, + speckleFilter, + decimationFilter); }; /** diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 3f81121..d47f994 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1349,6 +1349,26 @@ void DeviceBase::flashCalibration2(CalibrationHandler calibrationDataHandler) { } } +void DeviceBase::setCalibration(CalibrationHandler calibrationDataHandler) { + bool success; + std::string errorMsg; + std::tie(success, errorMsg) = pimpl->rpcClient->call("setCalibration", calibrationDataHandler.getEepromData()).as>(); + if(!success) { + throw std::runtime_error(errorMsg); + } +} + +CalibrationHandler DeviceBase::getCalibration() { + bool success; + std::string errorMsg; + dai::EepromData eepromData; + std::tie(success, errorMsg, eepromData) = pimpl->rpcClient->call("getCalibration").as>(); + if(!success) { + throw EepromError(errorMsg); + } + return CalibrationHandler(eepromData); +} + CalibrationHandler DeviceBase::readCalibration() { dai::EepromData eepromData{}; try { diff --git a/src/pipeline/datatype/EncodedFrame.cpp b/src/pipeline/datatype/EncodedFrame.cpp index a19d29b..626ca6f 100644 --- a/src/pipeline/datatype/EncodedFrame.cpp +++ b/src/pipeline/datatype/EncodedFrame.cpp @@ -18,6 +18,12 @@ EncodedFrame::EncodedFrame(std::shared_ptr ptr) : Buffer(std::m unsigned int EncodedFrame::getInstanceNum() const { return frame.instanceNum; } +unsigned int EncodedFrame::getWidth() const { + return frame.width; +} +unsigned int EncodedFrame::getHeight() const { + return frame.height; +} std::chrono::microseconds EncodedFrame::getExposureTime() const { return std::chrono::microseconds(frame.cam.exposureTimeUs); } @@ -99,6 +105,14 @@ EncodedFrame& EncodedFrame::setInstanceNum(unsigned int instanceNum) { frame.instanceNum = instanceNum; return *this; } +EncodedFrame& EncodedFrame::setWidth(unsigned int width) { + frame.width = width; + return *this; +} +EncodedFrame& EncodedFrame::setHeight(unsigned int height) { + frame.height = height; + return *this; +} EncodedFrame& EncodedFrame::setQuality(unsigned int quality) { frame.quality = quality; return *this; diff --git a/src/pipeline/datatype/StereoDepthConfig.cpp b/src/pipeline/datatype/StereoDepthConfig.cpp index d7715b7..1bbff1f 100644 --- a/src/pipeline/datatype/StereoDepthConfig.cpp +++ b/src/pipeline/datatype/StereoDepthConfig.cpp @@ -99,6 +99,48 @@ float StereoDepthConfig::getMaxDisparity() const { maxDisp += cfg.algorithmControl.disparityShift; if(cfg.algorithmControl.enableExtended) maxDisp *= 2; if(cfg.algorithmControl.enableSubpixel) maxDisp *= (1 << cfg.algorithmControl.subpixelFractionalBits); + + std::vector filtersToExecute; + for(auto filter : cfg.postProcessing.filteringOrder) { + switch(filter) { + case RawStereoDepthConfig::PostProcessing::Filter::SPECKLE: + if(cfg.postProcessing.speckleFilter.enable) { + filtersToExecute.push_back(filter); + } + break; + case RawStereoDepthConfig::PostProcessing::Filter::TEMPORAL: + if(cfg.postProcessing.temporalFilter.enable) { + filtersToExecute.push_back(filter); + } + break; + case RawStereoDepthConfig::PostProcessing::Filter::SPATIAL: + if(cfg.postProcessing.spatialFilter.enable) { + filtersToExecute.push_back(filter); + } + break; + case RawStereoDepthConfig::PostProcessing::Filter::DECIMATION: + if(cfg.postProcessing.decimationFilter.decimationFactor > 1) { + filtersToExecute.push_back(filter); + } + break; + case RawStereoDepthConfig::PostProcessing::Filter::MEDIAN: + if(cfg.postProcessing.median != dai::MedianFilter::MEDIAN_OFF) { + filtersToExecute.push_back(filter); + } + break; + case RawStereoDepthConfig::PostProcessing::Filter::NONE: + break; + default: + break; + } + } + + if(filtersToExecute.size() != 0) { + if(filtersToExecute.back() != RawStereoDepthConfig::PostProcessing::Filter::MEDIAN) { + maxDisp = 1 << 13; + } + } + return maxDisp; } diff --git a/src/pipeline/node/StereoDepth.cpp b/src/pipeline/node/StereoDepth.cpp index fba8d4e..2cbe6ca 100644 --- a/src/pipeline/node/StereoDepth.cpp +++ b/src/pipeline/node/StereoDepth.cpp @@ -216,6 +216,162 @@ void StereoDepth::setDefaultProfilePreset(PresetMode mode) { initialConfig.setLeftRightCheck(true); initialConfig.setLeftRightCheckThreshold(10); } break; + case PresetMode::DEFAULT: { + setDefaultProfilePreset(PresetMode::HIGH_DENSITY); + initialConfig.setLeftRightCheck(true); + initialConfig.setExtendedDisparity(false); + initialConfig.setSubpixel(true); + initialConfig.setSubpixelFractionalBits(3); + initialConfig.setMedianFilter(MedianFilter::KERNEL_7x7); + + dai::RawStereoDepthConfig config = initialConfig.get(); + + config.postProcessing.filteringOrder = {RawStereoDepthConfig::PostProcessing::Filter::DECIMATION, + RawStereoDepthConfig::PostProcessing::Filter::MEDIAN, + RawStereoDepthConfig::PostProcessing::Filter::SPECKLE, + RawStereoDepthConfig::PostProcessing::Filter::SPATIAL, + RawStereoDepthConfig::PostProcessing::Filter::TEMPORAL}; + config.postProcessing.decimationFilter.decimationFactor = 2; + config.postProcessing.decimationFilter.decimationMode = RawStereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode::PIXEL_SKIPPING; + + config.postProcessing.spatialFilter.enable = true; + config.postProcessing.spatialFilter.holeFillingRadius = 1; + config.postProcessing.spatialFilter.numIterations = 1; + config.postProcessing.spatialFilter.alpha = 0.5; + config.postProcessing.spatialFilter.delta = 3; + + config.postProcessing.temporalFilter.enable = true; + config.postProcessing.temporalFilter.alpha = 0.5; + config.postProcessing.temporalFilter.delta = 3; + + config.postProcessing.speckleFilter.enable = true; + config.postProcessing.speckleFilter.speckleRange = 200; + config.postProcessing.speckleFilter.differenceThreshold = 2; + + config.postProcessing.thresholdFilter.minRange = 0; + config.postProcessing.thresholdFilter.maxRange = 15000; + + initialConfig.set(config); + + setPostProcessingHardwareResources(3, 3); + } break; + case PresetMode::FACE: { + setDefaultProfilePreset(PresetMode::HIGH_DENSITY); + initialConfig.setLeftRightCheck(true); + initialConfig.setExtendedDisparity(true); + initialConfig.setSubpixel(true); + initialConfig.setSubpixelFractionalBits(5); + initialConfig.setMedianFilter(MedianFilter::MEDIAN_OFF); + + dai::RawStereoDepthConfig config = initialConfig.get(); + + config.postProcessing.filteringOrder = {RawStereoDepthConfig::PostProcessing::Filter::DECIMATION, + RawStereoDepthConfig::PostProcessing::Filter::MEDIAN, + RawStereoDepthConfig::PostProcessing::Filter::SPECKLE, + RawStereoDepthConfig::PostProcessing::Filter::SPATIAL, + RawStereoDepthConfig::PostProcessing::Filter::TEMPORAL}; + config.postProcessing.decimationFilter.decimationFactor = 2; + config.postProcessing.decimationFilter.decimationMode = RawStereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode::PIXEL_SKIPPING; + + config.postProcessing.spatialFilter.enable = true; + config.postProcessing.spatialFilter.holeFillingRadius = 1; + config.postProcessing.spatialFilter.numIterations = 1; + config.postProcessing.spatialFilter.alpha = 0.5; + config.postProcessing.spatialFilter.delta = 3; + + config.postProcessing.temporalFilter.enable = true; + config.postProcessing.temporalFilter.alpha = 0.5; + config.postProcessing.temporalFilter.delta = 3; + + config.postProcessing.speckleFilter.enable = true; + config.postProcessing.speckleFilter.speckleRange = 200; + config.postProcessing.speckleFilter.differenceThreshold = 2; + + config.postProcessing.thresholdFilter.minRange = 30; + config.postProcessing.thresholdFilter.maxRange = 3000; + + initialConfig.set(config); + + setPostProcessingHardwareResources(3, 3); + } break; + case PresetMode::HIGH_DETAIL: { + setDefaultProfilePreset(PresetMode::HIGH_ACCURACY); + initialConfig.setLeftRightCheck(true); + initialConfig.setExtendedDisparity(true); + initialConfig.setSubpixel(true); + initialConfig.setSubpixelFractionalBits(5); + initialConfig.setMedianFilter(MedianFilter::MEDIAN_OFF); + + dai::RawStereoDepthConfig config = initialConfig.get(); + + config.postProcessing.filteringOrder = {RawStereoDepthConfig::PostProcessing::Filter::DECIMATION, + RawStereoDepthConfig::PostProcessing::Filter::MEDIAN, + RawStereoDepthConfig::PostProcessing::Filter::SPECKLE, + RawStereoDepthConfig::PostProcessing::Filter::SPATIAL, + RawStereoDepthConfig::PostProcessing::Filter::TEMPORAL}; + config.postProcessing.decimationFilter.decimationFactor = 2; + config.postProcessing.decimationFilter.decimationMode = RawStereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode::PIXEL_SKIPPING; + + config.postProcessing.spatialFilter.enable = true; + config.postProcessing.spatialFilter.holeFillingRadius = 1; + config.postProcessing.spatialFilter.numIterations = 1; + config.postProcessing.spatialFilter.alpha = 0.5; + config.postProcessing.spatialFilter.delta = 3; + + config.postProcessing.temporalFilter.enable = true; + config.postProcessing.temporalFilter.alpha = 0.5; + config.postProcessing.temporalFilter.delta = 3; + + config.postProcessing.speckleFilter.enable = true; + config.postProcessing.speckleFilter.speckleRange = 200; + config.postProcessing.speckleFilter.differenceThreshold = 2; + + config.postProcessing.thresholdFilter.minRange = 0; + config.postProcessing.thresholdFilter.maxRange = 15000; + + initialConfig.set(config); + + setPostProcessingHardwareResources(3, 3); + } break; + case PresetMode::ROBOTICS: { + setDefaultProfilePreset(PresetMode::HIGH_DENSITY); + initialConfig.setLeftRightCheck(true); + initialConfig.setExtendedDisparity(false); + initialConfig.setSubpixel(true); + initialConfig.setSubpixelFractionalBits(3); + initialConfig.setMedianFilter(MedianFilter::KERNEL_7x7); + + dai::RawStereoDepthConfig config = initialConfig.get(); + + config.postProcessing.filteringOrder = {RawStereoDepthConfig::PostProcessing::Filter::DECIMATION, + RawStereoDepthConfig::PostProcessing::Filter::MEDIAN, + RawStereoDepthConfig::PostProcessing::Filter::SPECKLE, + RawStereoDepthConfig::PostProcessing::Filter::SPATIAL, + RawStereoDepthConfig::PostProcessing::Filter::TEMPORAL}; + config.postProcessing.decimationFilter.decimationFactor = 2; + config.postProcessing.decimationFilter.decimationMode = RawStereoDepthConfig::PostProcessing::DecimationFilter::DecimationMode::PIXEL_SKIPPING; + + config.postProcessing.spatialFilter.enable = true; + config.postProcessing.spatialFilter.holeFillingRadius = 2; + config.postProcessing.spatialFilter.numIterations = 1; + config.postProcessing.spatialFilter.alpha = 0.5; + config.postProcessing.spatialFilter.delta = 20; + + config.postProcessing.temporalFilter.enable = false; + config.postProcessing.temporalFilter.alpha = 0.5; + config.postProcessing.temporalFilter.delta = 3; + + config.postProcessing.speckleFilter.enable = true; + config.postProcessing.speckleFilter.speckleRange = 200; + config.postProcessing.speckleFilter.differenceThreshold = 2; + + config.postProcessing.thresholdFilter.minRange = 0; + config.postProcessing.thresholdFilter.maxRange = 10000; + + initialConfig.set(config); + + setPostProcessingHardwareResources(3, 3); + } break; } } diff --git a/tests/src/stability_stress_test.cpp b/tests/src/stability_stress_test.cpp index 5add128..e4a8540 100644 --- a/tests/src/stability_stress_test.cpp +++ b/tests/src/stability_stress_test.cpp @@ -340,9 +340,7 @@ int main(int argc, char** argv) { unique_lock l(countersMtx); bool failed = counters.size() == 0; - cout << "[" << duration_cast(steady_clock::now() - timeoutStopwatch).count() << "s] " - << "Usb speed " << usb_speed << " " - << "FPS: "; + cout << "[" << duration_cast(steady_clock::now() - timeoutStopwatch).count() << "s] " << "Usb speed " << usb_speed << " " << "FPS: "; for(const auto& kv : counters) { if(kv.second == 0) { failed = true;