Added SuperGlue support (Vis/CorNNType=6). Added rtabmap-matcher tool. DBViewer: show matches/inliers when refine also fails. SIFT: make sift always available on OpenCV 4.3.0 (#538). Parameters: changed SPTorch prefix to SuperPoint, replaced Vis/CorCrossCheck by Vis/CorNNType=5.

2026-09-02 09:30:25 +08:00 · 2020-05-09 21:13:41 -04:00
parent abc3ebee8c
commit 91285e5e97
35 changed files with 1554 additions and 249 deletions
--- a/corelib/include/rtabmap/core/CameraModel.h
+++ b/corelib/include/rtabmap/core/CameraModel.h
@@ -50,7 +50,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));

 	// minimal
 	CameraModel(
@@ -58,7 +58,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,7 +68,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));

--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/corelib/include/rtabmap/core/Features2d.h
@@ -116,6 +116,37 @@ public:
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
 		kFeatureSuperPointTorch=11}; //new 0.19.7
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kFeatureSurf:
+			return "SURF";
+		case kFeatureSift:
+			return "SIFT";
+		case kFeatureOrb:
+			return "ORB";
+		case kFeatureFastFreak:
+			return "FAST+FREAK";
+		case kFeatureFastBrief:
+			return "FAST+BRIEF";
+		case kFeatureGfttFreak:
+			return "GFTT+Freak";
+		case kFeatureGfttBrief:
+			return "GFTT+Brief";
+		case kFeatureBrisk:
+			return "BRISK";
+		case kFeatureGfttOrb:
+			return "GFTT+ORB";
+		case kFeatureKaze:
+			return "KAZE";
+		case kFeatureOrbOctree:
+			return "ORB-OCTREE";
+		case kFeatureSuperPointTorch:
+			return "SUPERPOINT";
+		default:
+			return "Unknown";
+		}
+	}

 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -324,11 +324,16 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
    RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");

-    RTABMAP_PARAM_STR(SPTorch, ModelPath,              "", "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
-    RTABMAP_PARAM(SPTorch, Threshold,         float,  0.200, "Detector response threshold to accept keypoint.");
-    RTABMAP_PARAM(SPTorch, NMS,               bool,  true, "If true, non-maximum suppression is applied to detected keypoints.");
-    RTABMAP_PARAM(SPTorch, MinDistance,       int,      4, uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSPTorchNMS().c_str()));
-    RTABMAP_PARAM(SPTorch, Cuda,              bool, false, "Use Cuda device for Torch, otherwise CPU device is used by default.");
+    RTABMAP_PARAM_STR(SuperPoint, ModelPath, "",           "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
+    RTABMAP_PARAM(SuperPoint, Threshold,     float, 0.010, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPoint, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
+    RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
+
+    RTABMAP_PARAM_STR(SuperGlue, Path,       "",           "Path to python script file \"rtabmap_superglue.py\" (rtabmap/corelib/src/superglue_pytorch/rtabmap_superglue.py) copied in SuperGlue's Git folder.");
+    RTABMAP_PARAM(SuperGlue, Iterations,     int, 20,      "Sinkhorn iterations.");
+    RTABMAP_PARAM(SuperGlue, MatchThreshold, float, 0.2,   "");
+    RTABMAP_PARAM(SuperGlue, Cuda,           bool, true,   "");

    // BayesFilter
    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
@@ -604,9 +609,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
    RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6. Used for features matching approach.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
-    RTABMAP_PARAM(Vis, CorCrossCheck,            bool, false,  uFormat("[%s=0] If true, brute force crosscheck matching is done instead of knn matching approach (%s).", kVisCorType().c_str(), kVisCorNNDR().c_str()));
    RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,     uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
    RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
--- a/corelib/include/rtabmap/core/RegistrationVis.h
+++ b/corelib/include/rtabmap/core/RegistrationVis.h
@@ -37,6 +37,10 @@ namespace rtabmap {

 class Feature2D;

+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+class SuperGlue;
+#endif
+
 // Visual registration
 class RTABMAP_EXP RegistrationVis : public Registration
 {
@@ -50,6 +54,11 @@ public:
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getMinInliers() const {return _minInliers;}
+	int getNNType() const {return _nnType;}
+	float getNNDR() const {return _nndr;}
+	int getEstimationType() const {return _estimationType;}
+
+	const Feature2D * getDetector() const {return _detectorFrom;}

 protected:
 	virtual Transform computeTransformationImpl(
@@ -78,8 +87,8 @@ private:
 	int _flowIterations;
 	float _flowEps;
 	int _flowMaxLevel;
-	bool _bfCrossCheck;
 	float _nndr;
+	int _nnType;
 	int _guessWinSize;
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
@@ -92,6 +101,10 @@ private:

 	Feature2D * _detectorFrom;
 	Feature2D * _detectorTo;
+
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+	SuperGlue * _superGlueMatcher;
+#endif
 };

 }
--- a/corelib/include/rtabmap/core/StereoCameraModel.h
+++ b/corelib/include/rtabmap/core/StereoCameraModel.h
@@ -43,7 +43,7 @@ public:
 			const cv::Size & imageSize2,
 			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
 			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));

 	// if R and T are not null, left and right camera models should be valid to be rectified.
 	StereoCameraModel(
@@ -68,7 +68,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
 	//minimal to be saved
 	StereoCameraModel(
@@ -78,7 +78,7 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
 	virtual ~StereoCameraModel() {}

--- a/corelib/include/rtabmap/core/VWDictionary.h
+++ b/corelib/include/rtabmap/core/VWDictionary.h
@@ -55,6 +55,23 @@ public:
 		kNNUndef};
 	static const int ID_START;
 	static const int ID_INVALID;
+	static std::string nnStrategyName(NNStrategy strategy)
+	{
+		switch(strategy) {
+		case kNNFlannNaive:
+			return "FLANN NAIVE";
+		case kNNFlannKdTree:
+			return "FLANN KD-TREE";
+		case kNNFlannLSH:
+			return "FLANN LSH";
+		case kNNBruteForce:
+			return "BRUTE FORCE";
+		case kNNBruteForceGPU:
+			return "BRUTE FORCE GPU";
+		default:
+			return "Unknown";
+		}
+	}

 public:
 	VWDictionary(const ParametersMap & parameters = ParametersMap());
--- a/corelib/include/rtabmap/core/util3d_features.h
+++ b/corelib/include/rtabmap/core/util3d_features.h
@@ -85,7 +85,8 @@ std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
 		float ransacParam1 = 3.0f,
 		float ransacParam2 = 0.99f,
 		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
-		double * variance = 0);
+		double * variance = 0,
+		std::vector<int> * matchesOut = 0);

 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -190,6 +190,24 @@ IF(TORCH_FOUND)
 	)
 ENDIF(TORCH_FOUND)

