Skip to content

fix: ToF example colorization flicker #1906

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

Merged
CasualMathEnjoyer merged 6 commits into from
Jul 27, 2026
Merged

fix: ToF example colorization flicker #1906

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3727e57
fix: colorization
CasualMathEnjoyer Jul 21, 2026
af87b74
fix: colorization in C++ ToF examples
CasualMathEnjoyer Jul 22, 2026
a8d1538
fix: drop unnecessary colorization comments in Python ToF examples
CasualMathEnjoyer Jul 22, 2026
f8d713c
remove comments
CasualMathEnjoyer Jul 23, 2026
079e80e
fix: unify ToF examples at 30 FPS and drop stray comments
CasualMathEnjoyer Jul 23, 2026
5374c08
fix: align C++ sync threshold precision with Python example
CasualMathEnjoyer Jul 23, 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
230 changes: 80 additions & 150 deletions examples/cpp/ToF/tof_align.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,200 +1,130 @@
#include <chrono>
#include <cmath>
#include <deque>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <vector>

#include <argparse/argparse.hpp>

#include "depthai/depthai.hpp"

// Constants from the Python script
constexpr float FPS = 30.0f;
Comment thread
coderabbitai[bot] marked this conversation as resolved.
const dai::CameraBoardSocket RGB_SOCKET = dai::CameraBoardSocket::CAM_C;
const dai::CameraBoardSocket TOF_SOCKET = dai::CameraBoardSocket::CAM_A;
const cv::Size SIZE(640, 400);

