add depthai superpoint detector

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

@@ -63,7 +63,9 @@ public:
 	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);
+	void setBlobPath(const std::string & blobPath);
+	void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
+	void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -75,6 +77,7 @@ protected:
 private:
 #ifdef RTABMAP_DEPTHAI
 	StereoCameraModel stereoModel_;
+	cv::Size targetSize_;
 	Transform imuLocalTransform_;
 	std::string deviceSerial_;
 	bool outputDepth_;
@@ -88,8 +91,12 @@ private:
 	float floodLightmA_;
 	int detectFeatures_;
 	bool useHarrisDetector_;
-	double minDistance_;
+	float minDistance_;
 	int numTargetFeatures_;
+	float threshold_;
+	bool nms_;
+	int nmsRadius_;
+	std::string blobPath_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;

corelib/src/camera/CameraDepthAI.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/camera/CameraDepthAI.h>
+#include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsManager.h>
@@ -65,7 +66,10 @@ CameraDepthAI::CameraDepthAI(
 	detectFeatures_(0),
 	useHarrisDetector_(false),
 	minDistance_(7.0),
-	numTargetFeatures_(1000)
+	numTargetFeatures_(1000),
+	threshold_(0.01),
+	nms_(true),
+	nmsRadius_(4)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
@@ -159,7 +163,16 @@ void CameraDepthAI::setDetectFeatures(int detectFeatures)
 #endif
 }
 
-void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, double minDistance, int numTargetFeatures)
+void CameraDepthAI::setBlobPath(const std::string & blobPath)
+{
+#ifdef RTABMAP_DEPTHAI
+	blobPath_ = blobPath;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, float minDistance, int numTargetFeatures)
 {
 #ifdef RTABMAP_DEPTHAI
 	useHarrisDetector_ = useHarrisDetector;
@@ -170,6 +183,17 @@ void CameraDepthAI::setGFTTDetector(bool useHarrisDetector, double minDistance,
 #endif
 }
 
+void CameraDepthAI::setSuperPointDetector(float threshold, bool nms, int nmsRadius)
+{
+#ifdef RTABMAP_DEPTHAI
+	threshold_ = threshold;
+	nms_ = nms;
+	nmsRadius_ = nmsRadius;
+#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("");
@@ -213,7 +237,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	// look for calibration files
 	stereoModel_ = StereoCameraModel();
-	cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
+	targetSize_ = cv::Size(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
 
 	dai::Pipeline p;
 	auto monoLeft  = p.create<dai::node::MonoCamera>();
@@ -223,8 +247,25 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	if(imuPublished_)
 		imu = p.create<dai::node::IMU>();
 	std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
+	std::shared_ptr<dai::node::ImageManip> manip;
+	std::shared_ptr<dai::node::NeuralNetwork> superPointNetwork;
 	if(detectFeatures_ == 1)
+	{
 		gfttDetector = p.create<dai::node::FeatureTracker>();
+	}
+	else if(detectFeatures_ == 2)
+	{
+		if(!blobPath_.empty())
+		{
+			manip = p.create<dai::node::ImageManip>();
+			superPointNetwork = p.create<dai::node::NeuralNetwork>();
+		}
+		else
+		{
+			UWARN("Missing SuperPoint blob file!");
+			detectFeatures_ = 0;
+		}
+	}
 
 	auto xoutLeft = p.create<dai::node::XLinkOut>();
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
@@ -248,7 +289,16 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	monoLeft->setCamera("left");
 	monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
 	monoRight->setCamera("right");
-	if(this->getImageRate()>0)
+	if(detectFeatures_ == 2)
+	{
+		if(this->getImageRate() <= 0 || this->getImageRate() > 15)
+		{
+			UWARN("On-device SuperPoint enabled, image rate is limited to 15 FPS!");
+			monoLeft->setFps(15);
+			monoRight->setFps(15);
+		}
+	}
+	else if(this->getImageRate() > 0)
 	{
 		monoLeft->setFps(this->getImageRate());
 		monoRight->setFps(this->getImageRate());
@@ -311,6 +361,19 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		stereo->rectifiedLeft.link(gfttDetector->inputImage);
 		gfttDetector->outputFeatures.link(xoutFeatures->input);
 	}
+	else if(detectFeatures_ == 2)
+	{
+		manip->setKeepAspectRatio(false);
+		manip->setMaxOutputFrameSize(320 * 200);
+		manip->initialConfig.setResize(320, 200);
+		superPointNetwork->setBlobPath(blobPath_);
+		superPointNetwork->setNumInferenceThreads(1);
+    	superPointNetwork->setNumNCEPerInferenceThread(2);
+    	superPointNetwork->input.setBlocking(false);
+		stereo->rectifiedLeft.link(manip->inputImage);
+		manip->out.link(superPointNetwork->input);
+		superPointNetwork->out.link(xoutFeatures->input);
+	}
 
 	device_.reset(new dai::Device(p, deviceToUse));
 
@@ -319,7 +382,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	cv::Mat cameraMatrix, distCoeffs, new_camera_matrix;
 
-	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::CAM_B, dai::Size2f(targetSize.width, targetSize.height));
+	std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::CAM_B, dai::Size2f(targetSize_.width, targetSize_.height));
 	cameraMatrix = (cv::Mat_<double>(3,3) <<
 		matrix[0][0], matrix[0][1], matrix[0][2],
 		matrix[1][0], matrix[1][1], matrix[1][2],
@@ -330,7 +393,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		distCoeffs = (cv::Mat_<double>(1,8) << coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6], coeffs[7]);
 
     if(alphaScaling_>-1.0f) {
-	    new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize, alphaScaling_);
+	    new_camera_matrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, targetSize_, alphaScaling_);
 	}
 	else {
 	    new_camera_matrix = cameraMatrix;
@@ -342,7 +405,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	double cy = new_camera_matrix.at<double>(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_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize_);
 
 	if(imuPublished_)
 	{
@@ -536,6 +599,38 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 			keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
 		data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
 	}
+	else if(detectFeatures_ == 2)
+	{
+		auto features = featuresQueue_->get<dai::NNData>();
+		while(features->getSequenceNum() < rectifL->getSequenceNum())
+			features = featuresQueue_->get<dai::NNData>();
+
+		auto heatmap = features->getLayerFp16("heatmap");
+		auto desc = features->getLayerFp16("desc");
+
+		cv::Mat prob(200, 320, CV_32FC1, heatmap.data());
+		cv::resize(prob, prob, targetSize_, 0, 0, cv::INTER_CUBIC);
+		std::vector<cv::Point> kpts;
+		cv::findNonZero(prob > threshold_, kpts);
+		std::vector<cv::KeyPoint> keypoints_no_nms, keypoints;
+		for(auto& kpt : kpts)
+		{
+			float response = prob.at<float>(kpt);
+			keypoints_no_nms.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
+		}
+
+		if(nms_ && !keypoints_no_nms.empty())
+		{
+			cv::Mat descEmpty;
+			util2d::NMS(keypoints_no_nms, descEmpty, keypoints, descEmpty, 0, nmsRadius_, targetSize_.width, targetSize_.height);
+		}
+		else if(!keypoints_no_nms.empty())
+		{
+			keypoints = keypoints_no_nms;
+		}
+
+		data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
+	}
 
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -380,6 +380,7 @@ private Q_SLOTS:
 	void selectSourceMKVPath();
 	void selectSourceSvoPath();
 	void selectSourceRealsense2JsonPath();
+	void selectSourceDepthaiBlobPath();
 	void updateSourceGrpVisibility();
 	void testOdometry();
 	void testCamera();

guilib/src/PreferencesDialog.cpp

@@ -811,6 +811,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	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->lineEdit_depthai_blob_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->toolButton_depthai_blob_path, SIGNAL(clicked()), this, SLOT(selectSourceDepthaiBlobPath()));
 
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -2069,6 +2071,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_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->lineEdit_depthai_blob_path->clear();
 
 		_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