+IF(Python3_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		Python3::Python
+	)
+	SET(SRC_FILES
+		${SRC_FILES}
+		superglue_pytorch/SuperGlue.cpp
+	)
+	 SET(INCLUDE_DIRS
+	    ${TORCH_INCLUDE_DIRS}
+	    ${CMAKE_CURRENT_SOURCE_DIR}/superglue_pytorch
+		${INCLUDE_DIRS}
+	)
+ENDIF(Python3_FOUND)
+
+
+
 IF(Freenect_FOUND)
 	IF(Freenect_DASH_INCLUDES)
 	   ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")
--- a/corelib/src/CameraModel.cpp
+++ b/corelib/src/CameraModel.cpp
@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

-CameraModel::CameraModel()
+CameraModel::CameraModel() :
+	localTransform_(0,0,1,0, -1,0,0,0, 0,-1,0,0)
 {

 }
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "opencv/ORBextractor.h"
 #endif

-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 #include "superpoint_torch/SuperPoint.h"
 #endif

@@ -472,6 +472,8 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
 }
 Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {
+
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION < 3)
 #ifndef RTABMAP_NONFREE
 	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
 	{
@@ -494,6 +496,18 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #endif
 #endif

+#else // >= 4.3.0
+
+#ifndef RTABMAP_NONFREE
+	if(type == Feature2D::kFeatureSurf)
+	{
+		UWARN("SURF features cannot be used because OpenCV was not built with xfeatures2d module. SIFT is used instead.");
+		type = Feature2D::kFeatureSift;
+	}
+#endif
+
+#endif // 4.3.0
+
 #if CV_MAJOR_VERSION < 3
 	if(type == Feature2D::kFeatureKaze)
 	{
@@ -515,7 +529,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif

-#ifndef RTABMAP_SP_TORCH
+#ifndef RTABMAP_SUPERPOINT_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.");
@@ -559,7 +573,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureOrbOctree:
 		feature2D = new ORBOctree(parameters);
 		break;
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	case Feature2D::kFeatureSuperPointTorch:
 		feature2D = new SuperPointTorch(parameters);
 		break;
@@ -909,6 +923,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);

+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION < 3)
 #ifdef RTABMAP_NONFREE
 #if CV_MAJOR_VERSION < 3
 	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
@@ -918,13 +933,16 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
+#else
+	_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#endif
 }

 std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#ifdef RTABMAP_NONFREE
+#if defined(RTABMAP_NONFREE) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 3)
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if(!mask.empty())
@@ -942,7 +960,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#ifdef RTABMAP_NONFREE
+#if defined(RTABMAP_NONFREE) || CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 3)
 	_sift->compute(image, keypoints, descriptors);
 #else
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
@@ -1866,11 +1884,11 @@ cv::Mat ORBOctree::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv
 //SuperPointTorch
 //////////////////////////
 SuperPointTorch::SuperPointTorch(const ParametersMap & parameters) :
-		path_(Parameters::defaultSPTorchModelPath()),
-		threshold_(Parameters::defaultSPTorchThreshold()),
-		nms_(Parameters::defaultSPTorchNMS()),
-		minDistance_(Parameters::defaultSPTorchMinDistance()),
-		cuda_(Parameters::defaultSPTorchCuda())
+		path_(Parameters::defaultSuperPointModelPath()),
+		threshold_(Parameters::defaultSuperPointThreshold()),
+		nms_(Parameters::defaultSuperPointNMS()),
+		minDistance_(Parameters::defaultSuperPointNMSRadius()),
+		cuda_(Parameters::defaultSuperPointCuda())
 {
 	parseParameters(parameters);
 }
@@ -1884,14 +1902,16 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)
 	Feature2D::parseParameters(parameters);

 	std::string previousPath = path_;
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	bool previousCuda = cuda_;
-	Parameters::parse(parameters, Parameters::kSPTorchModelPath(), path_);
-	Parameters::parse(parameters, Parameters::kSPTorchThreshold(), threshold_);
-	Parameters::parse(parameters, Parameters::kSPTorchNMS(), nms_);
-	Parameters::parse(parameters, Parameters::kSPTorchMinDistance(), minDistance_);
-	Parameters::parse(parameters, Parameters::kSPTorchCuda(), cuda_);
+#endif
+	Parameters::parse(parameters, Parameters::kSuperPointModelPath(), path_);
+	Parameters::parse(parameters, Parameters::kSuperPointThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kSuperPointNMS(), nms_);
+	Parameters::parse(parameters, Parameters::kSuperPointNMSRadius(), minDistance_);
+	Parameters::parse(parameters, Parameters::kSuperPointCuda(), cuda_);

