BayesFilter: improved performance when prediction didn't change or only a new id is appended to prediction. Memory: fixed seg fault when using local bundle adjustment on loop closure and memory management is enabled.

corelib/include/rtabmap/core/BayesFilter.h

@@ -76,6 +76,7 @@ private:
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
 	bool _fullPredictionUpdate;
 	float _totalPredictionLCValues;
+	float _predictionEpsilon;
 	std::map<int, std::map<int, int> > _neighborsIndex;
 };
 

corelib/src/BayesFilter.cpp

@@ -43,7 +43,8 @@ namespace rtabmap {
 BayesFilter::BayesFilter(const ParametersMap & parameters) :
 	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
 	_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
-	_totalPredictionLCValues(0.0f)
+	_totalPredictionLCValues(0.0f),
+	_predictionEpsilon(0.0f)
 {
 	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
 	this->parseParameters(parameters);
@@ -103,6 +104,14 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
 	for(unsigned int j=0; j<_predictionLC.size(); ++j)
 	{
 		_totalPredictionLCValues += _predictionLC[j];
+		if(j==0 || _predictionLC[j] < _predictionEpsilon)
+		{
+			_predictionEpsilon = _predictionLC[j];
+		}
+	}
+	if(!_predictionLC.empty())
+	{
+		UDEBUG("predictionEpsilon = %f", _predictionEpsilon);
 	}
 }
 
@@ -263,9 +272,16 @@ float addNeighborProb(cv::Mat & prediction,
 
 cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
 {
+	std::vector<int> oldIds = uKeys(_posterior);
+	if(oldIds.size() == ids.size() &&
+		memcmp(oldIds.data(), ids.data(), oldIds.size()*sizeof(int)) == 0)
+	{
+		return _prediction;
+	}
+
 	if(!_fullPredictionUpdate && !_prediction.empty())
 	{
-		return updatePrediction(_prediction, memory, uKeys(_posterior), ids);
+		return updatePrediction(_prediction, memory, oldIds, ids);
 	}
 	UDEBUG("");
 
@@ -441,6 +457,10 @@ void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float adde
 		for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
 		{
 			((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
+			if(((float*)prediction.data)[index + j*cols] < _predictionEpsilon)
+			{
+				((float*)prediction.data)[index + j*cols] = 0.0f;
+			}
 		}
 		addedProbabilitiesSum = maxNorm;
 	}
@@ -518,8 +538,17 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	}
 	UDEBUG("time getting removed ids = %fs", timer.restart());
 
+	bool oldAllCopied = false;
+	if(removedIds.empty() &&
+		newIds.size() > oldIds.size() &&
+		memcmp(oldIds.data(), newIds.data(), oldIds.size()*sizeof(int)) == 0)
+	{
+		oldPrediction.copyTo(cv::Mat(prediction, cv::Range(0, oldPrediction.rows), cv::Range(0, oldPrediction.cols)));
+		oldAllCopied = true;
+		UDEBUG("Copied all old prediction: = %fs", timer.ticks());
+	}
+
 	int added = 0;
-	float epsilon = 0.00001f;
 	// get ids to update
 	std::set<int> idsToUpdate;
 	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
@@ -532,9 +561,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 				int count = 0;
 				for(unsigned int j=0; j<cols; ++j)
 				{
-					if(((const float *)oldPrediction.data)[i + j*cols] > epsilon &&
+					if(j!=i && removedIds.find(oldIds[j]) == removedIds.end())
-					   j!=i &&
-					   removedIds.find(oldIds[j]) == removedIds.end())
 					{
 						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
 						idsToUpdate.insert(oldIds[j]);
@@ -565,6 +592,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 
 			float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
+
 			++added;
 			int count = 0;
 			for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
@@ -609,36 +637,39 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	}
 	UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
 
-	//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
-	//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
-	// copy not changed probabilities
 	int copied = 0;
-	for(unsigned int i=0; i<oldIds.size(); ++i)
+	if(!oldAllCopied)
 	{
-		if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
+		//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
+		//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
+		// copy not changed probabilities
+		for(unsigned int i=0; i<oldIds.size(); ++i)
 		{
-			for(int j=i; j<oldPrediction.cols; ++j)
+			if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
 			{
-				if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] > epsilon)
+				for(int j=0; j<oldPrediction.cols; ++j)
 				{
-					//UDEBUG("i=%d, j=%d", i, j);
+					if(oldIds[j]>0 && removedIds.find(oldIds[j]) == removedIds.end())
-					//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
-					//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
-					//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
-					float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
-					int ii = newIdToIndexMap.at(oldIds[i]);
-					int jj = newIdToIndexMap.at(oldIds[j]);
-					((float *)prediction.data)[ii + jj*prediction.cols] = v;
-					if(ii != jj)
 					{
-						((float *)prediction.data)[jj + ii*prediction.cols] = v;
+						//UDEBUG("i=%d, j=%d", i, j);
+						//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
+						//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
+						//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
+						float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
+						int ii = newIdToIndexMap.at(oldIds[i]);
+						int jj = newIdToIndexMap.at(oldIds[j]);
+						((float *)prediction.data)[ii + jj*prediction.cols] = v;
+						//if(ii != jj)
+						//{
+						//	((float *)prediction.data)[jj + ii*prediction.cols] = v;
+						//}
 					}
 				}
+				++copied;
 			}
-			++copied;
 		}
+		UDEBUG("time copying = %fs", timer.restart());
 	}
-	UDEBUG("time copying = %fs", timer.restart());
 
 	//update virtual place
 	if(newIds[0] < 0)

corelib/src/Memory.cpp

@@ -2574,16 +2574,19 @@ Transform Memory::computeTransform(
 			{
 				int id = iter->first;
 				const Signature * s = this->getSignature(id);
-				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
+				if(s)
-				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
 				{
-					if( jter->first > 0 &&
+					const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
-						util3d::isFinite(jter->second) &&
+					for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
-					    words3DMap.find(jter->first) == words3DMap.end())
 					{
-						words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
+						if( jter->first > 0 &&
-						wordsMap.insert(*s->getWords().find(jter->first));
+							util3d::isFinite(jter->second) &&
-						wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
+							words3DMap.find(jter->first) == words3DMap.end())
+						{
+							words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
+							wordsMap.insert(*s->getWords().find(jter->first));
+							wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
+						}
 					}
 				}
 			}
@@ -2619,47 +2622,50 @@ Transform Memory::computeTransform(
 					{
 						s = this->getSignature(id);
 					}
-					CameraModel model;
+					if(s)
-					if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
 					{
-						model = s->sensorData().cameraModels()[0];
+						CameraModel model;
-					}
+						if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
-					else if(s->sensorData().stereoCameraModel().isValidForProjection())
-					{
-						model = s->sensorData().stereoCameraModel().left();
-						// Set Tx for stereo BA
-						model = CameraModel(model.fx(),
-								model.fy(),
-								model.cx(),
-								model.cy(),
-								model.localTransform(),
-								-s->sensorData().stereoCameraModel().baseline()*model.fx());
-					}
-					else
-					{
-						UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
-					}
-					bundleModels.insert(std::make_pair(id, model));
-					Transform invLocalTransform = model.localTransform().inverse();
-					if(iter->second.isValid())
-					{
-						bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
-						bundlePoses.insert(std::make_pair(id, iter->second.transform()));
-					}
-					else
-					{
-						bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
-					}
-					const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
-					for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
-					{
-						if(points3DMap.find(jter->first)!=points3DMap.end() &&
-							(id == tmpTo.id() || jter->first > 0))
 						{
-							std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
+							model = s->sensorData().cameraModels()[0];
-							cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
+						}
-							wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
+						else if(s->sensorData().stereoCameraModel().isValidForProjection())
-							wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
+						{
+							model = s->sensorData().stereoCameraModel().left();
+							// Set Tx for stereo BA
+							model = CameraModel(model.fx(),
+									model.fy(),
+									model.cx(),
+									model.cy(),
+									model.localTransform(),
+									-s->sensorData().stereoCameraModel().baseline()*model.fx());
+						}
+						else
+						{
+							UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
+						}
+						bundleModels.insert(std::make_pair(id, model));
+						Transform invLocalTransform = model.localTransform().inverse();
+						if(iter->second.isValid())
+						{
+							bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
+							bundlePoses.insert(std::make_pair(id, iter->second.transform()));
+						}
+						else
+						{
+							bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
+						}
+						const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
+						for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
+						{
+							if(points3DMap.find(jter->first)!=points3DMap.end() &&
+								(id == tmpTo.id() || jter->first > 0))
+							{
+								std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
+								cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
+								wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
+								wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
+							}
 						}
 					}
 				}