From a2a75a28baea8b2488f79303debf1f87b72416a9 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Fri, 14 Jul 2023 20:18:48 +0800 Subject: [PATCH 1/8] add imu local transform for oak-d poe models --- corelib/src/camera/CameraDepthAI.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 6d1cc350..51d046bf 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -428,8 +428,15 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin else if(eeprom.boardName == "DM9098") { imuLocalTransform_ = Transform( - 0, 1, 0, 0.0754, - 1, 0, 0, 0.0026, + 0, 1, 0, 0.075445, + 1, 0, 0, 0.00079, + 0, 0, -1, -0.007); + } + else if(eeprom.boardName == "NG9097") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.0775, + 1, 0, 0, 0.020265, 0, 0, -1, -0.007); } else From ce81fe445a29348b43b8d4f4f103900176f7d303 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Sat, 15 Jul 2023 20:58:00 +0800 Subject: [PATCH 2/8] specify device using MXID or IP/USB name --- .../rtabmap/core/camera/CameraDepthAI.h | 4 +- corelib/src/camera/CameraDepthAI.cpp | 47 +++++++++++-------- 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index 9083a979..588f3a79 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -49,7 +49,7 @@ public: public: CameraDepthAI( - const std::string & deviceSerial = "", + const std::string & mxidOrName = "", int resolution = 1, // 0=720p, 1=800p, 2=400p float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); @@ -79,7 +79,7 @@ private: StereoCameraModel stereoModel_; cv::Size targetSize_; Transform imuLocalTransform_; - std::string deviceSerial_; + std::string mxidOrName_; bool outputDepth_; int depthConfidence_; int resolution_; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 51d046bf..f5c83f6c 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -46,14 +46,14 @@ bool CameraDepthAI::available() } CameraDepthAI::CameraDepthAI( - const std::string & deviceSerial, + const std::string & mxidOrName, int resolution, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform) #ifdef RTABMAP_DEPTHAI , - deviceSerial_(deviceSerial), + mxidOrName_(mxidOrName), outputDepth_(false), depthConfidence_(200), resolution_(resolution), @@ -202,38 +202,45 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin std::vector devices = dai::Device::getAllAvailableDevices(); if(devices.empty()) { + UERROR("No DepthAI device found"); return false; } if(device_.get()) - { device_->close(); - } + accBuffer_.clear(); gyroBuffer_.clear(); - dai::DeviceInfo deviceToUse; - if(deviceSerial_.empty()) - deviceToUse = devices[0]; - for(size_t i=0; i(1, 2); double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0; UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); - stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); + stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); if(imuPublished_) { @@ -517,7 +524,7 @@ bool CameraDepthAI::isCalibrated() const std::string CameraDepthAI::getSerial() const { #ifdef RTABMAP_DEPTHAI - return deviceSerial_; + return device_->getMxId(); #endif return ""; } From 6d690a315a9b2d67c45173f34a3c350b3af2c898 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 17 Jul 2023 03:32:18 +0800 Subject: [PATCH 3/8] Using VideoEncoder on PoE devices --- .../rtabmap/core/camera/CameraDepthAI.h | 2 + corelib/src/camera/CameraDepthAI.cpp | 103 ++++++++++++++---- 2 files changed, 83 insertions(+), 22 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index 588f3a79..cb3f27f3 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -104,6 +104,8 @@ private: std::map accBuffer_; std::map gyroBuffer_; UMutex imuMutex_; + + static std::string ProtocolToStr(XLinkProtocol_t val); #endif }; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index f5c83f6c..9e825e2b 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -226,7 +226,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin { for(auto& device : devices) { - if(deviceToUse.name.compare(device.name) == 0) + if(deviceToUse.name == device.name) { deviceFound = true; deviceToUse = device; @@ -313,8 +313,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin // StereoDepth stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT); - stereo->setSubpixel(true); - stereo->setSubpixelFractionalBits(4); stereo->setExtendedDisparity(false); stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout if(alphaScaling_>-1.0f) @@ -323,20 +321,40 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin stereo->initialConfig.setLeftRightCheck(true); stereo->initialConfig.setLeftRightCheckThreshold(5); stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5); - auto config = stereo->initialConfig.get(); - config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64; - config.costMatching.enableCompanding = true; - stereo->initialConfig.set(config); // Link plugins CAM -> STEREO -> XLINK monoLeft->out.link(stereo->left); monoRight->out.link(stereo->right); - stereo->rectifiedLeft.link(xoutLeft->input); - if(outputDepth_) - stereo->depth.link(xoutDepthOrRight->input); + // Using VideoEncoder on PoE devices, Subpixel is not supported + if(ProtocolToStr(deviceToUse.protocol) == "X_LINK_TCP_IP") + { + auto leftEnc = p.create(); + auto depthOrRightEnc = p.create(); + leftEnc->setDefaultProfilePreset(monoLeft->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + depthOrRightEnc->setDefaultProfilePreset(monoRight->getFps(), dai::VideoEncoderProperties::Profile::MJPEG); + stereo->rectifiedLeft.link(leftEnc->input); + if(outputDepth_) + stereo->disparity.link(depthOrRightEnc->input); + else + stereo->rectifiedRight.link(depthOrRightEnc->input); + leftEnc->bitstream.link(xoutLeft->input); + depthOrRightEnc->bitstream.link(xoutDepthOrRight->input); + } else - stereo->rectifiedRight.link(xoutDepthOrRight->input); + { + stereo->setSubpixel(true); + stereo->setSubpixelFractionalBits(4); + auto config = stereo->initialConfig.get(); + config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64; + config.costMatching.enableCompanding = true; + stereo->initialConfig.set(config); + stereo->rectifiedLeft.link(xoutLeft->input); + if(outputDepth_) + stereo->depth.link(xoutDepthOrRight->input); + else + stereo->rectifiedRight.link(xoutDepthOrRight->input); + } if(imuPublished_) { @@ -374,8 +392,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin manip->setMaxOutputFrameSize(320 * 200); manip->initialConfig.setResize(320, 200); superPointNetwork->setBlobPath(blobPath_); - superPointNetwork->setNumInferenceThreads(1); - superPointNetwork->setNumNCEPerInferenceThread(2); + superPointNetwork->setNumInferenceThreads(2); + superPointNetwork->setNumNCEPerInferenceThread(1); superPointNetwork->input.setBlocking(false); stereo->rectifiedLeft.link(manip->inputImage); manip->out.link(superPointNetwork->input); @@ -399,12 +417,10 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin if(calibHandler.getDistortionModel(dai::CameraBoardSocket::CAM_B) == dai::CameraModel::Perspective) distCoeffs = (cv::Mat_(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]); - if(alphaScaling_>-1.0f) { + if(alphaScaling_>-1.0f) new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_); - } - else { + else new_camera_matrix = cameraMatrix; - } double fx = new_camera_matrix.at(0, 0); double fy = new_camera_matrix.at(1, 1); @@ -544,13 +560,29 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) rectifRightOrDepth = rightOrDepthQueue_->get(); double stamp = std::chrono::duration(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); - left = rectifL->getCvFrame(); - depthOrRight = rectifRightOrDepth->getCvFrame(); - - if(depthOrRight.type() == CV_8UC1) - data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); + if(ProtocolToStr(device_->getDeviceInfo().protocol) == "X_LINK_TCP_IP") + { + left = cv::imdecode(rectifL->getData(), cv::IMREAD_GRAYSCALE); + depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); + if(outputDepth_) + { + cv::Mat depth(targetSize_, CV_16UC1); + depth.forEach([&](uint16_t& pixel, const int * position) -> void { + pixel = stereoModel_.computeDepth(depthOrRight.at(position))*1000; + }); + depthOrRight = depth; + } + } else + { + left = rectifL->getFrame(); + depthOrRight = rectifRightOrDepth->getFrame(); + } + + if(outputDepth_) data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); + else + data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); if(imuPublished_ && !publishInterIMU_) { @@ -671,4 +703,31 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) return data; } +std::string CameraDepthAI::ProtocolToStr(XLinkProtocol_t val) +{ +#ifdef RTABMAP_DEPTHAI + switch (val) + { + case X_LINK_USB_VSC: + return "X_LINK_USB_VSC"; + case X_LINK_USB_CDC: + return "X_LINK_USB_CDC"; + case X_LINK_PCIE: + return "X_LINK_PCIE"; + case X_LINK_IPC: + return "X_LINK_IPC"; + case X_LINK_TCP_IP: + return "X_LINK_TCP_IP"; + case X_LINK_NMB_OF_PROTOCOLS: + return "X_LINK_NMB_OF_PROTOCOLS"; + case X_LINK_ANY_PROTOCOL: + return "X_LINK_ANY_PROTOCOL"; + default: + return "INVALID_ENUM_VALUE"; + break; + } +#endif + return ""; +} + } // namespace rtabmap From 7d2272beb301489030c61a5bab4913891b5c6b42 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 17 Jul 2023 10:09:49 +0800 Subject: [PATCH 4/8] fix ProtocolToStr() visibility --- corelib/src/camera/CameraDepthAI.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 9e825e2b..4634ba1d 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -703,9 +703,9 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) return data; } +#ifdef RTABMAP_DEPTHAI std::string CameraDepthAI::ProtocolToStr(XLinkProtocol_t val) { -#ifdef RTABMAP_DEPTHAI switch (val) { case X_LINK_USB_VSC: @@ -726,8 +726,7 @@ std::string CameraDepthAI::ProtocolToStr(XLinkProtocol_t val) return "INVALID_ENUM_VALUE"; break; } -#endif - return ""; } +#endif } // namespace rtabmap From 0b4f5d292547248112c2392a347be305d24edc9d Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 17 Jul 2023 17:00:43 +0800 Subject: [PATCH 5/8] remove unnecessary conversion function --- .../rtabmap/core/camera/CameraDepthAI.h | 2 -- corelib/src/camera/CameraDepthAI.cpp | 30 ++----------------- 2 files changed, 2 insertions(+), 30 deletions(-) diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index cb3f27f3..588f3a79 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -104,8 +104,6 @@ private: std::map accBuffer_; std::map gyroBuffer_; UMutex imuMutex_; - - static std::string ProtocolToStr(XLinkProtocol_t val); #endif }; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 4634ba1d..fb17212a 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -327,7 +327,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin monoRight->out.link(stereo->right); // Using VideoEncoder on PoE devices, Subpixel is not supported - if(ProtocolToStr(deviceToUse.protocol) == "X_LINK_TCP_IP") + if(deviceToUse.protocol == X_LINK_TCP_IP) { auto leftEnc = p.create(); auto depthOrRightEnc = p.create(); @@ -560,7 +560,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) rectifRightOrDepth = rightOrDepthQueue_->get(); double stamp = std::chrono::duration(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); - if(ProtocolToStr(device_->getDeviceInfo().protocol) == "X_LINK_TCP_IP") + if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP) { left = cv::imdecode(rectifL->getData(), cv::IMREAD_GRAYSCALE); depthOrRight = cv::imdecode(rectifRightOrDepth->getData(), cv::IMREAD_GRAYSCALE); @@ -703,30 +703,4 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) return data; } -#ifdef RTABMAP_DEPTHAI -std::string CameraDepthAI::ProtocolToStr(XLinkProtocol_t val) -{ - switch (val) - { - case X_LINK_USB_VSC: - return "X_LINK_USB_VSC"; - case X_LINK_USB_CDC: - return "X_LINK_USB_CDC"; - case X_LINK_PCIE: - return "X_LINK_PCIE"; - case X_LINK_IPC: - return "X_LINK_IPC"; - case X_LINK_TCP_IP: - return "X_LINK_TCP_IP"; - case X_LINK_NMB_OF_PROTOCOLS: - return "X_LINK_NMB_OF_PROTOCOLS"; - case X_LINK_ANY_PROTOCOL: - return "X_LINK_ANY_PROTOCOL"; - default: - return "INVALID_ENUM_VALUE"; - break; - } -} -#endif - } // namespace rtabmap From d391f237eba3ffa1e744c6abdebf8dc4aa92bae7 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 17 Jul 2023 23:35:03 +0800 Subject: [PATCH 6/8] update imu callback --- corelib/src/camera/CameraDepthAI.cpp | 43 +++++++++++++--------------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index fb17212a..021257af 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -477,32 +477,29 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin { imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_; UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); - device_->getOutputQueue("imu", 50, false)->addCallback([this](std::shared_ptr callback) { - if(dynamic_cast(callback.get()) != nullptr) + device_->getOutputQueue("imu", 50, false)->addCallback([this](const std::shared_ptr data) { + auto imuData = std::dynamic_pointer_cast(data); + auto imuPackets = imuData->packets; + + for(auto& imuPacket : imuPackets) { - dai::IMUData* imuData = static_cast(callback.get()); - auto imuPackets = imuData->packets; + auto& acceleroValues = imuPacket.acceleroMeter; + auto& gyroValues = imuPacket.gyroscope; + double accStamp = std::chrono::duration(acceleroValues.getTimestampDevice().time_since_epoch()).count(); + double gyroStamp = std::chrono::duration(gyroValues.getTimestampDevice().time_since_epoch()).count(); - for(auto& imuPacket : imuPackets) + if(publishInterIMU_) { - auto& acceleroValues = imuPacket.acceleroMeter; - auto& gyroValues = imuPacket.gyroscope; - double accStamp = std::chrono::duration(acceleroValues.getTimestampDevice().time_since_epoch()).count(); - double gyroStamp = std::chrono::duration(gyroValues.getTimestampDevice().time_since_epoch()).count(); - - if(publishInterIMU_) - { - IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1), - cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1), - imuLocalTransform_); - UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2)); - } - else - { - UScopeMutex lock(imuMutex_); - accBuffer_.emplace_hint(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z))); - gyroBuffer_.emplace_hint(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z))); - } + IMU imu(cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z), cv::Mat::eye(3,3,CV_64FC1), + cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z), cv::Mat::eye(3,3,CV_64FC1), + imuLocalTransform_); + UEventsManager::post(new IMUEvent(imu, (accStamp+gyroStamp)/2)); + } + else + { + UScopeMutex lock(imuMutex_); + accBuffer_.emplace_hint(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z))); + gyroBuffer_.emplace_hint(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z))); } } }); From f9719c197d75fec2bc6ba39970c1cd77aa35d493 Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Thu, 20 Jul 2023 00:28:36 +0800 Subject: [PATCH 7/8] disparityToDepthUseSpecTranslation is enabled by default as recommended --- corelib/src/camera/CameraDepthAI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 021257af..7ebc9784 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -426,7 +426,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin double fy = new_camera_matrix.at(1, 1); double cx = new_camera_matrix.at(0, 2); double cy = new_camera_matrix.at(1, 2); - double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::CAM_C, dai::CameraBoardSocket::CAM_B, false)/100.0; + double baseline = calibHandler.getBaselineDistance()/100.0; UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline); stereoModel_ = StereoCameraModel(device_->getDeviceName(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_); From e8b54de94a1d607c9609e69f254d9a0cdce49106 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Jul 2023 09:10:06 -0400 Subject: [PATCH 8/8] CameraDepthAi: Fixed buffered frames --- corelib/src/camera/CameraDepthAI.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 7ebc9784..beb36ce7 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -572,8 +572,8 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) } else { - left = rectifL->getFrame(); - depthOrRight = rectifRightOrDepth->getFrame(); + left = rectifL->getFrame(true); + depthOrRight = rectifRightOrDepth->getFrame(true); } if(outputDepth_)