-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	if(superPoint_.get() == 0 || path_.compare(previousPath) != 0 || previousCuda != cuda_)
 	{
 		superPoint_ = cv::Ptr<SPDetector>(new SPDetector(path_, threshold_, nms_, minDistance_, cuda_));
@@ -1909,10 +1929,10 @@ void SuperPointTorch::parseParameters(const ParametersMap & parameters)

 std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_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);
+	return superPoint_->detect(image, mask);
 #else
 	UWARN("RTAB-Map is not built with SuperPoint Torch support so SuperPoint Torch feature cannot be used!");
 	return std::vector<cv::KeyPoint>();
@@ -1921,7 +1941,7 @@ std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat &

 cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	return superPoint_->compute(keypoints);
 #else
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -166,7 +166,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
 			group.compare("GFTT") == 0 ||
 			group.compare("BRISK") == 0 ||
 			group.compare("KAZE") == 0 ||
-			group.compare("SPTorch") == 0;
+			group.compare("SuperPoint") == 0;
 }

 rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, bool vis, bool icp)
@@ -184,7 +184,7 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			group.compare("Optimizer") == 0 ||
 			group.compare("g2o") == 0 ||
 			group.compare("GTSAM") == 0 ||
-			(vis && group.compare("Vis") == 0) ||
+			(vis && (group.compare("Vis") == 0 || group.compare("SuperGlue") == 0)) ||
 			iter->first.compare(kRtabmapPublishRAMUsage())==0)
 		{
 			if(stereo)
@@ -238,6 +238,14 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.20.
+		removedParameters_.insert(std::make_pair("Vis/CorCrossCheck",   std::make_pair(false, Parameters::kVisCorNNType())));
+		removedParameters_.insert(std::make_pair("SPTorch/ModelPath",   std::make_pair(true,  Parameters::kSuperPointModelPath())));
+		removedParameters_.insert(std::make_pair("SPTorch/Threshold",   std::make_pair(true,  Parameters::kSuperPointThreshold())));
+		removedParameters_.insert(std::make_pair("SPTorch/NMS",         std::make_pair(true,  Parameters::kSuperPointNMS())));
+		removedParameters_.insert(std::make_pair("SPTorch/MinDistance", std::make_pair(true,  Parameters::kSuperPointNMSRadius())));
+		removedParameters_.insert(std::make_pair("SPTorch/Cuda",        std::make_pair(true,  Parameters::kSuperPointCuda())));
+
 		// 0.19.4
 		removedParameters_.insert(std::make_pair("RGBD/MaxLocalizationDistance", std::make_pair(true,  Parameters::kRGBDMaxLoopClosureDistance())));

@@ -608,7 +616,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_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_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 SuperGlue PyTorch:";
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -46,6 +46,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtflann/flann.hpp>

+
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+	#include "superglue_pytorch/SuperGlue.h"
+#endif
+
 namespace rtabmap {

 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
@@ -66,7 +71,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
 		_nndr(Parameters::defaultVisCorNNDR()),
-		_bfCrossCheck(Parameters::defaultVisCorCrossCheck()),
+		_nnType(Parameters::defaultVisCorNNType()),
 		_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
@@ -74,7 +79,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
 		_maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
 		_detectorFrom(0),
-		_detectorTo(0)
+		_detectorTo(0),
+		_superGlueMatcher(0)
 {
 	_featureParameters = Parameters::getDefaultParameters();
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -114,7 +120,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
 	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
-	Parameters::parse(parameters, Parameters::kVisCorCrossCheck(), _bfCrossCheck);
+	Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
 	Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
@@ -131,6 +137,38 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
 	UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());

+	if(_nnType == 6)
+	{
+		// verify that we have SuperGlue support
+#ifndef RTABMAP_SUPERGLUE_PYTORCH
+		UWARN("%s is set to 6 but RTAB-MAp is not built with SuperGlue support, using default %d.",
+				Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
+		_nnType = Parameters::defaultVisCorNNType();
+#else
+		int iterations = _superGlueMatcher?_superGlueMatcher->iterations():Parameters::defaultSuperGlueIterations();
+		float matchThr = _superGlueMatcher?_superGlueMatcher->matchThreshold():Parameters::defaultSuperGlueMatchThreshold();
+		std::string path = _superGlueMatcher?_superGlueMatcher->path():Parameters::defaultSuperGluePath();
+		bool cuda = _superGlueMatcher?_superGlueMatcher->cuda():Parameters::defaultSuperGlueCuda();
+		Parameters::parse(parameters, Parameters::kSuperGlueIterations(), iterations);
+		Parameters::parse(parameters, Parameters::kSuperGlueMatchThreshold(), matchThr);
+		Parameters::parse(parameters, Parameters::kSuperGluePath(), path);
+		Parameters::parse(parameters, Parameters::kSuperGlueCuda(), cuda);
+		if(path.empty())
+		{
+			UERROR("%s parameter should be set to use SuperGlue matching (%s=6), using default %d.",
+					Parameters::kSuperGluePath().c_str(),
+					Parameters::kVisCorNNType().c_str(),
+					Parameters::defaultVisCorNNType());
+			_nnType = Parameters::defaultVisCorNNType();
+		}
+		else
+		{
+			delete _superGlueMatcher;
+			_superGlueMatcher = new SuperGlue(path, matchThr, iterations, cuda);
+		}
+#endif
+	}
+
 	// override feature parameters
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
@@ -143,7 +181,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)

 	if(uContains(parameters, Parameters::kVisCorNNType()))
 	{
-		uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
+		if(_nnType<VWDictionary::kNNUndef)
+		{
+			uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_nnType)));
+		}
 	}
 	if(uContains(parameters, Parameters::kVisCorNNDR()))
 	{
@@ -200,6 +241,9 @@ RegistrationVis::~RegistrationVis()
 {
 	delete _detectorFrom;
 	delete _detectorTo;
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+	delete _superGlueMatcher;
+#endif
 }

 Transform RegistrationVis::computeTransformationImpl(
@@ -222,7 +266,8 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
 	UDEBUG("%s=%f", Parameters::kVisCorNNDR().c_str(), _nndr);
-	UDEBUG("%s=%d", Parameters::kVisCorCrossCheck().c_str(), _bfCrossCheck?1:0);
+	UDEBUG("%s=%d", Parameters::kVisCorNNType().c_str(), _nnType);
+	UDEBUG("Feature Detector = %d", (int)_detectorFrom->getType());
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());

 	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
@@ -807,9 +852,8 @@ Transform RegistrationVis::computeTransformationImpl(
 									descriptorsIndices.resize(oi);
 									UASSERT(oi >=2);

-
-									cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _bfCrossCheck);
-									if(_bfCrossCheck)
+									cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _nnType == 5);
+									if(_nnType == 5) // bruteforce cross check
 									{
 										std::vector<cv::DMatch> matches;
 										matcher.match(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches);
@@ -818,7 +862,7 @@ Transform RegistrationVis::computeTransformationImpl(
 											matchedIndex = descriptorsIndices.at(matches.at(0).trainIdx);
 										}
 									}
