Fixed default nearest neighbor approach for the dictionary when the parameter is not set.

rtabmap_gui parameters panel can be opened without rtabmap parameters set on ROS (only GUI parameters can be changed).
Rtabmap: added getStatistics() method for convenience
Rtabmap/Memory: added deleteLastLocation() method to remove the last location in the STM.
 

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@779 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Memory.h

@@ -75,6 +75,7 @@ public:
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
+	void deleteLastLocation();
 
 	//getters
 	const std::set<int> & getWorkingMem() const {return _workingMem;}

corelib/include/rtabmap/core/Rtabmap.h

@@ -97,6 +97,7 @@ public:
 	std::multimap<int, cv::KeyPoint> getWords(int locationId);
 	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false);
 	bool isInSTM(int locationId);
+	Statistics getStatistics();
 
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
@@ -106,6 +107,7 @@ public:
 	void dumpData();
 	void updateParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
+	void deleteLastLocation();
 
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
@@ -173,6 +175,8 @@ private:
 	std::list<std::string> _bufferedLogsF;
 	std::list<std::string> _bufferedLogsI;
 
+	Statistics statistics_;
+
 	std::string _wDir;
 };
 

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -149,16 +149,16 @@ private:
 class RtabmapEvent : public UEvent
 {
 public:
-	RtabmapEvent(Statistics ** stats) :
+	RtabmapEvent(const Statistics & stats) :
 		UEvent(0),
-		_stats(*stats) {}
+		_stats(stats) {}
 
-	virtual ~RtabmapEvent() {if(_stats) delete _stats;}
-	const Statistics & getStats() const {return *_stats;}
+	virtual ~RtabmapEvent() {}
+	const Statistics & getStats() const {return _stats;}
 	virtual std::string getClassName() const {return std::string("RtabmapEvent");}
 
 private:
-	Statistics * _stats;
+	Statistics _stats;
 };
 
 class RtabmapEventCmd : public UEvent

corelib/src/Memory.cpp

@@ -1384,6 +1384,38 @@ const Signature * Memory::getLastSignature() const
 	return _lastSignature;
 }
 
+void Memory::deleteLastLocation()
+{
+	Signature * lastSignature = _lastSignature;
+	if(_lastSignature)
+	{
+		UDEBUG("deleting last location %d", lastSignature->id());
+		const std::set<int> & neighbors = lastSignature->getNeighbors();
+		for(std::set<int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+		{
+			Signature * s = _getSignature(*iter);
+			if(s)
+			{
+				s->removeNeighbor(lastSignature->id());
+				_lastSignature = s;
+			}
+		}
+
+		const std::set<int> & child = lastSignature->getChildLoopClosureIds();
+		for(std::set<int>::iterator iter=child.begin(); iter!=child.end(); ++iter)
+		{
+			Signature * s = _getSignature(*iter);
+			if(s)
+			{
+				s->removeLoopClosureId(lastSignature->id());
+				s->setWeight(s->getWeight() + lastSignature->getWeight());
+			}
+		}
+
+		this->moveToTrash(lastSignature);
+	}
+}
+
 bool Memory::addLoopClosureLink(int oldId, int newId)
 {
 	ULOGGER_INFO("old=%d, new=%d", oldId, newId);

corelib/src/Rtabmap.cpp

@@ -533,6 +533,12 @@ bool Rtabmap::isInSTM(int locationId)
 	return false;
 }
 
+Statistics Rtabmap::getStatistics()
+{
+	UScopeMutex s(&_threadMutex);
+	return statistics_;
+}
+
 void Rtabmap::clearBufferedSensors()
 {
 	_imageMutex.lock();
@@ -1146,60 +1152,59 @@ void Rtabmap::process()
 	// Posterior is empty if a bad signature is detected
 	float vpHypothesis = posterior.size()?posterior.at(Memory::kIdVirtual):0.0f;
 
-	// only prepare statistics if required or when there is a loop closure
-	Statistics * stat = 0;
+	// prepare statistics
+	statistics_ = Statistics(); // reset
 	if(_lcHypothesisId || _publishStats)
 	{
 		ULOGGER_INFO("sending stats...");
-		stat = new Statistics();
-		stat->setRefImageId(refId);
+		statistics_.setRefImageId(refId);
 		if(_lcHypothesisId != Memory::kIdInvalid)
 		{
-			stat->setLoopClosureId(_lcHypothesisId);
+			statistics_.setLoopClosureId(_lcHypothesisId);
 			ULOGGER_INFO("Loop closure detected! With id=%d", _lcHypothesisId);
 		}
 		if(_publishStats && refId != Memory::kIdInvalid)
 		{
 			ULOGGER_INFO("send all stats...");
-			stat->setExtended(1);
+			statistics_.setExtended(1);
 
-			stat->addStatistic(Statistics::kLoopHighest_hypothesis_id(), hypothesis.first);
-			stat->addStatistic(Statistics::kLoopHighest_hypothesis_value(), hypothesis.second);
-			stat->addStatistic(Statistics::kHypothesis_reactivated(), lcHypothesisReactivated);
-			stat->addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
-			stat->addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
-			stat->addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
+			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), hypothesis.first);
+			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), hypothesis.second);
+			statistics_.addStatistic(Statistics::kHypothesis_reactivated(), lcHypothesisReactivated);
+			statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
+			statistics_.addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
+			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 
-			stat->addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
-			stat->addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
-			stat->addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
-			stat->addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
+			statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
+			statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
+			statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
+			statistics_.addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
 
 			// timing...
