Added PyDetector (#677)

* Added PyDetector. Refactored PyMatcher.

* Fixed python freezing with multi-threading

corelib/src/CMakeLists.txt

@@ -195,11 +195,13 @@ IF(Python3_FOUND)
 	)
 	SET(SRC_FILES
 		${SRC_FILES}
-		pymatcher/PyMatcher.cpp
+		python/PythonInterface.cpp
+		python/PyMatcher.cpp
+		python/PyDetector.cpp
 	)
 	 SET(INCLUDE_DIRS
 	    ${TORCH_INCLUDE_DIRS}
-	    ${CMAKE_CURRENT_SOURCE_DIR}/pymatcher
+	    ${CMAKE_CURRENT_SOURCE_DIR}/python
 		${INCLUDE_DIRS}
 	)
 ENDIF(Python3_FOUND)
@@ -627,8 +629,8 @@ foreach(arg ${RESOURCES})
    set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h")
 endforeach(arg ${RESOURCES})
 
-MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
-MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")
+#MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
+#MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")
 
 IF(ANDROID)
 

corelib/src/Features2d.cpp

@@ -44,10 +44,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "opencv/ORBextractor.h"
 #endif
 
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 #include "superpoint_torch/SuperPoint.h"
 #endif
 
+#ifdef RTABMAP_PYTHON
+#include "python/PyDetector.h"
+#endif
+
 #if CV_MAJOR_VERSION < 3
 #include "opencv/Orb.h"
 #ifdef HAVE_OPENCV_GPU
@@ -584,7 +588,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif
 
-#ifndef RTABMAP_SUPERPOINT_TORCH
+#ifndef RTABMAP_TORCH
 	if(type == Feature2D::kFeatureSuperPointTorch)
 	{
 		UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
@@ -628,7 +632,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureOrbOctree:
 		feature2D = new ORBOctree(parameters);
 		break;
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 	case Feature2D::kFeatureSuperPointTorch:
 		feature2D = new SuperPointTorch(parameters);
 		break;
@@ -642,6 +646,11 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureSurfDaisy:
 		feature2D = new SURF_DAISY(parameters);
 		break;
+#ifdef RTABMAP_PYTHON
+	case Feature2D::kFeaturePyDetector:
+		feature2D = new PyDetector(parameters);
+		break;
+#endif
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -2051,7 +2060,7 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 	Feature2D::parseParameters(parameters);
 
 	std::string previousPath = path_;
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 	bool previousCuda = cuda_;
 #endif
 	Parameters::parse(parameters, Parameters::kSuperPointModelPath(), path_);
@@ -2060,7 +2069,7 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSuperPointNMSRadius(), minDistance_);
 	Parameters::parse(parameters, Parameters::kSuperPointCuda(), cuda_);
 
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 	if(superPoint_.get() == 0 || path_.compare(previousPath) != 0 || previousCuda != cuda_)
 	{
 		superPoint_ = cv::Ptr<SPDetector>(new SPDetector(path_, threshold_, nms_, minDistance_, cuda_));
@@ -2072,29 +2081,29 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 		superPoint_->setMinDistance(minDistance_);
 	}
 #else
-	UWARN("RTAB-Map is not built with SuperPoint Torch support so SuperPoint Torch feature cannot be used!");
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
 #endif
 }
 
 std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	UASSERT_MSG(roi.x==0 && roi.y ==0, "Not supporting ROI");
 	return superPoint_->detect(image, mask);
 #else
-	UWARN("RTAB-Map is not built with SuperPoint Torch support so SuperPoint Torch feature cannot be used!");
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
 	return std::vector<cv::KeyPoint>();
 #endif
 }
 
 cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	return superPoint_->compute(keypoints);
 #else
-	UWARN("RTAB-Map is not built with SuperPoint Torch support so SuperPoint Torch feature cannot be used!");
+	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
 	return cv::Mat();
 #endif
 }

corelib/src/Parameters.cpp

@@ -167,7 +167,8 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
 			group.compare("GFTT") == 0 ||
 			group.compare("BRISK") == 0 ||
 			group.compare("KAZE") == 0 ||
-			group.compare("SuperPoint") == 0;
+			group.compare("SuperPoint") == 0 ||
+			group.compare("PyDetector") == 0;
 }
 
 rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, bool vis, bool icp)
@@ -625,13 +626,13 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With SuperPoint Torch:";
-#ifdef RTABMAP_SUPERPOINT_TORCH
+#ifdef RTABMAP_TORCH
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With Python3:";
-#ifdef RTABMAP_PYMATCHER
+#ifdef RTABMAP_PYTHON
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;

