Rtabmap: Refactoring... removed all threading/events related stuff from Rtabmap object to RtabmapThread object (a thread wrapper of the rtabmap object)

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

app/src/main.cpp

@@ -20,7 +20,7 @@
 #include <QtGui/QApplication>
 #include <QtCore/QDir>
 #include "utilite/UEventsManager.h"
-#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/gui/MainWindow.h"
 #include <QtGui/QMessageBox>
 #include "utilite/UObjDeletionThread.h"
@@ -45,7 +45,7 @@ int main(int argc, char* argv[])
 	/* Start thread's task */
 	mainWindow->showNormal();
 
-	Rtabmap * rtabmap = new Rtabmap();
+	RtabmapThread * rtabmap = new RtabmapThread();
 	rtabmap->setWorkingDirectory(mainWindow->getWorkingDirectory().toStdString());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
 	UEventsManager::addHandler(rtabmap);
@@ -74,7 +74,7 @@ int main(int argc, char* argv[])
 	msg->setIconPixmap(QPixmap(":/images/RTAB-Map.ico"));
 	msg->setWindowIcon(QIcon(":/images/RTAB-Map.ico"));
 	msg->show();
-	UObjDeletionThread<Rtabmap> delThread(rtabmap);
+	UObjDeletionThread<RtabmapThread> delThread(rtabmap);
 	ObjDeletionHandler handler(delThread.id(), app, SLOT(quit()));
 	UEventsManager::addHandler(&handler);
 	delThread.startDeletion(1); // make sure that app-exec() is called before the deletion of the object

corelib/include/rtabmap/core/Rtabmap.h

@@ -22,14 +22,14 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include "utilite/UThreadNode.h"
+#include <utilite/USemaphore.h>
-#include "utilite/UEventsHandler.h"
+#include <utilite/UMutex.h>
-#include "utilite/USemaphore.h"
+#include <utilite/UVariant.h>
-#include "utilite/UMutex.h"
+
-#include "utilite/UVariant.h"
-#include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Image.h"
+#include "rtabmap/core/Statistics.h"
+
 #include <opencv2/core/core.hpp>
 #include <list>
 #include <stack>
@@ -42,26 +42,10 @@ class EpipolarGeometry;
 class Memory;
 class BayesFilter;
 
-class RTABMAP_EXP Rtabmap :
+class RTABMAP_EXP Rtabmap
-	public UThreadNode,
-	public UEventsHandler
 {
 public:
-	enum State {
-		kStateIdle,
-		kStateDetecting,
-		kStateReseting,
-		kStateChangingParameters,
-		kStateDumpingMemory,
-		kStateDumpingPrediction,
-		kStateGeneratingGraph,
-		kStateGeneratingLocalGraph,
-		kStateDeletingMemory,
-		kStateCleanSensorsBuffer
-	};
-
 	enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
-
 	static const char * kDefaultDatabaseName;
 
 public:
@@ -78,34 +62,33 @@ public:
 
 	void init(const ParametersMap & param, bool deleteMemory = true);
 	void init(const std::string & configFile = "", bool deleteMemory = true);
-	void clearBufferedSensors();
 
 	void close();
 
 	const std::string & getWorkingDir() const {return _wDir;}
 	int getLoopClosureId() const;
 	int getRetrievedId() const;
-	int getLastLocationId();
+	int getLastLocationId() const;
 	float getLcHypValue() const {return _lcHypothesisValue;}
-	std::list<int> getWM(); // working memory
+	std::list<int> getWM() const; // working memory
-	std::set<int> getSTM(); // short-term memory
+	std::set<int> getSTM() const; // short-term memory
-	int getWMSize(); // working memory size
+	int getWMSize() const; // working memory size
-	int getSTMSize(); // short-term memory size
+	int getSTMSize() const; // short-term memory size
-	std::map<int, int> getWeights();
+	std::map<int, int> getWeights() const;
-	int getTotalMemSize();
+	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
-	std::multimap<int, cv::KeyPoint> getWords(int locationId);
+	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
-	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false);
+	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;
-	bool isInSTM(int locationId);
+	bool isInSTM(int locationId) const;
-	Statistics getStatistics();
+	const Statistics & getStatistics() const;
 
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
-	void generateGraph(const std::string & path);
+	void generateGraph(const std::string & path, int id=0, int margin=5);
 	void resetMemory(bool dbOverwritten = false);
-	void dumpPrediction();
+	void dumpPrediction() const;
-	void dumpData();
+	void dumpData() const;
-	void updateParameters(const ParametersMap & parameters);
+	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
 	void deleteLastLocation();
 	void rejectLastLoopClosure();
@@ -114,23 +97,9 @@ public:
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
 
-protected:
-	virtual void handleEvent(UEvent * anEvent);
-
 private:
-	virtual void mainLoop();
-	virtual void mainLoopKill();
-	virtual void mainLoopBegin();
-	void process();
-	void addImage(const Image & image);
-	void getImage(Image & image);
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
-	void releaseAllStrategies();
-	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
-	void generateLocalGraph(const std::string & path, int id, int margin);
-	void parseParameters(const ParametersMap & parameters);
-	void setDataBufferSize(int size);
 
 private:
 	// Modifiable parameters
@@ -141,7 +110,6 @@ private:
 	bool _publishKeypoints;
 	float _maxTimeAllowed; // in ms
 	unsigned int _maxMemoryAllowed; // signatures count in WM
-	int _imageBufferMaxSize;
 	float _loopThr;
 	float _loopRatio;
 	unsigned int _maxRetrieved;
@@ -153,16 +121,6 @@ private:
 	int _retrievedId;
 	double _lastProcessTime;
 
-	UMutex _stateMutex;
-	std::stack<State> _state;
-	std::stack<ParametersMap> _stateParam;
-
-	std::list<Image> _imageBuffer;
-	UMutex _imageMutex;
-	USemaphore _imageAdded;
-
-	UMutex _threadMutex;
-
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
 	EpipolarGeometry * _epipolarGeometry;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -20,131 +20,14 @@
 #ifndef RTABMAPEVENT_H_
 #define RTABMAPEVENT_H_
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
 
 #include <utilite/UEvent.h>
