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.

CMakeLists.txt

@@ -163,7 +163,8 @@ ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
 ENDIF()
 option(WITH_ORB_OCTREE    "Include ORB Octree feature support"   ON)
-option(WITH_SP_TORCH      "Include SuperPoint Torch feature support"   ON)
+option(WITH_SUPERPOINT_TORCH  "Include SuperPoint Torch feature support"   ON)
+option(WITH_SUPERGLUE_PYTORCH "Include SuperGlue PyTorch matching support"  OFF)
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
 option(WITH_K4W2          "Include Kinect for Windows v2 support" ON)
@@ -321,12 +322,19 @@ IF(WITH_QT)
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
-IF(WITH_SP_TORCH)
+IF(WITH_SUPERPOINT_TORCH)
     FIND_PACKAGE(Torch QUIET)
     IF(TORCH_FOUND)
         MESSAGE(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
     ENDIF(TORCH_FOUND)
-ENDIF(WITH_SP_TORCH)
+ENDIF(WITH_SUPERPOINT_TORCH)
+
+IF(WITH_SUPERGLUE_PYTORCH)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    IF(Python3_FOUND)
+        MESSAGE(STATUS "Found Python3")
+    ENDIF(Python3_FOUND)
+ENDIF(WITH_SUPERGLUE_PYTORCH)
 
 IF(WITH_FREENECT)
     FIND_PACKAGE(Freenect QUIET)
@@ -592,7 +600,7 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
-IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1")
+IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND)
    #LOAM and PCL>=1.10 require c++14
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -826,7 +834,10 @@ IF(NOT WITH_ORB_OCTREE)
    SET(ORB_OCTREE "//")
 ENDIF()
 IF(NOT TORCH_FOUND)
-   SET(SP_TORCH "//")
+   SET(SUPERPOINT_TORCH "//")
+ENDIF()
+IF(NOT Python3_FOUND)
+   SET(SUPERGLUE_PYTORCH "//")
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
@@ -1045,10 +1056,18 @@ IF(OpenCV_FOUND)
       MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
     ENDIF()
   ELSE()
-    IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+    IF(OpenCV_VERSION VERSION_GREATER "4.2.0")
+      IF(OPENCV_XFEATURES2D_FOUND)
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      ELSE()
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      ENDIF()
     ELSE()
-      MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      IF(OPENCV_XFEATURES2D_FOUND)
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      ELSE()
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      ENDIF()
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
@@ -1079,11 +1098,19 @@ MESSAGE(STATUS "  With ORB OcTree           = NO (WITH_ORB_OCTREE=OFF)")
 ENDIF()
 
 IF(TORCH_FOUND)
-MESSAGE(STATUS "  With SupertPoint Torch    = YES (License: GPLv3) libtorch=${Torch_VERSION}")
-ELSEIF(NOT WITH_SP_TORCH)
-MESSAGE(STATUS "  With SupertPoint Torch    = NO (WITH_SP_TORCH=OFF)")
+MESSAGE(STATUS "  With SupertPoint          = YES (License: GPLv3) libtorch=${Torch_VERSION}")
+ELSEIF(NOT WITH_SUPERPOINT_TORCH)
+MESSAGE(STATUS "  With SupertPoint          = NO (WITH_SUPERPOINT_TORCH=OFF)")
 ELSE()
-MESSAGE(STATUS "  With SupertPoint Torch    = NO (libtorch not found)")
+MESSAGE(STATUS "  With SupertPoint          = NO (libtorch not found)")
+ENDIF()
+
+IF(Python3_FOUND)
+MESSAGE(STATUS "  With SupertGlue           = YES (License: GPLv3)")
+ELSEIF(NOT WITH_SUPERPOINT_TORCH)
+MESSAGE(STATUS "  With SupertGlue           = NO (WITH_SUPERGLUE_PYTORCH=OFF)")
+ELSE()
+MESSAGE(STATUS "  With SupertGlue           = NO (python3 not found)")
 ENDIF()
 
 IF(WITH_MADGWICK)

Version.h.in

@@ -73,7 +73,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @VINS@#define RTABMAP_VINS
 @ORB_SLAM2@#define RTABMAP_ORB_SLAM2
 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE
