SuperPoint: return at minimum 2 features or 0 (fixed crash when only one feature). Python: added main python instance from Rtabmap object (to get python instancied once for ROS). MultiSessionWidget: Fixed layout warning. Memory::computeTransform() avoid re-instanciating RegistrationVis when Reg/Strategy=1 for guess transform (to fix PyMatcher re-initialized every time). CMake: added video module to opencv required components.

corelib/src/CMakeLists.txt

@@ -199,6 +199,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
 		Python3::Python
+		Python3::NumPy
 	)
 	SET(SRC_FILES
 		${SRC_FILES}

corelib/src/Memory.cpp

@@ -122,14 +122,22 @@ Memory::Memory(const ParametersMap & parameters) :
 	_linksChanged(false),
 	_signaturesAdded(0),
 	_allNodesInWM(true),
-
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
-	_parallelized(Parameters::defaultKpParallelized())
+	_parallelized(Parameters::defaultKpParallelized()),
+	_registrationVis(0)
 {
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
+	if(!_registrationPipeline->isImageRequired())
+	{
+		// make sure feature matching is used instead of optical flow to compute the guess
+		ParametersMap tmp = parameters;
+		uInsert(tmp, ParametersPair(Parameters::kVisCorType(), "0"));
+		uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+		_registrationVis = new RegistrationVis(tmp);
+	}
 
 	// for local scan matching, correspondences ratio should be two times higher as we expect more matches
 	float corRatio = Parameters::defaultIcpCorrespondenceRatio();
@@ -531,6 +539,7 @@ Memory::~Memory()
 	delete _vwd;
 	delete _registrationPipeline;
 	delete _registrationIcpMulti;
+	delete _registrationVis;
 	delete _occupancy;
 }
 
@@ -647,12 +656,28 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
 	uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
 
+	Registration::Type currentStrategy = Registration::kTypeUndef;
+	if(_registrationPipeline)
+	{
+		if(_registrationPipeline->isImageRequired() && _registrationPipeline->isScanRequired())
+		{
+			currentStrategy = Registration::kTypeVisIcp;
+		}
+		else if(_registrationPipeline->isImageRequired())
+		{
+			currentStrategy = Registration::kTypeVis;
+		}
+		else if(_registrationPipeline->isScanRequired())
+		{
+			currentStrategy = Registration::kTypeIcp;
+		}
+	}
 	Registration::Type regStrategy = Registration::kTypeUndef;
 	if((iter=params.find(Parameters::kRegStrategy())) != params.end())
 	{
 		regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
 	}
-	if(regStrategy!=Registration::kTypeUndef)
+	if(regStrategy!=Registration::kTypeUndef && regStrategy != currentStrategy)
 	{
 		UDEBUG("new registration strategy %d", int(regStrategy));
 		if(_registrationPipeline)
@@ -662,10 +687,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}
 
 		_registrationPipeline = Registration::create(regStrategy, parameters_);
+
+		if(!_registrationPipeline->isImageRequired() && _registrationVis == 0)
+		{
+			ParametersMap tmp = params;
+			uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+			_registrationVis = new RegistrationVis(tmp);
+		}
+		else if(_registrationPipeline->isImageRequired() && _registrationVis)
+		{
+			delete _registrationVis;
+			_registrationVis = 0;
+		}
 	}
 	else if(_registrationPipeline)
 	{
 		_registrationPipeline->parseParameters(params);
+
+		if(_registrationVis)
+		{
+			ParametersMap tmp = params;
+			uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+			_registrationVis->parseParameters(tmp);
+		}
 	}
 
 	if(_registrationIcpMulti)
@@ -2835,12 +2879,8 @@ Transform Memory::computeTransform(
 		{
 			UDEBUG("");
 			// no visual in the pipeline, make visual registration for guess
-			// make sure feature matching is used instead of optical flow to compute the guess
-			ParametersMap parameters = parameters_;
-			uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
-			uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
-			RegistrationVis regVis(parameters);
-			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			UASSERT(_registrationVis!=0);
+			guess = _registrationVis->computeTransformation(tmpFrom, tmpTo, guess, info);
 			if(!guess.isNull())
 			{
 				transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);

corelib/src/Rtabmap.cpp

@@ -53,6 +53,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UProcessInfo.h>
 
+#ifdef RTABMAP_PYTHON
+#include "rtabmap/core/PythonInterface.h"
+#endif
+
 #include <pcl/search/kdtree.h>
 #include <pcl/filters/crop_box.h>
 #include <pcl/io/pcd_io.h>
@@ -161,6 +165,9 @@ Rtabmap::Rtabmap() :
 	_pathTransformToGoal(Transform::getIdentity()),
 	_pathStuckCount(0),
 	_pathStuckDistance(0.0f)
+#ifdef RTABMAP_PYTHON
+	,_python(new PythonInterface())
+#endif
 {
 }
 

corelib/src/python/PythonInterface.cpp

@@ -22,6 +22,7 @@ PythonInterface::PythonInterface() :
 	UScopeMutex lockM(mutex_);
 	if(refCount_ == 0)
 	{
+		UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId());
 		// initialize Python
 		Py_Initialize();
 
@@ -44,6 +45,7 @@ PythonInterface::~PythonInterface()
 	if(refCount_>0 && --refCount_==0)
 	{
 		// shut down the interpreter
+		UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId());
 		PyEval_RestoreThread(mainThreadState_);
 		Py_Finalize();
 	}
@@ -53,6 +55,7 @@ void PythonInterface::lock()
 {
 	mutex_.lock();
 
+	UDEBUG("Lock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_);
 	if(UThread::currentThreadId() == mainThreadID_)
 	{
 		PyEval_RestoreThread(mainThreadState_);
@@ -77,6 +80,7 @@ void PythonInterface::unlock()
 		PyThreadState_Clear(threadState_);
 		PyThreadState_DeleteCurrent();
 	}
+	UDEBUG("Unlock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_);
 	mutex_.unlock();
 }
 

corelib/src/superpoint_torch/SuperPoint.cc

@@ -198,11 +198,20 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat &
 			std::vector<cv::KeyPoint> keypoints;
 			cv::Mat descEmpty;
 			NMS(keypoints_no_nms, conf, descEmpty, keypoints, descEmpty, border, dist_thresh, width, height);
-			return keypoints;
+			if(keypoints.size()>1)
+			{
+				return keypoints;
+			}
+			return std::vector<cv::KeyPoint>();
 		}
-		else {
+		else if(keypoints_no_nms.size()>1)
+		{
 			return keypoints_no_nms;
 		}
+		else
+		{
+			return std::vector<cv::KeyPoint>();
+		}
 	}
 	else
 	{
@@ -218,6 +227,10 @@ cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &keypoints)
 		UERROR("SPDetector has been reset before extracting the descriptors! detect() should be called before compute().");
 		return cv::Mat();
 	}
+	if(keypoints.empty())
+	{
+		return cv::Mat();
+	}
 	if(model_.get())
 	{
 		cv::Mat kpt_mat(keypoints.size(), 2, CV_32F);  // [n_keypoints, 2]  (y, x)