-#include <opencv2/core/core.hpp>
+#include "rtabmap/core/Statistics.h"
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <utilite/ULogger.h>
-#include <list>
-#include <vector>
 
 namespace rtabmap
 {
 
-#define RTABMAP_STATS(PREFIX, NAME, UNIT) \
-	public: \
-		static std::string k##PREFIX##NAME() {return #PREFIX "/" #NAME "/" #UNIT;} \
-	private: \
-		class Dummy##PREFIX##NAME { \
-		public: \
-			Dummy##PREFIX##NAME() {if(!_defaultDataInitialized)_defaultData.insert(std::pair<std::string, float>(#PREFIX "/" #NAME "/" #UNIT, 0.0f));} \
-		}; \
-		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
-
-class RTABMAP_EXP Statistics
-{
-	RTABMAP_STATS(Loop, RejectedHypothesis,);
-	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
-	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
-	RTABMAP_STATS(Loop, Vp_hypothesis,);
-	RTABMAP_STATS(Loop, ReactivateId,);
-	RTABMAP_STATS(Loop, Hypothesis_ratio,);
-
-	RTABMAP_STATS(Memory, Working_memory_size,);
-	RTABMAP_STATS(Memory, Short_time_memory_size,);
-	RTABMAP_STATS(Memory, Signatures_removed,);
-	RTABMAP_STATS(Memory, Signatures_retrieved,);
-	RTABMAP_STATS(Memory, Images_buffered,);
-
-	RTABMAP_STATS(Timing, Memory_update, ms);
-	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
-	RTABMAP_STATS(Timing, Reactivation, ms);
-	RTABMAP_STATS(Timing, Add_loop_closure_link, ms);
-	RTABMAP_STATS(Timing, Likelihood_computation, ms);
-	RTABMAP_STATS(Timing, Posterior_computation, ms);
-	RTABMAP_STATS(Timing, Hypotheses_creation, ms);
-	RTABMAP_STATS(Timing, Hypotheses_validation, ms);
-	RTABMAP_STATS(Timing, Statistics_creation, ms);
-	RTABMAP_STATS(Timing, Memory_cleanup, ms);
-	RTABMAP_STATS(Timing, Total, ms);
-	RTABMAP_STATS(Timing, Forgetting, ms);
-	RTABMAP_STATS(Timing, Joining_trash, ms);
-	RTABMAP_STATS(Timing, Emptying_trash, ms);
-
-	RTABMAP_STATS(, Hypothesis_reactivated,);
-
-	RTABMAP_STATS(Keypoint, Dictionary_size, words);
-	RTABMAP_STATS(Keypoint, Response_threshold,);
-
-public:
-	static const std::map<std::string, float> & defaultData();
-
-public:
-	Statistics();
-	virtual ~Statistics();
-
-	// name format = "Grp/Name/unit"
-	void addStatistic(const std::string & name, float value);
-
-	// setters
-	void setExtended(bool extended) {_extended = extended;}
-	void setRefImageId(int refImageId) {_refImageId = refImageId;}
-	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
-	void setRefImage(const cv::Mat & image);
-	void setLoopImage(const cv::Mat & image);
-	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
-	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
-	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
-	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
-	void setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
-	void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
-
-	// getters
-	bool extended() const {return _extended;}
-	int refImageId() const {return _refImageId;}
-	int loopClosureId() const {return _loopClosureId;}
-	const cv::Mat & refImage() const {return _refImage;}
-	const cv::Mat & loopImage() const {return _loopImage;}
-	const std::map<int, int> & weights() const {return _weights;}
-	const std::map<int, float> & posterior() const {return _posterior;}
-	const std::map<int, float> & likelihood() const {return _likelihood;}
-	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
-	const std::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
-	const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
-
-	const std::map<std::string, float> & data() const {return _data;}
-
-private:
-	int _extended; // 0 -> only loop closure and last signature ID fields are filled
-
-	int _refImageId;
-	int _loopClosureId;
-
-	// extended data start here...
-	cv::Mat _refImage;
-	cv::Mat _loopImage;
-
-	std::map<int, int> _weights;
-	std::map<int, float> _posterior;
-	std::map<int, float> _likelihood;
-	std::map<int, float> _rawLikelihood;
-
-	//keypoint memory
-	std::multimap<int, cv::KeyPoint> _refWords;
-	std::multimap<int, cv::KeyPoint> _loopWords;
-
-	// Format for statistics (Plottable statistics must go in that map) :
-	// {"Group/Name/Unit", value}
-	// Example : {"Timing/Total time/ms", 500.0f}
-	std::map<std::string, float> _data;
-	static std::map<std::string, float> _defaultData;
-	static bool _defaultDataInitialized;
-	// end extended data
-};
-
-
 ////////// The RtabmapEvent class //////////////
 class RtabmapEvent : public UEvent
 {

corelib/include/rtabmap/core/RtabmapThread.h

@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef RTABMAPTHREAD_H_
+#define RTABMAPTHREAD_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <utilite/UThreadNode.h>
+#include <utilite/UEventsHandler.h>
+
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/Image.h"
+#include "rtabmap/core/Parameters.h"
+
+#include <stack>
+
+namespace rtabmap {
+
+class Rtabmap;
+
+class RTABMAP_EXP RtabmapThread :
+	public UThreadNode,
+	public UEventsHandler
+{
+public:
+	enum State {
+		kStateIdle,
+		kStateDetecting,
+		kStateReseting,
+		kStateChangingParameters,
+		kStateDumpingMemory,
+		kStateDumpingPrediction,
+		kStateGeneratingGraph,
+		kStateGeneratingLocalGraph,
+		kStateDeletingMemory,
+		kStateCleanSensorsBuffer
+	};
+
+public:
+	RtabmapThread();
+	virtual ~RtabmapThread();
+
+	void setWorkingDirectory(const std::string & path);
+	void clearBufferedSensors();
+
+protected:
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	virtual void mainLoop();
+	virtual void mainLoopKill();
+	void process();
+	void addImage(const Image & image);
+	void getImage(Image & image);
+	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
+	void setDataBufferSize(int size);
+
+private:
+	UMutex _stateMutex;
+	std::stack<State> _state;
+	std::stack<ParametersMap> _stateParam;
+
+	std::list<Image> _imageBuffer;
+	UMutex _imageMutex;
+	USemaphore _imageAdded;
+	int _imageBufferMaxSize;
+
+	Rtabmap * _rtabmap;
+};
+
+} /* namespace rtabmap */
+#endif /* RTABMAPTHREAD_H_ */

corelib/include/rtabmap/core/Statistics.h

@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef STATISTICS_H_
+#define STATISTICS_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <list>
+#include <vector>
+
+namespace rtabmap {
+
+#define RTABMAP_STATS(PREFIX, NAME, UNIT) \
+	public: \
+		static std::string k##PREFIX##NAME() {return #PREFIX "/" #NAME "/" #UNIT;} \
+	private: \
+		class Dummy##PREFIX##NAME { \
+		public: \
+			Dummy##PREFIX##NAME() {if(!_defaultDataInitialized)_defaultData.insert(std::pair<std::string, float>(#PREFIX "/" #NAME "/" #UNIT, 0.0f));} \
+		}; \
+		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+
+class RTABMAP_EXP Statistics
+{
+	RTABMAP_STATS(Loop, RejectedHypothesis,);
+	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
+	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
+	RTABMAP_STATS(Loop, Vp_hypothesis,);
+	RTABMAP_STATS(Loop, ReactivateId,);
+	RTABMAP_STATS(Loop, Hypothesis_ratio,);
+
+	RTABMAP_STATS(Memory, Working_memory_size,);
+	RTABMAP_STATS(Memory, Short_time_memory_size,);
+	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Signatures_retrieved,);
+	RTABMAP_STATS(Memory, Images_buffered,);
+
+	RTABMAP_STATS(Timing, Memory_update, ms);
+	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
+	RTABMAP_STATS(Timing, Reactivation, ms);
+	RTABMAP_STATS(Timing, Add_loop_closure_link, ms);
+	RTABMAP_STATS(Timing, Likelihood_computation, ms);
+	RTABMAP_STATS(Timing, Posterior_computation, ms);
+	RTABMAP_STATS(Timing, Hypotheses_creation, ms);
+	RTABMAP_STATS(Timing, Hypotheses_validation, ms);
+	RTABMAP_STATS(Timing, Statistics_creation, ms);
+	RTABMAP_STATS(Timing, Memory_cleanup, ms);
+	RTABMAP_STATS(Timing, Total, ms);
+	RTABMAP_STATS(Timing, Forgetting, ms);
+	RTABMAP_STATS(Timing, Joining_trash, ms);
+	RTABMAP_STATS(Timing, Emptying_trash, ms);
+
+	RTABMAP_STATS(, Hypothesis_reactivated,);
+
+	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Response_threshold,);
+
+public:
+	static const std::map<std::string, float> & defaultData();
+
+public:
+	Statistics();
+	virtual ~Statistics();
+
+	// name format = "Grp/Name/unit"
+	void addStatistic(const std::string & name, float value);
+
+	// setters
+	void setExtended(bool extended) {_extended = extended;}
+	void setRefImageId(int refImageId) {_refImageId = refImageId;}
+	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setRefImage(const cv::Mat & image);
+	void setLoopImage(const cv::Mat & image);
+	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
+	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
+	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
+	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
+	void setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
+	void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
+
+	// getters
+	bool extended() const {return _extended;}
+	int refImageId() const {return _refImageId;}
+	int loopClosureId() const {return _loopClosureId;}
+	const cv::Mat & refImage() const {return _refImage;}
+	const cv::Mat & loopImage() const {return _loopImage;}
+	const std::map<int, int> & weights() const {return _weights;}
+	const std::map<int, float> & posterior() const {return _posterior;}
+	const std::map<int, float> & likelihood() const {return _likelihood;}
+	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
+	const std::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
+	const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
+
+	const std::map<std::string, float> & data() const {return _data;}
+
+private:
+	int _extended; // 0 -> only loop closure and last signature ID fields are filled
+
+	int _refImageId;
+	int _loopClosureId;
+
+	// extended data start here...
+	cv::Mat _refImage;
+	cv::Mat _loopImage;
+
+	std::map<int, int> _weights;
+	std::map<int, float> _posterior;
+	std::map<int, float> _likelihood;
+	std::map<int, float> _rawLikelihood;
+
+	//keypoint memory
+	std::multimap<int, cv::KeyPoint> _refWords;
+	std::multimap<int, cv::KeyPoint> _loopWords;
+
+	// Format for statistics (Plottable statistics must go in that map) :
+	// {"Group/Name/Unit", value}
+	// Example : {"Timing/Total time/ms", 500.0f}
+	std::map<std::string, float> _data;
+	static std::map<std::string, float> _defaultData;
+	static bool _defaultDataInitialized;
+	// end extended data
+};
+
+}// end namespace rtabmap
+
+#endif /* STATISTICS_H_ */