corelib/src/RegistrationVis.cpp

@@ -51,8 +51,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtflann/flann.hpp>
 
 
-#ifdef RTABMAP_PYMATCHER
-	#include <pymatcher/PyMatcher.h>
+#ifdef RTABMAP_PYTHON
+	#include "python/PyMatcher.h"
 #endif
 
 namespace rtabmap {
@@ -87,7 +87,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
 		_detectorFrom(0),
 		_detectorTo(0)
-#ifdef RTABMAP_PYMATCHER
+#ifdef RTABMAP_PYTHON
 		,
 		_pyMatcher(0)
 #endif
@@ -153,7 +153,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	if(_nnType == 6)
 	{
 		// verify that we have Python3 support
-#ifndef RTABMAP_PYMATCHER
+#ifndef RTABMAP_PYTHON
 		UWARN("%s is set to 6 but RTAB-Map is not built with Python3 support, using default %d.",
 				Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
 		_nnType = Parameters::defaultVisCorNNType();
@@ -268,7 +268,7 @@ RegistrationVis::~RegistrationVis()
 {
 	delete _detectorFrom;
 	delete _detectorTo;
-#ifdef RTABMAP_PYMATCHER
+#ifdef RTABMAP_PYTHON
 	delete _pyMatcher;
 #endif
 }
@@ -1152,7 +1152,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					// match between all descriptors
 					std::list<int> fromWordIds;
 					std::list<int> toWordIds;
-#ifdef RTABMAP_PYMATCHER
+#ifdef RTABMAP_PYTHON
 					if(_nnType == 5 || (_nnType == 6 && _pyMatcher) || _nnType==7)
 #else
 					if(_nnType == 5 || _nnType == 7) // bruteforce cross check or GMS
@@ -1173,7 +1173,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						{
 							std::vector<int> toWordIdsV(descriptorsTo.rows, 0);
 							std::vector<cv::DMatch> matches;
-#ifdef RTABMAP_PYMATCHER
+#ifdef RTABMAP_PYTHON
 							if(_nnType == 6 && _pyMatcher &&
 								descriptorsTo.cols == descriptorsFrom.cols &&
 								descriptorsTo.rows == (int)kptsTo.size() &&

corelib/src/python/PyDetector.cpp

@@ -0,0 +1,234 @@
+/**
+ * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ */
+
+#include "PyDetector.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#define NPY_NO_DEPRECATED_API NPY_API_VERSION
+#include <numpy/arrayobject.h>
+
+namespace rtabmap
+{
+
+PyDetector::PyDetector(const ParametersMap & parameters) :
+		pModule_(0),
+		pFunc_(0),
+		path_(Parameters::defaultPyDetectorPath()),
+		cuda_(Parameters::defaultPyDetectorCuda())
+{
+	this->parseParameters(parameters);
+
+	UDEBUG("path = %s", path_.c_str());
+	if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
+	{
+		UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"",
+				Parameters::kPyDetectorPath().c_str(), path_.c_str());
+		return;
+	}
+
+	lock();
+	
+	std::string matcherPythonDir = UDirectory::getDir(path_);
+	if(!matcherPythonDir.empty())
+	{
+		PyRun_SimpleString("import sys");
+		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
+	}
+
+	_import_array();
+
+	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
+	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
+	UDEBUG("PyImport_Import() beg");
+	pModule_ = PyImport_Import(pName);
+	UDEBUG("PyImport_Import() end");
+
+	Py_DECREF(pName);
+
+	if(!pModule_)
+	{
+		UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
+		UERROR("%s", getTraceback().c_str());
+	}
+
+	unlock();
+}
+
+PyDetector::~PyDetector()
+{
+	lock();
+
+	if(pFunc_)
+	{
+		Py_DECREF(pFunc_);
+	}
+	if(pModule_)
+	{
+		Py_DECREF(pModule_);
+	}
+
+	unlock();
+}
+
+void PyDetector::parseParameters(const ParametersMap & parameters)
+{
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
+	Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
+
+	path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
+}
+
+std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
+{
+	UDEBUG("");
+	descriptors_ = cv::Mat();
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat imgRoi(image, roi);
+
+	UTimer timer;
+
+	if(!pModule_)
+	{
+		UERROR("Python detector module not loaded!");
+		return keypoints;
+	}
+
+	lock();
+
+	if(!pFunc_)
+	{
+		PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
+		if(pFunc)
+		{
+			if(PyCallable_Check(pFunc))
+			{
+				PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0);
+
+				if(result == NULL)
+				{
+					UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
+					UERROR("%s", getTraceback().c_str());
+					return keypoints;
+				}
+				Py_DECREF(result);
+
+				pFunc_ = PyObject_GetAttrString(pModule_, "detect");
+				if(pFunc_ && PyCallable_Check(pFunc_))
+				{
+					// we are ready!
+				}
+				else
+				{
+					UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
+					UERROR("%s", getTraceback().c_str());
+					if(pFunc_)
+					{
+						Py_DECREF(pFunc_);
+						pFunc_ = 0;
+					}
+					return keypoints;
+				}
+			}
+			else
+			{
+				UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
+				UERROR("%s", getTraceback().c_str());
+				return keypoints;
+			}
+			Py_DECREF(pFunc);
+		}
+		else
+		{
+			UERROR("Cannot find method \"init(...)\"");
+			UERROR("%s", getTraceback().c_str());
+			return keypoints;
+		}
+		UDEBUG("init time = %fs", timer.ticks());
+	}
+
+	if(pFunc_)
+	{
+		npy_intp dims[2] = {imgRoi.rows, imgRoi.cols};
+		PyObject* pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data);
+		UASSERT(pImageBuffer);
+
+		UDEBUG("Preparing data time = %fs", timer.ticks());
+
+		PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL);
+		if(pReturn == NULL)
+		{
+			UERROR("Failed to call match() function!");
+			UERROR("%s", getTraceback().c_str());
+		}
+		else
+		{
+			UDEBUG("Python detector time = %fs", timer.ticks());
+
+			if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2)
+			{
+				PyObject *kptsPtr = PyTuple_GET_ITEM(pReturn, 0);
+				PyObject *descPtr = PyTuple_GET_ITEM(pReturn, 1);
+				if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr))
+				{
+					PyArrayObject *arrayPtr = reinterpret_cast<PyArrayObject*>(kptsPtr);
+					int nKpts = PyArray_SHAPE(arrayPtr)[0];
+					int kptSize = PyArray_SHAPE(arrayPtr)[1];
+					int type = PyArray_TYPE(arrayPtr);
+					UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type);
+					UASSERT(kptSize == 3);
+					UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
+
+					float* c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
+					keypoints.reserve(nKpts);
+					for (int i = 0; i < nKpts*kptSize; i+=kptSize)
+					{
+						cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]);
+						keypoints.push_back(kpt);
+					}
+
+					arrayPtr = reinterpret_cast<PyArrayObject*>(descPtr);
+					int nDesc = PyArray_SHAPE(arrayPtr)[0];
+					UASSERT(nDesc = nKpts);
+					int dim = PyArray_SHAPE(arrayPtr)[1];
+					type = PyArray_TYPE(arrayPtr);
+					UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type);
+					UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
+
+					c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
+					for (int i = 0; i < nDesc*dim; i+=dim)
+					{
+						cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
+						descriptors_.push_back(descriptor);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features.");
+			}
+			Py_DECREF(pReturn);
+		}
+		Py_DECREF(pImageBuffer);
+	}
+
+	unlock();
+
+	return keypoints;
+}
+
+cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT((int)keypoints.size() == descriptors_.rows);
+	return descriptors_;
+}
+
+}

