diff --git a/.github/workflows/main.workflow.yml b/.github/workflows/main.workflow.yml index eec36eba8..4560feb6b 100644 --- a/.github/workflows/main.workflow.yml +++ b/.github/workflows/main.workflow.yml @@ -67,6 +67,8 @@ jobs: exclude: - os: windows-latest cmake: '3.10.x' + - os: macos-latest # Skip the old cmake on latest macos - doesn't handle ARM64 aarch correctly + cmake: '3.10.x' steps: - name: Cache .hunter folder @@ -333,4 +335,4 @@ jobs: upload_url: ${{ steps.createRelease.outputs.upload_url }} asset_path: ${{ github.workspace }}/depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv.zip asset_name: depthai-core-${{ steps.tag.outputs.version }}-win32-no-opencv.zip - asset_content_type: application/octet-stream \ No newline at end of file + asset_content_type: application/octet-stream diff --git a/CMakeLists.txt b/CMakeLists.txt index 405672194..c68a04c96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ if(WIN32) endif() # Create depthai project -project(depthai VERSION "2.25.1" LANGUAGES CXX C) +project(depthai VERSION "2.26.0" LANGUAGES CXX C) get_directory_property(has_parent PARENT_DIRECTORY) if(has_parent) set(DEPTHAI_VERSION ${PROJECT_VERSION} PARENT_SCOPE) @@ -219,6 +219,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/DetectionNetwork.cpp src/pipeline/node/Script.cpp src/pipeline/node/SpatialDetectionNetwork.cpp + src/pipeline/node/ImageAlign.cpp src/pipeline/node/SystemLogger.cpp src/pipeline/node/SpatialLocationCalculator.cpp src/pipeline/node/AprilTag.cpp @@ -230,6 +231,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/node/DetectionParser.cpp src/pipeline/node/UVC.cpp src/pipeline/node/PointCloud.cpp + src/pipeline/node/Cast.cpp src/pipeline/datatype/Buffer.cpp src/pipeline/datatype/ImgFrame.cpp src/pipeline/datatype/EncodedFrame.cpp @@ -250,6 +252,7 @@ add_library(${TARGET_CORE_NAME} src/pipeline/datatype/EdgeDetectorConfig.cpp src/pipeline/datatype/TrackedFeatures.cpp src/pipeline/datatype/FeatureTrackerConfig.cpp + src/pipeline/datatype/ImageAlignConfig.cpp src/pipeline/datatype/ToFConfig.cpp src/pipeline/datatype/PointCloudConfig.cpp src/pipeline/datatype/PointCloudData.cpp diff --git a/cmake/Depthai/DepthaiBootloaderConfig.cmake b/cmake/Depthai/DepthaiBootloaderConfig.cmake index 33809726c..3f1a15d1a 100644 --- a/cmake/Depthai/DepthaiBootloaderConfig.cmake +++ b/cmake/Depthai/DepthaiBootloaderConfig.cmake @@ -3,5 +3,5 @@ set(DEPTHAI_BOOTLOADER_MATURITY "release") # set(DEPTHAI_BOOTLOADER_MATURITY "snapshot") # "version if applicable" -set(DEPTHAI_BOOTLOADER_VERSION "0.0.27") -# set(DEPTHAI_BOOTLOADER_VERSION "0.0.24+57c26493754e2f00e57f6594b0b1a317f762d5f2") +set(DEPTHAI_BOOTLOADER_VERSION "0.0.28") +# set(DEPTHAI_BOOTLOADER_VERSION "0.0.27+5fb331f993adceeeda72202c233a9e3939ab3dab") diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake index 8375def30..ddf90cb8b 100644 --- a/cmake/Depthai/DepthaiDeviceSideConfig.cmake +++ b/cmake/Depthai/DepthaiDeviceSideConfig.cmake @@ -2,7 +2,7 @@ set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot") # "full commit hash of device side binary" -set(DEPTHAI_DEVICE_SIDE_COMMIT "c7127782f2da45aac89d5b5b816d04cc45ae40be") +set(DEPTHAI_DEVICE_SIDE_COMMIT "24a3b465b979de3f69410cd225914d8bd029f3ba") # "version if applicable" set(DEPTHAI_DEVICE_SIDE_VERSION "") diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index c9ce59876..f3c0762b8 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -196,6 +196,22 @@ hunter_private_data( LOCATION concat_model ) +# blur model +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/blur_simplified_openvino_2021.4_6shave.blob" + SHA1 "14d543bbaceffa438071f83be58ad22a07ce33ee" + FILE "blur_simplified_openvino_2021.4_6shave.blob" + LOCATION blur_model +) + +# diff model +hunter_private_data( + URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/diff_openvino_2022.1_6shave.blob" + SHA1 "fe9600e617d222f986a699f18e77e80ce2485000" + FILE "diff_openvino_2022.1_6shave.blob" + LOCATION diff_model +) + # normalization model hunter_private_data( URL "https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/network/normalize_openvino_2021.4_4shave.blob" @@ -386,3 +402,23 @@ dai_add_example(imu_video_synced Sync/imu_video_synced.cpp ON OFF) if(DEPTHAI_HAVE_PCL_SUPPORT) dai_add_example(visualize_pointcloud PointCloud/visualize_pointcloud.cpp ON ON) endif() + +# ImageAlign +dai_add_example(tof_align ImageAlign/tof_align.cpp OFF OFF) +dai_add_example(image_align ImageAlign/image_align.cpp ON OFF) +dai_add_example(thermal_align ImageAlign/thermal_align.cpp OFF OFF) +dai_add_example(depth_align ImageAlign/depth_align.cpp ON OFF) + +# Cast +dai_add_example(blur Cast/blur.cpp ON OFF) +target_compile_definitions(blur PRIVATE BLOB_PATH="${blur_model}") +dai_add_example(concat Cast/concat.cpp ON OFF) +target_compile_definitions(concat PRIVATE BLOB_PATH="${concat_model}") +dai_add_example(diff Cast/diff.cpp ON OFF) +target_compile_definitions(diff PRIVATE BLOB_PATH="${diff_model}") + +# ToF +dai_add_example(spatial_tiny_yolo_tof_v3 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF) +dai_add_example(spatial_tiny_yolo_tof_v4 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF) +target_compile_definitions(spatial_tiny_yolo_tof_v3 PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}") +target_compile_definitions(spatial_tiny_yolo_tof_v4 PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}") \ No newline at end of file diff --git a/examples/Camera/thermal_cam.cpp b/examples/Camera/thermal_cam.cpp index 8178655e8..2011dff5a 100644 --- a/examples/Camera/thermal_cam.cpp +++ b/examples/Camera/thermal_cam.cpp @@ -22,17 +22,18 @@ int main() { // Find the sensor width, height. int width, height; bool thermal_found = false; - for (auto &features : d.getConnectedCameraFeatures()) { - if (std::find_if(features.supportedTypes.begin(), features.supportedTypes.end(), [](const dai::CameraSensorType &type) { - return type == dai::CameraSensorType::THERMAL; - }) != features.supportedTypes.end()) { - thermal->setBoardSocket(features.socket); // Thermal will always be on CAM_E + for(auto& features : d.getConnectedCameraFeatures()) { + if(std::find_if(features.supportedTypes.begin(), + features.supportedTypes.end(), + [](const dai::CameraSensorType& type) { return type == dai::CameraSensorType::THERMAL; }) + != features.supportedTypes.end()) { + thermal->setBoardSocket(features.socket); // Thermal will always be on CAM_E width = features.width; height = features.height; thermal_found = true; } } - if (!thermal_found) { + if(!thermal_found) { throw std::runtime_error("Thermal camera not found!"); } thermal->setPreviewSize(width, height); diff --git a/examples/Cast/blur.cpp b/examples/Cast/blur.cpp new file mode 100644 index 000000000..8c762a2d0 --- /dev/null +++ b/examples/Cast/blur.cpp @@ -0,0 +1,50 @@ +#include +#include + +constexpr int SHAPE = 300; + +int main() { + dai::Pipeline p; + + auto camRgb = p.create(); + auto nn = p.create(); + auto rgbOut = p.create(); + auto cast = p.create(); + auto castXout = p.create(); + + camRgb->setPreviewSize(SHAPE, SHAPE); + camRgb->setInterleaved(false); + + nn->setBlobPath(BLOB_PATH); + + rgbOut->setStreamName("rgb"); + castXout->setStreamName("cast"); + + cast->setOutputFrameType(dai::ImgFrame::Type::BGR888p); + + // Linking + camRgb->preview.link(nn->input); + camRgb->preview.link(rgbOut->input); + nn->out.link(cast->input); + cast->output.link(castXout->input); + + dai::Device device(p); + auto qCam = device.getOutputQueue("rgb", 4, false); + auto qCast = device.getOutputQueue("cast", 4, false); + + while(true) { + auto inCast = qCast->get(); + auto inRgb = qCam->get(); + + if(inCast && inRgb) { + cv::imshow("Blur", inCast->getCvFrame()); + cv::imshow("Original", inRgb->getCvFrame()); + } + + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/examples/Cast/concat.cpp b/examples/Cast/concat.cpp new file mode 100644 index 000000000..7168ac4c3 --- /dev/null +++ b/examples/Cast/concat.cpp @@ -0,0 +1,63 @@ +#include +#include + +constexpr int SHAPE = 300; + +int main() { + dai::Pipeline p; + + auto camRgb = p.create(); + auto left = p.create(); + auto right = p.create(); + auto manipLeft = p.create(); + auto manipRight = p.create(); + auto nn = p.create(); + auto cast = p.create(); + auto castXout = p.create(); + + camRgb->setPreviewSize(SHAPE, SHAPE); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + + left->setCamera("left"); + left->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + manipLeft->initialConfig.setResize(SHAPE, SHAPE); + manipLeft->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + + right->setCamera("right"); + right->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P); + manipRight->initialConfig.setResize(SHAPE, SHAPE); + manipRight->initialConfig.setFrameType(dai::ImgFrame::Type::BGR888p); + + nn->setBlobPath(BLOB_PATH); + nn->setNumInferenceThreads(2); + + castXout->setStreamName("cast"); + cast->setOutputFrameType(dai::ImgFrame::Type::BGR888p); + + // Linking + left->out.link(manipLeft->inputImage); + right->out.link(manipRight->inputImage); + manipLeft->out.link(nn->inputs["img1"]); + camRgb->preview.link(nn->inputs["img2"]); + manipRight->out.link(nn->inputs["img3"]); + nn->out.link(cast->input); + cast->output.link(castXout->input); + + // Pipeline is defined, now we can connect to the device + dai::Device device(p); + auto qCast = device.getOutputQueue("cast", 4, false); + + while(true) { + auto inCast = qCast->get(); + if(inCast) { + cv::imshow("Concated frames", inCast->getCvFrame()); + } + + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/examples/Cast/diff.cpp b/examples/Cast/diff.cpp new file mode 100644 index 000000000..78960beaf --- /dev/null +++ b/examples/Cast/diff.cpp @@ -0,0 +1,66 @@ +#include +#include +#include + +constexpr int SHAPE = 720; + +int main() { + dai::Pipeline p; + + auto camRgb = p.create(); + auto nn = p.create(); + auto script = p.create(); + auto rgbXout = p.create(); + auto cast = p.create(); + auto castXout = p.create(); + + camRgb->setVideoSize(SHAPE, SHAPE); + camRgb->setPreviewSize(SHAPE, SHAPE); + camRgb->setInterleaved(false); + + nn->setBlobPath(BLOB_PATH); + + script->setScript(R"( + old = node.io['in'].get() + while True: + frame = node.io['in'].get() + node.io['img1'].send(old) + node.io['img2'].send(frame) + old = frame + )"); + + rgbXout->setStreamName("rgb"); + castXout->setStreamName("cast"); + cast->setOutputFrameType(dai::RawImgFrame::Type::GRAY8); + + // Linking + camRgb->preview.link(script->inputs["in"]); + script->outputs["img1"].link(nn->inputs["img1"]); + script->outputs["img2"].link(nn->inputs["img2"]); + camRgb->video.link(rgbXout->input); + nn->out.link(cast->input); + cast->output.link(castXout->input); + + // Pipeline is defined, now we can connect to the device + dai::Device device(p); + auto qCam = device.getOutputQueue("rgb", 4, false); + auto qCast = device.getOutputQueue("cast", 4, false); + + while(true) { + auto colorFrame = qCam->get(); + if(colorFrame) { + cv::imshow("Color", colorFrame->getCvFrame()); + } + + auto inCast = qCast->get(); + if(inCast) { + cv::imshow("Diff", inCast->getCvFrame()); + } + + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} \ No newline at end of file diff --git a/examples/FeatureTracker/feature_tracker.cpp b/examples/FeatureTracker/feature_tracker.cpp index e7b688158..887923af4 100644 --- a/examples/FeatureTracker/feature_tracker.cpp +++ b/examples/FeatureTracker/feature_tracker.cpp @@ -16,7 +16,7 @@ class FeatureTrackerDrawer { // for how many frames the feature is tracked static int trackedFeaturesPathLength; - using featureIdType = decltype(dai::Point2f::x); + using featureIdType = decltype(dai::TrackedFeature::id); std::unordered_set trackedIDs; std::unordered_map> trackedFeaturesPath; diff --git a/examples/ImageAlign/depth_align.cpp b/examples/ImageAlign/depth_align.cpp new file mode 100644 index 000000000..3a94d5ad1 --- /dev/null +++ b/examples/ImageAlign/depth_align.cpp @@ -0,0 +1,148 @@ +#include +#include +#include + +#include "depthai/depthai.hpp" + +constexpr auto FPS = 30.0; +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto LEFT_SOCKET = dai::CameraBoardSocket::CAM_B; +constexpr auto RIGHT_SOCKET = dai::CameraBoardSocket::CAM_C; +constexpr auto ALIGN_SOCKET = LEFT_SOCKET; + +constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; +constexpr auto LEFT_RIGHT_RESOLUTION = dai::MonoCameraProperties::SensorResolution::THE_400_P; +constexpr auto ISP_SCALE = 3; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 10) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +double rgbWeight = 0.4; +double depthWeight = 0.6; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + depthWeight = 1.0 - rgbWeight; +} + +cv::Mat colorizeDepth(const cv::Mat& frameDepth) { + cv::Mat invalidMask = (frameDepth == 0); + cv::Mat depthFrameColor; + try { + double minDepth = 0.0; + double maxDepth = 0.0; + cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); + if(minDepth == maxDepth) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + return depthFrameColor; + } + cv::Mat logDepth; + frameDepth.convertTo(logDepth, CV_32F); + cv::log(logDepth, logDepth); + logDepth.setTo(log(minDepth), invalidMask); + cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); + depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); + } catch(const std::exception& e) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + } + return depthFrameColor; +} + +int main() { + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); + auto stereo = pipeline.create(); + auto sync = pipeline.create(); + auto out = pipeline.create(); + auto align = pipeline.create(); + + left->setResolution(LEFT_RIGHT_RESOLUTION); + left->setBoardSocket(LEFT_SOCKET); + left->setFps(FPS); + + right->setResolution(LEFT_RIGHT_RESOLUTION); + right->setBoardSocket(RIGHT_SOCKET); + right->setFps(FPS); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(COLOR_RESOLUTION); + camRgb->setFps(FPS); + camRgb->setIspScale(1, ISP_SCALE); + + stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); + stereo->setDepthAlign(LEFT_SOCKET); + + out->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); + + // Linking + camRgb->isp.link(sync->inputs["rgb"]); + left->out.link(stereo->left); + right->out.link(stereo->right); + stereo->depth.link(align->input); + align->outputAligned.link(sync->inputs["depth_aligned"]); + camRgb->isp.link(align->inputAlignTo); + sync->out.link(out->input); + + dai::Device device(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + + FPSCounter fpsCounter; + + std::string windowName = "rgb-depth"; + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, 1280, 720); + cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto frameDepth = messageGroup->get("depth_aligned"); + + if(frameRgb && frameDepth) { + auto cvFrame = frameRgb->getCvFrame(); + + // Colorize the aligned depth + auto alignedDepthColorized = colorizeDepth(frameDepth->getCvFrame()); + + cv::imshow("Depth aligned", alignedDepthColorized); + + cv::Mat blended; + cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended); + cv::putText(blended, "FPS: " + std::to_string(fpsCounter.getFps()), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); + cv::imshow(windowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + + return 0; +} diff --git a/examples/ImageAlign/image_align.cpp b/examples/ImageAlign/image_align.cpp new file mode 100644 index 000000000..a676d13a0 --- /dev/null +++ b/examples/ImageAlign/image_align.cpp @@ -0,0 +1,135 @@ +#include +#include +#include + +#include "depthai/depthai.hpp" + +constexpr auto FPS = 30.0; + +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto LEFT_SOCKET = dai::CameraBoardSocket::CAM_B; +constexpr auto RIGHT_SOCKET = dai::CameraBoardSocket::CAM_C; +constexpr auto ALIGN_SOCKET = LEFT_SOCKET; + +constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; +constexpr auto LEFT_RIGHT_RESOLUTION = dai::MonoCameraProperties::SensorResolution::THE_720_P; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 100) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +double rgbWeight = 0.4; +double depthWeight = 0.6; +int staticDepthPlane = 0; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + depthWeight = 1.0 - rgbWeight; +} + +void updateDepthPlane(int depth, void*) { + staticDepthPlane = depth; +} + +int main() { + dai::Pipeline pipeline; + + auto camRgb = pipeline.create(); + auto left = pipeline.create(); + auto right = pipeline.create(); + auto sync = pipeline.create(); + auto out = pipeline.create(); + auto align = pipeline.create(); + auto cfgIn = pipeline.create(); + + left->setResolution(LEFT_RIGHT_RESOLUTION); + left->setBoardSocket(LEFT_SOCKET); + left->setFps(FPS); + + right->setResolution(LEFT_RIGHT_RESOLUTION); + right->setBoardSocket(RIGHT_SOCKET); + right->setFps(FPS); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(COLOR_RESOLUTION); + camRgb->setFps(FPS); + camRgb->setIspScale(1, 3); + + out->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); + + cfgIn->setStreamName("config"); + + align->outputAligned.link(sync->inputs["aligned"]); + camRgb->isp.link(sync->inputs["rgb"]); + camRgb->isp.link(align->inputAlignTo); + left->out.link(align->input); + sync->out.link(out->input); + cfgIn->out.link(align->inputConfig); + + dai::Device device(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + auto cfgQ = device.getInputQueue("config"); + + FPSCounter fpsCounter; + + std::string windowName = "rgb-left"; + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, 1280, 720); + cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); + cv::createTrackbar("Static Depth Plane [mm]", windowName, nullptr, 2000, updateDepthPlane); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto leftAligned = messageGroup->get("aligned"); + + if(frameRgb && leftAligned) { + auto frameRgbCv = frameRgb->getCvFrame(); + auto leftCv = leftAligned->getCvFrame(); + + if(leftCv.channels() == 1) { + cv::cvtColor(leftCv, leftCv, cv::COLOR_GRAY2BGR); + } + if(leftCv.size() != frameRgbCv.size()) { + cv::resize(leftCv, leftCv, frameRgbCv.size()); + } + + cv::Mat blended; + cv::addWeighted(frameRgbCv, rgbWeight, leftCv, depthWeight, 0, blended); + cv::imshow(windowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + + auto cfg = align->initialConfig.get(); + cfg.staticDepthPlane = staticDepthPlane; + auto alignConfig = std::make_shared(); + alignConfig->set(cfg); + cfgQ->send(alignConfig); + } + + return 0; +} diff --git a/examples/ImageAlign/thermal_align.cpp b/examples/ImageAlign/thermal_align.cpp new file mode 100644 index 000000000..e30d2651e --- /dev/null +++ b/examples/ImageAlign/thermal_align.cpp @@ -0,0 +1,164 @@ +#include +#include +#include + +#include "depthai/depthai.hpp" +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" + +constexpr auto FPS = 25.0; +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto COLOR_RESOLUTION = dai::ColorCameraProperties::SensorResolution::THE_1080_P; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 100) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +double rgbWeight = 0.4; +double thermalWeight = 0.6; +int staticDepthPlane = 0; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + thermalWeight = 1.0 - rgbWeight; +} + +void updateDepthPlane(int depth, void*) { + staticDepthPlane = depth; +} + +cv::Mat createNaNMask(const cv::Mat& frame) { + cv::Mat nanMask = cv::Mat::zeros(frame.size(), CV_8UC1); + for(int r = 0; r < frame.rows; ++r) { + for(int c = 0; c < frame.cols; ++c) { + if(std::isnan(frame.at(r, c))) { + nanMask.at(r, c) = 255; + } + } + } + return nanMask; +} + +int main() { + dai::Device device; + int thermalWidth = -1, thermalHeight = -1; + bool thermalFound = false; + dai::CameraBoardSocket thermalSocket; + + for(const auto& features : device.getConnectedCameraFeatures()) { + if(std::find(features.supportedTypes.begin(), features.supportedTypes.end(), dai::CameraSensorType::THERMAL) != features.supportedTypes.end()) { + thermalFound = true; + thermalSocket = features.socket; + thermalWidth = features.width; + thermalHeight = features.height; + break; + } + } + + if(!thermalFound) { + throw std::runtime_error("No thermal camera found!"); + } + + dai::Pipeline pipeline; + + // Define sources and outputs + auto camRgb = pipeline.create(); + auto thermalCam = pipeline.create(); + auto sync = pipeline.create(); + auto out = pipeline.create(); + auto align = pipeline.create(); + auto cfgIn = pipeline.create(); + + thermalCam->setBoardSocket(thermalSocket); + thermalCam->setFps(FPS); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(COLOR_RESOLUTION); + camRgb->setFps(FPS); + camRgb->setIspScale(1, 3); + + out->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast((1 / FPS) * 1000.0 * 0.5))); + + cfgIn->setStreamName("config"); + + align->outputAligned.link(sync->inputs["aligned"]); + camRgb->isp.link(sync->inputs["rgb"]); + camRgb->isp.link(align->inputAlignTo); + thermalCam->raw.link(align->input); + sync->out.link(out->input); + cfgIn->out.link(align->inputConfig); + + device.startPipeline(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + auto cfgQ = device.getInputQueue("config"); + + FPSCounter fpsCounter; + + std::string windowName = "rgb-thermal"; + cv::namedWindow(windowName, cv::WINDOW_NORMAL); + cv::resizeWindow(windowName, 1280, 720); + cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights); + cv::createTrackbar("Static Depth Plane [mm]", windowName, nullptr, 2000, updateDepthPlane); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto thermalAligned = messageGroup->get("aligned"); + + if(frameRgb && thermalAligned) { + auto frameRgbCv = frameRgb->getCvFrame(); + auto thermalFrame = thermalAligned->getFrame(true); + + // Colorize the aligned depth + cv::Mat mask; + cv::Mat colormappedFrame; + cv::Mat thermalFrameFloat; + thermalFrame.convertTo(thermalFrameFloat, CV_32F); + // Get a mask for the nan values + mask = createNaNMask(thermalFrameFloat); + auto meanValue = cv::mean(thermalFrameFloat, ~mask); + thermalFrameFloat.setTo(meanValue, mask); + cv::normalize(thermalFrameFloat, thermalFrameFloat, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::applyColorMap(thermalFrameFloat, colormappedFrame, cv::COLORMAP_MAGMA); + + colormappedFrame.setTo(cv::Scalar(0, 0, 0), mask); + + cv::Mat blended; + cv::addWeighted(frameRgbCv, rgbWeight, colormappedFrame, thermalWeight, 0, blended); + cv::putText(blended, "FPS: " + std::to_string(fpsCounter.getFps()), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); + cv::imshow(windowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + + auto cfg = align->initialConfig.get(); + cfg.staticDepthPlane = staticDepthPlane; + auto alignConfig = std::make_shared(); + alignConfig->set(cfg); + cfgQ->send(alignConfig); + } + + return 0; +} \ No newline at end of file diff --git a/examples/ImageAlign/tof_align.cpp b/examples/ImageAlign/tof_align.cpp new file mode 100644 index 000000000..8d62006d4 --- /dev/null +++ b/examples/ImageAlign/tof_align.cpp @@ -0,0 +1,135 @@ +#include + +#include "depthai/depthai.hpp" + +constexpr auto FPS = 30.0; + +constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_B; +constexpr auto TOF_SOCKET = dai::CameraBoardSocket::CAM_A; +constexpr auto ALIGN_SOCKET = RGB_SOCKET; + +class FPSCounter { + public: + void tick() { + auto now = std::chrono::steady_clock::now(); + frameTimes.push(now); + if(frameTimes.size() > 100) { + frameTimes.pop(); + } + } + + double getFps() { + if(frameTimes.size() <= 1) return 0; + auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); + return (frameTimes.size() - 1) * 1000.0 / duration; + } + + private: + std::queue frameTimes; +}; + +cv::Mat colorizeDepth(const cv::Mat& frameDepth) { + cv::Mat invalidMask = (frameDepth == 0); + cv::Mat depthFrameColor; + try { + double minDepth = 0.0; + double maxDepth = 0.0; + cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); + if(minDepth == maxDepth) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + return depthFrameColor; + } + cv::Mat logDepth; + frameDepth.convertTo(logDepth, CV_32F); + cv::log(logDepth, logDepth); + logDepth.setTo(log(minDepth), invalidMask); + cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); + depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); + } catch(const std::exception& e) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + } + return depthFrameColor; +} + +double rgbWeight = 0.4; +double depthWeight = 0.6; + +void updateBlendWeights(int percentRgb, void*) { + rgbWeight = static_cast(percentRgb) / 100.0; + depthWeight = 1.0 - rgbWeight; +} + +int main() { + dai::Pipeline pipeline; + + auto camRgb = pipeline.create(); + auto camTof = pipeline.create(); + auto tof = pipeline.create(); + auto sync = pipeline.create(); + auto align = pipeline.create(); + auto xout = pipeline.create(); + + camTof->setFps(FPS); + camTof->setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG); + camTof->setBoardSocket(TOF_SOCKET); + + camRgb->setBoardSocket(RGB_SOCKET); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P); + camRgb->setFps(FPS); + camRgb->setIspScale(1, 2); + + xout->setStreamName("out"); + + sync->setSyncThreshold(std::chrono::milliseconds(static_cast(1000 / FPS))); + + camRgb->isp.link(sync->inputs["rgb"]); + camTof->raw.link(tof->input); + tof->depth.link(align->input); + align->outputAligned.link(sync->inputs["depth_aligned"]); + camRgb->isp.link(align->inputAlignTo); + sync->out.link(xout->input); + + dai::Device device(pipeline); + auto queue = device.getOutputQueue("out", 8, false); + + FPSCounter fpsCounter; + + std::string rgbDepthWindowName = "rgb-depth"; + cv::namedWindow(rgbDepthWindowName); + cv::createTrackbar("RGB Weight %", rgbDepthWindowName, nullptr, 100, updateBlendWeights); + + while(true) { + auto messageGroup = queue->get(); + fpsCounter.tick(); + + auto frameRgb = messageGroup->get("rgb"); + auto frameDepth = messageGroup->get("depth_aligned"); + + if(frameRgb && frameDepth) { + auto cvFrame = frameRgb->getCvFrame(); + auto alignedDepthColorized = colorizeDepth(frameDepth->getFrame()); + + cv::putText(alignedDepthColorized, + "FPS: " + std::to_string(fpsCounter.getFps()), + cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, + 1, + cv::Scalar(255, 255, 255), + 2); + + cv::imshow("depth", alignedDepthColorized); + + cv::Mat blended; + cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended); + cv::imshow(rgbDepthWindowName, blended); + } + + int key = cv::waitKey(1); + if(key == 'q' || key == 'Q') { + break; + } + } + + return 0; +} diff --git a/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp new file mode 100644 index 000000000..c97f8adf3 --- /dev/null +++ b/examples/SpatialDetection/spatial_tiny_yolo_tof.cpp @@ -0,0 +1,169 @@ +#include +#include +#include +#include +#include + +constexpr auto FPS = 15; + +static const std::vector labelMap = { + "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", + "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", + "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", + "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", + "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", + "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", + "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", + "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", + "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + +static std::atomic syncNN{true}; + +int main(int argc, char** argv) { + dai::Pipeline pipeline; + + auto camRgb = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto tof = pipeline.create(); + auto camTof = pipeline.create(); + auto imageAlign = pipeline.create(); + + auto xoutRgb = pipeline.create(); + auto xoutNN = pipeline.create(); + auto xoutDepth = pipeline.create(); + + xoutRgb->setStreamName("rgb"); + xoutNN->setStreamName("detections"); + xoutDepth->setStreamName("depth"); + + camRgb->setPreviewSize(416, 416); + camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P); + camRgb->setInterleaved(false); + camRgb->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_B); + camRgb->setFps(FPS); + + camTof->setFps(FPS); + camTof->setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG); + camTof->setBoardSocket(dai::CameraBoardSocket::CAM_A); + + imageAlign->setOutputSize(640, 400); + + spatialDetectionNetwork->setBlobPath(BLOB_PATH); + spatialDetectionNetwork->setConfidenceThreshold(0.5f); + spatialDetectionNetwork->input.setBlocking(false); + spatialDetectionNetwork->setBoundingBoxScaleFactor(0.5); + spatialDetectionNetwork->setDepthLowerThreshold(100); + spatialDetectionNetwork->setDepthUpperThreshold(5000); + + spatialDetectionNetwork->setNumClasses(80); + spatialDetectionNetwork->setCoordinateSize(4); + spatialDetectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); + spatialDetectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}}); + spatialDetectionNetwork->setIouThreshold(0.5f); + + camTof->raw.link(tof->input); + tof->depth.link(imageAlign->input); + + camRgb->preview.link(spatialDetectionNetwork->input); + if(syncNN) { + spatialDetectionNetwork->passthrough.link(xoutRgb->input); + } else { + camRgb->preview.link(xoutRgb->input); + } + + spatialDetectionNetwork->out.link(xoutNN->input); + + camRgb->isp.link(imageAlign->inputAlignTo); + imageAlign->outputAligned.link(spatialDetectionNetwork->inputDepth); + spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); + + dai::Device device(pipeline); + + auto previewQueue = device.getOutputQueue("rgb", 4, false); + auto detectionNNQueue = device.getOutputQueue("detections", 4, false); + auto depthQueue = device.getOutputQueue("depth", 4, false); + + auto startTime = std::chrono::steady_clock::now(); + int counter = 0; + float fps = 0; + auto color = cv::Scalar(255, 255, 255); + + while(true) { + auto inPreview = previewQueue->get(); + auto inDet = detectionNNQueue->get(); + auto depth = depthQueue->get(); + + cv::Mat frame = inPreview->getCvFrame(); + cv::Mat depthFrame = depth->getFrame(); // depthFrame values are in millimeters + + cv::Mat depthFrameColor; + cv::normalize(depthFrame, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1); + cv::equalizeHist(depthFrameColor, depthFrameColor); + cv::applyColorMap(depthFrameColor, depthFrameColor, cv::COLORMAP_HOT); + + counter++; + auto currentTime = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast>(currentTime - startTime); + if(elapsed > std::chrono::seconds(1)) { + fps = counter / elapsed.count(); + counter = 0; + startTime = currentTime; + } + + auto detections = inDet->detections; + + for(const auto& detection : detections) { + auto roiData = detection.boundingBoxMapping; + auto roi = roiData.roi; + roi = roi.denormalize(depthFrameColor.cols, depthFrameColor.rows); + auto topLeft = roi.topLeft(); + auto bottomRight = roi.bottomRight(); + auto xmin = static_cast(topLeft.x); + auto ymin = static_cast(topLeft.y); + auto xmax = static_cast(bottomRight.x); + auto ymax = static_cast(bottomRight.y); + cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, 1); + + int x1 = detection.xmin * frame.cols; + int y1 = detection.ymin * frame.rows; + int x2 = detection.xmax * frame.cols; + int y2 = detection.ymax * frame.rows; + + uint32_t labelIndex = detection.label; + std::string labelStr = std::to_string(labelIndex); + if(labelIndex < labelMap.size()) { + labelStr = labelMap[labelIndex]; + } + cv::putText(frame, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + std::stringstream confStr; + confStr << std::fixed << std::setprecision(2) << detection.confidence * 100; + cv::putText(frame, confStr.str(), cv::Point(x1 + 10, y1 + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + + std::stringstream depthX; + depthX << "X: " << static_cast(detection.spatialCoordinates.x) << " mm"; + cv::putText(frame, depthX.str(), cv::Point(x1 + 10, y1 + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + std::stringstream depthY; + depthY << "Y: " << static_cast(detection.spatialCoordinates.y) << " mm"; + cv::putText(frame, depthY.str(), cv::Point(x1 + 10, y1 + 65), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + std::stringstream depthZ; + depthZ << "Z: " << static_cast(detection.spatialCoordinates.z) << " mm"; + cv::putText(frame, depthZ.str(), cv::Point(x1 + 10, y1 + 80), cv::FONT_HERSHEY_TRIPLEX, 0.5, 255); + + cv::rectangle(frame, cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)), color, cv::FONT_HERSHEY_SIMPLEX); + } + + std::stringstream fpsStr; + fpsStr << std::fixed << std::setprecision(2) << fps; + cv::putText(frame, fpsStr.str(), cv::Point(2, frame.rows - 4), cv::FONT_HERSHEY_TRIPLEX, 0.4, color); + + cv::imshow("depth", depthFrameColor); + cv::imshow("rgb", frame); + + if(cv::waitKey(1) == 'q') { + break; + } + } + + return 0; +} diff --git a/examples/ToF/tof_depth.cpp b/examples/ToF/tof_depth.cpp new file mode 100644 index 000000000..9a8c9f8ae --- /dev/null +++ b/examples/ToF/tof_depth.cpp @@ -0,0 +1,140 @@ +#include +#include +#include +#include +#include +#include +#include "depthai-shared/datatype/RawToFConfig.hpp" +#include "depthai/pipeline/datatype/ToFConfig.hpp" + +constexpr auto SHAPE = 720; + +std::shared_ptr createConfig(dai::RawToFConfig configRaw) { + auto config = std::make_shared(); + config->set(std::move(configRaw)); + return config; +} + +cv::Mat colorizeDepth(const cv::Mat& frameDepth) { + cv::Mat invalidMask = (frameDepth == 0); + cv::Mat depthFrameColor; + try { + double minDepth = 0.0; + double maxDepth = 0.0; + cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); + if(minDepth == maxDepth) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + return depthFrameColor; + } + cv::Mat logDepth; + frameDepth.convertTo(logDepth, CV_32F); + cv::log(logDepth, logDepth); + logDepth.setTo(log(minDepth), invalidMask); + cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); + cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); + depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); + } catch(const std::exception& e) { + depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); + } + return depthFrameColor; +} + +int main() { + dai::Pipeline pipeline; + cv::Mat cvColorMap; + + auto tof = pipeline.create(); + + // Configure the ToF node + auto tofConfig = tof->initialConfig.get(); + + tofConfig.enableOpticalCorrection = true; + tofConfig.enablePhaseShuffleTemporalFilter = true; + tofConfig.phaseUnwrappingLevel = 4; + tofConfig.phaseUnwrapErrorThreshold = 300; + + tofConfig.enableTemperatureCorrection = false; // Not yet supported + + auto xinTofConfig = pipeline.create(); + xinTofConfig->setStreamName("tofConfig"); + xinTofConfig->out.link(tof->inputConfig); + + tof->initialConfig.set(tofConfig); + + auto camTof = pipeline.create(); + camTof->setFps(30); + camTof->setBoardSocket(dai::CameraBoardSocket::CAM_A); + camTof->raw.link(tof->input); + + auto xout = pipeline.create(); + xout->setStreamName("depth"); + tof->depth.link(xout->input); + + tofConfig = tof->initialConfig.get(); + + + dai::Device device(pipeline); + std::cout << "Connected cameras: " << device.getConnectedCameraFeatures().size() << std::endl; + auto qDepth = device.getOutputQueue("depth"); + + auto tofConfigInQueue = device.getInputQueue("tofConfig"); + + int counter = 0; + while (true) { + auto start = std::chrono::high_resolution_clock::now(); + int key = cv::waitKey(1); + if (key == 'f') { + tofConfig.enableFPPNCorrection = !tofConfig.enableFPPNCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'o') { + tofConfig.enableOpticalCorrection = !tofConfig.enableOpticalCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'w') { + tofConfig.enableWiggleCorrection = !tofConfig.enableWiggleCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 't') { + tofConfig.enableTemperatureCorrection = !tofConfig.enableTemperatureCorrection; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'q') { + break; + } else if (key == '0') { + tofConfig.enablePhaseUnwrapping = false; + tofConfig.phaseUnwrappingLevel = 0; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '1') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 1; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '2') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 2; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '3') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 3; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '4') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 4; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == '5') { + tofConfig.enablePhaseUnwrapping = true; + tofConfig.phaseUnwrappingLevel = 5; + tofConfigInQueue->send(createConfig(tofConfig)); + } else if (key == 'm') { + std::vector medianSettings = { dai::MedianFilter::MEDIAN_OFF, dai::MedianFilter::KERNEL_3x3, dai::MedianFilter::KERNEL_5x5, dai::MedianFilter::KERNEL_7x7 }; + auto currentMedian = tofConfig.median; + auto nextMedian = medianSettings[(std::find(medianSettings.begin(), medianSettings.end(), currentMedian) - medianSettings.begin() + 1) % medianSettings.size()]; + tofConfig.median = nextMedian; + tofConfigInQueue->send(createConfig(tofConfig)); + } + + auto imgFrame = qDepth->get(); // blocking call, will wait until new data has arrived + auto depthColorized = colorizeDepth(imgFrame->getFrame()); + + cv::imshow("Colorized depth", depthColorized); + counter++; + } + + return 0; +} diff --git a/include/depthai/device/CalibrationHandler.hpp b/include/depthai/device/CalibrationHandler.hpp index b1bb2b4f1..cc9e8f09c 100644 --- a/include/depthai/device/CalibrationHandler.hpp +++ b/include/depthai/device/CalibrationHandler.hpp @@ -166,7 +166,7 @@ class CalibrationHandler { * Get the Distortion Coefficients object * * @param cameraId Uses the cameraId to identify which distortion Coefficients to return. - * @return the distortion coefficients of the requested camera in this order: [k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,τx,τy] for CameraModel::Perspective + * @return the distortion coefficients of the requested camera in this order: [k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,tx,ty] for CameraModel::Perspective * or [k1, k2, k3, k4] for CameraModel::Fisheye * see https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html for Perspective model (Rational Polynomial Model) * see https://docs.opencv.org/4.5.4/db/d58/group__calib3d__fisheye.html for Fisheye model diff --git a/include/depthai/device/DeviceBase.hpp b/include/depthai/device/DeviceBase.hpp index 380812be2..4d533064a 100644 --- a/include/depthai/device/DeviceBase.hpp +++ b/include/depthai/device/DeviceBase.hpp @@ -446,6 +446,16 @@ class DeviceBase { */ int getXLinkChunkSize(); + /** + * Sets the maximum transmission rate for the XLink connection on device side, + * using a simple token bucket algorithm. Useful for bandwidth throttling + * + * @param maxRateBytesPerSecond Rate limit in Bytes/second + * @param burstSize Size in Bytes for how much to attempt to send once, 0 = auto + * @param waitUs Time in microseconds to wait for replenishing tokens, 0 = auto + */ + void setXLinkRateLimit(int maxRateBytesPerSecond, int burstSize = 0, int waitUs = 0); + /** * Get the Device Info object o the device which is currently running * diff --git a/include/depthai/pipeline/datatype/ImageAlignConfig.hpp b/include/depthai/pipeline/datatype/ImageAlignConfig.hpp new file mode 100644 index 000000000..404e28f3b --- /dev/null +++ b/include/depthai/pipeline/datatype/ImageAlignConfig.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include "depthai-shared/datatype/RawImageAlignConfig.hpp" +#include "depthai/pipeline/datatype/Buffer.hpp" + +namespace dai { + +/** + * ImageAlignConfig message + */ +class ImageAlignConfig : public Buffer { + std::shared_ptr serialize() const override; + RawImageAlignConfig& cfg; + + public: + ImageAlignConfig(); + explicit ImageAlignConfig(std::shared_ptr ptr); + virtual ~ImageAlignConfig() = default; + + /** + * Set explicit configuration. + * @param config Explicit configuration + */ + ImageAlignConfig& set(dai::RawImageAlignConfig config); + + /** + * Retrieve configuration data for SpatialLocationCalculator. + * @returns config for SpatialLocationCalculator + */ + dai::RawImageAlignConfig get() const; +}; + +} // namespace dai \ No newline at end of file diff --git a/include/depthai/pipeline/datatype/ToFConfig.hpp b/include/depthai/pipeline/datatype/ToFConfig.hpp index 0d877ee99..dc19a9312 100644 --- a/include/depthai/pipeline/datatype/ToFConfig.hpp +++ b/include/depthai/pipeline/datatype/ToFConfig.hpp @@ -16,9 +16,6 @@ class ToFConfig : public Buffer { RawToFConfig& cfg; public: - // Raw* mirror - using DepthParams = RawToFConfig::DepthParams; - /** * Construct ToFConfig message. */ @@ -26,10 +23,6 @@ class ToFConfig : public Buffer { explicit ToFConfig(std::shared_ptr ptr); virtual ~ToFConfig() = default; - ToFConfig& setDepthParams(dai::ToFConfig::DepthParams config); - ToFConfig& setFreqModUsed(dai::ToFConfig::DepthParams::TypeFMod fmod); - ToFConfig& setAvgPhaseShuffle(bool enable); - ToFConfig& setMinAmplitude(float minamp); /** * @param median Set kernel size for median filtering, or disable */ diff --git a/include/depthai/pipeline/node/Cast.hpp b/include/depthai/pipeline/node/Cast.hpp new file mode 100644 index 000000000..d4d9b8c00 --- /dev/null +++ b/include/depthai/pipeline/node/Cast.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include + +// shared +#include + +namespace dai { +namespace node { + +/** + * @brief Cast node. + */ +class Cast : public NodeCRTP { + public: + constexpr static const char* NAME = "Cast"; + + Cast(const std::shared_ptr& par, int64_t nodeId); + Cast(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props); + + /** + * Input NNData or ImgFrame message. + */ + Input input{*this, "input", Input::Type::SReceiver, false, 4, true, {{DatatypeEnum::ImgFrame, false}, {DatatypeEnum::NNData, false}}}; + + /** + * Output ImgFrame message. + */ + Output output{*this, "output", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Passthrough input message. + */ + Output passthroughInput{*this, "passthroughInput", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}, {DatatypeEnum::NNData, false}}}; + + /** + * Set number of frames in pool + * @param numFramesPool Number of frames in pool + */ + Cast& setNumFramesPool(int numFramesPool); + + /** + * Set output frame type + * @param outputType Output frame type + */ + Cast& setOutputFrameType(dai::RawImgFrame::Type outputType); + + /** + * Set scale + * @param scale Scale + */ + Cast& setScale(float scale); + + /** + * Set offset + * @param offset Offset + */ + Cast& setOffset(float offset); +}; + +} // namespace node +} // namespace dai diff --git a/include/depthai/pipeline/node/ImageAlign.hpp b/include/depthai/pipeline/node/ImageAlign.hpp new file mode 100644 index 000000000..d6684df7f --- /dev/null +++ b/include/depthai/pipeline/node/ImageAlign.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include + +// shared +#include + +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" + +namespace dai { +namespace node { + +/** + * @brief ImageAlign node. Calculates spatial location data on a set of ROIs on depth map. + */ +class ImageAlign : public NodeCRTP { + public: + constexpr static const char* NAME = "ImageAlign"; + + protected: + Properties& getProperties(); + + private: + std::shared_ptr rawConfig; + + public: + ImageAlign(const std::shared_ptr& par, int64_t nodeId); + ImageAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props); + + /** + * Initial config to use when calculating spatial location data. + */ + ImageAlignConfig initialConfig; + + /** + * Input message with ability to modify parameters in runtime. + * Default queue is non-blocking with size 4. + */ + Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::ImageAlignConfig, false}}}; + + /** + * Input message. + * Default queue is non-blocking with size 4. + */ + Input input{*this, "input", Input::Type::SReceiver, false, 4, true, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Input align to message. + * Default queue is non-blocking with size 1. + */ + Input inputAlignTo{*this, "inputAlignTo", Input::Type::SReceiver, false, 1, true, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Outputs ImgFrame message that is aligned to inputAlignTo. + */ + Output outputAligned{*this, "outputAligned", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Passthrough message on which the calculation was performed. + * Suitable for when input queue is set to non-blocking behavior. + */ + Output passthroughInput{*this, "passthroughInput", Output::Type::MSender, {{DatatypeEnum::ImgFrame, false}}}; + + /** + * Specify the output size of the aligned image + */ + ImageAlign& setOutputSize(int alignWidth, int alignHeight); + + /** + * Specify whether to keep aspect ratio when resizing + */ + ImageAlign& setOutKeepAspectRatio(bool keep); + + /** + * Specify interpolation method to use when resizing + */ + ImageAlign& setInterpolation(Interpolation interp); + + /** + * Specify number of shaves to use for this node + */ + ImageAlign& setNumShaves(int numShaves); + + /** + * Specify number of frames in the pool + */ + ImageAlign& setNumFramesPool(int numFramesPool); +}; + +} // namespace node +} // namespace dai \ No newline at end of file diff --git a/include/depthai/pipeline/node/ToF.hpp b/include/depthai/pipeline/node/ToF.hpp index d4e01a7d2..59d1b1c7d 100644 --- a/include/depthai/pipeline/node/ToF.hpp +++ b/include/depthai/pipeline/node/ToF.hpp @@ -41,15 +41,39 @@ class ToF : public NodeCRTP { */ Input inputConfig{*this, "inputConfig", Input::Type::SReceiver, false, 4, {{DatatypeEnum::ToFConfig, false}}}; + /** + * Input raw ToF data. + * Default queue is blocking with size 8. + */ Input input{*this, "input", Input::Type::SReceiver, true, 8, {{DatatypeEnum::ImgFrame, true}}}; /** - * Outputs ImgFrame message that carries modified image. + * Outputs ImgFrame message that carries decoded depth image. */ Output depth{*this, "depth", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + /** + * Outputs ImgFrame message that carries amplitude image. + */ Output amplitude{*this, "amplitude", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + /** + * Outputs ImgFrame message that carries intensity image. + */ Output intensity{*this, "intensity", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; - Output error{*this, "error", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + /** + * Outputs ImgFrame message that carries phase image, useful for debugging. float32 type. + */ + Output phase{*this, "phase", Output::Type::MSender, {{DatatypeEnum::ImgFrame, true}}}; + + /** + * Specify number of shaves reserved for ToF decoding. + */ + ToF& setNumShaves(int numShaves); + + /** + * Specify number of frames in output pool + * @param numFramesPool Number of frames in output pool + */ + ToF& setNumFramesPool(int numFramesPool); }; } // namespace node diff --git a/include/depthai/pipeline/node/VideoEncoder.hpp b/include/depthai/pipeline/node/VideoEncoder.hpp index 59c186e52..904584d4c 100644 --- a/include/depthai/pipeline/node/VideoEncoder.hpp +++ b/include/depthai/pipeline/node/VideoEncoder.hpp @@ -78,7 +78,7 @@ class VideoEncoder : public NodeCRTP int getNumFramesPool() const; // encoder properties - /// Set rate control mode + /// Set rate control mode Applicable only to H264 and H265 profiles void setRateControlMode(Properties::RateControlMode mode); /// Set encoding profile void setProfile(Properties::Profile profile); @@ -89,9 +89,9 @@ class VideoEncoder : public NodeCRTP [[deprecated("Input width/height no longer needed, automatically determined from first frame")]] void setProfile(int width, int height, Properties::Profile profile); - /// Set output bitrate in bps, for CBR rate control mode. 0 for auto (based on frame size and FPS) + /// Set output bitrate in bps, for CBR rate control mode. 0 for auto (based on frame size and FPS). Applicable only to H264 and H265 profiles void setBitrate(int bitrate); - /// Set output bitrate in kbps, for CBR rate control mode. 0 for auto (based on frame size and FPS) + /// Set output bitrate in kbps, for CBR rate control mode. 0 for auto (based on frame size and FPS). Applicable only to H264 and H265 profiles void setBitrateKbps(int bitrateKbps); /** @@ -108,11 +108,11 @@ class VideoEncoder : public NodeCRTP */ void setKeyframeFrequency(int freq); - /// Set number of B frames to be inserted + /// Set number of B frames to be inserted. Applicable only to H264 and H265 profiles void setNumBFrames(int numBFrames); /** - * Set quality + * Set quality for [M]JPEG profile * @param quality Value between 0-100%. Approximates quality */ void setQuality(int quality); diff --git a/include/depthai/pipeline/nodes.hpp b/include/depthai/pipeline/nodes.hpp index 7047de48a..3dc939a3d 100644 --- a/include/depthai/pipeline/nodes.hpp +++ b/include/depthai/pipeline/nodes.hpp @@ -3,12 +3,14 @@ // all the nodes #include "node/AprilTag.hpp" #include "node/Camera.hpp" +#include "node/Cast.hpp" #include "node/ColorCamera.hpp" #include "node/DetectionNetwork.hpp" #include "node/DetectionParser.hpp" #include "node/EdgeDetector.hpp" #include "node/FeatureTracker.hpp" #include "node/IMU.hpp" +#include "node/ImageAlign.hpp" #include "node/ImageManip.hpp" #include "node/MessageDemux.hpp" #include "node/MonoCamera.hpp" diff --git a/shared/depthai-shared b/shared/depthai-shared index 4534104a5..9762f334f 160000 --- a/shared/depthai-shared +++ b/shared/depthai-shared @@ -1 +1 @@ -Subproject commit 4534104a58054357f38abb1618f9b82a059bfad1 +Subproject commit 9762f334f39fc841a5bbea9b1114bbac933710e0 diff --git a/src/device/DeviceBase.cpp b/src/device/DeviceBase.cpp index 6f6dfb487..7beaf4d2f 100644 --- a/src/device/DeviceBase.cpp +++ b/src/device/DeviceBase.cpp @@ -1219,6 +1219,10 @@ int DeviceBase::getXLinkChunkSize() { return pimpl->rpcClient->call("getXLinkChunkSize").as(); } +void DeviceBase::setXLinkRateLimit(int maxRateBytesPerSecond, int burstSize, int waitUs) { + pimpl->rpcClient->call("setXLinkRateLimit", maxRateBytesPerSecond, burstSize, waitUs); +} + DeviceInfo DeviceBase::getDeviceInfo() const { return deviceInfo; } diff --git a/src/device/DeviceBootloader.cpp b/src/device/DeviceBootloader.cpp index 7b06c9fa1..5a6ff0850 100644 --- a/src/device/DeviceBootloader.cpp +++ b/src/device/DeviceBootloader.cpp @@ -417,6 +417,19 @@ void DeviceBootloader::init(bool embeddedMvcmd, const dai::Path& pathToMvcmd, tl // Retrieve bootloader version version = requestVersion(); flashedVersion = version; + + auto recommendedMinVersion = DeviceBootloader::Version(0, 0, 28); + if(version < recommendedMinVersion) { + logger::warn( + "[{}] [{}] Flashed bootloader version {}, less than {} is susceptible to bootup/restart failure. Upgrading is advised, flashing " + "main/factory (not user) bootloader. Available: {}", + deviceInfo.mxid, + deviceInfo.name, + version.toString(), + recommendedMinVersion.toString(), + getEmbeddedBootloaderVersion().toString()); + } + if(version >= Version(0, 0, 12)) { // If version is adequate, do an in memory boot. diff --git a/src/pipeline/datatype/ImageAlignConfig.cpp b/src/pipeline/datatype/ImageAlignConfig.cpp new file mode 100644 index 000000000..8b6376eea --- /dev/null +++ b/src/pipeline/datatype/ImageAlignConfig.cpp @@ -0,0 +1,21 @@ +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" + +namespace dai { + +std::shared_ptr ImageAlignConfig::serialize() const { + return raw; +} + +ImageAlignConfig::ImageAlignConfig() : Buffer(std::make_shared()), cfg(*dynamic_cast(raw.get())) {} +ImageAlignConfig::ImageAlignConfig(std::shared_ptr ptr) : Buffer(std::move(ptr)), cfg(*dynamic_cast(raw.get())) {} + +dai::RawImageAlignConfig ImageAlignConfig::get() const { + return cfg; +} + +ImageAlignConfig& ImageAlignConfig::set(dai::RawImageAlignConfig config) { + cfg = config; + return *this; +} + +} // namespace dai \ No newline at end of file diff --git a/src/pipeline/datatype/ImgFrame.cpp b/src/pipeline/datatype/ImgFrame.cpp index 55c443bc9..a777c9925 100644 --- a/src/pipeline/datatype/ImgFrame.cpp +++ b/src/pipeline/datatype/ImgFrame.cpp @@ -99,6 +99,7 @@ ImgFrame& ImgFrame::setSequenceNum(int64_t sequenceNum) { ImgFrame& ImgFrame::setWidth(unsigned int width) { img.fb.width = width; img.fb.stride = width; + if(img.fb.bytesPP) img.fb.stride *= img.fb.bytesPP; return *this; } ImgFrame& ImgFrame::setHeight(unsigned int height) { @@ -117,6 +118,7 @@ ImgFrame& ImgFrame::setSize(std::tuple size) { ImgFrame& ImgFrame::setType(RawImgFrame::Type type) { img.fb.type = type; img.fb.bytesPP = RawImgFrame::typeToBpp(img.fb.type); + if(img.fb.width) img.fb.stride = img.fb.width * img.fb.bytesPP; return *this; } diff --git a/src/pipeline/datatype/StreamMessageParser.cpp b/src/pipeline/datatype/StreamMessageParser.cpp index bb22f0b8f..811cc62a0 100644 --- a/src/pipeline/datatype/StreamMessageParser.cpp +++ b/src/pipeline/datatype/StreamMessageParser.cpp @@ -8,6 +8,8 @@ #include #include +#include "utility/Logging.hpp" + // project #include "depthai/pipeline/datatype/ADatatype.hpp" #include "depthai/pipeline/datatype/AprilTagConfig.hpp" @@ -18,6 +20,7 @@ #include "depthai/pipeline/datatype/EncodedFrame.hpp" #include "depthai/pipeline/datatype/FeatureTrackerConfig.hpp" #include "depthai/pipeline/datatype/IMUData.hpp" +#include "depthai/pipeline/datatype/ImageAlignConfig.hpp" #include "depthai/pipeline/datatype/ImageManipConfig.hpp" #include "depthai/pipeline/datatype/ImgDetections.hpp" #include "depthai/pipeline/datatype/ImgFrame.hpp" @@ -44,6 +47,7 @@ #include "depthai-shared/datatype/RawEncodedFrame.hpp" #include "depthai-shared/datatype/RawFeatureTrackerConfig.hpp" #include "depthai-shared/datatype/RawIMUData.hpp" +#include "depthai-shared/datatype/RawImageAlignConfig.hpp" #include "depthai-shared/datatype/RawImageManipConfig.hpp" #include "depthai-shared/datatype/RawImgDetections.hpp" #include "depthai-shared/datatype/RawImgFrame.hpp" @@ -65,6 +69,8 @@ namespace dai { +static constexpr std::array endOfPacketMarker = {0xAB, 0xCD, 0xEF, 0x01, 0x23, 0x45, 0x67, 0x89, 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0xDE, 0xF0}; + // Reads int from little endian format inline int readIntLE(uint8_t* data) { return data[0] + data[1] * 256 + data[2] * 256 * 256 + data[3] * 256 * 256 * 256; @@ -83,28 +89,40 @@ inline std::shared_ptr parseDatatype(std::uint8_t* metadata, size_t size, std } static std::tuple parseHeader(streamPacketDesc_t* const packet) { - if(packet->length < 8) { - throw std::runtime_error("Bad packet, couldn't parse (not enough data)"); + if(packet->length < 24) { + throw std::runtime_error(fmt::format("Bad packet, couldn't parse (not enough data), total size {}", packet->length)); } - const int serializedObjectSize = readIntLE(packet->data + packet->length - 4); - const auto objectType = static_cast(readIntLE(packet->data + packet->length - 8)); + const std::uint32_t packetLength = packet->length - endOfPacketMarker.size(); + const int serializedObjectSize = readIntLE(packet->data + packetLength - 4); + const auto objectType = static_cast(readIntLE(packet->data + packetLength - 8)); + + uint8_t* marker = packet->data + packetLength; + if(memcmp(marker, endOfPacketMarker.data(), endOfPacketMarker.size()) != 0) { + std::string hex; + for(std::uint32_t i = 0; i < endOfPacketMarker.size(); i++) { + hex += fmt::format("{:02X}", marker[i]); + } + logger::warn("StreamMessageParser end-of-packet marker mismatch, got: " + hex); + } + + const auto info = fmt::format(", total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize); if(serializedObjectSize < 0) { - throw std::runtime_error("Bad packet, couldn't parse (metadata size negative)"); - } else if(serializedObjectSize > static_cast(packet->length)) { - throw std::runtime_error("Bad packet, couldn't parse (metadata size larger than packet length)"); + throw std::runtime_error("Bad packet, couldn't parse (metadata size negative)" + info); + } else if(serializedObjectSize > static_cast(packetLength)) { + throw std::runtime_error("Bad packet, couldn't parse (metadata size larger than packet length)" + info); } - if(static_cast(packet->length) - 8 - serializedObjectSize < 0) { - throw std::runtime_error("Bad packet, couldn't parse (data too small)"); + if(static_cast(packetLength) - 8 - serializedObjectSize < 0) { + throw std::runtime_error("Bad packet, couldn't parse (data too small)" + info); } - const std::uint32_t bufferLength = packet->length - 8 - serializedObjectSize; - if(bufferLength > packet->length) { - throw std::runtime_error("Bad packet, couldn't parse (data too large)"); + const std::uint32_t bufferLength = packetLength - 8 - serializedObjectSize; + if(bufferLength > packetLength) { + throw std::runtime_error("Bad packet, couldn't parse (data too large)" + info); } auto* const metadataStart = packet->data + bufferLength; - if(metadataStart < packet->data || metadataStart >= packet->data + packet->length) { - throw std::runtime_error("Bad packet, couldn't parse (metadata out of bounds)"); + if(metadataStart < packet->data || metadataStart >= packet->data + packetLength) { + throw std::runtime_error("Bad packet, couldn't parse (metadata out of bounds)" + info); } return {objectType, serializedObjectSize, bufferLength}; @@ -210,9 +228,13 @@ std::shared_ptr StreamMessageParser::parseMessage(streamPacketDesc_t* case DatatypeEnum::MessageGroup: return parseDatatype(metadataStart, serializedObjectSize, data); break; + case DatatypeEnum::ImageAlignConfig: + return parseDatatype(metadataStart, serializedObjectSize, data); + break; } - throw std::runtime_error("Bad packet, couldn't parse"); + throw std::runtime_error( + fmt::format("Bad packet, couldn't parse, total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize)); } std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* const packet, DatatypeEnum& objectType) { @@ -314,10 +336,15 @@ std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPa case DatatypeEnum::MessageGroup: return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); break; + case DatatypeEnum::ImageAlignConfig: + return std::make_shared(parseDatatype(metadataStart, serializedObjectSize, data)); + break; } - throw std::runtime_error("Bad packet, couldn't parse (invalid message type)"); + throw std::runtime_error(fmt::format( + "Bad packet, couldn't parse (invalid message type), total size {}, type {}, metadata size {}", packet->length, objectType, serializedObjectSize)); } + std::shared_ptr StreamMessageParser::parseMessageToADatatype(streamPacketDesc_t* const packet) { DatatypeEnum objectType; return parseMessageToADatatype(packet, objectType); @@ -329,6 +356,7 @@ std::vector StreamMessageParser::serializeMessage(const RawBuffer& // 2. serialize and append metadata // 3. append datatype enum (4B LE) // 4. append size (4B LE) of serialized metadata + // 5. append 16-byte marker/canary DatatypeEnum datatype; std::vector metadata; @@ -342,11 +370,12 @@ std::vector StreamMessageParser::serializeMessage(const RawBuffer& for(int i = 0; i < 4; i++) leMetadataSize[i] = (metadataSize >> i * 8) & 0xFF; std::vector ser; - ser.reserve(data.data.size() + metadata.size() + leDatatype.size() + leMetadataSize.size()); + ser.reserve(data.data.size() + metadata.size() + leDatatype.size() + leMetadataSize.size() + endOfPacketMarker.size()); ser.insert(ser.end(), data.data.begin(), data.data.end()); ser.insert(ser.end(), metadata.begin(), metadata.end()); ser.insert(ser.end(), leDatatype.begin(), leDatatype.end()); ser.insert(ser.end(), leMetadataSize.begin(), leMetadataSize.end()); + ser.insert(ser.end(), endOfPacketMarker.begin(), endOfPacketMarker.end()); return ser; } diff --git a/src/pipeline/datatype/ToFConfig.cpp b/src/pipeline/datatype/ToFConfig.cpp index da1ce41ed..bf530291e 100644 --- a/src/pipeline/datatype/ToFConfig.cpp +++ b/src/pipeline/datatype/ToFConfig.cpp @@ -13,33 +13,13 @@ dai::RawToFConfig ToFConfig::get() const { return cfg; } -ToFConfig& ToFConfig::setDepthParams(dai::ToFConfig::DepthParams config) { - cfg.depthParams = config; - return *this; -} - -ToFConfig& ToFConfig::setFreqModUsed(dai::ToFConfig::DepthParams::TypeFMod fmod) { - cfg.depthParams.freqModUsed = fmod; - return *this; -} - -ToFConfig& ToFConfig::setAvgPhaseShuffle(bool enable) { - cfg.depthParams.avgPhaseShuffle = enable; - return *this; -} - ToFConfig& ToFConfig::set(dai::RawToFConfig config) { cfg = config; return *this; } -ToFConfig& ToFConfig::setMinAmplitude(float minamp) { - cfg.depthParams.minimumAmplitude = minamp; - return *this; -} - ToFConfig& ToFConfig::setMedianFilter(MedianFilter median) { - cfg.depthParams.median = median; + cfg.median = median; return *this; } diff --git a/src/pipeline/node/Cast.cpp b/src/pipeline/node/Cast.cpp new file mode 100644 index 000000000..dfb3ff451 --- /dev/null +++ b/src/pipeline/node/Cast.cpp @@ -0,0 +1,34 @@ +#include "depthai/pipeline/node/Cast.hpp" + +namespace dai { +namespace node { + +Cast::Cast(const std::shared_ptr& par, int64_t nodeId) : Cast(par, nodeId, std::make_unique()) {} +Cast::Cast(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) + : NodeCRTP(par, nodeId, std::move(props)) { + setInputRefs({&input}); + setOutputRefs({&output, &passthroughInput}); +} + +Cast& Cast::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; + return *this; +} + +Cast& Cast::setOutputFrameType(dai::RawImgFrame::Type outputType) { + properties.outputType = outputType; + return *this; +} + +Cast& Cast::setScale(float scale) { + properties.scale = scale; + return *this; +} + +Cast& Cast::setOffset(float offset) { + properties.offset = offset; + return *this; +} + +} // namespace node +} // namespace dai diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp new file mode 100644 index 000000000..8f0ceb73b --- /dev/null +++ b/src/pipeline/node/ImageAlign.cpp @@ -0,0 +1,46 @@ +#include "depthai/pipeline/node/ImageAlign.hpp" + +namespace dai { +namespace node { + +ImageAlign::ImageAlign(const std::shared_ptr& par, int64_t nodeId) : ImageAlign(par, nodeId, std::make_unique()) {} +ImageAlign::ImageAlign(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) + : NodeCRTP(par, nodeId, std::move(props)), + rawConfig(std::make_shared()), + initialConfig(rawConfig) { + setInputRefs({&inputConfig, &input, &inputAlignTo}); + setOutputRefs({&outputAligned, &passthroughInput}); +} + +ImageAlign::Properties& ImageAlign::getProperties() { + properties.initialConfig = *rawConfig; + return properties; +} + +ImageAlign& ImageAlign::setOutputSize(int alignWidth, int alignHeight) { + properties.alignWidth = alignWidth; + properties.alignHeight = alignHeight; + return *this; +} +ImageAlign& ImageAlign::setOutKeepAspectRatio(bool keep) { + properties.outKeepAspectRatio = keep; + return *this; +} + +ImageAlign& ImageAlign::setInterpolation(Interpolation interp) { + properties.interpolation = interp; + return *this; +} + +ImageAlign& ImageAlign::setNumShaves(int numShaves) { + properties.numShaves = numShaves; + return *this; +} + +ImageAlign& ImageAlign::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; + return *this; +} + +} // namespace node +} // namespace dai \ No newline at end of file diff --git a/src/pipeline/node/ToF.cpp b/src/pipeline/node/ToF.cpp index 86dddc0f4..db5b04710 100644 --- a/src/pipeline/node/ToF.cpp +++ b/src/pipeline/node/ToF.cpp @@ -7,7 +7,7 @@ ToF::ToF(const std::shared_ptr& par, int64_t nodeId) : ToF(par, no ToF::ToF(const std::shared_ptr& par, int64_t nodeId, std::unique_ptr props) : NodeCRTP(par, nodeId, std::move(props)), rawConfig(std::make_shared()), initialConfig(rawConfig) { setInputRefs({&inputConfig, &input}); - setOutputRefs({&depth, &litude, &intensity, &error}); + setOutputRefs({&depth, &litude, &intensity, &phase}); } ToF::Properties& ToF::getProperties() { @@ -15,5 +15,15 @@ ToF::Properties& ToF::getProperties() { return properties; } +ToF& ToF::setNumShaves(int numShaves) { + properties.numShaves = numShaves; + return *this; +} + +ToF& ToF::setNumFramesPool(int numFramesPool) { + properties.numFramesPool = numFramesPool; + return *this; +} + } // namespace node } // namespace dai diff --git a/tests/src/image_manip_node_test.cpp b/tests/src/image_manip_node_test.cpp index e47fea211..c0819dd54 100644 --- a/tests/src/image_manip_node_test.cpp +++ b/tests/src/image_manip_node_test.cpp @@ -13,7 +13,6 @@ int main() { // Create pipeline dai::Pipeline p; - auto xin = p.create(); auto imageManip = p.create(); auto xout = p.create(); diff --git a/tests/src/stream_message_parser_test.cpp b/tests/src/stream_message_parser_test.cpp index 07ef8839b..a0f64e2e6 100644 --- a/tests/src/stream_message_parser_test.cpp +++ b/tests/src/stream_message_parser_test.cpp @@ -6,6 +6,8 @@ // TODO(themarpe) - fuzz me instead +constexpr auto MARKER_SIZE = 16; + TEST_CASE("Correct message") { dai::ImgFrame frm; auto ser = dai::StreamMessageParser::serializeMessage(frm); @@ -20,12 +22,29 @@ TEST_CASE("Correct message") { REQUIRE(ser == ser2); } +TEST_CASE("Correct message, but padding corrupted, a warning should be printed") { + dai::ImgFrame frm; + auto ser = dai::StreamMessageParser::serializeMessage(frm); + ser[ser.size() - 1] = 0x55; + + streamPacketDesc_t packet; + packet.data = ser.data(); + packet.length = ser.size(); + + auto des = dai::StreamMessageParser::parseMessageToADatatype(&packet); + auto ser2 = dai::StreamMessageParser::serializeMessage(des); + // Just for the binary compare to match, cause the same corruption in re-serialized msg + ser2[ser.size() - 1] = 0x55; + + REQUIRE(ser == ser2); +} + TEST_CASE("Incorrect message bad size") { dai::ImgFrame frm; auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 100; + ser[ser.size() - 1 - MARKER_SIZE] = 100; streamPacketDesc_t packet; packet.data = ser.data(); @@ -39,7 +58,7 @@ TEST_CASE("Incorrect message negative size") { auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 200; + ser[ser.size() - 1 - MARKER_SIZE] = 200; streamPacketDesc_t packet; packet.data = ser.data(); @@ -87,7 +106,7 @@ TEST_CASE("Raw - Incorrect message bad size") { auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 100; + ser[ser.size() - 1 - MARKER_SIZE] = 100; streamPacketDesc_t packet; packet.data = ser.data(); @@ -101,7 +120,7 @@ TEST_CASE("Raw - Incorrect message negative size") { auto ser = dai::StreamMessageParser::serializeMessage(frm); // wreak havoc on serialized data - ser[ser.size() - 1] = 200; + ser[ser.size() - 1 - MARKER_SIZE] = 200; streamPacketDesc_t packet; packet.data = ser.data(); @@ -128,4 +147,4 @@ TEST_CASE("Raw - Incorrect message too small size 2") { packet.length = ser.size(); REQUIRE_THROWS(dai::StreamMessageParser::parseMessage(&packet)); -} \ No newline at end of file +}