-@SP_TORCH@#define RTABMAP_SP_TORCH
+@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
+@SUPERGLUE_PYTORCH@#define RTABMAP_SUPERGLUE_PYTORCH
 @MADGWICK@#define RTABMAP_MADGWICK
 
 

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));
 

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

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()));

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
 };
 
 }

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() {}
 

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());

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,

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")

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)
 {
 
 }

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

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;

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;
 }

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);

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;

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();
 	}
 }
 

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;
+}
+
+}

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

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)

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;

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;}

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;

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)
 	{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -336,7 +336,8 @@ private Q_SLOTS:
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSConfigPath();
 	void changeIcpPMConfigPath();
-	void changeSPTorchModelPath();
+	void changeSuperPointModelPath();
+	void changeSuperGluePath();
 	void readSettingsEnd();
 	void setupTreeView();
 	void updateBasicParameter();

guilib/src/AboutDialog.cpp

@@ -66,13 +66,20 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_orboctree->setText("No");
 	_ui->label_orboctree_license->setEnabled(false);
 #endif
-#ifdef RTABMAP_SP_TORCH
+#ifdef RTABMAP_SUPERPOINT_TORCH
 	_ui->label_sptorch->setText("Yes");
 	_ui->label_sptorch_license->setEnabled(true);
 #else
 	_ui->label_sptorch->setText("No");
 	_ui->label_sptorch_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_SUPERGLUE_PYTORCH
+	_ui->label_sgpytorch->setText("Yes");
+	_ui->label_sgpytorch_license->setEnabled(true);
+#else
+	_ui->label_sptorch->setText("No");
+	_ui->label_sptorch_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_FASTCV
 	_ui->label_fastcv->setText("Yes");
 	_ui->label_fastcv_license->setEnabled(true);

guilib/src/DatabaseViewer.cpp

@@ -205,13 +205,14 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
 	uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
 	uInsert(parameters, Parameters::getDefaultParameters("KAZE"));
-	uInsert(parameters, Parameters::getDefaultParameters("SPTorch"));
+	uInsert(parameters, Parameters::getDefaultParameters("SuperPoint"));
 	uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
 	uInsert(parameters, Parameters::getDefaultParameters("g2o"));
 	uInsert(parameters, Parameters::getDefaultParameters("GTSAM"));
 	uInsert(parameters, Parameters::getDefaultParameters("Reg"));
 	uInsert(parameters, Parameters::getDefaultParameters("Vis"));
 	uInsert(parameters, Parameters::getDefaultParameters("Icp"));
+	uInsert(parameters, Parameters::getDefaultParameters("SuperGlue"));
 	uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
 	uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
 	uInsert(parameters, Parameters::getDefaultParameters("Grid"));
@@ -5117,10 +5118,6 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 			{
 				if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
 				{
-					// PINK features
-					ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
-					ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
-
 					// Add lines
 					// Draw lines between corresponding features...
 					float scaleAX = ui_->graphicsView_A->viewScale();
@@ -5159,6 +5156,13 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 					{
 						cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
 						cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
+						ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
+						ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
+					}
+					else
+					{
+						ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingLineColor());
+						ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingLineColor());
 					}
 
 					ui_->graphicsView_A->addLine(
@@ -7155,7 +7159,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 
 		if(!silent && ui_->dockWidget_constraints->isVisible())
 		{
-			if(fromS->id() > 0 && toS->id() > 0)
+			if(toS && fromS->id() > 0 && toS->id() > 0)
 			{
 				updateLoopClosuresSlider(fromS->id(), toS->id());
 				if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
@@ -7179,9 +7183,16 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			}
 		}
 	}
-
 	else if(!silent)
 	{
+		if(toS && fromS->id() > 0 && toS->id() > 0)
+		{
+			// just update matches in the views
+			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+			updateWordsMatching(info.inliersIDs);
+		}
+
 		QMessageBox::warning(this,
 				tr("Refine link"),
 				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));

guilib/src/ImageView.cpp

@@ -159,7 +159,7 @@ QIcon ImageView::createIcon(const QColor & color)
 ImageView::ImageView(QWidget * parent) :
 		QWidget(parent),
 		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
-		_alpha(50),
+		_alpha(100),
 		_featuresSize(0.0f),
 		_defaultBgColor(Qt::black),
 		_defaultFeatureColor(Qt::yellow),

guilib/src/PreferencesDialog.cpp

@@ -146,9 +146,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->checkBox_ORBGpu->setEnabled(false);
 		_ui->label_orbGpu->setEnabled(false);
 
-		// remove BruteForceGPU option
-		_ui->comboBox_dictionary_strategy->removeItem(4);
-		_ui->reextract_nn->removeItem(4);
+		// disable BruteForceGPU option
+		_ui->comboBox_dictionary_strategy->setItemData(4, 0, Qt::UserRole - 1);
+		_ui->reextract_nn->setItemData(4, 0, Qt::UserRole - 1);
 	}
 
 #ifndef RTABMAP_OCTOMAP
@@ -218,11 +218,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->vis_feature_detector->setItemData(10, 0, Qt::UserRole - 1);
 #endif
 
-#ifndef RTABMAP_SP_TORCH
+#ifndef RTABMAP_SUPERPOINT_TORCH
 		_ui->comboBox_detector_strategy->setItemData(11, 0, Qt::UserRole - 1);
 		_ui->vis_feature_detector->setItemData(11, 0, Qt::UserRole - 1);
 #endif
 
+#ifndef RTABMAP_SUPERGLUE_PYTORCH
+		_ui->reextract_nn->setItemData(6, 0, Qt::UserRole - 1);
+#endif
+
 #if CV_MAJOR_VERSION >= 3
 	_ui->groupBox_fast_opencv2->setEnabled(false);
 #else
@@ -948,12 +952,19 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_kaze_diffusivity->setObjectName(Parameters::kKAZEDiffusivity().c_str());
 
 	// SuperPoint Torch
-	_ui->lineEdit_sptorch_path->setObjectName(Parameters::kSPTorchModelPath().c_str());
-	connect(_ui->toolButton_sptorch_path, SIGNAL(clicked()), this, SLOT(changeSPTorchModelPath()));
-	_ui->doubleSpinBox_sptorch_threshold->setObjectName(Parameters::kSPTorchThreshold().c_str());
-	_ui->checkBox_sptorch_nms->setObjectName(Parameters::kSPTorchNMS().c_str());
-	_ui->spinBox_sptorch_minDistance->setObjectName(Parameters::kSPTorchMinDistance().c_str());
-	_ui->checkBox_sptorch_cuda->setObjectName(Parameters::kSPTorchCuda().c_str());
+	_ui->lineEdit_sptorch_path->setObjectName(Parameters::kSuperPointModelPath().c_str());
+	connect(_ui->toolButton_sptorch_path, SIGNAL(clicked()), this, SLOT(changeSuperPointModelPath()));
+	_ui->doubleSpinBox_sptorch_threshold->setObjectName(Parameters::kSuperPointThreshold().c_str());
+	_ui->checkBox_sptorch_nms->setObjectName(Parameters::kSuperPointNMS().c_str());
+	_ui->spinBox_sptorch_minDistance->setObjectName(Parameters::kSuperPointNMSRadius().c_str());
+	_ui->checkBox_sptorch_cuda->setObjectName(Parameters::kSuperPointCuda().c_str());
+
+	// SuperGlue PyTorch
+	_ui->lineEdit_sgpytorch_path->setObjectName(Parameters::kSuperGluePath().c_str());
+	connect(_ui->toolButton_sgpytorch_path, SIGNAL(clicked()), this, SLOT(changeSuperGluePath()));
+	_ui->sgpytorch_matchThreshold->setObjectName(Parameters::kSuperGlueMatchThreshold().c_str());
+	_ui->sgpytorch_iterations->setObjectName(Parameters::kSuperGlueIterations().c_str());
+	_ui->checkBox_sgpytorch_cuda->setObjectName(Parameters::kSuperGlueCuda().c_str());
 
 	// verifyHypotheses
 	_ui->groupBox_vh_epipolar2->setObjectName(Parameters::kVhEpEnabled().c_str());
@@ -1039,7 +1050,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
 	_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
 	_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
-	_ui->checkBox__visCorCrossCheck->setObjectName(Parameters::kVisCorCrossCheck().c_str());
 	_ui->spinBox_visCorGuessWinSize->setObjectName(Parameters::kVisCorGuessWinSize().c_str());
 	_ui->checkBox__visCorGuessMatchToProjection->setObjectName(Parameters::kVisCorGuessMatchToProjection().c_str());
 	_ui->vis_feature_detector->setObjectName(Parameters::kVisFeatureType().c_str());
