Skip to content

Buxfix/align runtime calibration change #1782

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 3 commits into
base: develop
Choose a base branch
from
+252 −101
Open

Buxfix/align runtime calibration change #1782

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
eec0e96
Fix runtime calibration change in Align node
JakubFara May 7, 2026
af47cfb
Add tests
JakubFara May 7, 2026
f5667d6
Fix tests
JakubFara May 11, 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
74 changes: 45 additions & 29 deletions src/pipeline/node/ImageAlign.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<ImgFrame>& 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<CameraBoardSocket>(inputAlignToImg->getInstanceNum());
if(alignWidth == 0 || alignHeight == 0) {
alignWidth = inputAlignToImg->getWidth();
alignHeight = inputAlignToImg->getHeight();
}

auto alignTransformForIntrinsics = alignTransform;
auto [alignTransformWidth, alignTransformHeight] = alignTransformForIntrinsics.getSize();
if(static_cast<int>(alignTransformWidth) != alignWidth || static_cast<int>(alignTransformHeight) != alignHeight) {
float scaleX = static_cast<float>(alignWidth) / static_cast<float>(alignTransformWidth);
float scaleY = static_cast<float>(alignHeight) / static_cast<float>(alignTransformHeight);
alignTransformForIntrinsics.addScale(scaleX, scaleY);
alignTransformForIntrinsics.setSize(alignWidth, alignHeight);
}

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

while(mainLoop()) {
std::shared_ptr<ImgFrame> inputImg = nullptr;
std::shared_ptr<ImageAlignConfig> inConfig = nullptr;
Expand All @@ -371,35 +403,7 @@ void ImageAlign::run() {
initialized = true;

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

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<CameraBoardSocket>(inputAlignToImg->getInstanceNum());
if(alignWidth == 0 || alignHeight == 0) {
alignWidth = inputAlignToImg->getWidth();
alignHeight = inputAlignToImg->getHeight();
}

auto alignTransformForIntrinsics = inputAlignToTransform;
auto [alignTransformWidth, alignTransformHeight] = alignTransformForIntrinsics.getSize();
if(static_cast<int>(alignTransformWidth) != alignWidth || static_cast<int>(alignTransformHeight) != alignHeight) {
float scaleX = static_cast<float>(alignWidth) / static_cast<float>(alignTransformWidth);
float scaleY = static_cast<float>(alignHeight) / static_cast<float>(alignTransformHeight);
alignTransformForIntrinsics.addScale(scaleX, scaleY);
alignTransformForIntrinsics.setSize(alignWidth, alignHeight);
}

alignSourceIntrinsics = alignTransformForIntrinsics.getIntrinsicMatrix();
inputAlignToTransform = alignTransformForIntrinsics;
updateAlignToData(inputAlignToImg);
}

if(inputConfig.getWaitForMessage()) {
Expand Down Expand Up @@ -451,10 +455,22 @@ void ImageAlign::run() {
if(latestEepromId > currentEepromId) {
logger->debug("EEPROM data changed (ID: {} -> {}), reconfiguring ...", currentEepromId, latestEepromId);
calibrationSet = false;
previousShiftFactor = 0;
Comment thread
MaticTonin marked this conversation as resolved.
refreshAlignToTransform = true;

@aljazkonec1 aljazkonec1 May 15, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need refreshAlignToTransform? To me it looks like the inverse of calibrationSet and they are inherently linked (when refreshAlignToTransform == True then setCalibration has to be false to properly update the maps). The updateAlignToData lambda has a capture by reference for setCalibration bool and could have the same logic as extractCalibrationData where if (calibrationSet) return;.

calibHandler = pipeline.getCalibrationData();
currentEepromId = latestEepromId;
}

if(refreshAlignToTransform) {
if(auto latestAlignToImg = inputAlignTo.tryGet<ImgFrame>()) {

@aljazkonec1 aljazkonec1 May 15, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would preffer get<ImgFrame>() instead of tryGet because this line will wait for new message to arrive instead of hoping that on the next iteration of this while loop there will be a message available.

updateAlignToData(latestAlignToImg);

@aljazkonec1 aljazkonec1 May 15, 2026

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is going to be unpredictable behaviour. Lets say that the eeprom ID got updated but an ImgFrame with the new ImgTransformation hasn`t gotten to the inputAlignTo queue (eg. Camera hasnt generated a new ImgFrame yet), then latestAlignToImg will be a message with the old ImgTransformation. So the update wont actually update the ImgTransformation.

I'm guessing this happens more often if you dont have camera node connected directly to the ImageAling node but have some other nodes in between that increase the time between between new inputAlignTo messages. IMO best solution is to store the previous ImgTransformation and only update when that changes. But I have that made in another branch where its also fallsback to previous impl in case no ImgTransformations available.

refreshAlignToTransform = false;
} else {
logger->trace("Waiting for updated inputAlignTo frame after calibration change.");
continue;
}
Comment on lines +464 to +471

@coderabbitai coderabbitai Bot May 7, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟠 Major | 🏗️ Heavy lift

Stale inputAlignTo frame may corrupt alignment after recalibration with no self-correction path.

The inputAlignTo queue is documented as non-blocking with size 1, which means overwrite semantics reduce the stale-frame window considerably. However, a narrow race still exists: if no new camera frame has arrived between the EEPROM change and the tryGet call, the single buffered frame may pre-date the calibration update. When updateAlignToData runs with that stale frame, alignSourceIntrinsics and inputAlignToTransform are set from the old calibration's metadata, while calibHandler already carries the new rotation/translation. The subsequent extractCalibrationData then mixes these inconsistently, producing incorrect rectification maps.

The critical problem is that currentEepromId is already advanced to latestEepromId at line 461, so no second recalibration cycle will fire to self-correct — the bad maps persist until the next external calibration change.

A targeted fix: after calling updateAlignToData, explicitly override alignSourceIntrinsics from the updated calibHandler for consistency, since the new intrinsics are already available there:

🛡️ Proposed hardening 
 if(refreshAlignToTransform) {
     if(auto latestAlignToImg = inputAlignTo.tryGet<ImgFrame>()) {
         updateAlignToData(latestAlignToImg);
+        // Guard against stale queued frames: re-read intrinsics from the freshly
+        // updated calibHandler so alignSourceIntrinsics is always consistent with
+        // the new calibration data used in extractCalibrationData.
+        auto freshIntrinsics = calibHandler.getCameraIntrinsics(alignTo, alignWidth, alignHeight);
+        for(size_t i = 0; i < 3; ++i)
+            for(size_t j = 0; j < 3; ++j)
+                alignSourceIntrinsics[i][j] = freshIntrinsics[i][j];
         refreshAlignToTransform = false;
     } else {
🤖 Prompt for AI Agents 
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

In `@src/pipeline/node/ImageAlign.cpp` around lines 464 - 471, After
updateAlignToData runs on a stale ImgFrame you must force-align the source
intrinsics to the already-updated calibration to avoid mixing old metadata with
new calibHandler data; so immediately after calling
updateAlignToData(latestAlignToImg) in the refreshAlignToTransform branch,
overwrite alignSourceIntrinsics from calibHandler (and similarly ensure
inputAlignToTransform is synchronized from calibHandler if that transform can be
derived there) before clearing refreshAlignToTransform, so
extractCalibrationData sees consistent intrinsics/transforms even if the
buffered frame predates the EEPROM change.

}

try {
extractCalibrationData(width, height, alignWidth, alignHeight);
} catch(const std::exception& e) {
Expand Down
Loading