add depthai gftt detector (#1047)

* add depthai feature detector draft

* update depthai gftt config

* optimize to detect more features

corelib/include/rtabmap/core/camera/CameraDepthAI.h

@@ -62,6 +62,8 @@ public:
 	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 @@ private:
 	bool publishInterIMU_;
 	float dotProjectormA_;
 	float floodLightmA_;
+	int detectFeatures_;
+	bool useHarrisDetector_;
+	double minDistance_;
+	int numTargetFeatures_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
+	std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
 	UMutex imuMutex_;

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<dai::node::IMU> imu;
 	if(imuPublished_)
 		imu = p.create<dai::node::IMU>();
+	std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
+	if(detectFeatures_ == 1)
+		gfttDetector = p.create<dai::node::FeatureTracker>();
 
 	auto xoutLeft = p.create<dai::node::XLinkOut>();
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
 	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
 	if(imuPublished_)
 		xoutIMU = p.create<dai::node::XLinkOut>();
+	std::shared_ptr<dai::node::XLinkOut> xoutFeatures;
+	if(detectFeatures_)
+		xoutFeatures = p.create<dai::node::XLinkOut>();
 
 	// 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<std::tuple<std::string, int, int>> 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<dai::TrackedFeatures>();
+		while(features->getSequenceNum() < rectifL->getSequenceNum())
+			features = featuresQueue_->get<dai::TrackedFeatures>();
+		auto detectedFeatures = features->trackedFeatures;
+
+		std::vector<cv::KeyPoint> keypoints;
+		for(auto& feature : detectedFeatures)
+			keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
+		data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
+	}
+
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif

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)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -6024,6 +6024,39 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="8" column="0">
+                                   <widget class="QComboBox" name="comboBox_depthai_detect_features">
+                                    <property name="currentIndex">
+                                     <number>0</number>
+                                    </property>
+                                    <property name="sizeAdjustPolicy">
+                                     <enum>QComboBox::AdjustToContents</enum>
+                                    </property>
+                                    <item>
+                                     <property name="text">
+                                      <string>None</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>GFTT</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="8" column="1">
+                                   <widget class="QLabel" name="label_679">
+                                    <property name="text">
+                                     <string>On-device feature detector.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>