Increased version 0.20.4. Added parameter Kp/ByteToFloat (default true to use less memory with kdtree and binary descriptors). Memory/Sqlite3: Setting weight to -9 for invalid nodes (to make sure they are not reloaded from database, to fix a graph reduced issue). Added 12-SURF/FREAK detector approach. rtabmap-info: added number of nodes in each sessions.

2026-09-02 09:30:25 +08:00 · 2020-09-28 10:29:14 -04:00
parent c5158cade5
commit 933ac736f1
13 changed files with 257 additions and 82 deletions
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -2377,6 +2377,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			query << "INNER JOIN Link ";
 			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
 			query << "WHERE from_id != to_id "; // ignore self referring links
+			query << "AND weight>-9 "; //ignore invalid nodes
 		}

 		if(ignoreBadSignatures)
@@ -3622,6 +3623,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		int descriptorSize;
 		const void * descriptor;
 		int dRealSize;
+		unsigned long dRealSizeTotal = 0;
 		for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
 		{
 			// bind id
@@ -3654,6 +3656,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 				}

 				memcpy(d.data, descriptor, dRealSize);
+				dRealSizeTotal+=dRealSize;
 				VisualWord * vw = new VisualWord(*iter, d);
 				if(vw)
 				{
@@ -3675,7 +3678,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

-		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+		UDEBUG("Time=%fs (%d words, %lu MB)", timer.ticks(), (int)vws.size(), dRealSizeTotal/1000000);

 		if(wordIds.size() != loaded.size())
 		{
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -511,7 +511,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame

 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifndef RTABMAP_NONFREE
-	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
+	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift || type == Feature2D::kFeatureSurfFreak)
 	{
 #if CV_MAJOR_VERSION < 3
 		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
@@ -524,7 +524,8 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	if(type == Feature2D::kFeatureFastBrief ||
 	   type == Feature2D::kFeatureFastFreak ||
 	   type == Feature2D::kFeatureGfttBrief ||
-	   type == Feature2D::kFeatureGfttFreak)
+	   type == Feature2D::kFeatureGfttFreak ||
+	   type == Feature2D::kFeatureSurfFreak)
 	{
 		UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
@@ -535,7 +536,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #else // >= 4.4.0 >= 3.4.11

 #ifndef RTABMAP_NONFREE
-	if(type == Feature2D::kFeatureSurf)
+	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSurfFreak)
 	{
 		UWARN("SURF features cannot be used because OpenCV was not built with nonfree module. SIFT is used instead.");
 		type = Feature2D::kFeatureSift;
@@ -614,6 +615,9 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 		feature2D = new SuperPointTorch(parameters);
 		break;
 #endif
+	case Feature2D::kFeatureSurfFreak:
+		feature2D = new SURF_FREAK(parameters);
+		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -1724,6 +1728,59 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }

+//////////////////////////
+//SURF-FREAK
+//////////////////////////
+SURF_FREAK::SURF_FREAK(const ParametersMap & parameters) :
+	SURF(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves())
+{
+	parseParameters(parameters);
+}
+
+SURF_FREAK::~SURF_FREAK()
+{
+}
+
+void SURF_FREAK::parseParameters(const ParametersMap & parameters)
+{
+	SURF::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
+}
+
+cv::Mat SURF_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
+	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
+	return descriptors;
+}
+
 //////////////////////////
 //GFTT-ORB
 //////////////////////////
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -1131,7 +1131,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 					}
 				}

-				this->moveToTrash(s, _notLinkedNodesKeptInDb);
+				this->moveToTrash(s, false);
 				s = 0;
 			}
 		}
@@ -2368,7 +2368,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 			}
 			s->removeLinks(true); // remove all links, but keep self referring link
 			s->removeLandmarks(); // remove all landmarks
-			s->setWeight(0);
+			s->setWeight(-9); // invalid
 			s->setLabel(""); // reset label
 		}
 		else
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -213,6 +213,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
 	}