corelib/src/CMakeLists.txt

@@ -1,7 +1,9 @@
 
 SET(SRC_FILES
 	Rtabmap.cpp
-	RtabmapEvent.cpp
+	RtabmapThread.cpp
+	
+	Statistics.cpp
 	
 	Memory.cpp
 	

corelib/src/Memory.cpp

@@ -68,6 +68,8 @@ Memory::Memory(const ParametersMap & parameters) :
 
 bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
 {
+	UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
+
 	ULOGGER_DEBUG("");
 	this->parseParameters(parameters);
 
@@ -188,6 +190,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	}
 	ULOGGER_DEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
 
+	UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
 	return success;
 }
 

corelib/src/Rtabmap.cpp

@@ -18,11 +18,9 @@
  */
 
 #include "rtabmap/core/Rtabmap.h"
-#include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/Version.h"
 #include "rtabmap/core/Features2d.h"
 
-#include "rtabmap/core/Camera.h"
 #include "Signature.h"
 
 #include "rtabmap/core/EpipolarGeometry.h"
@@ -30,7 +28,11 @@
 #include "rtabmap/core/Memory.h"
 #include "BayesFilter.h"
 
-#include <utilite/UtiLite.h>
+#include <utilite/ULogger.h>
+#include <utilite/UFile.h>
+#include <utilite/UTimer.h>
+#include <utilite/UConversion.h>
+#include <utilite/UMath.h>
 
 #include "SimpleIni.h"
 