-			stat->addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
-			stat->addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
-			stat->addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
-			stat->addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
-			stat->addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
-			stat->addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
-			stat->addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
-			stat->addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
+			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
+			statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
+			statistics_.addStatistic(Statistics::kTimingAdd_loop_closure_link(), timeAddLoopClosureLink*1000);
+			statistics_.addStatistic(Statistics::kTimingLikelihood_computation(), timeLikelihoodCalculation*1000);
+			statistics_.addStatistic(Statistics::kTimingPosterior_computation(), timePosteriorCalculation*1000);
+			statistics_.addStatistic(Statistics::kTimingHypotheses_creation(), timeHypothesesCreation*1000);
+			statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000);
+			statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000);
 
 			// memory update timings
 			for(std::map<std::string, float>::iterator iter = memUpdateStats.begin(); iter!=memUpdateStats.end(); ++iter)
 			{
-				stat->addStatistic(iter->first, iter->second);
+				statistics_.addStatistic(iter->first, iter->second);
 			}
 
 			// Surf specific parameters
-			stat->addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
+			statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize);
 
 			//Epipolar geometry constraint
-			stat->addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
+			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 
 			if(_publishImage)
 			{
-				stat->setRefImage(image.image()); // raw data
+				statistics_.setRefImage(image.image()); // raw data
 				if(sLoop)
 				{
 					UTimer tmpTimer;
@@ -1209,33 +1214,33 @@ void Rtabmap::process()
 					{
 						UWARN("getting image time = %fs", tmpTimer.ticks());
 					}
-					stat->setLoopImage(im);
+					statistics_.setLoopImage(im);
 				}
 			}
 
 			if(_publishLikelihood || _publishPdf)
 			{
 				// Child count by parent signature on the root of the memory ... for statistics
-				stat->setWeights(weights);
+				statistics_.setWeights(weights);
 				if(_publishPdf)
 				{
-					stat->setPosterior(posterior);
+					statistics_.setPosterior(posterior);
 				}
 				if(_publishLikelihood)
 				{
-					stat->setLikelihood(likelihood);
-					stat->setRawLikelihood(rawLikelihood);
+					statistics_.setLikelihood(likelihood);
+					statistics_.setRawLikelihood(rawLikelihood);
 				}
 			}
 
 			if(_publishKeypoints)
 			{
 				//Copy keypoints
-				stat->setRefWords(signature->getWords());
+				statistics_.setRefWords(signature->getWords());
 				if(sLoop)
 				{
 					//Copy keypoints
-					stat->setLoopWords(sLoop->getWords());
+					statistics_.setLoopWords(sLoop->getWords());
 				}
 			}
 		}
@@ -1280,21 +1285,18 @@ void Rtabmap::process()
 	//==============================================================
 	// Finalize statistics and log files
 	//==============================================================
-	if(stat)
+	if(_publishStats)
 	{
-		if(_publishStats)
-		{
-			stat->addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
-			stat->addStatistic(Statistics::kTimingTotal(), totalTime*1000);
-			stat->addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000);
-			stat->addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
-			stat->addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
-			stat->addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
-			stat->addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved);
-		}
-		ULOGGER_DEBUG("posting stat event...");
-		this->post(new RtabmapEvent(&stat)); // the stat will be automatically deleted
+		statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
+		statistics_.addStatistic(Statistics::kTimingTotal(), totalTime*1000);
+		statistics_.addStatistic(Statistics::kTimingForgetting(), timeRealTimeLimitReachedProcess*1000);
+		statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
+		statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
+		statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
+		statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved);
 	}
+	ULOGGER_DEBUG("posting statistics_ event...");
+	this->post(new RtabmapEvent(statistics_));
 
 	std::list<float> nonNulls;
 	float sumLikelihoods = 0.0f;
@@ -1497,6 +1499,15 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }
 
+void Rtabmap::deleteLastLocation()
+{
+	UScopeMutex s(&_threadMutex);
+	if(_memory)
+	{
+		_memory->deleteLastLocation();
+	}
+}
+
 void Rtabmap::process(const cv::Mat & image, int id)
 {
 	UScopeMutex s(&_threadMutex);

corelib/src/VWDictionary.cpp

@@ -50,6 +50,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_lastWordId(0),
 	_nn(0)
 {
+	this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy(), parameters);
 	this->parseParameters(parameters);
 }
 
@@ -103,7 +104,9 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	{
 		nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str());
 	}
+
 	NNStrategy currentNNStrategy = this->nnStrategy();
+
 	if(!_nn || ( nnStrategy!=kNNUndef && (nnStrategy != currentNNStrategy) ) )
 	{
 		this->setNNStrategy(nnStrategy, parameters);