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

@@ -453,9 +453,6 @@ 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
-				if(iter->first != wm.rbegin()->first)
 			{
 				likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
 				if(_tfIdfNormalized)
@@ -481,7 +478,6 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
 				}
 			}
 		}
-		}
 		else
 		{
 			for(std::set<int>::const_iterator i=signatureIds.begin(); i != signatureIds.end(); ++i)

corelib/src/Memory.cpp

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

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