@@ -64,7 +66,6 @@ Rtabmap::Rtabmap() :
 	_publishKeypoints(Parameters::defaultKpPublishKeypoints()),
 	_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
 	_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
-	_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
 	_loopRatio(Parameters::defaultRtabmapLoopRatio()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
@@ -200,9 +201,52 @@ void Rtabmap::flushStatisticLogs()
 	}
 }
 
-void Rtabmap::releaseAllStrategies()
+void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
 {
-	ULOGGER_DEBUG("");
+	if(deleteMemory)
+	{
+		// Set the working directory before
+		ParametersMap::const_iterator iter;
+		if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
+		{
+			this->setWorkingDirectory(iter->second.c_str());
+		}
+		this->resetMemory(true);
+	}
+
+	this->parseParameters(parameters);
+
+	setupLogFiles();
+}
+
+void Rtabmap::init(const std::string & configFile, bool deleteMemory)
+{
+	// fill ctrl struct with values from the configuration file
+	ParametersMap param;// = Parameters::defaultParameters;
+
+	if(!configFile.empty())
+	{
+		ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
+		this->readParameters(configFile, param);
+	}
+
+	this->init(param, deleteMemory);
+}
+
+void Rtabmap::close()
+{
+	flushStatisticLogs();
+	if(_foutFloat)
+	{
+		fclose(_foutFloat);
+		_foutFloat = 0;
+	}
+	if(_foutInt)
+	{
+		fclose(_foutInt);
+		_foutInt = 0;
+	}
+
 	if(_epipolarGeometry)
 	{
 		delete _epipolarGeometry;
@@ -220,86 +264,6 @@ void Rtabmap::releaseAllStrategies()
 	}
 }
 
-void Rtabmap::pushNewState(State newState, const ParametersMap & parameters)
-{
-	ULOGGER_DEBUG("to %d", newState);
-
-	_stateMutex.lock();
-	{
-		_state.push(newState);
-		_stateParam.push(parameters);
-	}
-	_stateMutex.unlock();
-
-	_imageAdded.release();
-}
-
-void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
-{
-	UScopeMutex s(&_threadMutex);
-	if(this->isRunning())
-	{
-		pushNewState(kStateChangingParameters, parameters);
-		if(deleteMemory)
-		{
-			pushNewState(kStateDeletingMemory);
-		}
-	}
-	else
-	{
-		if(deleteMemory)
-		{
-			// Set the working directory before
-			ParametersMap::const_iterator iter;
-			if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
-			{
-				this->setWorkingDirectory(iter->second.c_str());
-			}
-			this->resetMemory(true);
-		}
-
-		this->parseParameters(parameters);
-	}
-	setupLogFiles();
-}
-
-void Rtabmap::init(const std::string & configFile, bool deleteMemory)
-{
-	UScopeMutex s(&_threadMutex);
-	// fill ctrl struct with values from the configuration file
-	ParametersMap param;// = Parameters::defaultParameters;
-
-	if(!configFile.empty())
-	{
-		ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
-		this->readParameters(configFile, param);
-	}
-
-	this->init(param, deleteMemory);
-}
-
-void Rtabmap::close()
-{
-	UEventsManager::removeHandler(this);
-
-	// Stop the thread first
-	join(true);
-
-	flushStatisticLogs();
-	if(_foutFloat)
-	{
-		fclose(_foutFloat);
-		_foutFloat = 0;
-	}
-	if(_foutInt)
-	{
-		fclose(_foutInt);
-		_foutInt = 0;
-	}
-
-	this->releaseAllStrategies();
-}
-
 void Rtabmap::parseParameters(const ParametersMap & parameters)
 {
 	ULOGGER_DEBUG("");
@@ -340,10 +304,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	{
 		_loopRatio = std::atof(iter->second.c_str());
 	}
-	if((iter=parameters.find(Parameters::kRtabmapImageBufferSize())) != parameters.end())
-	{
-		_imageBufferMaxSize = std::atoi(iter->second.c_str());
-	}
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
 	{
 		this->setWorkingDirectory(iter->second.c_str());
@@ -366,12 +326,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	if(!_memory)
 	{
-		UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
-		UEventsManager::post(new RtabmapEventInit("Creating memory..."));
 		_memory = new Memory(parameters);
-		UEventsManager::post(new RtabmapEventInit("Creating memory, done!"));
 		_memory->init(_wDir + kDefaultDatabaseName, false, parameters);
-		UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
 	}
 	else
 	{
@@ -419,9 +375,8 @@ int Rtabmap::getRetrievedId() const
 	return _retrievedId;
 }
 
-int Rtabmap::getLastLocationId()
+int Rtabmap::getLastLocationId() const
 {
-	UScopeMutex s(&_threadMutex);
 	int id = 0;
 	if(_memory && _memory->getLastSignature())
 	{
@@ -430,9 +385,8 @@ int Rtabmap::getLastLocationId()
 	return id;
 }
 
-std::list<int> Rtabmap::getWM()
+std::list<int> Rtabmap::getWM() const
 {
-	UScopeMutex s(&_threadMutex);
 	std::list<int> mem;
 	if(_memory)
 	{
@@ -442,9 +396,8 @@ std::list<int> Rtabmap::getWM()
 	return mem;
 }
 
-int Rtabmap::getWMSize()
+int Rtabmap::getWMSize() const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->getWorkingMem().size()-1; // remove virtual place
@@ -452,9 +405,8 @@ int Rtabmap::getWMSize()
 	return 0;
 }
 
-std::map<int, int> Rtabmap::getWeights()
+std::map<int, int> Rtabmap::getWeights() const
 {
-	UScopeMutex s(&_threadMutex);
 	std::map<int, int> weights;
 	if(_memory)
 	{
@@ -464,9 +416,8 @@ std::map<int, int> Rtabmap::getWeights()
 	return weights;
 }
 
-std::set<int> Rtabmap::getSTM()
+std::set<int> Rtabmap::getSTM() const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->getStMem();
@@ -474,9 +425,8 @@ std::set<int> Rtabmap::getSTM()
 	return std::set<int>();
 }
 
-int Rtabmap::getSTMSize()
+int Rtabmap::getSTMSize() const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->getStMem().size();
@@ -484,9 +434,8 @@ int Rtabmap::getSTMSize()
 	return 0;
 }
 
-int Rtabmap::getTotalMemSize()
+int Rtabmap::getTotalMemSize() const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		const Signature * s  =_memory->getLastSignature();
@@ -498,9 +447,8 @@ int Rtabmap::getTotalMemSize()
 	return 0;
 }
 
-std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId)
+std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		const Signature * s = _memory->getSignature(locationId);
@@ -512,9 +460,8 @@ std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId)
 	return std::multimap<int, cv::KeyPoint>();
 }
 
