Retrieval margin spreading +2

Rollback change about last location in WM not used in posterior (now it is)
In consoleApp, surf extraction is done outside the rtabmap processing

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

corelib/ConsoleApp/main.cpp

@@ -368,9 +368,6 @@ int main(int argc, char * argv[])
 	pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), uBool2str(false)));
 	rtabmap->init(pm);
 
-
-
-
 	printf("Avpd init time = %fs\n", timer.ticks());
 
 	// Start thread's task
@@ -396,6 +393,12 @@ int main(int argc, char * argv[])
 	}
 	printf("\nProcessing images...\n");
 
+	//setup camera
+	ParametersMap allParam;
+	Rtabmap::readParameters(rtabmap->getIniFilePath().c_str(), allParam);
+	pm.insert(allParam.begin(), allParam.end());
+	CamKeypointTreatment imageToSMState(pm);
+
 	UTimer iterationTimer;
 	int imagesProcessed = 0;
 	std::list<std::vector<float> > teleopActions;
@@ -407,9 +410,9 @@ int main(int argc, char * argv[])
 		int i=0;
 		while(image && g_forever)
 		{
+			SMState * smState = imageToSMState.process(image);
 			++imagesProcessed;
 			iterationTimer.start();
-			SMState * smState;
 			if(i<maxTeleopActions)
 			{
 				// ONLY TESTING HERE if maxTeleopActions>0
@@ -417,12 +420,12 @@ int main(int argc, char * argv[])
 				v[0] = 2;
 				v[1] = 0;
 				teleopActions.push_back(v);
-				smState = new SMState(std::list<std::vector<float> >(), teleopActions);
+				smState->setActuators(teleopActions);
 				smState->setImage(image);
 			}
 			else
 			{
-				smState = new SMState(std::list<std::vector<float> >(), actions);
+				smState->setActuators(actions);
 				smState->setImage(image);
 			}
 			rtabmap->process(smState);

corelib/src/KeypointMemory.cpp

@@ -454,30 +454,26 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
 			const std::map<int, int> & wm = this->getWorkingMem();
 			for(std::map<int, int>::const_iterator iter = wm.begin(); iter!=wm.end(); ++iter)
 			{
-				//ignore the last in WM
+				likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
-				if(iter->first != wm.rbegin()->first)
+				if(_tfIdfNormalized)
 				{
-					likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
+					const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
-					if(_tfIdfNormalized)
+					float wordsCountRatio = -1; // default invalid
+					if(s)
 					{
-						const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
+						if(s->getWords().size() > newSurf->getWords().size())
-						float wordsCountRatio = -1; // default invalid
-						if(s)
 						{
-							if(s->getWords().size() > newSurf->getWords().size())
+							wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
-							{
-								wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
-							}
-							else if(newSurf->getWords().size())
-							{
-								wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
-							}
-							calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
 						}
-						else
+						else if(newSurf->getWords().size())
 						{
-							calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
+							wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
 						}
+						calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
+					}
+					else
+					{
+						calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
 					}
 				}
 			}

corelib/src/Memory.cpp

@@ -660,25 +660,21 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 		std::map<int, int>::const_iterator iter = wm.begin();
 		for(; iter!=wm.end(); ++iter)
 		{
-			//ignore the last in WM
+			float sim = signature->compareTo(this->getSignature(iter->first));
-			if(iter->first != wm.rbegin()->first)
+			likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, sim));
+			sumSimilarity += sim;
+			UDEBUG("sim %d with %d = %f", signature->id(), iter->first, sim);
+			if(sim>maxSim)
 			{
-				float sim = signature->compareTo(this->getSignature(iter->first));
+				maxSim = sim;
-				likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, sim));
+			}
-				sumSimilarity += sim;
+			if(sim)
-				UDEBUG("sim %d with %d = %f", signature->id(), iter->first, sim);
+			{
-				if(sim>maxSim)
+				++nonNulls;
-				{
+			}
-					maxSim = sim;
+			else
-				}
+			{
-				if(sim)
+				++nulls;
-				{
-					++nonNulls;
-				}
-				else
-				{
-					++nulls;
-				}
 			}
 		}
 		ULOGGER_DEBUG("sumSimilarity=%f, maxSim=%f, nonNulls=%d, nulls=%d)", sumSimilarity, maxSim, nonNulls, nulls);

corelib/src/Rtabmap.cpp

@@ -892,7 +892,7 @@ void Rtabmap::process()
 						// We are loosing the next loop closures (it
 						// can be temporary occlusions/bad images),
 						//increment the retrieval margin
-						++_spreadMargin;
+						_spreadMargin+=2;
 						UDEBUG("Margin++");
 					}
 					//else don't change the margin