@@ -4711,7 +4721,7 @@ void PreferencesDialog::changeIcpPMConfigPath()
 	}
 }
 
-void PreferencesDialog::changeSPTorchModelPath()
+void PreferencesDialog::changeSuperPointModelPath()
 {
 	QString path;
 	if(_ui->lineEdit_sptorch_path->text().isEmpty())
@@ -4728,6 +4738,23 @@ void PreferencesDialog::changeSPTorchModelPath()
 	}
 }
 
+void PreferencesDialog::changeSuperGluePath()
+{
+	QString path;
+	if(_ui->lineEdit_sgpytorch_path->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Select file"), this->getWorkingDirectory(), tr("SuperGlue wrapper (*.py)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_sgpytorch_path->text(), tr("SuperGlue wrapper (*.py)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_sgpytorch_path->setText(path);
+	}
+}
+
 void PreferencesDialog::updateSourceGrpVisibility()
 {
 	_ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0);

guilib/src/ui/aboutDialog.ui

@@ -164,7 +164,7 @@ p, li { white-space: pre-wrap; }
              <x>0</x>
              <y>0</y>
              <width>596</width>
-             <height>820</height>
+             <height>843</height>
             </rect>
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_2">
@@ -185,7 +185,7 @@ p, li { white-space: pre-wrap; }
             </property>
             <item>
              <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
-              <item row="15" column="0">
+              <item row="16" column="0">
                <widget class="QLabel" name="label_21">
                 <property name="text">
                  <string>With stereo Zed :</string>
@@ -195,7 +195,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="0">
+              <item row="29" column="0">
                <widget class="QLabel" name="label_25">
                 <property name="text">
                  <string>With FOVIS :</string>
@@ -205,7 +205,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="2">
+              <item row="29" column="2">
                <widget class="QLabel" name="label_fovis_license">
                 <property name="text">
                  <string>GPLv2</string>
@@ -238,7 +238,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="1">
+              <item row="13" column="1">
                <widget class="QLabel" name="label_realsense2">
                 <property name="text">
                  <string/>
@@ -251,7 +251,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="0">
+              <item row="33" column="0">
                <widget class="QLabel" name="label_35">
                 <property name="text">
                  <string>With OKVIS :</string>
@@ -261,7 +261,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="1">
+              <item row="31" column="1">
                <widget class="QLabel" name="label_dvo">
                 <property name="text">
                  <string/>
@@ -274,7 +274,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="1">
+              <item row="10" column="1">
                <widget class="QLabel" name="label_openni2">
                 <property name="text">
                  <string/>
@@ -297,7 +297,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="2">
+              <item row="13" column="2">
                <widget class="QLabel" name="label_realsense2_license">
                 <property name="text">
                  <string>Apache-2</string>
@@ -307,7 +307,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="1">
+              <item row="33" column="1">
                <widget class="QLabel" name="label_okvis">
                 <property name="text">
                  <string/>
@@ -320,7 +320,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="0">
+              <item row="35" column="0">
                <widget class="QLabel" name="label_34">
                 <property name="text">
                  <string>With MSCKF :</string>
@@ -330,7 +330,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="1">
+              <item row="25" column="1">
                <widget class="QLabel" name="label_octomap">
                 <property name="text">
                  <string/>
@@ -343,7 +343,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="1">
+              <item row="35" column="1">
                <widget class="QLabel" name="label_msckf">
                 <property name="text">
                  <string/>
@@ -356,7 +356,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="2">
+              <item row="10" column="2">
                <widget class="QLabel" name="label_openni2_license">
                 <property name="text">
                  <string>Apache v2</string>
@@ -376,7 +376,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="16" column="0">
+              <item row="17" column="0">
                <widget class="QLabel" name="label_38">
                 <property name="text">
                  <string>With K4W2 :</string>
@@ -386,7 +386,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="15" column="1">
+              <item row="16" column="1">
                <widget class="QLabel" name="label_zed">
                 <property name="text">
                  <string/>
@@ -399,7 +399,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="0">
+              <item row="26" column="0">
                <widget class="QLabel" name="label_24">
                 <property name="text">
                  <string>With CPU-TSDF :</string>
@@ -409,7 +409,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="13" column="0">
+              <item row="14" column="0">
                <widget class="QLabel" name="label_16">
                 <property name="text">
                  <string>With stereo dc1394 :</string>
@@ -419,7 +419,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="0">
+              <item row="30" column="0">
                <widget class="QLabel" name="label_26">
                 <property name="text">
                  <string>With Viso2 :</string>
@@ -429,7 +429,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="0">
+              <item row="32" column="0">
                <widget class="QLabel" name="label_28">
                 <property name="text">
                  <string>With ORB SLAM 2 :</string>
@@ -449,7 +449,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="1">
+              <item row="32" column="1">
                <widget class="QLabel" name="label_orbslam2">
                 <property name="text">
                  <string/>
@@ -462,7 +462,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="13" column="2">
+              <item row="14" column="2">
                <widget class="QLabel" name="label_dc1394_license">
                 <property name="text">
                  <string>LGPL</string>
@@ -472,7 +472,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="2">
+              <item row="30" column="2">
                <widget class="QLabel" name="label_viso2_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -482,7 +482,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="14" column="0">
+              <item row="15" column="0">
                <widget class="QLabel" name="label_17">
                 <property name="text">
                  <string>With stereo FlyCapture2 :</string>
@@ -492,7 +492,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="0">
+              <item row="25" column="0">
                <widget class="QLabel" name="label_20">
                 <property name="text">
                  <string>With Octomap :</string>
@@ -502,7 +502,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="18" column="0">
+              <item row="19" column="0">
                <widget class="QLabel" name="label_77">
                 <property name="text">
                  <string>With TORO :</string>
@@ -512,7 +512,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="1">
+              <item row="29" column="1">
                <widget class="QLabel" name="label_fovis">
                 <property name="text">
                  <string/>
@@ -525,7 +525,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="2">
+              <item row="25" column="2">
                <widget class="QLabel" name="label_octomap_license">
                 <property name="text">
                  <string>BSD</string>
@@ -535,7 +535,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="10" column="0">
+              <item row="11" column="0">
                <widget class="QLabel" name="label_15">
                 <property name="text">
                  <string>With Freenect2 :</string>
@@ -545,7 +545,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="14" column="1">
+              <item row="15" column="1">
                <widget class="QLabel" name="label_flycapture2">
                 <property name="text">
                  <string/>
@@ -568,7 +568,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="10" column="2">
+              <item row="11" column="2">
                <widget class="QLabel" name="label_freenect2_license">
                 <property name="text">
                  <string>Apache v2 and/or GPLv2</string>
@@ -578,7 +578,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="1">
+              <item row="26" column="1">
                <widget class="QLabel" name="label_cputsdf">
                 <property name="text">
                  <string/>
@@ -591,7 +591,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="11" column="0">
+              <item row="12" column="0">
                <widget class="QLabel" name="label_22">
                 <property name="text">
                  <string>With RealSense :</string>
@@ -601,7 +601,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="0">
+              <item row="22" column="0">
                <widget class="QLabel" name="label_18">
                 <property name="text">
                  <string>With cvsba :</string>
@@ -611,7 +611,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="0">
+              <item row="20" column="0">
                <widget class="QLabel" name="label_14">
                 <property name="text">
                  <string>With g2o :</string>
@@ -621,7 +621,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="1">
+              <item row="22" column="1">
                <widget class="QLabel" name="label_cvsba">
                 <property name="text">
                  <string/>
@@ -634,7 +634,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="2">
+              <item row="9" column="2">
                <widget class="QLabel" name="label_freenect_license">
                 <property name="text">
                  <string>Apache v2 and/or GPLv2</string>
@@ -644,7 +644,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="1">
+              <item row="21" column="1">
                <widget class="QLabel" name="label_gtsam">
                 <property name="text">
                  <string/>
@@ -670,7 +670,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="0">
+              <item row="21" column="0">
                <widget class="QLabel" name="label_19">
                 <property name="text">
                  <string>With GTSAM :</string>
@@ -680,7 +680,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="13" column="1">
+              <item row="14" column="1">
                <widget class="QLabel" name="label_dc1394">
                 <property name="text">
                  <string/>
@@ -703,7 +703,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="0">
+              <item row="9" column="0">
                <widget class="QLabel" name="label_12">
                 <property name="text">
                  <string>With Freenect :</string>
@@ -713,7 +713,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="1">
+              <item row="9" column="1">
                <widget class="QLabel" name="label_freenect">
                 <property name="text">
                  <string/>
@@ -736,7 +736,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="0">
+              <item row="24" column="0">
                <widget class="QLabel" name="label_29">
                 <property name="text">
                  <string>With libpointmatcher :</string>
@@ -746,7 +746,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="1">
+              <item row="20" column="1">
                <widget class="QLabel" name="label_g2o">
                 <property name="text">
                  <string/>
@@ -772,7 +772,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="10" column="1">
+              <item row="11" column="1">
                <widget class="QLabel" name="label_freenect2">
                 <property name="text">
                  <string/>
@@ -785,7 +785,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="0">
+              <item row="13" column="0">
                <widget class="QLabel" name="label_33">
                 <property name="text">
                  <string>With RealSense2 :</string>
@@ -795,7 +795,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="2">
+              <item row="35" column="2">
                <widget class="QLabel" name="label_msckf_license">
                 <property name="text">
                  <string>Penn Software License</string>
@@ -805,7 +805,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="11" column="1">
+              <item row="12" column="1">
                <widget class="QLabel" name="label_realsense">
                 <property name="text">
                  <string/>
@@ -818,7 +818,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="2">
+              <item row="31" column="2">
                <widget class="QLabel" name="label_dvo_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -828,7 +828,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="0">
+              <item row="34" column="0">
                <widget class="QLabel" name="label_36">
                 <property name="text">
                  <string>With loam_velodyne :</string>
@@ -861,7 +861,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="1">
+              <item row="27" column="1">
                <widget class="QLabel" name="label_openchisel">
                 <property name="text">
                  <string/>
@@ -874,7 +874,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="2">
+              <item row="21" column="2">
                <widget class="QLabel" name="label_gtsam_license">
                 <property name="text">
                  <string>BSD</string>
@@ -884,7 +884,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="1">
+              <item row="24" column="1">
                <widget class="QLabel" name="label_libpointmatcher">
                 <property name="text">
                  <string/>
@@ -897,7 +897,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="1">
+              <item row="34" column="1">
                <widget class="QLabel" name="label_loam">
                 <property name="text">
                  <string/>
@@ -910,7 +910,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="0">
+              <item row="10" column="0">
                <widget class="QLabel" name="label_13">
                 <property name="text">
                  <string>With OpenNI2 :</string>
@@ -920,7 +920,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="0">
+              <item row="31" column="0">
                <widget class="QLabel" name="label_27">
                 <property name="text">
                  <string>With DVO :</string>
@@ -930,7 +930,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="1">
+              <item row="30" column="1">
                <widget class="QLabel" name="label_viso2">
                 <property name="text">
                  <string/>
@@ -943,7 +943,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="2">
+              <item row="32" column="2">
                <widget class="QLabel" name="label_orbslam2_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -953,7 +953,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="0">
+              <item row="27" column="0">
                <widget class="QLabel" name="label_30">
                 <property name="text">
                  <string>With OpenChisel :</string>
@@ -976,7 +976,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="18" column="1">
+              <item row="19" column="1">
                <widget class="QLabel" name="label_toro">
                 <property name="text">
                  <string/>
@@ -989,7 +989,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="2">
+              <item row="20" column="2">
                <widget class="QLabel" name="label_g2o_license">
                 <property name="text">
                  <string>BSD</string>
@@ -999,7 +999,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="2">
+              <item row="33" column="2">
                <widget class="QLabel" name="label_okvis_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1009,7 +1009,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="2">
+              <item row="34" column="2">
                <widget class="QLabel" name="label_loam_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1039,7 +1039,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="2">
+              <item row="24" column="2">
                <widget class="QLabel" name="label_libpointmatcher_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1049,7 +1049,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="2">
+              <item row="26" column="2">
                <widget class="QLabel" name="label_cputsdf_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1069,7 +1069,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="2">
+              <item row="22" column="2">
                <widget class="QLabel" name="label_cvsba_license">
                 <property name="text">
                  <string>GPLv2</string>
@@ -1079,7 +1079,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="18" column="2">
+              <item row="19" column="2">
                <widget class="QLabel" name="label_toro_license">
                 <property name="text">
                  <string>Creative Commons [Attribution-NonCommercial-ShareAlike]</string>
@@ -1089,7 +1089,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="11" column="2">
+              <item row="12" column="2">
                <widget class="QLabel" name="label_realsense_license">
                 <property name="text">
                  <string>Apache-2</string>
@@ -1112,7 +1112,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="17" column="0">
+              <item row="18" column="0">
                <widget class="QLabel" name="label_68">
                 <property name="text">
                  <string>With K4A :</string>
@@ -1122,7 +1122,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="16" column="1">
+              <item row="17" column="1">
                <widget class="QLabel" name="label_k4w2">
                 <property name="text">
                  <string/>
@@ -1135,7 +1135,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="17" column="1">
+              <item row="18" column="1">
                <widget class="QLabel" name="label_k4a">
                 <property name="text">
                  <string/>
@@ -1148,7 +1148,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="17" column="2">
+              <item row="18" column="2">
                <widget class="QLabel" name="label_g2o_license_3">
                 <property name="text">
                  <string>MIT</string>
@@ -1158,7 +1158,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="7" column="0">
+              <item row="8" column="0">
                <widget class="QLabel" name="label_39">
                 <property name="text">
                  <string>With FastCV :</string>
@@ -1168,7 +1168,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="7" column="1">
+              <item row="8" column="1">
                <widget class="QLabel" name="label_fastcv">
                 <property name="text">
                  <string/>
@@ -1181,7 +1181,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="7" column="2">
+              <item row="8" column="2">
                <widget class="QLabel" name="label_fastcv_license">
                 <property name="text">
                  <string>Apache v2</string>
@@ -1191,7 +1191,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="0">
+              <item row="23" column="0">
                <widget class="QLabel" name="label_40">
                 <property name="text">
                  <string>With Ceres :</string>
@@ -1201,7 +1201,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="2">
+              <item row="23" column="2">
                <widget class="QLabel" name="label_ceres_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1211,7 +1211,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="1">
+              <item row="23" column="1">
                <widget class="QLabel" name="label_ceres">
                 <property name="text">
                  <string/>
@@ -1224,7 +1224,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="0">
+              <item row="28" column="0">
                <widget class="QLabel" name="label_41">
                 <property name="text">
                  <string>With AliceVision :</string>
@@ -1234,7 +1234,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="1">
+              <item row="28" column="1">
                <widget class="QLabel" name="label_aliceVision">
                 <property name="text">
                  <string/>
@@ -1247,7 +1247,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="2">
+              <item row="28" column="2">
                <widget class="QLabel" name="label_aliceVision_license">
                 <property name="text">
                  <string>MPL2</string>
@@ -1290,6 +1290,39 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="7" column="2">
+               <widget class="QLabel" name="label_sgpytorch_license">
+                <property name="text">
+                 <string>GPLv3</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="7" column="0">
+               <widget class="QLabel" name="label_43">
+                <property name="text">
+                 <string>With SuperGlue PyTorch :</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="7" column="1">
+               <widget class="QLabel" name="label_sgpytorch">
+                <property name="text">
+                 <string/>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
              </layout>
             </item>
            </layout>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-798</y>
             <width>680</width>
             <height>3497</height>
            </rect>
@@ -16830,7 +16830,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                 </layout>
                </widget>
                <widget class="QWidget" name="page_21">
-                <layout class="QVBoxLayout" name="verticalLayout_50">
+                <layout class="QVBoxLayout" name="verticalLayout_50" stretch="0,1">
                  <item>
                   <widget class="QGroupBox" name="groupBox_visualTransform2">
                    <property name="title">
@@ -17056,7 +17056,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
+                       <item row="2" column="0">
                         <widget class="QSpinBox" name="spinBox_visCorGuessWinSize">
                          <property name="suffix">
                           <string> pixels</string>
@@ -17094,7 +17094,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="0" column="0">
                         <widget class="QComboBox" name="reextract_nn">
                          <property name="currentIndex">
-                          <number>3</number>
+                          <number>0</number>
                          </property>
                          <property name="sizeAdjustPolicy">
                           <enum>QComboBox::AdjustToContents</enum>
@@ -17124,6 +17124,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            <string>Brute Force GPU</string>
                           </property>
                          </item>
+                         <item>
+                          <property name="text">
+                           <string>Brute Force Cross Check</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>SuperGlue PyTorch</string>
+                          </property>
+                         </item>
                         </widget>
                        </item>
                        <item row="1" column="1">
@@ -17141,7 +17151,7 @@ Lower the ratio -&gt; higher the precision.</string>
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="2" column="1">
                         <widget class="QLabel" name="label_303">
                          <property name="text">
                           <string>Matching window size around projected points when a guess transform is provided to find correspondences. 0 means that global matching will be done.</string>
@@ -17154,7 +17164,7 @@ Lower the ratio -&gt; higher the precision.</string>
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="4" column="1">
                         <spacer name="verticalSpacer_44">
                          <property name="orientation">
                           <enum>Qt::Vertical</enum>
@@ -17167,7 +17177,7 @@ Lower the ratio -&gt; higher the precision.</string>
                          </property>
                         </spacer>
                        </item>
-                       <item row="4" column="1">
+                       <item row="3" column="1">
                         <widget class="QLabel" name="label_448">
                          <property name="text">
                           <string>Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.</string>
@@ -17180,33 +17190,13 @@ Lower the ratio -&gt; higher the precision.</string>
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
+                       <item row="3" column="0">
                         <widget class="QCheckBox" name="checkBox__visCorGuessMatchToProjection">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_582">
-                         <property name="text">
-                          <string>Cross check matching. If enabled, brute force matching with cross check is done instead of one of the nearest neighbor strategy above based on NNDR ratio.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="0">
-                        <widget class="QCheckBox" name="checkBox__visCorCrossCheck">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -17528,7 +17518,7 @@ Lower the ratio -&gt; higher the precision.</string>
                       <property name="checked">
                        <bool>false</bool>
                       </property>
-                      <layout class="QVBoxLayout" name="verticalLayout_46" stretch="0,0,1">
+                      <layout class="QVBoxLayout" name="verticalLayout_46" stretch="0,0,1,0">
                        <item>
                         <widget class="QLabel" name="label_182">
                          <property name="text">
@@ -17903,8 +17893,154 @@ Lower the ratio -&gt; higher the precision.</string>
                             </property>
                             <property name="sizeHint" stdset="0">
                              <size>
-                              <width>20</width>
-                              <height>40</height>
+                              <width>0</width>
+                              <height>0</height>
+                             </size>
+                            </property>
+                           </spacer>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_32">
+                         <property name="title">
+                          <string>SuperGlue</string>
+                         </property>
+                         <layout class="QVBoxLayout" name="verticalLayout_149">
+                          <item>
+                           <widget class="QLabel" name="label_586">
+                            <property name="text">
+                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Python wrapper of &lt;a href=&quot;https://github.com/magicleap/SuperGluePretrainedNetwork&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;SuperGlue&lt;/span&gt;&lt;/a&gt; project. Download &lt;a href=&quot;https://github.com/introlab/rtabmap/blob/master/corelib/src/superglue_pytorch/rtabmap_superglue.py&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;rtabmap_superpoint.py&lt;/span&gt;&lt;/a&gt; and copy it at the root folder of SuperGlue git, then set its path below. &lt;span style=&quot; font-weight:600;&quot;&gt;Important&lt;/span&gt;: SuperGlue works only with SuperPoint descriptors!&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="openExternalLinks">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
+                          <item>
+                           <layout class="QGridLayout" name="gridLayout_116" columnstretch="0,0,1">
+                            <item row="0" column="0">
+                             <widget class="QToolButton" name="toolButton_sgpytorch_path">
+                              <property name="text">
+                               <string>...</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="0" column="1">
+                             <widget class="QLineEdit" name="lineEdit_sgpytorch_path"/>
+                            </item>
+                            <item row="2" column="2">
+                             <widget class="QLabel" name="label_585">
+                              <property name="text">
+                               <string>Match threshold.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="1">
+                             <widget class="QDoubleSpinBox" name="sgpytorch_matchThreshold">
+                              <property name="decimals">
+                               <number>3</number>
+                              </property>
+                              <property name="minimum">
+                               <double>0.001000000000000</double>
+                              </property>
+                              <property name="maximum">
+                               <double>1.000000000000000</double>
+                              </property>
+                              <property name="singleStep">
+                               <double>0.010000000000000</double>
+                              </property>
+                              <property name="value">
+                               <double>0.200000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="1" column="2">
+                             <widget class="QLabel" name="label_584">
+                              <property name="text">
+                               <string>Iterations.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="0" column="2">
+                             <widget class="QLabel" name="label_583">
+                              <property name="text">
+                               <string>[Required] Path to rtabmap_superpoint.py.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="1" column="1">
+                             <widget class="QSpinBox" name="sgpytorch_iterations">
+                              <property name="minimum">
+                               <number>0</number>
+                              </property>
+                              <property name="maximum">
+                               <number>999999</number>
+                              </property>
+                              <property name="singleStep">
+                               <number>1</number>
+                              </property>
+                              <property name="value">
+                               <number>20</number>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="2">
+                             <widget class="QLabel" name="label_587">
+                              <property name="text">
+                               <string>Cuda.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="1">
+                             <widget class="QCheckBox" name="checkBox_sgpytorch_cuda">
+                              <property name="text">
+                               <string/>
+                              </property>
+                             </widget>
+                            </item>
+                           </layout>
+                          </item>
+                          <item>
+                           <spacer name="verticalSpacer_86">
+                            <property name="orientation">
+                             <enum>Qt::Vertical</enum>
+                            </property>
+                            <property name="sizeHint" stdset="0">
+                             <size>
+                              <width>0</width>
+                              <height>0</height>
                              </size>
                             </property>
                            </spacer>
@@ -17922,8 +18058,8 @@ Lower the ratio -&gt; higher the precision.</string>
                       </property>
                       <property name="sizeHint" stdset="0">
                        <size>
-                        <width>20</width>
-                        <height>40</height>
+                        <width>0</width>
+                        <height>0</height>
                        </size>
                       </property>
                      </spacer>

tools/CMakeLists.txt

@@ -25,6 +25,7 @@ IF(TARGET rtabmap_gui)
 	ADD_SUBDIRECTORY( OdometryViewer )
 	ADD_SUBDIRECTORY( DataRecorder )
 	ADD_SUBDIRECTORY( Calibration )
+	ADD_SUBDIRECTORY( Matcher )
 ELSE()
    MESSAGE(STATUS "RTAB-Map GUI lib is not built, some tools won't be built...")
 ENDIF()

tools/EpipolarGeometry/main.cpp

@@ -175,8 +175,8 @@ int main(int argc, char** argv)
     cv::Mat image2;
 	if(argc == 3)
 	{
-		image1 = cv::imread(argv[1]);
-		image2 = cv::imread(argv[2]);
+		image1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
+		image2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
 	}
 	else
 	{

tools/Matcher/CMakeLists.txt

@@ -0,0 +1,30 @@
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+IF(QT4_FOUND)
+    INCLUDE(${QT_USE_FILE})
+ENDIF(QT4_FOUND)
+
+find_package (Python3 COMPONENTS Interpreter Development REQUIRED)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+	${QT_LIBRARIES} 
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(matcher main.cpp)
+TARGET_LINK_LIBRARIES(matcher Python3::Python rtabmap_core rtabmap_utilite rtabmap_gui ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( matcher 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-matcher)
+ 

tools/Matcher/main.cpp

@@ -0,0 +1,434 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/core/EpipolarGeometry.h>
+#include <rtabmap/core/Features2d.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/KeypointItem.h>
+#include <rtabmap/gui/CloudViewer.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <fstream>
+#include <string>
+#include <QApplication>
+#include <QDialog>
+#include <QHBoxLayout>
+#include <QMultiMap>
+#include <QString>
+#include <opencv2/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"   rtabmap-matcher [Options] from.png to.png\n"
+			"Examples:\n"
+			"   rtabmap-matcher --Vis/CorNNType 5 --Vis/PnPReprojError 3 from.png to.png\n"
+			"   rtabmap-matcher --Vis/CorNNDR 0.8 from.png to.png\n"
+			"   rtabmap-matcher --Vis/FeatureType 11 --SuperPoint/ModelPath \"superpoint.pt\" --Vis/CorNNType 6 --SuperGlue/Path \"~/SuperGluePretrainedNetwork/rtabmap_superglue.py\" from.png to.png\n"
+			"   rtabmap-matcher --calibration calib.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
+			"\n"
+			"Note: Use \"Vis/\" parameters for feature stuff.\n"
+			"Options:\n"
+			"   --calibration \"calibration.yaml\" Calibration file. If not set, a\n"
+			"                                        fake one is created from image's\n"
+			"                                        size (which may not be optimal).\n"
+			"                                        Required if from_depth option is set.\n"
+			"                                        Assuming same calibration for both images.\n"
+			"   --from_depth \"from_depth.png\"    Depth or right image file of the first image.\n"
+			"                                        If not set, 2D->2D estimation is done by \n"
+			"                                        default. For 3D->2D estimation, from_depth\n"
+			"                                        should be set.\n"
+			"   --to_depth \"to_depth.png\"        Depth or right image file of the second image.\n"
+			"                                        For 3D->3D estimation, from_depth and to_depth\n"
+			"                                        should be both set.\n"
+			"\n"
+			"%s\n",
+			Parameters::showUsage());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 3)
+	{
+		showUsage();
+	}
+
+	ULogger::setLevel(ULogger::kWarning);
+	ULogger::setType(ULogger::kTypeConsole);
+
+	std::string fromDepthPath;
+	std::string toDepthPath;
+	std::string calibrationPath;
+	for(int i=1; i<argc-2; ++i)
+	{
+		if(strcmp(argv[i], "--from_depth") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				fromDepthPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--to_depth") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				toDepthPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--calibration") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				calibrationPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+	}
+
+	printf("Options\n");
+	printf("  --calibration = \"%s\"\n", calibrationPath.c_str());
+	printf("  --from_depth  = \"%s\"\n", fromDepthPath.c_str());
+	printf("  --to_depth    = \"%s\"\n", toDepthPath.c_str());
+
+	ParametersMap parameters = Parameters::parseArguments(argc, argv);
+	parameters.insert(ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+
+	cv::Mat imageFrom = cv::imread(argv[argc-2], cv::IMREAD_COLOR);
+	cv::Mat imageTo = cv::imread(argv[argc-1], cv::IMREAD_COLOR);
+
+	if(!imageFrom.empty() && !imageTo.empty())
+	{
+		//////////////////
+		// Load data
+		//////////////////
+
+		cv::Mat fromDepth;
+		cv::Mat toDepth;
+		if(!calibrationPath.empty())
+		{
+			if(!fromDepthPath.empty())
+			{
+				fromDepth = cv::imread(fromDepthPath, cv::IMREAD_UNCHANGED);
+				if(fromDepth.type() == CV_8UC3)
+				{
+					cv::cvtColor(fromDepth, fromDepth, cv::COLOR_BGR2GRAY);
+				}
+				else if(fromDepth.empty())
+				{
+					printf("Failed loading from_depth image: \"%s\"!", fromDepthPath.c_str());
+				}
+			}
+			if(!toDepthPath.empty())
+			{
+				toDepth = cv::imread(toDepthPath, cv::IMREAD_UNCHANGED);
+				if(toDepth.type() == CV_8UC3)
+				{
+					cv::cvtColor(toDepth, toDepth, cv::COLOR_BGR2GRAY);
+				}
+				else if(toDepth.empty())
+				{
+					printf("Failed loading to_depth image: \"%s\"!", toDepthPath.c_str());
+				}
+			}
+			UASSERT(toDepth.empty() || (!fromDepth.empty() && fromDepth.type() == toDepth.type()));
+		}
+		else if(!fromDepthPath.empty() || !fromDepthPath.empty())
+		{
+			printf("A calibration file should be provided if depth images are used.\n");
+			showUsage();
+		}
+
+		CameraModel model;
+		StereoCameraModel stereoModel;
+		if(!fromDepth.empty())
+		{
+			if(fromDepth.type() != CV_8UC1)
+			{
+				if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+					exit(-1);
+				}
+			}
+			else // fromDepth.type() == CV_8UC1
+			{
+				if(!stereoModel.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+					exit(-1);
+				}
+			}
+		}
+		else if(!calibrationPath.empty())
+		{
+			if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+			{
+				printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+				exit(-1);
+			}
+		}
+		else
+		{
+			printf("Using fake calibration model (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
+					imageFrom.cols, imageFrom.rows, imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2);
+			model = CameraModel(imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2); // Fake model
+			model.setImageSize(imageFrom.size());
+		}
+
+		Signature dataFrom;
+		Signature dataTo;
+		if(model.isValidForProjection())
+		{
+			printf("Mono calibration model detected.\n");
+			dataFrom = SensorData(imageFrom, fromDepth, model, 1);
+			dataTo = SensorData(imageTo, toDepth, model, 2);
+		}
+		else //stereo
+		{
+			printf("Stereo calibration model detected.\n");
+			dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
+			dataTo = SensorData(imageTo, toDepth, stereoModel, 2);
+		}
+
+		//////////////////
+		// Registration
+		//////////////////
+
+		if(fromDepth.empty())
+		{
+			parameters.insert(ParametersPair(Parameters::kVisEstimationType(), "2")); // Set 2D->2D estimation for mono images
+			printf("Calibration not set, setting %s=2 by default (2D->2D estimation)\n", Parameters::kVisEstimationType().c_str());
+		}
+		RegistrationVis reg(parameters);
+		RegistrationInfo info;
+
+
+		// Do it one time before to make sure everything is loaded to get realistic timing.
+		reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
+
+		UTimer timer;
+		Transform t = reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
+		double matchingTime = timer.ticks();
+		printf("Time matching and motion estimation: %fs\n", matchingTime);
+
+		//////////////////
+		// Visualization
+		//////////////////
+
+		QApplication app(argc, argv);
+		QDialog dialog;
+		dialog.setWindowTitle(QString("Matches (%1/%2) %3 sec [%4=%5 (%6) %7=%8 (%9)%10 %11=%12 (%13)]")
+				.arg(info.inliers)
+				.arg(info.matches)
+				.arg(matchingTime)
+				.arg(Parameters::kVisFeatureType().c_str())
+				.arg(reg.getDetector()?reg.getDetector()->getType():-1)
+				.arg(reg.getDetector()?Feature2D::typeName(reg.getDetector()->getType()).c_str():"?")
+				.arg(Parameters::kVisCorNNType().c_str())
+				.arg(reg.getNNType())
+				.arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():reg.getNNType()==5?"BFCrossCheck":reg.getNNType()==6?"SuperGlue":"?")
+				.arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
+				.arg(Parameters::kVisEstimationType().c_str())
+				.arg(reg.getEstimationType())
+				.arg(reg.getEstimationType()==0?"3D->3D":reg.getEstimationType()==1?"3D->2D":reg.getEstimationType()==2?"2D->2D":"?"));
+
+		CloudViewer * viewer = 0;
+		if(!t.isNull() && !fromDepth.empty() && !toDepth.empty())
+		{
+			viewer = new CloudViewer(&dialog);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom = util3d::cloudRGBFromSensorData(dataFrom.sensorData());
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo = util3d::cloudRGBFromSensorData(dataTo.sensorData());
+			viewer->addCloud(uFormat("cloud_%d", dataFrom.id()), cloudFrom, Transform::getIdentity(), Qt::magenta);
+			viewer->addCloud(uFormat("cloud_%d", dataTo.id()), cloudTo, t, Qt::cyan);
+			viewer->addOrUpdateCoordinate(uFormat("frame_%d", dataTo.id()), t, 0.2);
+		}
+
+		QBoxLayout * mainLayout = new QHBoxLayout();
+		QBoxLayout * layout;
+		bool vertical=true;
+		if(imageFrom.cols > imageFrom.rows)
+		{
+			dialog.setMinimumWidth(640*(viewer?2:1));
+			dialog.setMinimumHeight(640*imageFrom.rows/imageFrom.cols*2);
+			layout = new QVBoxLayout();
+		}
+		else
+		{
+			dialog.setMinimumWidth((640*imageFrom.cols/imageFrom.rows*2)*(viewer?2:1));
+			dialog.setMinimumHeight(640);
+			layout = new QHBoxLayout();
+			vertical = false;
+		}
+
+		ImageView * viewA = new ImageView(&dialog);
+		ImageView * viewB = new ImageView(&dialog);
+
+		layout->addWidget(viewA, 1);
+		layout->addWidget(viewB, 1);
+
+		mainLayout->addLayout(layout, 1);
+		if(viewer)
+		{
+			mainLayout->addWidget(viewer, 1);
+		}
+
+		dialog.setLayout(mainLayout);
+
+		dialog.show();
+
+		viewA->setImage(uCvMat2QImage(imageFrom));
+		viewA->setAlpha(200);
+		if(!fromDepth.empty())
+		{
+			viewA->setImageDepth(uCvMat2QImage(fromDepth, false, uCvQtDepthRedToBlue));
+			viewA->setImageDepthShown(true);
+		}
+		viewB->setImage(uCvMat2QImage(imageTo));
+		viewB->setAlpha(200);
+		if(!toDepth.empty())
+		{
+			viewB->setImageDepth(uCvMat2QImage(toDepth, false, uCvQtDepthRedToBlue));
+			viewB->setImageDepthShown(true);
+		}
+		viewA->setFeatures(dataFrom.getWords());
+		viewB->setFeatures(dataTo.getWords());
+		std::set<int> inliersSet(info.inliersIDs.begin(), info.inliersIDs.end());
+
+		const QMultiMap<int, KeypointItem*> & wordsA = viewA->getFeatures();
+		const QMultiMap<int, KeypointItem*> & wordsB = viewB->getFeatures();
+		if(wordsA.size() && wordsB.size())
+		{
+			QList<int> ids = wordsA.uniqueKeys();
+			for(int i=0; i<ids.size(); ++i)
+			{
+				if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
+				{
+					// Add lines
+					// Draw lines between corresponding features...
+					float scaleAX = viewA->viewScale();
+					float scaleBX = viewB->viewScale();
+
+					float scaleDiff = viewA->viewScale() / viewB->viewScale();
+					float deltaAX = 0;
+					float deltaAY = 0;
+
+					if(vertical)
+					{
+						deltaAY = viewA->height()/scaleAX;
+					}
+					else
+					{
+						deltaAX = viewA->width()/scaleAX;
+					}
+
+					float deltaBX = 0;
+					float deltaBY = 0;
+
+					if(vertical)
+					{
+						deltaBY = viewB->height()/scaleBX;
+					}
+					else
+					{
+						deltaBX = viewA->width()/scaleBX;
+					}
+
+					const KeypointItem * kptA = wordsA.value(ids[i]);
+					const KeypointItem * kptB = wordsB.value(ids[i]);
+
+					QColor cA = viewA->getDefaultMatchingLineColor();
+					QColor cB = viewB->getDefaultMatchingLineColor();
+					if(inliersSet.find(ids[i])!=inliersSet.end())
+					{
+						cA = viewA->getDefaultMatchingFeatureColor();
+						cB = viewB->getDefaultMatchingFeatureColor();
+						viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingFeatureColor());
+						viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingFeatureColor());
+					}
+					else
+					{
+						viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingLineColor());
+						viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingLineColor());
+					}
+
+					viewA->addLine(
+							kptA->rect().x()+kptA->rect().width()/2,
+							kptA->rect().y()+kptA->rect().height()/2,
+							kptB->rect().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
+							kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
+							cA);
+
+					viewB->addLine(
+							kptA->rect().x()*scaleDiff+kptA->rect().width()*scaleDiff/2-deltaBX,
+							kptA->rect().y()*scaleDiff+kptA->rect().height()*scaleDiff/2-deltaBY,
+							kptB->rect().x()+kptB->rect().width()/2,
+							kptB->rect().y()+kptB->rect().height()/2,
+							cB);
+				}
+			}
+			viewA->update();
+			viewB->update();
+		}
+
+		printf("Transform: %s\n", t.prettyPrint().c_str());
+		printf("Features: from=%d to=%d\n", (int)dataFrom.getWords().size(), (int)dataTo.getWords().size());
+		printf("Matches: %d\n", info.matches);
+		printf("Inliers: %d (%s=%d)\n", info.inliers, Parameters::kVisMinInliers().c_str(), reg.getMinInliers());
+		app.exec();
+		delete viewer;
+	}
+
+
+	return 0;
+}
+