-std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM)
+std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM) const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->getNeighborsId(nodeId, margin, lookInLTM?-1:0);
@@ -522,9 +469,8 @@ std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM)
 	return std::map<int, int>();
 }
 
-bool Rtabmap::isInSTM(int locationId)
+bool Rtabmap::isInSTM(int locationId) const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		return _memory->isInSTM(locationId);
@@ -532,125 +478,48 @@ bool Rtabmap::isInSTM(int locationId)
 	return false;
 }
 
-Statistics Rtabmap::getStatistics()
+const Statistics & Rtabmap::getStatistics() const
 {
-	UScopeMutex s(&_threadMutex);
 	return statistics_;
 }
 
-void Rtabmap::clearBufferedSensors()
+void Rtabmap::generateGraph(const std::string & path, int id, int margin)
 {
-	_imageMutex.lock();
-	{
-		_imageBuffer.clear();
-	}
-	_imageMutex.unlock();
-}
-
-
-void Rtabmap::mainLoopBegin()
-{
-	if(!_memory || !_bayesFilter)
-	{
-		ULOGGER_DEBUG("Rtabmap thread started without all strategies defined...");
-		//this->killSafely();
-	}
-}
-
-void Rtabmap::mainLoopKill()
-{
-	this->clearBufferedSensors();
-
-	// this will post the newData semaphore
-	_imageAdded.release();
-}
-
-void Rtabmap::mainLoop()
-{
-	UScopeMutex s(&_threadMutex);
-
-	State state = kStateDetecting;
-	ParametersMap parameters;
-
-	_stateMutex.lock();
-	{
-		if(!_state.empty() && !_stateParam.empty())
-		{
-			state = _state.top();
-			_state.pop();
-			parameters = _stateParam.top();
-			_stateParam.pop();
-		}
-	}
-	_stateMutex.unlock();
-
-	switch(state)
-	{
-	case kStateDetecting:
-		this->process();
-		break;
-	case kStateChangingParameters:
-		this->parseParameters(parameters);
-		break;
-	case kStateReseting:
-		this->resetMemory();
-		break;
-	case kStateDumpingMemory:
-		if(_memory)
-		{
-			_memory->dumpMemory(this->getWorkingDir());
-		}
-		break;
-	case kStateDumpingPrediction:
-		if(_memory && _bayesFilter)
-		{
-			this->dumpPrediction();
-		}
-		break;
-	case kStateGeneratingGraph:
-		if(_memory)
-		{
-			_memory->joinTrashThread(); // make sure the trash is flushed
-			_memory->generateGraph(parameters.at("path"));
-		}
-		break;
-	case kStateGeneratingLocalGraph:
-		if(_memory)
-		{
-			_memory->joinTrashThread(); // make sure the trash is flushed
-			this->generateLocalGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
-		}
-		break;
-	case kStateDeletingMemory:
-		this->resetMemory(true);
-		break;
-	case kStateCleanSensorsBuffer:
-		this->clearBufferedSensors();
-		break;
-	default:
-		UFATAL("Invalid state !?!?");
-		break;
-	}
-}
-
-void Rtabmap::generateGraph(const std::string & path)
-{
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		_memory->joinTrashThread(); // make sure the trash is flushed
-		_memory->generateGraph(path);
+
+		if(id > 0)
+		{
+			std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
+
+			if(ids.size() > 0)
+			{
+				ids.insert(std::pair<int,int>(id, 0));
+				std::set<int> idsSet;
+				for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+				{
+					idsSet.insert(idsSet.end(), iter->first);
+				}
+				_memory->generateGraph(path, idsSet);
+			}
+			else
+			{
+				UERROR("No neighbors found for signature %d.", id);
+			}
+		}
+		else
+		{
+			_memory->generateGraph(path);
+		}
 	}
 }
 
 void Rtabmap::resetMemory(bool dbOverwritten)
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
-		UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
 		_memory->init(_wDir + kDefaultDatabaseName, dbOverwritten);
-		UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
 		if(_bayesFilter)
 		{
 			_bayesFilter->reset();
@@ -662,7 +531,6 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 		// May be memory should be already created here, and use init above...
 		UFile::erase(_wDir + kDefaultDatabaseName);
 	}