@@ -2557,6 +2560,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_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());
+	_ui->lineEdit_depthai_blob_path->setText(settings.value("blob_path", _ui->lineEdit_depthai_blob_path->text()).toString());
 	settings.endGroup(); // DepthAI
 
 	settings.beginGroup("Images");
@@ -3088,6 +3092,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	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.setValue("blob_path",             _ui->lineEdit_depthai_blob_path->text());
 	settings.endGroup(); // DepthAI
 
 	settings.beginGroup("Images");
@@ -4420,6 +4425,20 @@ void PreferencesDialog::selectSourceRealsense2JsonPath()
 	}
 }
 
+void PreferencesDialog::selectSourceDepthaiBlobPath()
+{
+	QString dir = _ui->lineEdit_depthai_blob_path->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("MyriadX blob (*.blob)"));
+	if(path.size())
+	{
+		_ui->lineEdit_depthai_blob_path->setText(path);
+	}
+}
+
 void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
 {
 	UDEBUG("%s=%s", key.c_str(), value.c_str());
@@ -6340,10 +6359,15 @@ Camera * PreferencesDialog::createCamera(
 		((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());
+		((CameraDepthAI*)camera)->setBlobPath(_ui->lineEdit_depthai_blob_path->text().toStdString());
 		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(_ui->comboBox_depthai_detect_features->currentIndex() == 2)
+		{
+			((CameraDepthAI*)camera)->setSuperPointDetector(_ui->doubleSpinBox_sptorch_threshold->value(), _ui->checkBox_sptorch_nms->isChecked(), _ui->spinBox_sptorch_minDistance->value());
+		}
 	}
 	else if(driver == kSrcUsbDevice)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -6042,6 +6042,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                       <string>GFTT</string>
                                      </property>
                                     </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>SuperPoint</string>
+                                     </property>
+                                    </item>
                                    </widget>
                                   </item>
                                   <item row="8" column="1">
@@ -6057,6 +6062,43 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="9" column="0">
+                                   <layout class="QHBoxLayout" name="horizontalLayout_15" stretch="0,1">
+                                    <item>
+                                     <widget class="QToolButton" name="toolButton_depthai_blob_path">
+                                      <property name="text">
+                                       <string>...</string>
+                                      </property>
+                                     </widget>
+                                    </item>
+                                    <item>
+                                     <widget class="QLineEdit" name="lineEdit_depthai_blob_path">
+                                      <property name="minimumSize">
+                                       <size>
+                                        <width>100</width>
+                                        <height>0</height>
+                                       </size>
+                                      </property>
+                                      <property name="text">
+                                       <string/>
+                                      </property>
+                                     </widget>
+                                    </item>
+                                   </layout>
+                                  </item>
+                                  <item row="9" column="1">
+                                   <widget class="QLabel" name="label_680">
+                                    <property name="text">
+                                     <string>Path to SuperPoint blob file.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>