Added GMS matching approach (Vis/CorNNType=7). Added GMS/ parameters. Added SuperGlue/Indoor parameter. ImageView: updating feature and line colors directly when changing color (option menu), fixed items already added to scene warning when switching to GraphicsView.

2026-09-02 09:30:25 +08:00 · 2020-05-11 13:23:26 -04:00
parent cc9fd439fe
commit d2673ed2fa
12 changed files with 600 additions and 309 deletions
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -330,11 +330,6 @@ class RTABMAP_EXP Parameters
    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");
    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
@@ -609,7 +604,7 @@ 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, BruteForceCrossCheck=5, SuperGlue=6. 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, GMS=7. 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, 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()));
@@ -623,6 +618,17 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,      "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
 #endif

+    // Features matching approaches
+    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,   "");
+	RTABMAP_PARAM(SuperGlue, Indoor,         bool, true,   "Use indoor model, otherwise outdoor model is used.");
+
+	RTABMAP_PARAM(GMS, WithRotation,         bool, false,   "Take rotation transformation into account.");
+	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
+	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
+
    // ICP registration parameters
    RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
    RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
--- a/corelib/include/rtabmap/core/RegistrationVis.h
+++ b/corelib/include/rtabmap/core/RegistrationVis.h
@@ -89,6 +89,9 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _nnType;
+	bool _gmsWithRotation;
+	bool _gmsWithScale;
+	double _gmsThresholdFactor;
 	int _guessWinSize;
 	bool _guessMatchToProjection;
 	int _bundleAdjustment;
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -184,16 +184,9 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			group.compare("Optimizer") == 0 ||
 			group.compare("g2o") == 0 ||
 			group.compare("GTSAM") == 0 ||
-			(vis && (group.compare("Vis") == 0 || group.compare("SuperGlue") == 0)) ||
+			(vis && (group.compare("Vis") == 0 || group.compare("SuperGlue") == 0 || group.compare("GMS") == 0)) ||
 			iter->first.compare(kRtabmapPublishRAMUsage())==0)
 		{
-			if(stereo)
-			{
-				if(iter->first.compare(Parameters::kVisEstimationType()) == 0)
-				{
-					iter->second = "1"; // 3D->2D (PNP)
-				}
-			}
 			odomParameters.insert(*iter);
 		}
 	}
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -44,6 +44,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <opencv2/core/core_c.h>

+#if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
+#include <opencv2/xfeatures2d.hpp> // For GMS matcher
+#endif
+
 #include <rtflann/flann.hpp>


@@ -72,6 +76,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
 		_nndr(Parameters::defaultVisCorNNDR()),
 		_nnType(Parameters::defaultVisCorNNType()),
+		_gmsWithRotation(Parameters::defaultGMSWithRotation()),
+		_gmsWithScale(Parameters::defaultGMSWithScale()),
+		_gmsThresholdFactor(Parameters::defaultGMSThresholdFactor()),
 		_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
@@ -124,6 +131,9 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
 	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
 	Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
+	Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
+	Parameters::parse(parameters, Parameters::kGMSWithScale(), _gmsWithScale);
+	Parameters::parse(parameters, Parameters::kGMSThresholdFactor(), _gmsThresholdFactor);
 	Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
@@ -144,7 +154,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		// 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.",
+		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
@@ -152,10 +162,12 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 		float matchThr = _superGlueMatcher?_superGlueMatcher->matchThreshold():Parameters::defaultSuperGlueMatchThreshold();
 		std::string path = _superGlueMatcher?_superGlueMatcher->path():Parameters::defaultSuperGluePath();
 		bool cuda = _superGlueMatcher?_superGlueMatcher->cuda():Parameters::defaultSuperGlueCuda();
+		bool indoor = _superGlueMatcher?_superGlueMatcher->indoor():Parameters::defaultSuperGlueIndoor();
 		Parameters::parse(parameters, Parameters::kSuperGlueIterations(), iterations);
 		Parameters::parse(parameters, Parameters::kSuperGlueMatchThreshold(), matchThr);
 		Parameters::parse(parameters, Parameters::kSuperGluePath(), path);
 		Parameters::parse(parameters, Parameters::kSuperGlueCuda(), cuda);
