From ebc6f090f2b636a21d8b0c5434e5b43b98e60e78 Mon Sep 17 00:00:00 2001 From: kmakd Date: Fri, 10 Oct 2025 09:09:52 +0000 Subject: [PATCH] fix disparity to depth conversion --- depthai_bridge/include/depthai_bridge/ImageConverter.hpp | 4 +++- depthai_bridge/src/ImageConverter.cpp | 5 +++-- depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp | 6 ++++-- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index a9d6be829..d27a558c7 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -71,8 +71,9 @@ class ImageConverter { /** * @brief Sets converter behavior to convert from disparity to depth when converting messages from bitstream. * @param baseline: The baseline of the stereo pair. + * @param focalLength: The focal length of the camera. */ - void convertDispToDepth(double baseline); + void convertDispToDepth(double baseline, double focalLength); /** * @brief Reverses the order of the stereo sockets when creating CameraInfo to calculate Tx component of Projection matrix. @@ -140,6 +141,7 @@ class ImageConverter { dai::CameraExposureOffset expOffset; bool reversedStereoSocketOrder = false; double baseline; + double focalLength; double alphaScalingFactor = 0.0; int camHeight = -1; int camWidth = -1; diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 50d56980e..840eb1a26 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -50,9 +50,10 @@ void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) { this->srcType = srcType; } -void ImageConverter::convertDispToDepth(double baseline) { +void ImageConverter::convertDispToDepth(double baseline, double focalLength) { dispToDepth = true; this->baseline = baseline; + this->focalLength = focalLength; } void ImageConverter::addExposureOffset(dai::CameraExposureOffset& offset) { @@ -133,7 +134,7 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr i // converting disparity if(dispToDepth) { - auto factor = std::abs(baseline * 10) * info.p[0]; + auto factor = std::abs(baseline * 10) * focalLength; cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1); depthOut.forEach([&output, &factor](uint16_t& pixel, const int* position) -> void { auto disp = output.at(position); diff --git a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp index 3f8001621..58a3b8688 100644 --- a/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp +++ b/depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp @@ -92,10 +92,11 @@ void ImagePublisher::createImageConverter(std::shared_ptr device) { try { auto calHandler = device->readCalibration(); double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false); + double focalLength = calHandler.getCameraIntrinsics(pubConfig.leftSocket).at(0).at(0); if(convConfig.reverseSocketOrder) { baseline = calHandler.getBaselineDistance(pubConfig.rightSocket, pubConfig.leftSocket, false); } - converter->convertDispToDepth(baseline); + converter->convertDispToDepth(baseline, focalLength); } catch(const std::exception& e) { RCLCPP_DEBUG(node->get_logger(), "Failed to convert disparity to depth: %s", e.what()); } @@ -113,10 +114,11 @@ void ImagePublisher::createImageConverter(std::shared_ptr device) { if(convConfig.isStereo && !convConfig.outputDisparity) { auto calHandler = device->readCalibration(); double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false); + double focalLength = calHandler.getCameraIntrinsics(pubConfig.leftSocket).at(0).at(0); if(convConfig.reverseSocketOrder) { baseline = calHandler.getBaselineDistance(pubConfig.rightSocket, pubConfig.leftSocket, false); } - converter->convertDispToDepth(baseline); + converter->convertDispToDepth(baseline, focalLength); } converter->setFFMPEGEncoding(convConfig.ffmpegEncoder); }