Tango: updated export workflow, added sketchfab activity, added graph optimization parameters, fixed raw images kept in memory (disabled reexctract words on loop closure while updating memory to be able to create more features for transform estimation than needed in vocabulary), portrait/landscape orientation, updated to Eisa TangoSDK. DBReader: fixed high variance when node has no links (if not the first in the current map id). MainWindow: remove frustums not in the current graph.

corelib/src/DBDriver.cpp

@@ -513,6 +513,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 		{
 			for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
 			{
+				UASSERT(*iter>0);
 				wIter = _trashVisualWords.find(*iter);
 				if(wIter != _trashVisualWords.end())
 				{

corelib/src/DBDriverSqlite3.cpp

@@ -2322,37 +2322,32 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 		{
 			int index=0;
 			id = sqlite3_column_int(ppStmt, index++); 			// VisualWord Id
-			if(id>0)
+
+			descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
+			descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
+			dRealSize = sqlite3_column_bytes(ppStmt, index++);
+
+			cv::Mat d;
+			if(dRealSize == descriptorSize)
 			{
-				descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
-				descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
-				dRealSize = sqlite3_column_bytes(ppStmt, index++);
-
-				cv::Mat d;
-				if(dRealSize == descriptorSize)
-				{
-					// CV_8U binary descriptors
-					d = cv::Mat(1, descriptorSize, CV_8U);
-				}
-				else if(dRealSize/int(sizeof(float)) == descriptorSize)
-				{
-					// CV_32F
-					d = cv::Mat(1, descriptorSize, CV_32F);
-				}
-				else
-				{
-					UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
-				}
-
-				memcpy(d.data, descriptor, dRealSize);
-				VisualWord * vw = new VisualWord(id, d);
-				vw->setSaved(true);
-				dictionary->addWord(vw);
+				// CV_8U binary descriptors
+				d = cv::Mat(1, descriptorSize, CV_8U);
+			}
+			else if(dRealSize/int(sizeof(float)) == descriptorSize)
+			{
+				// CV_32F
+				d = cv::Mat(1, descriptorSize, CV_32F);
 			}
 			else
 			{
-				ULOGGER_ERROR("Wrong word id ?!? (%d)", id);
+				UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
 			}
+
+			memcpy(d.data, descriptor, dRealSize);
+			VisualWord * vw = new VisualWord(id, d);
+			vw->setSaved(true);
+			dictionary->addWord(vw);
+
 			if(++count % 5000 == 0)
 			{
 				ULOGGER_DEBUG("Loaded %d words...", count);
@@ -2888,18 +2883,18 @@ void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool up
 		{
 			w = *i;
 			int index = 1;
-			if(w)
-			{
-				rc = sqlite3_bind_int(ppStmt, index++, w->id());
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			UASSERT(w);
 
-				//step
-				rc=sqlite3_step(ppStmt);
-				UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_bind_int(ppStmt, index++, w->id());
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			//step
+			rc=sqlite3_step(ppStmt);
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+			rc = sqlite3_reset(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-				rc = sqlite3_reset(ppStmt);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			}
 		}
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -3083,7 +3078,8 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 			for(std::list<VisualWord *>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
 				const VisualWord * w = *iter;
-				if(w && !w->isSaved())
+				UASSERT(w);
+				if(!w->isSaved())
 				{
 					rc = sqlite3_bind_int(ppStmt, 1, w->id());
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBReader.cpp

@@ -59,6 +59,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
+	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
 	_calibrated(false)
@@ -81,6 +82,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
+	_previousMapId(-1),
 	_previousStamp(0),
 	_previousMapID(0),
 	_calibrated(false)
@@ -108,6 +110,8 @@ bool DBReader::init(
 	}
 	_ids.clear();
 	_currentId=_ids.end();
+	_previousMapId = -1;
+	_previousInfMatrix = cv::Mat();
 	_previousStamp = 0;
 	_previousMapID = 0;
 	_calibrated = false;
@@ -327,12 +331,24 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				{
 					// assume the first is the backward neighbor, take its variance
 					infMatrix = links.begin()->second.infMatrix();
+					_previousInfMatrix = infMatrix;
 				}
-				else
+				else if(_previousMapId != mapId)
 				{
 					// first node, set high variance to make rtabmap trigger a new map
 					infMatrix /= 9999.0;
+					UDEBUG("First node of map %d, variance set to 9999", mapId);
 				}
+				else
+				{
+					if(_previousInfMatrix.empty())
+					{
+						_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+					}
+					// we have a node not linked to map, use last variance
+					infMatrix = _previousInfMatrix;
+				}
+				_previousMapId = mapId;
 			}
 			else
 			{
@@ -435,6 +451,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				{
 					info->odomPose = pose;
 					info->odomCovariance = infMatrix.inv();
+					UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
 				}
 			}
 		}

corelib/src/EpipolarGeometry.cpp

@@ -413,17 +413,21 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 int EpipolarGeometry::findPairs(
 		const std::map<int, cv::KeyPoint> & wordsA,
 		const std::map<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	int realPairsCount = 0;
 	pairs.clear();
 	for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
 	{
-		std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
-		if(ptB != wordsB.end())
+		if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
 		{
-			pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
-			++realPairsCount;
+			std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+			if(ptB != wordsB.end())
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
+				++realPairsCount;
+			}
 		}
 	}
 	return realPairsCount;
@@ -435,7 +439,8 @@ int EpipolarGeometry::findPairs(
  */
 int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	const std::list<int> & ids = uUniqueKeys(wordsA);
 	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
@@ -444,14 +449,17 @@ int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
 	int realPairsCount = 0;
 	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
 	{
-		iterA = wordsA.find(*i);
-		iterB = wordsB.find(*i);
-		while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
 		{
-			pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
-			++iterA;
-			++iterB;
-			++realPairsCount;
+			iterA = wordsA.find(*i);
+			iterB = wordsB.find(*i);
+			while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
+				++iterA;
+				++iterB;
+				++realPairsCount;
+			}
 		}
 	}
 	return realPairsCount;
@@ -464,24 +472,28 @@ int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
 int EpipolarGeometry::findPairsUnique(
 		const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	const std::list<int> & ids = uUniqueKeys(wordsA);
 	int realPairsCount = 0;
 	pairs.clear();
 	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
 	{
-		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
-		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
-		if(ptsA.size() == 1 && ptsB.size() == 1)
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
 		{
-			pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
-			++realPairsCount;
-		}
-		else if(ptsA.size()>1 && ptsB.size()>1)
-		{
-			// just update the count
-			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
+			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
+			if(ptsA.size() == 1 && ptsB.size() == 1)
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
+				++realPairsCount;
+			}
+			else if(ptsA.size()>1 && ptsB.size()>1)
+			{
+				// just update the count
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+			}
 		}
 	}
 	return realPairsCount;
@@ -493,7 +505,8 @@ int EpipolarGeometry::findPairsUnique(
  */
 int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
 {
 	UTimer timer;
 	timer.start();
@@ -502,16 +515,19 @@ int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & word
 	int realPairsCount = 0;;
 	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 	{
-		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
-		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
-
-		realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-
-		for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
 		{
-			for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
+			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
+
+			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+			for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
 			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
+				for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+				{
+					pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
+				}
 			}
 		}
 	}

corelib/src/Features2d.cpp

@@ -272,6 +272,41 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 	}
 }
 
+void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
+{
+	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
+	{
+		UTimer timer;
+		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+		// Remove words under the new hessian threshold
+
+		// Sort words by hessian
+		std::multimap<float, int> hessianMap; // <hessian,id>
+		for(unsigned int i = 0; i <keypoints.size(); ++i)
+		{
+			//Keep track of the data, to be easier to manage the data in the next step
+			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
+		}
+
+		// Keep keypoints with highest response
+		int removed = (int)hessianMap.size()-maxKeypoints;
+		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+		inliers.resize(keypoints.size(), false);
+		float minimumHessian = 0.0f;
+		for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
+		{
+			inliers[iter->second] = true;
+			minimumHessian = iter->first;
+		}
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
+		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
+	}
+	else
+	{
+		inliers.resize(keypoints.size(), true);
+	}
+}
+
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
 {
 	return util2d::computeRoi(image, roiRatios);

corelib/src/Memory.cpp

@@ -93,6 +93,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
+	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -246,7 +247,13 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			{
 				const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
 				std::list<int> keys = uUniqueKeys(words);
-				wordIds.insert(keys.begin(), keys.end());
+				for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end();)
+				{
+					if(*iter > 0)
+					{
+						wordIds.insert(*iter);
+					}
+				}
 			}
 			if(wordIds.size())
 			{
@@ -285,7 +292,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				UDEBUG("node=%d, word references=%d", s->id(), words.size());
 				for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
-					_vwd->addWordRef(iter->first, i->first);
+					if(iter->first > 0)
+					{
+						_vwd->addWordRef(iter->first, i->first);
+					}
 				}
 				s->setEnabled(true);
 			}
@@ -427,6 +437,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 	Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
+	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _visMaxFeatures);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -1464,10 +1475,12 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 			// Pour chaque mot dans la signature SURF
 			for(std::list<int>::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
 			{
-				// "Inverted index" - Pour chaque endroit contenu dans chaque mot
-				vw = _vwd->getWord(*i);
-				if(vw)
+				if(*i>0)
 				{
+					// "Inverted index" - Pour chaque endroit contenu dans chaque mot
+					vw = _vwd->getWord(*i);
+					UASSERT(vw!=0);
+
 					const std::map<int, int> & refs = vw->getReferences();
 					nw = refs.size();
 					if(nw)
@@ -2229,13 +2242,12 @@ Transform Memory::computeTransform(
 			// verify if it is a 180 degree transform, well verify > 90
 			float x,y,z, roll,pitch,yaw;
 			transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-			if(fabs(roll) > CV_PI/2 ||
-			   fabs(pitch) > CV_PI/2 ||
+			if(fabs(pitch) > CV_PI/2 ||
 			   fabs(yaw) > CV_PI/2)
 			{
 				transform.setNull();
-				std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
-						roll, pitch, yaw);
+				std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
+						roll, pitch, yaw, CV_PI/2);
 				UINFO(msg.c_str());
 				if(info)
 				{
@@ -3302,9 +3314,26 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				}
 			}
 
+			int oldMaxFeatures = _feature2D->getMaxFeatures();
+			UDEBUG("rawDescriptorsKept=%d, pose=%d, maxFeatures=%d, visMaxFeatures=%d", _rawDescriptorsKept?1:0, pose.isNull()?0:1, _feature2D->getMaxFeatures(), _visMaxFeatures);
+			ParametersMap tmpMaxFeatureParameter;
+			if(_rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures)
+			{
+				// The total extracted features should match the number of features used for transformation estimation
+				UDEBUG("Changing temporary max features from %d to %d", _feature2D->getMaxFeatures(), _visMaxFeatures);
+				tmpMaxFeatureParameter.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(_visMaxFeatures)));
+				_feature2D->parseParameters(tmpMaxFeatureParameter);
+			}
+
 			keypoints = _feature2D->generateKeypoints(
 					imageMono,
 					depthMask);
+
+			if(tmpMaxFeatureParameter.size())
+			{
+				tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) = uNumber2Str(oldMaxFeatures);
+				_feature2D->parseParameters(tmpMaxFeatureParameter); // reset back
+			}
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3351,9 +3380,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
 		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
 
-		if((int)keypoints.size() > _feature2D->getMaxFeatures())
+		int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
+		if((int)keypoints.size() > maxFeatures)
 		{
-			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, _feature2D->getMaxFeatures());
+			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
 		}
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
@@ -3443,10 +3473,60 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
 		}
 
-		wordIds = _vwd->addNewWords(descriptors, id);
+		// In case the number of features we want to do quantization is lower
+		// than extracted ones (that would be used for transform estimation)
+		std::vector<bool> inliers;
+		cv::Mat descriptorsForQuantization = descriptors;
+		std::vector<int> quantizedToRawIndices;
+		if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
+		{
+			UASSERT((int)keypoints.size() == descriptors.rows);
+			Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+
+			descriptorsForQuantization = cv::Mat(_feature2D->getMaxFeatures(), descriptors.cols, descriptors.type());
+			quantizedToRawIndices.resize(_feature2D->getMaxFeatures());
+			unsigned int oi=0;
+			UASSERT((int)inliers.size() == descriptors.rows);
+			for(int k=0; k < descriptors.rows; ++k)
+			{
+				if(inliers[k])
+				{
+					UASSERT(oi < quantizedToRawIndices.size());
+					if(descriptors.type() == CV_32FC1)
+					{
+						memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
+					}
+					else
+					{
+						memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
+					}
+					quantizedToRawIndices[oi] = k;
+					++oi;
+				}
+			}
+			UASSERT((int)oi == _feature2D->getMaxFeatures());
+		}
+
+		// Quantization to vocabulary
+		wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
+
+		// Set ID -1 to features not used for quantization
+		if(wordIds.size() < keypoints.size())
+		{
+			std::vector<int> allWordIds;
+			allWordIds.resize(keypoints.size(),-1);
+			int i=0;
+			for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
+			{
+				allWordIds[quantizedToRawIndices[i]] = *iter;
+				++i;
+			}
+			wordIds = uVectorToList(allWordIds);
+		}
+
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
-		UDEBUG("time addNewWords %fs", t);
+		UDEBUG("time addNewWords %fs indexed=%d not=%d", t, _vwd->getIndexedWordsCount(), _vwd->getNotIndexedWordsCount());
 	}
 	else if(id>0)
 	{
@@ -3805,7 +3885,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 			//Find words in the signature which they are not in the current dictionary
 			for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
 			{
-				if(_vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
+				if(*k>0 && _vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
 				{
 					oldWordIds.insert(oldWordIds.end(), *k);
 				}
@@ -3875,13 +3955,13 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 		const std::vector<int> & keys = uKeys((*j)->getWords());
 		if(keys.size())
 		{
-			const VisualWord * wordFirst = _vwd->getWord(keys.front()); //get descriptor size
-			UASSERT(wordFirst!=0);
-
 			// Add all references
 			for(unsigned int i=0; i<keys.size(); ++i)
 			{
-				_vwd->addWordRef(keys.at(i), (*j)->id());
+				if(keys.at(i)>0)
+				{
+					_vwd->addWordRef(keys.at(i), (*j)->id());
+				}
 			}
 			(*j)->setEnabled(true);
 		}

corelib/src/RegistrationVis.cpp

@@ -301,10 +301,21 @@ Transform RegistrationVis::computeTransformationImpl(
 			kptsFrom.resize(fromSignature.getWords().size());
 			orignalWordsFromIds.resize(fromSignature.getWords().size());
 			int i=0;
+			bool allUniques = true;
 			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
 			{
 				kptsFrom[i] = iter->second;
-				orignalWordsFromIds[i++] = iter->first;
+				orignalWordsFromIds[i] = iter->first;
+				if(i>0 && iter->first==orignalWordsFromIds[i-1])
+				{
+					allUniques = false;
+				}
+				++i;
+			}
+			if(!allUniques)
+			{
+				UDEBUG("IDs are not unique, IDs will be regenerated!");
+				orignalWordsFromIds.clear();
 			}
 		}
 

corelib/src/Rtabmap.cpp

@@ -1782,6 +1782,7 @@ bool Rtabmap::process(
 	//=============================================================
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
+	int loopClosureVisualMatches = 0;
 	if(_loopClosureHypothesis.first>0)
 	{
 		//Compute transform if metric data are present
@@ -1792,6 +1793,7 @@ bool Rtabmap::process(
 		{
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
 			loopClosureVisualInliers = info.inliers;
+			loopClosureVisualMatches = info.matches;
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
@@ -1920,6 +1922,10 @@ bool Rtabmap::process(
 									{
 										loopClosureVisualInliers = info.inliers;
 									}
+									if(loopClosureVisualMatches == 0)
+									{
+										loopClosureVisualMatches = info.matches;
+									}
 
 									if(_loopClosureHypothesis.first == 0)
 									{
@@ -2314,6 +2320,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
+			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
 			statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);

corelib/src/RtabmapThread.cpp

@@ -573,7 +573,16 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 						odomEvent.rotVariance()>=9999 ||
 						odomEvent.transVariance()>=9999))
 		{
-			UWARN("Odometry is reset (identity pose or high variance >=9999 detected). Increment map id!");
+			if(odomEvent.pose().isIdentity())
+			{
+				UWARN("Odometry is reset (identity pose detected). Increment map id!");
+			}
+			else
+			{
+				UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!",
+						odomEvent.info().varianceLin>odomEvent.transVariance()?odomEvent.info().varianceLin:odomEvent.transVariance(),
+						odomEvent.info().varianceAng>odomEvent.rotVariance()?odomEvent.info().varianceAng:odomEvent.rotVariance());
+			}
 			pushNewState(kStateTriggeringMap);
 			_rotVariance = 0;
 			_transVariance = 0;

corelib/src/Signature.cpp

@@ -44,7 +44,8 @@ Signature::Signature() :
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_enabled(false)
+	_enabled(false),
+	_invalidWordsCount(0)
 {
 }
 
@@ -66,6 +67,7 @@ Signature::Signature(
 	_modified(true),
 	_linksModified(true),
 	_enabled(false),
+	_invalidWordsCount(0),
 	_pose(pose),
 	_groundTruthPose(groundTruthPose),
 	_sensorData(sensorData)
@@ -87,6 +89,7 @@ Signature::Signature(const SensorData & data) :
 	_modified(true),
 	_linksModified(true),
 	_enabled(false),
+	_invalidWordsCount(0),
 	_pose(Transform::getIdentity()),
 	_groundTruthPose(data.groundTruth()),
 	_sensorData(data)
@@ -178,10 +181,12 @@ float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
 	const std::multimap<int, cv::KeyPoint> & words = s.getWords();
-	if(words.size() != 0 && _words.size() != 0)
+
+	if(!s.isBadSignature() && !this->isBadSignature())
 	{
 		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
-		unsigned int totalWords = _words.size()>words.size()?_words.size():words.size();
+		int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
+		UASSERT(totalWords > 0);
 		EpipolarGeometry::findPairs(words, _words, pairs);
 
 		similarity = float(pairs.size()) / float(totalWords);
@@ -196,7 +201,15 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 	{
 		std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
 		std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
-		_words.erase(oldWordId);
+		if(oldWordId<=0)
+		{
+			_invalidWordsCount-=(int)_words.erase(oldWordId);
+			UASSERT(_invalidWordsCount>=0);
+		}
+		else
+		{
+			_words.erase(oldWordId);
+		}
 		_words3.erase(oldWordId);
 		_wordsDescriptors.erase(oldWordId);
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
@@ -215,9 +228,24 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 	}
 }
 
+void Signature::setWords(const std::multimap<int, cv::KeyPoint> & words)
+{
+	_enabled = false;
+	_words = words;
+	_invalidWordsCount = 0;
+	for(std::multimap<int, cv::KeyPoint>::iterator iter=_words.begin(); iter!=_words.end(); ++iter)
+	{
+		if(iter->first>0)
+		{
+			break;
+		}
+		++_invalidWordsCount;
+	}
+}
+
 bool Signature::isBadSignature() const
 {
-	return !_words.size();
+	return _words.size()-_invalidWordsCount <= 0;
 }
 
 void Signature::removeAllWords()
@@ -225,11 +253,20 @@ void Signature::removeAllWords()
 	_words.clear();
 	_words3.clear();
 	_wordsDescriptors.clear();
+	_invalidWordsCount = 0;
 }
 
 void Signature::removeWord(int wordId)
 {
-	_words.erase(wordId);
+	if(wordId<=0)
+	{
+		_invalidWordsCount-=(int)_words.erase(wordId);
+		UASSERT(_invalidWordsCount>=0);
+	}
+	else
+	{
+		_words.erase(wordId);
+	}
 	_words3.erase(wordId);
 	_wordsDescriptors.clear();
 }

corelib/src/VWDictionary.cpp

@@ -551,7 +551,7 @@ int VWDictionary::getNextId()
 
 void VWDictionary::addWordRef(int wordId, int signatureId)
 {
-	if(signatureId > 0 && wordId > 0)
+	if(signatureId > 0)
 	{
 		VisualWord * vw = 0;
 		vw = uValue(_visualWords, wordId, vw);

corelib/src/resources/DatabaseSchema.sql.in

@@ -81,8 +81,7 @@ CREATE TABLE Map_Node_Word (
 	depth_z FLOAT,
 	descriptor_size INTEGER,
 	descriptor BLOB,
-	FOREIGN KEY (node_id) REFERENCES Node(id),
-	FOREIGN KEY (word_id) REFERENCES Word(id)
+	FOREIGN KEY (node_id) REFERENCES Node(id)
 );
 
 CREATE TABLE Info (