// FPSCounter class, similar to the one in the Python script
class FPSCounter {
public:
void tick() {
auto now = std::chrono::steady_clock::now();
frameTimes.push_back(now);
// Keep the last 100 timestamps, similar to the Python example
while(frameTimes.size() > 100) {
frameTimes.pop_front();
}
}
const cv::Size CAMERA_SIZE(640, 400);

double getFps() {
if(frameTimes.size() <= 1) {
return 0.0;
}
auto duration = std::chrono::duration_cast<std::chrono::duration<double>>(frameTimes.back() - frameTimes.front()).count();
return (static_cast<double>(frameTimes.size()) - 1.0) / duration;
}
constexpr float MIN_DEPTH = 100.0f;
constexpr float MAX_DEPTH = 7000.0f;

private:
std::deque<std::chrono::steady_clock::time_point> frameTimes;
};
cv::Mat colorizeDepth(const cv::Mat& frame, float minDepth, float maxDepth) {
cv::Mat depth32f;
frame.convertTo(depth32f, CV_32F);

cv::Mat colorizeDepth(const cv::Mat& frameDepth) {
// -----------------------------------------------------------------------
// 1. Basic checks & convert to CV_32F
// -----------------------------------------------------------------------
if(frameDepth.empty() || frameDepth.channels() != 1) return cv::Mat::zeros(frameDepth.size(), CV_8UC3);
cv::Mat invalidMask = depth32f == 0.0f;

cv::Mat depth32f;
frameDepth.convertTo(depth32f, CV_32F); // safe for any input type

// -----------------------------------------------------------------------
// 2. Build mask of valid (non-zero) pixels
// -----------------------------------------------------------------------
const cv::Mat nonZeroMask = depth32f != 0.0f;
const int nz = cv::countNonZero(nonZeroMask);
if(nz == 0) return cv::Mat::zeros(frameDepth.size(), CV_8UC3);

// -----------------------------------------------------------------------
// 3. 3 % / 95 % percentiles (identical to Python version)
// -----------------------------------------------------------------------
std::vector<float> values;
values.reserve(nz);
for(int r = 0; r < depth32f.rows; ++r) {
const float* d = depth32f.ptr<float>(r);
const uchar* m = nonZeroMask.ptr<uchar>(r);
for(int c = 0; c < depth32f.cols; ++c)
if(m[c]) values.push_back(d[c]);
}
try {
cv::Mat logDepth = depth32f + 1e-6f;
cv::log(logDepth, logDepth);
logDepth.setTo(0.0f, invalidMask);

std::sort(values.begin(), values.end());
auto pct = [&](double p) {
size_t idx = static_cast<size_t>(std::round((p / 100.0) * (values.size() - 1)));
return values[idx];
};

const float minDepth = pct(3.0);
const float maxDepth = pct(95.0);

// -----------------------------------------------------------------------
// 4. Logarithm (zeros replaced by minDepth to avoid -inf)
// -----------------------------------------------------------------------
cv::Mat logDepth;
depth32f.copyTo(logDepth);
logDepth.setTo(minDepth, ~nonZeroMask); // overwrite zeros
cv::log(logDepth, logDepth);

const float logMinDepth = std::log(minDepth);
const float logMaxDepth = std::log(maxDepth);

// -----------------------------------------------------------------------
// 5. Clip & linearly scale to [0,255] (same as np.interp)
// -----------------------------------------------------------------------
cv::min(logDepth, logMaxDepth, logDepth);
cv::max(logDepth, logMinDepth, logDepth);
logDepth = (logDepth - logMinDepth) * (255.0f / (logMaxDepth - logMinDepth));

cv::Mat depth8U;
logDepth.convertTo(depth8U, CV_8U);

// -----------------------------------------------------------------------
// 6. Colour map + set invalid pixels to black
// -----------------------------------------------------------------------
cv::Mat depthFrameColor;
cv::applyColorMap(depth8U, depthFrameColor, cv::COLORMAP_JET);
depthFrameColor.setTo(cv::Scalar::all(0), ~nonZeroMask);

return depthFrameColor;
const float logMinDepth = std::log(minDepth + 1e-6f);
const float logMaxDepth = std::log(maxDepth + 1e-6f);

cv::min(logDepth, logMaxDepth, logDepth);
cv::max(logDepth, logMinDepth, logDepth);

cv::Mat colored;
logDepth.convertTo(
colored, CV_8U, 255.0 / (logMaxDepth - logMinDepth), -logMinDepth * 255.0 / (logMaxDepth - logMinDepth));
cv::applyColorMap(colored, colored, cv::COLORMAP_JET);
colored.setTo(cv::Scalar::all(0), invalidMask);
return colored;
} catch(const cv::Exception&) {
return cv::Mat::zeros(frame.size(), CV_8UC3);
}
}

// Global variables for blending weights
float rgbWeight = 0.4f;
float depthWeight = 0.6f;
float rgbWeight = 0.5f;
float depthWeight = 0.5f;

// Callback function for the trackbar
void updateBlendWeights(int percentRgb, void*) {
rgbWeight = static_cast<float>(percentRgb) / 100.0f;
depthWeight = 1.0f - rgbWeight;
}

int main() {
int main(int argc, char** argv) {
argparse::ArgumentParser program("tof_align");
program.add_description("Align ToF depth over left or right camera and show a blended overlay.");
program.add_argument("--camera")
.default_value(std::string("left"))
.choices("left", "right")
.help("Camera to align depth onto: left=CAM_B, right=CAM_C (default: left)");

try {
program.parse_args(argc, argv);
} catch(const std::runtime_error& err) {
std::cerr << err.what() << '\n';
std::cerr << program;
return EXIT_FAILURE;
}

const std::string cameraArg = program.get<std::string>("--camera");
const dai::CameraBoardSocket alignSocket = (cameraArg == "right") ? dai::CameraBoardSocket::CAM_C : dai::CameraBoardSocket::CAM_B;
std::cout << "Aligning ToF depth over " << cameraArg << " camera\n";

dai::Pipeline pipeline;

// Define sources and outputs
auto camRgb = pipeline.create<dai::node::Camera>();
auto tof = pipeline.create<dai::node::ToF>();
auto sync = pipeline.create<dai::node::Sync>();
tof->build(dai::CameraBoardSocket::AUTO, dai::ToFConfig::Profile::MID_RANGE, FPS);

auto cam = pipeline.create<dai::node::Camera>()->build(alignSocket);
auto camOut = cam->requestOutput(std::make_pair(CAMERA_SIZE.width, CAMERA_SIZE.height), std::nullopt, dai::ImgResizeMode::CROP, FPS, true);

auto align = pipeline.create<dai::node::ImageAlign>();
align->setRunOnHost(true);
tof->depth.link(align->input);
camOut->link(align->inputAlignTo);

camRgb->build(RGB_SOCKET);
const auto profile = dai::ToFConfig::Profile::MID_RANGE;
tof->build(TOF_SOCKET, profile, FPS);

// Set sync threshold
sync->setSyncThreshold(std::chrono::milliseconds(static_cast<uint32_t>(500 / FPS)));
auto sync = pipeline.create<dai::node::Sync>();
sync->setSyncThreshold(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(0.5 / FPS)));
sync->setRunOnHost(true);

// Linking
auto cameraOutput = camRgb->requestOutput(std::make_pair(SIZE.width, SIZE.height), std::nullopt, dai::ImgResizeMode::CROP, FPS, true);

cameraOutput->link(sync->inputs["rgb"]);
tof->depth.link(align->input);
camOut->link(sync->inputs["rgb"]);
align->outputAligned.link(sync->inputs["depth_aligned"]);
sync->inputs["rgb"].setBlocking(false);
cameraOutput->link(align->inputAlignTo);
auto syncQueue = sync->out.createOutputQueue();

auto confFilter = pipeline.create<dai::node::ToFDepthConfidenceFilter>();
tof->depth.link(confFilter->depth);
tof->amplitude.link(confFilter->amplitude);
confFilter->setRunOnHost(true);
auto syncQueue = sync->out.createOutputQueue();

auto filteredDepthQ = confFilter->filteredDepth.createOutputQueue();
const std::string windowBlend = "tof-overlay-" + cameraArg;
const std::string windowDepth = "depth-aligned";

// Start the pipeline
pipeline.start();
cv::namedWindow(windowBlend);
cv::namedWindow(windowDepth);
cv::createTrackbar("RGB Weight %", windowBlend, nullptr, 100, updateBlendWeights);
cv::setTrackbarPos("RGB Weight %", windowBlend, static_cast<int>(rgbWeight * 100));

// Configure windows and trackbar
const std::string rgbDepthWindowName = "rgb-depth";
cv::namedWindow(rgbDepthWindowName);
cv::createTrackbar("RGB Weight %", rgbDepthWindowName, nullptr, 100, updateBlendWeights);
cv::setTrackbarPos("RGB Weight %", rgbDepthWindowName, static_cast<int>(rgbWeight * 100));

FPSCounter fpsCounter;

while(true) {
while(pipeline.isRunning()) {
auto messageGroup = syncQueue->get<dai::MessageGroup>();
if(messageGroup == nullptr) continue;

fpsCounter.tick();

auto frameRgb = messageGroup->get<dai::ImgFrame>("rgb");
auto frameDepth = messageGroup->get<dai::ImgFrame>("depth_aligned");
auto filteredDepthMsg = filteredDepthQ->get<dai::ImgFrame>();

if(filteredDepthMsg) {
cv::Mat filteredDepthMat = filteredDepthMsg->getCvFrame();
// Display filtered depth map
cv::imshow("Filtered Depth", colorizeDepth(filteredDepthMat));
cv::Mat cvFrame = frameRgb->getCvFrame();
if(cvFrame.channels() == 1) {
cv::cvtColor(cvFrame, cvFrame, cv::COLOR_GRAY2BGR);
}

if(frameRgb && frameDepth) {
cv::Mat cvFrame = frameRgb->getCvFrame();
cv::Mat alignedDepthColorized = colorizeDepth(frameDepth->getFrame());
cv::Mat depthColorized = colorizeDepth(frameDepth->getFrame(), MIN_DEPTH, MAX_DEPTH);
if(depthColorized.size() != cvFrame.size()) {
cv::resize(depthColorized, depthColorized, cvFrame.size());
}

// Add FPS text to the depth frame
std::string fpsText = "FPS: " + std::to_string(fpsCounter.getFps());
cv::putText(alignedDepthColorized, fpsText, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2);
cv::imshow("depth", alignedDepthColorized);
cv::imshow(windowDepth, depthColorized);

// Blend the RGB and depth frames
cv::Mat blended;
cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended);
cv::imshow(rgbDepthWindowName, blended);
}
cv::Mat blended;
cv::addWeighted(cvFrame, rgbWeight, depthColorized, depthWeight, 0, blended);
cv::imshow(windowBlend, blended);

int key = cv::waitKey(1);
if(key == 'q' || key == 27) { // 'q' or ESC
if(cv::waitKey(1) == 'q') {
break;
}
}
Expand Down
30 changes: 15 additions & 15 deletions examples/cpp/ToF/tof_all_queues.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,13 @@
#include <cmath>
#include <iostream>
#include <map>
#include <opencv2/opencv.hpp>
#include <string>

#include "depthai/depthai.hpp"

constexpr float FPS = 30.0f;

cv::Mat colorizeDepth(const cv::Mat& frame, float minDepth, float maxDepth) {
cv::Mat depth32f;
frame.convertTo(depth32f, CV_32F);
Expand All @@ -22,17 +25,9 @@ cv::Mat colorizeDepth(const cv::Mat& frame, float minDepth, float maxDepth) {
cv::min(logDepth, logMaxDepth, logDepth);
cv::max(logDepth, logMinDepth, logDepth);

cv::Mat validMask = invalidMask == 0;
double validMin = 0.0;
double validMax = 0.0;
cv::minMaxLoc(logDepth, &validMin, &validMax, nullptr, nullptr, validMask);

if(validMax <= validMin) {
return cv::Mat::zeros(frame.size(), CV_8UC3);
}

cv::Mat colored;
logDepth.convertTo(colored, CV_8U, 255.0 / (validMax - validMin), -validMin * 255.0 / (validMax - validMin));
logDepth.convertTo(
colored, CV_8U, 255.0 / (logMaxDepth - logMinDepth), -logMinDepth * 255.0 / (logMaxDepth - logMinDepth));
cv::applyColorMap(colored, colored, cv::COLORMAP_JET);
colored.setTo(cv::Scalar::all(0), invalidMask);
return colored;
Expand All @@ -50,22 +45,27 @@ cv::Mat normalizeFrame(const cv::Mat& frame) {
int main() {
dai::Pipeline pipeline;

// show depth in range 0.1m - 7m
constexpr float minDepth = 100.0f;
constexpr float maxDepth = 7000.0f;

// choose one of profiles LOW_RANGE / MID_RANGE / HIGH_RANGE
auto profile = dai::ToFConfig::Profile::MID_RANGE;

auto tof = pipeline.create<dai::node::ToF>()->build(dai::CameraBoardSocket::AUTO, profile);
auto tof = pipeline.create<dai::node::ToF>()->build(dai::CameraBoardSocket::AUTO, profile, FPS);

bool isRVC2 = pipeline.getDefaultDevice()->getPlatform() == dai::Platform::RVC2;

std::map<std::string, std::shared_ptr<dai::MessageQueue>> outputQueues = {
{"depth", tof->depth.createOutputQueue(1, false)},
{"amplitude", tof->amplitude.createOutputQueue(1, false)},
{"intensity", tof->intensity.createOutputQueue(1, false)},
// {"rawDepth", tof->rawDepth.createOutputQueue(1, false)}, // not supported on RVC4
// {"confidence", tof->confidence.createOutputQueue(1, false)}, // not supported on RVC2
};
if(isRVC2) {
outputQueues["rawDepth"] = tof->rawDepth.createOutputQueue(1, false);
} else {
outputQueues["confidence"] = tof->confidence.createOutputQueue(1, false);
}

std::cout << "Detected " << (isRVC2 ? "RVC2" : "RVC4") << std::endl;

pipeline.start();
while(pipeline.isRunning()) {
Expand Down
18 changes: 5 additions & 13 deletions examples/cpp/ToF/tof_minimal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@

#include "depthai/depthai.hpp"

constexpr float FPS = 30.0f;

cv::Mat colorizeDepth(const cv::Mat& frame, float minDepth, float maxDepth) {
cv::Mat depth32f;
frame.convertTo(depth32f, CV_32F);
Expand All @@ -20,17 +22,9 @@ cv::Mat colorizeDepth(const cv::Mat& frame, float minDepth, float maxDepth) {
cv::min(logDepth, logMaxDepth, logDepth);
cv::max(logDepth, logMinDepth, logDepth);

cv::Mat validMask = invalidMask == 0;
double validMin = 0.0;
double validMax = 0.0;
cv::minMaxLoc(logDepth, &validMin, &validMax, nullptr, nullptr, validMask);

if(validMax <= validMin) {
return cv::Mat::zeros(frame.size(), CV_8UC3);
}

cv::Mat colored;
logDepth.convertTo(colored, CV_8U, 255.0 / (validMax - validMin), -validMin * 255.0 / (validMax - validMin));
logDepth.convertTo(
colored, CV_8U, 255.0 / (logMaxDepth - logMinDepth), -logMinDepth * 255.0 / (logMaxDepth - logMinDepth));
cv::applyColorMap(colored, colored, cv::COLORMAP_JET);
colored.setTo(cv::Scalar::all(0), invalidMask);
return colored;
Expand All @@ -43,14 +37,12 @@ int main() {
auto device = std::make_shared<dai::Device>();
dai::Pipeline pipeline(device);

// Show depth in range 0.1 m to 7 m.
constexpr float minDepth = 100.0f;
constexpr float maxDepth = 7000.0f;

// Choose one of the profiles: LOW_RANGE, MID_RANGE, or HIGH_RANGE.
auto profile = dai::ToFConfig::Profile::MID_RANGE;

auto tof = pipeline.create<dai::node::ToF>()->build(dai::CameraBoardSocket::AUTO, profile);
auto tof = pipeline.create<dai::node::ToF>()->build(dai::CameraBoardSocket::AUTO, profile, FPS);

auto depthOutputQueue = tof->depth.createOutputQueue();

Expand Down
Loading