+		Parameters::parse(parameters, Parameters::kSuperGlueIndoor(), indoor);
 		if(path.empty())
 		{
 			UERROR("%s parameter should be set to use SuperGlue matching (%s=6), using default %d.",
@@ -167,10 +179,18 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 		else
 		{
 			delete _superGlueMatcher;
-			_superGlueMatcher = new SuperGlue(path, matchThr, iterations, cuda);
+			_superGlueMatcher = new SuperGlue(path, matchThr, iterations, cuda, indoor);
 		}
 #endif
 	}
+#if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1))
+	else if(_nnType == 7)
+	}
+		UWARN("%s is set to 7 but RTAB-Map is not built with OpenCV's xfeatures2d support (OpenCV >= 3.4.1 also required), using default %d.",
+				Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
+		_nnType = Parameters::defaultVisCorNNType();
+	}
+#endif

 	// override feature parameters
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
@@ -1115,9 +1135,9 @@ Transform RegistrationVis::computeTransformationImpl(
 					std::list<int> fromWordIds;
 					std::list<int> toWordIds;
 #ifdef RTABMAP_SUPERGLUE_PYTORCH
-					if(_nnType == 5 || (_nnType == 6 && _superGlueMatcher))
+					if(_nnType == 5 || (_nnType == 6 && _superGlueMatcher) || _nnType==7)
 #else
-					if(_nnType == 5) // bruteforce cross check
+					if(_nnType == 5 || _nnType == 7) // bruteforce cross check or GMS
 #endif
 					{
 						std::vector<int> fromWordIdsV(descriptorsFrom.rows);
@@ -1156,9 +1176,42 @@ Transform RegistrationVis::computeTransformationImpl(
 #else
 							{
 #endif
-								UDEBUG("BruteForce matching with crosscheck");
-								cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, true);
+								bool doCrossCheck = true;
+#ifdef HAVE_OPENCV_XFEATURES2D
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1)
+								cv::Size imageSizeFrom;
+								if(_nnType == 7)
+								{
+									imageSizeFrom = imageFrom.size();
+									if(imageSizeFrom.height == 0 || imageSizeFrom.width == 0)
+									{
+										imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+									}
+									if(imageSize.height > 0 && imageSize.width > 0 &&
+									   imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
+									{
+										doCrossCheck = false;
+									}
+									else
+									{
+										UDEBUG("Invalid inputs for GMS matching, image size should be set for both inputs, doing bruteforce matching instead.");
+									}
+								}
+#endif
+#endif
+
+								UDEBUG("BruteForce matching%s", _nnType!=7?" with crosscheck":" with GMS");
+								cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, doCrossCheck);
 								matcher.match(descriptorsTo, descriptorsFrom, matches);
+
+#if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
+								if(!doCrossCheck)
+								{
+									std::vector<cv::DMatch> matchesGMS;
+									cv::xfeatures2d::matchGMS(imageSize, imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
+									matches = matchesGMS;
+								}
+#endif
 							}
 							for(size_t i=0; i<matches.size(); ++i)
 							{
--- a/corelib/src/superglue_pytorch/SuperGlue.cpp
+++ b/corelib/src/superglue_pytorch/SuperGlue.cpp
@@ -30,12 +30,13 @@ private:

 static PythonSingleTon g_python;

-SuperGlue::SuperGlue(const std::string & path, float matchThreshold, int iterations, bool cuda) :
+SuperGlue::SuperGlue(const std::string & path, float matchThreshold, int iterations, bool cuda, bool indoor) :
 		pModule_(0),
 		pFunc_(0),
 		matchThreshold_(matchThreshold),
 		iterations_(iterations),
-		cuda_(cuda)
+		cuda_(cuda),
+		indoor_(indoor)
 {
 	path_ = uReplaceChar(path, '~', UDirectory::homeDir());
 	UINFO("path = %s", path_.c_str());
@@ -117,7 +118,7 @@ std::vector<cv::DMatch> SuperGlue::match(
 			{
 				if(PyCallable_Check(pFunc))
 				{
-					PyObject_CallFunction(pFunc, "ifii", descriptorsQuery.cols, matchThreshold_, iterations_, cuda_?1:0);
+					PyObject_CallFunction(pFunc, "ifiii", descriptorsQuery.cols, matchThreshold_, iterations_, cuda_?1:0, indoor_?1:0);

 					pFunc_ = PyObject_GetAttrString(pModule_, "match");
 					if(pFunc_ && PyCallable_Check(pFunc_))
--- a/corelib/src/superglue_pytorch/SuperGlue.h
+++ b/corelib/src/superglue_pytorch/SuperGlue.h
@@ -17,13 +17,14 @@ namespace rtabmap
 class SuperGlue
 {
 public:
-  SuperGlue(const std::string & supergluePythonPath, float matchThreshold = 0.2f, int iterations = 20, bool cuda = false);
+  SuperGlue(const std::string & supergluePythonPath, float matchThreshold = 0.2f, int iterations = 20, bool cuda = true, bool indoor=true);
  virtual ~SuperGlue();

  const std::string & path() const {return path_;}
  float matchThreshold() const {return matchThreshold_;}
  int iterations() const {return iterations_;}
  bool cuda() const {return cuda_;}
+  bool indoor() const {return indoor_;}

  std::vector<cv::DMatch> match(
 		  const cv::Mat & descriptorsQuery,
@@ -39,6 +40,7 @@ private:
  float matchThreshold_;
  int iterations_;
  bool cuda_;
+  bool indoor_;
 };

 }
--- a/corelib/src/superglue_pytorch/rtabmap_superglue.py
+++ b/corelib/src/superglue_pytorch/rtabmap_superglue.py
@@ -21,14 +21,14 @@ torch.set_grad_enabled(False)
 device = 'cpu'
 superglue = []

-def init(descriptorDim, matchThreshold, iterations, cuda):
+def init(descriptorDim, matchThreshold, iterations, cuda, indoor):
    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',
+            'weights': 'indoor' if indoor else 'outdoor',
            'sinkhorn_iterations': iterations,
            'match_threshold': matchThreshold,
            'descriptor_dim' : descriptorDim
@@ -81,5 +81,5 @@ def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo

 if __name__ == '__main__':
    #test
-    init(256, 0.2, 20, True)
+    init(256, 0.2, 20, True, 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)