Vocabulary: Binary descriptors are saved as is even if there is a float conversion for flann

corelib/src/VWDictionary.cpp

@@ -626,6 +626,25 @@ void VWDictionary::update()
 				{
 					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
 					UASSERT(w);
+
+					cv::Mat descriptor;
+					if(w->getDescriptor().type() == CV_8U)
+					{
+						useDistanceL1_ = true;
+						if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+						{
+							w->getDescriptor().convertTo(descriptor, CV_32F);
+						}
+						else
+						{
+							descriptor = w->getDescriptor();
+						}
+					}
+					else
+					{
+						descriptor = w->getDescriptor();
+					}
+
 					int index = 0;
 					if(!_flannIndex->isBuilt())
 					{
@@ -633,15 +652,15 @@ void VWDictionary::update()
 						switch(_strategy)
 						{
 						case kNNFlannNaive:
-							_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams(), useDistanceL1_);
+							_flannIndex->build(descriptor, rtflann::LinearIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannKdTree:
-							UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+							UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams(), useDistanceL1_);
+							_flannIndex->build(descriptor, rtflann::KDTreeIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannLSH:
-							UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+							UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
+							_flannIndex->build(descriptor, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
 							break;
 						default:
 							UFATAL("Not supposed to be here!");
@@ -651,9 +670,9 @@ void VWDictionary::update()
 					}
 					else
 					{
-						UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim());
+						UASSERT(descriptor.cols == _flannIndex->featuresDim());
-						UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
+						UASSERT(descriptor.type() == _flannIndex->featuresType());
-						index = _flannIndex->addPoint(w->getDescriptor());
+						index = _flannIndex->addPoint(descriptor);
 					}
 					std::pair<std::map<int, int>::iterator, bool> inserted;
 					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
@@ -698,7 +717,23 @@ void VWDictionary::update()
 				UTimer timer;
 				timer.start();
 
-				int type = _visualWords.begin()->second->getDescriptor().type();
+				int type;
+				if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
+				{
+					useDistanceL1_ = true;
+					if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+					{
+						type = CV_32F;
+					}
+					else
+					{
+						type = _visualWords.begin()->second->getDescriptor().type();
+					}
+				}
+				else
+				{
+					type = _visualWords.begin()->second->getDescriptor().type();
+				}
 				int dim = _visualWords.begin()->second->getDescriptor().cols;
 
 				UASSERT(type == CV_32F || type == CV_8U);
@@ -709,10 +744,27 @@ void VWDictionary::update()
 				std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
 				for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
 				{
-					UASSERT(iter->second->getDescriptor().cols == dim);
+					cv::Mat descriptor;
-					UASSERT(iter->second->getDescriptor().type() == type);
+					if(iter->second->getDescriptor().type() == CV_8U)
+					{
+						if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+						{
+							iter->second->getDescriptor().convertTo(descriptor, CV_32F);
+						}
+						else
+						{
+							descriptor = iter->second->getDescriptor();
+						}
+					}
+					else
+					{
+						descriptor = iter->second->getDescriptor();
+					}
 
-					iter->second->getDescriptor().copyTo(_dataTree.row(i));
+					UASSERT(descriptor.cols == dim);
+					UASSERT(descriptor.type() == type);
+
+					descriptor.copyTo(_dataTree.row(i));
 					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
 					_mapIdIndex.insert(_mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
 				}
@@ -827,6 +879,42 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 {
 	UASSERT(signatureId > 0);
 
+	UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
+	UTimer timer;
+	std::list<int> wordIds;
+	if(descriptorsIn.rows == 0 || descriptorsIn.cols == 0)
+	{
+		UERROR("Descriptors size is null!");
+		return wordIds;
+	}
+
+	if(!_incrementalDictionary && _visualWords.empty())
+	{
+		UERROR("Dictionary mode is set to fixed but no words are in it!");
+		return wordIds;
+	}
+
+	// verify we have the same features
+	int dim = 0;
+	int type = -1;
+	if(_visualWords.size())
+	{
+		dim = _visualWords.begin()->second->getDescriptor().cols;
+		type = _visualWords.begin()->second->getDescriptor().type();
+		UASSERT(type == CV_32F || type == CV_8U);
+	}
+	if(dim && dim != descriptorsIn.cols)
+	{
+		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
+		return wordIds;
+	}
+	if(type>=0 && type != descriptorsIn.type())
+	{
+		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
+		return wordIds;
+	}
+
+	// now compare with the actual index
 	cv::Mat descriptors;
 	if(descriptorsIn.type() == CV_8U)
 	{
@@ -844,21 +932,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 	{
 		descriptors = descriptorsIn;
 	}
-
+	dim = 0;
-	UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
+	type = -1;
-	UTimer timer;
+	if(_dataTree.rows || _flannIndex->isBuilt())
-	std::list<int> wordIds;
-	if(descriptors.rows == 0 || descriptors.cols == 0)
 	{
-		UERROR("Descriptors size is null!");
+		dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
-		return wordIds;
+		type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
-	}
-	int dim = 0;
-	int type = -1;
-	if(_visualWords.size())
-	{
-		dim = _visualWords.begin()->second->getDescriptor().cols;
-		type = _visualWords.begin()->second->getDescriptor().type();
 		UASSERT(type == CV_32F || type == CV_8U);
 	}
 
@@ -867,20 +946,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 		UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptors.cols, dim);
 		return wordIds;
 	}
-	dim = descriptors.cols;
 
 	if(type>=0 && type != descriptors.type())
 	{
 		UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptors.type(), type);
 		return wordIds;
 	}
-	type = descriptors.type();
-
-	if(!_incrementalDictionary && _visualWords.empty())
-	{
-		UERROR("Dictionary mode is set to fixed but no words are in it!");
-		return wordIds;
-	}
 
 	int dupWordsCountFromDict= 0;
 	int dupWordsCountFromLast= 0;
@@ -910,7 +981,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 		else if(_strategy == kNNBruteForce)
 		{
 			bruteForce = true;
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
 			matcher.knnMatch(descriptors, _dataTree, matches, k);
 		}
 		else if(_strategy == kNNBruteForceGPU)
@@ -920,7 +991,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 #ifdef HAVE_OPENCV_GPU
 			cv::gpu::GpuMat newDescriptorsGpu(descriptors);
 			cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
-			if(type==CV_8U)
+			if(descriptors.type()==CV_8U)
 			{
 				cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
 				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -938,7 +1009,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			cv::cuda::GpuMat newDescriptorsGpu(descriptors);
 			cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
 			cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
-			if(type==CV_8U)
+			if(descriptors.type()==CV_8U)
 			{
 				gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
 				gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -1010,7 +1081,8 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 		if(_newWordsComparedTogether && newWords.rows)
 		{
 			std::vector<std::vector<cv::DMatch> > matchesNewWords;
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
+			UASSERT(descriptors.cols == newWords.cols && descriptors.type() == newWords.type());
 			matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1);
 			UASSERT(matchesNewWords.size() == 1);
 			for(unsigned int j=0; j<matchesNewWords.at(0).size(); ++j)
@@ -1053,10 +1125,11 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 
 			if(badDist)
 			{
-				VisualWord * vw = new VisualWord(getNextId(), descriptors.row(i), signatureId);
+				// use original descriptor
+				VisualWord * vw = new VisualWord(getNextId(), descriptorsIn.row(i), signatureId);
 				_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
 				_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
-				newWords.push_back(vw->getDescriptor());
+				newWords.push_back(descriptors.row(i));
 				newWordsId.push_back(vw->id());
 				wordIds.push_back(vw->id());
 				UASSERT(vw->id()>0);
@@ -1103,16 +1176,16 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 
 	if(_visualWords.size() && vws.size())
 	{
-		int dim = _visualWords.begin()->second->getDescriptor().cols;
+		int type = (*vws.begin())->getDescriptor().type();
-		int type = _visualWords.begin()->second->getDescriptor().type();
+		int dim = (*vws.begin())->getDescriptor().cols;
 
-		if(dim != (*vws.begin())->getDescriptor().cols)
+		if(dim != _visualWords.begin()->second->getDescriptor().cols)
 		{
 			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", (*vws.begin())->getDescriptor().cols, dim);
 			return std::vector<int>(vws.size(), 0);
 		}
 
-		if(type != (*vws.begin())->getDescriptor().type())
+		if(type != _visualWords.begin()->second->getDescriptor().type())
 		{
 			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", (*vws.begin())->getDescriptor().type(), type);
 			return std::vector<int>(vws.size(), 0);
@@ -1126,6 +1199,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		{
 			vw = *iter;
 			UASSERT(vw);
+
 			UASSERT(vw->getDescriptor().cols == dim);
 			UASSERT(vw->getDescriptor().type() == type);
 
@@ -1137,25 +1211,64 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 	}
 	return std::vector<int>(vws.size(), 0);
 }
-std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
+std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 {
 	UTimer timer;
 	timer.start();
-	std::vector<int> resultIds(query.rows, 0);
+	std::vector<int> resultIds(queryIn.rows, 0);
 	unsigned int k=2; // k nearest neighbor
 
-	if(_visualWords.size() && query.rows)
+	if(_visualWords.size() && queryIn.rows)
 	{
+		// verify we have the same features
 		int dim = _visualWords.begin()->second->getDescriptor().cols;
 		int type = _visualWords.begin()->second->getDescriptor().type();
+		UASSERT(type == CV_32F || type == CV_8U);
 
-		if(dim != query.cols)
+		if(dim != queryIn.cols)
+		{
+			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", queryIn.cols, dim);
+			return resultIds;
+		}
+		if(type != queryIn.type())
+		{
+			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", queryIn.type(), type);
+			return resultIds;
+		}
+
+		// now compare with the actual index
+		cv::Mat query;
+		if(queryIn.type() == CV_8U)
+		{
+			if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+			{
+				queryIn.convertTo(query, CV_32F);
+			}
+			else
+			{
+				query = queryIn;
+			}
+		}
+		else
+		{
+			query = queryIn;
+		}
+		dim = 0;
+		type = -1;
+		if(_dataTree.rows || _flannIndex->isBuilt())
+		{
+			dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
+			type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
+			UASSERT(type == CV_32F || type == CV_8U);
+		}
+
+		if(dim && dim != query.cols)
 		{
 			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", query.cols, dim);
 			return resultIds;
 		}
 
-		if(type != query.type())
+		if(type>=0 && type != query.type())
 		{
 			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", query.type(), type);
 			return resultIds;
@@ -1178,7 +1291,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 			else if(_strategy == kNNBruteForce)
 			{
 				bruteForce = true;
-				cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+				cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
 				matcher.knnMatch(query, _dataTree, matches, k);
 			}
 			else if(_strategy == kNNBruteForceGPU)
@@ -1188,7 +1301,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 #ifdef HAVE_OPENCV_GPU
 				cv::gpu::GpuMat newDescriptorsGpu(query);
 				cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
-				if(type==CV_8U)
+				if(query.type()==CV_8U)
 				{
 					cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
 					gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -1206,7 +1319,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 				cv::cuda::GpuMat newDescriptorsGpu(query);
 				cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
 				cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
-				if(type==CV_8U)
+				if(query.type()==CV_8U)
 				{
 					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
 					gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
@@ -1240,20 +1353,38 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
 		std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
 		if(_notIndexedWords.size())
 		{
-			cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type);
+			cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type());
 			unsigned int index = 0;
 			VisualWord * vw;
 			for(std::set<int>::iterator iter = _notIndexedWords.begin(); iter != _notIndexedWords.end(); ++iter, ++index)
 			{
 				vw = _visualWords.at(*iter);
-				UASSERT(vw != 0 && vw->getDescriptor().cols == dim && vw->getDescriptor().type() == type);
+
+				cv::Mat descriptor;
+				if(vw->getDescriptor().type() == CV_8U)
+				{
+					if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+					{
+						vw->getDescriptor().convertTo(descriptor, CV_32F);
+					}
+					else
+					{
+						descriptor = vw->getDescriptor();
+					}
+				}
+				else
+				{
+					descriptor = vw->getDescriptor();
+				}
+
+				UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type());
 				vw->getDescriptor().copyTo(dataNotIndexed.row(index));
 				mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair<int,int>(index, vw->id()));
 			}
 
 			// Find nearest neighbor
 			ULOGGER_DEBUG("Searching in words not indexed...");
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
 			matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1);
 		}
 		ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());

guilib/src/MainWindow.cpp

@@ -1303,7 +1303,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				"  Dark Blue = Weight Update\n"
 				"  Dark Yellow = Proximity Detection in Time\n"
 				"  Dark Cyan = Neighbor Link Refined\n"
-				"  Gray = Small Movement");
+				"  Gray = Small Movement\n"
+				"Feature Color code:\n"
+				"  Green = New\n"
+				"  Yellow = New but Not Unique\n"
+				"  Red = In Vocabulary\n"
+		        "  Blue = In Vocabulary and in Previous Signature\n"
+				"  Pink = In Vocabulary and in Loop Closure Signature");
 		}
 		// Set color code as tooltip
 		if(_ui->label_matchId->toolTip().isEmpty())
@@ -1312,7 +1318,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				"Background Color Code:\n"
 				"  Green = Accepted Loop Closure Detection\n"
 				"  Red = Rejected Loop Closure Detection\n"
-				"  Yellow = Proximity Detection in Space");
+				"  Yellow = Proximity Detection in Space\n"
+				"Feature Color code:\n"
+				"  Red = In Vocabulary\n"
+				"  Pink = In Vocabulary and in Loop Closure Signature");
 		}
 
 		UDEBUG("time= %d ms", time.restart());