-									else
+									else // bruteforce knn
 									{
 										std::vector<std::vector<cv::DMatch> > matches;
 										matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
@@ -829,7 +873,6 @@ Transform RegistrationVis::computeTransformationImpl(
 											matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
 										}
 									}
-
 								}
 								else if(indices[i].size() == 1)
 								{
@@ -957,8 +1000,8 @@ Transform RegistrationVis::computeTransformationImpl(
 										bruteForceDescCopy += bruteForceTimer.ticks();
 										UASSERT(oi >=2);

-										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _bfCrossCheck);
-										if(_bfCrossCheck)
+										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _nnType==5);
+										if(_nnType==5) // bruteforce cross check
 										{
 											std::vector<cv::DMatch> matches;
 											matcher.match(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches);
@@ -967,7 +1010,7 @@ Transform RegistrationVis::computeTransformationImpl(
 												matchedIndexTo = descriptorsIndices.at(matches.at(0).trainIdx);
 											}
 										}
-										else
+										else // bruteforce knn
 										{
 											std::vector<std::vector<cv::DMatch> > matches;
 											matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
@@ -1068,7 +1111,11 @@ Transform RegistrationVis::computeTransformationImpl(
 					// match between all descriptors
 					std::list<int> fromWordIds;
 					std::list<int> toWordIds;
-					if(_bfCrossCheck)
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+					if(_nnType == 5 || (_nnType == 6 && _superGlueMatcher))
+#else
+					if(_nnType == 5) // bruteforce cross check
+#endif
 					{
 						std::vector<int> fromWordIdsV(descriptorsFrom.rows);
 						for (int i = 0; i < descriptorsFrom.rows; ++i)
@@ -1083,10 +1130,33 @@ Transform RegistrationVis::computeTransformationImpl(
 						}
 						if(descriptorsTo.rows)
 						{
-							cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, true);
 							std::vector<int> toWordIdsV(descriptorsTo.rows, 0);
 							std::vector<cv::DMatch> matches;
-							matcher.match(descriptorsTo, descriptorsFrom, matches);
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+							if(_nnType == 6 && _superGlueMatcher &&
+								descriptorsTo.cols == descriptorsFrom.cols &&
+								descriptorsTo.rows == (int)kptsTo.size() &&
+								descriptorsTo.type() == CV_32F &&
+								descriptorsFrom.type() == CV_32F &&
+								descriptorsFrom.rows == (int)kptsFrom.size() &&
+								imageSize.width > 0 && imageSize.height > 0)
+							{
+								UDEBUG("SuperGlue matching");
+								matches = _superGlueMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, imageSize);
+							}
+							else
+							{
+								if(_nnType == 6 && _superGlueMatcher)
+								{
+									UDEBUG("Invalid inputs for SuperGlue (desc type=%d, only float descriptors supported), doing bruteforce matching instead.", descriptorsFrom.type());
+								}
+#else
+							{
+#endif
+								UDEBUG("BruteForce matching with crosscheck");
+								cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, true);
+								matcher.match(descriptorsTo, descriptorsFrom, matches);
+							}
 							for(size_t i=0; i<matches.size(); ++i)
 							{
 								toWordIdsV[matches[i].queryIdx] = fromWordIdsV[matches[i].trainIdx];
@@ -1104,6 +1174,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					}
 					else
 					{
+						UDEBUG("VWDictionary knn matching");
 						VWDictionary dictionary(_featureParameters);
 						if(orignalWordsFromIds.empty())
 						{
@@ -1241,6 +1312,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					// we only need the camera transform, send guess words3 for scale estimation
 					Transform cameraTransform;
 					double variance = 1.0f;
+					std::vector<int> matchesV;
 					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
 							uMultimapToMapUnique(signatureA->getWords()),
 							uMultimapToMapUnique(signatureB->getWords()),
@@ -1250,12 +1322,14 @@ Transform RegistrationVis::computeTransformationImpl(
 							_PnPReprojError,
 							_PnPFlags, // cv::SOLVEPNP_ITERATIVE
 							_PnPRefineIterations,
-							1.0f,
+							_PnPReprojError,
 							0.99f,
 							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
-							&variance);
+							&variance,
+							&matchesV);
 					covariances[dir] *= variance;
 					inliers[dir] = uKeys(inliers3D);
+					matches[dir] = matchesV;

 					if(!cameraTransform.isNull())
 					{
@@ -1763,6 +1837,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.rejectedMsg = msg;
 	info.covariance = covariance;

+	UDEBUG("inliers=%d/%d", info.inliers, info.matches);
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;
 }
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -5435,7 +5435,7 @@ void Rtabmap::updateGoalIndex()
 			unsigned int nearestNodeIndex = 0;
 			float distance = -1.0f;
 			bool sameCurrentIndex = false;
-			UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0);
+			UASSERT(_pathGoalIndex < _path.size());
 			for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
 			{
 				std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[i].first);
--- a/corelib/src/RtabmapThread.cpp
+++ b/corelib/src/RtabmapThread.cpp
@@ -209,7 +209,6 @@ void RtabmapThread::mainLoop()
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
 		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->init(parameters, str);
 		break;
@@ -217,7 +216,6 @@ void RtabmapThread::mainLoop()
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
 		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
-		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->parseParameters(parameters);
 		break;
--- a/corelib/src/VWDictionary.cpp
+++ b/corelib/src/VWDictionary.cpp
@@ -279,47 +279,50 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)

 void VWDictionary::setNNStrategy(NNStrategy strategy)
 {
-	if(strategy!=kNNUndef)
-	{
 #if CV_MAJOR_VERSION < 3
 #ifdef HAVE_OPENCV_GPU
-		if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #else
-		if(strategy == kNNBruteForceGPU)
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV is not built with GPU/cuda module! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU)
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV is not built with GPU/cuda module! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #endif
 #else
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
-		if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #else
-		if(strategy == kNNBruteForceGPU)
-		{
-			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
-			strategy = kNNBruteForce;
-		}
+	if(strategy == kNNBruteForceGPU)
+	{
+		UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
 #endif
 #endif

-		bool update = _strategy != strategy;
-		_strategy = strategy;
-		if(update)
-		{
-			_dataTree = cv::Mat();
-			_notIndexedWords = uKeysSet(_visualWords);
-			_removedIndexedWords.clear();
-			this->update();
-		}
+	if(strategy>=kNNUndef)
+	{
+		UERROR("Nearest neighobr strategy \"%d\" chosen but this strategy cannot be used with a dictionary! Doing \"kNNBruteForce\" instead.");
+		strategy = kNNBruteForce;
+	}
+
+	bool update = _strategy != strategy;
+	_strategy = strategy;
+	if(update)
+	{
+		_dataTree = cv::Mat();
+		_notIndexedWords = uKeysSet(_visualWords);
+		_removedIndexedWords.clear();
+		this->update();
 	}
 }

--- a/corelib/src/superglue_pytorch/SuperGlue.cpp
+++ b/corelib/src/superglue_pytorch/SuperGlue.cpp
@@ -0,0 +1,249 @@
+/**
+ * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ */
+
+#include <superglue_pytorch/SuperGlue.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
+{
+
+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;
+
+SuperGlue::SuperGlue(const std::string & path, float matchThreshold, int iterations, bool cuda) :
+		pModule_(0),
+		pFunc_(0),
+		matchThreshold_(matchThreshold),
+		iterations_(iterations),
+		cuda_(cuda)
+{
+	path_ = uReplaceChar(path, '~', UDirectory::homeDir());
+	UINFO("path = %s", path_.c_str());
+
+	if(!UFile::exists(path_))
+	{
+		UERROR("Cannot initialize SuperGlue, the path is not valid: \"%s\"", path_.c_str());
+		return;
+	}
+
+	if(!g_python.initialized())
+	{
+		g_python.init();
+	}
+
+	std::string superGluePythonDir = UDirectory::getDir(path_);
+	if(!superGluePythonDir.empty())
+	{
+		PyRun_SimpleString("import sys");
+		PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", superGluePythonDir.c_str()).c_str());
+	}
+
+	_import_array();
+
+	std::string scriptName = uSplit(UFile::getName(path_), '.').front();
+	PyObject * pName = PyUnicode_FromString(scriptName.c_str());
+	pModule_ = PyImport_Import(pName);
+	Py_DECREF(pName);
+
+	if(!pModule_)
+	{
+		UERROR("Module %s could not be imported!", scriptName.c_str());
+	}
+}
+
+SuperGlue::~SuperGlue()
+{
+	if(pFunc_)
+	{
+		Py_DECREF(pFunc_);
+	}
+	if(pModule_)
+	{
+		Py_DECREF(pModule_);
+	}
+}
+
+std::vector<cv::DMatch> SuperGlue::match(
+		  const cv::Mat & descriptorsQuery,
+		  const cv::Mat & descriptorsTrain,
+		  const std::vector<cv::KeyPoint> & keypointsQuery,
+		  const std::vector<cv::KeyPoint> & keypointsTrain,
+		  const cv::Size & imageSize)
+{
+	UTimer timer;
+	std::vector<cv::DMatch> matches;
+
+	if(!pModule_)
+	{
+		UERROR("SuperGlue python module not loaded!");
+		return matches;
+	}
+
+	if(descriptorsQuery.cols == 256 && // Only SuperPoint is supported!
+	   descriptorsQuery.cols == descriptorsTrain.cols &&
+	   descriptorsQuery.type() == CV_32F &&
+	   descriptorsTrain.type() == CV_32F &&
+	   descriptorsQuery.rows == (int)keypointsQuery.size() &&
+	   descriptorsTrain.rows == (int)keypointsTrain.size() &&
+	   imageSize.width>0 && imageSize.height>0)
+	{
+
+		UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0);
+
+		if(!pFunc_)
+		{
+			PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
+			if(pFunc)
+			{
+				if(PyCallable_Check(pFunc))
+				{
+					PyObject_CallFunction(pFunc, "ifii", descriptorsQuery.cols, matchThreshold_, iterations_, cuda_?1:0);
+
+					pFunc_ = PyObject_GetAttrString(pModule_, "match");
+					if(pFunc_ && PyCallable_Check(pFunc_))
+					{
+						// we are ready!
+					}
+					else
+					{
+						UERROR("Cannot find method \"match(...)\" in %s", path_.c_str());
+						if(pFunc_)
+						{
+							Py_DECREF(pFunc_);
+							pFunc_ = 0;
+						}
+						return matches;
+					}
+				}
+				else
+				{
+					UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
+					return matches;
+				}
+				Py_DECREF(pFunc);
+			}
+			else
+			{
+				UERROR("Cannot find method \"init(...)\"");
+				return matches;
+			}
+			UDEBUG("init time = %fs", timer.ticks());
+		}
+
+		if(pFunc_)
+		{
+			std::vector<float> descriptorsQueryV(descriptorsQuery.rows * descriptorsQuery.cols);
+			memcpy(descriptorsQueryV.data(), descriptorsQuery.data, descriptorsQuery.total()*sizeof(float));
+			npy_intp dimsFrom[2] = {descriptorsQuery.rows, descriptorsQuery.cols};
+			PyObject* pDescriptorsQuery = PyArray_SimpleNewFromData(2, dimsFrom, NPY_FLOAT, (void*)descriptorsQueryV.data());
+			UASSERT(pDescriptorsQuery);
+
+			npy_intp dimsTo[2] = {descriptorsTrain.rows, descriptorsTrain.cols};
+			std::vector<float> descriptorsTrainV(descriptorsTrain.rows * descriptorsTrain.cols);
+			memcpy(descriptorsTrainV.data(), descriptorsTrain.data, descriptorsTrain.total()*sizeof(float));
+			PyObject* pDescriptorsTrain = PyArray_SimpleNewFromData(2, dimsTo, NPY_FLOAT, (void*)descriptorsTrainV.data());
+			UASSERT(pDescriptorsTrain);
+
+			std::vector<float> keypointsQueryV(keypointsQuery.size()*2);
+			std::vector<float> scoresQuery(keypointsQuery.size());
+			for(size_t i=0; i<keypointsQuery.size(); ++i)
+			{
+				keypointsQueryV[i*2] = keypointsQuery[i].pt.x;
+				keypointsQueryV[i*2+1] = keypointsQuery[i].pt.y;
+				scoresQuery[i] = keypointsQuery[i].response;
+			}
+
+			std::vector<float> keypointsTrainV(keypointsTrain.size()*2);
+			std::vector<float> scoresTrain(keypointsTrain.size());
+			for(size_t i=0; i<keypointsTrain.size(); ++i)
+			{
+				keypointsTrainV[i*2] = keypointsTrain[i].pt.x;
+				keypointsTrainV[i*2+1] = keypointsTrain[i].pt.y;
+				scoresTrain[i] = keypointsTrain[i].response;
+			}
+
+			npy_intp dimsKpQuery[2] = {(int)keypointsQuery.size(), 2};
+			PyObject* pKeypointsQuery = PyArray_SimpleNewFromData(2, dimsKpQuery, NPY_FLOAT, (void*)keypointsQueryV.data());
+			UASSERT(pKeypointsQuery);
+
+			npy_intp dimsKpTrain[2] = {(int)keypointsTrain.size(), 2};
+			PyObject* pkeypointsTrain = PyArray_SimpleNewFromData(2, dimsKpTrain, NPY_FLOAT, (void*)keypointsTrainV.data());
+			UASSERT(pkeypointsTrain);
+
+			npy_intp dimsScoresQuery[1] = {(int)keypointsQuery.size()};
+			PyObject* pScoresQuery = PyArray_SimpleNewFromData(1, dimsScoresQuery, NPY_FLOAT, (void*)scoresQuery.data());
+			UASSERT(pScoresQuery);
+
+			npy_intp dimsScoresTrain[1] = {(int)keypointsTrain.size()};
+			PyObject* pScoresTrain = PyArray_SimpleNewFromData(1, dimsScoresTrain, NPY_FLOAT, (void*)scoresTrain.data());
+			UASSERT(pScoresTrain);
+
+			PyObject * pImageWidth = PyLong_FromLong(imageSize.width);
+			PyObject * pImageHeight = PyLong_FromLong(imageSize.height);
+
+			UDEBUG("Preparing data time = %fs", timer.ticks());
+
+			PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pKeypointsQuery, pkeypointsTrain, pScoresQuery, pScoresTrain, pDescriptorsQuery, pDescriptorsTrain, pImageWidth, pImageHeight, NULL);
+			UASSERT(pReturn);
+
+			UDEBUG("Python matching time = %fs", timer.ticks());
+
+			PyArrayObject *np_ret = reinterpret_cast<PyArrayObject*>(pReturn);
+
+			// Convert back to C++ array and print.
+			int len1 = PyArray_SHAPE(np_ret)[0];
+			int len2 = PyArray_SHAPE(np_ret)[1];
+			//int type = PyArray_TYPE(np_ret); // Should be long
+			long* c_out = reinterpret_cast<long*>(PyArray_DATA(np_ret));
+			for (int i = 0; i < len1*len2; i+=2)
+			{
+				matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
+			}
+
+			Py_DECREF(pReturn);
+			Py_DECREF(pDescriptorsQuery);
+			Py_DECREF(pDescriptorsTrain);
+			Py_DECREF(pKeypointsQuery);
+			Py_DECREF(pkeypointsTrain);
+			Py_DECREF(pScoresQuery);
+			Py_DECREF(pScoresTrain);
+			Py_DECREF(pImageWidth);
+			Py_DECREF(pImageHeight);
+
+			UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks());
+		}
+	}
+	else if(descriptorsQuery.cols != 256)
+	{
+		UERROR("Only descriptor size of 256 (SuperPoint) is "
+			   "supported with SuperGlue! Current descriptor size=%d.",
+				descriptorsQuery.cols);
+	}
+	else
+	{
+		UERROR("Invalid inputs! SuperGlue requires SuperPoint descriptors (dim=256).");
+	}
+	return matches;
+}
+
+}
--- a/corelib/src/superglue_pytorch/SuperGlue.h
+++ b/corelib/src/superglue_pytorch/SuperGlue.h
@@ -0,0 +1,46 @@
+/**
+ * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
+ */
+
+#ifndef SUPERGLUE_H
+#define SUPERGLUE_H
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/core/mat.hpp>
+#include <vector>
+
+#include <Python.h>
+
+namespace rtabmap
+{
+
+class SuperGlue
+{
+public:
+  SuperGlue(const std::string & supergluePythonPath, float matchThreshold = 0.2f, int iterations = 20, bool cuda = false);
+  virtual ~SuperGlue();
+
+  const std::string & path() const {return path_;}
+  float matchThreshold() const {return matchThreshold_;}
+  int iterations() const {return iterations_;}
+  bool cuda() const {return cuda_;}
+
+  std::vector<cv::DMatch> match(
+		  const cv::Mat & descriptorsQuery,
+		  const cv::Mat & descriptorsTrain,
+		  const std::vector<cv::KeyPoint> & keypointsQuery,
+		  const std::vector<cv::KeyPoint> & keypointsTrain,
+		  const cv::Size & imageSize);
+
+private:
+  PyObject * pModule_;
+  PyObject * pFunc_;
+  std::string path_;
+  float matchThreshold_;
+  int iterations_;
+  bool cuda_;
+};
+
+}
+
+#endif
--- a/corelib/src/superglue_pytorch/rtabmap_superglue.py
+++ b/corelib/src/superglue_pytorch/rtabmap_superglue.py
@@ -0,0 +1,85 @@
+#! /usr/bin/env python3
+#
+# Drop this file in the folder of SuperGlue git: https://github.com/magicleap/SuperGluePretrainedNetwork
+# To use with rtabmap:
+#   --Vis/CorNNType 6 --SuperGlue/Path "~/SuperGluePretrainedNetwork/rtabmap_superglue.py"
+#
+
+import random
+import numpy as np
+import torch
+
+#import sys
+#import os
+#print(os.sys.path)
+#print(sys.version)
+
+from models.matching import SuperGlue
+
+torch.set_grad_enabled(False)
+
+device = 'cpu'
+superglue = []
+
+def init(descriptorDim, matchThreshold, iterations, cuda):
+    print("Python init()")
+    # Load the SuperPoint and SuperGlue models.
+    global device
+    device = 'cuda' if torch.cuda.is_available() and cuda else 'cpu'
+    config = {
+        'superglue': {
+            'weights': 'indoor',
+            'sinkhorn_iterations': iterations,
+            'match_threshold': matchThreshold,
+            'descriptor_dim' : descriptorDim
+        }
+    }
+    global superglue
+    superglue = SuperGlue(config.get('superglue', {})).eval().to(device)
+
+
+def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight):
+    #print("Python match()")
+    global device
+    kptsFrom = np.asarray(kptsFrom)
+    kptsFrom = kptsFrom[None, :, :]
+    kptsTo = np.asarray(kptsTo)
+    kptsTo = kptsTo[None, :, :]
+    scoresFrom = np.asarray(scoresFrom)
+    scoresFrom = scoresFrom[None, :]
+    scoresTo = np.asarray(scoresTo)
+    scoresTo = scoresTo[None, :]
+    descriptorsFrom = np.transpose(np.asarray(descriptorsFrom))
+    descriptorsFrom = descriptorsFrom[None, :, :]
+    descriptorsTo = np.transpose(np.asarray(descriptorsTo))
+    descriptorsTo = descriptorsTo[None, :, :]
+      
+    data = {
+       'image0': torch.rand(1, 1, imageHeight, imageWidth).to(device),
+       'image1': torch.rand(1, 1, imageHeight, imageWidth).to(device),
+       'scores0': torch.from_numpy(scoresFrom).to(device),
+       'scores1': torch.from_numpy(scoresTo).to(device),
+       'keypoints0': torch.from_numpy(kptsFrom).to(device),
+       'keypoints1': torch.from_numpy(kptsTo).to(device),
+       'descriptors0': torch.from_numpy(descriptorsFrom).to(device),
+       'descriptors1': torch.from_numpy(descriptorsTo).to(device),
+    }
+    
+
+    global superglue
+    results = superglue(data)
+
+    matches0 = results['matches0'].to('cpu').numpy()
+  
+    matchesFrom = np.nonzero(matches0!=-1)[1]
+    matchesTo = matches0[np.nonzero(matches0!=-1)]
+       
+    matchesArray = np.stack((matchesFrom, matchesTo), axis=1);
+    
+    return matchesArray
+
+
+if __name__ == '__main__':
+    #test
+    init(256, 0.2, 20, True)
+    match([[1, 2], [1,3]], [[1, 3], [1,2]], [1, 3], [1,3], np.full((2, 256), 1),np.full((2, 256), 1), 640, 480)
--- a/corelib/src/superpoint_torch/SuperPoint.cc
+++ b/corelib/src/superpoint_torch/SuperPoint.cc
@@ -4,6 +4,9 @@

 #include <superpoint_torch/SuperPoint.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>


 namespace rtabmap
