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.

CMakeLists.txt

@@ -221,7 +221,7 @@ ENDIF()
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching optflow photo OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching optflow photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
 
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -357,7 +357,7 @@ IF(WITH_TORCH)
 ENDIF(WITH_TORCH)
 
 IF(WITH_PYTHON)
-    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy)
     IF(Python3_FOUND)
         MESSAGE(STATUS "Found Python3")
     ENDIF(Python3_FOUND)

app/src/main.cpp

@@ -38,10 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <vtkObject.h>
 
-#ifdef RTABMAP_PYTHON
-#include "rtabmap/core/PythonInterface.h"
-#endif
-
 using namespace rtabmap;
 
 int main(int argc, char* argv[])
@@ -54,10 +50,6 @@ int main(int argc, char* argv[])
 	CoInitialize(nullptr);
 #endif
 
-#ifdef RTABMAP_PYTHON
-	PythonInterface python; // Make sure we initialize python in main thread
-#endif
-
 #if VTK_MAJOR_VERSION >= 8
 	vtkObject::GlobalWarningDisplayOff();
 #endif

corelib/include/rtabmap/core/Memory.h

@@ -55,6 +55,7 @@ class Statistics;
 class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
+class RegistrationVis;
 class Stereo;
 class OccupancyGrid;
 class MarkerDetector;
@@ -367,6 +368,7 @@ private:
 
 	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcpMulti;
+	RegistrationVis * _registrationVis;
 
 	OccupancyGrid * _occupancy;
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -49,6 +49,7 @@ class Memory;
 class BayesFilter;
 class Signature;
 class Optimizer;
+class PythonInterface;
 
 class RTABMAP_EXP Rtabmap
 {
@@ -374,6 +375,10 @@ private:
 	int _pathStuckCount;
 	float _pathStuckDistance;
 
+#ifdef RTABMAP_PYTHON
+	PythonInterface * _python;
+#endif
+
 };
 
 } // namespace rtabmap

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
+			UASSERT(_registrationVis!=0);
-			ParametersMap parameters = parameters_;
+			guess = _registrationVis->computeTransformation(tmpFrom, tmpTo, guess, info);
-			uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
-			uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
-			RegistrationVis regVis(parameters);
-			guess = regVis.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)

guilib/src/MultiSessionLocWidget.cpp

@@ -57,9 +57,9 @@ MultiSessionLocWidget::MultiSessionLocWidget(
 
 	// setup layout
 	this->setLayout(new QHBoxLayout());
-	QVBoxLayout * vLayout = new QVBoxLayout(this);
+	QVBoxLayout * vLayout = new QVBoxLayout();
 	vLayout->addWidget(imageView_, 1);
-	QHBoxLayout * hLayout = new QHBoxLayout(this);
+	QHBoxLayout * hLayout = new QHBoxLayout();
 	hLayout->addWidget(resetbutton_, 0);
 	hLayout->addWidget(totalLocProgressBar_, 1);
 	vLayout->addLayout(hLayout, 0);