corelib/src/python/PyDetector.h

@@ -0,0 +1,44 @@
+/**
+ * Python interface for python matchers like:
+ *  - SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ *  - OANET https://github.com/zjhthu/OANet
+ */
+
+#ifndef PYMATCHER_H
+#define PYMATCHER_H
+
+#include <rtabmap/core/Features2d.h>
+#include <opencv2/core/types.hpp>
+#include <opencv2/core/mat.hpp>
+#include <vector>
+
+#include "rtabmap/core/PythonInterface.h"
+#include <Python.h>
+
+namespace rtabmap
+{
+
+class PyDetector : public Feature2D, public PythonInterface
+{
+public:
+	PyDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~PyDetector();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeaturePyDetector;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+  PyObject * pModule_;
+  PyObject * pFunc_;
+  std::string path_;
+  bool cuda_;
+  cv::Mat descriptors_;
+};
+
+}
+
+#endif

corelib/src/python/PyMatcher.cpp

@@ -2,7 +2,7 @@
  * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
  */
 
-#include <pymatcher/PyMatcher.h>
+#include <python/PyMatcher.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
@@ -16,52 +16,6 @@
 namespace rtabmap
 {
 
-class PythonSingleTon
-{
-public:
-	PythonSingleTon() : initialized_(false) {}
-	void init() {UScopeMutex lock(mutex_); if(!initialized_)Py_Initialize(); initialized_=true;}
-	bool initialized() const {return initialized_;}
-	virtual ~PythonSingleTon() {if(initialized_) Py_Finalize();}
-private:
-	bool initialized_;
-	UMutex mutex_;
-};
-
-static PythonSingleTon g_python;
-
-std::string getTraceback()
-{
-	// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated
-
-	PyObject* type;
-	PyObject* value;
-	PyObject* traceback;
-
-	PyErr_Fetch(&type, &value, &traceback);
-	PyErr_NormalizeException(&type, &value, &traceback);
-
-	std::string fcn = "";
-	fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n";
-	fcn += "    import sys, traceback\n";
-	fcn += "    lines = []\n";
-	fcn += "    lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n";
-	fcn += "    output = '\\n'.join(lines)\n";
-	fcn += "    return output\n";
-
-	PyRun_SimpleString(fcn.c_str());
-	PyObject* mod = PyImport_ImportModule("__main__");
-	PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback");
-	PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback));
-	std::string pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr));
-
-	Py_DECREF(method);
-	Py_DECREF(outStr);
-	Py_DECREF(mod);
-
-	return pretty;
-}
-
 PyMatcher::PyMatcher(
 		const std::string & pythonMatcherPath,
 		float matchThreshold,
@@ -85,10 +39,7 @@ PyMatcher::PyMatcher(
 		return;
 	}
 
-	if(!g_python.initialized())
-	{
-		g_python.init();
-	}
+	lock();
 
 	std::string matcherPythonDir = UDirectory::getDir(path_);
 	if(!matcherPythonDir.empty())
@@ -101,6 +52,7 @@ PyMatcher::PyMatcher(
 
 	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
 	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
+	UDEBUG("PyImport_Import");
 	pModule_ = PyImport_Import(pName);
 	Py_DECREF(pName);
 
@@ -109,10 +61,13 @@ PyMatcher::PyMatcher(
 		UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
 		UERROR("%s", getTraceback().c_str());
 	}
+
+	unlock();
 }
 
 PyMatcher::~PyMatcher()
 {
+	lock();
 	if(pFunc_)
 	{
 		Py_DECREF(pFunc_);
@@ -121,6 +76,7 @@ PyMatcher::~PyMatcher()
 	{
 		Py_DECREF(pModule_);
 	}
+	unlock();
 }
 
 std::vector<cv::DMatch> PyMatcher::match(
@@ -148,6 +104,8 @@ std::vector<cv::DMatch> PyMatcher::match(
 	   imageSize.width>0 && imageSize.height>0)
 	{
 
+		lock();
+
 		UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0);
 
 		if(!pFunc_)
@@ -302,6 +260,7 @@ std::vector<cv::DMatch> PyMatcher::match(
 
 			UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks());
 		}
+		unlock();
 	}
 	else
 	{

corelib/src/python/PyMatcher.h

@@ -9,14 +9,14 @@
 
 #include <opencv2/core/types.hpp>
 #include <opencv2/core/mat.hpp>
+#include "rtabmap/core/PythonInterface.h"
 #include <vector>
-
 #include <Python.h>
 
 namespace rtabmap
 {
 
-class PyMatcher
+class PyMatcher : public PythonInterface
 {
 public:
   PyMatcher(const std::string & pythonMatcherPath,

corelib/src/python/PythonInterface.cpp

@@ -0,0 +1,119 @@
+/*
+ * PythonSingleTon.h
+ *
+ *  Created on: Jan. 14, 2021
+ *      Author: mathieu
+ */
+
+#include <rtabmap/core/PythonInterface.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UThread.h>
+
+namespace rtabmap {
+
+UMutex PythonInterface::mutex_;
+int PythonInterface::refCount_ = 0;
+PyThreadState * PythonInterface::mainThreadState_ = 0;
+unsigned long PythonInterface::mainThreadID_ = 0;
+
+PythonInterface::PythonInterface() :
+		threadState_(0)
+{
+	UScopeMutex lockM(mutex_);
+	if(refCount_ == 0)
+	{
+		// initialize Python
+		Py_Initialize();
+
+		// initialize thread support
+		PyEval_InitThreads();
+		Py_DECREF(PyImport_ImportModule("threading"));
+
+		//release the GIL, store thread state, set the current thread state to NULL
+		mainThreadState_ = PyEval_SaveThread();
+		UASSERT(mainThreadState_);
+		mainThreadID_ = UThread::currentThreadId();
+	}
+
+	++refCount_;
+}
+
+PythonInterface::~PythonInterface()
+{
+	UScopeMutex lock(mutex_);
+	if(refCount_>0 && --refCount_==0)
+	{
+		// shut down the interpreter
+		PyEval_RestoreThread(mainThreadState_);
+		Py_Finalize();
+	}
+}
+
+void PythonInterface::lock()
+{
+	mutex_.lock();
+
+	if(UThread::currentThreadId() == mainThreadID_)
+	{
+		PyEval_RestoreThread(mainThreadState_);
+	}
+	else
+	{
+		// create a thread state object for this thread
+		threadState_ = PyThreadState_New(mainThreadState_->interp);
+		UASSERT(threadState_);
+		PyEval_RestoreThread(threadState_);
+	}
+}
+
+void PythonInterface::unlock()
+{
+	if(UThread::currentThreadId() == mainThreadID_)
+	{
+		mainThreadState_ = PyEval_SaveThread();
+	}
+	else
+	{
+		PyThreadState_Clear(threadState_);
+		PyThreadState_DeleteCurrent();
+	}
+	mutex_.unlock();
+}
+
+std::string PythonInterface::getTraceback()
+{
+	// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated
+
+	PyObject* type;
+	PyObject* value;
+	PyObject* traceback;
+
+	PyErr_Fetch(&type, &value, &traceback);
+	PyErr_NormalizeException(&type, &value, &traceback);
+
+	std::string fcn = "";
+	fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n";
+	fcn += "    import sys, traceback\n";
+	fcn += "    lines = []\n";
+	fcn += "    lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n";
+	fcn += "    output = '\\n'.join(lines)\n";
+	fcn += "    return output\n";
+
+	PyRun_SimpleString(fcn.c_str());
+	PyObject* mod = PyImport_ImportModule("__main__");
+	UASSERT(mod);
+	PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback");
+	UASSERT(method);
+	PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback));
+	std::string pretty;
+	if(outStr)
+		pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr));
+
+	Py_DECREF(method);
+	Py_DECREF(outStr);
+	Py_DECREF(mod);
+
+	return pretty;
+}
+
+}

corelib/src/python/rtabmap_superpoint.py

@@ -0,0 +1,56 @@
+#! /usr/bin/env python3
+#
+# Drop this file in the root folder of SuperPoint git: https://github.com/magicleap/SuperPointPretrainedNetwork
+# To use with rtabmap:
+#   --Vis/FeatureType 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py" --PyDetector/Model "~/SuperPointPretrainedNetwork/superpoint_v1.pth"
+#
+
+import random
+import numpy as np
+import torch
+
+#import sys
+#import os
+#print(os.sys.path)
+#print(sys.version)
+
+from demo_superpoint import SuperPointFrontend
+
+torch.set_grad_enabled(False)
+
+device = 'cpu'
+superpoint = []
+
+def init(cuda):
+    #print("SuperPoint python init()")
+    
+    global device
+    device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
+    
+    # This class runs the SuperPoint network and processes its outputs.
+    global superpoint
+    superpoint = SuperPointFrontend(weights_path="superpoint_v1.pth",
+                          nms_dist=4,
+                          conf_thresh=0.015,
+                          nn_thresh=1,
+                          cuda=cuda)
+
+def detect(imageBuffer):
+    #print("SuperPoint python detect()")
+    global device
+    image = np.asarray(imageBuffer)
+    image = (image.astype('float32') / 255.)
+   
+    global superpoint
+    pts, desc, heatmap = superpoint.run(image)
+    # return float: Kpts:Nx3, Desc:NxDim
+    # use copy to make sure memory is correctly re-ordered
+    pts = np.float32(np.transpose(pts)).copy()
+    desc = np.float32(np.transpose(desc)).copy()
+    return pts, desc
+
+
+if __name__ == '__main__':
+    #test
+    init(True)
+    detect(np.random.rand(640,480)*255)

corelib/src/superpoint_torch/SuperPoint.cc

@@ -168,9 +168,9 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 		auto kpts = (prob_ > threshold_);
 		kpts = torch::nonzero(kpts);  // [n_keypoints, 2]  (y, x)
 
-    //convert back to cpu if in gpu
-    auto kpts_cpu = kpts.to(torch::kCPU);
-    auto prob_cpu = prob_.to(torch::kCPU);
+		//convert back to cpu if in gpu
+		auto kpts_cpu = kpts.to(torch::kCPU);
+		auto prob_cpu = prob_.to(torch::kCPU);
 
 		std::vector<cv::KeyPoint> keypoints_no_nms;
 		for (int i = 0; i < kpts_cpu.size(0); i++) {