diff --git a/src/pipeline/node/ImageAlign.cpp b/src/pipeline/node/ImageAlign.cpp index 94413e09bf..fd2553cada 100644 --- a/src/pipeline/node/ImageAlign.cpp +++ b/src/pipeline/node/ImageAlign.cpp @@ -353,11 +353,43 @@ void ImageAlign::run() { auto latestConfig = initialConfig; int previousShiftFactor = 0; + bool refreshAlignToTransform = false; ImgTransformation inputAlignToTransform; ImgFrame inputAlignToImgFrame; uint32_t currentEepromId = getParentPipeline().getEepromId(); + auto updateAlignToData = [&](const std::shared_ptr& inputAlignToImg) { + inputAlignToImgFrame = *inputAlignToImg; + + auto alignTransform = inputAlignToImg->transformation; + const auto alignToDistortion = alignTransform.getDistortionCoefficients(); + const bool hasDistortion = std::any_of(alignToDistortion.begin(), alignToDistortion.end(), [](float value) { return std::abs(value) > 0.0f; }); + if(hasDistortion) { + logger->warn( + "The input connected to inputAlignTo is distorted. The aligned image will still be undistorted, meaning it won't be perfectly " + "aligned."); + } + + alignTo = static_cast(inputAlignToImg->getInstanceNum()); + if(alignWidth == 0 || alignHeight == 0) { + alignWidth = inputAlignToImg->getWidth(); + alignHeight = inputAlignToImg->getHeight(); + } + + auto alignTransformForIntrinsics = alignTransform; + auto [alignTransformWidth, alignTransformHeight] = alignTransformForIntrinsics.getSize(); + if(static_cast(alignTransformWidth) != alignWidth || static_cast(alignTransformHeight) != alignHeight) { + float scaleX = static_cast(alignWidth) / static_cast(alignTransformWidth); + float scaleY = static_cast(alignHeight) / static_cast(alignTransformHeight); + alignTransformForIntrinsics.addScale(scaleX, scaleY); + alignTransformForIntrinsics.setSize(alignWidth, alignHeight); + } + + alignSourceIntrinsics = alignTransformForIntrinsics.getIntrinsicMatrix(); + inputAlignToTransform = alignTransformForIntrinsics; + }; + while(mainLoop()) { std::shared_ptr inputImg = nullptr; std::shared_ptr inConfig = nullptr; @@ -371,35 +403,7 @@ void ImageAlign::run() { initialized = true; auto inputAlignToImg = inputAlignTo.get(); - - inputAlignToImgFrame = *inputAlignToImg; - - inputAlignToTransform = inputAlignToImg->transformation; - const auto alignToDistortion = inputAlignToTransform.getDistortionCoefficients(); - const bool hasDistortion = std::any_of(alignToDistortion.begin(), alignToDistortion.end(), [](float value) { return std::abs(value) > 0.0f; }); - if(hasDistortion) { - logger->warn( - "The input connected to inputAlignTo is distorted. The aligned image will still be undistorted, meaning it won't be perfectly " - "aligned."); - } - - alignTo = static_cast(inputAlignToImg->getInstanceNum()); - if(alignWidth == 0 || alignHeight == 0) { - alignWidth = inputAlignToImg->getWidth(); - alignHeight = inputAlignToImg->getHeight(); - } - - auto alignTransformForIntrinsics = inputAlignToTransform; - auto [alignTransformWidth, alignTransformHeight] = alignTransformForIntrinsics.getSize(); - if(static_cast(alignTransformWidth) != alignWidth || static_cast(alignTransformHeight) != alignHeight) { - float scaleX = static_cast(alignWidth) / static_cast(alignTransformWidth); - float scaleY = static_cast(alignHeight) / static_cast(alignTransformHeight); - alignTransformForIntrinsics.addScale(scaleX, scaleY); - alignTransformForIntrinsics.setSize(alignWidth, alignHeight); - } - - alignSourceIntrinsics = alignTransformForIntrinsics.getIntrinsicMatrix(); - inputAlignToTransform = alignTransformForIntrinsics; + updateAlignToData(inputAlignToImg); } if(inputConfig.getWaitForMessage()) { @@ -451,10 +455,22 @@ void ImageAlign::run() { if(latestEepromId > currentEepromId) { logger->debug("EEPROM data changed (ID: {} -> {}), reconfiguring ...", currentEepromId, latestEepromId); calibrationSet = false; + previousShiftFactor = 0; + refreshAlignToTransform = true; calibHandler = pipeline.getCalibrationData(); currentEepromId = latestEepromId; } + if(refreshAlignToTransform) { + if(auto latestAlignToImg = inputAlignTo.tryGet()) { + updateAlignToData(latestAlignToImg); + refreshAlignToTransform = false; + } else { + logger->trace("Waiting for updated inputAlignTo frame after calibration change."); + continue; + } + } + try { extractCalibrationData(width, height, alignWidth, alignHeight); } catch(const std::exception& e) { diff --git a/tests/src/ondevice_tests/image_align_node_test.cpp b/tests/src/ondevice_tests/image_align_node_test.cpp index 9364f36f08..6984692ede 100644 --- a/tests/src/ondevice_tests/image_align_node_test.cpp +++ b/tests/src/ondevice_tests/image_align_node_test.cpp @@ -1,92 +1,227 @@ -#include #include -#include -#include -#include +#include +#include #include "depthai/depthai.hpp" -using namespace std; -using namespace std::chrono; -using namespace std::chrono_literals; - namespace { -void runImageAlignTest(bool useDepth, bool runOnHost, dai::ImgResizeMode resizeMode) { - dai::Pipeline p; - auto rgbCam = p.create()->build(dai::CameraBoardSocket::CAM_A); - auto leftCam = p.create()->build(dai::CameraBoardSocket::CAM_B); - auto rightCam = p.create()->build(dai::CameraBoardSocket::CAM_C); - std::shared_ptr stereo; - auto align = p.create(); - auto* rgbOut = rgbCam->requestOutput({1280, 640}, std::nullopt, resizeMode, std::nullopt, true); - auto* leftOut = leftCam->requestOutput({1280, 800}, std::nullopt); - auto* rightOut = rightCam->requestOutput({1280, 800}, std::nullopt); - - if(useDepth) { - stereo = p.create(); - leftOut->link(stereo->left); - rightOut->link(stereo->right); - stereo->depth.link(align->input); - } else { - leftOut->link(align->input); - rightOut->createOutputQueue(); // TODO remove once left&rgb only streaming on RVC4 is supported - } - rgbOut->link(align->inputAlignTo); - if(!useDepth) { - align->initialConfig->staticDepthPlane = 0x5AB1; - } - if(runOnHost) { - align->setRunOnHost(true); - } +constexpr unsigned kDepthWidth = 64; +constexpr unsigned kDepthHeight = 48; +constexpr float kFocalLength = 100.0f; +constexpr float kPrincipalPointX = static_cast(kDepthWidth) / 2.0f; +constexpr float kPrincipalPointY = static_cast(kDepthHeight) / 2.0f; - auto alignedQueue = align->outputAligned.createOutputQueue(); - auto alignToQueue = rgbOut->createOutputQueue(); - p.start(); +std::array, 3> makeIntrinsics() { + return {{{kFocalLength, 0.0f, kPrincipalPointX}, {0.0f, kFocalLength, kPrincipalPointY}, {0.0f, 0.0f, 1.0f}}}; +} - auto alignToFrame = alignToQueue->get(); - REQUIRE(alignToFrame != nullptr); - const auto alignToIntrinsics = alignToFrame->transformation.getIntrinsicMatrix(); +std::vector> toVectorIntrinsics(const std::array, 3>& intrinsics) { + return { + {intrinsics[0][0], intrinsics[0][1], intrinsics[0][2]}, + {intrinsics[1][0], intrinsics[1][1], intrinsics[1][2]}, + {intrinsics[2][0], intrinsics[2][1], intrinsics[2][2]}, + }; +} - constexpr size_t N = 20; - for(size_t i = 0; i < N; ++i) { - auto aligned = alignedQueue->get(); - REQUIRE(aligned != nullptr); - REQUIRE(aligned->transformation.isAlignedTo(alignToFrame->transformation)); - REQUIRE(aligned->getInstanceNum() == alignToFrame->getInstanceNum()); - } - p.stop(); +std::array, 3> makeScaledIntrinsics(float fx, float fy, float cx, float cy) { + return {{{fx, 0.0f, cx}, {0.0f, fy, cy}, {0.0f, 0.0f, 1.0f}}}; } -} // namespace -TEST_CASE("Test ImageAlign node image to image alignment") { - bool useDepth = false; - bool runOnHost = false; - for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { - runImageAlignTest(useDepth, runOnHost, resizeMode); - } +std::shared_ptr makeAlignToFrame() { + auto intrinsics = makeIntrinsics(); + dai::Extrinsics extrinsics({{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}, {0.0f, 0.0f, 0.0f}, dai::CameraBoardSocket::CAM_A); + dai::ImgTransformation transformation( + kDepthWidth, kDepthHeight, intrinsics, dai::CameraModel::Perspective, std::vector(14, 0.0f), extrinsics); + + auto frame = std::make_shared(); + frame->setWidth(kDepthWidth); + frame->setHeight(kDepthHeight); + frame->setStride(kDepthWidth); + frame->setType(dai::ImgFrame::Type::GRAY8); + frame->setInstanceNum(static_cast(dai::CameraBoardSocket::CAM_A)); + frame->setTransformation(transformation); + frame->setData(std::vector(kDepthWidth * kDepthHeight, 127)); + return frame; } -TEST_CASE("Test ImageAlign node depth to image alignment") { - bool useDepth = true; - bool runOnHost = false; - for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { - runImageAlignTest(useDepth, runOnHost, resizeMode); - } +std::shared_ptr makeAlignToFrameWithTransformationSize( + unsigned transformWidth, unsigned transformHeight, const std::array, 3>& intrinsics) { + dai::Extrinsics extrinsics({{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}, {0.0f, 0.0f, 0.0f}, dai::CameraBoardSocket::CAM_A); + dai::ImgTransformation transformation(kDepthWidth, kDepthHeight, intrinsics, dai::CameraModel::Perspective, std::vector(14, 0.0f), extrinsics); + transformation.setSize(transformWidth, transformHeight); + + auto frame = std::make_shared(); + frame->setWidth(kDepthWidth); + frame->setHeight(kDepthHeight); + frame->setStride(kDepthWidth); + frame->setType(dai::ImgFrame::Type::GRAY8); + frame->setInstanceNum(static_cast(dai::CameraBoardSocket::CAM_A)); + frame->setTransformation(transformation); + frame->setData(std::vector(kDepthWidth * kDepthHeight, 63)); + return frame; } -TEST_CASE("Test ImageAlign node image to image alignment on host") { - bool useDepth = false; - bool runOnHost = true; - for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { - runImageAlignTest(useDepth, runOnHost, resizeMode); +void refreshAlignToTransformation(dai::ImgTransformation& transformation, unsigned alignWidth, unsigned alignHeight) { + auto [transformWidth, transformHeight] = transformation.getSize(); + if(transformWidth != alignWidth || transformHeight != alignHeight) { + const float scaleX = static_cast(alignWidth) / static_cast(transformWidth); + const float scaleY = static_cast(alignHeight) / static_cast(transformHeight); + transformation.addScale(scaleX, scaleY); + transformation.setSize(alignWidth, alignHeight); } } -TEST_CASE("Test ImageAlign node depth to image alignment on host") { - bool useDepth = true; - bool runOnHost = true; - for(const auto resizeMode : {dai::ImgResizeMode::CROP, dai::ImgResizeMode::LETTERBOX, dai::ImgResizeMode::STRETCH}) { - runImageAlignTest(useDepth, runOnHost, resizeMode); +std::shared_ptr makeDepthFrame() { + auto intrinsics = makeIntrinsics(); + dai::Extrinsics extrinsics({{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}, {0.0f, 0.0f, 0.0f}, dai::CameraBoardSocket::CAM_B); + dai::ImgTransformation transformation( + kDepthWidth, kDepthHeight, intrinsics, dai::CameraModel::Perspective, std::vector(14, 0.0f), extrinsics); + + std::vector depthValues(kDepthWidth * kDepthHeight, 0); + for(unsigned y = 8; y < 40; ++y) { + for(unsigned x = 14; x < 20; ++x) { + depthValues[y * kDepthWidth + x] = 1000; + } + for(unsigned x = 34; x < 42; ++x) { + depthValues[y * kDepthWidth + x] = 500; + } } + + std::vector depthBytes(depthValues.size() * sizeof(uint16_t)); + std::memcpy(depthBytes.data(), depthValues.data(), depthBytes.size()); + + auto frame = std::make_shared(); + frame->setWidth(kDepthWidth); + frame->setHeight(kDepthHeight); + frame->setStride(kDepthWidth * sizeof(uint16_t)); + frame->setType(dai::ImgFrame::Type::RAW16); + frame->setInstanceNum(static_cast(dai::CameraBoardSocket::CAM_B)); + frame->setTransformation(transformation); + frame->setData(std::move(depthBytes)); + return frame; +} + +dai::CalibrationHandler makeCalibration(float translationXcm) { + dai::CalibrationHandler handler; + auto eeprom = handler.getEepromData(); + eeprom.stereoUseSpecTranslation = false; + eeprom.stereoEnableDistortionCorrection = true; + handler = dai::CalibrationHandler(eeprom); + + auto intrinsics = toVectorIntrinsics(makeIntrinsics()); + auto distortion = std::vector(14, 0.0f); + auto identityRotation = std::vector>{{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}; + + handler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_A, intrinsics, kDepthWidth, kDepthHeight); + handler.setCameraIntrinsics(dai::CameraBoardSocket::CAM_B, intrinsics, kDepthWidth, kDepthHeight); + handler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_A, distortion); + handler.setDistortionCoefficients(dai::CameraBoardSocket::CAM_B, distortion); + handler.setCameraExtrinsics( + dai::CameraBoardSocket::CAM_B, dai::CameraBoardSocket::CAM_A, identityRotation, {translationXcm, 0.0f, 0.0f}, {translationXcm, 0.0f, 0.0f}); + return handler; +} + +} // namespace + +TEST_CASE("ImageAlign runtime calibration update changes aligned depth output") { + dai::Pipeline pipeline; + auto align = pipeline.create(); + align->setRunOnHost(true); + + auto depthInQ = align->input.createInputQueue(); + auto alignToInQ = align->inputAlignTo.createInputQueue(); + auto alignedOutQ = align->outputAligned.createOutputQueue(); + + auto initialCalibration = makeCalibration(2.0f); + auto updatedCalibration = makeCalibration(5.0f); + + pipeline.getDefaultDevice()->setCalibration(initialCalibration); + pipeline.start(); + + alignToInQ->send(makeAlignToFrame()); + depthInQ->send(makeDepthFrame()); + + auto firstAligned = alignedOutQ->get(); + REQUIRE(firstAligned != nullptr); + REQUIRE(firstAligned->getWidth() == kDepthWidth); + REQUIRE(firstAligned->getHeight() == kDepthHeight); + REQUIRE(firstAligned->getType() == dai::ImgFrame::Type::RAW16); + REQUIRE(firstAligned->getInstanceNum() == static_cast(dai::CameraBoardSocket::CAM_A)); + + pipeline.getDefaultDevice()->setCalibration(updatedCalibration); + + alignToInQ->send(makeAlignToFrame()); + depthInQ->send(makeDepthFrame()); + + auto secondAligned = alignedOutQ->get(); + REQUIRE(secondAligned != nullptr); + REQUIRE(secondAligned->getWidth() == kDepthWidth); + REQUIRE(secondAligned->getHeight() == kDepthHeight); + REQUIRE(secondAligned->getType() == dai::ImgFrame::Type::RAW16); + REQUIRE(secondAligned->getInstanceNum() == static_cast(dai::CameraBoardSocket::CAM_A)); + + REQUIRE(firstAligned->transformation.isEqualTransformation(secondAligned->transformation)); + + const auto firstData = firstAligned->getData(); + const auto secondData = secondAligned->getData(); + REQUIRE(firstData.size() == secondData.size()); + REQUIRE_FALSE(std::equal(firstData.begin(), firstData.end(), secondData.begin(), secondData.end())); + + pipeline.stop(); +} + +TEST_CASE("ImageAlign refreshes align-to intrinsics after runtime calibration update") { + dai::Pipeline pipeline; + auto align = pipeline.create(); + align->setRunOnHost(true); + + auto depthInQ = align->input.createInputQueue(); + auto alignToInQ = align->inputAlignTo.createInputQueue(); + auto alignedOutQ = align->outputAligned.createOutputQueue(); + + const auto initialCalibration = makeCalibration(2.0f); + const auto updatedCalibration = makeCalibration(5.0f); + + const auto preUpdateIntrinsics = makeScaledIntrinsics(80.0f, 84.0f, 15.0f, 11.0f); + const auto postUpdateIntrinsics = makeScaledIntrinsics(124.0f, 118.0f, 19.0f, 17.0f); + + auto preUpdateAlignTo = makeAlignToFrameWithTransformationSize(32, 24, preUpdateIntrinsics); + auto postUpdateAlignTo = makeAlignToFrameWithTransformationSize(48, 36, postUpdateIntrinsics); + + auto expectedPreTransform = preUpdateAlignTo->transformation; + auto expectedPostTransform = postUpdateAlignTo->transformation; + refreshAlignToTransformation(expectedPreTransform, kDepthWidth, kDepthHeight); + refreshAlignToTransformation(expectedPostTransform, kDepthWidth, kDepthHeight); + + pipeline.getDefaultDevice()->setCalibration(initialCalibration); + pipeline.start(); + + alignToInQ->send(preUpdateAlignTo); + depthInQ->send(makeDepthFrame()); + + auto firstAligned = alignedOutQ->get(); + REQUIRE(firstAligned != nullptr); + REQUIRE(firstAligned->getWidth() == kDepthWidth); + REQUIRE(firstAligned->getHeight() == kDepthHeight); + REQUIRE(firstAligned->getInstanceNum() == static_cast(dai::CameraBoardSocket::CAM_A)); + REQUIRE(firstAligned->transformation.isEqualTransformation(expectedPreTransform)); + REQUIRE(firstAligned->transformation.getIntrinsicMatrix() == expectedPreTransform.getIntrinsicMatrix()); + + pipeline.getDefaultDevice()->setCalibration(updatedCalibration); + + alignToInQ->send(postUpdateAlignTo); + depthInQ->send(makeDepthFrame()); + + auto secondAligned = alignedOutQ->get(); + REQUIRE(secondAligned != nullptr); + REQUIRE(secondAligned->getWidth() == kDepthWidth); + REQUIRE(secondAligned->getHeight() == kDepthHeight); + REQUIRE(secondAligned->getInstanceNum() == static_cast(dai::CameraBoardSocket::CAM_A)); + REQUIRE(secondAligned->transformation.isEqualTransformation(expectedPostTransform)); + REQUIRE(secondAligned->transformation.getIntrinsicMatrix() == expectedPostTransform.getIntrinsicMatrix()); + REQUIRE_FALSE(secondAligned->transformation.isEqualTransformation(expectedPreTransform)); + REQUIRE(secondAligned->transformation.getIntrinsicMatrix() != firstAligned->transformation.getIntrinsicMatrix()); + + pipeline.stop(); }