+	if(uContains(parameters, Parameters::kKpByteToFloat()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpByteToFloat(), parameters.at(Parameters::kKpByteToFloat())));
+	}
 	if(uContains(parameters, Parameters::kVisFeatureType()))
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -432,6 +432,16 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	{
 		if(databaseSaved)
 		{
+			if(_memory->isGraphReduced() && _memory->isIncremental())
+			{
+				// Force reducing graph, then remove filtered nodes from the optimized poses
+				std::map<int, int> reducedIds;
+				_memory->incrementMapId(&reducedIds);
+				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				{
+					_optimizedPoses.erase(iter->first);
+				}
+			}
 			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
 		}
 		_memory->close(databaseSaved, true, ouputDatabasePath);
--- a/corelib/src/VWDictionary.cpp
+++ b/corelib/src/VWDictionary.cpp
@@ -65,6 +65,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
 	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
+	_byteToFloat(Parameters::defaultKpByteToFloat()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
@@ -90,6 +91,8 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
+	bool byteToFloat = _byteToFloat;
+	Parameters::parse(parameters, Parameters::kKpByteToFloat(), _byteToFloat);

 	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());

@@ -104,10 +107,19 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	}

 	// Verifying hypotheses strategy
+	bool treeUpdated = false;
 	if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end())
 	{
 		NNStrategy nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str());
-		this->setNNStrategy(nnStrategy);
+		treeUpdated = this->setNNStrategy(nnStrategy);
+	}
+	if(!treeUpdated && byteToFloat!=_byteToFloat && _strategy == kNNFlannKdTree)
+	{
+		UINFO("KDTree: Binary to Float conversion approach has changed, re-initialize kd-tree.");
+		_dataTree = cv::Mat();
+		_notIndexedWords = uKeysSet(_visualWords);
+		_removedIndexedWords.clear();
+		this->update();
 	}

 	if(incrementalDictionary)
@@ -277,7 +289,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	_newDictionaryPath = dictionaryPath;
 }

-void VWDictionary::setNNStrategy(NNStrategy strategy)
+bool VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 #if CV_MAJOR_VERSION < 3
 #ifdef HAVE_OPENCV_GPU
@@ -319,11 +331,14 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 	_strategy = strategy;
 	if(update)
 	{
+		UINFO("Nearest neighbor strategy has changed, re-initialize search tree.");
 		_dataTree = cv::Mat();
 		_notIndexedWords = uKeysSet(_visualWords);
 		_removedIndexedWords.clear();
 		this->update();
+		return true;
 	}
+	return false;
 }

 int VWDictionary::getLastIndexedWordId() const
@@ -348,59 +363,75 @@ unsigned int VWDictionary::getIndexMemoryUsed() const
 	return _flannIndex->memoryUsed();
 }

-cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn)
+cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat)
 {
-	// Old approach
-	//cv::Mat descriptorsOut;
-	//descriptorsIn.convertTo(descriptorsOut, CV_32F);
-	//return descriptorsOut;
-
-	// New approach
-	UASSERT(descriptorsIn.type() == CV_8UC1);
-	cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1);
-	for(int i=0; i<descriptorsIn.rows; ++i)
+	if(byteToFloat)
 	{
-		const unsigned char * ptrIn = descriptorsIn.ptr(i);
-		float * ptrOut = descriptorsOut.ptr<float>(i);
-		for(int j=0; j<descriptorsIn.cols; ++j)
-		{
-			int jo = j*8;
-			ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
-			ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
-			ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
-			ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
-			ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
-			ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
-			ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
-			ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
-		}
+		// Old approach
+		cv::Mat descriptorsOut;
+		descriptorsIn.convertTo(descriptorsOut, CV_32F);
+		return descriptorsOut;
+	}
+	else
+	{
+		// New approach
+		UASSERT(descriptorsIn.type() == CV_8UC1);
+		cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1);
+		for(int i=0; i<descriptorsIn.rows; ++i)
+		{
+			const unsigned char * ptrIn = descriptorsIn.ptr(i);
+			float * ptrOut = descriptorsOut.ptr<float>(i);
+			for(int j=0; j<descriptorsIn.cols; ++j)
+			{
+				int jo = j*8;
+				ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
+				ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
+				ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
+				ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
+				ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
+				ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
+				ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
+				ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
+			}
+		}
+		return descriptorsOut;
 	}
-	return descriptorsOut;
 }

-cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn)
+cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat)
 {
-	UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
-	cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
-	for(int i=0; i<descriptorsIn.rows; ++i)
+	if(byteToFloat)
 	{
-		const float * ptrIn = descriptorsIn.ptr<float>(i);
-		unsigned char * ptrOut = descriptorsOut.ptr(i);
-		for(int j=0; j<descriptorsOut.cols; ++j)
-		{
-			int jo = j*8;
-			ptrOut[j] =
-					(unsigned char)(ptrIn[jo] == 0?0:1) |
-					(ptrIn[jo+1] == 0?0:(1<<1)) |
-					(ptrIn[jo+2] == 0?0:(1<<2)) |
-					(ptrIn[jo+3] == 0?0:(1<<3)) |
-					(ptrIn[jo+4] == 0?0:(1<<4)) |
-					(ptrIn[jo+5] == 0?0:(1<<5)) |
-					(ptrIn[jo+6] == 0?0:(1<<6)) |
-					(ptrIn[jo+7] == 0?0:(1<<7));
-		}
+		// Old approach
+		cv::Mat descriptorsOut;
+		descriptorsIn.convertTo(descriptorsOut, CV_8UC1);
+		return descriptorsOut;
+	}
+	else
+	{
+		// New approach
+		UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
+		cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
+		for(int i=0; i<descriptorsIn.rows; ++i)
+		{
+			const float * ptrIn = descriptorsIn.ptr<float>(i);
+			unsigned char * ptrOut = descriptorsOut.ptr(i);
+			for(int j=0; j<descriptorsOut.cols; ++j)
+			{
+				int jo = j*8;
+				ptrOut[j] =
+						(unsigned char)(ptrIn[jo] == 0?0:1) |
+						(ptrIn[jo+1] == 0?0:(1<<1)) |
+						(ptrIn[jo+2] == 0?0:(1<<2)) |
+						(ptrIn[jo+3] == 0?0:(1<<3)) |
+						(ptrIn[jo+4] == 0?0:(1<<4)) |
+						(ptrIn[jo+5] == 0?0:(1<<5)) |
+						(ptrIn[jo+6] == 0?0:(1<<6)) |
+						(ptrIn[jo+7] == 0?0:(1<<7));
+			}
+		}
+		return descriptorsOut;
 	}
-	return descriptorsOut;
 }

 void VWDictionary::update()
@@ -451,7 +482,7 @@ void VWDictionary::update()
 						useDistanceL1_ = true;
 						if(_strategy == kNNFlannKdTree)
 						{
-							descriptor = convertBinTo32F(w->getDescriptor());
+							descriptor = convertBinTo32F(w->getDescriptor(), _byteToFloat);
 						}
 						else
 						{
@@ -543,7 +574,10 @@ void VWDictionary::update()
 					if(_strategy == kNNFlannKdTree)
 					{
 						type = CV_32F;
-						dim *= 8;
+						if(!_byteToFloat)
+						{
+							dim *= 8;
+						}
 					}
 					else
 					{
@@ -568,7 +602,7 @@ void VWDictionary::update()
 					{
 						if(_strategy == kNNFlannKdTree)
 						{
-							descriptor = convertBinTo32F(iter->second->getDescriptor());
+							descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
 						}
 						else
 						{
@@ -736,7 +770,7 @@ std::list<int> VWDictionary::addNewWords(
 		useDistanceL1_ = true;
 		if(_strategy == kNNFlannKdTree)
 		{
-			descriptors = convertBinTo32F(descriptorsIn);
+			descriptors = convertBinTo32F(descriptorsIn, _byteToFloat);
 		}
 		else
 		{
@@ -1074,7 +1108,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 		{
 			if(_strategy == kNNFlannKdTree)
 			{
-				query = convertBinTo32F(queryIn);
+				query = convertBinTo32F(queryIn, _byteToFloat);
 			}
 			else
 			{
@@ -1202,7 +1236,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					if(_strategy == kNNFlannKdTree)
 					{
-						descriptor = convertBinTo32F(vw->getDescriptor());
+						descriptor = convertBinTo32F(vw->getDescriptor(), _byteToFloat);
 					}
 					else
 					{