From ad22489c236289a0a2787937003738996b073fb2 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 1 Jul 2023 12:24:49 +0800 Subject: [PATCH 1/6] add CameraExposureOffset to ImageConverter --- .../include/depthai_bridge/ImageConverter.hpp | 5 +++-- depthai_bridge/src/ImageConverter.cpp | 12 ++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index 0937faaa..ae7f07d9 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -27,8 +27,8 @@ using TimePoint = std::chrono::time_point ImageConverter::planarEn {dai::RawImgFrame::Type::NV12, "rgb8"}, {dai::RawImgFrame::Type::YUV420p, "rgb8"}}; -ImageConverter::ImageConverter(bool interleaved, bool getBaseDeviceTimestamp) - : _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { +ImageConverter::ImageConverter(bool interleaved, bool getBaseDeviceTimestamp, dai::CameraExposureOffset offset) + : _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp), _offset(offset) { _rosBaseTime = ::ros::Time::now(); } -ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp) - : _frameName(frameName), _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { +ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp, dai::CameraExposureOffset offset) + : _frameName(frameName), _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp), _offset(offset) { _rosBaseTime = ::ros::Time::now(); } @@ -47,9 +47,9 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) - tstamp = inData->getTimestampDevice(); + tstamp = inData->getTimestampDevice(_offset); else - tstamp = inData->getTimestamp(); + tstamp = inData->getTimestamp(_offset); ImageMsgs::Image outImageMsg; StdMsgs::Header header; header.frame_id = _frameName; From bdffa82be37c68214d58b69f940a5361ef5881c2 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sun, 2 Jul 2023 11:52:18 +0800 Subject: [PATCH 2/6] code format --- depthai_bridge/include/depthai_bridge/ImageConverter.hpp | 5 ++++- depthai_bridge/src/ImageConverter.cpp | 6 +++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp index ae7f07d9..d135b995 100644 --- a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp @@ -27,7 +27,10 @@ using TimePoint = std::chrono::time_point Date: Mon, 10 Jul 2023 23:57:05 +0800 Subject: [PATCH 3/6] add exact sync function --- .../depthai_bridge/BridgePublisher.hpp | 111 +++++++++++++----- depthai_bridge/src/ImageConverter.cpp | 16 +-- 2 files changed, 92 insertions(+), 35 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp index 5ba0e5b4..45f533c0 100644 --- a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp +++ b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp @@ -29,7 +29,7 @@ class BridgePublisher { using ConvertFunc = std::function, std::deque&)>; using CustomPublisher = typename std:: - conditional::value, std::shared_ptr, std::shared_ptr >::type; + conditional::value, std::shared_ptr, std::shared_ptr>::type; BridgePublisher(std::shared_ptr daiMessageQueue, rosOrigin::NodeHandle nh, @@ -45,7 +45,12 @@ class BridgePublisher { ConvertFunc converter, int queueSize, ImageMsgs::CameraInfo cameraInfoData, - std::string cameraName); + std::string cameraName, + std::vector> syncImageQueues = {}, + std::vector syncImageTopics = {}, + std::vector, std::deque&)>> syncImageConverters = {}, + std::vector syncCameraInfo = {}, + std::vector syncCameraNames = {}); /** * Tag Dispacher function to to overload the Publisher to ImageTransport Publisher @@ -76,18 +81,23 @@ class BridgePublisher { static const std::string LOG_TAG; std::shared_ptr _daiMessageQueue; + std::vector> _syncImageQueues; ConvertFunc _converter; + std::vector, std::deque&)>> _syncImageConverters; rosOrigin::NodeHandle _nh; std::shared_ptr _cameraInfoPublisher; + std::vector> _syncCameraInfoPublishers; image_transport::ImageTransport _it; ImageMsgs::CameraInfo _cameraInfoData; CustomPublisher _rosPublisher; + std::vector> _syncImagePublishers; std::thread _readingThread; std::string _rosTopic, _camInfoFrameId, _cameraName, _cameraParamUri; std::unique_ptr _camInfoManager; + std::vector> _syncCamInfoManagers; bool _isCallbackAdded = false; bool _isImageMessage = false; // used to enable camera info manager }; @@ -115,22 +125,43 @@ BridgePublisher::BridgePublisher(std::shared_ptr -BridgePublisher::BridgePublisher(std::shared_ptr daiMessageQueue, - rosOrigin::NodeHandle nh, - std::string rosTopic, - ConvertFunc converter, - int queueSize, - ImageMsgs::CameraInfo cameraInfoData, - std::string cameraName) +BridgePublisher::BridgePublisher( + std::shared_ptr daiMessageQueue, + rosOrigin::NodeHandle nh, + std::string rosTopic, + ConvertFunc converter, + int queueSize, + ImageMsgs::CameraInfo cameraInfoData, + std::string cameraName, + std::vector> syncImageQueues, + std::vector syncImageTopics, + std::vector, std::deque&)>> syncImageConverters, + std::vector syncCameraInfo, + std::vector syncCameraNames) : _daiMessageQueue(daiMessageQueue), _nh(nh), _converter(converter), _it(_nh), _cameraInfoData(cameraInfoData), _rosTopic(rosTopic), - _cameraName(cameraName) { + _cameraName(cameraName), + _syncImageQueues(syncImageQueues), + _syncImageConverters(syncImageConverters) { // ROS_DEBUG_STREAM_NAMED(LOG_TAG, "Publisher Type : " << typeid(CustomPublisher).name()); _rosPublisher = advertise(queueSize, std::is_same{}); + if((syncImageQueues.size() | syncImageTopics.size() | syncImageConverters.size() | syncCameraInfo.size() | syncCameraNames.size()) + == (syncImageQueues.size() & syncImageTopics.size() & syncImageConverters.size() & syncCameraInfo.size() & syncCameraNames.size())) { + for(size_t i = 0; i < syncImageTopics.size(); i++) { + _syncCamInfoManagers.emplace_back( + std::make_unique(rosOrigin::NodeHandle{_nh, syncCameraNames[i]}, syncCameraNames[i])); + _syncCamInfoManagers[i]->setCameraInfo(syncCameraInfo[i]); + _syncCameraInfoPublishers.emplace_back( + std::make_shared(_nh.advertise(syncCameraNames[i] + "/camera_info", queueSize))); + _syncImagePublishers.emplace_back(std::make_shared(_it.advertise(syncImageTopics[i], queueSize))); + } + } else { + throw std::runtime_error("sync params should have the same size"); + } } template @@ -176,7 +207,7 @@ void BridgePublisher::daiCallback(std::string name, std::shared_ template void BridgePublisher::startPublisherThread() { if(_isCallbackAdded) { - std::runtime_error( + throw std::runtime_error( "addPublisherCallback() function adds a callback to the" "depthai which handles the publishing so no need to start" "the thread using startPublisherThread() "); @@ -219,27 +250,53 @@ void BridgePublisher::publishHelper(std::shared_ptr inDa } mainSubCount = _rosPublisher->getNumSubscribers(); - if(mainSubCount > 0 || infoSubCount > 0) { - _converter(inDataPtr, opMsgs); + _converter(inDataPtr, opMsgs); + + while(opMsgs.size()) { + RosMsg currMsg = opMsgs.front(); - while(opMsgs.size()) { - RosMsg currMsg = opMsgs.front(); - if(mainSubCount > 0) { - _rosPublisher->publish(currMsg); + for(size_t i = 0; i < _syncImagePublishers.size(); i++) { + std::shared_ptr syncImagePtr = _syncImageQueues[i]->get(); + while((uint32_t)syncImagePtr->getSequenceNum() < currMsg.header.seq) { + syncImagePtr = _syncImageQueues[i]->get(); } - if(infoSubCount > 0) { - // if (_isImageMessage){ - // _camInfoFrameId = curr.header.frame_id - // } - auto localCameraInfo = _camInfoManager->getCameraInfo(); - localCameraInfo.header.seq = currMsg.header.seq; - localCameraInfo.header.stamp = currMsg.header.stamp; - localCameraInfo.header.frame_id = currMsg.header.frame_id; - _cameraInfoPublisher->publish(localCameraInfo); + int syncInfoSubCount = 0, syncMainSubCount = 0; + syncInfoSubCount = _syncCameraInfoPublishers[i]->getNumSubscribers(); + syncMainSubCount = _syncImagePublishers[i]->getNumSubscribers(); + + if(syncInfoSubCount > 0 || syncMainSubCount > 0) { + std::deque syncImageMsgs; + _syncImageConverters[i](syncImagePtr, syncImageMsgs); + + if(syncInfoSubCount > 0) { + auto cameraInfo = _syncCamInfoManagers[i]->getCameraInfo(); + cameraInfo.header = syncImageMsgs.front().header; + cameraInfo.header.stamp = currMsg.header.stamp; + _syncCameraInfoPublishers[i]->publish(cameraInfo); + } + + if(syncMainSubCount > 0) { + ImageMsgs::Image imageMsg = syncImageMsgs.front(); + imageMsg.header.stamp = currMsg.header.stamp; + _syncImagePublishers[i]->publish(imageMsg); + } } - opMsgs.pop_front(); } + + if(mainSubCount > 0) { + _rosPublisher->publish(currMsg); + } + + if(infoSubCount > 0) { + // if (_isImageMessage){ + // _camInfoFrameId = curr.header.frame_id + // } + auto localCameraInfo = _camInfoManager->getCameraInfo(); + localCameraInfo.header = currMsg.header; + _cameraInfoPublisher->publish(localCameraInfo); + } + opMsgs.pop_front(); } } diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 2ead160a..6f3210f4 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -78,7 +78,7 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData break; } default: { - throw(std::runtime_error("Converted type not supported!")); + throw std::runtime_error("Converted type not supported!"); } } @@ -113,9 +113,9 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< tstamp = inData->getTimestamp(); ImageMsgs::Image outImageMsg; StdMsgs::Header header; - header.frame_id = _frameName; - + header.seq = (uint32_t)inData->getSequenceNum(); header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + header.frame_id = _frameName; if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) { cv::Mat mat, output; @@ -134,7 +134,7 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< break; default: - std::runtime_error("Invalid dataType inputs.."); + throw std::runtime_error("Invalid dataType inputs.."); break; } mat = cv::Mat(size, type, inData->getData().data()); @@ -208,7 +208,7 @@ void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outD return pair.second == inMsg.encoding; }); if(revEncodingIter == encodingEnumMap.end()) - std::runtime_error( + throw std::runtime_error( "Unable to find DAI encoding for the corresponding " "sensor_msgs::image.encoding stream"); @@ -271,7 +271,7 @@ void ImageConverter::planarToInterleaved(const std::vector& srcData, st destData[i * 3 + 2] = r; } } else { - std::runtime_error( + throw std::runtime_error( "If you encounter the scenario where you need this " "please create an issue on github"); } @@ -291,7 +291,7 @@ void ImageConverter::interleavedToPlanar(const std::vector& srcData, st destData[i + w * h * 2] = r; } } else { - std::runtime_error( + throw std::runtime_error( "If you encounter the scenario where you need this " "please create an issue on github"); } @@ -305,7 +305,7 @@ cv::Mat ImageConverter::rosMsgtoCvMat(ImageMsgs::Image& inMsg) { cv::cvtColor(nv_frame, rgb, cv::COLOR_YUV2BGR_NV12); return rgb; } else { - std::runtime_error("This frature is still WIP"); + throw std::runtime_error("This frature is still WIP"); return rgb; } } From d6693cdf1353bf172f22cebe81b977933d4191a8 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Fri, 14 Jul 2023 17:11:36 +0800 Subject: [PATCH 4/6] add CameraExposureOffset to toRosMsg() --- depthai_bridge/src/ImageConverter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 6f3210f4..5393f11f 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -108,9 +108,9 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< } std::chrono::_V2::steady_clock::time_point tstamp; if(_getBaseDeviceTimestamp) - tstamp = inData->getTimestampDevice(); + tstamp = inData->getTimestampDevice(_offset); else - tstamp = inData->getTimestamp(); + tstamp = inData->getTimestamp(_offset); ImageMsgs::Image outImageMsg; StdMsgs::Header header; header.seq = (uint32_t)inData->getSequenceNum(); From 1e9e5334a33f7f7552d5822d652e4317ecdecd0d Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Tue, 18 Jul 2023 19:49:33 +0800 Subject: [PATCH 5/6] Revert "Auxiliary commit to revert individual files from 7a50a85b88f68f588d29a329fee7a2d5a84670c3" This reverts commit 3ccb68124a6511b81969cd782b568f2dd96fc8eb. --- .../depthai_bridge/BridgePublisher.hpp | 111 +++++------------- depthai_bridge/src/ImageConverter.cpp | 6 +- 2 files changed, 30 insertions(+), 87 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp index 45f533c0..5ba0e5b4 100644 --- a/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp +++ b/depthai_bridge/include/depthai_bridge/BridgePublisher.hpp @@ -29,7 +29,7 @@ class BridgePublisher { using ConvertFunc = std::function, std::deque&)>; using CustomPublisher = typename std:: - conditional::value, std::shared_ptr, std::shared_ptr>::type; + conditional::value, std::shared_ptr, std::shared_ptr >::type; BridgePublisher(std::shared_ptr daiMessageQueue, rosOrigin::NodeHandle nh, @@ -45,12 +45,7 @@ class BridgePublisher { ConvertFunc converter, int queueSize, ImageMsgs::CameraInfo cameraInfoData, - std::string cameraName, - std::vector> syncImageQueues = {}, - std::vector syncImageTopics = {}, - std::vector, std::deque&)>> syncImageConverters = {}, - std::vector syncCameraInfo = {}, - std::vector syncCameraNames = {}); + std::string cameraName); /** * Tag Dispacher function to to overload the Publisher to ImageTransport Publisher @@ -81,23 +76,18 @@ class BridgePublisher { static const std::string LOG_TAG; std::shared_ptr _daiMessageQueue; - std::vector> _syncImageQueues; ConvertFunc _converter; - std::vector, std::deque&)>> _syncImageConverters; rosOrigin::NodeHandle _nh; std::shared_ptr _cameraInfoPublisher; - std::vector> _syncCameraInfoPublishers; image_transport::ImageTransport _it; ImageMsgs::CameraInfo _cameraInfoData; CustomPublisher _rosPublisher; - std::vector> _syncImagePublishers; std::thread _readingThread; std::string _rosTopic, _camInfoFrameId, _cameraName, _cameraParamUri; std::unique_ptr _camInfoManager; - std::vector> _syncCamInfoManagers; bool _isCallbackAdded = false; bool _isImageMessage = false; // used to enable camera info manager }; @@ -125,43 +115,22 @@ BridgePublisher::BridgePublisher(std::shared_ptr -BridgePublisher::BridgePublisher( - std::shared_ptr daiMessageQueue, - rosOrigin::NodeHandle nh, - std::string rosTopic, - ConvertFunc converter, - int queueSize, - ImageMsgs::CameraInfo cameraInfoData, - std::string cameraName, - std::vector> syncImageQueues, - std::vector syncImageTopics, - std::vector, std::deque&)>> syncImageConverters, - std::vector syncCameraInfo, - std::vector syncCameraNames) +BridgePublisher::BridgePublisher(std::shared_ptr daiMessageQueue, + rosOrigin::NodeHandle nh, + std::string rosTopic, + ConvertFunc converter, + int queueSize, + ImageMsgs::CameraInfo cameraInfoData, + std::string cameraName) : _daiMessageQueue(daiMessageQueue), _nh(nh), _converter(converter), _it(_nh), _cameraInfoData(cameraInfoData), _rosTopic(rosTopic), - _cameraName(cameraName), - _syncImageQueues(syncImageQueues), - _syncImageConverters(syncImageConverters) { + _cameraName(cameraName) { // ROS_DEBUG_STREAM_NAMED(LOG_TAG, "Publisher Type : " << typeid(CustomPublisher).name()); _rosPublisher = advertise(queueSize, std::is_same{}); - if((syncImageQueues.size() | syncImageTopics.size() | syncImageConverters.size() | syncCameraInfo.size() | syncCameraNames.size()) - == (syncImageQueues.size() & syncImageTopics.size() & syncImageConverters.size() & syncCameraInfo.size() & syncCameraNames.size())) { - for(size_t i = 0; i < syncImageTopics.size(); i++) { - _syncCamInfoManagers.emplace_back( - std::make_unique(rosOrigin::NodeHandle{_nh, syncCameraNames[i]}, syncCameraNames[i])); - _syncCamInfoManagers[i]->setCameraInfo(syncCameraInfo[i]); - _syncCameraInfoPublishers.emplace_back( - std::make_shared(_nh.advertise(syncCameraNames[i] + "/camera_info", queueSize))); - _syncImagePublishers.emplace_back(std::make_shared(_it.advertise(syncImageTopics[i], queueSize))); - } - } else { - throw std::runtime_error("sync params should have the same size"); - } } template @@ -207,7 +176,7 @@ void BridgePublisher::daiCallback(std::string name, std::shared_ template void BridgePublisher::startPublisherThread() { if(_isCallbackAdded) { - throw std::runtime_error( + std::runtime_error( "addPublisherCallback() function adds a callback to the" "depthai which handles the publishing so no need to start" "the thread using startPublisherThread() "); @@ -250,53 +219,27 @@ void BridgePublisher::publishHelper(std::shared_ptr inDa } mainSubCount = _rosPublisher->getNumSubscribers(); - _converter(inDataPtr, opMsgs); - - while(opMsgs.size()) { - RosMsg currMsg = opMsgs.front(); + if(mainSubCount > 0 || infoSubCount > 0) { + _converter(inDataPtr, opMsgs); - for(size_t i = 0; i < _syncImagePublishers.size(); i++) { - std::shared_ptr syncImagePtr = _syncImageQueues[i]->get(); - while((uint32_t)syncImagePtr->getSequenceNum() < currMsg.header.seq) { - syncImagePtr = _syncImageQueues[i]->get(); + while(opMsgs.size()) { + RosMsg currMsg = opMsgs.front(); + if(mainSubCount > 0) { + _rosPublisher->publish(currMsg); } - int syncInfoSubCount = 0, syncMainSubCount = 0; - syncInfoSubCount = _syncCameraInfoPublishers[i]->getNumSubscribers(); - syncMainSubCount = _syncImagePublishers[i]->getNumSubscribers(); - - if(syncInfoSubCount > 0 || syncMainSubCount > 0) { - std::deque syncImageMsgs; - _syncImageConverters[i](syncImagePtr, syncImageMsgs); - - if(syncInfoSubCount > 0) { - auto cameraInfo = _syncCamInfoManagers[i]->getCameraInfo(); - cameraInfo.header = syncImageMsgs.front().header; - cameraInfo.header.stamp = currMsg.header.stamp; - _syncCameraInfoPublishers[i]->publish(cameraInfo); - } - - if(syncMainSubCount > 0) { - ImageMsgs::Image imageMsg = syncImageMsgs.front(); - imageMsg.header.stamp = currMsg.header.stamp; - _syncImagePublishers[i]->publish(imageMsg); - } + if(infoSubCount > 0) { + // if (_isImageMessage){ + // _camInfoFrameId = curr.header.frame_id + // } + auto localCameraInfo = _camInfoManager->getCameraInfo(); + localCameraInfo.header.seq = currMsg.header.seq; + localCameraInfo.header.stamp = currMsg.header.stamp; + localCameraInfo.header.frame_id = currMsg.header.frame_id; + _cameraInfoPublisher->publish(localCameraInfo); } + opMsgs.pop_front(); } - - if(mainSubCount > 0) { - _rosPublisher->publish(currMsg); - } - - if(infoSubCount > 0) { - // if (_isImageMessage){ - // _camInfoFrameId = curr.header.frame_id - // } - auto localCameraInfo = _camInfoManager->getCameraInfo(); - localCameraInfo.header = currMsg.header; - _cameraInfoPublisher->publish(localCameraInfo); - } - opMsgs.pop_front(); } } diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 5393f11f..4bc0797e 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -113,10 +113,10 @@ void ImageConverter::toRosMsg(std::shared_ptr inData, std::deque< tstamp = inData->getTimestamp(_offset); ImageMsgs::Image outImageMsg; StdMsgs::Header header; - header.seq = (uint32_t)inData->getSequenceNum(); - header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); header.frame_id = _frameName; + header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + if(planarEncodingEnumMap.find(inData->getType()) != planarEncodingEnumMap.end()) { cv::Mat mat, output; cv::Size size = {0, 0}; @@ -391,4 +391,4 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo( } } // namespace ros -} // namespace dai \ No newline at end of file +} // namespace dai From 92c22d195fe1f7719f4f5bda659f86811e9de674 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Tue, 18 Jul 2023 22:38:45 +0800 Subject: [PATCH 6/6] fix disparity to depth computation --- depthai_bridge/src/ImageConverter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/depthai_bridge/src/ImageConverter.cpp b/depthai_bridge/src/ImageConverter.cpp index 4bc0797e..3d2c0591 100644 --- a/depthai_bridge/src/ImageConverter.cpp +++ b/depthai_bridge/src/ImageConverter.cpp @@ -86,10 +86,10 @@ void ImageConverter::toRosMsgFromBitStream(std::shared_ptr inData // converting disparity if(type == dai::RawImgFrame::Type::RAW8) { - auto factor = (info.K[0] * info.P[3]); + auto factor = std::abs(info.P[3]) * 1000; cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1); - depthOut.forEach([&output, &factor](short& pixel, const int* position) -> void { - auto disp = output.at(position); + depthOut.forEach([&output, &factor](uint16_t& pixel, const int* position) -> void { + auto disp = output.at(position); if(disp == 0) pixel = 0; else