-	this->clearBufferedSensors();
 	_retrievedId = 0;
 	_lcHypothesisValue = 0;
 	_lcHypothesisId = 0;
@@ -670,89 +538,10 @@ void Rtabmap::resetMemory(bool dbOverwritten)
 	this->setupLogFiles(dbOverwritten);
 }
 
-void Rtabmap::handleEvent(UEvent* event)
-{
-	if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
-	{
-		CameraEvent * e = (CameraEvent*)event;
-		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeFeatures)
-		{
-			this->addImage(e->image());
-		}
-	}
-	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
-	{
-		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
-		RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
-		if(cmd == RtabmapEventCmd::kCmdResetMemory)
-		{
-			if(_memory)
-			{
-				ULOGGER_DEBUG("CMD_RESET_MEMORY");
-				pushNewState(kStateReseting);
-			}
-		}
-		else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
-		{
-			if(_memory)
-			{
-				ULOGGER_DEBUG("CMD_DUMP_MEMORY");
-				pushNewState(kStateDumpingMemory);
-			}
-		}
-		else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
-		{
-			if(_memory && _bayesFilter)
-			{
-				ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
-				pushNewState(kStateDumpingPrediction);
-			}
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateGraph)
-		{
-			if(_memory && !rtabmapEvent->getStr().empty())
-			{
-				ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
-				ParametersMap param;
-				param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-				pushNewState(kStateGeneratingGraph, param);
-			}
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateLocalGraph)
-		{
-			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
-			if(_memory && values.size() == 3)
-			{
-				ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
-				ParametersMap param;
-				param.insert(ParametersPair("path", *values.begin()));
-				param.insert(ParametersPair("id", *(++values.begin())));
-				param.insert(ParametersPair("margin", *values.rbegin()));
-				pushNewState(kStateGeneratingLocalGraph, param);
-			}
-		}
-		else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
-		{
-			ULOGGER_DEBUG("CMD_DELETE_MEMORY");
-			pushNewState(kStateDeletingMemory);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
-		{
-			ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
-			pushNewState(kStateCleanSensorsBuffer);
-		}
-	}
-	else if(event->getClassName().compare("ParamEvent") == 0)
-	{
-		ULOGGER_DEBUG("changing parameters");
-		pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
-	}
-}
-
 //============================================================
 // MAIN LOOP
 //============================================================
-void Rtabmap::process()
+void Rtabmap::process(const Image & image)
 {
 	UDEBUG("");
 
@@ -794,7 +583,6 @@ void Rtabmap::process()
 
 	const Signature * signature = 0;
 	const Signature * sLoop = 0;
-	Image image;
 
 	_lcHypothesisId = 0;
 
@@ -804,7 +592,6 @@ void Rtabmap::process()
 	// Wait for an image...
 	//============================================================
 	ULOGGER_INFO("getting data...");
-	this->getImage(image);
 	if(image.empty())
 	{
 		ULOGGER_INFO("image is null...");
@@ -859,11 +646,8 @@ void Rtabmap::process()
 			rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
 
 			// Adjust the likelihood (with mean and std dev)
-			adjustedLikelihood = rawLikelihood;
+			likelihood = rawLikelihood;
-			this->adjustLikelihood(adjustedLikelihood);
+			this->adjustLikelihood(likelihood);
-
-			// Apply sensory attention
-			likelihood = adjustedLikelihood;
 
 			timeLikelihoodCalculation = timer.ticks();
 			ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
@@ -879,10 +663,7 @@ void Rtabmap::process()
 			timePosteriorCalculation = timer.ticks();
 			ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
 
-			//for(std::map<int,float>::iterator iter=posterior.begin(); iter!=posterior.end(); ++iter)
+
-			//{
-			//	UDEBUG("posterior (%d) = %f", iter->first, iter->second);
-			//}
 			timer.start();
 			//============================================================
 			// Select the highest hypothesis
@@ -1179,7 +960,6 @@ void Rtabmap::process()
 			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...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -1296,36 +1076,6 @@ void Rtabmap::process()
 		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;
-	float maxLikelihood = 0.0f;
-	float mean = 0.0f;
-	float stddev = 0.0f;
-	std::map<int, float>::iterator iter=likelihood.begin();
-	if(iter->first == -1)
-	{
-		++iter;
-	}
-	for(; iter!=likelihood.end(); ++iter)
-	{
-		if(iter->second > 0)
-		{
-			sumLikelihoods += iter->second;
-			nonNulls.push_back(iter->second);
-		}
-		if(iter->second > maxLikelihood)
-		{
-			maxLikelihood = iter->second;
-		}
-	}
-	if(nonNulls.size())
-	{
-		mean = sumLikelihoods / float(nonNulls.size());
-		stddev = uStdDev(nonNulls, mean);
-	}
 
 	// Log info...
 	// TODO : use a specific class which will handle the RtabmapEvent
@@ -1341,10 +1091,10 @@ void Rtabmap::process()
 								timeStatsCreation,
 								_lcHypothesisValue,
 								0.0f,
-								maxLikelihood,
+								0.0f,
-								sumLikelihoods,
+								0.0f,
-								mean,
+								0.0f,
-								stddev,
+								0.0f,
 								vpHypothesis,
 								timeJoiningTrash,
 								rehearsalValue,
@@ -1366,7 +1116,7 @@ void Rtabmap::process()
 								lcHypothesisReactivated,
 								refUniqueWordsCount,
 								_retrievedId,
-								int(nonNulls.size()),
+								0.0f,
 								rehearsalMaxId,
 								rehearsalMaxId>0?1:0);
 	if(_statisticLogsBufferedInRAM)
@@ -1390,52 +1140,6 @@ void Rtabmap::process()
 
 }
 
