diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index 85b59fb2f9..109fb846ed 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -58,6 +58,9 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : void setOutputDepth(bool enabled, int confidence = 200); void setIMUFirmwareUpdate(bool enabled); void setIMUPublished(bool published); + void publishInterIMU(bool enabled); + void setLaserDotBrightness(float dotProjectormA = 0.0f); + void setFloodLightBrightness(float floodLightmA = 200.0f); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; @@ -76,12 +79,15 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : int resolution_; bool imuFirmwareUpdate_; bool imuPublished_; + bool publishInterIMU_; + float dotProjectormA_; + float floodLightmA_; std::shared_ptr device_; std::shared_ptr leftQueue_; std::shared_ptr rightOrDepthQueue_; - std::shared_ptr imuQueue_; std::map accBuffer_; std::map gyroBuffer_; + UMutex imuMutex_; #endif }; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 25e1f46c4e..b4d2f064ad 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -57,7 +57,10 @@ CameraDepthAI::CameraDepthAI( depthConfidence_(200), resolution_(resolution), imuFirmwareUpdate_(false), - imuPublished_(true) + imuPublished_(true), + publishInterIMU_(false), + dotProjectormA_(0.0), + floodLightmA_(200.0) #endif { #ifdef RTABMAP_DEPTHAI @@ -106,6 +109,33 @@ void CameraDepthAI::setIMUPublished(bool published) #endif } +void CameraDepthAI::publishInterIMU(bool enabled) +{ +#ifdef RTABMAP_DEPTHAI + publishInterIMU_ = enabled; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setLaserDotBrightness(float dotProjectormA) +{ +#ifdef RTABMAP_DEPTHAI + dotProjectormA_ = dotProjectormA; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setFloodLightBrightness(float floodLightmA) +{ +#ifdef RTABMAP_DEPTHAI + floodLightmA_ = floodLightmA; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); @@ -183,31 +213,29 @@ 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 stereo->initialConfig.setConfidenceThreshold(depthConfidence_); + stereo->initialConfig.setLeftRightCheck(true); stereo->initialConfig.setLeftRightCheckThreshold(5); - stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout - stereo->setLeftRightCheck(true); - stereo->setSubpixel(false); - stereo->setExtendedDisparity(false); + 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_) - { - // Depth is registered to right image by default, so subscribe to right image when depth is used - if(outputDepth_) - stereo->rectifiedRight.link(xoutLeft->input); - else - stereo->rectifiedLeft.link(xoutLeft->input); stereo->depth.link(xoutDepthOrRight->input); - } else - { - stereo->rectifiedLeft.link(xoutLeft->input); stereo->rectifiedRight.link(xoutDepthOrRight->input); - } if(imuPublished_) { @@ -235,8 +263,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin double fy = matrix[1][1]; double cx = matrix[0][2]; double cy = matrix[1][2]; - matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT); - double baseline = matrix[0][3]/100.0; + double baseline = calibHandler.getBaselineDistance(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT, 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); @@ -249,12 +276,27 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin // matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3], // matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3], // matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]); - // Hard-coded: x->down, y->left, z->forward - imuLocalTransform_ = Transform( - 0, 0, 1, 0, - 0, 1, 0, 0, - -1 ,0, 0, 0); - UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str()); + auto eeprom = calibHandler.getEepromData(); + if(eeprom.boardName == "OAK-D" || + eeprom.boardName == "BW1098OBC") + { + imuLocalTransform_ = Transform( + 0, -1, 0, 0.0525, + 1, 0, 0, 0.0137, + 0, 0, 1, 0); + } + else if(eeprom.boardName == "DM9098") + { + imuLocalTransform_ = Transform( + 0, 1, 0, 0.0754, + 1, 0, 0, 0.0026, + 0, 0, -1, -0.007); + } + else + { + UWARN("Unknown boardName (%s)! Disabling IMU!", eeprom.boardName.c_str()); + imuPublished_ = false; + } } else { @@ -263,10 +305,47 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin if(imuPublished_) { - imuQueue_ = device_->getOutputQueue("imu", 50, false); + 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) + { + dai::IMUData* imuData = static_cast(callback.get()); + auto imuPackets = imuData->packets; + + for(auto& imuPacket : imuPackets) + { + 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))); + } + } + } + }); + } + leftQueue_ = device_->getOutputQueue("rectified_left", 8, false); + rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false); + + std::vector> irDrivers = device_->getIrDrivers(); + if(!irDrivers.empty()) + { + device_->setIrLaserDotProjectorBrightness(dotProjectormA_); + device_->setIrFloodLightBrightness(floodLightmA_); } - leftQueue_ = device_->getOutputQueue("rectified_left", 1, false); - rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false); uSleep(2000); // avoid bad frames on start @@ -303,173 +382,67 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) auto rectifL = leftQueue_->get(); auto rectifRightOrDepth = rightOrDepthQueue_->get(); - if(rectifL.get() && rectifRightOrDepth.get()) - { - auto stampLeft = rectifL->getTimestamp().time_since_epoch().count(); - auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count(); - double stamp = double(stampLeft)/10e8; - left = rectifL->getCvFrame(); - depthOrRight = rectifRightOrDepth->getCvFrame(); - - if(!left.empty() && !depthOrRight.empty()) - { - if(depthOrRight.type() == CV_8UC1) - { - if(stereoModel_.isValidForRectification()) - { - left = stereoModel_.left().rectifyImage(left); - depthOrRight = stereoModel_.right().rectifyImage(depthOrRight); - } - data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); - } - else - { - data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); - } + while(rectifL->getSequenceNum() < rectifRightOrDepth->getSequenceNum()) + rectifL = leftQueue_->get(); + while(rectifL->getSequenceNum() > rectifRightOrDepth->getSequenceNum()) + rectifRightOrDepth = rightOrDepthQueue_->get(); - if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms - { - UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8); - } + double stamp = std::chrono::duration(rectifL->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count(); + left = rectifL->getCvFrame(); + depthOrRight = rectifRightOrDepth->getCvFrame(); - //get imu - double stampStart = UTimer::now(); - while(imuPublished_ && imuQueue_.get()) - { - if(imuQueue_->has()) - { - auto imuData = imuQueue_->get(); - - auto imuPackets = imuData->packets; - double accStamp = 0.0; - double gyroStamp = 0.0; - for(auto& imuPacket : imuPackets) { - auto& acceleroValues = imuPacket.acceleroMeter; - auto& gyroValues = imuPacket.gyroscope; - - accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8; - gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8; - accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z))); - gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z))); - if(accBuffer_.size() > 1000) - { - accBuffer_.erase(accBuffer_.begin()); - } - if(gyroBuffer_.size() > 1000) - { - gyroBuffer_.erase(gyroBuffer_.begin()); - } - } - if(accStamp >= stamp && gyroStamp >= stamp) - { - break; - } - } - if((UTimer::now() - stampStart) > 0.01) - { - UWARN("Could not received IMU after 10 ms! Disabling IMU!"); - imuPublished_ = false; - } - } + if(depthOrRight.type() == CV_8UC1) + data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp); + else + data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp); - cv::Vec3d acc, gyro; - bool valid = !accBuffer_.empty() && !gyroBuffer_.empty(); - //acc - if(!accBuffer_.empty()) - { - std::map::const_iterator iterB = accBuffer_.lower_bound(stamp); - std::map::const_iterator iterA = iterB; - if(iterA != accBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == accBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && stamp == iterA->first) - { - acc[0] = iterA->second[0]; - acc[1] = iterA->second[1]; - acc[2] = iterA->second[2]; - } - else if(stamp >= iterA->first && stamp <= iterB->first) - { - float t = (stamp-iterA->first) / (iterB->first-iterA->first); - acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]); - acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]); - acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]); - } - else - { - valid = false; - if(stamp < iterA->first) - { - UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); - } - else if(stamp > iterB->first) - { - UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first); - } - else - { - UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first); - } - } - } - //gyro - if(!gyroBuffer_.empty()) - { - std::map::const_iterator iterB = gyroBuffer_.lower_bound(stamp); - std::map::const_iterator iterA = iterB; - if(iterA != gyroBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == gyroBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && stamp == iterA->first) - { - gyro[0] = iterA->second[0]; - gyro[1] = iterA->second[1]; - gyro[2] = iterA->second[2]; - } - else if(stamp >= iterA->first && stamp <= iterB->first) - { - float t = (stamp-iterA->first) / (iterB->first-iterA->first); - gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]); - gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]); - gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]); - } - else - { - valid = false; - if(stamp < iterA->first) - { - UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); - } - else if(stamp > iterB->first) - { - UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first); - } - else - { - UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first); - } - } - } + if(imuPublished_ && !publishInterIMU_) + { + cv::Vec3d acc, gyro; + std::map::const_iterator iterA, iterB; + + imuMutex_.lock(); + while(accBuffer_.empty() || gyroBuffer_.empty() || accBuffer_.rbegin()->first < stamp || gyroBuffer_.rbegin()->first < stamp) + { + imuMutex_.unlock(); + uSleep(1); + imuMutex_.lock(); + } - if(valid) - { - data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); - } + //acc + iterB = accBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != accBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + acc = iterB->second; } - } - else - { - UWARN("Null images received!?"); + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + acc = iterA->second + t*(iterB->second - iterA->second); + } + accBuffer_.erase(accBuffer_.begin(), iterB); + + //gyro + iterB = gyroBuffer_.lower_bound(stamp); + iterA = iterB; + if(iterA != gyroBuffer_.begin()) + iterA = --iterA; + if(iterA == iterB || stamp == iterB->first) + { + gyro = iterB->second; + } + else if(stamp > iterA->first && stamp < iterB->first) + { + float t = (stamp-iterA->first) / (iterB->first-iterA->first); + gyro = iterA->second + t*(iterB->second - iterA->second); + } + gyroBuffer_.erase(gyroBuffer_.begin(), iterB); + + imuMutex_.unlock(); + data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); } #else diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 11f7cf8e26..af83f7be53 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -807,6 +807,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_imu_published, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_laser_dot_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_depthai_floodlight_brightness, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -2061,6 +2063,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->spinBox_depthai_confidence->setValue(200); _ui->checkBox_depthai_imu_published->setChecked(true); _ui->checkBox_depthai_imu_firmware_update->setChecked(false); + _ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(0.0); + _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(200.0); _ui->checkBox_cameraImages_configForEachFrame->setChecked(false); _ui->checkBox_cameraImages_timestamps->setChecked(false); @@ -2545,6 +2549,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt()); _ui->checkBox_depthai_imu_published->setChecked(settings.value("imu_published", _ui->checkBox_depthai_imu_published->isChecked()).toBool()); _ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool()); + _ui->doubleSpinBox_depthai_laser_dot_brightness->setValue(settings.value("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()).toDouble()); + _ui->doubleSpinBox_depthai_floodlight_brightness->setValue(settings.value("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()).toDouble()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -3071,7 +3077,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked()); settings.setValue("confidence", _ui->spinBox_depthai_confidence->value()); settings.setValue("imu_published", _ui->checkBox_depthai_imu_published->isChecked()); - settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()); + settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()); + settings.setValue("laser_dot_brightness", _ui->doubleSpinBox_depthai_laser_dot_brightness->value()); + settings.setValue("floodlight_brightness", _ui->doubleSpinBox_depthai_floodlight_brightness->value()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -6319,6 +6327,9 @@ Camera * PreferencesDialog::createCamera( ((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value()); ((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked()); ((CameraDepthAI*)camera)->setIMUPublished(_ui->checkBox_depthai_imu_published->isChecked()); + ((CameraDepthAI*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + ((CameraDepthAI*)camera)->setLaserDotBrightness(_ui->doubleSpinBox_depthai_laser_dot_brightness->value()); + ((CameraDepthAI*)camera)->setFloodLightBrightness(_ui->doubleSpinBox_depthai_floodlight_brightness->value()); } else if(driver == kSrcUsbDevice) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 479d6b9ff0..b3af30a31d 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -5943,6 +5943,58 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + mA + + + 1200.000000000000000 + + + 0.000000000000000 + + + + + + + Laser dot brightness. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + mA + + + 1500.000000000000000 + + + 200.000000000000000 + + + + + + + Floodlight brightness. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + +