Added Vis/CorCrossCheck parameter (to use BFMatcher with crosscheck option instead of knn with NNDR for features matching). DBViewer: after refine or add constraint, lines indicating feature correspondences are now shown with different color if they are inliers. Re-enabled saving/loading settings of the ImageViews (feature color, line color, transparency...).

corelib/include/rtabmap/core/Parameters.h

@@ -605,7 +605,8 @@ class RTABMAP_EXP Parameters
     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, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. 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

@@ -78,6 +78,7 @@ private:
 	int _flowIterations;
 	float _flowEps;
 	int _flowMaxLevel;
+	bool _bfCrossCheck;
 	float _nndr;
 	int _guessWinSize;
 	bool _guessMatchToProjection;

corelib/src/RegistrationVis.cpp

@@ -66,6 +66,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
 		_nndr(Parameters::defaultVisCorNNDR()),
+		_bfCrossCheck(Parameters::defaultVisCorCrossCheck()),
 		_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
 		_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
 		_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
@@ -113,6 +114,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::kVisCorGuessWinSize(), _guessWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
 	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
@@ -219,6 +221,8 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
 	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("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",
@@ -757,7 +761,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(_guessMatchToProjection)
 						{
-							// match frame to projected
+							UDEBUG("match frame to projected");
 							// Create kd-tree for projected keypoints
 							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
 							rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
@@ -803,15 +807,29 @@ Transform RegistrationVis::computeTransformationImpl(
 									descriptorsIndices.resize(oi);
 									UASSERT(oi >=2);
 
-									std::vector<std::vector<cv::DMatch> > matches;
+
-									cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+									cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _bfCrossCheck);
-									matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
+									if(_bfCrossCheck)
-									UASSERT(matches.size() == 1);
-									UASSERT(matches[0].size() == 2);
-									if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
 									{
-										matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
+										std::vector<cv::DMatch> matches;
+										matcher.match(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches);
+										if(!matches.empty())
+										{
+											matchedIndex = descriptorsIndices.at(matches.at(0).trainIdx);
+										}
 									}
+									else
+									{
+										std::vector<std::vector<cv::DMatch> > matches;
+										matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
+										UASSERT(matches.size() == 1);
+										UASSERT(matches[0].size() == 2);
+										if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
+										{
+											matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
+										}
+									}
+
 								}
 								else if(indices[i].size() == 1)
 								{
@@ -883,7 +901,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						}
 						else
 						{
-							// match projected to frame
+							UDEBUG("match projected to frame");
 							std::vector<cv::Point2f> pointsTo;
 							cv::KeyPoint::convert(kptsTo, pointsTo);
 							rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
@@ -939,15 +957,27 @@ Transform RegistrationVis::computeTransformationImpl(
 										bruteForceDescCopy += bruteForceTimer.ticks();
 										UASSERT(oi >=2);
 
-										std::vector<std::vector<cv::DMatch> > matches;
+										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _bfCrossCheck);
-										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+										if(_bfCrossCheck)
-										matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
-										UASSERT(matches.size() == 1);
-										UASSERT(matches[0].size() == 2);
-										bruteForceTotalTime+=bruteForceTimer.elapsed();
-										if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
 										{
-											matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
+											std::vector<cv::DMatch> matches;
+											matcher.match(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches);
+											if(!matches.empty())
+											{
+												matchedIndexTo = descriptorsIndices.at(matches.at(0).trainIdx);
+											}
+										}
+										else
+										{
+											std::vector<std::vector<cv::DMatch> > matches;
+											matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
+											UASSERT(matches.size() == 1);
+											UASSERT(matches[0].size() == 2);
+											bruteForceTotalTime+=bruteForceTimer.elapsed();
+											if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
+											{
+												matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
+											}
 										}
 									}
 									else if(indices[i].size() == 1)
@@ -1036,29 +1066,66 @@ Transform RegistrationVis::computeTransformationImpl(
 
 					UDEBUG("");
 					// match between all descriptors
-					VWDictionary dictionary(_featureParameters);
 					std::list<int> fromWordIds;
-					if(orignalWordsFromIds.empty())
+					std::list<int> toWordIds;
+					if(_bfCrossCheck)
 					{
-						fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
+						std::vector<int> fromWordIdsV(descriptorsFrom.rows);
+						for (int i = 0; i < descriptorsFrom.rows; ++i)
+						{
+							int id = i+1;
+							if(!orignalWordsFromIds.empty())
+							{
+								id = orignalWordsFromIds[i];
+							}
+							fromWordIds.push_back(id);
+							fromWordIdsV[i] = id;
+						}
+						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);
+							for(size_t i=0; i<matches.size(); ++i)
+							{
+								toWordIdsV[matches[i].queryIdx] = fromWordIdsV[matches[i].trainIdx];
+							}
+							for(size_t i=0; i<toWordIdsV.size(); ++i)
+							{
+								int toId = toWordIdsV[i];
+								if(toId==0)
+								{
+									toId = fromWordIds.back()+i+1;
+								}
+								toWordIds.push_back(toId);
+							}
+						}
 					}
 					else
 					{
-						for (int i = 0; i < descriptorsFrom.rows; ++i)
+						VWDictionary dictionary(_featureParameters);
+						if(orignalWordsFromIds.empty())
 						{
-							int id = orignalWordsFromIds[i];
+							fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
-							dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
+						}
-							fromWordIds.push_back(id);
+						else
+						{
+							for (int i = 0; i < descriptorsFrom.rows; ++i)
+							{
+								int id = orignalWordsFromIds[i];
+								dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
+								fromWordIds.push_back(id);
+							}
 						}
-					}
 
-					std::list<int> toWordIds;
+						if(descriptorsTo.rows)
-					if(descriptorsTo.rows)
+						{
-					{
+							dictionary.update();
-						dictionary.update();
+							toWordIds = dictionary.addNewWords(descriptorsTo, 2);
-						toWordIds = dictionary.addNewWords(descriptorsTo, 2);
+						}
+						dictionary.clear(false);
 					}
-					dictionary.clear(false);
 
 					std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
 					std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -170,7 +170,7 @@ private:
 				QLabel * labelSensors,
 				bool updateConstraintView);
 	void updateStereo(const SensorData * data);
-	void updateWordsMatching();
+	void updateWordsMatching(const std::vector<int> & inliers = std::vector<int>());
 	void updateConstraintView(
 			const rtabmap::Link & link,
 			bool updateImageSliders = true,

guilib/src/DatabaseViewer.cpp

@@ -398,8 +398,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 
 	// connect configuration changed
 	connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
-	//connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
+	connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
-	//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
+	connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
 	connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -561,8 +561,8 @@ void DatabaseViewer::readSettings()
 	settings.endGroup();
 
 	// ImageViews
-	//ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
+	ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
-	//ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
+	ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
 
 	// ICP parameters
 	settings.beginGroup("icp");
@@ -646,8 +646,8 @@ void DatabaseViewer::writeSettings()
 	settings.endGroup();
 
 	// ImageViews
-	//ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
+	ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
-	//ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
+	ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
 
 	// save ICP parameters
 	settings.beginGroup("icp");
@@ -5096,7 +5096,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		}
 		ui_->graphicsView_stereo->update();
 	}
-}
+}
 
 void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
 {
@@ -5109,6 +5109,7 @@ void DatabaseViewer::updateWordsMatching()
 		ui_->graphicsView_B->clearLines();
 		ui_->graphicsView_B->setFeaturesColor(ui_->graphicsView_B->getDefaultFeatureColor());
 
+		const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
 		const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
 		std::set<int> inliersSet(inliers.begin(), inliers.end());
 		if(wordsA.size() && wordsB.size())
@@ -5152,19 +5153,27 @@ void DatabaseViewer::updateWordsMatching()
 					}
 
 					const KeypointItem * kptA = wordsA.value(ids[i]);
+					const KeypointItem * kptB = wordsB.value(ids[i]);
+
+					QColor cA = ui_->graphicsView_A->getDefaultMatchingLineColor();
+					QColor cB = ui_->graphicsView_B->getDefaultMatchingLineColor();
+					if(inliersSet.find(ids[i])!=inliersSet.end())
+					{
+						cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
+						cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
 					}
 
 					ui_->graphicsView_A->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().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
 							kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
 							cA);
 
 					ui_->graphicsView_B->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().x()+kptB->rect().width()/2,
 							kptB->rect().y()+kptB->rect().height()/2,
 							cB);
 				}
@@ -7162,7 +7171,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 					this->updateConstraintView(newLink, true, *fromS, *toS);
 					ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
 					ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
-				}
+				}
 
 				updateWordsMatching(info.inliersIDs);
 			}
@@ -7208,6 +7217,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 	std::list<Signature*> signatures;
 	Signature * fromS=0;
 	Signature * toS=0;
+
 	Link newLink;
 	RegistrationInfo info;
 	if(!containsLink(linksAdded_, from, to) &&
@@ -7226,7 +7236,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		{
 			odomMaxInf = graph::getMaxOdomInf(updateLinksWithModifications(links_));
 		}
-
 
 		Transform t;
 
@@ -7517,7 +7526,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				this->updateConstraintView(newLink, false, *fromS, *toS);
 			}
 
-			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
 			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
 			updateWordsMatching(info.inliersIDs);
 		}