Skip to content

ImageAlign: Use imageTransformations #1905

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JakubFara wants to merge 7 commits into
base: develop
Choose a base branch
from
Open

ImageAlign: Use imageTransformations #1905

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2785a7b
Use imageTransformations
JakubFara Jul 21, 2026
a8c557b
Fix metadata
JakubFara Jul 22, 2026
a5ded13
Fix previousShiftFactor
JakubFara Jul 22, 2026
0d0410b
Metadata is properly set for the alignedImg
aljazkonec1 Jul 23, 2026
e3c99c4
inputAlignTo should not be blocking get for every input frame. Expand…
aljazkonec1 Jul 23, 2026
95742d2
Improve test flaky-ness and expose trySend on InputQueue node
aljazkonec1 Jul 24, 2026
8ee2055
Bump FW
aljazkonec1 Jul 24, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions bindings/python/src/pipeline/node/NodeBindings.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -337,6 +337,7 @@ void NodeBindings::bind(pybind11::module& m, void* pCallstack) {

py::class_<InputQueue, std::shared_ptr<InputQueue>> pyInputQueue(m, "InputQueue", DOC(dai, InputQueue));
pyInputQueue.def("send", &InputQueue::send, py::arg("msg"), DOC(dai, InputQueue, send));
pyInputQueue.def("trySend", &InputQueue::trySend, py::arg("msg"), DOC(dai, InputQueue, trySend));

// Node::Id bindings
py::class_<Node::Id>(pyNode, "Id", "Node identificator. Unique for every node on a single Pipeline");
Expand Down
2 changes: 1 addition & 1 deletion cmake/Depthai/DepthaiDeviceRVC4Config.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,4 +3,4 @@
set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot")

# "version if applicable"
set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+b52f10a00fee1fd8eb886753c8801a808f7197ba")
set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+b52f10a00fee1fd8eb886753c8801a808f7197ba")
10 changes: 10 additions & 0 deletions include/depthai/pipeline/InputQueue.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,13 @@ class InputQueue {
*/
void send(const std::shared_ptr<ADatatype>& msg);

/**
* @brief Try to send a message to the connected input without waiting for queue space.
*
* @returns True if the message was accepted by the host-side input queue.
*/
bool trySend(const std::shared_ptr<ADatatype>& msg);

private:
/**
* @brief Construct a new Input Queue object. The constructor is private as we only want to expose the relevant methods - only send for now
Expand All @@ -33,6 +40,9 @@ class InputQueue {
/** Send message from host*/
void send(const std::shared_ptr<ADatatype>& msg);

/** Try to send a message from host without waiting for queue space */
bool trySend(const std::shared_ptr<ADatatype>& msg);

void run() override;
const char* getName() const override;
bool isBuiltInNode() const override {
Expand Down
8 changes: 8 additions & 0 deletions src/pipeline/InputQueue.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,10 @@ void InputQueue::send(const std::shared_ptr<ADatatype>& msg) {
inputQueueNode->send(msg);
}

bool InputQueue::trySend(const std::shared_ptr<ADatatype>& msg) {
return inputQueueNode->trySend(msg);
}

InputQueue::InputQueue(unsigned int maxSize, bool blocking) : inputQueueNode(std::make_shared<InputQueueNode>(maxSize, blocking)) {}

InputQueue::InputQueueNode::InputQueueNode(unsigned int maxSize, bool blocking) : ThreadedHostNode() {
Expand All @@ -23,6 +27,10 @@ void InputQueue::InputQueueNode::send(const std::shared_ptr<ADatatype>& msg) {
input.send(msg);
}

bool InputQueue::InputQueueNode::trySend(const std::shared_ptr<ADatatype>& msg) {
return input.trySend(msg);
}

const char* InputQueue::InputQueueNode::getName() const {
return "InputQueue";
}
Expand Down
98 changes: 43 additions & 55 deletions src/pipeline/node/ImageAlign.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
#include <sstream>
#include <unordered_set>

#include "depthai/pipeline/Pipeline.hpp"
#include "pipeline/ThreadedNodeImpl.hpp"

#if defined(DEPTHAI_HAVE_OPENCV_SUPPORT)
Expand Down Expand Up @@ -67,20 +66,6 @@ void ImageAlign::run() {

namespace {

template <typename T>
std::vector<T> flatten(const std::vector<std::vector<T> >& orig) {
std::vector<T> ret;
for(const auto& v : orig) ret.insert(ret.end(), v.begin(), v.end());
return ret;
}

cv::Mat vecToCvMat(int rows, int cols, int type, const std::vector<std::vector<float> >& orig) {
std::vector<float> flat = flatten(orig);
cv::Mat cvMat = cv::Mat(rows, cols, type);
memcpy(cvMat.data, flat.data(), flat.size() * sizeof(flat[0]));
return cvMat;
}

cv::Mat vecToCvMat(int rows, int cols, int type, const std::vector<float>& orig) {
cv::Mat cvMat = cv::Mat(rows, cols, type);
memcpy(cvMat.data, orig.data(), orig.size() * sizeof(orig[0]));
Expand Down Expand Up @@ -194,13 +179,13 @@ void ImageAlign::run() {
using namespace std::chrono;
auto& logger = pimpl->logger;

dai::CalibrationHandler calibHandler;

bool calibrationSet = false;
std::array<std::array<float, 3>, 3> depthSourceIntrinsics;
std::array<std::array<float, 3>, 3> alignSourceIntrinsics;
std::array<std::array<float, 4>, 4> depthToAlignExtrinsics;
std::vector<float> depthDistortionCoefficients;
ImgTransformation depthSourceTransformation;
ImgTransformation alignToTransformation;

dai::CameraBoardSocket alignFrom;
dai::CameraBoardSocket alignTo;
Expand Down Expand Up @@ -247,22 +232,18 @@ void ImageAlign::run() {
if(depthDistortionCoefficients.empty()) {
depthDistortionCoefficients.assign(14, 0.0f);
}
auto alignDistortionCoefficients = calibHandler.getDistortionCoefficients(alignTo);

auto depthToAlignRotation = calibHandler.getCameraRotationMatrix(alignFrom, alignTo);
auto depthToAlignTranslation = calibHandler.getCameraTranslationVector(alignFrom, alignTo, false);

for(auto& t : depthToAlignTranslation) {
t *= 10; // convert to mm
}
const auto alignDistortionCoefficients = alignToTransformation.getDistortionCoefficients();
const auto depthToAlignRotation = depthSourceTransformation.getRotationMatrixTo(alignToTransformation);
const auto depthToAlignTranslationArray = depthSourceTransformation.getTranslationVectorTo(alignToTransformation, false, LengthUnit::MILLIMETER);
const std::vector<float> depthToAlignTranslation(depthToAlignTranslationArray.begin(), depthToAlignTranslationArray.end());

auto cv_M1 = arrayToCvMat(3, 3, CV_32FC1, depthSourceIntrinsics);
auto cv_M2 = arrayToCvMat(3, 3, CV_32FC1, alignSourceIntrinsics);

auto cv_d1 = vecToCvMat(1, depthDistortionCoefficients.size(), CV_32FC1, depthDistortionCoefficients);
auto cv_dNone = vecToCvMat(
1, alignDistortionCoefficients.size(), CV_32FC1, std::vector<float>(alignDistortionCoefficients.size(), 0.0f)); // No distortion for aligned frame
auto cv_R = vecToCvMat(3, 3, CV_32FC1, depthToAlignRotation);
auto cv_R = arrayToCvMat(3, 3, CV_32FC1, depthToAlignRotation);
auto cv_T = vecToCvMat(1, 3, CV_32FC1, depthToAlignTranslation);

cv::Mat cv_R1, cv_R2;
Expand Down Expand Up @@ -336,14 +317,6 @@ void ImageAlign::run() {

auto shiftMesh = [](cv::Mat& meshX, int shiftX) { meshX = meshX + cv::Scalar(shiftX); };

auto pipeline = getParentPipeline();

try {
calibHandler = pipeline.getDefaultDevice()->getCalibration();
} catch(const std::exception& e) {
logger->error("Failed to get calibration data: {}", e.what());
}

alignWidth = properties.alignWidth;
alignHeight = properties.alignHeight;
// bool keepAspectRatio = properties.outKeepAspectRatio;
Expand All @@ -355,9 +328,9 @@ void ImageAlign::run() {
int previousShiftFactor = 0;

ImgTransformation inputAlignToTransform;
ImgFrame inputAlignToImgFrame;
uint32_t currentEepromId = getParentPipeline().getEepromId();

ImgTransformation previousInputTransformation;
ImgTransformation previousInputAlignToTransformation;
std::shared_ptr<ImgFrame> inputAlignToImg = inputAlignTo.get<ImgFrame>();
while(mainLoop()) {
std::shared_ptr<ImgFrame> inputImg = nullptr;
std::shared_ptr<ImageAlignConfig> inConfig = nullptr;
Expand All @@ -366,15 +339,21 @@ void ImageAlign::run() {
auto blockEvent = this->inputBlockEvent();

inputImg = input.get<ImgFrame>();
auto newInputAlignToImg = inputAlignTo.tryGet<ImgFrame>();
if(newInputAlignToImg) {
inputAlignToImg = newInputAlignToImg;
}

if(!initialized) {
initialized = true;

auto inputAlignToImg = inputAlignTo.get<ImgFrame>();

inputAlignToImgFrame = *inputAlignToImg;
if(!previousInputTransformation.isEqualTransformation(inputImg->transformation)
|| !previousInputAlignToTransformation.isEqualTransformation(inputAlignToImg->transformation)) {
initialized = false;
calibrationSet = false;
previousShiftFactor = 0;
}
Comment thread
coderabbitai[bot] marked this conversation as resolved.

inputAlignToTransform = inputAlignToImg->transformation;
if(!initialized) {
alignToTransformation = inputAlignToImg->transformation;
Comment thread
coderabbitai[bot] marked this conversation as resolved.
inputAlignToTransform = alignToTransformation;
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) {
Expand All @@ -400,6 +379,10 @@ void ImageAlign::run() {

alignSourceIntrinsics = alignTransformForIntrinsics.getIntrinsicMatrix();
inputAlignToTransform = alignTransformForIntrinsics;

previousInputTransformation = inputImg->transformation;
previousInputAlignToTransformation = inputAlignToImg->transformation;
initialized = true;
}

if(inputConfig.getWaitForMessage()) {
Expand Down Expand Up @@ -446,14 +429,7 @@ void ImageAlign::run() {
throw std::runtime_error(msg);
}

uint32_t latestEepromId = getParentPipeline().getEepromId();

if(latestEepromId > currentEepromId) {
logger->debug("EEPROM data changed (ID: {} -> {}), reconfiguring ...", currentEepromId, latestEepromId);
calibrationSet = false;
calibHandler = pipeline.getCalibrationData();
currentEepromId = latestEepromId;
}
depthSourceTransformation = inputImg->transformation;

try {
extractCalibrationData(width, height, alignWidth, alignHeight);
Expand Down Expand Up @@ -566,11 +542,25 @@ void ImageAlign::run() {
t1 = steady_clock::now();
}

alignedImg->setMetadata(inputAlignToImgFrame);
// manually set metadata
alignedImg->cam = inputImg->cam;
alignedImg->category = inputImg->category;
alignedImg->event = inputImg->event;
alignedImg->sourceFb = inputAlignToImg->sourceFb;
alignedImg->setWidth(alignWidth);
alignedImg->setHeight(alignHeight);
alignedImg->setType(inputImg->getType());
alignedImg->fb.stride = alignedImg->fb.width * alignedImg->getBytesPerPixel();
alignedImg->fb.p1Offset = 0;
alignedImg->fb.p2Offset = 0;
alignedImg->fb.p3Offset = 0;
if(alignedImg->getType() == ImgFrame::Type::NV12) {
alignedImg->fb.p2Offset = alignedImg->fb.stride * alignedImg->fb.height;
alignedImg->fb.p3Offset = alignedImg->fb.p2Offset;
} else if(alignedImg->getType() == ImgFrame::Type::YUV420p) {
alignedImg->fb.p2Offset = alignedImg->fb.stride * alignedImg->fb.height;
alignedImg->fb.p3Offset = alignedImg->fb.p2Offset + (alignedImg->fb.stride / 2) * (alignedImg->fb.height / 2);
Comment thread
coderabbitai[bot] marked this conversation as resolved.
}

auto warp2InputFrame = warp2Input->getFrame();
auto alignedImgFrame = alignedImg->getFrame();
Expand All @@ -592,9 +582,7 @@ void ImageAlign::run() {
}

alignedImg->setInstanceNum((uint32_t)alignTo);

alignedImg->setBufferMetadataFrom(inputImg);

alignedImg->transformation = inputAlignToTransform;
const auto alignToDistortion = inputAlignToTransform.getDistortionCoefficients();
alignedImg->transformation.setDistortionCoefficients(std::vector<float>(alignToDistortion.size(), 0.0f));
Expand Down
Loading