-void Rtabmap::addImage(const Image & image)
-{
-	if(image.empty())
-	{
-		ULOGGER_ERROR("image empty !?");
-		return;
-	}
-
-	bool notify = true;
-	_imageMutex.lock();
-	{
-		_imageBuffer.push_back(image);
-		while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
-		{
-			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
-			_imageBuffer.pop_front();
-			notify = false;
-		}
-	}
-	_imageMutex.unlock();
-
-	if(notify)
-	{
-		_imageAdded.release();
-	}
-}
-
-void Rtabmap::getImage(Image & image)
-{
-	ULOGGER_DEBUG("");
-
-	ULOGGER_INFO("waiting for data");
-	_imageAdded.acquire();
-	ULOGGER_INFO("wake-up");
-
-	_imageMutex.lock();
-	{
-		if(!_imageBuffer.empty())
-		{
-			image = _imageBuffer.front();
-			_imageBuffer.pop_front();
-		}
-	}
-	_imageMutex.unlock();
-}
-
 // SETTERS
 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
 {
@@ -1452,22 +1156,8 @@ void Rtabmap::setTimeThreshold(float maxTimeAllowed)
 	}
 }
 
-void Rtabmap::setDataBufferSize(int size)
-{
-	if(size < 0)
-	{
-		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
-		_imageBufferMaxSize = 0;
-	}
-	else
-	{
-		_imageBufferMaxSize = size;
-	}
-}
-
 void Rtabmap::setWorkingDirectory(std::string path)
 {
-	UScopeMutex scopeMutex(&_threadMutex);
 	if(path.size() && (path.at(path.size()-1) != '\\' || path.at(path.size()-1) != '/' ))
 	{
 		path += UDirectory::separator();
@@ -1481,12 +1171,8 @@ void Rtabmap::setWorkingDirectory(std::string path)
 			_wDir = path;
 			if(_memory)
 			{
-				UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
 				_memory->init(_wDir + kDefaultDatabaseName);
-				UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
 				setupLogFiles();
-				//clear all buffered images
-				this->clearBufferedSensors();
 			}
 			else
 			{
@@ -1502,7 +1188,7 @@ void Rtabmap::setWorkingDirectory(std::string path)
 
 void Rtabmap::deleteLastLocation()
 {
-	UScopeMutex s(&_threadMutex);
+
 	if(_memory)
 	{
 		_memory->deleteLastLocation();
@@ -1511,7 +1197,6 @@ void Rtabmap::deleteLastLocation()
 
 void Rtabmap::rejectLastLoopClosure()
 {
-	UScopeMutex s(&_threadMutex);
 	UDEBUG("_lcHypothesisId=%d", _lcHypothesisId);
 	if(_lcHypothesisId)
 	{
@@ -1530,40 +1215,18 @@ void Rtabmap::rejectLastLoopClosure()
 
 void Rtabmap::process(const cv::Mat & image, int id)
 {
-	UScopeMutex s(&_threadMutex);
+
 	this->process(Image(image, id));
 }
 
-void Rtabmap::process(const Image & image)
+void Rtabmap::dumpData() const
 {
-	UScopeMutex s(&_threadMutex);
-	if(!this->isRunning())
-	{
-		this->addImage(image);
-		this->process();
-	}
-	else
-	{
-		UERROR("The rtabmap thread is running! Don't start rtabmap thread if you want to use this method directly.");
-	}
-}
-
-void Rtabmap::dumpData()
-{
-	UScopeMutex s(&_threadMutex);
 	if(_memory)
 	{
 		_memory->dumpMemory(this->getWorkingDir());
 	}
 }
 
-void Rtabmap::updateParameters(const ParametersMap & parameters)
-{
-	UScopeMutex s(&_threadMutex);
-	this->parseParameters(parameters);
-}
-
-
 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 {
 	ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
@@ -1634,9 +1297,8 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 	UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
 }
 
-void Rtabmap::dumpPrediction()
+void Rtabmap::dumpPrediction() const
 {
-	UScopeMutex s(&_threadMutex);
 	if(_memory && _bayesFilter)
 	{
 		const std::set<int> & wm = _memory->getWorkingMem();
@@ -1669,31 +1331,6 @@ void Rtabmap::dumpPrediction()
 	}
 }
 
-void Rtabmap::generateLocalGraph(const std::string & path, int id, int margin)
-{
-	if(_memory)
-	{
-		std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
-
-		if(ids.size() > 0)
-		{
-			ids.insert(std::pair<int,int>(id, 0));
-			std::set<int> idsSet;
-			for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
-			{
-				idsSet.insert(idsSet.end(), iter->first);
-				UINFO("Node %d", iter->first);
-			}
-			UINFO("idsSet=%d", idsSet.size());
-			_memory->generateGraph(path, idsSet);
-		}
-		else
-		{
-			UERROR("No neighbors found for signature %d.", id);
-		}
-	}
-}
-
 void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)
 {
 	CSimpleIniA ini;

corelib/src/RtabmapThread.cpp

@@ -0,0 +1,291 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/Camera.h"
+
+#include <utilite/ULogger.h>
+#include <utilite/UEventsManager.h>
+#include <utilite/UStl.h>
+
+namespace rtabmap {
+
+RtabmapThread::RtabmapThread() :
+		_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
+		_rtabmap(new Rtabmap())
+
+{
+}
+
+RtabmapThread::~RtabmapThread()
+{
+	UEventsManager::removeHandler(this);
+
+	// Stop the thread first
+	join(true);
+
+	delete _rtabmap;
+}
+
+void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
+{
+	ULOGGER_DEBUG("to %d", newState);
+
+	_stateMutex.lock();
+	{
+		_state.push(newState);
+		_stateParam.push(parameters);
+	}
+	_stateMutex.unlock();
+
+	_imageAdded.release();
+}
+
+void RtabmapThread::setWorkingDirectory(const std::string & path)
+{
+	_rtabmap->setWorkingDirectory(path);
+}
+
+void RtabmapThread::clearBufferedSensors()
+{
+	_imageMutex.lock();
+	{
+		_imageBuffer.clear();
+	}
+	_imageMutex.unlock();
+}
+
+void RtabmapThread::mainLoopKill()
+{
+	this->clearBufferedSensors();
+
+	// this will post the newData semaphore
+	_imageAdded.release();
+}
+
+void RtabmapThread::mainLoop()
+{
+	State state = kStateDetecting;
+	ParametersMap parameters;
+
+	_stateMutex.lock();
+	{
+		if(!_state.empty() && !_stateParam.empty())
+		{
+			state = _state.top();
+			_state.pop();
+			parameters = _stateParam.top();
+			_stateParam.pop();
+		}
+	}
+	_stateMutex.unlock();
+
+	ParametersMap::iterator iter;
+	switch(state)
+	{
+	case kStateDetecting:
+		this->process();
+		break;
+	case kStateChangingParameters:
+		if((iter=parameters.find(Parameters::kRtabmapImageBufferSize())) != parameters.end())
+		{
+			_imageBufferMaxSize = std::atoi(iter->second.c_str());
+		}
+		_rtabmap->parseParameters(parameters);
+		break;
+	case kStateReseting:
+		_rtabmap->resetMemory();
+		this->clearBufferedSensors();
+		break;
+	case kStateDumpingMemory:
+		_rtabmap->dumpData();
+		break;
+	case kStateDumpingPrediction:
+		_rtabmap->dumpPrediction();
+		break;
+	case kStateGeneratingGraph:
+		_rtabmap->generateGraph(parameters.at("path"));
+		break;
+	case kStateGeneratingLocalGraph:
+		_rtabmap->generateGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
+		break;
+	case kStateDeletingMemory:
+		_rtabmap->resetMemory(true);
+		this->clearBufferedSensors();
+		break;
+	case kStateCleanSensorsBuffer:
+		this->clearBufferedSensors();
+		break;
+	default:
+		UFATAL("Invalid state !?!?");
+		break;
+	}
+}
+
+
+void RtabmapThread::handleEvent(UEvent* event)
+{
+	if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
+	{
+		CameraEvent * e = (CameraEvent*)event;
+		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeFeatures)
+		{
+			this->addImage(e->image());
+		}
+	}
+	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
+	{
+		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
+		RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
+		if(cmd == RtabmapEventCmd::kCmdResetMemory)
+		{
+			ULOGGER_DEBUG("CMD_RESET_MEMORY");
+			pushNewState(kStateReseting);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
+		{
+			ULOGGER_DEBUG("CMD_DUMP_MEMORY");
+			pushNewState(kStateDumpingMemory);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
+		{
+			ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
+			pushNewState(kStateDumpingPrediction);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGenerateGraph)
+		{
+			if(!rtabmapEvent->getStr().empty())
+			{
+				ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
+				ParametersMap param;
+				param.insert(ParametersPair("path", rtabmapEvent->getStr()));
+				pushNewState(kStateGeneratingGraph, param);
+			}
+		}
+		else if(cmd == RtabmapEventCmd::kCmdGenerateLocalGraph)
+		{
+			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
+			if(values.size() == 3)
+			{
+				ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
+				ParametersMap param;
+				param.insert(ParametersPair("path", *values.begin()));
+				param.insert(ParametersPair("id", *(++values.begin())));
+				param.insert(ParametersPair("margin", *values.rbegin()));
+				pushNewState(kStateGeneratingLocalGraph, param);
+			}
+		}
+		else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
+		{
+			ULOGGER_DEBUG("CMD_DELETE_MEMORY");
+			pushNewState(kStateDeletingMemory);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
+		{
+			ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
+			pushNewState(kStateCleanSensorsBuffer);
+		}
+	}
+	else if(event->getClassName().compare("ParamEvent") == 0)
+	{
+		ULOGGER_DEBUG("changing parameters");
+		pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
+	}
+}
+
+//============================================================
+// MAIN LOOP
+//============================================================
+void RtabmapThread::process()
+{
+	Image image;
+	getImage(image);
+	if(!image.empty())
+	{
+		_rtabmap->process(image);
+
+		Statistics stats = _rtabmap->getStatistics();
+		stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
+		ULOGGER_DEBUG("posting statistics_ event...");
+		this->post(new RtabmapEvent(stats));
+	}
+}
+
+void RtabmapThread::addImage(const Image & image)
+{
+	if(image.empty())
+	{
+		ULOGGER_ERROR("image empty !?");
+		return;
+	}
+
+	bool notify = true;
+	_imageMutex.lock();
+	{
+		_imageBuffer.push_back(image);
+		while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
+		{
+			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+			_imageBuffer.pop_front();
+			notify = false;
+		}
+	}
+	_imageMutex.unlock();
+
+	if(notify)
+	{
+		_imageAdded.release();
+	}
+}
+
+void RtabmapThread::getImage(Image & image)
+{
+	ULOGGER_DEBUG("");
+
+	ULOGGER_INFO("waiting for data");
+	_imageAdded.acquire();
+	ULOGGER_INFO("wake-up");
+
+	_imageMutex.lock();
+	{
+		if(!_imageBuffer.empty())
+		{
+			image = _imageBuffer.front();
+			_imageBuffer.pop_front();
+		}
+	}
+	_imageMutex.unlock();
+}
+
+void RtabmapThread::setDataBufferSize(int size)
+{
+	if(size < 0)
+	{
+		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
+		_imageBufferMaxSize = 0;
+	}
+	else
+	{
+		_imageBufferMaxSize = size;
+	}
+}
+
+} /* namespace rtabmap */

corelib/src/Statistics.cpp

@@ -17,7 +17,7 @@
  * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/Statistics.h"
 #include <utilite/UStl.h>
 
 namespace rtabmap {

examples/main.cpp

@@ -20,6 +20,7 @@
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/Camera.h"
 #include <opencv2/core/core.hpp>
+#include <stdio.h>
 
 void showUsage()
 {