@@ -119,10 +122,17 @@ SPDetector::SPDetector(const std::string & modelPath, float threshold, bool nms,
 	UDEBUG("modelPath=%s thr=%f nms=%d cuda=%d", modelPath.c_str(), threshold, nms?1:0, cuda?1:0);
 	if(modelPath.empty())
 	{
+		UERROR("Model's path is empty!");
+		return;
+	}
+	std::string path = uReplaceChar(modelPath, '~', UDirectory::homeDir());
+	if(!UFile::exists(path))
+	{
+		UERROR("Model's path \"%s\" doesn't exist!", path.c_str());
 		return;
 	}
 	model_ = std::make_shared<SuperPoint>();
-	torch::load(model_, modelPath);
+	torch::load(model_, uReplaceChar(path, '~', UDirectory::homeDir()));

 	if(cuda && !torch::cuda::is_available())
 	{
@@ -137,8 +147,10 @@ SPDetector::~SPDetector()
 {
 }

-std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img)
+std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img, const cv::Mat & mask)
 {
+	UASSERT(img.type() == CV_8UC1);
+	UASSERT(mask.empty() || (mask.type() == CV_8UC1 && img.cols == mask.cols && img.rows == mask.rows));
 	detected_ = false;
 	if(model_)
 	{
@@ -158,8 +170,11 @@ std::vector<cv::KeyPoint> SPDetector::detect(const cv::Mat &img)

 		std::vector<cv::KeyPoint> keypoints_no_nms;
 		for (int i = 0; i < kpts.size(0); i++) {
-			float response = prob_[kpts[i][0]][kpts[i][1]].item<float>();
-			keypoints_no_nms.push_back(cv::KeyPoint(kpts[i][1].item<float>(), kpts[i][0].item<float>(), 8, -1, response));
+			if(mask.empty() || mask.at<unsigned char>(kpts[i][0].item<int>(), kpts[i][1].item<int>()) != 0)
+			{
+				float response = prob_[kpts[i][0]][kpts[i][1]].item<float>();
+				keypoints_no_nms.push_back(cv::KeyPoint(kpts[i][1].item<float>(), kpts[i][0].item<float>(), 8, -1, response));
+			}
 		}

 		detected_ = true;
--- a/corelib/src/superpoint_torch/SuperPoint.h
+++ b/corelib/src/superpoint_torch/SuperPoint.h
@@ -50,7 +50,7 @@ class SPDetector {
 public:
    SPDetector(const std::string & modelPath, float threshold = 0.2f, bool nms = true, int minDistance = 4, bool cuda = false);
    virtual ~SPDetector();
-    std::vector<cv::KeyPoint> detect(const cv::Mat &img);
+    std::vector<cv::KeyPoint> detect(const cv::Mat &img, const cv::Mat & mask = cv::Mat());
    cv::Mat compute(const std::vector<cv::KeyPoint> &keypoints);

    void setThreshold(float threshold) {threshold_ = threshold;}
--- a/corelib/src/util3d_features.cpp
+++ b/corelib/src/util3d_features.cpp
@@ -215,7 +215,8 @@ std::map<int, cv::Point3f> generateWords3DMono(
 		float ransacParam1,
 		float ransacParam2,
 		const std::map<int, cv::Point3f> & refGuess3D,
-		double * varianceOut)
+		double * varianceOut,
+		std::vector<int> * matchesOut)
 {
 	UASSERT(cameraModel.isValidForProjection());
 	std::map<int, cv::Point3f> words3D;
@@ -238,6 +239,11 @@ std::map<int, cv::Point3f> generateWords3DMono(
 			std::vector<int> indexes(status.size());
 			for(unsigned int i=0; i<status.size(); ++i)
 			{
+				if(matchesOut)
+				{
+					matchesOut->push_back(iter->first);
+				}
+
 				if(status[i])
 				{
 					refCorners[oi] = iter->second.first.pt;
--- a/corelib/src/util3d_motion_estimation.cpp
+++ b/corelib/src/util3d_motion_estimation.cpp
@@ -94,8 +94,9 @@ Transform estimateMotion3DTo2D(
 	imagePoints.resize(oi);
 	matches.resize(oi);

-	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s",
-			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), guess.prettyPrint().c_str());
+	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d",
+			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
+			guess.prettyPrint().c_str(), reprojError, iterations);

 	if((int)matches.size() >= minInliers)
 	{