diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index db1f40eb6c..5d3e8960bc 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -62,6 +62,8 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : void publishInterIMU(bool enabled); void setLaserDotBrightness(float dotProjectormA = 0.0f); void setFloodLightBrightness(float floodLightmA = 200.0f); + void setDetectFeatures(int detectFeatures = 0); + void setGFTTDetector(bool useHarrisDetector, double minDistance = 7.0f, int numTargetFeatures = 1000); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; @@ -84,9 +86,14 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : bool publishInterIMU_; float dotProjectormA_; float floodLightmA_; + int detectFeatures_; + bool useHarrisDetector_; + double minDistance_; + int numTargetFeatures_; std::shared_ptr device_; std::shared_ptr leftQueue_; std::shared_ptr rightOrDepthQueue_; + std::shared_ptr featuresQueue_; std::map accBuffer_; std::map gyroBuffer_; UMutex imuMutex_; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index 02b96c8e9d..126041f723 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -61,7 +61,11 @@ CameraDepthAI::CameraDepthAI( imuPublished_(true), publishInterIMU_(false), dotProjectormA_(0.0), - floodLightmA_(200.0) + floodLightmA_(200.0), + detectFeatures_(0), + useHarrisDetector_(false), + minDistance_(7.0), + numTargetFeatures_(1000) #endif { #ifdef RTABMAP_DEPTHAI @@ -146,6 +150,26 @@ void CameraDepthAI::setFloodLightBrightness(float floodLightmA) #endif } +void CameraDepthAI::setDetectFeatures(int detectFeatures) +{ +#ifdef RTABMAP_DEPTHAI + detectFeatures_ = detectFeatures; +#else + UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); +#endif +} + +void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, double minDistance, int numTargetFeatures) +{ +#ifdef RTABMAP_DEPTHAI + useHarrisDetector_ = useHarrisDetector; + minDistance_ = minDistance; + numTargetFeatures_ = numTargetFeatures; +#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(""); @@ -198,18 +222,26 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin std::shared_ptr imu; if(imuPublished_) imu = p.create(); + std::shared_ptr gfttDetector; + if(detectFeatures_ == 1) + gfttDetector = p.create(); auto xoutLeft = p.create(); auto xoutDepthOrRight = p.create(); std::shared_ptr xoutIMU; if(imuPublished_) xoutIMU = p.create(); + std::shared_ptr xoutFeatures; + if(detectFeatures_) + xoutFeatures = p.create(); // XLinkOut xoutLeft->setStreamName("rectified_left"); xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right"); if(imuPublished_) xoutIMU->setStreamName("imu"); + if(detectFeatures_) + xoutFeatures->setStreamName("features"); // MonoCamera monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_); @@ -266,6 +298,20 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin imu->enableFirmwareUpdate(imuFirmwareUpdate_); } + if(detectFeatures_ == 1) + { + gfttDetector->setHardwareResources(1, 2); + gfttDetector->initialConfig.setCornerDetector( + useHarrisDetector_?dai::FeatureTrackerConfig::CornerDetector::Type::HARRIS:dai::FeatureTrackerConfig::CornerDetector::Type::SHI_THOMASI); + gfttDetector->initialConfig.setNumTargetFeatures(numTargetFeatures_); + gfttDetector->initialConfig.setMotionEstimator(false); + auto cfg = gfttDetector->initialConfig.get(); + cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_; + gfttDetector->initialConfig.set(cfg); + stereo->rectifiedLeft.link(gfttDetector->inputImage); + gfttDetector->outputFeatures.link(xoutFeatures->input); + } + device_.reset(new dai::Device(p, deviceToUse)); UINFO("Loading eeprom calibration data"); @@ -370,6 +416,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin } leftQueue_ = device_->getOutputQueue("rectified_left", 8, false); rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 8, false); + if(detectFeatures_) + featuresQueue_ = device_->getOutputQueue("features", 8, false); std::vector> irDrivers = device_->getIrDrivers(); if(!irDrivers.empty()) @@ -476,6 +524,19 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info) data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_)); } + if(detectFeatures_ == 1) + { + auto features = featuresQueue_->get(); + while(features->getSequenceNum() < rectifL->getSequenceNum()) + features = featuresQueue_->get(); + auto detectedFeatures = features->trackedFeatures; + + std::vector keypoints; + for(auto& feature : detectedFeatures) + keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3)); + data.setFeatures(keypoints, std::vector(), cv::Mat()); + } + #else UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!"); #endif diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index dc3d5ed3ba..e795da7f98 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -810,6 +810,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : 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->comboBox_depthai_detect_features, SIGNAL(currentIndexChanged(int)), 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())); @@ -2067,6 +2068,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _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->comboBox_depthai_detect_features->setCurrentIndex(0); _ui->checkBox_cameraImages_configForEachFrame->setChecked(false); _ui->checkBox_cameraImages_timestamps->setChecked(false); @@ -2554,6 +2556,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _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()); + _ui->comboBox_depthai_detect_features->setCurrentIndex(settings.value("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()).toInt()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -3084,6 +3087,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const 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.setValue("detect_features", _ui->comboBox_depthai_detect_features->currentIndex()); settings.endGroup(); // DepthAI settings.beginGroup("Images"); @@ -6335,6 +6339,11 @@ Camera * PreferencesDialog::createCamera( ((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()); + ((CameraDepthAI*)camera)->setDetectFeatures(_ui->comboBox_depthai_detect_features->currentIndex()); + if(_ui->comboBox_depthai_detect_features->currentIndex() == 1) + { + ((CameraDepthAI*)camera)->setGFTTDetector(_ui->checkBox_GFTT_useHarrisDetector->isChecked(), _ui->doubleSpinBox_GFTT_minDistance->value(), _ui->reextract_maxFeatures->value()); + } } else if(driver == kSrcUsbDevice) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index cb310d7f7f..f16a5a647e 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6024,6 +6024,39 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + GFTT + + + + + + + + On-device feature detector. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + +