From d161d211de680ecf04ce52f8bea4b855d75fda8b Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 26 Sep 2025 13:33:18 +0200 Subject: [PATCH 1/3] Add missing references --- src/pipeline/node/DynamicCalibrationNode.cpp | 26 +++++++++---------- src/pipeline/node/DynamicCalibrationUtils.hpp | 20 +++++++------- 2 files changed, 22 insertions(+), 24 deletions(-) diff --git a/src/pipeline/node/DynamicCalibrationNode.cpp b/src/pipeline/node/DynamicCalibrationNode.cpp index cd32d0f91..1d145b97e 100644 --- a/src/pipeline/node/DynamicCalibrationNode.cpp +++ b/src/pipeline/node/DynamicCalibrationNode.cpp @@ -55,8 +55,8 @@ std::pair, std::shared_ptr resolutionA, - const std::pair resolutionB) { + const std::pair& resolutionA, + const std::pair& resolutionB) { // clang-format off std::shared_ptr calibA = DclUtils::createDclCalibration( currentCalibration.getCameraIntrinsics(boardSocketA, resolutionA.first, resolutionA.second, Point2f(), Point2f(),false), @@ -96,10 +96,10 @@ dcl::PerformanceMode DclUtils::daiPerformanceModeToDclPerformanceMode(const dai: } #define DCL_DISTORTION_SIZE (14) -std::shared_ptr DclUtils::createDclCalibration(const std::vector> cameraMatrix, - const std::vector distortionCoefficients, - const std::vector> rotationMatrix, - const std::vector translationVector) { +std::shared_ptr DclUtils::createDclCalibration(const std::vector>& cameraMatrix, + const std::vector& distortionCoefficients, + const std::vector>& rotationMatrix, + const std::vector& translationVector) { dcl::scalar_t cameraMatrixArr[9]; dcl::scalar_t distortion[DCL_DISTORTION_SIZE] = {0}; dcl::scalar_t rvec[3]; @@ -131,12 +131,12 @@ std::shared_ptr DclUtils::createDclCalibration(con } void DclUtils::convertDclCalibrationToDai(CalibrationHandler& calibHandler, - const std::shared_ptr dclCalibrationA, - const std::shared_ptr dclCalibrationB, + const std::shared_ptr& dclCalibrationA, + const std::shared_ptr& dclCalibrationB, const CameraBoardSocket socketSrc, const CameraBoardSocket socketDest, - const std::pair resolutionA, - const std::pair resolutionB) { + const std::pair& resolutionA, + const std::pair& resolutionB) { dcl::scalar_t tvecA[3]; dclCalibrationA->getTvec(tvecA); dcl::scalar_t rvecA[3]; @@ -389,10 +389,8 @@ DynamicCalibration::ErrorCode DynamicCalibration::initializePipeline(const std:: return DynamicCalibration::ErrorCode::PIPELINE_INITIALIZATION_FAILED; } - resolutionA.first = leftFrame->getWidth(); - resolutionA.second = leftFrame->getHeight(); - resolutionB.first = rightFrame->getWidth(); - resolutionB.second = rightFrame->getHeight(); + resolutionA = std::make_pair(leftFrame->getWidth(), leftFrame->getHeight()); + resolutionB = std::make_pair(rightFrame->getWidth(), rightFrame->getHeight()); daiSocketA = static_cast(leftFrame->instanceNum); daiSocketB = static_cast(rightFrame->instanceNum); diff --git a/src/pipeline/node/DynamicCalibrationUtils.hpp b/src/pipeline/node/DynamicCalibrationUtils.hpp index dc038ec61..1b650579f 100644 --- a/src/pipeline/node/DynamicCalibrationUtils.hpp +++ b/src/pipeline/node/DynamicCalibrationUtils.hpp @@ -10,24 +10,24 @@ namespace dai { namespace node { struct DclUtils { static void convertDclCalibrationToDai(CalibrationHandler& calibHandler, - const std::shared_ptr dclCalibrationA, - const std::shared_ptr dclCalibrationB, + const std::shared_ptr& dclCalibrationA, + const std::shared_ptr& dclCalibrationB, const CameraBoardSocket socketSrc, const CameraBoardSocket socketDest, - const std::pair resolutionA, - const std::pair resolutionB); + const std::pair& resolutionA, + const std::pair& resolutionB); - static std::shared_ptr createDclCalibration(const std::vector> cameraMatrix, - const std::vector distortionCoefficients, - const std::vector> rotationMatrix, - const std::vector translationVector); + static std::shared_ptr createDclCalibration(const std::vector>& cameraMatrix, + const std::vector& distortionCoefficients, + const std::vector>& rotationMatrix, + const std::vector& translationVector); static std::pair, std::shared_ptr> convertDaiCalibrationToDcl( const CalibrationHandler& currentCalibration, const CameraBoardSocket boardSocketA, const CameraBoardSocket boardSocketB, - const std::pair resolutionA, - const std::pair resolutionB); + const std::pair& resolutionA, + const std::pair& resolutionB); static dcl::ImageData cvMatToImageData(const cv::Mat& mat); From b4b716c1597d247a8b90f5388a22370c24534815 Mon Sep 17 00:00:00 2001 From: Jakub Fara Date: Fri, 26 Sep 2025 13:06:32 +0200 Subject: [PATCH 2/3] Update DCL api in DynamicCalibrationNode --- .../datatype/DynamicCalibrationResults.hpp | 2 +- src/pipeline/node/DynamicCalibrationNode.cpp | 109 ++++++++++-------- src/pipeline/node/DynamicCalibrationUtils.hpp | 3 +- 3 files changed, 61 insertions(+), 53 deletions(-) diff --git a/include/depthai/pipeline/datatype/DynamicCalibrationResults.hpp b/include/depthai/pipeline/datatype/DynamicCalibrationResults.hpp index dd99fe5c5..5f50ed37f 100644 --- a/include/depthai/pipeline/datatype/DynamicCalibrationResults.hpp +++ b/include/depthai/pipeline/datatype/DynamicCalibrationResults.hpp @@ -85,7 +85,7 @@ struct CalibrationQuality : public Buffer { * Reported at reference distances [1m, 2m, 5m, 10m]. * Units: percent [%]. */ - std::vector depthErrorDifference; + std::vector depthErrorDifference; /** * Current calibration Sampson error. diff --git a/src/pipeline/node/DynamicCalibrationNode.cpp b/src/pipeline/node/DynamicCalibrationNode.cpp index 1d145b97e..d9f714b3d 100644 --- a/src/pipeline/node/DynamicCalibrationNode.cpp +++ b/src/pipeline/node/DynamicCalibrationNode.cpp @@ -22,10 +22,7 @@ class DynamicCalibration::Impl { */ std::shared_ptr sensorA; std::shared_ptr sensorB; - std::shared_ptr dcDevice; - dcl::mxid_t deviceName; - dcl::socket_t socketA; - dcl::socket_t socketB; + std::shared_ptr device; dcl::DynamicCalibration dynCalibImpl; }; @@ -66,13 +63,15 @@ std::pair, std::shared_ptr calibB = DclUtils::createDclCalibration( currentCalibration.getCameraIntrinsics(boardSocketB, resolutionB.first, resolutionB.second, Point2f(), Point2f(),false), currentCalibration.getDistortionCoefficients(boardSocketB), currentCalibration.getCameraRotationMatrix(boardSocketA, boardSocketB), - currentCalibration.getCameraTranslationVector(boardSocketA, boardSocketB, false) + currentCalibration.getCameraTranslationVector(boardSocketA, boardSocketB, false), + currentCalibration.getDistortionModel(boardSocketB) ); // clang-format on return std::make_pair(calibA, calibB); @@ -95,13 +94,14 @@ dcl::PerformanceMode DclUtils::daiPerformanceModeToDclPerformanceMode(const dai: } } -#define DCL_DISTORTION_SIZE (14) +#define DCL_PERSPECTIVE_DISTORTION_SIZE (14) +#define DCL_FISHEYE_DISTORTION_SIZE (4) std::shared_ptr DclUtils::createDclCalibration(const std::vector>& cameraMatrix, const std::vector& distortionCoefficients, const std::vector>& rotationMatrix, - const std::vector& translationVector) { + const std::vector& translationVector, + const CameraModel distortionModel) { dcl::scalar_t cameraMatrixArr[9]; - dcl::scalar_t distortion[DCL_DISTORTION_SIZE] = {0}; dcl::scalar_t rvec[3]; dcl::scalar_t tvec[3]; @@ -112,11 +112,6 @@ std::shared_ptr DclUtils::createDclCalibration(con } } - // Convert distortion - for(size_t i = 0; i < DCL_DISTORTION_SIZE; ++i) { - distortion[i] = static_cast(distortionCoefficients[i]); - } - // Convert rotation to vector std::vector rvecVec = matrix::rotationMatrixToVector(rotationMatrix); for(int i = 0; i < 3; ++i) { @@ -127,7 +122,37 @@ std::shared_ptr DclUtils::createDclCalibration(con tvec[i] = static_cast(translationVector[i] / 100.0f); // Convert to m } - return std::make_shared(rvec, tvec, cameraMatrixArr, distortion); + dcl::distortion_t dclDistortion; + if(distortionModel == dai::CameraModel::Perspective) { + // Convert distortion + dcl::scalar_t distortion[DCL_PERSPECTIVE_DISTORTION_SIZE] = {0}; + for(size_t i = 0; i < DCL_PERSPECTIVE_DISTORTION_SIZE; ++i) { + distortion[i] = static_cast(distortionCoefficients[i]); + } + dclDistortion = dcl::PerspectiveDistortion::fromArray(distortion); + } else if(distortionModel == dai::CameraModel::Fisheye) { + dcl::scalar_t distortion[DCL_FISHEYE_DISTORTION_SIZE] = {0}; + for(size_t i = 0; i < DCL_FISHEYE_DISTORTION_SIZE; ++i) { + distortion[i] = static_cast(distortionCoefficients[i]); + } + dclDistortion = dcl::FisheyeDistortion::fromArray(distortion); + } else { + throw std::runtime_error("Unsupported distortion model"); + } + return std::make_shared(rvec, tvec, cameraMatrixArr, dclDistortion); +} + +std::vector distortionToVector(const dcl::distortion_t& dist) { + return std::visit( + [](auto&& d) -> std::vector { + using T = std::decay_t; + if constexpr(std::is_same_v) { + return std::vector(std::begin(d.data), std::end(d.data)); + } else if constexpr(std::is_same_v) { + return std::vector(std::begin(d.data), std::end(d.data)); + } + }, + dist); } void DclUtils::convertDclCalibrationToDai(CalibrationHandler& calibHandler, @@ -154,7 +179,7 @@ void DclUtils::convertDclCalibrationToDai(CalibrationHandler& calibHandler, throw std::runtime_error("Extrinsics of the left camera are not zero within the allowed threshold."); } - dcl::scalar_t distortionA[14]; + dcl::distortion_t distortionA; dclCalibrationA->getDistortion(distortionA); dcl::scalar_t cameraMatrixA[9]; @@ -167,7 +192,7 @@ void DclUtils::convertDclCalibrationToDai(CalibrationHandler& calibHandler, }; // clang-format on - dcl::scalar_t distortionB[14]; + dcl::distortion_t distortionB; dclCalibrationB->getDistortion(distortionB); dcl::scalar_t cameraMatrixB[9]; @@ -192,8 +217,8 @@ void DclUtils::convertDclCalibrationToDai(CalibrationHandler& calibHandler, calibHandler.setCameraIntrinsics(socketSrc, matA, resolutionA.first, resolutionA.second); calibHandler.setCameraIntrinsics(socketDest, matB, resolutionB.first, resolutionB.second); - calibHandler.setDistortionCoefficients(socketSrc, std::vector(distortionA, distortionA + 14)); - calibHandler.setDistortionCoefficients(socketDest, std::vector(distortionB, distortionB + 14)); + calibHandler.setDistortionCoefficients(socketSrc, distortionToVector(distortionA)); + calibHandler.setDistortionCoefficients(socketDest, distortionToVector(distortionB)); auto specTranslation = calibHandler.getCameraTranslationVector(socketSrc, socketDest, true); calibHandler.setCameraExtrinsics(socketSrc, socketDest, rotationMatrix, translation, specTranslation); } @@ -238,18 +263,18 @@ dai::CalibrationQuality calibQualityfromDCL(const dcl::CalibrationDifference& sr } void DynamicCalibration::setCalibration(CalibrationHandler& handler) { - logger->info("Applying calibration to device: {}", pimplDCL->deviceName); + logger->info("Applying calibration to device"); device->setCalibration(handler); auto [calibA, calibB] = DclUtils::convertDaiCalibrationToDcl(handler, daiSocketA, daiSocketB, resolutionA, resolutionB); - pimplDCL->dynCalibImpl.setNewCalibration(pimplDCL->deviceName, pimplDCL->socketA, calibA->getCalibration()); - pimplDCL->dynCalibImpl.setNewCalibration(pimplDCL->deviceName, pimplDCL->socketB, calibB->getCalibration()); + pimplDCL->sensorA->setCalibration(calibA); + pimplDCL->sensorB->setCalibration(calibB); } DynamicCalibration::ErrorCode DynamicCalibration::runQualityCheck(const bool force) { dcl::PerformanceMode pm = force ? dcl::PerformanceMode::SKIP_CHECKS : DclUtils::daiPerformanceModeToDclPerformanceMode(performanceMode); logger->info("Running calibration quality check (force={} mode={})", force, static_cast(pm)); - auto dclResult = pimplDCL->dynCalibImpl.checkCalibration(pimplDCL->dcDevice, pimplDCL->socketA, pimplDCL->socketB, pm); + auto dclResult = pimplDCL->dynCalibImpl.checkCalibration(pimplDCL->sensorA, pimplDCL->sensorB, pm); if(!dclResult.passed()) { auto result = std::make_shared(); @@ -272,7 +297,7 @@ DynamicCalibration::ErrorCode DynamicCalibration::runQualityCheck(const bool for DynamicCalibration::ErrorCode DynamicCalibration::runCalibration(const dai::CalibrationHandler& currentHandler, const bool force) { dcl::PerformanceMode pm = force ? dcl::PerformanceMode::SKIP_CHECKS : DclUtils::daiPerformanceModeToDclPerformanceMode(performanceMode); logger->info("Running calibration (force={} mode={})", force, static_cast(pm)); - auto dclResult = pimplDCL->dynCalibImpl.findNewCalibration(pimplDCL->dcDevice, pimplDCL->socketA, pimplDCL->socketB, pm); + auto dclResult = pimplDCL->dynCalibImpl.findNewCalibration(pimplDCL->sensorA, pimplDCL->sensorB, pm); if(!dclResult.passed()) { auto result = std::make_shared(dclResult.errorMessage()); logger->info("WARNING: Calibration failed: {}", dclResult.errorMessage()); @@ -342,12 +367,8 @@ DynamicCalibration::ErrorCode DynamicCalibration::runLoadImage(const bool blocki rightFrame->getHeight(), timestamp); - pimplDCL->dynCalibImpl.loadStereoImagePair(DclUtils::cvMatToImageData(leftCvFrame), - DclUtils::cvMatToImageData(rightCvFrame), - pimplDCL->deviceName, - pimplDCL->socketA, - pimplDCL->socketB, - timestamp); + pimplDCL->dynCalibImpl.loadStereoImagePair( + DclUtils::cvMatToImageData(leftCvFrame), DclUtils::cvMatToImageData(rightCvFrame), pimplDCL->sensorA, pimplDCL->sensorB, timestamp); return DynamicCalibration::ErrorCode::OK; } @@ -369,7 +390,7 @@ DynamicCalibration::ErrorCode DynamicCalibration::computeCoverage() { coverageResult->coverageAcquired = coverage.coverageAcquired; coverageResult->dataAcquired = coverage.dataAcquired; - logger->info("Computing coverage for sockets A={} and B={}", static_cast(pimplDCL->socketA), static_cast(pimplDCL->socketB)); + logger->info("Computing coverage"); coverageOutput.send(coverageResult); @@ -399,18 +420,7 @@ DynamicCalibration::ErrorCode DynamicCalibration::initializePipeline(const std:: return DynamicCalibration::ErrorCode::PIPELINE_INITIALIZATION_FAILED; } - logger->info("Detected sockets: A={} B={}, resolution={}x{} resolution{}x{}", - static_cast(daiSocketA), - static_cast(daiSocketB), - resolutionA.first, - resolutionA.second, - resolutionB.first, - resolutionB.second); - - pimplDCL->socketA = static_cast(daiSocketA); - pimplDCL->socketB = static_cast(daiSocketB); - - logger->info("Converting dai calibration to dcl for sockets A={} B={}", static_cast(daiSocketA), static_cast(daiSocketB)); + logger->info("Converting dai calibration to dcl"); calibrationHandler = daiDevice->getCalibration(); auto eepromData = calibrationHandler.getEepromData(); @@ -421,17 +431,14 @@ DynamicCalibration::ErrorCode DynamicCalibration::initializePipeline(const std:: auto [calibA, calibB] = DclUtils::convertDaiCalibrationToDcl(calibrationHandler, daiSocketA, daiSocketB, resolutionA, resolutionB); // set up the dynamic calibration - pimplDCL->deviceName = daiDevice->getDeviceId(); - pimplDCL->dcDevice = pimplDCL->dynCalibImpl.addDevice(pimplDCL->deviceName); + pimplDCL->device = pimplDCL->dynCalibImpl.addDevice(); dcl::resolution_t resolutionDclA{static_cast(resolutionA.first), static_cast(resolutionA.second)}; dcl::resolution_t resolutionDclB{static_cast(resolutionB.first), static_cast(resolutionB.second)}; - pimplDCL->sensorA = std::make_shared(calibA, resolutionDclA); - pimplDCL->sensorB = std::make_shared(calibB, resolutionDclB); - logger->info("Added sensors for sockets A={} and B={} to dynCalibImpl", static_cast(pimplDCL->socketA), static_cast(pimplDCL->socketB)); + logger->info("Added sensors to dynCalibImpl"); - pimplDCL->dynCalibImpl.addSensor(pimplDCL->deviceName, pimplDCL->sensorA, pimplDCL->socketA); - pimplDCL->dynCalibImpl.addSensor(pimplDCL->deviceName, pimplDCL->sensorB, pimplDCL->socketB); + pimplDCL->sensorA = pimplDCL->dynCalibImpl.addSensor(pimplDCL->device, calibA, resolutionDclA); + pimplDCL->sensorB = pimplDCL->dynCalibImpl.addSensor(pimplDCL->device, calibB, resolutionDclB); return DynamicCalibration::ErrorCode::OK; } @@ -478,7 +485,7 @@ DynamicCalibration::ErrorCode DynamicCalibration::evaluateCommand(const std::sha // Apply calibration else if(std::holds_alternative(cmd)) { const auto& c = std::get(cmd); - logger->info("Received ApplyCalibrationCommand: applying new calibration to device {}", pimplDCL->deviceName); + logger->info("Received ApplyCalibrationCommand: applying new calibration to device"); calibrationHandler = c.calibration; setCalibration(calibrationHandler); return ErrorCode::OK; diff --git a/src/pipeline/node/DynamicCalibrationUtils.hpp b/src/pipeline/node/DynamicCalibrationUtils.hpp index 1b650579f..b24b58498 100644 --- a/src/pipeline/node/DynamicCalibrationUtils.hpp +++ b/src/pipeline/node/DynamicCalibrationUtils.hpp @@ -20,7 +20,8 @@ struct DclUtils { static std::shared_ptr createDclCalibration(const std::vector>& cameraMatrix, const std::vector& distortionCoefficients, const std::vector>& rotationMatrix, - const std::vector& translationVector); + const std::vector& translationVector, + const CameraModel distortionModel); static std::pair, std::shared_ptr> convertDaiCalibrationToDcl( const CalibrationHandler& currentCalibration, From b8d7a1b9cf08fe353c1ca994f69c42a6c39a484c Mon Sep 17 00:00:00 2001 From: MaticTonin Date: Fri, 14 Nov 2025 11:29:10 +0100 Subject: [PATCH 3/3] Update the DCL to 0.3.0rc2 --- cmake/Depthai/DepthaiDynamicCalibrationConfig.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Depthai/DepthaiDynamicCalibrationConfig.cmake b/cmake/Depthai/DepthaiDynamicCalibrationConfig.cmake index 23e0835fe..2037f1c92 100644 --- a/cmake/Depthai/DepthaiDynamicCalibrationConfig.cmake +++ b/cmake/Depthai/DepthaiDynamicCalibrationConfig.cmake @@ -1,2 +1,2 @@ # "full commit hash of Dynamic calibration library" -set(DEPTHAI_DYNAMIC_CALIBRATION_VERSION "8b257073638de81e14a195777ac76e3cfe304001") +set(DEPTHAI_DYNAMIC_CALIBRATION_VERSION "f124a388f05e8972bd8211c17959aa8f963e9e16")