increased version to 0.8.3

Don't update timestamps if only links are modified

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 8)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

app/src/main.cpp

@@ -43,16 +43,23 @@ int main(int argc, char* argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
 
-	ULOGGER_INFO("Program started...");
-
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	MainWindow * mainWindow = new MainWindow();
 
+	UINFO("Program started...");
+
 	UEventsManager::addHandler(mainWindow);
 
 	/* Start thread's task */
-	mainWindow->showNormal();
+	if(mainWindow->isSavedMaximized())
+	{
+		mainWindow->showMaximized();
+	}
+	else
+	{
+		mainWindow->show();
+	}
 
 	RtabmapThread * rtabmap = new RtabmapThread(new Rtabmap());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
@@ -67,37 +74,14 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);
 
-	ULOGGER_INFO("Killing threads...");
+	UINFO("Killing threads...");
 	rtabmap->join(true);
 
-	ULogger::setLevel(ULogger::kInfo);
+	UINFO("Closing RTAB-Map...");
-
+	delete rtabmap;
-	ULOGGER_INFO("Closing RTAB-Map core...");
-
-	//Since we can't put the Rtabmap object in the MainWindow class,
-	//we pop up a message box indicating that the rtabmap object
-	// is being deleted (saving data to the database)
-	QMessageBox * msg = new QMessageBox(QMessageBox::Information,
-			QObject::tr("RTAB-Map is closing..."),
-			QObject::tr("The detector is saving the working memory to database (located in RTAB-Map's working directory)..."),
-			QMessageBox::NoButton,
-			mainWindow);
-	msg->setEnabled(false);
-	msg->setIconPixmap(QPixmap(":/images/RTAB-Map.ico"));
-	msg->setWindowIcon(QIcon(":/images/RTAB-Map.ico"));
-	msg->show();
-	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
-	app->exec();
-
-	ULOGGER_INFO("Closing RTAB-Map gui...");
 	delete mainWindow;
-
 	delete app;
-
+	UINFO("All done!");
-	ULOGGER_INFO("All done!");
 
     return 0;
 }

corelib/include/rtabmap/core/Camera.h

@@ -61,6 +61,10 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setImageSize(unsigned int width, unsigned int height);
 
+	void setCalibration(const std::string & fileName);
+	void setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients);
+	void resetCalibration();
+
 protected:
 	/**
 	 * Constructor
@@ -78,6 +82,8 @@ private:
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
 	UTimer * _frameRateTimer;
+	cv::Mat _k; // camera_matrix
+	cv::Mat _d; // distorsion_coefficients
 };
 
 

corelib/include/rtabmap/core/Compression.h

@@ -0,0 +1,87 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef COMPRESSION_H_
+#define COMPRESSION_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UThread.h>
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+/**
+ * Compress image or data
+ *
+ * Example compression:
+ *   cv::Mat image;// an image
+ *   CompressionThread ct(image);
+ *   ct.start();
+ *   ct.join();
+ *   std::vector<unsigned char> bytes = ct.getCompressedData();
+ *
+ * Example uncompression
+ *   std::vector<unsigned char> bytes;// a compressed image
+ *   CompressionThread ct(bytes);
+ *   ct.start();
+ *   ct.join();
+ *   cv::Mat image = ct.getUncompressedData();
+ */
+class RTABMAP_EXP CompressionThread : public UThread
+{
+public:
+	// format : ".png" ".jpg" "" (empty is general)
+	CompressionThread(const cv::Mat & mat, const std::string & format = "");
+	CompressionThread(const cv::Mat & bytes, bool isImage);
+	const cv::Mat & getCompressedData() const {return compressedData_;}
+	cv::Mat & getUncompressedData() {return uncompressedData_;}
+protected:
+	virtual void mainLoop();
+private:
+	cv::Mat compressedData_;
+	cv::Mat uncompressedData_;
+	std::string format_;
+	bool image_;
+	bool compressMode_;
+};
+
+std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
+cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
+
+cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
+cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
+
+std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
+cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
+
+cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
+cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
+
+} /* namespace rtabmap */
+#endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -72,6 +72,7 @@ public:
 	void asyncSave(VisualWord * vw); //ownership transferred
 	void emptyTrashes(bool async = false);
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
+	void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
 
 public:
 	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
@@ -121,8 +122,8 @@ private:
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
 
 
 	// Load objects
@@ -157,6 +158,7 @@ private:
 	USemaphore _addSem;
 	double _emptyTrashesTime;
 	std::string _url;
+	bool _timestampUpdate;
 };
 
 }

corelib/include/rtabmap/core/Graph.h

@@ -0,0 +1,125 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef GRAPH_H_
+#define GRAPH_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <map>
+#include <list>
+#include <rtabmap/core/Link.h>
+
+namespace rtabmap {
+
+namespace graph {
+
+std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
+		std::multimap<int, Link> & links,
+		int from,
+		int to);
+
+// <int, depth> depth=0 means infinite depth
+std::map<int, int> RTABMAP_EXP generateDepthGraph(
+		const std::multimap<int, Link> & links,
+		int fromId,
+		int depth = 0);
+
+void RTABMAP_EXP optimizeTOROGraph(
+		const std::map<int, int> & depthGraph,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		std::map<int, Transform> & optimizedPoses,
+		int toroIterations = 100,
+		bool toroInitialGuess = true,
+		bool ignoreCovariance = false,
+		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+
+void RTABMAP_EXP optimizeTOROGraph(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		std::map<int, Transform> & optimizedPoses,
+		int toroIterations = 100,
+		bool toroInitialGuess = true,
+		bool ignoreCovariance = false,
+		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+
+bool RTABMAP_EXP saveTOROGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints);
+
+bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
+		std::map<int, Transform> & poses,
+		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
+
+std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		bool keepLatest = true);
+
+/**
+ * Get all neighbor nodes in a fixed radius around each pose.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius
+ * @return A map between each pose id and its neighbors found in the radius
+ */
+std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle);
+
+/**
+ * Perform A* path planning in the graph.
+ * @param poses The graph's poses
+ * @param links The graph's links (from node id -> to node id)
+ * @param from initial node
+ * @param to final node
+ * @return the path ids from id "from" to id "to" including initial and final nodes.
+ */
+std::vector<int> RTABMAP_EXP computePath(
+			const std::map<int, rtabmap::Transform> & poses,
+			const std::multimap<int, int> & links,
+			int from,
+			int to);
+
+int RTABMAP_EXP findNearestNode(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose);
+
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		int maxNearestNeighbors,
+		float radius);
+
+} /* namespace graph */
+
+} /* namespace rtabmap */
+#endif /* GRAPH_H_ */

corelib/include/rtabmap/core/Link.h

@@ -35,7 +35,7 @@ namespace rtabmap {
 class Link
 {
 public:
-	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kUndef};
+	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
 	Link() :
 		from_(0),
 		to_(0),

corelib/include/rtabmap/core/Memory.h

@@ -80,8 +80,9 @@ public:
 	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
+	bool addLink(int to, int from, const Transform & transform, Link::Type type, float variance);
-	void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
+	void updateLink(int fromId, int toId, const Transform & transform, float variance);
+	void removeAllVirtualLinks();
 	std::map<int, int> getNeighborsId(int signatureId,
 			int margin,
 			int maxCheckedInDatabase = -1,
@@ -89,7 +90,7 @@ public:
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
-	void rejectLoopClosure(int oldId, int newId);
+	void removeLink(int idA, int idB);
 
 	//getters
 	const std::set<int> & getWorkingMem() const {return _workingMem;}
@@ -103,6 +104,7 @@ public:
 	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
 	bool isRawDataKept() const {return _rawDataKept;}
+	bool isBinDataKept() const {return _binDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getLastSignatureId() const;
@@ -183,7 +185,6 @@ private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const SensorData & data,
-			bool keepRawData=false,
 			Statistics * stats = 0);
 
 	//keypoint stuff
@@ -199,6 +200,7 @@ private:
 	// parameters
 	float _similarityThreshold;
 	bool _rawDataKept;
+	bool _binDataKept;
 	bool _keepRehearsedNodesInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;
@@ -212,7 +214,8 @@ private:
 	Signature * _lastSignature;
 	int _lastGlobalLoopClosureParentId;
 	int _lastGlobalLoopClosureChildId;
-	bool _memoryChanged; // False by default, become true when Memory::update() is called.
+	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
+	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	bool _postInitClosingEvents;
 

corelib/include/rtabmap/core/Parameters.h

@@ -175,7 +175,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");
 	RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,   	 bool, true, "Add column header description to log files.");
-	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.")
+	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
@@ -183,7 +183,8 @@ class RTABMAP_EXP Parameters
 
 	// Memory
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
-	RTABMAP_PARAM(Mem, ImageKept, 		        bool, true, 	"Keep images in db.");
+	RTABMAP_PARAM(Mem, ImageKept, 		        bool, false, 	"Keep raw images in RAM.");
+	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
 	RTABMAP_PARAM(Mem, RehearsedNodesKept, 	    bool, true, 	"Keep rehearsed ndoes in db.");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, othwersize it is Localization mode.");
@@ -285,6 +286,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations");
 	RTABMAP_PARAM(RGBD, ToroIgnoreVariance,   bool, false,    "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 1.0, "Goal reached radius (m).");
+	RTABMAP_PARAM(RGBD, MaxAnticipatedNodes,  unsigned int, 10, "Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by \"Rtabmap/MaxRetrieved\").");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -71,10 +71,11 @@ public:
 	void close();
 
 	const std::string & getWorkingDir() const {return _wDir;}
-	int getLoopClosureId() const;
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
-	int getRetrievedId() const;
+	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
 	int getLastLocationId() const;
-	float getLcHypValue() const {return _lcHypothesisValue;}
 	std::list<int> getWM() const; // working memory
 	std::set<int> getSTM() const; // short-term memory
 	int getWMSize() const; // working memory size
@@ -90,6 +91,7 @@ public:
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
+	float getGoalReachedRadius() const {return _goalReachedRadius;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -115,8 +117,17 @@ public:
 			std::map<int, int> & mapIds,
 			bool optimized,
 			bool global);
+	void clearPath();
+	std::list<std::pair<int, Transform> > computePath(int targetNode, bool global);
+	std::list<std::pair<int, Transform> > computePath(const Transform & targetPose, bool global);
+	const std::vector<int> & getPath() const {return _path;}
+	std::list<std::pair<int, Transform> > getPathNextPoses() const;
+	std::vector<int> getPathNextNodes() const;
+	int getPathCurrentGoalId() const;
+	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
+	std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
+	std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
@@ -126,6 +137,8 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	void updateGoalIndex();
+	bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -166,10 +179,11 @@ private:
 	int _reextractFeatureType;
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
+	float _goalReachedRadius; // meters
+	unsigned int _maxAnticipatedNodes;
 
-	int _lcHypothesisId;
+	std::pair<int, float> _loopClosureHypothesis;
-	float _lcHypothesisValue;
+	std::pair<int, float> _highestHypothesis;
-	int _retrievedId;
 	double _lastProcessTime;
 
 	// Abstract classes containing all loop closure
@@ -193,6 +207,13 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _mapTransform; // for localization mode
+
+	// Planning stuff
+	std::vector<int> _path;
+	unsigned int _pathCurrentIndex;
+	unsigned int _pathGoalIndex;
+	Transform _pathTransformToGoal;
+
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/Signature.h

@@ -89,6 +89,7 @@ public:
 
 	void removeLinks();
 	void removeLink(int idTo);
+	void removeVirtualLinks();
 
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}

corelib/include/rtabmap/core/Statistics.h

@@ -140,6 +140,7 @@ public:
 	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 setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -158,6 +159,7 @@ public:
 	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::vector<int> & localPath() const {return _localPath;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -184,6 +186,8 @@ private:
 	std::map<int, float> _likelihood;
 	std::map<int, float> _rawLikelihood;
 
+	std::vector<int> _localPath;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/include/rtabmap/core/Transform.h

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <vector>
 #include <string>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
 
 namespace rtabmap {
 
@@ -89,6 +91,8 @@ public:
 	void getTranslation(float & x, float & y, float & z) const;
 	float getNorm() const;
 	float getNormSquared() const;
+	float getDistance(const Transform & t) const;
+	float getDistanceSquared(const Transform & t) const;
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;
@@ -96,10 +100,17 @@ public:
 	bool operator==(const Transform & t) const;
 	bool operator!=(const Transform & t) const;
 
-	static Transform getIdentity()
+	Eigen::Matrix4f toEigen4f() const;
-	{
+	Eigen::Matrix4d toEigen4d() const;
-		return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
+	Eigen::Affine3f toEigen3f() const;
-	}
+	Eigen::Affine3d toEigen3d() const;
+
+public:
+	static Transform getIdentity();
+	static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
+	static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
+	static Transform fromEigen3f(const Eigen::Affine3f & matrix);
+	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 
 private:
 	std::vector<float> data_;

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -127,7 +127,7 @@ typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
 	typedef typename pcl::PointCloud<PointT> PointCloud;
 	typedef typename PointCloud::Ptr PointCloudPtr;
 	PointCloudPtr output(new PointCloud);
-	pcl::transformPointCloud<PointT>(*cloud, *output, transformToEigen4f(transform));
+	pcl::transformPointCloud<PointT>(*cloud, *output, transform.toEigen4f());
 	return output;
 }
 
@@ -136,7 +136,7 @@ PointT transformPoint(
 		const PointT & pt,
 		const Transform & transform)
 {
-	return pcl::transformPoint(pt, transformToEigen3f(transform));
+	return pcl::transformPoint(pt, transform.toEigen3f());
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/util3d.h

@@ -49,41 +49,6 @@ namespace rtabmap
 namespace util3d
 {
 
-/**
- * Compress image or data
- *
- * Example compression:
- *   cv::Mat image;// an image
- *   CompressionThread ct(image);
- *   ct.start();
- *   ct.join();
- *   std::vector<unsigned char> bytes = ct.getCompressedData();
- *
- * Example uncompression
- *   std::vector<unsigned char> bytes;// a compressed image
- *   CompressionThread ct(bytes);
- *   ct.start();
- *   ct.join();
- *   cv::Mat image = ct.getUncompressedData();
- */
-class RTABMAP_EXP CompressionThread : public UThread
-{
-public:
-	// format : ".png" ".jpg" "" (empty is general)
-	CompressionThread(const cv::Mat & mat, const std::string & format = "");
-	CompressionThread(const cv::Mat & bytes, bool isImage);
-	const cv::Mat & getCompressedData() const {return compressedData_;}
-	cv::Mat & getUncompressedData() {return uncompressedData_;}
-protected:
-	virtual void mainLoop();
-private:
-	cv::Mat compressedData_;
-	cv::Mat uncompressedData_;
-	std::string format_;
-	bool image_;
-	bool compressMode_;
-};
-
 cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
 cv::Mat RTABMAP_EXP depthFromCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
@@ -235,19 +200,6 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
 
-std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
-cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
-
-cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
-
-std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
-cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
-
-cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
-
 // remove depth by z axis
 void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
 									  const std::multimap<int, pcl::PointXYZ> & words2,
@@ -374,61 +326,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		int samples,
 		const Transform & transform = Transform::getIdentity());
 
-inline Eigen::Matrix4f transformToEigen4f(const Transform & transform)
-{
-	Eigen::Matrix4f m;
-	m << transform[0], transform[1], transform[2], transform[3],
-		 transform[4], transform[5], transform[6], transform[7],
-		 transform[8], transform[9], transform[10], transform[11],
-		 0,0,0,1;
-	return m;
-}
-inline Eigen::Matrix4d transformToEigen4d(const Transform & transform)
-{
-	Eigen::Matrix4d m;
-	m << transform[0], transform[1], transform[2], transform[3],
-		 transform[4], transform[5], transform[6], transform[7],
-		 transform[8], transform[9], transform[10], transform[11],
-		 0,0,0,1;
-	return m;
-}
-
-inline Eigen::Affine3f transformToEigen3f(const Transform & transform)
-{
-	return Eigen::Affine3f(transformToEigen4f(transform));
-}
-
-inline Eigen::Affine3d transformToEigen3d(const Transform & transform)
-{
-	return Eigen::Affine3d(transformToEigen4d(transform));
-}
-
-inline Transform transformFromEigen4f(const Eigen::Matrix4f & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-inline Transform transformFromEigen4d(const Eigen::Matrix4d & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-
-inline Transform transformFromEigen3f(const Eigen::Affine3f & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-inline Transform transformFromEigen3d(const Eigen::Affine3d & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
@@ -449,56 +346,6 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumAngle = 2*M_PI/3,
 		bool gp3NormalConsistency = false);
 
-std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
-		std::multimap<int, Link> & links,
-		int from,
-		int to);
-
-// <int, depth> depth=0 means infinite depth
-std::map<int, int> RTABMAP_EXP generateDepthGraph(
-		const std::multimap<int, Link> & links,
-		int fromId,
-		int depth = 0);
-
-void RTABMAP_EXP optimizeTOROGraph(
-		const std::map<int, int> & depthGraph,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations = 100,
-		bool toroInitialGuess = true,
-		bool ignoreCovariance = false,
-		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
-
-void RTABMAP_EXP optimizeTOROGraph(
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations = 100,
-		bool toroInitialGuess = true,
-		bool ignoreCovariance = false,
-		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
-
-bool RTABMAP_EXP saveTOROGraph(
-		const std::string & fileName,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints);
-
-bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
-		std::map<int, Transform> & poses,
-		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
-
-std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
-		const std::map<int, Transform> & poses,
-		float radius,
-		float angle,
-		bool keepLatest = true);
-
-std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
-		const std::map<int, Transform> & poses,
-		float radius,
-		float angle);
-
 bool RTABMAP_EXP occupancy2DFromCloud3D(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		cv::Mat & ground,

corelib/src/BayesFilter.cpp

@@ -75,7 +75,7 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
 		bool valid = true;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			tmpValues[i] = std::atof((*iter).c_str());
+			tmpValues[i] = uStr2Float((*iter).c_str());
 			//UINFO("%d=%e", i, tmpValues[i]);
 			if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
 			{

corelib/src/CMakeLists.txt

@@ -27,6 +27,8 @@ SET(SRC_FILES
 	util3d.cpp
 	Odometry.cpp
 	SensorData.cpp
+	Graph.cpp
+	Compression.cpp
 	
 	toro3d/posegraph3.cpp
 	toro3d/treeoptimizer3_iteration.cpp

corelib/src/Camera.cpp

@@ -74,6 +74,79 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
 	height = _imageHeight;
 }
 
+void Camera::setCalibration(const std::string & fileName)
+{
+	if(UFile::getExtension(fileName).compare("yaml") == 0)
+	{
+		cv::FileStorage fs;
+		fs.open(fileName, cv::FileStorage::READ);
+
+		if (!fs.isOpened())
+		{
+			UERROR("Failed to open file \"%s\"", fileName.c_str());
+			return;
+		}
+
+		cv::Mat k,d;
+
+		cv::FileNode n = fs["camera_matrix"];
+		int rows = n["rows"];
+		int cols = n["cols"];
+		std::vector<double> data;
+		n["data"] >> data;
+		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
+		{
+			k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+		}
+
+		cv::FileNode nd = fs["distortion_coefficients"];
+		rows = nd["rows"];
+		cols = nd["cols"];
+		data.clear();
+		nd["data"] >> data;
+		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
+		{
+			d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+		}
+
+		if(k.empty())
+		{
+			UERROR("Failed to load \"camera_matrix\" matrix.");
+		}
+		if(d.empty())
+		{
+			UERROR("Failed to load \"distortion_coefficients\" matrix.");
+		}
+		if(!k.empty() && !d.empty())
+		{
+			this->setCalibration(k, d);
+		}
+	}
+	else
+	{
+		UERROR("Calibration file must be in \"*.yaml\" format");
+	}
+}
+
+void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients)
+{
+	UASSERT(cameraMatrix.type() == CV_64FC1 &&
+			cameraMatrix.rows == 3 &&
+			cameraMatrix.cols == 3);
+	UASSERT(distorsionCoefficients.type() == CV_64FC1 &&
+			distorsionCoefficients.rows ==1 &&
+			(distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8));
+
+	_k = cameraMatrix;
+	_d = distorsionCoefficients;
+}
+
+void Camera::resetCalibration()
+{
+	_k = cv::Mat();
+	_d = cv::Mat();
+}
+
 cv::Mat Camera::takeImage()
 {
 	cv::Mat img;
@@ -99,6 +172,11 @@ cv::Mat Camera::takeImage()
 
 	UTimer timer;
 	img = this->captureImage();
+	if(!img.empty() && !_k.empty() && !_d.empty())
+	{
+		cv::Mat temp = img.clone();
+		cv::undistort(temp, img, _k, _d);
+	}
 	UDEBUG("Time capturing image = %fs", timer.ticks());
 	return img;
 }

corelib/src/Compression.cpp

@@ -0,0 +1,247 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Compression.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/opencv.hpp>
+
+#include <zlib.h>
+
+namespace rtabmap {
+
+// format : ".png" ".jpg" "" (empty is general)
+CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
+	uncompressedData_(mat),
+	format_(format),
+	image_(!format.empty()),
+	compressMode_(true)
+{
+	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
+}
+// assume image
+CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
+	compressedData_(bytes),
+	image_(isImage),
+	compressMode_(false)
+{}
+void CompressionThread::mainLoop()
+{
+	if(compressMode_)
+	{
+		if(!uncompressedData_.empty())
+		{
+			if(image_)
+			{
+				compressedData_ = compressImage2(uncompressedData_, format_);
+			}
+			else
+			{
+				compressedData_ = compressData2(uncompressedData_);
+			}
+		}
+	}
+	else // uncompress
+	{
+		if(!compressedData_.empty())
+		{
+			if(image_)
+			{
+				uncompressedData_ = uncompressImage(compressedData_);
+			}
+			else
+			{
+				uncompressedData_ = uncompressData(compressedData_);
+			}
+		}
+	}
+	this->kill();
+}
+
+// ".png" or ".jpg"
+std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
+{
+	std::vector<unsigned char> bytes;
+	if(!image.empty())
+	{
+		cv::imencode(format, image, bytes);
+	}
+	return bytes;
+}
+
+// ".png" or ".jpg"
+cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
+{
+	std::vector<unsigned char> bytes = compressImage(image, format);
+	if(bytes.size())
+	{
+		return cv::Mat(1, bytes.size(), CV_8UC1, bytes.data()).clone();
+	}
+	return cv::Mat();
+}
+
+cv::Mat uncompressImage(const cv::Mat & bytes)
+{
+	 cv::Mat image;
+	if(!bytes.empty())
+	{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+		image = cv::imdecode(bytes, -1);
+#endif
+	}
+	return image;
+}
+
+cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
+{
+	 cv::Mat image;
+	if(bytes.size())
+	{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+		image = cv::imdecode(bytes, -1);
+#endif
+	}
+	return image;
+}
+
+std::vector<unsigned char> compressData(const cv::Mat & data)
+{
+	std::vector<unsigned char> bytes;
+	if(!data.empty())
+	{
+		uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
+		uLong destLen = compressBound(sourceLen);
+		bytes.resize(destLen);
+		int errCode = compress(
+						(Bytef *)bytes.data(),
+						&destLen,
+						(const Bytef *)data.data,
+						sourceLen);
+
+		bytes.resize(destLen+3*sizeof(int));
+		*((int*)&bytes[destLen]) = data.rows;
+		*((int*)&bytes[destLen+sizeof(int)]) = data.cols;
+		*((int*)&bytes[destLen+2*sizeof(int)]) = data.type();
+
+		if(errCode == Z_MEM_ERROR)
+		{
+			UERROR("Z_MEM_ERROR : Insufficient memory.");
+		}
+		else if(errCode == Z_BUF_ERROR)
+		{
+			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
+		}
+	}
+	return bytes;
+}
+
+cv::Mat compressData2(const cv::Mat & data)
+{
+	cv::Mat bytes;
+	if(!data.empty())
+	{
+		uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
+		uLong destLen = compressBound(sourceLen);
+		bytes = cv::Mat(1, destLen+3*sizeof(int), CV_8UC1);
+		int errCode = compress(
+						(Bytef *)bytes.data,
+						&destLen,
+						(const Bytef *)data.data,
+						sourceLen);
+		bytes = cv::Mat(bytes, cv::Rect(0,0, destLen+3*sizeof(int), 1));
+		*((int*)&bytes.data[destLen]) = data.rows;
+		*((int*)&bytes.data[destLen+sizeof(int)]) = data.cols;
+		*((int*)&bytes.data[destLen+2*sizeof(int)]) = data.type();
+
+		if(errCode == Z_MEM_ERROR)
+		{
+			UERROR("Z_MEM_ERROR : Insufficient memory.");
+		}
+		else if(errCode == Z_BUF_ERROR)
+		{
+			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
+		}
+	}
+	return bytes;
+}
+
+cv::Mat uncompressData(const cv::Mat & bytes)
+{
+	UASSERT(bytes.empty() || bytes.type() == CV_8UC1);
+	return uncompressData(bytes.data, bytes.cols*bytes.rows);
+}
+
+cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
+{
+	return uncompressData(bytes.data(), bytes.size());
+}
+
+cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
+{
+	cv::Mat data;
+	if(bytes && size>=3*sizeof(int))
+	{
+		//last 3 int elements are matrix size and type
+		int height = *((int*)&bytes[size-3*sizeof(int)]);
+		int width = *((int*)&bytes[size-2*sizeof(int)]);
+		int type = *((int*)&bytes[size-1*sizeof(int)]);
+
+		// If the size is higher, it may be a wrong data format.
+		UASSERT_MSG(height>=0 && height<10000 &&
+				    width>=0 && width<10000,
+				    uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
+
+		data = cv::Mat(height, width, type);
+		uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
+
+		int errCode = uncompress(
+						(Bytef*)data.data,
+						&totalUncompressed,
+						(const Bytef*)bytes,
+						uLong(size));
+
+		if(errCode == Z_MEM_ERROR)
+		{
+			UERROR("Z_MEM_ERROR : Insufficient memory.");
+		}
+		else if(errCode == Z_BUF_ERROR)
+		{
+			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
+		}
+		else if(errCode == Z_DATA_ERROR)
+		{
+			UERROR("Z_DATA_ERROR : The compressed data (referenced by source) was corrupted.");
+		}
+	}
+	return data;
+}
+
+} /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -38,7 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 DBDriver::DBDriver(const ParametersMap & parameters) :
-	_emptyTrashesTime(0)
+	_emptyTrashesTime(0),
+	_timestampUpdate(true)
 {
 	this->parseParameters(parameters);
 }
@@ -242,7 +243,7 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 
 		if(toUpdate.size())
 		{
-			this->updateQuery(toUpdate);
+			this->updateQuery(toUpdate, _timestampUpdate);
 		}
 		if(toSave.size())
 		{
@@ -272,7 +273,7 @@ void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 
 		if(toUpdate.size())
 		{
-			this->updateQuery(toUpdate);
+			this->updateQuery(toUpdate, _timestampUpdate);
 		}
 		if(toSave.size())
 		{

corelib/src/DBDriverSqlite3.cpp

@@ -340,7 +340,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	}
 	if(rc != SQLITE_OK)
 	{
-		UFATAL("DB error : %s", sqlite3_errmsg(_ppDb));
+		UFATAL("DB error : %s (path=\"%s\")", sqlite3_errmsg(_ppDb), url.c_str());
 		_ppDb = 0;
 		return false;
 	}
@@ -1548,7 +1548,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 }
 
 
-void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
+void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
 {
 	UDEBUG("nodes = %d", nodes.size());
 	if(_ppDb && nodes.size())
@@ -1559,7 +1559,15 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 		sqlite3_stmt * ppStmt = 0;
 		Signature * s = 0;
 
-		std::string query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
+		std::string query;
+		if(updateTimestamp)
+		{
+			query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
+		}
+		else
+		{
+			query = "UPDATE Node SET weight=? WHERE id=?;";
+		}
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
@@ -1655,10 +1663,11 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 	}
 }
 
-void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words) const
+void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
 {
-	if(_ppDb && words.size())
+	if(_ppDb && words.size() && updateTimestamp)
 	{
+		// Only timestamp update is done here, so don't enter this if at all if false
 		UTimer timer;
 		timer.start();
 		int rc = SQLITE_OK;
@@ -2027,13 +2036,21 @@ std::string DBDriverSqlite3::queryStepLink() const
 		return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
 	}
 }
-void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const
+void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const
 {
 	if(!ppStmt)
 	{
 		UFATAL("");
 	}
 	UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type);
+
+	// Don't save virtual links
+	if(type==Link::kVirtualClosure)
+	{
+		UDEBUG("Virtual link ignored....");
+		return;
+	}
+
 	int rc = SQLITE_OK;
 	int index = 1;
 	rc = sqlite3_bind_int(ppStmt, index++, fromId);

corelib/src/DBDriverSqlite3.h

@@ -60,8 +60,8 @@ private:
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
@@ -109,7 +109,7 @@ private:
 			float cx,
 			float cy,
 			const Transform & localTransform) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const;
+	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 

corelib/src/DBReader.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Compression.h"
 
 namespace rtabmap {
 
@@ -212,9 +213,9 @@ SensorData DBReader::getNextData()
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
 
-			util3d::CompressionThread ctImage(imageBytes, true);
+			rtabmap::CompressionThread ctImage(imageBytes, true);
-			util3d::CompressionThread ctDepth(depthBytes, true);
+			rtabmap::CompressionThread ctDepth(depthBytes, true);
-			util3d::CompressionThread ctLaserScan(laserScanBytes, false);
+			rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
 			ctImage.start();
 			ctDepth.start();
 			ctLaserScan.start();

corelib/src/Features2d.cpp

@@ -251,7 +251,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRat
 		unsigned int i=0;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			values[i] = std::atof((*iter).c_str());
+			values[i] = uStr2Float(*iter);
 			++i;
 		}
 

corelib/src/Graph.cpp

@@ -0,0 +1,839 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "rtabmap/core/Graph.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/common/eigen.h>
+#include <pcl/common/common.h>
+#include <set>
+#include <queue>
+#include "toro3d/treeoptimizer3.hh"
+
+namespace rtabmap {
+
+namespace graph {
+
+std::multimap<int, Link>::iterator findLink(
+		std::multimap<int, Link> & links,
+		int from,
+		int to)
+{
+	std::multimap<int, Link>::iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second.to() == to)
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	// let's try to -> from
+	iter = links.find(to);
+	while(iter != links.end() && iter->first == to)
+	{
+		if(iter->second.to() == from)
+		{
+			return iter;
+		}
+		++iter;
+	}
+	return links.end();
+}
+
+
+// <int, depth> margin=0 means infinite margin
+std::map<int, int> generateDepthGraph(
+		const std::multimap<int, Link> & links,
+		int fromId,
+		int depth)
+{
+	UASSERT(depth >= 0);
+	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
+	std::map<int, int> ids;
+	if(fromId<=0)
+	{
+		return ids;
+	}
+
+	std::list<int> curentDepthList;
+	std::set<int> nextDepth;
+	nextDepth.insert(fromId);
+	int d = 0;
+	while((depth == 0 || d < depth) && nextDepth.size())
+	{
+		curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
+		nextDepth.clear();
+
+		for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
+		{
+			if(ids.find(*jter) == ids.end())
+			{
+				std::set<int> marginIds;
+
+				ids.insert(std::pair<int, int>(*jter, d));
+
+				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->second.from() == *jter)
+					{
+						marginIds.insert(iter->second.to());
+					}
+					else if(iter->second.to() == *jter)
+					{
+						marginIds.insert(iter->second.from());
+					}
+				}
+
+				// Margin links
+				for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
+				{
+					if( !uContains(ids, *iter) && nextDepth.find(*iter) == nextDepth.end())
+					{
+						nextDepth.insert(*iter);
+					}
+				}
+			}
+		}
+		++d;
+	}
+	return ids;
+}
+
+void optimizeTOROGraph(
+		const std::map<int, int> & depthGraph,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		std::map<int, Transform> & optimizedPoses,
+		int toroIterations,
+		bool toroInitialGuess,
+		bool ignoreCovariance,
+		std::list<std::map<int, Transform> > * intermediateGraphes)
+{
+	optimizedPoses.clear();
+	if(depthGraph.size() && poses.size()>=2 && links.size()>=1)
+	{
+		// Modify IDs using the margin from the current signature (TORO root will be the last signature)
+		int m = 0;
+		int toroId = 1;
+		std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
+		std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
+		std::map<int, int> idsTmp = depthGraph;
+		while(idsTmp.size())
+		{
+			for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
+			{
+				if(m == iter->second)
+				{
+					rtabmapToToro.insert(std::make_pair(iter->first, toroId));
+					toroToRtabmap.insert(std::make_pair(toroId, iter->first));
+					++toroId;
+					idsTmp.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+			++m;
+		}
+
+		std::map<int, rtabmap::Transform> posesToro;
+		std::multimap<int, rtabmap::Link> edgeConstraintsToro;
+		for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(uContains(depthGraph, iter->first))
+			{
+				UASSERT_MSG(!iter->second.isNull(), uFormat("Poses should not be null! Id=%d", iter->first).c_str());
+				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
+			}
+		}
+		for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
+			iter!=links.end();
+			++iter)
+		{
+			if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
+			{
+				UASSERT(!iter->second.transform().isNull());
+				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
+			}
+		}
+
+		std::map<int, rtabmap::Transform> optimizedPosesToro;
+
+		if(posesToro.size() && edgeConstraintsToro.size())
+		{
+			std::list<std::map<int, rtabmap::Transform> > graphesToro;
+
+			// Optimize!
+			optimizeTOROGraph(
+					posesToro,
+					edgeConstraintsToro,
+					optimizedPosesToro,
+					toroIterations,
+					toroInitialGuess,
+					ignoreCovariance,
+					&graphesToro);
+
+			for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
+			{
+				optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
+			}
+
+			if(intermediateGraphes)
+			{
+				for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
+				{
+					std::map<int, rtabmap::Transform> tmp;
+					for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
+					{
+						tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
+					}
+					intermediateGraphes->push_back(tmp);
+				}
+			}
+		}
+		else
+		{
+			UERROR("No TORO poses and constraints!?");
+		}
+	}
+	else if(links.size() == 0 && poses.size() == 1)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UERROR("Wrong inputs! depthGraph=%d poses=%d links=%d",
+				(int)depthGraph.size(), (int)poses.size(), (int)links.size());
+	}
+}
+
+//On success, optimizedPoses is cleared and new poses are inserted in
+void optimizeTOROGraph(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		std::map<int, Transform> & optimizedPoses,
+		int toroIterations,
+		bool toroInitialGuess,
+		bool ignoreCovariance,
+		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
+{
+	UASSERT(toroIterations>0);
+	optimizedPoses.clear();
+	if(edgeConstraints.size()>=1 && poses.size()>=2)
+	{
+		// Apply TORO optimization
+		AISNavigation::TreeOptimizer3 pg;
+		pg.verboseLevel = 0;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			float x,y,z, roll,pitch,yaw;
+			UASSERT(!iter->second.isNull());
+			pcl::getTranslationAndEulerAngles(iter->second.toEigen3f(), x,y,z, roll,pitch,yaw);
+			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
+			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v = pg.addVertex(iter->first, p);
+			if (v)
+			{
+				v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
+			}
+			else
+			{
+				UERROR("cannot insert vertex %d!?", iter->first);
+			}
+		}
+
+		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			int id1 = iter->first;
+			int id2 = iter->second.to();
+			float x,y,z, roll,pitch,yaw;
+			UASSERT(!iter->second.transform().isNull());
+			pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll,pitch,yaw);
+			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
+			AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
+			if(!ignoreCovariance && iter->second.variance()>0)
+			{
+				inf[0][0] = 1.0f/iter->second.variance(); // x
+				inf[1][1] = 1.0f/iter->second.variance(); // y
+				inf[2][2] = 1.0f/iter->second.variance(); // z
+				inf[3][3] = 1.0f/iter->second.variance(); // roll
+				inf[4][4] = 1.0f/iter->second.variance(); // pitch
+				inf[5][5] = 1.0f/iter->second.variance(); // yaw
+			}
+
+			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
+			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
+			AISNavigation::TreePoseGraph3::Transformation t(p);
+			if (!pg.addEdge(v1, v2, t, inf))
+			{
+				UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
+				return;
+			}
+		}
+		pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
+
+		UDEBUG("Initial guess...");
+		if(toroInitialGuess)
+		{
+			pg.initializeOnTree(); // optional
+		}
+
+		pg.initializeTreeParameters();
+		UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
+			   "TORO is not able to find the root of the graph!)");
+		pg.initializeOptimization();
+
+		UDEBUG("TORO iterate begin (iterations=%d)", toroIterations);
+		for (int i=0; i<toroIterations; i++)
+		{
+			if(intermediateGraphes && (toroInitialGuess || i>0))
+			{
+				std::map<int, Transform> tmpPoses;
+				for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+				{
+					AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
+					v->pose=v->transformation.toPoseType();
+					Transform newPose = Transform::fromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
+
+					tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
+				}
+				intermediateGraphes->push_back(tmpPoses);
+			}
+
+			pg.iterate();
+		}
+		UDEBUG("TORO iterate end");
+
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
+			v->pose=v->transformation.toPoseType();
+			Transform newPose = Transform::fromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
+
+			optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
+		}
+
+		//Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
+		//Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
+		//Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
+		//Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
+		//mapCorrection = transformFromEigen4f(result);
+	}
+	else if(edgeConstraints.size() == 0 && poses.size() == 1)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UWARN("This method should be called at least with 1 pose!");
+	}
+}
+
+bool saveTOROGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints)
+{
+	FILE * file = 0;
+
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "w");
+#else
+	file = fopen(fileName.c_str(), "w");
+#endif
+
+	if(file)
+	{
+		// VERTEX3 id x y z phi theta psi
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			float x,y,z, yaw,pitch,roll;
+			pcl::getTranslationAndEulerAngles(iter->second.toEigen3f(), x,y,z, roll, pitch, yaw);
+			fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
+					iter->first,
+					x,
+					y,
+					z,
+					roll,
+					pitch,
+					yaw);
+		}
+
+		//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
+		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			float x,y,z, yaw,pitch,roll;
+			pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw);
+			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
+					iter->first,
+					iter->second.to(),
+					x,
+					y,
+					z,
+					roll,
+					pitch,
+					yaw,
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance());
+		}
+		UINFO("Graph saved to %s", fileName.c_str());
+		fclose(file);
+	}
+	else
+	{
+		UERROR("Cannot save to file %s", fileName.c_str());
+		return false;
+	}
+	return true;
+}
+
+bool loadTOROGraph(const std::string & fileName,
+		std::map<int, Transform> & poses,
+		std::multimap<int, std::pair<int, Transform> > & edgeConstraints)
+{
+	FILE * file = 0;
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "r");
+#else
+	file = fopen(fileName.c_str(), "r");
+#endif
+
+	if(file)
+	{
+		char line[200];
+		while ( fgets (line , 200 , file) != NULL )
+		{
+			std::vector<std::string> strList = uListToVector(uSplit(line, ' '));
+			if(strList.size() == 8)
+			{
+				//VERTEX3
+				int id = atoi(strList[1].c_str());
+				float x = uStr2Float(strList[2]);
+				float y = uStr2Float(strList[3]);
+				float z = uStr2Float(strList[4]);
+				float roll = uStr2Float(strList[5]);
+				float pitch = uStr2Float(strList[6]);
+				float yaw = uStr2Float(strList[7]);
+				Transform pose = Transform::fromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
+				std::map<int, Transform>::iterator iter = poses.find(id);
+				if(iter != poses.end())
+				{
+					iter->second = pose;
+				}
+				else
+				{
+					UFATAL("");
+				}
+			}
+			else if(strList.size() == 30)
+			{
+				//EDGE3
+				int idFrom = atoi(strList[1].c_str());
+				int idTo = atoi(strList[2].c_str());
+				float x = uStr2Float(strList[3]);
+				float y = uStr2Float(strList[4]);
+				float z = uStr2Float(strList[5]);
+				float roll = uStr2Float(strList[6]);
+				float pitch = uStr2Float(strList[7]);
+				float yaw = uStr2Float(strList[8]);
+				Transform transform = Transform::fromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
+				if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
+				{
+					std::pair<int, Transform> edge(idTo, transform);
+					edgeConstraints.insert(std::pair<int, std::pair<int, Transform> >(idFrom, edge));
+				}
+				else
+				{
+					UFATAL("");
+				}
+			}
+			else
+			{
+				UFATAL("Error parsing map file %s", fileName.c_str());
+			}
+		}
+
+		UINFO("Graph loaded from %s", fileName.c_str());
+		fclose(file);
+	}
+	else
+	{
+		UERROR("Cannot open file %s", fileName.c_str());
+		return false;
+	}
+	return true;
+}
+
+
+std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
+{
+	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		cloud->resize(poses.size());
+		int i=0;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		}
+
+		// radius filtering
+		std::vector<int> names = uKeys(poses);
+		std::vector<Transform> transforms = uValues(poses);
+
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
+		tree->setInputCloud(cloud);
+		std::set<int> indicesChecked;
+		std::set<int> indicesKept;
+
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			// ignore scans
+			if(indicesChecked.find(i) == indicesChecked.end())
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kDistances;
+				tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
+
+				std::set<int> cloudIndices;
+				const Transform & currentT = transforms.at(i);
+				Eigen::Vector3f vA = currentT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				for(unsigned int j=0; j<kIndices.size(); ++j)
+				{
+					if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
+					{
+						const Transform & checkT = transforms.at(kIndices[j]);
+						// same orientation?
+						Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+						if(a <= angle)
+						{
+							cloudIndices.insert(kIndices[j]);
+						}
+					}
+				}
+
+				if(keepLatest)
+				{
+					bool lastAdded = false;
+					for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
+					{
+						if(!lastAdded)
+						{
+							indicesKept.insert(*iter);
+							lastAdded = true;
+						}
+						indicesChecked.insert(*iter);
+					}
+				}
+				else
+				{
+					bool firstAdded = false;
+					for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
+					{
+						if(!firstAdded)
+						{
+							indicesKept.insert(*iter);
+							firstAdded = true;
+						}
+						indicesChecked.insert(*iter);
+					}
+				}
+			}
+		}
+
+		//pcl::IndicesPtr indicesOut(new std::vector<int>);
+		//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
+		UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
+		//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
+		//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
+
+		std::map<int, Transform> keptPoses;
+		for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
+		{
+			keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
+		}
+
+		return keptPoses;
+	}
+	else
+	{
+		return poses;
+	}
+}
+
+std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
+{
+	std::multimap<int, int> clusters;
+	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		cloud->resize(poses.size());
+		int i=0;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		}
+
+		// radius clustering (nearest neighbors)
+		std::vector<int> ids = uKeys(poses);
+		std::vector<Transform> transforms = uValues(poses);
+
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
+		tree->setInputCloud(cloud);
+
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
+
+			std::set<int> cloudIndices;
+			const Transform & currentT = transforms.at(i);
+			Eigen::Vector3f vA = currentT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+			for(unsigned int j=0; j<kIndices.size(); ++j)
+			{
+				if((int)i != kIndices[j])
+				{
+					const Transform & checkT = transforms.at(kIndices[j]);
+					// same orientation?
+					Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+					if(a <= angle)
+					{
+						clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
+					}
+				}
+			}
+		}
+	}
+	return clusters;
+}
+
+
+class Node
+{
+public:
+	Node(int id, int fromId, const rtabmap::Transform & pose) :
+		id_(id),
+		costSoFar_(0.0f),
+		distToEnd_(0.0f),
+		fromId_(fromId),
+		closed_(false),
+		pose_(pose)
+	{}
+
+	int id() const {return id_;}
+	int fromId() const {return fromId_;}
+	bool isClosed() const {return closed_;}
+	bool isOpened() const {return !closed_;}
+	float costSoFar() const {return costSoFar_;} // Dijkstra cost
+	float distToEnd() const {return distToEnd_;} // Breath-first cost
+	float totalCost() const {return costSoFar_ + distToEnd_;} // A* cost
+	rtabmap::Transform pose() const {return pose_;}
+	float distFrom(const rtabmap::Transform & pose) const
+	{
+		return pose_.getDistance(pose);
+	}
+
+	void setClosed(bool closed) {closed_ = closed;}
+	void setFromId(int fromId) {fromId_ = fromId;}
+	void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
+	void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
+
+private:
+	int id_;
+	float costSoFar_;
+	float distToEnd_;
+	int fromId_;
+	bool closed_;
+	rtabmap::Transform pose_;
+};
+
+typedef std::pair<int, float> Pair; // first is id, second is cost
+struct Order
+{
+    bool operator()(Pair const& a, Pair const& b) const
+    {
+        return a.second > b.second;
+    }
+};
+
+std::vector<int> computePath(
+			const std::map<int, rtabmap::Transform> & poses,
+			const std::multimap<int, int> & links,
+			int from,
+			int to)
+{
+	std::list<int> path;
+
+	//A*
+	int startNode = from;
+	int endNode = to;
+	rtabmap::Transform endPose = poses.at(endNode);
+	std::map<int, Node> nodes;
+	nodes.insert(std::make_pair(startNode, Node(startNode, 0, poses.at(startNode))));
+	std::priority_queue<Pair, std::vector<Pair>, Order> pq;
+	pq.push(Pair(startNode, 0));
+
+	while(pq.size())
+	{
+		Node & currentNode = nodes.find(pq.top().first)->second;
+		pq.pop();
+		currentNode.setClosed(true);
+
+		if(currentNode.id() == endNode)
+		{
+			while(currentNode.id()!=startNode)
+			{
+				path.push_front(currentNode.id());
+				currentNode = nodes.find(currentNode.fromId())->second;
+			}
+			path.push_front(startNode);
+			break;
+		}
+
+		// lookup neighbors
+		for(std::multimap<int, int>::const_iterator iter = links.find(currentNode.id());
+			iter!=links.end() && iter->first == currentNode.id();
+			++iter)
+		{
+			std::map<int, Node>::iterator nodeIter = nodes.find(iter->second);
+			if(nodeIter == nodes.end())
+			{
+				std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
+				UASSERT(poseIter != poses.end());
+				Node n(iter->second, currentNode.id(), poseIter->second);
+				n.setCostSoFar(currentNode.costSoFar() + currentNode.distFrom(poseIter->second));
+				n.setDistToEnd(n.distFrom(endPose));
+				nodes.insert(std::make_pair(iter->second, n));
+				pq.push(Pair(n.id(), n.totalCost()));
+			}
+			else if(nodeIter->second.isOpened())
+			{
+				float newCostSoFar = currentNode.costSoFar() + currentNode.distFrom(nodeIter->second.pose());
+				if(nodeIter->second.costSoFar() > newCostSoFar)
+				{
+					UWARN("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
+				}
+			}
+		}
+	}
+	return uListToVector(path);
+}
+
+int findNearestNode(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose)
+{
+	int id = 0;
+	if(nodes.size() && !targetPose.isNull())
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		cloud->resize(nodes.size());
+		std::vector<int> ids(nodes.size());
+		int oi = 0;
+		for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			ids[oi++] = iter->first;
+		}
+
+		std::map<int, float> foundNodes;
+		if(cloud->size())
+		{
+			pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+			kdTree->setInputCloud(cloud);
+			std::vector<int> ind;
+			std::vector<float> dist;
+			pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
+			kdTree->nearestKSearch(pt, 1, ind, dist);
+			if(ind.size() && dist.size() && ind[0] >= 0)
+			{
+				UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
+				id = ids[ind[0]];
+			}
+		}
+	}
+	return id;
+}
+
+// return <id, distance>, including query
+std::map<int, float> getNodesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		int maxNearestNeighbors,
+		float radius)
+{
+	UASSERT(uContains(nodes, nodeId));
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(nodes.size());
+	std::vector<int> ids(nodes.size());
+	int oi = 0;
+	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	{
+		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		ids[oi++] = iter->first;
+	}
+
+	Transform fromT = nodes.at(nodeId);
+
+	std::map<int, float> foundNodes;
+	if(cloud->size())
+	{
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+		kdTree->setInputCloud(cloud);
+		std::vector<int> ind;
+		std::vector<float> dist;
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
+		for(unsigned int i=0; i<ind.size(); ++i)
+		{
+			if(ind[i] >=0)
+			{
+				UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
+				foundNodes.insert(std::make_pair(ids[ind[i]], dist[i]));
+			}
+		}
+	}
+	UDEBUG("found nodes=%d", (int)foundNodes.size());
+	return foundNodes;
+}
+
+} /* namespace graph */
+
+} /* namespace rtabmap */

corelib/src/Memory.cpp

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "DBDriverSqlite3.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Compression.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
@@ -57,6 +58,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_dbDriver(0),
 	_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
 	_rawDataKept(Parameters::defaultMemImageKept()),
+	_binDataKept(Parameters::defaultMemBinDataKept()),
 	_keepRehearsedNodesInDb(Parameters::defaultMemRehearsedNodesKept()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
@@ -70,6 +72,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_lastGlobalLoopClosureParentId(0),
 	_lastGlobalLoopClosureChildId(0),
 	_memoryChanged(false),
+	_linksChanged(false),
 	_signaturesAdded(0),
 	_postInitClosingEvents(false),
 
@@ -129,7 +132,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
 	DBDriver * tmpDriver = 0;
-	if(!_memoryChanged)
+	if(!_memoryChanged && !_linksChanged)
 	{
 		if(_dbDriver)
 		{
@@ -137,6 +140,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			_dbDriver = 0; // HACK for the clear() below to think that there is no db
 		}
 	}
+	else if(!_memoryChanged && _linksChanged)
+	{
+		_dbDriver->setTimestampUpdateEnabled(false); // update links only
+	}
 	this->clear();
 	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory, done!"));
 
@@ -160,6 +167,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 	bool success = true;
 	if(_dbDriver)
 	{
+		_dbDriver->setTimestampUpdateEnabled(true); // make sure that timestamp update is enabled (may be disabled above)
 		success = false;
 		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + "..."));
 		if(_dbDriver->openConnection(dbUrl, dbOverwritten))
@@ -310,7 +318,7 @@ Memory::~Memory()
 {
 	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
 	UDEBUG("");
-	if(!_memoryChanged)
+	if(!_memoryChanged && !_linksChanged)
 	{
 		UDEBUG("");
 		if(_dbDriver)
@@ -329,6 +337,11 @@ Memory::~Memory()
 	{
 		UDEBUG("");
 		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
+		if(!_memoryChanged && _linksChanged && _dbDriver)
+		{
+			// don't update the time stamps!
+			_dbDriver->setTimestampUpdateEnabled(false);
+		}
 		this->clear();
 		if(_dbDriver)
 		{
@@ -363,6 +376,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
+	Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
 	Parameters::parse(parameters, Parameters::kMemRehearsedNodesKept(), _keepRehearsedNodesInDb);
 	Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
@@ -514,7 +528,7 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 	//============================================================
 	// Create a signature with the image received.
 	//============================================================
-	Signature * signature = this->createSignature(data, this->isRawDataKept(), stats);
+	Signature * signature = this->createSignature(data, stats);
 	if (signature == 0)
 	{
 		UERROR("Failed to create a signature...");
@@ -597,7 +611,7 @@ void Memory::setRoi(const std::string & roi)
 		unsigned int i=0;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			tmpValues[i] = std::atof((*iter).c_str());
+			tmpValues[i] = uStr2Float(*iter);
 			++i;
 		}
 
@@ -809,18 +823,6 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	{
 		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
 		nextMargin.clear();
-		// first pass: count number of node in current margin in database
-		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end();++jter)
-		{
-			if(!uContains(ids, *jter))
-			{
-				const Signature * s = this->getSignature(*jter);
-				if(!s)
-				{
-					++nbLoadedFromDb;
-				}
-			}
-		}
 
 		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
 		{
@@ -839,6 +841,7 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 				}
 				else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
 				{
+					++nbLoadedFromDb;
 					ids.insert(std::pair<int, int>(*jter, m));
 
 					UTimer timer;
@@ -869,12 +872,6 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 							{
 								if(currentMargin.insert(iter->first).second)
 								{
-									const Signature * s = this->getSignature(iter->first);
-									if(!s)
-									{
-										// update db count because it's on current margin
-										++nbLoadedFromDb;
-									}
 									curentMarginList.push_back(iter->first);
 								}
 							}
@@ -960,11 +957,14 @@ void Memory::clear()
 						_workingMem.size(), _stMem.size(), _signatures.size()).c_str());
 
 		UDEBUG("Adding statistics after run...");
-		_dbDriver->addStatisticsAfterRun(memSize,
+		if(_memoryChanged)
-				_lastSignature?_lastSignature->id():0,
+		{
-				UProcessInfo::getMemoryUsage(),
+			_dbDriver->addStatisticsAfterRun(memSize,
-				_dbDriver->getMemoryUsed(),
+					_lastSignature?_lastSignature->id():0,
-				(int)_vwd->getVisualWords().size());
+					UProcessInfo::getMemoryUsage(),
+					_dbDriver->getMemoryUsed(),
+					(int)_vwd->getVisualWords().size());
+		}
 	}
 	UDEBUG("");
 
@@ -1008,6 +1008,7 @@ void Memory::clear()
 	_idCount = kIdStart;
 	_idMapCount = kIdStart;
 	_memoryChanged = false;
+	_linksChanged = false;
 
 	if(_dbDriver)
 	{
@@ -1541,43 +1542,63 @@ void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 	}
 }
 
-void Memory::rejectLoopClosure(int oldId, int newId)
+void Memory::removeLink(int oldId, int newId)
 {
-	Signature * oldS = this->_getSignature(oldId);
+	//this method assumes receiving oldId < newId, if not switch them
-	Signature * newS = this->_getSignature(newId);
+	Signature * oldS = this->_getSignature(oldId<newId?oldId:newId);
+	Signature * newS = this->_getSignature(oldId<newId?newId:oldId);
 	if(oldS && newS)
 	{
-		UDEBUG("removing loop closure from location %d", newS->id());
+		UINFO("removing link between location %d and %d", oldS->id(), newS->id());
-		oldS->removeLink(newS->id());
-		oldS->setWeight(oldS->getWeight()+1);
 
-		newS->removeLink(oldS->id());
+		if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
-		newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
-
-		bool noChildrenAnymore = true;
-		for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
 		{
-			if(iter->second.type() > Link::kNeighbor && iter->first < newS->id())
+			Link::Type type = oldS->getLinks().at(newS->id()).type();
+			if(type == Link::kGlobalClosure && newS->getWeight() > 0)
 			{
-				noChildrenAnymore = false;
+				// adjust the weight
-				break;
+				oldS->setWeight(oldS->getWeight()+1);
+				newS->setWeight(newS->getWeight()>0?newS->getWeight()-1:0);
+			}
+
+
+			oldS->removeLink(newS->id());
+			newS->removeLink(oldS->id());
+
+			if(type!=Link::kVirtualClosure)
+			{
+				_linksChanged = true;
+			}
+
+			bool noChildrenAnymore = true;
+			for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
+			{
+				if(iter->second.type() > Link::kNeighbor && iter->first < newS->id())
+				{
+					noChildrenAnymore = false;
+					break;
+				}
+			}
+			if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureParentId)
+			{
+				_lastGlobalLoopClosureParentId = 0;
+				_lastGlobalLoopClosureChildId = 0;
 			}
 		}
-		if(noChildrenAnymore && newId == _lastGlobalLoopClosureParentId)
+		else
 		{
-			_lastGlobalLoopClosureParentId = 0;
+			UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
-			_lastGlobalLoopClosureChildId = 0;
 		}
 	}
 	else
 	{
 		if(!newS)
 		{
-			UERROR("Signature %d is not in working memory... cannot remove loop closure links.", newS->id());
+			UERROR("Signature %d is not in working memory... cannot remove link.", newS->id());
 		}
 		if(!oldS)
 		{
-			UERROR("Signature %d is not in working memory... cannot remove loop closure links.", oldS->id());
+			UERROR("Signature %d is not in working memory... cannot remove link.", oldS->id());
 		}
 	}
 }
@@ -1656,7 +1677,7 @@ Transform Memory::computeVisualTransform(
 					UDEBUG("Forcing 2D...");
 					float x,y,z,r,p,yaw;
 					transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
-					transform = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+					transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
 				}
 			}
 			else if(inliersCount < _bowMinInliers)
@@ -1921,7 +1942,7 @@ Transform Memory::computeIcpTransform(
 		// We are 2D here, make sure the guess has only YAW rotation
 		float x,y,z,r,p,yaw;
 		guess.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
-		guess = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+		guess = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
 		if(r!=0 || p!=0)
 		{
 			UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p);
@@ -2038,7 +2059,7 @@ Transform Memory::computeScanMatchingTransform(
 			const Signature * s = this->getSignature(iter->first);
 			if(!s->getLaserScanCompressed().empty())
 			{
-				*assembledOldClouds += *util3d::cvMat2Cloud(util3d::uncompressData(s->getLaserScanCompressed()), iter->second);
+				*assembledOldClouds += *util3d::cvMat2Cloud(rtabmap::uncompressData(s->getLaserScanCompressed()), iter->second);
 			}
 			else
 			{
@@ -2057,7 +2078,7 @@ Transform Memory::computeScanMatchingTransform(
 	const Signature * newS = getSignature(newId);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
 	UASSERT(uContains(poses, newId));
-	newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getLaserScanCompressed()), poses.at(newId));
+	newCloud = util3d::cvMat2Cloud(rtabmap::uncompressData(newS->getLaserScanCompressed()), poses.at(newId));
 
 	//voxelize
 	if(newCloud->size() && _icp2VoxelSize > 0.0f)
@@ -2132,7 +2153,7 @@ Transform Memory::computeScanMatchingTransform(
 }
 
 // Transform from new to old
-bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance)
+bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance)
 {
 	UASSERT(type > Link::kNeighbor && type != Link::kUndef);
 
@@ -2148,20 +2169,32 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
 			return true;
 		}
 
-		_memoryChanged = true;
+		UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
-		UDEBUG("Add loop closure link between %d and %d", oldS->id(), newS->id());
 
 		oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), variance));
 		newS->addLink(Link(newS->id(), oldS->id(), type, transform, variance));
 
+		if(type!=Link::kVirtualClosure)
+		{
+			_linksChanged = true;
+		}
+
 		if(_incrementalMemory && type == Link::kGlobalClosure)
 		{
-			_lastGlobalLoopClosureParentId = newS->id();
+			_lastGlobalLoopClosureParentId = newS->id()>oldS->id()?newS->id():oldS->id();
-			_lastGlobalLoopClosureChildId = oldS->id();
+			_lastGlobalLoopClosureChildId = newS->id()>oldS->id()?oldS->id():newS->id();
 
 			// udpate weights only if the memory is incremental
-			newS->setWeight(newS->getWeight() + oldS->getWeight());
+			if(newS->id() > oldS->id())
-			oldS->setWeight(0);
+			{
+				newS->setWeight(newS->getWeight() + oldS->getWeight());
+				oldS->setWeight(0);
+			}
+			else
+			{
+				oldS->setWeight(oldS->getWeight() + newS->getWeight());
+				newS->setWeight(0);
+			}
 		}
 		return true;
 	}
@@ -2179,22 +2212,37 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
 	return false;
 }
 
-void Memory::updateNeighborLink(int fromId, int toId, const Transform & transform, float variance)
+void Memory::updateLink(int fromId, int toId, const Transform & transform, float variance)
 {
 	Signature * fromS = this->_getSignature(fromId);
 	Signature * toS = this->_getSignature(toId);
 
 	if(fromS->hasLink(toId) && toS->hasLink(fromId))
 	{
+		Link::Type type = fromS->getLinks().at(toId).type();
 		fromS->removeLink(toId);
 		toS->removeLink(fromId);
 
-		fromS->addLink(Link(fromId, toId, Link::kNeighbor, transform, variance));
+		fromS->addLink(Link(fromId, toId, type, transform, variance));
-		toS->addLink(Link(toId, fromId, Link::kNeighbor, transform.inverse(), variance));
+		toS->addLink(Link(toId, fromId, type, transform.inverse(), variance));
+
+		if(type!=Link::kVirtualClosure)
+		{
+			_linksChanged = true;
+		}
 	}
 	else
 	{
-		UERROR("fromId=%d and toId=%d are not neighbors!", fromId, toId);
+		UERROR("fromId=%d and toId=%d are not linked!", fromId, toId);
+	}
+}
+
+void Memory::removeAllVirtualLinks()
+{
+	UDEBUG("");
+	for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		iter->second->removeVirtualLinks();
 	}
 }
 
@@ -2484,7 +2532,7 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
 	{
 		image = s->getImageCompressed();
 	}
-	if(image.empty() && this->isRawDataKept() && _dbDriver)
+	if(image.empty() && this->isBinDataKept() && _dbDriver)
 	{
 		_dbDriver->getNodeData(signatureId, image);
 	}
@@ -2877,7 +2925,7 @@ private:
 	VWDictionary * _vwp;
 };
 
-Signature * Memory::createSignature(const SensorData & data, bool keepRawData, Statistics * stats)
+Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 {
 	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
 	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
@@ -3283,7 +3331,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 	}
 
 	Signature * s;
-	if(keepRawData)
+	if(this->isBinDataKept())
 	{
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
@@ -3300,9 +3348,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 		{
 			depthOrRightImage = data.rightImage();
 		}
-		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
+		rtabmap::CompressionThread ctImage(data.image(), std::string(".jpg"));
-		util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
+		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
-		util3d::CompressionThread ctDepth2d(data.laserScan());
+		rtabmap::CompressionThread ctDepth2d(data.laserScan());
 		ctImage.start();
 		ctDepth.start();
 		ctDepth2d.start();
@@ -3323,9 +3371,6 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 			data.cx(),
 			data.cy(),
 			data.localTransform());
-		s->setImageRaw(data.image());
-		s->setDepthRaw(depthOrRightImage);
-		s->setLaserScanRaw(data.laserScan());
 	}
 	else
 	{
@@ -3334,7 +3379,13 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 			words,
 			words3D,
 			data.pose(),
-			util3d::compressData2(data.laserScan()));
+			rtabmap::compressData2(data.laserScan()));
+	}
+	if(this->isRawDataKept())
+	{
+		s->setImageRaw(data.image());
+		s->setDepthRaw(data.depth());
+		s->setLaserScanRaw(data.laserScan());
 	}
 
 

corelib/src/Odometry.cpp

@@ -158,7 +158,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
 	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
 	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
 	int nn = Parameters::defaultOdomBowNNType();
 	float nndr = Parameters::defaultOdomBowNNDR();

corelib/src/Parameters.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <math.h>
 #include <stdlib.h>
+#include <sstream>
 
 namespace rtabmap
 {
@@ -112,7 +113,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
-		value = atof(iter->second.c_str());
+		value = uStr2Float(iter->second);
 	}
 }
 void Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
@@ -120,7 +121,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
-		value = atof(iter->second.c_str());
+		value = uStr2Double(iter->second);
 	}
 }
 void Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)

corelib/src/Signature.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/core/Memory.h"
-#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Compression.h"
 #include <opencv2/highgui/highgui.hpp>
 
 #include <rtabmap/utilite/UtiLite.h>
@@ -149,6 +149,21 @@ void Signature::removeLink(int idTo)
 	}
 }
 
+void Signature::removeVirtualLinks()
+{
+	for(std::map<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
+	{
+		if(iter->second.type() == Link::kVirtualClosure)
+		{
+			_links.erase(iter++);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+}
+
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
@@ -283,9 +298,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()))
 	{
-		util3d::CompressionThread ctImage(_imageCompressed, true);
+		rtabmap::CompressionThread ctImage(_imageCompressed, true);
-		util3d::CompressionThread ctDepth(_depthCompressed, true);
+		rtabmap::CompressionThread ctDepth(_depthCompressed, true);
-		util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
+		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
 		if(imageRaw && imageRaw->empty())
 		{
 			ctImage.start();

corelib/src/Transform.cpp

@@ -74,7 +74,7 @@ Transform::Transform(float r11, float r12, float r13, float o14,
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
 {
 	Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
-	*this = util3d::transformFromEigen3f(t);
+	*this = fromEigen3f(t);
 }
 
 bool Transform::isNull() const
@@ -133,8 +133,7 @@ void Transform::setIdentity()
 
 Transform Transform::inverse() const
 {
-	Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
+	return fromEigen4f(toEigen4f().inverse());
-	return util3d::transformFromEigen4f(m.inverse());
 }
 
 Transform Transform::rotation() const
@@ -153,7 +152,7 @@ Transform Transform::translation() const
 
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
 {
-	pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
+	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
 }
 
 void Transform::getTranslation(float & x, float & y, float & z) const
@@ -165,12 +164,22 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 
 float Transform::getNorm() const
 {
-	return std::sqrt(this->getNormSquared());
+	return uNorm(this->x(), this->y(), this->z());
 }
 
 float Transform::getNormSquared() const
 {
-	return this->x()*this->x() + this->y()*this->y() + this->z()*this->z();
+	return uNormSquared(this->x(), this->y(), this->z());
+}
+
+float Transform::getDistance(const Transform & t) const
+{
+	return uNorm(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
+}
+
+float Transform::getDistanceSquared(const Transform & t) const
+{
+	return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
 }
 
 std::string Transform::prettyPrint() const
@@ -182,9 +191,7 @@ std::string Transform::prettyPrint() const
 
 Transform Transform::operator*(const Transform & t) const
 {
-	Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
+	return fromEigen4f(toEigen4f()*t.toEigen4f());
-	Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
-	return util3d::transformFromEigen4f(m1*m2);
 }
 
 Transform & Transform::operator*=(const Transform & t)
@@ -216,5 +223,64 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 	return os;
 }
 
+Eigen::Matrix4f Transform::toEigen4f() const
+{
+	Eigen::Matrix4f m;
+	m << data_[0], data_[1], data_[2], data_[3],
+		 data_[4], data_[5], data_[6], data_[7],
+		 data_[8], data_[9], data_[10], data_[11],
+		 0,0,0,1;
+	return m;
+}
+Eigen::Matrix4d Transform::toEigen4d() const
+{
+	Eigen::Matrix4d m;
+	m << data_[0], data_[1], data_[2], data_[3],
+		 data_[4], data_[5], data_[6], data_[7],
+		 data_[8], data_[9], data_[10], data_[11],
+		 0,0,0,1;
+	return m;
+}
+
+Eigen::Affine3f Transform::toEigen3f() const
+{
+	return Eigen::Affine3f(toEigen4f());
+}
+
+Eigen::Affine3d Transform::toEigen3d() const
+{
+	return Eigen::Affine3d(toEigen4d());
+}
+
+Transform Transform::getIdentity()
+{
+	return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
+}
+
+Transform Transform::fromEigen4f(const Eigen::Matrix4f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen4d(const Eigen::Matrix4d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+Transform Transform::fromEigen3f(const Eigen::Affine3f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
 
 }

corelib/src/VWDictionary.cpp

@@ -71,7 +71,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
 
-	UASSERT(_nndrRatio > 0.0f);
+	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
 
 	std::string dictionaryPath = _dictionaryPath;
 	bool incrementalDictionary = _incrementalDictionary;
@@ -167,7 +167,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 							//get descriptor
 							for(;i<dimension && iter != strList.end(); ++i, ++iter)
 							{
-								descriptor.at<float>(i) = std::atof(iter->c_str());
+								descriptor.at<float>(i) = uStr2Float(*iter);
 							}
 							if(i != dimension)
 							{

corelib/src/util3d.cpp

@@ -47,13 +47,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cmath>
 #include <stdio.h>
 
-#include <zlib.h>
-
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Signature.h"
-#include "toro3d/treeoptimizer3.hh"
 
 #include <pcl/filters/random_sample.h>
 
@@ -63,54 +60,6 @@ namespace rtabmap
 namespace util3d
 {
 
-// format : ".png" ".jpg" "" (empty is general)
-CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
-	uncompressedData_(mat),
-	format_(format),
-	image_(!format.empty()),
-	compressMode_(true)
-{
-	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
-}
-// assume image
-CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
-	compressedData_(bytes),
-	image_(isImage),
-	compressMode_(false)
-{}
-void CompressionThread::mainLoop()
-{
-	if(compressMode_)
-	{
-		if(!uncompressedData_.empty())
-		{
-			if(image_)
-			{
-				compressedData_ = compressImage2(uncompressedData_, format_);
-			}
-			else
-			{
-				compressedData_ = compressData2(uncompressedData_);
-			}
-		}
-	}
-	else // uncompress
-	{
-		if(!compressedData_.empty())
-		{
-			if(image_)
-			{
-				uncompressedData_ = uncompressImage(compressedData_);
-			}
-			else
-			{
-				uncompressedData_ = uncompressData(compressedData_);
-			}
-		}
-	}
-	this->kill();
-}
-
 cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
 {
 	cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
@@ -345,7 +294,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 
 			if(!transform.isNull() && !transform.isIdentity())
 			{
-				pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
+				pt = pcl::transformPoint(pt, transform.toEigen3f());
 			}
 			keypoints3d->at(i) = pt;
 		}
@@ -377,7 +326,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 
 		if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
 		{
-			pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
+			pt = pcl::transformPoint(pt, transform.toEigen3f());
 		}
 		keypoints3d->at(i) = pt;
 	}
@@ -444,7 +393,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 					pt = tmpPt;
 					if(!transform.isNull() && !transform.isIdentity())
 					{
-						pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
+						pt = pcl::transformPoint(pt, transform.toEigen3f());
 					}
 				}
 			}
@@ -1095,168 +1044,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 	return output;
 }
 
-// ".png" or ".jpg"
-std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
-{
-	std::vector<unsigned char> bytes;
-	if(!image.empty())
-	{
-		cv::imencode(format, image, bytes);
-	}
-	return bytes;
-}
-
-// ".png" or ".jpg"
-cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
-{
-	std::vector<unsigned char> bytes = compressImage(image, format);
-	if(bytes.size())
-	{
-		return cv::Mat(1, bytes.size(), CV_8UC1, bytes.data()).clone();
-	}
-	return cv::Mat();
-}
-
-cv::Mat uncompressImage(const cv::Mat & bytes)
-{
-	 cv::Mat image;
-	if(!bytes.empty())
-	{
-#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-	}
-	return image;
-}
-
-cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
-{
-	 cv::Mat image;
-	if(bytes.size())
-	{
-#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
-#else
-		image = cv::imdecode(bytes, -1);
-#endif
-	}
-	return image;
-}
-
-std::vector<unsigned char> compressData(const cv::Mat & data)
-{
-	std::vector<unsigned char> bytes;
-	if(!data.empty())
-	{
-		uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
-		uLong destLen = compressBound(sourceLen);
-		bytes.resize(destLen);
-		int errCode = compress(
-						(Bytef *)bytes.data(),
-						&destLen,
-						(const Bytef *)data.data,
-						sourceLen);
-
-		bytes.resize(destLen+3*sizeof(int));
-		*((int*)&bytes[destLen]) = data.rows;
-		*((int*)&bytes[destLen+sizeof(int)]) = data.cols;
-		*((int*)&bytes[destLen+2*sizeof(int)]) = data.type();
-
-		if(errCode == Z_MEM_ERROR)
-		{
-			UERROR("Z_MEM_ERROR : Insufficient memory.");
-		}
-		else if(errCode == Z_BUF_ERROR)
-		{
-			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
-		}
-	}
-	return bytes;
-}
-
-cv::Mat compressData2(const cv::Mat & data)
-{
-	cv::Mat bytes;
-	if(!data.empty())
-	{
-		uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
-		uLong destLen = compressBound(sourceLen);
-		bytes = cv::Mat(1, destLen+3*sizeof(int), CV_8UC1);
-		int errCode = compress(
-						(Bytef *)bytes.data,
-						&destLen,
-						(const Bytef *)data.data,
-						sourceLen);
-		bytes = cv::Mat(bytes, cv::Rect(0,0, destLen+3*sizeof(int), 1));
-		*((int*)&bytes.data[destLen]) = data.rows;
-		*((int*)&bytes.data[destLen+sizeof(int)]) = data.cols;
-		*((int*)&bytes.data[destLen+2*sizeof(int)]) = data.type();
-
-		if(errCode == Z_MEM_ERROR)
-		{
-			UERROR("Z_MEM_ERROR : Insufficient memory.");
-		}
-		else if(errCode == Z_BUF_ERROR)
-		{
-			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
-		}
-	}
-	return bytes;
-}
-
-cv::Mat uncompressData(const cv::Mat & bytes)
-{
-	UASSERT(bytes.empty() || bytes.type() == CV_8UC1);
-	return uncompressData(bytes.data, bytes.cols*bytes.rows);
-}
-
-cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
-{
-	return uncompressData(bytes.data(), bytes.size());
-}
-
-cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
-{
-	cv::Mat data;
-	if(bytes && size>=3*sizeof(int))
-	{
-		//last 3 int elements are matrix size and type
-		int height = *((int*)&bytes[size-3*sizeof(int)]);
-		int width = *((int*)&bytes[size-2*sizeof(int)]);
-		int type = *((int*)&bytes[size-1*sizeof(int)]);
-
-		// If the size is higher, it may be a wrong data format.
-		UASSERT_MSG(height>=0 && height<10000 &&
-				    width>=0 && width<10000,
-				    uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
-
-		data = cv::Mat(height, width, type);
-		uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
-
-		int errCode = uncompress(
-						(Bytef*)data.data,
-						&totalUncompressed,
-						(const Bytef*)bytes,
-						uLong(size));
-
-		if(errCode == Z_MEM_ERROR)
-		{
-			UERROR("Z_MEM_ERROR : Insufficient memory.");
-		}
-		else if(errCode == Z_BUF_ERROR)
-		{
-			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
-		}
-		else if(errCode == Z_DATA_ERROR)
-		{
-			UERROR("Z_DATA_ERROR : The compressed data (referenced by source) was corrupted.");
-		}
-	}
-	return data;
-}
-
 void extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
 									  const std::multimap<int, pcl::PointXYZ> & words2,
 									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
@@ -1643,7 +1430,7 @@ Transform transformFromXYZCorrespondences(
 				bestTransformation.row (2) = model_coefficients.segment<4>(8);
 				bestTransformation.row (3) = model_coefficients.segment<4>(12);
 
-				transform = util3d::transformFromEigen4f(bestTransformation);
+				transform = Transform::fromEigen4f(bestTransformation);
 				UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
 
 				return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
@@ -1747,7 +1534,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		*hasConvergedOut = hasConverged;
 	}
 
-	return transformFromEigen4f(icp.getFinalTransformation());
+	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
 // return transform from source to target (All points/normals must be finite!!!)
@@ -1837,7 +1624,7 @@ Transform icpPointToPlane(
 		*hasConvergedOut = hasConverged;
 	}
 
-	return transformFromEigen4f(icp.getFinalTransformation());
+	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
 // return transform from source to target (All points must be finite!!!)
@@ -1926,7 +1713,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		*hasConvergedOut = hasConverged;
 	}
 
-	return transformFromEigen4f(icp.getFinalTransformation());
+	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
 pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
@@ -2054,7 +1841,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
 	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
 	UASSERT(matrix.rows == 1);
 
-	Eigen::Affine3f t = transformToEigen3f(tranform);
+	Eigen::Affine3f t = tranform.toEigen3f();
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	cloud->resize(matrix.cols);
 	if(matrix.channels() == 2)
@@ -2219,608 +2006,6 @@ pcl::PolygonMesh::Ptr createMesh(
 	return mesh;
 }
 
-std::multimap<int, Link>::iterator findLink(
-		std::multimap<int, Link> & links,
-		int from,
-		int to)
-{
-	std::multimap<int, Link>::iterator iter = links.find(from);
-	while(iter != links.end() && iter->first == from)
-	{
-		if(iter->second.to() == to)
-		{
-			return iter;
-		}
-		++iter;
-	}
-
-	// let's try to -> from
-	iter = links.find(to);
-	while(iter != links.end() && iter->first == to)
-	{
-		if(iter->second.to() == from)
-		{
-			return iter;
-		}
-		++iter;
-	}
-	return links.end();
-}
-
-
-// <int, depth> margin=0 means infinite margin
-std::map<int, int> generateDepthGraph(
-		const std::multimap<int, Link> & links,
-		int fromId,
-		int depth)
-{
-	UASSERT(depth >= 0);
-	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
-	std::map<int, int> ids;
-	if(fromId<=0)
-	{
-		return ids;
-	}
-
-	std::list<int> curentDepthList;
-	std::set<int> nextDepth;
-	nextDepth.insert(fromId);
-	int d = 0;
-	while((depth == 0 || d < depth) && nextDepth.size())
-	{
-		curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
-		nextDepth.clear();
-
-		for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
-		{
-			if(ids.find(*jter) == ids.end())
-			{
-				std::set<int> marginIds;
-
-				ids.insert(std::pair<int, int>(*jter, d));
-
-				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
-				{
-					if(iter->second.from() == *jter)
-					{
-						marginIds.insert(iter->second.to());
-					}
-					else if(iter->second.to() == *jter)
-					{
-						marginIds.insert(iter->second.from());
-					}
-				}
-
-				// Margin links
-				for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
-				{
-					if( !uContains(ids, *iter) && nextDepth.find(*iter) == nextDepth.end())
-					{
-						nextDepth.insert(*iter);
-					}
-				}
-			}
-		}
-		++d;
-	}
-	return ids;
-}
-
-void optimizeTOROGraph(
-		const std::map<int, int> & depthGraph,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations,
-		bool toroInitialGuess,
-		bool ignoreCovariance,
-		std::list<std::map<int, Transform> > * intermediateGraphes)
-{
-	optimizedPoses.clear();
-	if(depthGraph.size() && poses.size()>=2 && links.size()>=1)
-	{
-		// Modify IDs using the margin from the current signature (TORO root will be the last signature)
-		int m = 0;
-		int toroId = 1;
-		std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
-		std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
-		std::map<int, int> idsTmp = depthGraph;
-		while(idsTmp.size())
-		{
-			for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
-			{
-				if(m == iter->second)
-				{
-					rtabmapToToro.insert(std::make_pair(iter->first, toroId));
-					toroToRtabmap.insert(std::make_pair(toroId, iter->first));
-					++toroId;
-					idsTmp.erase(iter++);
-				}
-				else
-				{
-					++iter;
-				}
-			}
-			++m;
-		}
-
-		std::map<int, rtabmap::Transform> posesToro;
-		std::multimap<int, rtabmap::Link> edgeConstraintsToro;
-		for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			if(uContains(depthGraph, iter->first))
-			{
-				UASSERT(!iter->second.isNull());
-				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
-			}
-		}
-		for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
-			iter!=links.end();
-			++iter)
-		{
-			if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
-			{
-				UASSERT(!iter->second.transform().isNull());
-				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
-			}
-		}
-
-		std::map<int, rtabmap::Transform> optimizedPosesToro;
-
-		if(posesToro.size() && edgeConstraintsToro.size())
-		{
-			std::list<std::map<int, rtabmap::Transform> > graphesToro;
-
-			// Optimize!
-			rtabmap::util3d::optimizeTOROGraph(
-					posesToro,
-					edgeConstraintsToro,
-					optimizedPosesToro,
-					toroIterations,
-					toroInitialGuess,
-					ignoreCovariance,
-					&graphesToro);
-
-			for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
-			{
-				optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
-			}
-
-			if(intermediateGraphes)
-			{
-				for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
-				{
-					std::map<int, rtabmap::Transform> tmp;
-					for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
-					{
-						tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
-					}
-					intermediateGraphes->push_back(tmp);
-				}
-			}
-		}
-		else
-		{
-			UERROR("No TORO poses and constraints!?");
-		}
-	}
-	else if(links.size() == 0 && poses.size() == 1)
-	{
-		optimizedPoses = poses;
-	}
-	else
-	{
-		UERROR("Wrong inputs! depthGraph=%d poses=%d links=%d",
-				(int)depthGraph.size(), (int)poses.size(), (int)links.size());
-	}
-}
-
-//On success, optimizedPoses is cleared and new poses are inserted in
-void optimizeTOROGraph(
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations,
-		bool toroInitialGuess,
-		bool ignoreCovariance,
-		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
-{
-	UASSERT(toroIterations>0);
-	optimizedPoses.clear();
-	if(edgeConstraints.size()>=1 && poses.size()>=2)
-	{
-		// Apply TORO optimization
-		AISNavigation::TreeOptimizer3 pg;
-		pg.verboseLevel = 0;
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			float x,y,z, roll,pitch,yaw;
-			UASSERT(!iter->second.isNull());
-			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second), x,y,z, roll,pitch,yaw);
-			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
-			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v = pg.addVertex(iter->first, p);
-			if (v)
-			{
-				v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
-			}
-			else
-			{
-				UERROR("cannot insert vertex %d!?", iter->first);
-			}
-		}
-
-		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
-		{
-			int id1 = iter->first;
-			int id2 = iter->second.to();
-			float x,y,z, roll,pitch,yaw;
-			UASSERT(!iter->second.transform().isNull());
-			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
-			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
-			AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
-			if(!ignoreCovariance && iter->second.variance()>0)
-			{
-				inf[0][0] = 1.0f/iter->second.variance(); // x
-				inf[1][1] = 1.0f/iter->second.variance(); // y
-				inf[2][2] = 1.0f/iter->second.variance(); // z
-				inf[3][3] = 1.0f/iter->second.variance(); // roll
-				inf[4][4] = 1.0f/iter->second.variance(); // pitch
-				inf[5][5] = 1.0f/iter->second.variance(); // yaw
-			}
-
-			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
-			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
-			AISNavigation::TreePoseGraph3::Transformation t(p);
-			if (!pg.addEdge(v1, v2, t, inf))
-			{
-				UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
-				return;
-			}
-		}
-		pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
-
-		UDEBUG("Initial guess...");
-		if(toroInitialGuess)
-		{
-			pg.initializeOnTree(); // optional
-		}
-
-		pg.initializeTreeParameters();
-		UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
-			   "TORO is not able to find the root of the graph!)");
-		pg.initializeOptimization();
-
-		UDEBUG("TORO iterate begin (iterations=%d)", toroIterations);
-		for (int i=0; i<toroIterations; i++)
-		{
-			if(intermediateGraphes && (toroInitialGuess || i>0))
-			{
-				std::map<int, Transform> tmpPoses;
-				for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-				{
-					AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
-					v->pose=v->transformation.toPoseType();
-					Transform newPose = transformFromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
-
-					tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
-				}
-				intermediateGraphes->push_back(tmpPoses);
-			}
-
-			pg.iterate();
-		}
-		UDEBUG("TORO iterate end");
-
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
-			v->pose=v->transformation.toPoseType();
-			Transform newPose = transformFromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
-
-			optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
-		}
-
-		//Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
-		//Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
-		//Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
-		//Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
-		//mapCorrection = transformFromEigen4f(result);
-	}
-	else if(edgeConstraints.size() == 0 && poses.size() == 1)
-	{
-		optimizedPoses = poses;
-	}
-	else
-	{
-		UWARN("This method should be called at least with 1 pose!");
-	}
-}
-
-bool saveTOROGraph(
-		const std::string & fileName,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints)
-{
-	FILE * file = 0;
-
-#ifdef _MSC_VER
-	fopen_s(&file, fileName.c_str(), "w");
-#else
-	file = fopen(fileName.c_str(), "w");
-#endif
-
-	if(file)
-	{
-		// VERTEX3 id x y z phi theta psi
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			float x,y,z, yaw,pitch,roll;
-			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second), x,y,z, roll, pitch, yaw);
-			fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
-					iter->first,
-					x,
-					y,
-					z,
-					roll,
-					pitch,
-					yaw);
-		}
-
-		//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
-		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
-		{
-			float x,y,z, yaw,pitch,roll;
-			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll, pitch, yaw);
-			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
-					iter->first,
-					iter->second.to(),
-					x,
-					y,
-					z,
-					roll,
-					pitch,
-					yaw,
-					1.0f/iter->second.variance(),
-					1.0f/iter->second.variance(),
-					1.0f/iter->second.variance(),
-					1.0f/iter->second.variance(),
-					1.0f/iter->second.variance(),
-					1.0f/iter->second.variance());
-		}
-		UINFO("Graph saved to %s", fileName.c_str());
-		fclose(file);
-	}
-	else
-	{
-		UERROR("Cannot save to file %s", fileName.c_str());
-		return false;
-	}
-	return true;
-}
-
-bool loadTOROGraph(const std::string & fileName,
-		std::map<int, Transform> & poses,
-		std::multimap<int, std::pair<int, Transform> > & edgeConstraints)
-{
-	FILE * file = 0;
-#ifdef _MSC_VER
-	fopen_s(&file, fileName.c_str(), "r");
-#else
-	file = fopen(fileName.c_str(), "r");
-#endif
-
-	if(file)
-	{
-		char line[200];
-		while ( fgets (line , 200 , file) != NULL )
-		{
-			std::vector<std::string> strList = uListToVector(uSplit(line, ' '));
-			if(strList.size() == 8)
-			{
-				//VERTEX3
-				int id = atoi(strList[1].c_str());
-				float x = atof(strList[2].c_str());
-				float y = atof(strList[3].c_str());
-				float z = atof(strList[4].c_str());
-				float roll = atof(strList[5].c_str());
-				float pitch = atof(strList[6].c_str());
-				float yaw = atof(strList[7].c_str());
-				Transform pose = transformFromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
-				std::map<int, Transform>::iterator iter = poses.find(id);
-				if(iter != poses.end())
-				{
-					iter->second = pose;
-				}
-				else
-				{
-					UFATAL("");
-				}
-			}
-			else if(strList.size() == 30)
-			{
-				//EDGE3
-				int idFrom = atoi(strList[1].c_str());
-				int idTo = atoi(strList[2].c_str());
-				float x = atof(strList[3].c_str());
-				float y = atof(strList[4].c_str());
-				float z = atof(strList[5].c_str());
-				float roll = atof(strList[6].c_str());
-				float pitch = atof(strList[7].c_str());
-				float yaw = atof(strList[8].c_str());
-				Transform transform = transformFromEigen3f(pcl::getTransformation(x, y, z, roll, pitch, yaw));
-				if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
-				{
-					std::pair<int, Transform> edge(idTo, transform);
-					edgeConstraints.insert(std::pair<int, std::pair<int, Transform> >(idFrom, edge));
-				}
-				else
-				{
-					UFATAL("");
-				}
-			}
-			else
-			{
-				UFATAL("Error parsing map file %s", fileName.c_str());
-			}
-		}
-
-		UINFO("Graph loaded from %s", fileName.c_str());
-		fclose(file);
-	}
-	else
-	{
-		UERROR("Cannot open file %s", fileName.c_str());
-		return false;
-	}
-	return true;
-}
-
-
-std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
-{
-	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
-	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		cloud->resize(poses.size());
-		int i=0;
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		}
-
-		// radius filtering
-		std::vector<int> names = uKeys(poses);
-		std::vector<Transform> transforms = uValues(poses);
-
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
-		tree->setInputCloud(cloud);
-		std::set<int> indicesChecked;
-		std::set<int> indicesKept;
-
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			// ignore scans
-			if(indicesChecked.find(i) == indicesChecked.end())
-			{
-				std::vector<int> kIndices;
-				std::vector<float> kDistances;
-				tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
-
-				std::set<int> cloudIndices;
-				const Transform & currentT = transforms.at(i);
-				Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
-				for(unsigned int j=0; j<kIndices.size(); ++j)
-				{
-					if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
-					{
-						const Transform & checkT = transforms.at(kIndices[j]);
-						// same orientation?
-						Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
-						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-						if(a <= angle)
-						{
-							cloudIndices.insert(kIndices[j]);
-						}
-					}
-				}
-
-				if(keepLatest)
-				{
-					bool lastAdded = false;
-					for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
-					{
-						if(!lastAdded)
-						{
-							indicesKept.insert(*iter);
-							lastAdded = true;
-						}
-						indicesChecked.insert(*iter);
-					}
-				}
-				else
-				{
-					bool firstAdded = false;
-					for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
-					{
-						if(!firstAdded)
-						{
-							indicesKept.insert(*iter);
-							firstAdded = true;
-						}
-						indicesChecked.insert(*iter);
-					}
-				}
-			}
-		}
-
-		//pcl::IndicesPtr indicesOut(new std::vector<int>);
-		//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
-		UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
-		//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
-		//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
-
-		std::map<int, Transform> keptPoses;
-		for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
-		{
-			keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
-		}
-
-		return keptPoses;
-	}
-	else
-	{
-		return poses;
-	}
-}
-
-std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
-{
-	std::multimap<int, int> clusters;
-	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
-	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		cloud->resize(poses.size());
-		int i=0;
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		}
-
-		// radius clustering (nearest neighbors)
-		std::vector<int> ids = uKeys(poses);
-		std::vector<Transform> transforms = uValues(poses);
-
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
-		tree->setInputCloud(cloud);
-
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			std::vector<int> kIndices;
-			std::vector<float> kDistances;
-			tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
-
-			std::set<int> cloudIndices;
-			const Transform & currentT = transforms.at(i);
-			Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
-			for(unsigned int j=0; j<kIndices.size(); ++j)
-			{
-				if((int)i != kIndices[j])
-				{
-					const Transform & checkT = transforms.at(kIndices[j]);
-					// same orientation?
-					Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
-					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-					if(a <= angle)
-					{
-						clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
-					}
-				}
-			}
-		}
-	}
-	return clusters;
-}
-
 bool occupancy2DFromCloud3D(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		cv::Mat & ground,
@@ -3123,14 +2308,15 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		pcl::PointXYZ min, max;
 		pcl::getMinMax3D(minMax, min, max);
 
-		xMin = min.x-cellSize;
+		// Added X2 to make sure that all points are inside the map (when rounded to integer)
-		yMin = min.y-cellSize;
+		xMin = min.x-cellSize*2.0f;
-		float xMax = max.x+cellSize;
+		yMin = min.y-cellSize*2.0f;
-		float yMax = max.y+cellSize;
+		float xMax = max.x+cellSize*2.0f;
+		float yMax = max.y+cellSize*2.0f;
 
 		UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
 
-		//UTimer timer;
+		UTimer timer;
 
 		map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
 		std::vector<float> maxSquaredLength(localScans.size(), 0.0f);
@@ -3158,7 +2344,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			}
 			++j;
 		}
-		//UWARN("timer=%fs", timer.ticks());
+		UDEBUG("Ray trace known space=%fs", timer.ticks());
 
 		// now fill unknown spaces
 		if(unknownSpaceFilled)
@@ -3222,8 +2408,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 					}
 				}
 				++j;
-				//UWARN("timer=%fs", timer.ticks());
 			}
+			UDEBUG("Fill empty space=%fs", timer.ticks());
 		}
 	}
 	return map;
@@ -3261,26 +2447,18 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 		//ROS_WARN("lowerbound=%f upperbound=%f", lowerbound, upperbound);
 		UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
 		UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
-		// verify if there is no obstacle
+
-		bool stopped = false;
-		if(stopOnObstacle)
-		{
-			for(int y = lowerbound; y<=(int)upperbound; ++y)
-			{
-				if(grid.at<char>(y, x) == 100)
-				{
-					stopped = true;
-					break;
-				}
-			}
-		}
-		if(stopped)
-		{
-			break;
-		}
 		for(int y = lowerbound; y<=(int)upperbound; ++y)
 		{
-			grid.at<char>(y, x) = 0; // free space
+			char & v = grid.at<char>(y, x);
+			if(v == 100 && stopOnObstacle)
+			{
+				return;
+			}
+			else
+			{
+				v = 0; // free space
+			}
 		}
 	}
 }

examples/BOWMapping/main.cpp

@@ -139,7 +139,7 @@ int main(int argc, char * argv[])
 					(int)rtabmap.getSTM().size(), // short-term memory
 					(int)rtabmap.getWM().size(), // working memory
 					rtabmap.getLoopClosureId(),
-					rtabmap.getLcHypValue(),
+					rtabmap.getLoopClosureValue(),
 					nextIndex,
 					rtabmap.getLoopClosureId());
 		}
@@ -150,8 +150,8 @@ int main(int argc, char * argv[])
 					rtabmap.getLastProcessTime(),
 					(int)rtabmap.getSTM().size(), // short-term memory
 					(int)rtabmap.getWM().size(), // working memory
-					rtabmap.getRetrievedId(), // highest loop closure hypothesis
+					rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
-					rtabmap.getLcHypValue());
+					rtabmap.getLoopClosureValue());
 		}
 
 		++nextIndex;

examples/RGBDMapping/MapBuilder.h

@@ -126,10 +126,10 @@ private slots:
 			if(!data.pose().isNull())
 			{
 				// update camera position
-				cloudViewer_->updateCameraPosition(data.pose());
+				cloudViewer_->updateCameraTargetPosition(data.pose());
 			}
 		}
-		cloudViewer_->render();
+		cloudViewer_->update();
 
 		_lastOdometryProcessed = true;
 	}
@@ -165,14 +165,16 @@ private slots:
 				else if(iter->first == stats.refImageId() &&
 						stats.getSignature().id() == iter->first)
 				{
+					Signature s = stats.getSignature();
+					s.uncompressData(); // make sure data is uncompressed
 					// Add the new cloud
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
-							stats.getSignature().getImageRaw(),
+							s.getImageRaw(),
-							stats.getSignature().getDepthRaw(),
+							s.getDepthRaw(),
-							stats.getSignature().getDepthCx(),
+							s.getDepthCx(),
-							stats.getSignature().getDepthCy(),
+							s.getDepthCy(),
-							stats.getSignature().getDepthFx(),
+							s.getDepthFx(),
-							stats.getSignature().getDepthFy(),
+							s.getDepthFy(),
 						   8); // decimation
 
 					if(cloud->size())
@@ -191,7 +193,7 @@ private slots:
 			}
 		}
 
-		cloudViewer_->render();
+		cloudViewer_->update();
 
 		_processingStatistics = false;
 	}

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -64,6 +64,7 @@ private slots:
 	void processImage(const cv::Mat & image);
 	void restart();
 	void calibrate();
+	void save();
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);

guilib/include/rtabmap/gui/CloudViewer.h

@@ -127,7 +127,7 @@ public:
 			float opacity);
 	void removeOccupancyGridMap();
 
-	void updateCameraPosition(
+	void updateCameraTargetPosition(
 		const Transform & pose);
 
 	void addOrUpdateGraph(
@@ -137,8 +137,10 @@ public:
 	void removeGraph(const std::string & id);
 	void removeAllGraphs();
 
+	bool isTrajectoryShown() const;
+	unsigned int getTrajectorySize() const;
 	void setTrajectoryShown(bool shown);
-	void setTrajectorySize(int value);
+	void setTrajectorySize(unsigned int value);
 	void clearTrajectory();
 
 	void removeAllClouds(); //including meshes
@@ -149,25 +151,47 @@ public:
 
 	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
 	const QColor & getBackgroundColor() const;
+	Transform getTargetPose() const;
+	void getCameraPosition(
+			float & x, float & y, float & z,
+			float & focalX, float & focalY, float & focalZ,
+			float & upX, float & upY, float & upZ) const;
+	bool isCameraTargetLocked() const;
+	bool isCameraTargetFollow() const;
+	bool isCameraFree() const;
+	bool isCameraLockZ() const;
+	bool isGridShown() const;
+	unsigned int getGridCellCount() const;
+	float getGridCellSize() const;
 
+	void setCameraPosition(
+			float x, float y, float z,
+			float focalX, float focalY, float focalZ,
+			float upX, float upY, float upZ);
 	void setCameraTargetLocked(bool enabled = true);
 	void setCameraTargetFollow(bool enabled = true);
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
 	void setGridShown(bool shown);
+	void setGridCellCount(unsigned int count);
+	void setGridCellSize(float size);
 	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
 
 public slots:
-	void render();
 	void setBackgroundColor(const QColor & color);
 	void setCloudVisibility(const std::string & id, bool isVisible);
 	void setCloudOpacity(const std::string & id, double opacity = 1.0);
 	void setCloudPointSize(const std::string & id, int size);
 	virtual void clear() {removeAllClouds(); clearTrajectory();}
 
+signals:
+	void configChanged();
+
 protected:
 	virtual void keyReleaseEvent(QKeyEvent * event);
 	virtual void keyPressEvent(QKeyEvent * event);
+	virtual void mousePressEvent(QMouseEvent * event);
+	virtual void mouseMoveEvent(QMouseEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 	virtual void handleAction(QAction * event);
 	QMenu * menu() {return _menu;}
@@ -188,11 +212,15 @@ private:
     QAction * _aSetTrajectorySize;
     QAction * _aClearTrajectory;
     QAction * _aShowGrid;
+    QAction * _aSetGridCellCount;
+    QAction * _aSetGridCellSize;
     QAction * _aSetBackgroundColor;
     QMenu * _menu;
     std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr > _graphes;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
+    unsigned int _gridCellCount;
+    float _gridCellSize;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes
     Transform _lastPose;
     std::list<std::string> _gridLines;

guilib/include/rtabmap/gui/ImageView.h

@@ -53,10 +53,11 @@ public:
 
 	void resetZoom();
 
-	bool isImageShown();
+	bool isImageShown() const;
-	bool isImageDepthShown();
+	bool isImageDepthShown() const;
-	bool isFeaturesShown();
+	bool isFeaturesShown() const;
-	bool isLinesShown();
+	bool isLinesShown() const;
+	int getAlpha() const {return _alpha;}
 
 	void setFeaturesShown(bool shown);
 	void setImageShown(bool shown);
@@ -69,12 +70,16 @@ public:
 	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, const QColor & color);
 	void setFeaturesColor(const QColor & color);
+	void setAlpha(int alpha);
 
 	const QMultiMap<int, rtabmap::KeypointItem *> & getFeatures() const {return _features;}
 
 	void clearLines();
 	void clear();
 
+signals:
+	void configChanged();
+
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * e);
 	virtual void wheelEvent(QWheelEvent * e);
@@ -89,6 +94,7 @@ private:
 	int _zoom;
 	int _minZoom;
 	QString _savedFileName;
+	int _alpha;
 
 	QMenu * _menu;
 	QAction * _showImage;
@@ -96,6 +102,7 @@ private:
 	QAction * _showFeatures;
 	QAction * _showLines;
 	QAction * _saveImage;
+	QAction * _setAlpha;
 
 	QMultiMap<int, rtabmap::KeypointItem *> _features;
 	QGraphicsPixmapItem * _image;

guilib/include/rtabmap/gui/MainWindow.h

@@ -102,6 +102,7 @@ public:
 
 	QString getWorkingDirectory() const;
 	void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
+	bool isSavedMaximized() const {return _savedMaximized;}
 
 public slots:
 	void processStats(const rtabmap::Statistics & stat);
@@ -109,11 +110,15 @@ public slots:
 protected:
 	virtual void closeEvent(QCloseEvent* event);
 	virtual void handleEvent(UEvent* anEvent);
+	virtual void showEvent(QShowEvent* anEvent);
+	virtual void moveEvent(QMoveEvent* anEvent);
 	virtual void resizeEvent(QResizeEvent* anEvent);
 
 private slots:
 	void changeState(MainWindow::State state);
 	void beep();
+	void configGUIModified();
+	void saveConfigGUI();
 	void newDatabase();
 	void openDatabase();
 	void closeDatabase();
@@ -200,7 +205,7 @@ private:
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
-	void updateSelectSourceImageMenu(int type);
+	void updateSelectSourceImageMenu(bool used, PreferencesDialog::Src src);
 	void updateSelectSourceDatabase(bool used);
 	void updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src);
 
@@ -260,6 +265,9 @@ private:
 	bool _odometryReceived;
 	QString _openedDatabasePath;
 	bool _emptyNewDatabase;
+	bool _odomImageShow;
+	bool _odomImageDepthShow;
+	bool _savedMaximized;
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -97,10 +97,13 @@ public:
 	virtual QString getIniFilePath() const;
 	void init();
 
-	void saveWindowGeometry(const QString & windowName, const QWidget * window);
+	void saveWindowGeometry(const QWidget * window);
-	void loadWindowGeometry(const QString & windowName, QWidget * window);
+	void loadWindowGeometry(QWidget * window);
 	void saveMainWindowState(const QMainWindow * mainWindow);
-	void loadMainWindowState(QMainWindow * mainWindow);
+	void loadMainWindowState(QMainWindow * mainWindow, bool & maximized);
+
+	void saveWidgetState(const QWidget * widget);
+	void loadWidgetState(QWidget * widget);
 
 	void saveCustomConfig(const QString & section, const QString & key, const QString & value);
 	QString loadCustomConfig(const QString & section, const QString & key);
@@ -114,11 +117,9 @@ public:
 	int getGeneralLoggerType() const;
 	bool getGeneralLoggerPrintTime() const;
 	bool isVerticalLayoutUsed() const;
-	bool isImageFlipped() const;
 	bool imageRejectedShown() const;
 	bool imageHighestHypShown() const;
 	bool beepOnPause() const;
-	int getKeypointsOpacity() const;
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
 
@@ -158,7 +159,7 @@ public:
 	bool isSourceImageUsed() const;
 	bool isSourceDatabaseUsed() const;
 	bool isSourceOpenniUsed() const;
-	int getSourceImageType() const;
+	PreferencesDialog::Src getSourceImageType() const;
 	QString getSourceImageTypeStr() const;
 	int getSourceWidth() const;
 	int getSourceHeight() const;
@@ -211,9 +212,9 @@ public slots:
 	void setHardThr(int value);
 	void setTimeLimit(float value);
 	void setSLAMMode(bool enabled);
-	void selectSourceImage(Src src = kSrcUndef);
+	void selectSourceImage(Src src = kSrcUndef, bool checked = true);
-	void selectSourceDatabase(bool user = false);
+	void selectSourceDatabase(bool user = false, bool checked = true);
-	void selectSourceRGBD(Src src = kSrcUndef);
+	void selectSourceRGBD(Src src = kSrcUndef, bool checked = true);
 
 private slots:
 	void closeDialog ( QAbstractButton * button );

guilib/src/CMakeLists.txt

@@ -22,6 +22,7 @@ SET(headers_ui
 	./PostProcessingDialog.h
 	./ExportCloudsDialog.h
 	./MapVisibilityWidget.h
+	./GraphViewer.h
 )
 
 SET(uis

guilib/src/CalibrationDialog.cpp

@@ -33,6 +33,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/highgui/highgui.hpp>
 
+#include <QtGui/QFileDialog>
+#include <QtGui/QMessageBox>
+
 #include <rtabmap/core/CameraEvent.h>
 #include <rtabmap/gui/UCv2Qt.h>
 
@@ -57,6 +60,7 @@ CalibrationDialog::CalibrationDialog(QWidget * parent) :
 
 	connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
 	connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
+	connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
 
 	connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
 	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
@@ -241,7 +245,7 @@ void CalibrationDialog::processImage(const cv::Mat & image)
 	}
 
 	//show frame
-	ui_->image_view->setImage(uCvMat2QImage(image));
+	ui_->image_view->setImage(uCvMat2QImage(image).mirrored(ui_->checkBox_mirror->isChecked(), false));
 }
 
 void CalibrationDialog::restart()
@@ -254,6 +258,7 @@ void CalibrationDialog::restart()
 	ui_->pushButton_calibrate->setEnabled(false);
 	ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
 	ui_->checkBox_rectified->setEnabled(false);
+	//ui_->pushButton_save->setEnabled(false);
 
 	ui_->progressBar_count->reset();
 	ui_->progressBar_count->setMaximum(COUNT_MIN);
@@ -343,6 +348,34 @@ void CalibrationDialog::calibrate()
 		ui_->checkBox_rectified->setChecked(true);
 
 		ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
+		ui_->pushButton_save->setEnabled(true);
+	}
+}
+
+void CalibrationDialog::save()
+{
+	QString fileName = QFileDialog::getSaveFileName(this, tr("Export"), "calibration.yaml", "*.yaml");
+
+	if(!fileName.isEmpty())
+	{
+		cv::FileStorage fs(fileName.toStdString(), cv::FileStorage::WRITE);
+
+		// export in ROS calibration format
+		fs << "camera_matrix" << "{";
+		fs << "rows" << cameraMatrix_.rows;
+		fs << "cols" << cameraMatrix_.cols;
+		fs << "data" << std::vector<double>((double*)cameraMatrix_.data, ((double*)cameraMatrix_.data)+(cameraMatrix_.rows*cameraMatrix_.cols));
+		fs << "}";
+
+		fs << "distortion_coefficients" << "{";
+		fs << "rows" << distCoeffs_.rows;
+		fs << "cols" << distCoeffs_.cols;
+		fs << "data" << std::vector<double>((double*)distCoeffs_.data, ((double*)distCoeffs_.data)+(distCoeffs_.rows*distCoeffs_.cols));
+		fs << "}";
+
+		fs.release();
+		QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(fileName));
+		UINFO("Saved \"%s\"!", fileName.toStdString().c_str());
 	}
 }
 

guilib/src/CloudViewer.cpp

@@ -51,17 +51,13 @@ void CloudViewer::mouseEventOccurred (const pcl::visualization::MouseEvent &even
 	if (event.getButton () == pcl::visualization::MouseEvent::LeftButton ||
 	  event.getButton () == pcl::visualization::MouseEvent::MiddleButton)
 	{
-		this->render(); // this will apply frustum
+		this->update(); // this will apply frustum
 	}
 }
 
 CloudViewer::CloudViewer(QWidget *parent) :
 		QVTKWidget(parent),
-#if defined(_WIN32) || defined(__APPLE__)
-		_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer")),
-#else
 		_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)),
-#endif
 		_aLockCamera(0),
 		_aFollowCamera(0),
 		_aResetCamera(0),
@@ -70,10 +66,14 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_aSetTrajectorySize(0),
 		_aClearTrajectory(0),
 		_aShowGrid(0),
+		_aSetGridCellCount(0),
+		_aSetGridCellSize(0),
 		_aSetBackgroundColor(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
+		_gridCellCount(50),
+		_gridCellSize(1),
 		_workingDirectory("."),
 		_backgroundColor(Qt::black)
 {
@@ -81,9 +81,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 
 	this->SetRenderWindow(_visualizer->getRenderWindow());
 
-#if !defined(_WIN32) && !defined(__APPLE__)
 	_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
-#endif
 
 	_visualizer->registerMouseCallback (&CloudViewer::mouseEventOccurred, *this, (void*)_visualizer);
 	_visualizer->setCameraPosition(
@@ -93,6 +91,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 
 	//setup menu/actions
 	createMenu();
+
+	setMouseTracking(false);
 }
 
 CloudViewer::~CloudViewer()
@@ -125,6 +125,8 @@ void CloudViewer::createMenu()
 	_aClearTrajectory = new QAction("Clear trajectory", this);
 	_aShowGrid = new QAction("Show grid", this);
 	_aShowGrid->setCheckable(true);
+	_aSetGridCellCount = new QAction("Set cell count...", this);
+	_aSetGridCellSize = new QAction("Set cell size...", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);
 
 	QMenu * cameraMenu = new QMenu("Camera", this);
@@ -144,11 +146,16 @@ void CloudViewer::createMenu()
 	trajectoryMenu->addAction(_aSetTrajectorySize);
 	trajectoryMenu->addAction(_aClearTrajectory);
 
+	QMenu * gridMenu = new QMenu("Grid", this);
+	gridMenu->addAction(_aShowGrid);
+	gridMenu->addAction(_aSetGridCellCount);
+	gridMenu->addAction(_aSetGridCellSize);
+
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
 	_menu->addMenu(trajectoryMenu);
-	_menu->addAction(_aShowGrid);
+	_menu->addMenu(gridMenu);
 	_menu->addAction(_aSetBackgroundColor);
 }
 
@@ -160,7 +167,7 @@ bool CloudViewer::updateCloudPose(
 	{
 		UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
 		if(_addedClouds.find(id).value() == pose ||
-		   _visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose)))
+		   _visualizer->updatePointCloudPose(id, pose.toEigen3f()))
 		{
 			_addedClouds.find(id).value() = pose;
 			return true;
@@ -243,7 +250,7 @@ bool CloudViewer::addCloud(
 	if(!_addedClouds.contains(id))
 	{
 		Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
-		Eigen::Quaternionf orientation = Eigen::Quaternionf(util3d::transformToEigen3f(pose).rotation());
+		Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().rotation());
 
 		// add random color channel
 		 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
@@ -323,7 +330,7 @@ bool CloudViewer::addCloudMesh(
 		UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 		if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id))
 		{
-			_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose));
+			_visualizer->updatePointCloudPose(id, pose.toEigen3f());
 			_addedClouds.insert(id, pose);
 			return true;
 		}
@@ -341,7 +348,7 @@ bool CloudViewer::addCloudMesh(
 		UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
 		if(_visualizer->addPolygonMesh(*mesh, id))
 		{
-			_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose));
+			_visualizer->updatePointCloudPose(id, pose.toEigen3f());
 			_addedClouds.insert(id, pose);
 			return true;
 		}
@@ -480,12 +487,22 @@ void CloudViewer::removeAllGraphs()
 	_graphes.clear();
 }
 
+bool CloudViewer::isTrajectoryShown() const
+{
+	return _aShowTrajectory->isChecked();
+}
+
+unsigned int CloudViewer::getTrajectorySize() const
+{
+	return _maxTrajectorySize;
+}
+
 void CloudViewer::setTrajectoryShown(bool shown)
 {
 	_aShowTrajectory->setChecked(shown);
 }
 
-void CloudViewer::setTrajectorySize(int value)
+void CloudViewer::setTrajectorySize(unsigned int value)
 {
 	_maxTrajectorySize = value;
 }
@@ -494,8 +511,7 @@ void CloudViewer::clearTrajectory()
 {
 	_trajectory->clear();
 	_visualizer->removeShape("trajectory");
-	_lastPose.setNull();
+	this->update();
-	this->render();
 }
 
 void CloudViewer::removeAllClouds()
@@ -521,11 +537,46 @@ bool CloudViewer::getPose(const std::string & id, Transform & pose)
 	return false;
 }
 
-void CloudViewer::updateCameraPosition(const Transform & pose)
+Transform CloudViewer::getTargetPose() const
+{
+	if(_lastPose.isNull())
+	{
+		return Transform::getIdentity();
+	}
+	return _lastPose;
+}
+
+void CloudViewer::getCameraPosition(
+		float & x, float & y, float & z,
+		float & focalX, float & focalY, float & focalZ,
+		float & upX, float & upY, float & upZ) const
+{
+	std::vector<pcl::visualization::Camera> cameras;
+	_visualizer->getCameras(cameras);
+	x = cameras.front().pos[0];
+	y = cameras.front().pos[1];
+	z = cameras.front().pos[2];
+	focalX = cameras.front().focal[0];
+	focalY = cameras.front().focal[1];
+	focalZ = cameras.front().focal[2];
+	upX = cameras.front().view[0];
+	upY = cameras.front().view[1];
+	upZ = cameras.front().view[2];
+}
+
+void CloudViewer::setCameraPosition(
+		float x, float y, float z,
+		float focalX, float focalY, float focalZ,
+		float upX, float upY, float upZ)
+{
+	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
+}
+
+void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 {
 	if(!pose.isNull())
 	{
-		Eigen::Affine3f m = util3d::transformToEigen3f(pose);
+		Eigen::Affine3f m = pose.toEigen3f();
 		Eigen::Vector3f pos = m.translation();
 
 		Eigen::Vector3f lastPos(0,0,0);
@@ -614,9 +665,9 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 				cameras.front().focal[1] = Fp.y();
 				cameras.front().focal[2] = Fp.z();
 				//FIXME: the view up is not set properly...
-				cameras.front().view[0] = Fp[8];
+				cameras.front().view[0] = _aLockViewZ->isChecked()?0:Fp[8];
-				cameras.front().view[1] = Fp[9];
+				cameras.front().view[1] = _aLockViewZ->isChecked()?0:Fp[9];
-				cameras.front().view[2] = Fp[10];
+				cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10];
 			}
 
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
@@ -636,28 +687,6 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 	_lastPose = pose;
 }
 
-void CloudViewer::render()
-{
-	// camera view up z locked?
-	if(_aLockViewZ->isChecked())
-	{
-		std::vector<pcl::visualization::Camera> cameras;
-		_visualizer->getCameras(cameras);
-
-		cameras.front().view[0] = 0;
-		cameras.front().view[1] = 0;
-		cameras.front().view[2] = 1;
-
-		_visualizer->setCameraPosition(
-			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
-			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
-
-	}
-
-	this->GetRenderWindow()->Render();
-}
-
 const QColor & CloudViewer::getBackgroundColor() const
 {
 	return _backgroundColor;
@@ -752,12 +781,73 @@ void CloudViewer::setGridShown(bool shown)
 	}
 }
 
+bool CloudViewer::isCameraTargetLocked() const
+{
+	return _aLockCamera->isChecked();
+}
+bool CloudViewer::isCameraTargetFollow() const
+{
+	return _aFollowCamera->isChecked();
+}
+bool CloudViewer::isCameraFree() const
+{
+	return !_aFollowCamera->isChecked() && !_aLockCamera->isChecked();
+}
+bool CloudViewer::isCameraLockZ() const
+{
+	return _aLockViewZ->isChecked();
+}
+bool CloudViewer::isGridShown() const
+{
+	return _aShowGrid->isChecked();
+}
+unsigned int CloudViewer::getGridCellCount() const
+{
+	return _gridCellCount;
+}
+float CloudViewer::getGridCellSize() const
+{
+	return _gridCellSize;
+}
+
+void CloudViewer::setGridCellCount(unsigned int count)
+{
+	if(count > 0)
+	{
+		_gridCellCount = count;
+		if(_aShowGrid->isChecked())
+		{
+			this->removeGrid();
+			this->addGrid();
+		}
+	}
+	else
+	{
+		UERROR("Cannot set grid cell count < 1, count=%d", count);
+	}
+}
+void CloudViewer::setGridCellSize(float size)
+{
+	if(size > 0)
+	{
+		_gridCellSize = size;
+		if(_aShowGrid->isChecked())
+		{
+			this->removeGrid();
+			this->addGrid();
+		}
+	}
+	else
+	{
+		UERROR("Cannot set grid cell size <= 0, value=%f", size);
+	}
+}
 void CloudViewer::addGrid()
 {
 	if(_gridLines.empty())
 	{
-		float cellSize = 1.0f;
+		float cellSize = _gridCellSize;
-		int cellCount = 50;
+		int cellCount = _gridCellCount;
 		double r=0.5;
 		double g=0.5;
 		double b=0.5;
@@ -927,7 +1017,9 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
 			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 
-		render();
+		update();
+
+		emit configChanged();
 	}
 	else
 	{
@@ -935,12 +1027,47 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 	}
 }
 
+void CloudViewer::mousePressEvent(QMouseEvent * event)
+{
+	if(event->button() == Qt::RightButton)
+	{
+		event->accept();
+	}
+	else
+	{
+		QVTKWidget::mousePressEvent(event);
+	}
+}
+
+void CloudViewer::mouseMoveEvent(QMouseEvent * event)
+{
+	QVTKWidget::mouseMoveEvent(event);
+	// camera view up z locked?
+	if(_aLockViewZ->isChecked())
+	{
+		std::vector<pcl::visualization::Camera> cameras;
+		_visualizer->getCameras(cameras);
+
+		cameras.front().view[0] = 0;
+		cameras.front().view[1] = 0;
+		cameras.front().view[2] = 1;
+
+		_visualizer->setCameraPosition(
+			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+
+	}
+	emit configChanged();
+}
+
 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QAction * a = _menu->exec(event->globalPos());
 	if(a)
 	{
 		handleAction(a);
+		emit configChanged();
 	}
 }
 
@@ -961,11 +1088,32 @@ void CloudViewer::handleAction(QAction * a)
 	}
 	else if(a == _aResetCamera)
 	{
-		_visualizer->setCameraPosition(
+		if((_aFollowCamera->isChecked() || _aLockCamera->isChecked()) && !_lastPose.isNull())
-				-1, 0, 0,
+		{
-				0, 0, 0,
+			// reset relative to last current pose
-				0, 0, 1);
+			if(_aLockViewZ->isChecked())
-		this->render();
+			{
+				_visualizer->setCameraPosition(
+						_lastPose.x()-1, _lastPose.y(), _lastPose.z(),
+						_lastPose.x(), _lastPose.y(), _lastPose.z(),
+						0, 0, 1);
+			}
+			else
+			{
+				_visualizer->setCameraPosition(
+						_lastPose.x()-1, _lastPose.y(), _lastPose.z(),
+						_lastPose.x(), _lastPose.y(), _lastPose.z(),
+						_lastPose.r31(), _lastPose.r32(), _lastPose.r33());
+			}
+		}
+		else
+		{
+			_visualizer->setCameraPosition(
+					-1, 0, 0,
+					0, 0, 0,
+					0, 0, 1);
+		}
+		this->update();
 	}
 	else if(a == _aShowGrid)
 	{
@@ -978,14 +1126,39 @@ void CloudViewer::handleAction(QAction * a)
 			this->removeGrid();
 		}
 
-		this->render();
+		this->update();
+	}
+	else if(a == _aSetGridCellCount)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set grid cell count"), tr("Count"), _gridCellCount, 1, 10000, 10, &ok);
+		if(ok)
+		{
+			this->setGridCellCount(value);
+		}
+	}
+	else if(a == _aSetGridCellSize)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
+		if(ok)
+		{
+			this->setGridCellSize(value);
+		}
 	}
 	else if(a == _aSetBackgroundColor)
 	{
-		QColor color = Qt::black;
+		QColor color = this->getBackgroundColor();
 		color = QColorDialog::getColor(color, this);
 		this->setBackgroundColor(color);
 	}
+	else if(a == _aLockViewZ)
+	{
+		if(_aLockViewZ->isChecked())
+		{
+			this->update();
+		}
+	}
 }
 
 } /* namespace rtabmap */

guilib/src/DataRecorder.cpp

@@ -60,7 +60,7 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
 		ParametersMap customParameters;
 		customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
 		customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
-		customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true")); // to keep images
+		customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
 		if(!recordInRAM)
 		{
 			customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // to keep images

guilib/src/DatabaseViewer.cpp

@@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/core/Graph.h"
 #include "rtabmap/gui/DataRecorder.h"
 #include "rtabmap/core/SensorData.h"
 #include "ExportDialog.h"
@@ -239,14 +241,14 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 			// Added links
 			for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
 			{
-				std::multimap<int, rtabmap::Link>::iterator refinedIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
+				std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
-					memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
+					memory_->addLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
 				}
 				else
 				{
-					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
+					memory_->addLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
 				}
 			}
 
@@ -255,15 +257,14 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 			{
 				if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
 				{
-					memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
+					memory_->updateLink(iter->second.from(), iter->second.to(), iter->second.transform(), iter->second.variance());
-					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
 				}
 			}
 
 			// Rejected links
 			for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
 			{
-				memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
+				memory_->removeLink(iter->second.to(), iter->second.from());
 			}
 		}
 
@@ -373,7 +374,7 @@ void DatabaseViewer::extractImages()
 			cv::Mat compressedRgb = memory_->getImageCompressed(id);
 			if(!compressedRgb.empty())
 			{
-				cv::Mat imageMat = rtabmap::util3d::uncompressImage(compressedRgb);
+				cv::Mat imageMat = rtabmap::uncompressImage(compressedRgb);
 				cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat);
 				UINFO(QString("Saved %1/%2.png").arg(path).arg(id).toStdString().c_str());
 			}
@@ -414,6 +415,10 @@ void DatabaseViewer::updateIds()
 				if((!iter->second.isNull() && nullPoses) ||
 					(iter->second.isNull() && !nullPoses))
 				{
+					if(iter->second.isNull())
+					{
+						UWARN("Pose %d is null!", iter->first);
+					}
 					UWARN("Mixed valid and null poses! Ignoring graph...");
 					poses_.clear();
 					links_.clear();
@@ -569,7 +574,7 @@ void DatabaseViewer::generateTOROGraph()
 		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
 		if(!path.isEmpty())
 		{
-			rtabmap::util3d::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
+			rtabmap::graph::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
 		}
 	}
 }
@@ -801,7 +806,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("iteration %d/%d", n+1, iterations);
-		std::multimap<int, int> clusters = util3d::radiusPosesClustering(
+		std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
 				optimizedPoses,
 				ui_->doubleSpinBox_detectMore_radius->value(),
 				ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
@@ -1129,7 +1134,7 @@ void DatabaseViewer::update(int value,
 			ui_->horizontalSlider_loops->setValue(0);
 			ui_->horizontalSlider_neighbors->setValue(0);
 			ui_->constraintsViewer->removeAllClouds();
-			ui_->constraintsViewer->render();
+			ui_->constraintsViewer->update();
 			ui_->horizontalSlider_loops->blockSignals(false);
 			ui_->horizontalSlider_neighbors->blockSignals(false);
 		}
@@ -1216,7 +1221,7 @@ void DatabaseViewer::updateStereo(const Signature * data)
 
 						if(pcl::isFinite(tmpPt))
 						{
-							pt = pcl::transformPoint(tmpPt, util3d::transformToEigen3f(data->getLocalTransform()));
+							pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
 							if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2)
 							{
 								status[i] = 100; //blue
@@ -1240,9 +1245,9 @@ void DatabaseViewer::updateStereo(const Signature * data)
 		UINFO("correspondences = %d/%d (%f) (time kpt=%fs flow=%fs)",
 				(int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeFlow);
 
-		ui_->stereoViewer->updateCameraPosition(Transform::getIdentity());
+		ui_->stereoViewer->updateCameraTargetPosition(Transform::getIdentity());
 		ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
-		ui_->stereoViewer->render();
+		ui_->stereoViewer->update();
 
 		std::vector<cv::KeyPoint> rightKpts;
 		cv::KeyPoint::convert(rightCorners, rightKpts);
@@ -1413,7 +1418,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
 		bool updateImageSliders)
 {
-	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, linkIn.from(), linkIn.to());
+	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
 	if(iter != linksRefined_.end())
 	{
@@ -1655,10 +1660,10 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 		}
 
 		//update cordinate
-		ui_->constraintsViewer->updateCameraPosition(t);
+		ui_->constraintsViewer->updateCameraTargetPosition(t);
 		ui_->constraintsViewer->clearTrajectory();
 
-		ui_->constraintsViewer->render();
+		ui_->constraintsViewer->update();
 	}
 
 	// update buttons
@@ -1696,7 +1701,7 @@ void DatabaseViewer::updateConstraintButtons()
 
 		//check for modified link
 		bool modified = false;
-		std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, currentLink.from(), currentLink.to());
+		std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
 		if(iter != linksRefined_.end())
 		{
 			currentLink = iter->second;
@@ -1726,7 +1731,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				if(!data.getLaserScanCompressed().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cv::Mat laserScan = rtabmap::util3d::uncompressData(data.getLaserScanCompressed());
+					cv::Mat laserScan = rtabmap::uncompressData(data.getLaserScanCompressed());
 					cloud = rtabmap::util3d::laserScanToPointCloud(laserScan);
 					scans_.insert(std::make_pair(ids_.at(i), cloud));
 				}
@@ -1786,8 +1791,8 @@ void DatabaseViewer::updateGraphView()
 		graphes_.push_back(poses_);
 		ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
 		std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-		std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
+		std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
-		util3d::optimizeTOROGraph(
+		rtabmap::graph::optimizeTOROGraph(
 				depthGraph,
 				poses_,
 				links, finalPoses,
@@ -1813,21 +1818,21 @@ void DatabaseViewer::updateGraphView()
 Link DatabaseViewer::findActiveLink(int from, int to)
 {
 	Link link;
-	std::multimap<int, Link>::iterator findIter = util3d::findLink(linksRefined_, from ,to);
+	std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
 	if(findIter != linksRefined_.end())
 	{
 		link = findIter->second;
 	}
 	else
 	{
-		findIter = util3d::findLink(linksAdded_, from ,to);
+		findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
 		if(findIter != linksAdded_.end())
 		{
 			link = findIter->second;
 		}
 		else if(!containsLink(linksRemoved_, from ,to))
 		{
-			findIter = util3d::findLink(links_, from ,to);
+			findIter = rtabmap::graph::findLink(links_, from ,to);
 			if(findIter != links_.end())
 			{
 				link = findIter->second;
@@ -1839,7 +1844,7 @@ Link DatabaseViewer::findActiveLink(int from, int to)
 
 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
 {
-	return util3d::findLink(links, from, to) != links.end();
+	return rtabmap::graph::findLink(links, from, to) != links.end();
 }
 
 void DatabaseViewer::refineConstraint()
@@ -1897,8 +1902,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
-		cv::Mat oldLaserScan = util3d::uncompressData(dataFrom.getLaserScanCompressed());
+		cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed());
-		cv::Mat newLaserScan = util3d::uncompressData(dataTo.getLaserScanCompressed());
+		cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed());
 
 		if(!oldLaserScan.empty() && !newLaserScan.empty())
 		{
@@ -1928,13 +1933,13 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 	else
 	{
 		//3D
-		cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepthCompressed());
+		cv::Mat depthA = rtabmap::uncompressImage(dataFrom.getDepthCompressed());
-		cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepthCompressed());
+		cv::Mat depthB = rtabmap::uncompressImage(dataTo.getDepthCompressed());
 
 		if(depthA.type() == CV_8UC1)
 		{
 			cv::Mat leftMono;
-			cv::Mat left = rtabmap::util3d::uncompressImage(dataFrom.getImageCompressed());
+			cv::Mat left = rtabmap::uncompressImage(dataFrom.getImageCompressed());
 			if(left.channels() > 1)
 			{
 				cv::cvtColor(left, leftMono, CV_BGR2GRAY);
@@ -1967,7 +1972,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 		if(depthB.type() == CV_8UC1)
 		{
 			cv::Mat leftMono;
-			cv::Mat left = rtabmap::util3d::uncompressImage(dataTo.getImageCompressed());
+			cv::Mat left = rtabmap::uncompressImage(dataTo.getImageCompressed());
 			if(left.channels() > 1)
 			{
 				cv::cvtColor(left, leftMono, CV_BGR2GRAY);
@@ -2266,7 +2271,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 				// We are 2D here, make sure the guess has only YAW rotation
 				float x,y,z,r,p,yaw;
 				t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
-				t = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+				t = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
 			}
 
 			// transform is valid, make a link
@@ -2277,7 +2282,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 	else if(containsLink(linksRemoved_, from, to))
 	{
 		//simply remove from linksRemoved
-		linksRemoved_.erase(util3d::findLink(linksRemoved_, from, to));
+		linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
 		updateSlider = true;
 	}
 
@@ -2310,19 +2315,19 @@ void DatabaseViewer::resetConstraint()
 	}
 
 
-	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, from, to);
+	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 		this->updateGraphView();
 	}
 
-	iter = util3d::findLink(links_, from, to);
+	iter = rtabmap::graph::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		this->updateConstraintView(iter->second);
 	}
-	iter = util3d::findLink(linksAdded_, from, to);
+	iter = rtabmap::graph::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		this->updateConstraintView(iter->second);
@@ -2350,7 +2355,7 @@ void DatabaseViewer::rejectConstraint()
 
 	// find the original one
 	std::multimap<int, Link>::iterator iter;
-	iter = util3d::findLink(links_, from, to);
+	iter = rtabmap::graph::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		if(iter->second.type() == Link::kNeighbor)
@@ -2363,13 +2368,13 @@ void DatabaseViewer::rejectConstraint()
 	}
 
 	// remove from refined and added
-	iter = util3d::findLink(linksRefined_, from, to);
+	iter = rtabmap::graph::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 		removed = true;
 	}
-	iter = util3d::findLink(linksAdded_, from, to);
+	iter = rtabmap::graph::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		linksAdded_.erase(iter);
@@ -2392,7 +2397,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 	{
 		std::multimap<int, rtabmap::Link>::iterator findIter;
 
-		findIter = util3d::findLink(linksRemoved_, iter->second.from(), iter->second.to());
+		findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
 		if(findIter != linksRemoved_.end())
 		{
 			if(!(iter->second.from() == findIter->second.from() &&
@@ -2410,7 +2415,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			}
 		}
 
-		findIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
+		findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
 		if(findIter!=linksRefined_.end())
 		{
 			if(iter->second.from() == findIter->second.from() &&
@@ -2488,7 +2493,7 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
 	{
 		ui_->horizontalSlider_loops->setEnabled(false);
 		ui_->constraintsViewer->removeAllClouds();
-		ui_->constraintsViewer->render();
+		ui_->constraintsViewer->update();
 		updateConstraintButtons();
 	}
 }

guilib/src/DetailedProgressDialog.cpp

@@ -82,10 +82,10 @@ void DetailedProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec)
 	_closeWhenDoneCheckBox->setChecked(on);
 }
 
-void DetailedProgressDialog::appendText(const QString & text)
+void DetailedProgressDialog::appendText(const QString & text, const QColor & color)
 {
 	_text->setText(text);
-	QString html = tr("<html><font color=\"#999999\">%1 </font>%2</html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(text);
+	QString html = tr("<html><font color=\"#999999\">%1 </font><font color=\"%2\">%3</font></html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text);
 	_detailedText->append(html);
 	_detailedText->ensureCursorVisible();
 }

guilib/src/DetailedProgressDialog.h

@@ -56,7 +56,7 @@ protected:
 	virtual void closeEvent(QCloseEvent * event);
 
 public slots:
-	void appendText(const QString & text);
+	void appendText(const QString & text ,const QColor & color = Qt::black);
 	void incrementStep();
 	void clear();
 	void resetProgress();

guilib/src/ExportCloudsDialog.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ExportCloudsDialog.h"
 #include "ui_exportCloudsDialog.h"
 
-#include <QtGui/QFileDialog>
+#include <QtGui/QPushButton>
 
 namespace rtabmap {
 
@@ -37,6 +37,21 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 {
 	_ui = new Ui_ExportCloudsDialog();
 	_ui->setupUi(this);
+
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->groupBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->groupBox_regenerate, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->groupBox_mls, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_mlsRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->groupBox_gp3, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 }
 
 ExportCloudsDialog::~ExportCloudsDialog()
@@ -44,18 +59,49 @@ ExportCloudsDialog::~ExportCloudsDialog()
 	delete _ui;
 }
 
+void ExportCloudsDialog::restoreDefaults()
+{
+	setAssemble(true);
+	setAssembleVoxel(0.005);
+	if(_ui->groupBox_regenerate->isEnabled())
+	{
+		setGenerate(true);
+	}
+	setGenerateDecimation(1);
+	setGenerateVoxel(0.005);
+	setGenerateMaxDepth(4);
+	setBinaryFile(true);
+	setMLS(false);
+	setMLSRadius(0.04);
+	setMesh(false);
+	setMeshNormalKSearch(20);
+	setMeshGp3Radius(0.04);
+}
+
 void ExportCloudsDialog::setSaveButton()
 {
-	_ui->buttonBox->setStandardButtons(QDialogButtonBox::Cancel|QDialogButtonBox::Save);
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(false);
+	_ui->buttonBox->button(QDialogButtonBox::Save)->setVisible(true);
 	_ui->checkBox_binary->setVisible(true);
 }
 
 void ExportCloudsDialog::setOkButton()
 {
-	_ui->buttonBox->setStandardButtons(QDialogButtonBox::Cancel|QDialogButtonBox::Ok);
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(true);
+	_ui->buttonBox->button(QDialogButtonBox::Save)->setVisible(false);
 	_ui->checkBox_binary->setVisible(false);
 }
 
+void ExportCloudsDialog::enableRegeneration(bool enabled)
+{
+	if(!enabled)
+	{
+		_ui->groupBox_regenerate->setChecked(false);
+	}
+	_ui->groupBox_regenerate->setEnabled(enabled);
+}
+
+//getters
 bool ExportCloudsDialog::getAssemble() const
 {
 	return _ui->groupBox_assemble->isChecked();
@@ -116,5 +162,55 @@ double ExportCloudsDialog::getMeshGp3Radius() const
 	return _ui->doubleSpinBox_gp3Radius->value();
 }
 
+//setters
+void ExportCloudsDialog::setAssemble(bool on)
+{
+	_ui->groupBox_assemble->setChecked(on);
+}
+void ExportCloudsDialog::setAssembleVoxel(double voxel)
+{
+	_ui->doubleSpinBox_voxelSize_assembled->setValue(voxel);
+}
+void ExportCloudsDialog::setGenerate(bool on)
+{
+	_ui->groupBox_regenerate->setChecked(on);
+}
+void ExportCloudsDialog::setGenerateDecimation(int decimation)
+{
+	_ui->spinBox_decimation->setValue(decimation);
+}
+void ExportCloudsDialog::setGenerateVoxel(double voxel)
+{
+	_ui->doubleSpinBox_voxelSize->setValue(voxel);
+}
+void ExportCloudsDialog::setGenerateMaxDepth(double maxDepth)
+{
+	_ui->doubleSpinBox_maxDepth->setValue(maxDepth);
+}
+void ExportCloudsDialog::setBinaryFile(bool on)
+{
+	_ui->checkBox_binary->setChecked(on);
+}
+void ExportCloudsDialog::setMLS(bool on)
+{
+	_ui->groupBox_mls->setChecked(on);
+}
+void ExportCloudsDialog::setMLSRadius(double radius)
+{
+	_ui->doubleSpinBox_mlsRadius->setValue(radius);
+}
+void ExportCloudsDialog::setMesh(bool on)
+{
+	_ui->groupBox_gp3->setChecked(on);
+}
+void ExportCloudsDialog::setMeshNormalKSearch(int k)
+{
+	_ui->spinBox_normalKSearch->setValue(k);
+}
+void ExportCloudsDialog::setMeshGp3Radius(double radius)
+{
+	_ui->doubleSpinBox_gp3Radius->setValue(radius);
+}
+
 
 }

guilib/src/ExportCloudsDialog.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QDialog>
 
 class Ui_ExportCloudsDialog;
+class QAbstractButton;
 
 namespace rtabmap {
 
@@ -45,7 +46,9 @@ public:
 
 	void setSaveButton();
 	void setOkButton();
+	void enableRegeneration(bool enabled);
 
+	//getters
 	bool getAssemble() const;
 	double getAssembleVoxel() const;
 	bool getGenerate() const;
@@ -59,6 +62,26 @@ public:
 	int getMeshNormalKSearch() const;
 	double getMeshGp3Radius() const;
 
+	//setters
+	void setAssemble(bool on);
+	void setAssembleVoxel(double voxel);
+	void setGenerate(bool on);
+	void setGenerateDecimation(int decimation);
+	void setGenerateVoxel(double voxel);
+	void setGenerateMaxDepth(double maxDepth);
+	void setBinaryFile(bool on);
+	void setMLS(bool on);
+	void setMLSRadius(double radius);
+	void setMesh(bool on);
+	void setMeshNormalKSearch(int k);
+	void setMeshGp3Radius(double radius);
+
+signals:
+	void configChanged();
+
+public slots:
+	void restoreDefaults();
+
 private:
 	Ui_ExportCloudsDialog * _ui;
 };

guilib/src/ExportDialog.cpp

@@ -40,6 +40,12 @@ ExportDialog::ExportDialog(QWidget * parent) :
 
 	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
 
+	connect(_ui->spinBox_ignored, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_rgb, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_depth2d, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+
 	_ui->lineEdit_path->setText(QDir::homePath()+QDir::separator()+"output.db");
 }
 

guilib/src/ExportDialog.h

@@ -50,6 +50,9 @@ public:
 	bool isDepth2dExported() const;
 	bool isOdomExported() const;
 
+signals:
+	void configChanged();
+
 private slots:
 	void getPath();
 

guilib/src/GraphViewer.cpp

@@ -101,13 +101,13 @@ class LinkItem: public QGraphicsLineItem
 {
 public:
 	// in meter
-	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, bool loopClosure) :
+	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, Link::Type type) :
 		QGraphicsLineItem(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x()),
 		_from(from),
 		_to(to),
 		_poseA(poseA),
 		_poseB(poseB),
-		_loopClosure(loopClosure)
+		_type(type)
 	{
 		this->setAcceptHoverEvents(true);
 	}
@@ -126,7 +126,7 @@ public:
 		_poseB = poseB;
 	}
 
-	bool isLoopClosure() const {return _loopClosure;}
+	Link::Type linkType() const {return _type;}
 	int from() const {return _from;}
 	int to() const {return _to;}
 
@@ -146,7 +146,7 @@ private:
 	int _to;
 	Transform _poseA;
 	Transform _poseB;
-	bool _loopClosure;
+	Link::Type _type;
 };
 
 GraphViewer::GraphViewer(QWidget * parent) :
@@ -154,6 +154,10 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_nodeColor(Qt::blue),
 		_neighborColor(Qt::blue),
 		_loopClosureColor(Qt::red),
+		_loopClosureLocalColor(Qt::yellow),
+		_loopClosureUserColor(Qt::red),
+		_loopClosureVirtualColor(Qt::magenta),
+		_pathColor(Qt::cyan),
 		_root(0),
 		_nodeRadius(0.01),
 		_linkWidth(0),
@@ -192,6 +196,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	_gridMap = this->scene()->addPixmap(QPixmap());
 	_gridMap->setZValue(0);
 	_gridMap->setParentItem(_root);
+
+	this->restoreDefaults();
 }
 
 GraphViewer::~GraphViewer()
@@ -201,6 +207,7 @@ GraphViewer::~GraphViewer()
 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::multimap<int, Link> & constraints)
 {
+	bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
 	UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
 	//Hide nodes and links
 	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
@@ -208,11 +215,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 		iter.value()->hide();
 		iter.value()->setColor(_nodeColor); // reset color
 	}
-	for(QMultiMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end(); ++iter)
+	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
-	{
-		iter.value()->hide();
-	}
-	for(QMultiMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end(); ++iter)
 	{
 		iter.value()->hide();
 	}
@@ -243,41 +246,50 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
-		std::map<int, Transform>::const_iterator jterA = poses.find(iter->first);
+		// make the first id the smallest one
-		std::map<int, Transform>::const_iterator jterB = poses.find(iter->second.to());
+		int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
+		int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
+
+		std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
+		std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
 		if(jterA != poses.end() && jterB != poses.end() &&
-		   _nodeItems.contains(iter->first) && _nodeItems.contains(iter->second.to()))
+		   _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;
-			bool loopClosure = iter->second.type() != Link::kNeighbor;
 
 			bool added = false;
-			if(loopClosure && _loopLinkItems.contains(iter->first))
+			if(_linkItems.contains(idFrom))
 			{
-				QMultiMap<int, LinkItem*>::iterator itemIter = _loopLinkItems.find(iter->first);
+				QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(iter->first);
-				while(itemIter.key() == iter->first && itemIter != _loopLinkItems.end())
+				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
 				{
-					if(itemIter.value()->to() == iter->second.to())
+					if(itemIter.value()->to() == idTo)
-					{
-						itemIter.value()->setPoses(poseA, poseB);
-						itemIter.value()->show();
-						added = true;
-						break;
-					}
-					++itemIter;
-				}
-			}
-			else if(!loopClosure && _neighborLinkItems.contains(iter->first))
-			{
-				QMultiMap<int, LinkItem*>::iterator itemIter = _neighborLinkItems.find(iter->first);
-				while(itemIter.key() == iter->first && itemIter != _neighborLinkItems.end())
-				{
-					if(itemIter.value()->to() == iter->second.to())
 					{
 						itemIter.value()->setPoses(poseA, poseB);
 						itemIter.value()->show();
 						added = true;
+						// reset color
+						if(iter->second.type() == Link::kNeighbor)
+						{
+							itemIter.value()->setColor(_neighborColor);
+						}
+						else if(iter->second.type() == Link::kVirtualClosure)
+						{
+							itemIter.value()->setColor(_loopClosureVirtualColor);
+						}
+						else if(iter->second.type() == Link::kUserClosure)
+						{
+							itemIter.value()->setColor(_loopClosureUserColor);
+						}
+						else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
+						{
+							itemIter.value()->setColor(_loopClosureLocalColor);
+						}
+						else
+						{
+							itemIter.value()->setColor(_loopClosureColor);
+						}
 						break;
 					}
 					++itemIter;
@@ -286,22 +298,34 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			if(!added)
 			{
 				//create a link item
-				LinkItem * item = new LinkItem(iter->first, iter->second.to(), poseA, poseB, loopClosure);
+				LinkItem * item = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type());
 				QPen p = item->pen();
 				p.setWidthF(_linkWidth);
 				item->setPen(p);
-				item->setColor(loopClosure?_loopClosureColor:_neighborColor);
+				if(iter->second.type() == Link::kNeighbor)
-				this->scene()->addItem(item);
-				item->setZValue(1);
-				item->setParentItem(_root);
-				if(loopClosure)
 				{
-					_loopLinkItems.insert(iter->first, item);
+					item->setColor(_neighborColor);
+				}
+				else if(iter->second.type() == Link::kVirtualClosure)
+				{
+					item->setColor(_loopClosureVirtualColor);
+				}
+				else if(iter->second.type() == Link::kUserClosure)
+				{
+					item->setColor(_loopClosureUserColor);
+				}
+				else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
+				{
+					item->setColor(_loopClosureLocalColor);
 				}
 				else
 				{
-					_neighborLinkItems.insert(iter->first, item);
+					item->setColor(_loopClosureColor);
 				}
+				this->scene()->addItem(item);
+				item->setZValue(1);
+				item->setParentItem(_root);
+				_linkItems.insert(idFrom, item);
 			}
 		}
 	}
@@ -319,24 +343,12 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			++iter;
 		}
 	}
-	for(QMultiMap<int, LinkItem*>::iterator iter = _loopLinkItems.begin(); iter!=_loopLinkItems.end();)
+	for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end();)
 	{
 		if(!iter.value()->isVisible())
 		{
 			delete iter.value();
-			iter = _loopLinkItems.erase(iter);
+			iter = _linkItems.erase(iter);
-		}
-		else
-		{
-			++iter;
-		}
-	}
-	for(QMultiMap<int, LinkItem*>::iterator iter = _neighborLinkItems.begin(); iter!=_neighborLinkItems.end();)
-	{
-		if(!iter.value()->isVisible())
-		{
-			delete iter.value();
-			iter = _neighborLinkItems.erase(iter);
 		}
 		else
 		{
@@ -356,7 +368,15 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	}
 
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-	this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+
+	if(poses.size() == 0 || wasEmpty)
+	{
+		this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+	}
+	else
+	{
+		this->centerOn(_lastReferential);
+	}
 }
 
 void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
@@ -407,14 +427,37 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
 	}
 }
 
+void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
+{
+	if(localPath.size() > 1)
+	{
+		for(unsigned int i=0; i<localPath.size()-1; ++i)
+		{
+			if(_linkItems.contains(localPath[i]))
+			{
+				int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
+				int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
+				QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(idFrom);
+				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
+				{
+					if(itemIter.value()->to() == idTo)
+					{
+						itemIter.value()->setColor(_pathColor);
+						break;
+					}
+					++itemIter;
+				}
+			}
+		}
+	}
+}
+
 void GraphViewer::clearGraph()
 {
 	qDeleteAll(_nodeItems);
 	_nodeItems.clear();
-	qDeleteAll(_neighborLinkItems);
+	qDeleteAll(_linkItems);
-	_neighborLinkItems.clear();
+	_linkItems.clear();
-	qDeleteAll(_loopLinkItems);
-	_loopLinkItems.clear();
 	_lastReferential->resetTransform();
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
 }
@@ -438,6 +481,129 @@ void GraphViewer::clearAll()
 	clearGraph();
 }
 
+bool GraphViewer::isGridMapVisible() const
+{
+	return _gridMap->isVisible();
+}
+
+void GraphViewer::setWorkingDirectory(const QString & path)
+{
+	_workingDirectory = path;
+}
+void GraphViewer::setNodeRadius(float radius)
+{
+	_nodeRadius = radius;
+	for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+	{
+		iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
+	}
+}
+void GraphViewer::setLinkWidth(float width)
+{
+	_linkWidth = width;
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
+		if(line)
+		{
+			QPen pen = line->pen();
+			pen.setWidthF(_linkWidth);
+			line->setPen(pen);
+		}
+	}
+}
+void GraphViewer::setNodeColor(const QColor & color)
+{
+	_nodeColor = color;
+	for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+	{
+		iter.value()->setColor(_nodeColor);
+	}
+}
+void GraphViewer::setNeighborColor(const QColor & color)
+{
+	_neighborColor = color;
+	for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
+	{
+		if(iter.value()->linkType() == Link::kNeighbor)
+		{
+			iter.value()->setColor(_neighborColor);
+		}
+	}
+}
+void GraphViewer::setGlobalLoopClosureColor(const QColor & color)
+{
+	_loopClosureColor = color;
+	for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
+	{
+		if(iter.value()->linkType() != Link::kNeighbor &&
+			iter.value()->linkType() != Link::kLocalSpaceClosure &&
+			iter.value()->linkType() != Link::kLocalTimeClosure &&
+			iter.value()->linkType() != Link::kUserClosure &&
+			iter.value()->linkType() != Link::kVirtualClosure)
+		{
+			iter.value()->setColor(_loopClosureColor);
+		}
+	}
+}
+void GraphViewer::setLocalLoopClosureColor(const QColor & color)
+{
+	_loopClosureLocalColor = color;
+	for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
+	{
+		if(iter.value()->linkType() == Link::kLocalSpaceClosure ||
+		   iter.value()->linkType() == Link::kLocalTimeClosure)
+		{
+			iter.value()->setColor(_loopClosureLocalColor);
+		}
+	}
+}
+void GraphViewer::setUserLoopClosureColor(const QColor & color)
+{
+	_loopClosureUserColor = color;
+	for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
+	{
+		if(iter.value()->linkType() == Link::kUserClosure)
+		{
+			iter.value()->setColor(_loopClosureUserColor);
+		}
+	}
+}
+void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
+{
+	_loopClosureVirtualColor = color;
+	for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
+	{
+		if(iter.value()->linkType() == Link::kVirtualClosure)
+		{
+			iter.value()->setColor(_loopClosureVirtualColor);
+		}
+	}
+}
+void GraphViewer::setLocalPathColor(const QColor & color)
+{
+	_pathColor = color;
+}
+void GraphViewer::setGridMapVisible(bool visible)
+{
+	_gridMap->setVisible(visible);
+}
+
+void GraphViewer::restoreDefaults()
+{
+	setNodeRadius(0.01f);
+	setLinkWidth(0.0f);
+	setNodeColor(Qt::blue);
+	setNeighborColor(Qt::blue);
+	setGlobalLoopClosureColor(Qt::red);
+	setLocalLoopClosureColor(Qt::yellow);
+	setUserLoopClosureColor(Qt::red);
+	setVirtualLoopClosureColor(Qt::magenta);
+	setGridMapVisible(true);
+}
+
+
 void GraphViewer::wheelEvent ( QWheelEvent * event )
 {
 	if(event->delta() > 0)
@@ -450,15 +616,44 @@ void GraphViewer::wheelEvent ( QWheelEvent * event )
 	}
 }
 
+QIcon createIcon(const QColor & color)
+{
+	QPixmap pixmap(50, 50);
+	pixmap.fill(color);
+	return QIcon(pixmap);
+}
+
 void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QMenu menu;
 	QAction * aScreenShotPNG = menu.addAction(tr("Take a screenshot (PNG)"));
 	QAction * aScreenShotSVG = menu.addAction(tr("Take a screenshot (SVG)"));
 	menu.addSeparator();
-	QAction * aChangeNodeColor = menu.addAction(tr("Set node color..."));
+
-	QAction * aChangeNeighborColor = menu.addAction(tr("Set neighbor link color..."));
+	QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
-	QAction * aChangeLoopColor = menu.addAction(tr("Set loop closure link color..."));
+	aChangeNodeColor->setIconVisibleInMenu(true);
+
+	// Links
+	QMenu * menuLink = menu.addMenu(tr("Set link color..."));
+	QAction * aChangeNeighborColor = menuLink->addAction(tr("Neighbor"));
+	QAction * aChangeGlobalLoopColor = menuLink->addAction(tr("Global loop closure"));
+	QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
+	QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
+	QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
+	QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
+	aChangeNeighborColor->setIcon(createIcon(_neighborColor));
+	aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
+	aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
+	aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
+	aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
+	aChangeLocalPathColor->setIcon(createIcon(_pathColor));
+	aChangeNeighborColor->setIconVisibleInMenu(true);
+	aChangeGlobalLoopColor->setIconVisibleInMenu(true);
+	aChangeLocalLoopColor->setIconVisibleInMenu(true);
+	aChangeUserLoopColor->setIconVisibleInMenu(true);
+	aChangeVirtualLoopColor->setIconVisibleInMenu(true);
+	aChangeLocalPathColor->setIconVisibleInMenu(true);
+
 	menu.addSeparator();
 	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
 	QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
@@ -472,6 +667,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		aShowHideGridMap = menu.addAction(tr("Show grid map"));
 	}
+	menu.addSeparator();
+	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
 	QAction * r = menu.exec(event->globalPos());
 	if(r == aScreenShotPNG || r == aScreenShotSVG)
@@ -535,56 +732,81 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 
 			QDesktopServices::openUrl(QUrl::fromLocalFile(targetDir + name));
 		}
+		return; // without emitting configChanged
 	}
 	else if(r == aChangeNodeColor ||
 			r == aChangeNeighborColor ||
-			r == aChangeLoopColor)
+			r == aChangeGlobalLoopColor ||
+			r == aChangeLocalLoopColor ||
+			r == aChangeUserLoopColor ||
+			r == aChangeVirtualLoopColor ||
+			r == aChangeLocalPathColor)
 	{
 		QColor color;
 		if(r == aChangeNodeColor)
 		{
 			color = _nodeColor;
 		}
-		else if(r == aChangeNeighborColor)
+		else if(r == aChangeGlobalLoopColor)
-		{
-			color = _neighborColor;
-		}
-		else if(r == aChangeLoopColor)
 		{
 			color = _loopClosureColor;
 		}
+		else if(r == aChangeLocalLoopColor)
+		{
+			color = _loopClosureLocalColor;
+		}
+		else if(r == aChangeUserLoopColor)
+		{
+			color = _loopClosureUserColor;
+		}
+		else if(r == aChangeVirtualLoopColor)
+		{
+			color = _loopClosureVirtualColor;
+		}
+		else if(r == aChangeLocalPathColor)
+		{
+			color = _pathColor;
+		}
+		else //if(r == aChangeNeighborColor)
+		{
+			color = _neighborColor;
+		}
 		color = QColorDialog::getColor(color, this);
+		if(color.isValid())
+		{
 
-		if(r == aChangeNodeColor)
+			if(r == aChangeNodeColor)
-		{
-			_nodeColor = color;
-		}
-		else if(r == aChangeNeighborColor)
-		{
-			_neighborColor = color;
-		}
-		else if(r == aChangeLoopColor)
-		{
-			_loopClosureColor = color;
-		}
-
-		QList<QGraphicsItem*> items = this->scene()->items();
-		for(int i=0; i<items.size(); ++i)
-		{
-			LinkItem * link = qgraphicsitem_cast<LinkItem *>(items[i]);
-			NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
-			if(r == aChangeNodeColor && node)
 			{
-				node->setColor(color);
+				this->setNodeColor(color);
 			}
-			else if(r == aChangeNeighborColor && link && !link->isLoopClosure())
+			else if(r == aChangeGlobalLoopColor)
 			{
-				link->setColor(color);
+				this->setGlobalLoopClosureColor(color);
 			}
-			else if(r == aChangeLoopColor && link && link->isLoopClosure())
+			else if(r == aChangeLocalLoopColor)
 			{
-				link->setColor(color);
+				this->setLocalLoopClosureColor(color);
 			}
+			else if(r == aChangeUserLoopColor)
+			{
+				this->setUserLoopClosureColor(color);
+			}
+			else if(r == aChangeVirtualLoopColor)
+			{
+				this->setVirtualLoopClosureColor(color);
+			}
+			else if(r == aChangeLocalPathColor)
+			{
+				this->setLocalPathColor(color);
+			}
+			else //if(r == aChangeNeighborColor)
+			{
+				this->setNeighborColor(color);
+			}
+		}
+		else
+		{
+			return; // without emitting configChanged
 		}
 	}
 	else if(r == aSetNodeSize)
@@ -593,16 +815,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 100, 2, &ok);
 		if(ok)
 		{
-			_nodeRadius = value;
+			setNodeRadius(value);
-			QList<QGraphicsItem*> items = this->scene()->items();
-			for(int i=0; i<items.size(); ++i)
-			{
-				NodeItem * node = qgraphicsitem_cast<NodeItem *>(items[i]);
-				if(node)
-				{
-					node->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
-				}
-			}
 		}
 	}
 	else if(r == aSetLinkSize)
@@ -611,23 +824,20 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 100, 2, &ok);
 		if(ok)
 		{
-			_linkWidth = value;
+			setLinkWidth(value);
-			QList<QGraphicsItem*> items = this->scene()->items();
-			for(int i=0; i<items.size(); ++i)
-			{
-				QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
-				if(line)
-				{
-					QPen pen = line->pen();
-					pen.setWidthF(_linkWidth);
-					line->setPen(pen);
-				}
-			}
 		}
 	}
 	else if(r == aShowHideGridMap)
 	{
-		_gridMap->setVisible(!_gridMap->isVisible());
+		this->setGridMapVisible(!this->isGridMapVisible());
+	}
+	else if(r == aRestoreDefaults)
+	{
+		this->restoreDefaults();
+	}
+	if(r)
+	{
+		emit configChanged();
 	}
 }
 

guilib/src/GraphViewer.h

@@ -44,6 +44,9 @@ class NodeItem;
 class LinkItem;
 
 class GraphViewer : public QGraphicsView {
+
+	Q_OBJECT;
+
 public:
 	GraphViewer(QWidget * parent = 0);
 	virtual ~GraphViewer();
@@ -52,12 +55,43 @@ public:
 					 const std::multimap<int, Link> & constraints);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void updatePosterior(const std::map<int, float> & posterior);
+	void updateLocalPath(const std::vector<int> & localPath);
 	void clearGraph();
 	void clearMap();
 	void clearPosterior();
 	void clearAll();
 
-	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
+	//getters
+	const QString & getWorkingDirectory() const {return _workingDirectory;}
+	float getNodeRadius() const {return _nodeRadius;}
+	float getLinkWidth() const {return _linkWidth;}
+	const QColor & getNodeColor() const {return _nodeColor;}
+	const QColor & getNeighborColor() const {return _neighborColor;}
+	const QColor & getGlobalLoopClosureColor() const {return _loopClosureColor;}
+	const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
+	const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
+	const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
+	const QColor & getLocalPathColor() const {return _pathColor;}
+	bool isGridMapVisible() const;
+
+	// setters
+	void setWorkingDirectory(const QString & path);
+	void setNodeRadius(float radius);
+	void setLinkWidth(float width);
+	void setNodeColor(const QColor & color);
+	void setNeighborColor(const QColor & color);
+	void setGlobalLoopClosureColor(const QColor & color);
+	void setLocalLoopClosureColor(const QColor & color);
+	void setUserLoopClosureColor(const QColor & color);
+	void setVirtualLoopClosureColor(const QColor & color);
+	void setLocalPathColor(const QColor & color);
+	void setGridMapVisible(bool visible);
+
+signals:
+	void configChanged();
+
+public slots:
+	void restoreDefaults();
 
 protected:
 	virtual void wheelEvent ( QWheelEvent * event );
@@ -68,10 +102,13 @@ private:
 	QColor _nodeColor;
 	QColor _neighborColor;
 	QColor _loopClosureColor;
+	QColor _loopClosureLocalColor;
+	QColor _loopClosureUserColor;
+	QColor _loopClosureVirtualColor;
+	QColor _pathColor;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
-	QMultiMap<int, LinkItem*> _neighborLinkItems;
+	QMultiMap<int, LinkItem*> _linkItems;
-	QMultiMap<int, LinkItem*> _loopLinkItems;
 	float _nodeRadius;
 	float _linkWidth;
 	QGraphicsPixmapItem * _gridMap;

guilib/src/ImageView.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QDir>
 #include <QtGui/QAction>
 #include <QtGui/QGraphicsEffect>
+#include <QtGui/QInputDialog>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
 
@@ -44,6 +45,7 @@ ImageView::ImageView(QWidget * parent) :
 		_zoom(250),
 		_minZoom(250),
 		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
+		_alpha(100),
 		_image(0),
 		_imageDepth(0)
 {
@@ -64,6 +66,7 @@ ImageView::ImageView(QWidget * parent) :
 	_showLines = _menu->addAction(tr("Show lines"));
 	_showLines->setCheckable(true);
 	_showLines->setChecked(true);
+	_setAlpha = _menu->addAction(tr("Set alpha..."));
 	_saveImage = _menu->addAction(tr("Save picture..."));
 }
 
@@ -77,17 +80,17 @@ void ImageView::resetZoom()
 	this->setDragMode(QGraphicsView::NoDrag);
 }
 
-bool ImageView::isImageShown()
+bool ImageView::isImageShown() const
 {
 	return _showImage->isChecked();
 }
 
-bool ImageView::isImageDepthShown()
+bool ImageView::isImageDepthShown() const
 {
 	return _showImageDepth->isChecked();
 }
 
-bool ImageView::isFeaturesShown()
+bool ImageView::isFeaturesShown() const
 {
 	return _showFeatures->isChecked();
 }
@@ -121,7 +124,7 @@ void ImageView::setImageDepthShown(bool shown)
 	}
 }
 
-bool ImageView::isLinesShown()
+bool ImageView::isLinesShown() const
 {
 	return _showLines->isChecked();
 }
@@ -162,23 +165,38 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	else if(action == _showFeatures)
 	{
 		this->setFeaturesShown(_showFeatures->isChecked());
+		emit configChanged();
 	}
 	else if(action == _showImage)
 	{
 		this->setImageShown(_showImage->isChecked());
+		emit configChanged();
 	}
 	else if(action == _showImageDepth)
 	{
 		this->setImageDepthShown(_showImageDepth->isChecked());
+		emit configChanged();
 	}
 	else if(action == _showLines)
 	{
 		this->setLinesShown(_showLines->isChecked());
+		emit configChanged();
+	}
+	else if(action == _setAlpha)
+	{
+		bool ok = false;
+		int value = QInputDialog::getInt(this, tr("Set features and lines alpha"), tr("alpha (0-255)"), _alpha, 0, 255, 10, &ok);
+		if(ok)
+		{
+			this->setAlpha(value);
+			emit configChanged();
+		}
 	}
 
 	if(action == _showImage || action ==_showImageDepth)
 	{
 		this->updateOpacity();
+		emit configChanged();
 	}
 }
 
@@ -359,6 +377,19 @@ void ImageView::setFeaturesColor(const QColor & color)
 	}
 }
 
+void ImageView::setAlpha(int alpha)
+{
+	UASSERT(alpha >=0 && alpha <= 255);
+	_alpha = alpha;
+	for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
+	{
+		QColor c = iter.value()->pen().color();
+		c.setAlpha(_alpha);
+		iter.value()->setPen(QPen(c));
+		iter.value()->setBrush(QBrush(c));
+	}
+}
+
 void ImageView::clearLines()
 {
 	if(this->scene())

guilib/src/LoopClosureViewer.cpp

@@ -197,7 +197,7 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			UERROR("loop transform is null !?!?");
 			ui_->cloudViewerTransform->removeAllClouds();
 		}
-		ui_->cloudViewerTransform->render();
+		ui_->cloudViewerTransform->update();
 	}
 }
 

guilib/src/MainWindow.cpp

@@ -82,6 +82,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Graph.h"
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
@@ -119,6 +120,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_odometryReceived(false),
 	_openedDatabasePath(""),
 	_emptyNewDatabase(true),
+	_odomImageShow(true),
+	_odomImageDepthShow(false),
 	_odometryCorrection(Transform::getIdentity()),
 	_lastOdometryProcessed(true),
 	_oneSecondTimer(0),
@@ -128,7 +131,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_rawLikelihoodCurve(0),
 	_autoScreenCaptureOdomSync(false)
 {
-	ULOGGER_DEBUG("");
+	UDEBUG("");
 
 	initGuiResource();
 
@@ -140,15 +143,19 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 
 	// Create dialogs
 	_aboutDialog = new AboutDialog(this);
+	_aboutDialog->setObjectName("AboutDialog");
 	_exportDialog = new ExportCloudsDialog(this);
+	_exportDialog->setObjectName("ExportCloudsDialog");
 	_postProcessingDialog = new PostProcessingDialog(this);
+	_postProcessingDialog->setObjectName("PostProcessingDialog");
 
 	_ui = new Ui_mainWindow();
 	_ui->setupUi(this);
 
-	QString title("RTAB-Map: Real-Time Appearance-Based Mapping");
+	QString title("RTAB-Map[*]");
 	this->setWindowTitle(title);
 	this->setWindowIconText(tr("RTAB-Map"));
+	this->setObjectName("MainWindow");
 
 	//Setup dock widgets position if it is the first time the application is started.
 	//if(!QFile::exists(PreferencesDialog::getIniFilePath()))
@@ -177,10 +184,15 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	{
 		_preferencesDialog = new PreferencesDialog(this);
 	}
+	_preferencesDialog->setObjectName("PreferencesDialog");
 	_preferencesDialog->init();
 
 	// Restore window geometry
-	_preferencesDialog->loadMainWindowState(this);
+	_preferencesDialog->loadMainWindowState(this, _savedMaximized);
+	_preferencesDialog->loadWindowGeometry(_preferencesDialog);
+	_preferencesDialog->loadWindowGeometry(_exportDialog);
+	_preferencesDialog->loadWindowGeometry(_postProcessingDialog);
+	_preferencesDialog->loadWindowGeometry(_aboutDialog);
 	setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
 
 	// Timer
@@ -196,6 +208,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
+	_ui->imageView_odometry->setAlpha(200);
+	_preferencesDialog->loadWidgetState(_ui->imageView_source);
+	_preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
+	_preferencesDialog->loadWidgetState(_ui->imageView_odometry);
+	_preferencesDialog->loadWidgetState(_ui->graphicsView_graphView);
 
 	_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
 	_ui->posteriorPlot->addCurve(_posteriorCurve, false);
@@ -250,6 +267,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show()));
 
 	// connect actions with custom slots
+	connect(_ui->actionSave_GUI_config, SIGNAL(triggered()), this, SLOT(saveConfigGUI()));
 	connect(_ui->actionNew_database, SIGNAL(triggered()), this, SLOT(newDatabase()));
 	connect(_ui->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
 	connect(_ui->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
@@ -290,6 +308,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
 
 	_ui->actionPause->setShortcut(Qt::Key_Space);
+	_ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
 	_ui->actionSave_point_cloud->setEnabled(false);
 	_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
 	_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
@@ -305,7 +324,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 #endif
 
 	//Settings menu
-	this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
+	this->updateSelectSourceImageMenu(_preferencesDialog->isSourceImageUsed(), _preferencesDialog->getSourceImageType());
 	connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages()));
 	connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo()));
 	connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
@@ -344,6 +363,22 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_preferencesDialog, SIGNAL(settingsChanged(PreferencesDialog::PANEL_FLAGS)), this, SLOT(applyPrefSettings(PreferencesDialog::PANEL_FLAGS)));
 	qRegisterMetaType<rtabmap::ParametersMap>("rtabmap::ParametersMap");
 	connect(_preferencesDialog, SIGNAL(settingsChanged(rtabmap::ParametersMap)), this, SLOT(applyPrefSettings(rtabmap::ParametersMap)));
+	// config GUI modified
+	connect(_ui->imageView_source, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_ui->imageView_loopClosure, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_ui->imageView_odometry, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_ui->graphicsView_graphView, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_ui->widget_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_exportDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_postProcessingDialog, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
+	connect(_ui->toolBar->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
+	connect(_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)), this, SLOT(configGUIModified()));
+	QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
+	for(int i=0; i<dockWidgets.size(); ++i)
+	{
+		connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configGUIModified()));
+		connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configGUIModified()));
+	}
 
 	// more connects...
 	connect(_ui->doubleSpinBox_stats_imgRate, SIGNAL(editingFinished()), this, SLOT(changeImgRateSetting()));
@@ -370,6 +405,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 	_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
 	_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
+	_preferencesDialog->loadWidgetState(_ui->widget_cloudViewer);
+
+	//dialog states
+	_preferencesDialog->loadWidgetState(_exportDialog);
+	_preferencesDialog->loadWidgetState(_postProcessingDialog);
 
 	if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
 	{
@@ -383,7 +423,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	// update loop closure viewer parameters
 	ParametersMap parameters = _preferencesDialog->getAllParameters();
 	_ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
-	_ui->widget_loopClosureViewer->setMaxDepth(atof(parameters.at(Parameters::kLccIcp3MaxDepth()).c_str()));
+	_ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
 	_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
 
 	//update ui
@@ -394,6 +434,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	splash.close();
 
 	this->setFocus();
+
+	UDEBUG("");
 }
 
 MainWindow::~MainWindow()
@@ -444,10 +486,31 @@ void MainWindow::closeEvent(QCloseEvent* event)
 
 	if(processStopped)
 	{
-		_ui->statsToolBox->closeFigures();
-
 		//write settings before quit?
-		_preferencesDialog->saveMainWindowState(this);
+		bool save = false;
+		if(this->isWindowModified())
+		{
+			QMessageBox::Button b=QMessageBox::question(this,
+					tr("RTAB-Map"),
+					tr("There are unsaved GUI changes. Save them?"),
+					QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
+			if(b == QMessageBox::Save)
+			{
+				save = true;
+			}
+			else if(b != QMessageBox::Discard)
+			{
+				event->ignore();
+				return;
+			}
+		}
+
+		if(save)
+		{
+			saveConfigGUI();
+		}
+
+		_ui->statsToolBox->closeFigures();
 
 		_ui->dockWidget_imageView->close();
 		_ui->dockWidget_likelihood->close();
@@ -577,10 +640,12 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 	UTimer time;
 	Transform pose = data.pose();
 	bool lost = false;
+	bool lostStateChanged = false;
 	_ui->imageView_odometry->resetTransform();
 	if(pose.isNull())
 	{
 		UDEBUG("odom lost"); // use last pose
+		lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() != Qt::darkRed;
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed);
 		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkRed));
 
@@ -592,12 +657,14 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			info.inliers < _preferencesDialog->getOdomQualityWarnThr())
 	{
 		UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
+		lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
 		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow));
 	}
 	else
 	{
 		UDEBUG("odom ok");
+		lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
 		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
 	}
@@ -646,19 +713,19 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			_odometryReceived = true;
 
 			// 3d cloud
-			if(data.depth().cols == data.image().cols &&
+			if(data.depthOrRightImage().cols == data.image().cols &&
-			   data.depth().rows == data.image().rows &&
+			   data.depthOrRightImage().rows == data.image().rows &&
-			   !data.depth().empty() &&
+			   !data.depthOrRightImage().empty() &&
 			   data.fx() > 0.0f &&
-			   data.fy() > 0.0f &&
+			   data.fyOrBaseline() > 0.0f &&
 			   _preferencesDialog->isCloudsShown(1))
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 				cloud = createCloud(0,
 						data.image(),
-						data.depth(),
+						data.depthOrRightImage(),
 						data.fx(),
-						data.fy(),
+						data.fyOrBaseline(),
 						data.cx(),
 						data.cy(),
 						data.localTransform(),
@@ -695,63 +762,95 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			if(!data.pose().isNull())
 			{
 				// update camera position
-				_ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose());
+				_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
 			}
 		}
-		_ui->widget_cloudViewer->render();
+		_ui->widget_cloudViewer->update();
 	}
 
 	if(_ui->dockWidget_odometry->isVisible() &&
 	   !data.image().empty())
 	{
-		int alpha = _preferencesDialog->getKeypointsOpacity()*255/100;
+		if(_ui->imageView_odometry->isFeaturesShown())
-		if(info.type == 0)
 		{
-			_ui->imageView_odometry->setFeatures(info.words, QColor(255,255,0, alpha));
+			int alpha = _ui->imageView_odometry->getAlpha();
-		}
+			if(info.type == 0)
-		else if(info.type == 1)
+			{
-		{
+				_ui->imageView_odometry->setFeatures(info.words, QColor(255,255,0, alpha));
-			_ui->imageView_odometry->setFeatures(info.refCorners, QColor(255,0,0, alpha));
+			}
+			else if(info.type == 1)
+			{
+				_ui->imageView_odometry->setFeatures(info.refCorners, QColor(255,0,0, alpha));
+			}
 		}
 		_ui->imageView_odometry->clearLines();
 		if(lost)
 		{
+			if(lostStateChanged)
+			{
+				// save state
+				_odomImageShow = _ui->imageView_odometry->isImageShown();
+				_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
+			}
 			_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
 			_ui->imageView_odometry->setImageShown(true);
 			_ui->imageView_odometry->setImageDepthShown(true);
 		}
 		else
 		{
+			if(lostStateChanged)
+			{
+				// restore state
+				_ui->imageView_odometry->setImageShown(_odomImageShow);
+				_ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
+			}
+
 			_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
-			_ui->imageView_odometry->setImageShown(true);
+			if(_ui->imageView_odometry->isImageDepthShown())
-			_ui->imageView_odometry->setImageDepthShown(false);
+			{
+				_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+			}
 
 			if(info.type == 0)
 			{
-				for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+				if(_ui->imageView_odometry->isFeaturesShown())
 				{
-					_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], QColor(255,0,0, alpha)); // outliers
+					int alpha = _ui->imageView_odometry->getAlpha();
-				}
+					for(unsigned int i=0; i<info.wordMatches.size(); ++i)
-				for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+					{
-				{
+						_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], QColor(255,0,0, alpha)); // outliers
-					_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], QColor(0,255,0, alpha)); // inliers
+					}
+					for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+					{
+						_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], QColor(0,255,0, alpha)); // inliers
+					}
 				}
 			}
 			else if(info.type == 1)
 			{
-				//draw lines
+				if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
-				UASSERT(info.refCorners.size() == info.newCorners.size());
-				for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
 				{
-					_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], QColor(0,255,0, alpha)); // inliers
+					//draw lines
-					QGraphicsLineItem * item = _ui->imageView_odometry->scene()->addLine(
+					int alpha = _ui->imageView_odometry->getAlpha();
-							info.refCorners[info.cornerInliers[i]].pt.x,
+					UASSERT(info.refCorners.size() == info.newCorners.size());
-							info.refCorners[info.cornerInliers[i]].pt.y,
+					for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
-							info.newCorners[info.cornerInliers[i]].pt.x,
+					{
-							info.newCorners[info.cornerInliers[i]].pt.y,
+						if(_ui->imageView_odometry->isFeaturesShown())
-							QPen(QColor(0, 0, 255, alpha)));
+						{
-					item->setVisible(_ui->imageView_odometry->isLinesShown());
+							_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], QColor(0,255,0, alpha)); // inliers
-					item->setZValue(1);
+						}
+						if(_ui->imageView_odometry->isLinesShown())
+						{
+							QGraphicsLineItem * item = _ui->imageView_odometry->scene()->addLine(
+									info.refCorners[info.cornerInliers[i]].pt.x,
+									info.refCorners[info.cornerInliers[i]].pt.y,
+									info.newCorners[info.cornerInliers[i]].pt.x,
+									info.newCorners[info.cornerInliers[i]].pt.y,
+									QPen(QColor(0, 0, 255, alpha)));
+							item->setVisible(_ui->imageView_odometry->isLinesShown());
+							item->setZValue(1);
+						}
+					}
 				}
 			}
 		}
@@ -759,10 +858,6 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 		_ui->imageView_odometry->resetZoom();
 		_ui->imageView_odometry->setSceneRect(_ui->imageView_odometry->scene()->itemsBoundingRect());
 		_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
-		if(_preferencesDialog->isImageFlipped())
-		{
-			_ui->imageView_odometry->scale(-1.0, 1.0);
-		}
 	}
 
 	_lastOdometryProcessed = true;
@@ -951,12 +1046,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		// do it after scaling
 		this->drawKeypoints(signature.getWords(), loopSignature.getWords());
 
-		if(_preferencesDialog->isImageFlipped())
-		{
-			_ui->imageView_source->scale(-1.0, 1.0);
-			_ui->imageView_loopClosure->scale(-1.0, 1.0);
-		}
-
 		UDEBUG("time= %d ms", time.restart());
 
 		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), signature.getWords().size());
@@ -1044,6 +1133,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			_ui->graphicsView_graphView->updatePosterior(stat.posterior());
 		}
+		// update local path on the graph view
+		_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
 
 		UDEBUG("");
 	}
@@ -1060,6 +1151,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		this->captureScreen();
 	}
 	_processingStatistics = false;
+
+	if(!_preferencesDialog->isImagesKept())
+	{
+		_cachedSignatures.clear();
+	}
 }
 
 void MainWindow::updateMapCloud(
@@ -1101,7 +1197,7 @@ void MainWindow::updateMapCloud(
 			}
 		}
 
-		_ui->actionPost_processing->setEnabled(_currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
+		_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 	}
 
 	// filter duplicated poses
@@ -1111,7 +1207,7 @@ void MainWindow::updateMapCloud(
 	{
 		float radius = _preferencesDialog->getCloudFilteringRadius();
 		float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
-		poses = util3d::radiusPosesFiltering(posesIn, radius, angle);
+		poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
 		// make sure the last is here
 		poses.insert(*posesIn.rbegin());
 		for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
@@ -1152,7 +1248,8 @@ void MainWindow::updateMapCloud(
 			std::string cloudName = uFormat("cloud%d", iter->first);
 
 			// 3d point cloud
-			if(_preferencesDialog->isCloudsShown(0))
+			if((_ui->widget_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
+				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud()))
 			{
 				if(viewerClouds.contains(cloudName))
 				{
@@ -1187,7 +1284,8 @@ void MainWindow::updateMapCloud(
 
 			// 2d point cloud
 			std::string scanName = uFormat("scan%d", iter->first);
-			if(_preferencesDialog->isScansShown(0) || _ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())
+			if((_ui->widget_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
+				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
 			{
 				if(viewerClouds.contains(scanName))
 				{
@@ -1277,7 +1375,7 @@ void MainWindow::updateMapCloud(
 			QColor color = Qt::gray;
 			if(iter->first >= 0)
 			{
-				color = (Qt::GlobalColor)(iter->first % 12 + 7 );
+				color = (Qt::GlobalColor)((iter->first+2) % 12 + 7 );
 			}
 			_ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
@@ -1320,6 +1418,8 @@ void MainWindow::updateMapCloud(
 			}
 		}
 	}
+	_ui->graphicsView_graphView->update();
+
 	if(!_preferencesDialog->getGridMapShown())
 	{
 		_ui->widget_cloudViewer->removeOccupancyGridMap();
@@ -1354,10 +1454,10 @@ void MainWindow::updateMapCloud(
 
 	if(!currentPose.isNull())
 	{
-		_ui->widget_cloudViewer->updateCameraPosition(currentPose);
+		_ui->widget_cloudViewer->updateCameraTargetPosition(currentPose);
 	}
 
-	_ui->widget_cloudViewer->render();
+	_ui->widget_cloudViewer->update();
 }
 
 void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId)
@@ -1518,10 +1618,10 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 	if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
 	{
 		QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
-		if(_preferencesDialog->isCloudsShown(0) && _cachedSignatures.contains(nodeId))
+		if(_preferencesDialog->isCloudsShown(0))
 		{
 			std::string cloudName = uFormat("cloud%d", nodeId);
-			if(visible && !viewerClouds.contains(cloudName))
+			if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId))
 			{
 				createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
 			}
@@ -1536,10 +1636,10 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 			}
 		}
 
-		if(_preferencesDialog->isScansShown(0) && _cachedSignatures.contains(nodeId))
+		if(_preferencesDialog->isScansShown(0))
 		{
 			std::string scanName = uFormat("scan%d", nodeId);
-			if(visible && !viewerClouds.contains(scanName))
+			if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId))
 			{
 				createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1));
 			}
@@ -1554,7 +1654,7 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 			}
 		}
 	}
-	_ui->widget_cloudViewer->render();
+	_ui->widget_cloudViewer->update();
 }
 
 void MainWindow::processRtabmapEventInit(int status, const QString & info)
@@ -1590,7 +1690,7 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
 			// Temp database used, automatically backup with unique name (timestamp)
 			QString newName = QDateTime::currentDateTime().toString("yyMMdd-hhmmss");
 			bool ok = false;
-			newName = QInputDialog::getText(this, tr("Saving database..."), tr("Database name:"), QLineEdit::Normal, newName, &ok);
+			newName = QInputDialog::getText(this, tr("Save database?"), tr("Database name:"), QLineEdit::Normal, newName, &ok);
 			while(ok)
 			{
 				QString newPath = _preferencesDialog->getWorkingDirectory()+QDir::separator()+newName+".db";
@@ -1714,6 +1814,11 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		}
 
 		_initProgressDialog->appendText(tr("%1 locations are updated to/inserted in the cache.").arg(event.getPoses().size()));
+
+		if(!_preferencesDialog->isImagesKept())
+		{
+			_cachedSignatures.clear();
+		}
 	}
 	_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
 }
@@ -1725,7 +1830,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 	{
 		// Camera settings...
 		_ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
-		this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
+		this->updateSelectSourceImageMenu(_preferencesDialog->isSourceImageUsed(), _preferencesDialog->getSourceImageType());
 		this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
 		this->updateSelectSourceRGBDMenu(_preferencesDialog->isSourceOpenniUsed(), _preferencesDialog->getSourceRGBD());
 		QString src;
@@ -1837,7 +1942,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
 		}
 		if(uContains(parameters, Parameters::kLccIcp3MaxDepth()))
 		{
-			_ui->widget_loopClosureViewer->setMaxDepth(atof(parameters.at(Parameters::kLccIcp3MaxDepth()).c_str()));
+			_ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
 		}
 		if(uContains(parameters, Parameters::kLccIcp3Samples()))
 		{
@@ -1861,7 +1966,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 
 	timer.start();
 	KeypointItem * item = 0;
-	int alpha = _preferencesDialog->getKeypointsOpacity()*255/100;
+	int alphaA = _ui->imageView_source->getAlpha();
 	ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
 	QMap<int, KeypointItem*> addedKeypoints;
 	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
@@ -1880,27 +1985,27 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 		if(uContains(loopWords, id))
 		{
 			// PINK = FOUND IN LOOP SIGNATURE
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 255, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 255, alphaA));
 		}
 		else if(_lastIds.contains(id))
 		{
 			// BLUE = FOUND IN LAST SIGNATURE
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 0, 255, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 0, 255, alphaA));
 		}
 		else if(id<=_lastId)
 		{
 			// RED = ALREADY EXISTS
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 0, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 0, alphaA));
 		}
 		else if(refWords.count(id) > 1)
 		{
 			// YELLOW = NEW and multiple times
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alphaA));
 		}
 		else
 		{
 			// GREEN = NEW
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 255, 0, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 255, 0, alphaA));
 		}
 		item->setVisible(this->_ui->imageView_source->isFeaturesShown());
 		this->_ui->imageView_source->scene()->addItem(item);
@@ -1911,6 +2016,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 
 	timer.start();
 	item = 0;
+	int alphaB = _ui->imageView_loopClosure->getAlpha();
 	ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
 	QList<QPair<KeypointItem*, KeypointItem*> > uniqueCorrespondences;
 	for(std::multimap<int, cv::KeyPoint>::const_iterator i = loopWords.begin(); i != loopWords.end(); ++i )
@@ -1929,7 +2035,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 		if(uContains(refWords, id))
 		{
 			// PINK = FOUND IN LOOP SIGNATURE
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 255, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 255, alphaB));
 			//To draw lines... get only unique correspondences
 			if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
 			{
@@ -1939,17 +2045,17 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 		else if(id<=_lastId)
 		{
 			// RED = ALREADY EXISTS
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 0, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 0, alphaB));
 		}
 		else if(refWords.count(id) > 1)
 		{
 			// YELLOW = NEW and multiple times
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alphaB));
 		}
 		else
 		{
 			// GREEN = NEW
-			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 255, 0, alpha));
+			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 255, 0, alphaB));
 		}
 		item->setVisible(this->_ui->imageView_loopClosure->isFeaturesShown());
 		this->_ui->imageView_loopClosure->scene()->addItem(item);
@@ -1987,7 +2093,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 				iter->first->rect().y()+iter->first->rect().height()/2,
 				iter->second->rect().x()+iter->second->rect().width()/2+deltaX,
 				iter->second->rect().y()+iter->second->rect().height()/2+deltaY,
-				QPen(QColor(0, 255, 255, alpha)));
+				QPen(QColor(0, 255, 255, alphaA)));
 		item->setVisible(_ui->imageView_source->isLinesShown());
 		item->setZValue(1);
 
@@ -1996,12 +2102,31 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 				iter->first->rect().y()+iter->first->rect().height()/2-deltaY,
 				iter->second->rect().x()+iter->second->rect().width()/2,
 				iter->second->rect().y()+iter->second->rect().height()/2,
-				QPen(QColor(0, 255, 255, alpha)));
+				QPen(QColor(0, 255, 255, alphaB)));
 		item->setVisible(_ui->imageView_loopClosure->isLinesShown());
 		item->setZValue(1);
 	}
 }
 
+void MainWindow::showEvent(QShowEvent* anEvent)
+{
+	this->setWindowModified(false);
+}
+
+void MainWindow::moveEvent(QMoveEvent* anEvent)
+{
+	if(this->isVisible())
+	{
+		// HACK, there is a move event when the window is shown the first time.
+		static bool firstCall = true;
+		if(!firstCall)
+		{
+			this->configGUIModified();
+		}
+		firstCall = false;
+	}
+}
+
 void MainWindow::resizeEvent(QResizeEvent* anEvent)
 {
 	_ui->imageView_source->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
@@ -2010,34 +2135,17 @@ void MainWindow::resizeEvent(QResizeEvent* anEvent)
 	_ui->imageView_source->resetZoom();
 	_ui->imageView_loopClosure->resetZoom();
 	_ui->imageView_odometry->resetZoom();
+	if(this->isVisible())
+	{
+		this->configGUIModified();
+	}
 }
 
-void MainWindow::updateSelectSourceImageMenu(int type)
+void MainWindow::updateSelectSourceImageMenu(bool used, PreferencesDialog::Src src)
 {
-	if(_preferencesDialog->isSourceImageUsed())
+	_ui->actionUsbCamera->setChecked(used && src == PreferencesDialog::kSrcUsbDevice);
-	{
+	_ui->actionImageFiles->setChecked(used && src == PreferencesDialog::kSrcImages);
-		switch(type)
+	_ui->actionVideo->setChecked(used && src == PreferencesDialog::kSrcVideo);
-		{
-		case 0:
-			_ui->actionUsbCamera->setChecked(true);
-			break;
-		case 1:
-			_ui->actionImageFiles->setChecked(true);
-			break;
-		case 2:
-			_ui->actionVideo->setChecked(true);
-			break;
-		default:
-			UERROR("Unknown source image type");
-			break;
-		}
-	}
-	else
-	{
-		// they are exclusive actions, so check/uncheck one should disable all.
-		_ui->actionUsbCamera->setChecked(true);
-		_ui->actionUsbCamera->setChecked(false);
-	}
 }
 
 void MainWindow::updateSelectSourceDatabase(bool used)
@@ -2104,7 +2212,28 @@ void MainWindow::beep()
 	QApplication::beep();
 }
 
+void MainWindow::configGUIModified()
+{
+	this->setWindowModified(true);
+}
+
 //ACTIONS
+void MainWindow::saveConfigGUI()
+{
+	_savedMaximized = this->isMaximized();
+	_preferencesDialog->saveMainWindowState(this);
+	_preferencesDialog->saveWindowGeometry(_preferencesDialog);
+	_preferencesDialog->saveWindowGeometry(_aboutDialog);
+	_preferencesDialog->saveWidgetState(_ui->widget_cloudViewer);
+	_preferencesDialog->saveWidgetState(_ui->imageView_source);
+	_preferencesDialog->saveWidgetState(_ui->imageView_loopClosure);
+	_preferencesDialog->saveWidgetState(_ui->imageView_odometry);
+	_preferencesDialog->saveWidgetState(_exportDialog);
+	_preferencesDialog->saveWidgetState(_postProcessingDialog);
+	_preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
+	this->setWindowModified(false);
+}
+
 void MainWindow::newDatabase()
 {
 	if(_state != MainWindow::kIdle)
@@ -2191,8 +2320,8 @@ void MainWindow::startDetection()
 	   _preferencesDialog->isSourceDatabaseUsed())
 	{
 		float inputRate = _preferencesDialog->getGeneralInputRate();
-		float detectionRate = std::atof(parameters.at(Parameters::kRtabmapDetectionRate()).c_str());
+		float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
-		int bufferingSize = std::atof(parameters.at(Parameters::kRtabmapImageBufferSize()).c_str());
+		int bufferingSize = uStr2Float(parameters.at(Parameters::kRtabmapImageBufferSize()));
 		if((detectionRate!=0.0f && detectionRate < inputRate) || (detectionRate > 0.0f && inputRate == 0.0f))
 		{
 			int button = QMessageBox::question(this,
@@ -2732,6 +2861,11 @@ void MainWindow::generateTOROMap()
 
 void MainWindow::postProcessing()
 {
+	if(_cachedSignatures.size() == 0)
+	{
+		UERROR("Signatures must be cached in the GUI to post processing.");
+		return;
+	}
 	if(_postProcessingDialog->exec() != QDialog::Accepted)
 	{
 		return;
@@ -2845,7 +2979,7 @@ void MainWindow::postProcessing()
 			ParametersMap customParameters;
 			// override some parameters
 			customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-			customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+			customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
 			customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
 			customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
 			customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kLccReextractNNType())));
@@ -2862,7 +2996,7 @@ void MainWindow::postProcessing()
 			_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
 					.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
 
-			std::multimap<int, int> clusters = util3d::radiusPosesClustering(
+			std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
 					_currentPosesMap,
 					clusterRadius,
 					clusterAngle*CV_PI/180.0);
@@ -2884,7 +3018,7 @@ void MainWindow::postProcessing()
 
 				// only add new links and one per cluster per iteration
 				if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
-				   util3d::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
+				   rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
 				{
 					if(!_cachedSignatures.contains(from))
 					{
@@ -2967,8 +3101,8 @@ void MainWindow::postProcessing()
 				_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
 						.arg(odomPoses.size()).arg(_currentLinksMap.size()));
 				std::map<int, rtabmap::Transform> optimizedPoses;
-				std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+				std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
-				util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
+				rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 				_currentPosesMap = optimizedPoses;
 				_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
 			}
@@ -3109,7 +3243,9 @@ void MainWindow::postProcessing()
 					}
 					else
 					{
-						UWARN("Cannot refine link %d->%d (converged=%s variance=%f)", from, to, hasConverged?"true":"false", variance);
+						QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
+						_initProgressDialog->appendText(str, Qt::darkYellow);
+						UWARN("%s", str.toStdString().c_str());
 					}
 				}
 			}
@@ -3120,8 +3256,8 @@ void MainWindow::postProcessing()
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
 			.arg(odomPoses.size()).arg(_currentLinksMap.size()));
 	std::map<int, rtabmap::Transform> optimizedPoses;
-	std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+	std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
-	util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
+	rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
 	_initProgressDialog->incrementStep();
 
@@ -3205,47 +3341,47 @@ void MainWindow::updateEditMenu()
 
 void MainWindow::selectImages()
 {
-	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcImages);
+	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcImages, _ui->actionImageFiles->isChecked());
 }
 
 void MainWindow::selectVideo()
 {
-	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcVideo);
+	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcVideo, _ui->actionVideo->isChecked());
 }
 
 void MainWindow::selectStream()
 {
-	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcUsbDevice);
+	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcUsbDevice, _ui->actionUsbCamera->isChecked());
 }
 
 void MainWindow::selectDatabase()
 {
-	_preferencesDialog->selectSourceDatabase(true);
+	_preferencesDialog->selectSourceDatabase(true, _ui->actionDatabase->isChecked());
 }
 
 void MainWindow::selectOpenni()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL, _ui->actionOpenNI_PCL->isChecked());
 }
 
 void MainWindow::selectFreenect()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect, _ui->actionFreenect->isChecked());
 }
 
 void MainWindow::selectOpenniCv()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV, _ui->actionOpenNI_CV->isChecked());
 }
 
 void MainWindow::selectOpenniCvAsus()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS, _ui->actionOpenNI_CV_ASUS->isChecked());
 }
 
 void MainWindow::selectOpenni2()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2, _ui->actionOpenNI2->isChecked());
 }
 
 
@@ -3507,6 +3643,10 @@ void MainWindow::selectScreenCaptureFormat(bool checked)
 				_autoScreenCaptureOdomSync = true;
 			}
 		}
+		else
+		{
+			_ui->actionAuto_screen_capture->setChecked(false);
+		}
 	}
 }
 
@@ -3918,6 +4058,7 @@ bool MainWindow::getExportedClouds(
 	{
 		_exportDialog->setOkButton();
 	}
+	_exportDialog->enableRegeneration(_preferencesDialog->isImagesKept());
 	if(_exportDialog->exec() == QDialog::Accepted)
 	{
 		std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
@@ -4470,14 +4611,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 		bool inserted = false;
 		if(!iter->second.isNull())
 		{
-			if(_cachedSignatures.contains(iter->first))
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			if(regenerateClouds)
 			{
-				const Signature & s = _cachedSignatures.find(iter->first).value();
+				if(_cachedSignatures.contains(iter->first))
-				cv::Mat image, depth;
-				s.uncompressDataConst(&image, &depth, 0);
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-				if(regenerateClouds)
 				{
+					const Signature & s = _cachedSignatures.find(iter->first).value();
+					cv::Mat image, depth;
+					s.uncompressDataConst(&image, &depth, 0);
+
 					cloud = createCloud(iter->first,
 							image,
 							depth,
@@ -4491,22 +4633,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 							regenerateDecimation,
 							regenerateMaxDepth);
 				}
-				else if(uContains(_createdClouds, iter->first))
+				else
 				{
-					cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(_createdClouds.at(iter->first), iter->second);
+					UWARN("Cloud %d not found in cache!", iter->first);
-				}
-
-				if(cloud->size())
-				{
-					*assembledCloud += *cloud;
-
-					inserted = true;
-					++count;
 				}
 			}
-			else
+			else if(uContains(_createdClouds, iter->first))
 			{
-				UERROR("Cloud %d not found?!?", iter->first);
+				cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(_createdClouds.at(iter->first), iter->second);
+			}
+
+			if(cloud->size())
+			{
+				*assembledCloud += *cloud;
+
+				inserted = true;
+				++count;
 			}
 		}
 		else
@@ -4556,41 +4698,41 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
 		bool inserted = false;
 		if(!iter->second.isNull())
 		{
-			if(_cachedSignatures.contains(iter->first))
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			if(regenerateClouds)
 			{
-				const Signature & s = _cachedSignatures.find(iter->first).value();
+				if(_cachedSignatures.contains(iter->first))
-				cv::Mat image, depth;
-				s.uncompressDataConst(&image, &depth, 0);
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-				if(regenerateClouds)
 				{
-					cloud = createCloud(iter->first,
+					const Signature & s = _cachedSignatures.find(iter->first).value();
-							image,
+					cv::Mat image, depth;
-							depth,
+					s.uncompressDataConst(&image, &depth, 0);
-							s.getDepthFx(),
+					cloud =	 createCloud(iter->first,
-							s.getDepthFy(),
+						image,
-							s.getDepthCx(),
+						depth,
-							s.getDepthCy(),
+						s.getDepthFx(),
-							s.getLocalTransform(),
+						s.getDepthFy(),
-							Transform::getIdentity(),
+						s.getDepthCx(),
-							regenerateVoxelSize,
+						s.getDepthCy(),
-							regenerateDecimation,
+						s.getLocalTransform(),
-							regenerateMaxDepth);
+						Transform::getIdentity(),
+						regenerateVoxelSize,
+						regenerateDecimation,
+						regenerateMaxDepth);
 				}
-				else if(uContains(_createdClouds, iter->first))
+				else
 				{
-					cloud = _createdClouds.at(iter->first);
+					UERROR("Cloud %d not found in cache!", iter->first);
-				}
-
-				if(cloud->size())
-				{
-					clouds.insert(std::make_pair(iter->first, cloud));
-					inserted = true;
 				}
 			}
-			else
+			else if(uContains(_createdClouds, iter->first))
 			{
-				UERROR("Cloud %d not found?!?", iter->first);
+				cloud = _createdClouds.at(iter->first);
+			}
+
+			if(cloud->size())
+			{
+				clouds.insert(std::make_pair(iter->first, cloud));
+				inserted = true;
 			}
 		}
 		else

guilib/src/OdometryViewer.cpp

@@ -147,7 +147,7 @@ void OdometryViewer::processData()
 				this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
 			}
 
-			this->updateCameraPosition(data.pose());
+			this->updateCameraTargetPosition(data.pose());
 
 			if(qualityWarningThr_ && quality>=0 && quality < qualityWarningThr_)
 			{
@@ -164,7 +164,7 @@ void OdometryViewer::processData()
 			this->setBackgroundColor(Qt::darkRed);
 		}
 
-		this->render();
+		this->update();
 	}
 }
 

guilib/src/PostProcessingDialog.cpp

@@ -41,6 +41,15 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
 	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
 	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
+	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
+	connect(_ui->clusterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->clusterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->reextractFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 }
 
 PostProcessingDialog::~PostProcessingDialog()
@@ -48,6 +57,17 @@ PostProcessingDialog::~PostProcessingDialog()
 	delete _ui;
 }
 
+void PostProcessingDialog::restoreDefaults()
+{
+	setDetectMoreLoopClosures(true);
+	setClusterRadius(0.3);
+	setClusterAngle(30);
+	setIterations(1);
+	setReextractFeatures(false);
+	setRefineNeighborLinks(false);
+	setRefineLoopClosureLinks(false);
+}
+
 void PostProcessingDialog::updateButtonBox()
 {
 	_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
@@ -89,4 +109,34 @@ bool PostProcessingDialog::isRefineLoopClosureLinks() const
 	return _ui->refineLoopClosureLinks->isChecked();
 }
 
+//setters
+void PostProcessingDialog::setDetectMoreLoopClosures(bool on)
+{
+	_ui->detectMoreLoopClosures->setChecked(on);
+}
+void PostProcessingDialog::setClusterRadius(double radius)
+{
+	_ui->clusterRadius->setValue(radius);
+}
+void PostProcessingDialog::setClusterAngle(double angle)
+{
+	_ui->clusterAngle->setValue(angle);
+}
+void PostProcessingDialog::setIterations(int iterations)
+{
+	_ui->iterations->setValue(iterations);
+}
+void PostProcessingDialog::setReextractFeatures(bool on)
+{
+	_ui->reextractFeatures->setChecked(on);
+}
+void PostProcessingDialog::setRefineNeighborLinks(bool on)
+{
+	_ui->refineNeighborLinks->setChecked(on);
+}
+void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
+{
+	_ui->refineLoopClosureLinks->setChecked(on);
+}
+
 }

guilib/src/PostProcessingDialog.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QDialog>
 
 class Ui_PostProcessingDialog;
+class QAbstractButton;
 
 namespace rtabmap {
 
@@ -43,6 +44,7 @@ public:
 
 	virtual ~PostProcessingDialog();
 
+	//getters
 	bool isDetectMoreLoopClosures() const;
 	double clusterRadius() const;
 	double clusterAngle() const;
@@ -51,9 +53,25 @@ public:
 	bool isRefineNeighborLinks() const;
 	bool isRefineLoopClosureLinks() const;
 
+	//setters
+	void setDetectMoreLoopClosures(bool on);
+	void setClusterRadius(double radius);
+	void setClusterAngle(double angle);
+	void setIterations(int iterations);
+	void setReextractFeatures(bool on);
+	void setRefineNeighborLinks(bool on);
+	void setRefineLoopClosureLinks(bool on);
+
+signals:
+	void configChanged();
+
+public slots:
+	void restoreDefaults();
+
 private slots:
 	void updateButtonBox();
 
+
 private:
 	Ui_PostProcessingDialog * _ui;
 };

guilib/src/PreferencesDialog.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QDir>
 #include <QtCore/QTimer>
 
+#include <QtGui/QVector3D>
 #include <QtGui/QFileDialog>
 #include <QtGui/QMessageBox>
 #include <QtGui/QStandardItemModel>
@@ -56,6 +57,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/LoopClosureViewer.h"
 #include "rtabmap/gui/DataRecorder.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/ImageView.h"
+#include "GraphViewer.h"
+#include "ExportCloudsDialog.h"
+#include "PostProcessingDialog.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -147,11 +153,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// General panel
 	connect(_ui->general_checkBox_imagesKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_verticalLayoutUsed, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
-	connect(_ui->checkBox_imageFlipped, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_imageRejectedShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_imageHighestHypShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_beep, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
-	connect(_ui->horizontalSlider_keypointsOpacity, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
@@ -327,6 +331,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// Memory
 	_ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str());
+	_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
 	_ui->general_checkBox_keepRehearsedNodes->setObjectName(Parameters::kMemRehearsedNodesKept().c_str());
 	_ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str());
 	_ui->doubleSpinBox_similarityThreshold->setObjectName(Parameters::kMemRehearsalSimilarity().c_str());
@@ -444,6 +449,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str());
 	_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 
+	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
+	_ui->graphPlan_maxAnticipatedNodes->setObjectName(Parameters::kRGBDMaxAnticipatedNodes().c_str());
+
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
 	_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalLoopDetectionRadius().c_str());
@@ -537,7 +545,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 }
 
 PreferencesDialog::~PreferencesDialog() {
-	this->saveWindowGeometry("PreferencesDialog", this);
 	delete _ui;
 }
 
@@ -553,8 +560,6 @@ void PreferencesDialog::init()
 	this->readSettings();
 	this->writeSettings();// This will create the ini file if not exist
 
-	this->loadWindowGeometry("PreferencesDialog", this);
-
 	_initialized = true;
 }
 
@@ -814,10 +819,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->general_checkBox_imagesKept->setChecked(true);
 		_ui->checkBox_beep->setChecked(false);
 		_ui->checkBox_verticalLayoutUsed->setChecked(true);
-		_ui->checkBox_imageFlipped->setChecked(false);
 		_ui->checkBox_imageRejectedShown->setChecked(true);
 		_ui->checkBox_imageHighestHypShown->setChecked(false);
-		_ui->horizontalSlider_keypointsOpacity->setSliderPosition(20);
 		_ui->spinBox_odomQualityWarnThr->setValue(50);
 		_ui->checkBox_posteriorGraphView->setChecked(true);
 	}
@@ -853,7 +856,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
 		_ui->checkBox_map_fillEmptySpace->setChecked(true);
 		_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
-		_ui->spinbox_map_fillEmptyRadius->setValue(0);
+		_ui->spinbox_map_fillEmptyRadius->setValue(1);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1058,11 +1061,9 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->comboBox_loggerType->setCurrentIndex(settings.value("loggerType", _ui->comboBox_loggerType->currentIndex()).toInt());
 	_ui->checkBox_logger_printTime->setChecked(settings.value("loggerPrintTime", _ui->checkBox_logger_printTime->isChecked()).toBool());
 	_ui->checkBox_verticalLayoutUsed->setChecked(settings.value("verticalLayoutUsed", _ui->checkBox_verticalLayoutUsed->isChecked()).toBool());
-	_ui->checkBox_imageFlipped->setChecked(settings.value("imageFlipped", _ui->checkBox_imageFlipped->isChecked()).toBool());
 	_ui->checkBox_imageRejectedShown->setChecked(settings.value("imageRejectedShown", _ui->checkBox_imageRejectedShown->isChecked()).toBool());
 	_ui->checkBox_imageHighestHypShown->setChecked(settings.value("imageHighestHypShown", _ui->checkBox_imageHighestHypShown->isChecked()).toBool());
 	_ui->checkBox_beep->setChecked(settings.value("beep", _ui->checkBox_beep->isChecked()).toBool());
-	_ui->horizontalSlider_keypointsOpacity->setValue(settings.value("keypointsOpacity", _ui->horizontalSlider_keypointsOpacity->value()).toInt());
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 
@@ -1309,11 +1310,9 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
 	settings.setValue("loggerType", _ui->comboBox_loggerType->currentIndex());
 	settings.setValue("loggerPrintTime", _ui->checkBox_logger_printTime->isChecked());
 	settings.setValue("verticalLayoutUsed", _ui->checkBox_verticalLayoutUsed->isChecked());
-	settings.setValue("imageFlipped", _ui->checkBox_imageFlipped->isChecked());
 	settings.setValue("imageRejectedShown", _ui->checkBox_imageRejectedShown->isChecked());
 	settings.setValue("imageHighestHypShown", _ui->checkBox_imageHighestHypShown->isChecked());
 	settings.setValue("beep", _ui->checkBox_beep->isChecked());
-	settings.setValue("keypointsOpacity", _ui->horizontalSlider_keypointsOpacity->value());
 	settings.setValue("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value());
 	settings.setValue("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked());
 
@@ -1632,60 +1631,282 @@ void PreferencesDialog::readSettingsEnd()
 	_progressDialog->setValue(2); // this will make closing...
 }
 
-void PreferencesDialog::saveWindowGeometry(const QString & windowName, const QWidget * window)
+void PreferencesDialog::saveWindowGeometry(const QWidget * window)
 {
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
+	if(!window->objectName().isNull() && !window->isMaximized())
-	settings.beginGroup("Gui");
+	{
-	settings.beginGroup(windowName);
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
-	settings.setValue("geometry", window->saveGeometry());
+		settings.beginGroup("Gui");
-	settings.endGroup(); // "windowName"
+		settings.beginGroup(window->objectName());
-	settings.endGroup(); // rtabmap
+		settings.setValue("geometry", window->saveGeometry());
+		settings.endGroup(); // "windowName"
+		settings.endGroup(); // rtabmap
+	}
 }
 
-void PreferencesDialog::loadWindowGeometry(const QString & windowName, QWidget * window)
+void PreferencesDialog::loadWindowGeometry(QWidget * window)
 {
-	QByteArray bytes;
+	if(!window->objectName().isNull())
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
-	settings.beginGroup("Gui");
-	settings.beginGroup(windowName);
-	bytes = settings.value("geometry", QByteArray()).toByteArray();
-	if(!bytes.isEmpty())
 	{
-		window->restoreGeometry(bytes);
+		QByteArray bytes;
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(window->objectName());
+		bytes = settings.value("geometry", QByteArray()).toByteArray();
+		if(!bytes.isEmpty())
+		{
+			window->restoreGeometry(bytes);
+		}
+		settings.endGroup(); // "windowName"
+		settings.endGroup(); // rtabmap
 	}
-	settings.endGroup(); // "windowName"
-	settings.endGroup(); // rtabmap
 }
 
 void PreferencesDialog::saveMainWindowState(const QMainWindow * mainWindow)
 {
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
+	if(!mainWindow->objectName().isNull())
-	settings.beginGroup("Gui");
-	settings.beginGroup("MainWindow");
-	settings.setValue("state", mainWindow->saveState());
-	settings.endGroup(); // "MainWindow"
-	settings.endGroup(); // rtabmap
-
-	saveWindowGeometry("MainWindow", mainWindow);
-}
-
-void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow)
-{
-	QByteArray bytes;
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
-	settings.beginGroup("Gui");
-	settings.beginGroup("MainWindow");
-	bytes = settings.value("state", QByteArray()).toByteArray();
-	if(!bytes.isEmpty())
 	{
-		mainWindow->restoreState(bytes);
+		saveWindowGeometry(mainWindow);
-	}
-	settings.endGroup(); // "MainWindow"
-	settings.endGroup(); // rtabmap
 
-	loadWindowGeometry("MainWindow", mainWindow);
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(mainWindow->objectName());
+		settings.setValue("state", mainWindow->saveState());
+		settings.setValue("maximized", mainWindow->isMaximized());
+		settings.endGroup(); // "MainWindow"
+		settings.endGroup(); // rtabmap
+	}
 }
 
+void PreferencesDialog::loadMainWindowState(QMainWindow * mainWindow,  bool & maximized)
+{
+	if(!mainWindow->objectName().isNull())
+	{
+		loadWindowGeometry(mainWindow);
+
+		QByteArray bytes;
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(mainWindow->objectName());
+		bytes = settings.value("state", QByteArray()).toByteArray();
+		if(!bytes.isEmpty())
+		{
+			mainWindow->restoreState(bytes);
+		}
+		maximized = settings.value("maximized", false).toBool();
+		settings.endGroup(); // "MainWindow"
+		settings.endGroup(); // rtabmap
+	}
+}
+
+void PreferencesDialog::saveWidgetState(const QWidget * widget)
+{
+	if(!widget->objectName().isNull())
+	{
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(widget->objectName());
+
+		const CloudViewer * cloudViewer = qobject_cast<const CloudViewer*>(widget);
+		const ImageView * imageView = qobject_cast<const ImageView*>(widget);
+		const ExportCloudsDialog * exportCloudsDialog = qobject_cast<const ExportCloudsDialog*>(widget);
+		const PostProcessingDialog * postProcessingDialog = qobject_cast<const PostProcessingDialog *>(widget);
+		const GraphViewer * graphViewer = qobject_cast<const GraphViewer *>(widget);
+
+		if(cloudViewer)
+		{
+			float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
+			cloudViewer->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
+			QVector3D pose(poseX, poseY, poseZ);
+			QVector3D focal(focalX, focalY, focalZ);
+			if(!cloudViewer->isCameraFree())
+			{
+				// make camera position relative to target
+				Transform T = cloudViewer->getTargetPose();
+				if(cloudViewer->isCameraTargetLocked())
+				{
+					T = Transform(T.x(), T.y(), T.z(), 0,0,0);
+				}
+				Transform F(focalX, focalY, focalZ, 0,0,0);
+				Transform P(poseX, poseY, poseZ, 0,0,0);
+				Transform newFocal = T.inverse() * F;
+				Transform newPose = newFocal * F.inverse() * P;
+				pose = QVector3D(newPose.x(), newPose.y(), newPose.z());
+				focal = QVector3D(newFocal.x(), newFocal.y(), newFocal.z());
+			}
+			settings.setValue("camera_pose", pose);
+			settings.setValue("camera_focal", focal);
+			settings.setValue("camera_up", QVector3D(upX, upY, upZ));
+
+			settings.setValue("grid", cloudViewer->isGridShown());
+			settings.setValue("grid_cell_count", cloudViewer->getGridCellCount());
+			settings.setValue("grid_cell_size", cloudViewer->getGridCellSize());
+
+			settings.setValue("trajectory_shown", cloudViewer->isTrajectoryShown());
+			settings.setValue("trajectory_size", cloudViewer->getTrajectorySize());
+
+			settings.setValue("camera_target_locked", cloudViewer->isCameraTargetLocked());
+			settings.setValue("camera_target_follow", cloudViewer->isCameraTargetFollow());
+			settings.setValue("camera_free", cloudViewer->isCameraFree());
+			settings.setValue("camera_lockZ", cloudViewer->isCameraLockZ());
+
+			settings.setValue("bg_color", cloudViewer->getBackgroundColor());
+		}
+		else if(imageView)
+		{
+			settings.setValue("image_shown", imageView->isImageShown());
+			settings.setValue("depth_shown", imageView->isImageDepthShown());
+			settings.setValue("features_shown", imageView->isFeaturesShown());
+			settings.setValue("lines_shown", imageView->isLinesShown());
+			settings.setValue("alpha", imageView->getAlpha());
+		}
+		else if(exportCloudsDialog)
+		{
+			settings.setValue("assemble", exportCloudsDialog->getAssemble());
+			settings.setValue("assemble_voxel", exportCloudsDialog->getAssembleVoxel());
+			settings.setValue("regenerate", exportCloudsDialog->getGenerate());
+			settings.setValue("regenerate_decimation", exportCloudsDialog->getGenerateDecimation());
+			settings.setValue("regenerate_voxel", exportCloudsDialog->getGenerateVoxel());
+			settings.setValue("regenerate_max_depth", exportCloudsDialog->getGenerateMaxDepth());
+			settings.setValue("binary", exportCloudsDialog->getBinaryFile());
+			settings.setValue("mls", exportCloudsDialog->getMLS());
+			settings.setValue("mls_radius", exportCloudsDialog->getMLSRadius());
+			settings.setValue("mesh", exportCloudsDialog->getMesh());
+			settings.setValue("mesh_k", exportCloudsDialog->getMeshNormalKSearch());
+			settings.setValue("mesh_radius", exportCloudsDialog->getMeshGp3Radius());
+		}
+		else if(postProcessingDialog)
+		{
+			settings.setValue("detect_more_lc", postProcessingDialog->isDetectMoreLoopClosures());
+			settings.setValue("cluster_radius", postProcessingDialog->clusterRadius());
+			settings.setValue("cluster_angle", postProcessingDialog->clusterAngle());
+			settings.setValue("iterations", postProcessingDialog->iterations());
+			settings.setValue("reextract_features", postProcessingDialog->isReextractFeatures());
+			settings.setValue("refine_neigbors", postProcessingDialog->isRefineNeighborLinks());
+			settings.setValue("refine_lc", postProcessingDialog->isRefineLoopClosureLinks());
+		}
+		else if(graphViewer)
+		{
+			settings.setValue("node_radius", graphViewer->getNodeRadius());
+			settings.setValue("link_width", graphViewer->getLinkWidth());
+			settings.setValue("node_color", graphViewer->getNodeColor());
+			settings.setValue("neighbor_color", graphViewer->getNeighborColor());
+			settings.setValue("global_color", graphViewer->getGlobalLoopClosureColor());
+			settings.setValue("local_color", graphViewer->getLocalLoopClosureColor());
+			settings.setValue("user_color", graphViewer->getUserLoopClosureColor());
+			settings.setValue("virtual_color", graphViewer->getVirtualLoopClosureColor());
+			settings.setValue("local_path_color", graphViewer->getLocalPathColor());
+			settings.setValue("grid_visible", graphViewer->isGridMapVisible());
+		}
+		else
+		{
+			UERROR("Widget \"%s\" cannot be exported in config file.", widget->objectName().toStdString().c_str());
+		}
+
+		settings.endGroup(); // "name"
+		settings.endGroup(); // Gui
+	}
+}
+
+void PreferencesDialog::loadWidgetState(QWidget * widget)
+{
+	if(!widget->objectName().isNull())
+	{
+		QByteArray bytes;
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(widget->objectName());
+
+		CloudViewer * cloudViewer = qobject_cast<CloudViewer*>(widget);
+		ImageView * imageView = qobject_cast<ImageView*>(widget);
+		ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
+		PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
+		GraphViewer * graphViewer = qobject_cast<GraphViewer *>(widget);
+
+		if(cloudViewer)
+		{
+			float poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ;
+			cloudViewer->getCameraPosition(poseX, poseY, poseZ, focalX, focalY, focalZ, upX, upY, upZ);
+			QVector3D pose(poseX, poseY, poseZ), focal(focalX, focalY, focalZ), up(upX, upY, upZ);
+			pose = settings.value("camera_pose", pose).value<QVector3D>();
+			focal = settings.value("camera_focal", focal).value<QVector3D>();
+			up = settings.value("camera_up", up).value<QVector3D>();
+			cloudViewer->setCameraPosition(pose.x(),pose.y(),pose.z(), focal.x(),focal.y(),focal.z(), up.x(),up.y(),up.z());
+
+			cloudViewer->setGridShown(settings.value("grid", cloudViewer->isGridShown()).toBool());
+			cloudViewer->setGridCellCount(settings.value("grid_cell_count", cloudViewer->getGridCellCount()).toUInt());
+			cloudViewer->setGridCellSize(settings.value("grid_cell_size", cloudViewer->getGridCellSize()).toFloat());
+
+			cloudViewer->setTrajectoryShown(settings.value("trajectory_shown", cloudViewer->isTrajectoryShown()).toBool());
+			cloudViewer->setTrajectorySize(settings.value("trajectory_size", cloudViewer->getTrajectorySize()).toUInt());
+
+			cloudViewer->setCameraTargetLocked(settings.value("camera_target_locked", cloudViewer->isCameraTargetLocked()).toBool());
+			cloudViewer->setCameraTargetFollow(settings.value("camera_target_follow", cloudViewer->isCameraTargetFollow()).toBool());
+			if(settings.value("camera_free", cloudViewer->isCameraFree()).toBool())
+			{
+				cloudViewer->setCameraFree();
+			}
+			cloudViewer->setCameraLockZ(settings.value("camera_lockZ", cloudViewer->isCameraLockZ()).toBool());
+
+			cloudViewer->setBackgroundColor(settings.value("bg_color", cloudViewer->getBackgroundColor()).value<QColor>());
+		}
+		else if(imageView)
+		{
+			imageView->setImageShown(settings.value("image_shown", imageView->isImageShown()).toBool());
+			imageView->setImageDepthShown(settings.value("depth_shown", imageView->isImageDepthShown()).toBool());
+			imageView->setFeaturesShown(settings.value("features_shown", imageView->isFeaturesShown()).toBool());
+			imageView->setLinesShown(settings.value("lines_shown", imageView->isLinesShown()).toBool());
+			imageView->setAlpha(settings.value("alpha", imageView->getAlpha()).toInt());
+		}
+		else if(exportCloudsDialog)
+		{
+			exportCloudsDialog->setAssemble(settings.value("assemble", exportCloudsDialog->getAssemble()).toBool());
+			exportCloudsDialog->setAssembleVoxel(settings.value("assemble_voxel", exportCloudsDialog->getAssembleVoxel()).toDouble());
+			exportCloudsDialog->setGenerate(settings.value("regenerate", exportCloudsDialog->getGenerate()).toBool());
+			exportCloudsDialog->setGenerateDecimation(settings.value("regenerate_decimation", exportCloudsDialog->getGenerateDecimation()).toInt());
+			exportCloudsDialog->setGenerateVoxel(settings.value("regenerate_voxel", exportCloudsDialog->getGenerateVoxel()).toDouble());
+			exportCloudsDialog->setGenerateMaxDepth(settings.value("regenerate_max_depth", exportCloudsDialog->getGenerateMaxDepth()).toDouble());
+			exportCloudsDialog->setBinaryFile(settings.value("binary", exportCloudsDialog->getBinaryFile()).toBool());
+			exportCloudsDialog->setMLS(settings.value("mls", exportCloudsDialog->getMLS()).toBool());
+			exportCloudsDialog->setMLSRadius(settings.value("mls_radius", exportCloudsDialog->getMLSRadius()).toDouble());
+			exportCloudsDialog->setMesh(settings.value("mesh", exportCloudsDialog->getMesh()).toBool());
+			exportCloudsDialog->setMeshNormalKSearch(settings.value("mesh_k", exportCloudsDialog->getMeshNormalKSearch()).toInt());
+			exportCloudsDialog->setMeshGp3Radius(settings.value("mesh_radius", exportCloudsDialog->getMeshGp3Radius()).toDouble());
+		}
+		else if(postProcessingDialog)
+		{
+			postProcessingDialog->setDetectMoreLoopClosures(settings.value("detect_more_lc", postProcessingDialog->isDetectMoreLoopClosures()).toBool());
+			postProcessingDialog->setClusterRadius(settings.value("cluster_radius", postProcessingDialog->clusterRadius()).toDouble());
+			postProcessingDialog->setClusterAngle(settings.value("cluster_angle", postProcessingDialog->clusterAngle()).toDouble());
+			postProcessingDialog->setIterations(settings.value("iterations", postProcessingDialog->iterations()).toInt());
+			postProcessingDialog->setReextractFeatures(settings.value("reextract_features", postProcessingDialog->isReextractFeatures()).toBool());
+			postProcessingDialog->setRefineNeighborLinks(settings.value("refine_neigbors", postProcessingDialog->isRefineNeighborLinks()).toBool());
+			postProcessingDialog->setRefineLoopClosureLinks(settings.value("refine_lc", postProcessingDialog->isRefineLoopClosureLinks()).toBool());
+		}
+		else if(graphViewer)
+		{
+			graphViewer->setNodeRadius(settings.value("node_radius", graphViewer->getNodeRadius()).toDouble());
+			graphViewer->setLinkWidth(settings.value("link_width", graphViewer->getLinkWidth()).toDouble());
+			graphViewer->setNodeColor(settings.value("node_color", graphViewer->getNodeColor()).value<QColor>());
+			graphViewer->setNeighborColor(settings.value("neighbor_color", graphViewer->getNeighborColor()).value<QColor>());
+			graphViewer->setGlobalLoopClosureColor(settings.value("global_color", graphViewer->getGlobalLoopClosureColor()).value<QColor>());
+			graphViewer->setLocalLoopClosureColor(settings.value("local_color", graphViewer->getLocalLoopClosureColor()).value<QColor>());
+			graphViewer->setUserLoopClosureColor(settings.value("user_color", graphViewer->getUserLoopClosureColor()).value<QColor>());
+			graphViewer->setVirtualLoopClosureColor(settings.value("virtual_color", graphViewer->getVirtualLoopClosureColor()).value<QColor>());
+			graphViewer->setLocalPathColor(settings.value("local_path_color", graphViewer->getLocalPathColor()).value<QColor>());
+			graphViewer->setGridMapVisible(settings.value("grid_visible", graphViewer->isGridMapVisible()).toBool());
+		}
+		else
+		{
+			UERROR("Widget \"%s\" cannot be loaded from config file.", widget->objectName().toStdString().c_str());
+		}
+
+		settings.endGroup(); //"name"
+		settings.endGroup(); // Gui
+	}
+}
+
+
 void PreferencesDialog::saveCustomConfig(const QString & section, const QString & key, const QString & value)
 {
 	QSettings settings(getIniFilePath(), QSettings::IniFormat);
@@ -1708,23 +1929,10 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri
 	return value;
 }
 
-void PreferencesDialog::selectSourceImage(Src src)
+void PreferencesDialog::selectSourceImage(Src src, bool ckecked)
 {
 	ULOGGER_DEBUG("");
 
-	if(_ui->general_checkBox_activateRGBD->isChecked())
-	{
-		int button = QMessageBox::information(this,
-				tr("Desactivate RGB-D SLAM?"),
-				tr("You've selected source input as images only and RGB-D SLAM mode is activated. "
-				   "RGB-D SLAM cannot work with images only so do you want to desactivate it?"),
-				QMessageBox::Yes | QMessageBox::No);
-		if(button & QMessageBox::Yes)
-		{
-			_ui->general_checkBox_activateRGBD->setChecked(false);
-		}
-	}
-
 	bool fromPrefDialog = false;
 	//bool modified = false;
 	if(src == kSrcUndef)
@@ -1743,56 +1951,85 @@ void PreferencesDialog::selectSourceImage(Src src)
 			src = kSrcUsbDevice;
 		}
 	}
-	else
-	{
-		// from user
-		_ui->groupBox_sourceImage->setChecked(true);
-	}
 
-	if(src == kSrcImages)
+	if(!fromPrefDialog)
 	{
-		QString path = QFileDialog::getExistingDirectory(this, QString(), _ui->source_images_lineEdit_path->text());
+		if(ckecked)
-		QDir dir(path);
-		if(!path.isEmpty() && dir.exists())
 		{
-			QStringList filters;
+			if(_ui->general_checkBox_activateRGBD->isChecked())
-			filters << "*.jpg" << "*.ppm" << "*.bmp" << "*.png" << "*.pnm" << "*.tiff";
-			dir.setNameFilters(filters);
-			QFileInfoList files = dir.entryInfoList();
-			if(!files.empty())
 			{
-				_ui->source_comboBox_image_type->setCurrentIndex(1);
+				int button = QMessageBox::information(this,
-				_ui->source_images_lineEdit_path->setText(path);
+						tr("Desactivate RGB-D SLAM?"),
-				_ui->source_images_spinBox_startPos->setValue(1);
+						tr("You've selected source input as images only and RGB-D SLAM mode is activated. "
-				_ui->source_images_refreshDir->setChecked(false);
+						   "RGB-D SLAM cannot work with images only so do you want to desactivate it?"),
-			}
+						QMessageBox::Yes | QMessageBox::No);
-			else
+				if(button & QMessageBox::Yes)
-			{
+				{
-				QMessageBox::information(this,
+					_ui->general_checkBox_activateRGBD->setChecked(false);
-										   tr("RTAB-Map"),
+				}
-										   tr("Images must be one of these formats: ") + filters.join(" "));
 			}
 		}
-	}
+
-	else if(src == kSrcVideo)
+		_ui->groupBox_sourceImage->setChecked(false);
-	{
-		QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4)"));
-		QFile file(path);
-		if(!path.isEmpty() && file.exists())
-		{
-			_ui->source_comboBox_image_type->setCurrentIndex(2);
-			_ui->source_video_lineEdit_path->setText(path);
-		}
-	}
-	else // kSrcUsbDevice
-	{
-		_ui->source_comboBox_image_type->setCurrentIndex(0);
 	}
 
-	if(!fromPrefDialog && _obsoletePanels)
+	if(ckecked)
+	{
+		if(src == kSrcImages)
+		{
+			QString path = QFileDialog::getExistingDirectory(this, QString(), _ui->source_images_lineEdit_path->text());
+			QDir dir(path);
+			if(!path.isEmpty() && dir.exists())
+			{
+				QStringList filters;
+				filters << "*.jpg" << "*.ppm" << "*.bmp" << "*.png" << "*.pnm" << "*.tiff";
+				dir.setNameFilters(filters);
+				QFileInfoList files = dir.entryInfoList();
+				if(!files.empty())
+				{
+					_ui->source_comboBox_image_type->setCurrentIndex(1);
+					_ui->source_images_lineEdit_path->setText(path);
+					_ui->source_images_spinBox_startPos->setValue(1);
+					_ui->source_images_refreshDir->setChecked(false);
+					_ui->groupBox_sourceImage->setChecked(true);
+				}
+				else
+				{
+					QMessageBox::information(this,
+											   tr("RTAB-Map"),
+											   tr("Images must be one of these formats: ") + filters.join(" "));
+				}
+			}
+		}
+		else if(src == kSrcVideo)
+		{
+			QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->source_video_lineEdit_path->text(), tr("Videos (*.avi *.mpg *.mp4)"));
+			QFile file(path);
+			if(!path.isEmpty() && file.exists())
+			{
+				_ui->source_comboBox_image_type->setCurrentIndex(2);
+				_ui->source_video_lineEdit_path->setText(path);
+				_ui->groupBox_sourceImage->setChecked(true);
+			}
+		}
+		else // kSrcUsbDevice
+		{
+			_ui->source_comboBox_image_type->setCurrentIndex(0);
+			_ui->groupBox_sourceImage->setChecked(true);
+		}
+	}
+
+	if(_ui->groupBox_sourceImage->isChecked())
 	{
 		_ui->groupBox_sourceDatabase->setChecked(false);
 		_ui->groupBox_sourceOpenni->setChecked(false);
+	}
+
+	if(!fromPrefDialog)
+	{
+		// Even if there is no change, MainWindow should be notified
+		makeObsoleteSourcePanel();
+
 		if(validateForm())
 		{
 			this->writeSettings();
@@ -1804,72 +2041,85 @@ void PreferencesDialog::selectSourceImage(Src src)
 	}
 }
 
-void PreferencesDialog::selectSourceDatabase(bool user)
+void PreferencesDialog::selectSourceDatabase(bool user, bool ckecked)
 {
 	ULOGGER_DEBUG("");
 
-	QString dir = _ui->source_database_lineEdit_path->text();
+	if(user)
-	if(dir.isEmpty())
 	{
-		dir = getWorkingDirectory();
+		_ui->groupBox_sourceDatabase->setChecked(false);
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
+
-	QFile file(path);
+	if(ckecked)
-	if(!path.isEmpty() && file.exists())
 	{
-		int r = QMessageBox::question(this, tr("Odometry in database..."), tr("Use odometry saved in database (if some saved)?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+		QString dir = _ui->source_database_lineEdit_path->text();
-
+		if(dir.isEmpty())
-		if(user)
 		{
-			// from user
+			dir = getWorkingDirectory();
-			_ui->groupBox_sourceDatabase->setChecked(true);
 		}
-
+		QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
-		_ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes);
+		QFile file(path);
-		_ui->source_database_lineEdit_path->setText(path);
+		if(!path.isEmpty() && file.exists())
-		_ui->source_spinBox_databaseStartPos->setValue(0);
-
-		if(user && _obsoletePanels)
 		{
-			_ui->groupBox_sourceImage->setChecked(false);
+			int r = QMessageBox::question(this, tr("Odometry in database..."), tr("Use odometry saved in database (if some saved)?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
-			_ui->groupBox_sourceOpenni->setChecked(false);
+
-			if(validateForm())
+			_ui->groupBox_sourceDatabase->setChecked(true);
-			{
+			_ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes);
-				this->writeSettings();
+			_ui->source_database_lineEdit_path->setText(path);
-			}
+			_ui->source_spinBox_databaseStartPos->setValue(0);
-			else
+		}
-			{
+	}
-				this->readSettingsBegin();
+
-			}
+	if(_ui->groupBox_sourceDatabase->isChecked())
+	{
+		_ui->groupBox_sourceImage->setChecked(false);
+		_ui->groupBox_sourceOpenni->setChecked(false);
+	}
+
+	if(user)
+	{
+		// Even if there is no change, MainWindow should be notified
+		makeObsoleteSourcePanel();
+
+		if(validateForm())
+		{
+			this->writeSettings();
+		}
+		else
+		{
+			this->readSettingsBegin();
 		}
 	}
 }
 
-void PreferencesDialog::selectSourceRGBD(Src src)
+void PreferencesDialog::selectSourceRGBD(Src src, bool ckecked)
 {
 	ULOGGER_DEBUG("");
 
-	if(!_ui->general_checkBox_activateRGBD->isChecked())
+	if(ckecked)
 	{
-		int button = QMessageBox::information(this,
+		if(!_ui->general_checkBox_activateRGBD->isChecked())
-				tr("Activate RGB-D SLAM?"),
-				tr("You've selected RGB-D camera as source input, "
-				   "would you want to activate RGB-D SLAM mode?"),
-				QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
-		if(button & QMessageBox::Yes)
 		{
-			_ui->general_checkBox_activateRGBD->setChecked(true);
+			int button = QMessageBox::information(this,
+					tr("Activate RGB-D SLAM?"),
+					tr("You've selected RGB-D camera as source input, "
+					   "would you want to activate RGB-D SLAM mode?"),
+					QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+			if(button & QMessageBox::Yes)
+			{
+				_ui->general_checkBox_activateRGBD->setChecked(true);
+			}
 		}
 	}
 
-	_ui->groupBox_sourceOpenni->setChecked(true);
+	_ui->groupBox_sourceOpenni->setChecked(ckecked);
 	_ui->radioButton_opennipcl->setChecked(src == kSrcOpenNI_PCL);
 	_ui->radioButton_freenect->setChecked(src == kSrcFreenect);
 	_ui->radioButton_opennicv->setChecked(src == kSrcOpenNI_CV);
 	_ui->radioButton_opennicvasus->setChecked(src == kSrcOpenNI_CV_ASUS);
 	_ui->radioButton_openni2->setChecked(src == kSrcOpenNI2);
 
-	if(_obsoletePanels)
+	if(_ui->groupBox_sourceOpenni->isChecked())
 	{
 		_ui->groupBox_sourceImage->setChecked(false);
 		_ui->groupBox_sourceDatabase->setChecked(false);
@@ -1893,15 +2143,18 @@ void PreferencesDialog::selectSourceRGBD(Src src)
 				this->resetCalibration();
 			}
 		}
+	}
 
-		if(validateForm())
+	if(validateForm())
-		{
+	{
-			this->writeSettings();
+		// Even if there is no change, MainWindow should be notified
-		}
+		makeObsoleteSourcePanel();
-		else
+
-		{
+		this->writeSettings();
-			this->readSettingsBegin();
+	}
-		}
+	else
+	{
+		this->readSettingsBegin();
 	}
 }
 
@@ -2636,10 +2889,6 @@ bool PreferencesDialog::isVerticalLayoutUsed() const
 {
 	return _ui->checkBox_verticalLayoutUsed->isChecked();
 }
-bool PreferencesDialog::isImageFlipped() const
-{
-	return _ui->checkBox_imageFlipped->isChecked();
-}
 bool PreferencesDialog::imageRejectedShown() const
 {
 	return _ui->checkBox_imageRejectedShown->isChecked();
@@ -2652,10 +2901,6 @@ bool PreferencesDialog::beepOnPause() const
 {
 	return _ui->checkBox_beep->isChecked();
 }
-int PreferencesDialog::getKeypointsOpacity() const
-{
-	return _ui->horizontalSlider_keypointsOpacity->value();
-}
 int PreferencesDialog::getOdomQualityWarnThr() const
 {
 	return _ui->spinBox_odomQualityWarnThr->value();
@@ -2792,9 +3037,20 @@ bool PreferencesDialog::isSourceOpenniUsed() const
 }
 
 
-int PreferencesDialog::getSourceImageType() const
+PreferencesDialog::Src PreferencesDialog::getSourceImageType() const
 {
-	return _ui->source_comboBox_image_type->currentIndex();
+	if(_ui->source_comboBox_image_type->currentIndex() == 1)
+	{
+		return kSrcImages;
+	}
+	else if(_ui->source_comboBox_image_type->currentIndex() == 2)
+	{
+		return kSrcVideo;
+	}
+	else
+	{
+		return kSrcUsbDevice;
+	}
 }
 QString PreferencesDialog::getSourceImageTypeStr() const
 {

guilib/src/ui/calibrationDialog.ui

@@ -15,18 +15,42 @@
   </property>
   <layout class="QVBoxLayout" name="verticalLayout_3">
    <item>
-    <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="1,0">
+    <layout class="QHBoxLayout" name="horizontalLayout_3" stretch="1,0">
      <item>
       <layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0">
        <item>
         <widget class="UImageView" name="image_view" native="true"/>
        </item>
        <item>
-        <widget class="QCheckBox" name="checkBox_rectified">
+        <layout class="QHBoxLayout" name="horizontalLayout_2">
-         <property name="text">
+         <item>
-          <string>Show rectified</string>
+          <widget class="QCheckBox" name="checkBox_rectified">
-         </property>
+           <property name="text">
-        </widget>
+            <string>Show rectified</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QCheckBox" name="checkBox_mirror">
+           <property name="text">
+            <string>Mirror</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <spacer name="horizontalSpacer">
+           <property name="orientation">
+            <enum>Qt::Horizontal</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>40</width>
+             <height>20</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+        </layout>
        </item>
       </layout>
      </item>
@@ -88,7 +112,7 @@
           <item row="2" column="1">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_squareSize">
             <property name="decimals">
-             <number>3</number>
+             <number>4</number>
             </property>
             <property name="maximum">
              <double>1.000000000000000</double>
@@ -199,6 +223,9 @@
           <string>Camera intrinsic parameters</string>
          </property>
          <layout class="QFormLayout" name="formLayout_2">
+          <property name="fieldGrowthPolicy">
+           <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+          </property>
           <item row="0" column="0">
            <widget class="QLabel" name="label_8">
             <property name="text">
@@ -328,6 +355,13 @@
          </layout>
         </widget>
        </item>
+       <item>
+        <widget class="QPushButton" name="pushButton_save">
+         <property name="text">
+          <string>Export calibration (*.yaml)</string>
+         </property>
+        </widget>
+       </item>
        <item>
         <layout class="QHBoxLayout" name="horizontalLayout">
          <item>

guilib/src/ui/exportCloudsDialog.ui

@@ -301,7 +301,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
       <enum>Qt::Horizontal</enum>
      </property>
      <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults|QDialogButtonBox::Save</set>
      </property>
     </widget>
    </item>

guilib/src/ui/mainWindow.ui

@@ -178,6 +178,7 @@
      <addaction name="action360p"/>
      <addaction name="action240p"/>
     </widget>
+    <addaction name="actionSave_GUI_config"/>
     <addaction name="menuShow_view"/>
     <addaction name="menuFigures"/>
     <addaction name="actionScreenshot"/>
@@ -1116,6 +1117,11 @@
     <string>Post-processing...</string>
    </property>
   </action>
+  <action name="actionSave_GUI_config">
+   <property name="text">
+    <string>Save GUI config</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/postProcessingDialog.ui

@@ -206,7 +206,7 @@
       <enum>Qt::Horizontal</enum>
      </property>
      <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults</set>
      </property>
     </widget>
    </item>

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>732</width>
+            <width>744</width>
-            <height>1302</height>
+            <height>1074</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -143,7 +143,7 @@
                        <string>Loop closure detection view</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_31" columnstretch="0,1">
-                       <item row="2" column="0">
+                       <item row="1" column="0">
                         <widget class="QCheckBox" name="checkBox_imageRejectedShown">
                          <property name="text">
                           <string/>
@@ -153,7 +153,7 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="2" column="1">
                         <widget class="QLabel" name="label_105">
                          <property name="text">
                           <string>Show the image of the highest hypothesis.</string>
@@ -163,7 +163,7 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
+                       <item row="2" column="0">
                         <widget class="QCheckBox" name="checkBox_imageHighestHypShown">
                          <property name="text">
                           <string/>
@@ -174,32 +174,6 @@
                         </widget>
                        </item>
                        <item row="1" column="1">
-                        <widget class="QLabel" name="label_80">
-                         <property name="text">
-                          <string>Flip image (like a mirror).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>false</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="0">
-                        <widget class="QSlider" name="horizontalSlider_keypointsOpacity">
-                         <property name="minimum">
-                          <number>0</number>
-                         </property>
-                         <property name="maximum">
-                          <number>100</number>
-                         </property>
-                         <property name="sliderPosition">
-                          <number>16</number>
-                         </property>
-                         <property name="orientation">
-                          <enum>Qt::Horizontal</enum>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="1">
                         <widget class="QLabel" name="label_73">
                          <property name="text">
                           <string>Show image when an hypothesis is rejected.</string>
@@ -229,24 +203,7 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="0">
+                       <item row="3" column="1">
-                        <widget class="QCheckBox" name="checkBox_imageFlipped">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="1">
-                        <widget class="QLabel" name="label_102">
-                         <property name="text">
-                          <string>Keypoints opacity.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="1">
                         <widget class="QLabel" name="label_222">
                          <property name="text">
                           <string>Show loop closure hypotheses on the graph view. Nodes are colorized from blue to red depending on the highest hypothesis (which is red). Posterior hypotheses should be published. </string>
@@ -256,7 +213,7 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
+                       <item row="3" column="0">
                         <widget class="QCheckBox" name="checkBox_posteriorGraphView">
                          <property name="text">
                           <string/>
@@ -586,7 +543,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                             <number>10</number>
                            </property>
                            <property name="value">
-                            <number>0</number>
+                            <number>1</number>
                            </property>
                           </widget>
                          </item>
@@ -2811,6 +2768,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_10">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_42" columnstretch="0,1">
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_retrieved_4">
+                        <property name="text">
+                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QSpinBox" name="general_spinBox_maxStMemSize">
                         <property name="minimum">
@@ -2936,16 +2903,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_retrieved_4">
+                       <widget class="QLabel" name="label_retrieved_5">
                         <property name="text">
-                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
+                         <string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
                         </property>
                        </widget>
                       </item>
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_keepRawData">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -3021,7 +2998,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    </property>
                    <layout class="QGridLayout" name="gridLayout_11" columnstretch="0,1">
                     <item row="0" column="0">
-                     <widget class="QCheckBox" name="general_checkBox_keepRawData">
+                     <widget class="QCheckBox" name="general_checkBox_keepBinaryData">
                       <property name="text">
                        <string/>
                       </property>
@@ -3033,7 +3010,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item row="0" column="1">
                      <widget class="QLabel" name="label_keepRawData">
                       <property name="text">
-                       <string>Keep raw sensor data.</string>
+                       <string>Keep sensor compressed data.</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -4923,7 +4900,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="title">
                        <string>Map update</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_20">
+                      <layout class="QGridLayout" name="gridLayout_47" columnstretch="0,1">
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
                          <property name="suffix">
@@ -4973,16 +4950,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_163">
-                         <property name="text">
-                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
                          <property name="suffix">
@@ -5002,10 +4969,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_scanMatching">
+                        <widget class="QLabel" name="label_163">
                          <property name="text">
-                          <string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
+                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5019,6 +4986,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_scanMatching">
+                         <property name="text">
+                          <string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5077,10 +5054,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </layout>
                        </item>
                        <item>
-                        <layout class="QFormLayout" name="formLayout_5">
+                        <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
-                         <property name="fieldGrowthPolicy">
-                          <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                         </property>
                          <item row="0" column="0">
                           <widget class="QComboBox" name="globalDetection_icpType">
                            <property name="sizeAdjustPolicy">
@@ -5149,7 +5123,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <property name="title">
                        <string>Graph optimization</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_19">
+                      <layout class="QGridLayout" name="gridLayout_49" columnstretch="0,1">
                        <item row="0" column="0">
                         <widget class="QSpinBox" name="globalDetection_toroIterations">
                          <property name="minimum">
@@ -5170,13 +5144,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
+                       <item row="1" column="0">
-                        <widget class="QLabel" name="label_151">
+                        <widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
                          <property name="text">
-                          <string>Optimize graph from the newest node.
+                          <string/>
--If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). An odometry correction from frames /map to /odom is computed.
+                         </property>
--If true, there is no odometry correction computed since it is all previous poses in the map that are corrected, not the last one (which corresponds to latest odometry value).
+                        </widget>
-Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_141">
+                         <property name="text">
+                          <string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5190,17 +5168,13 @@ Warning when set to false: when some nodes are transferred, the first referentia
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="0">
+                       <item row="2" column="1">
-                        <widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
+                        <widget class="QLabel" name="label_151">
                          <property name="text">
-                          <string/>
+                          <string>Optimize graph from the newest node.
-                         </property>
+-If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). An odometry correction from frames /map to /odom is computed.
-                        </widget>
+-If true, there is no odometry correction computed since it is all previous poses in the map that are corrected, not the last one (which corresponds to latest odometry value).
-                       </item>
+Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
-                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_141">
-                         <property name="text">
-                          <string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5252,7 +5226,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
                         </widget>
                        </item>
                        <item>
-                        <layout class="QFormLayout" name="formLayout_13">
+                        <layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
                          <item row="0" column="0">
                           <widget class="QDoubleSpinBox" name="localDetection_radius">
                            <property name="suffix">
@@ -5290,16 +5264,6 @@ Warning when set to false: when some nodes are transferred, the first referentia
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_space3_2">
-                           <property name="text">
-                            <string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="2" column="0">
                           <widget class="QSpinBox" name="localDetection_maxDiffID">
                            <property name="maximum">
@@ -5310,11 +5274,70 @@ Warning when set to false: when some nodes are transferred, the first referentia
                            </property>
                           </widget>
                          </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_space3_2">
+                           <property name="text">
+                            <string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
                      </widget>
                     </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_12">
+                      <property name="title">
+                       <string>Graph planning</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_51" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QDoubleSpinBox" name="graphPlan_goalReachedRadius">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_space2_2">
+                         <property name="text">
+                          <string>Goal reached radius.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="graphPlan_maxAnticipatedNodes">
+                         <property name="maximum">
+                          <number>999999999</number>
+                         </property>
+                         <property name="value">
+                          <number>10</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_space3_3">
+                         <property name="text">
+                          <string>Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by &quot;Maximum locations retrieved&quot; from the Memory panel).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.8.2</version>
+  <version>0.8.3</version>
   <description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/CMakeLists.txt

@@ -2,6 +2,7 @@
 ADD_SUBDIRECTORY( ConsoleApp )
 ADD_SUBDIRECTORY( ImagesJoiner )
 ADD_SUBDIRECTORY( ExtractObject )
+ADD_SUBDIRECTORY( Camera )
 ADD_SUBDIRECTORY( CameraRGBD )
 
 IF(OPENCV_NONFREE_FOUND)

tools/Calibration/main.cpp

@@ -25,17 +25,22 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/gui/CalibrationDialog.h"
 #include <QtGui/QApplication>
 
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-calibration driver\n"
+			"rtabmap-calibration [options]\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+			"Options:\n"
+			"  --driver #     Driver number to use: 0=USB camera, 1=OpenNI-PCL, 2=OpenNI2,\n"
+			"                                       3=Freenect, 4=OpenNI-CV, 5=OpenNI-CV-ASUS\n"
+			"  --device #     Device id\n\n");
 	exit(1);
 }
 
@@ -45,28 +50,72 @@ int main(int argc, char * argv[])
 	ULogger::setLevel(ULogger::kInfo);
 
 	int driver = 0;
-	if(argc < 2)
+	int device = 0;
+	for(int i=1; i<argc; ++i)
 	{
-		showUsage();
+		if(strcmp(argv[i], "--driver") == 0)
-	}
+		{
-	else
+			++i;
-	{
+			if(i < argc)
-		driver = atoi(argv[argc-1]);
+			{
-		if(driver < 0 || driver > 4)
+				driver = std::atoi(argv[i]);
+				if(driver < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--device") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				device = std::atoi(argv[i]);
+				if(device < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--help") == 0)
 		{
-			UERROR("driver should be between 0 and 4.");
 			showUsage();
 		}
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
 	}
+	driver = atoi(argv[argc-1]);
+	if(driver < 0 || driver > 5)
+	{
+		UERROR("driver should be between 0 and 5.");
+		showUsage();
+	}
+
 	UINFO("Using driver %d", driver);
+	UINFO("Using device %d", device);
 
 	float imageRate = 1.0f;
+	rtabmap::Camera * cameraUsb = 0;
 	rtabmap::CameraRGBD * camera = 0;
 	if(driver == 0)
 	{
-		camera = new rtabmap::CameraOpenni("", imageRate);
+		cameraUsb = new rtabmap::CameraVideo(device, imageRate);
 	}
 	else if(driver == 1)
+	{
+		camera = new rtabmap::CameraOpenni(uNumber2Str(device), imageRate);
+	}
+	else if(driver == 2)
 	{
 		if(!rtabmap::CameraOpenNI2::available())
 		{
@@ -75,16 +124,16 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraOpenNI2(imageRate);
 	}
-	else if(driver == 2)
+	else if(driver == 3)
 	{
 		if(!rtabmap::CameraFreenect::available())
 		{
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect(0, imageRate);
+		camera = new rtabmap::CameraFreenect(device, imageRate);
 	}
-	else if(driver == 3)
+	else if(driver == 4)
 	{
 		if(!rtabmap::CameraOpenNICV::available())
 		{
@@ -93,7 +142,7 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraOpenNICV(false, imageRate);
 	}
-	else if(driver == 4)
+	else if(driver == 5)
 	{
 		if(!rtabmap::CameraOpenNICV::available())
 		{
@@ -107,21 +156,36 @@ int main(int argc, char * argv[])
 		UFATAL("");
 	}
 
-	if(!camera->init())
+	rtabmap::CameraThread * cameraThread = 0;
-	{
-		printf("Camera init failed!\n");
-		delete camera;
-		exit(1);
-	}
 
-	rtabmap::CameraThread cameraThread(camera);
+	if(cameraUsb)
+	{
+		if(!cameraUsb->init())
+		{
+			printf("Camera init failed!\n");
+			delete cameraUsb;
+			exit(1);
+		}
+		cameraThread = new rtabmap::CameraThread(cameraUsb);
+	}
+	else if(camera)
+	{
+		if(!camera->init())
+		{
+			printf("Camera init failed!\n");
+			delete camera;
+			exit(1);
+		}
+		cameraThread = new rtabmap::CameraThread(camera);
+	}
 
 	QApplication app(argc, argv);
 	rtabmap::CalibrationDialog dialog;
 	dialog.registerToEventsManager();
 
 	dialog.show();
-	cameraThread.start();
+	cameraThread->start();
 	app.exec();
-	cameraThread.join(true);
+	cameraThread->join(true);
+	delete cameraThread;
 }

tools/Camera/CMakeLists.txt

@@ -0,0 +1,25 @@
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(camera main.cpp)
+TARGET_LINK_LIBRARIES(camera rtabmap_core rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( camera 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-camera)
+  
+INSTALL(TARGETS camera
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/Camera/main.cpp

@@ -0,0 +1,214 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/DBReader.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <stdio.h>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-camera [option] \n"
+			" Options:\n"
+			"    --device #            USB camera device id (default 0).\n"
+			"    --rate #              Frame rate (default 30 Hz). 0 means as fast as possible.\n"
+			"    --path ""             Path to a directory of images or a video file.\n"
+			"    --calibration ""      Calibration file (*.yaml).\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	int device = 0;
+	std::string path;
+	float rate = 30.0f;
+	std::string calibrationFile;
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "--rate") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = uStr2Float(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--device") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				device = std::atoi(argv[i]);
+				if(device < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--path") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				path = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--calibration") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				calibrationFile = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	if(path.empty())
+	{
+		UINFO("Using device %d", device);
+	}
+	else
+	{
+		UINFO("Using path %s", path.c_str());
+	}
+
+	rtabmap::Camera * camera = 0;
+	rtabmap::DBReader * dbReader = 0;
+
+	if(!path.empty())
+	{
+		if(UFile::exists(path))
+		{
+			if(UFile::getExtension(path).compare("db") == 0)
+			{
+				dbReader = new rtabmap::DBReader(path, rate);
+			}
+			else
+			{
+				camera = new rtabmap::CameraVideo(path, rate);
+			}
+		}
+		else if(UDirectory::exists(path))
+		{
+			camera = new rtabmap::CameraImages(path, rate);
+		}
+		else
+		{
+			UERROR("Path not valid! \"%s\"", path.c_str());
+			return -1;
+		}
+	}
+	else
+	{
+		camera = new rtabmap::CameraVideo(device, rate);
+	}
+
+	if(camera)
+	{
+		if(!camera->init())
+		{
+			delete camera;
+			UERROR("Cannot initialize the camera.");
+			return -1;
+		}
+
+		if(!calibrationFile.empty())
+		{
+			UINFO("Set calibration: %s", calibrationFile.c_str());
+			camera->setCalibration(calibrationFile);
+		}
+	}
+
+	if(dbReader)
+	{
+		if(!dbReader->init())
+		{
+			delete dbReader;
+			UERROR("Cannot initialize the camera.");
+			return -1;
+		}
+	}
+
+	cv::Mat rgb;
+	rgb = camera?camera->takeImage():dbReader->getNextData().image();
+	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
+	while(!rgb.empty())
+	{
+		cv::imshow("Video", rgb); // show frame
+
+		int c = cv::waitKey(10); // wait 10 ms or for key stroke
+		if(c == 27)
+			break; // if ESC, break and quit
+
+		rgb = camera?camera->takeImage():dbReader->getNextData().image();
+	}
+	cv::destroyWindow("Video");
+	if(camera)
+	{
+		delete camera;
+	}
+	if(dbReader)
+	{
+		delete dbReader;
+	}
+	return 0;
+}

tools/ConsoleApp/main.cpp

@@ -49,7 +49,6 @@ void showUsage()
 			"  path                            For images, use the directory path. For videos or databases, use full\n "
 			"                                  path name\n"
 			"Options:\n"
-			"  -t #.##                         Time threshold (ms)\n"
 			"  -rate #.##                      Acquisition time (seconds)\n"
 			"  -rateHz #.##                    Acquisition rate (Hz), for convenience\n"
 			"  -repeat #                       Repeat the process on the data set # times (minimum of 1)\n"
@@ -114,7 +113,6 @@ int main(int argc, char * argv[])
 	printf("\n");
 
 	std::string path;
-	float timeThreshold = 0.0;
 	float rate = 0.0;
 	int loopDataset = 0;
 	int repeat = 0;
@@ -141,29 +139,12 @@ int main(int argc, char * argv[])
 			}
 			break;
 		}
-		if(strcmp(argv[i], "-t") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				timeThreshold = std::atof(argv[i]);
-				if(timeThreshold < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
 		if(strcmp(argv[i], "-rate") == 0)
 		{
 			++i;
 			if(i < argc)
 			{
-				rate = std::atof(argv[i]);
+				rate = uStr2Float(argv[i]);
 				if(rate < 0)
 				{
 					showUsage();
@@ -180,7 +161,7 @@ int main(int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				rate = std::atof(argv[i]);
+				rate = uStr2Float(argv[i]);
 				if(rate < 0)
 				{
 					showUsage();
@@ -401,7 +382,6 @@ int main(int argc, char * argv[])
 	pm.insert(ParametersPair(Parameters::kRGBDEnabled(), "false"));
 
 	rtabmap.init(pm, inputDbPath);
-	rtabmap.setTimeThreshold(timeThreshold); // in ms
 
 	printf("Avpd init time = %fs\n", timer.ticks());
 
@@ -412,7 +392,7 @@ int main(int argc, char * argv[])
 
 	printf("\nParameters : \n");
 	printf(" Data set : %s\n", path.c_str());
-	printf(" Time threshold = %1.2f ms\n", timeThreshold);
+	printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
 	printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
 	printf(" Repeating data set = %s\n", repeat?"true":"false");
 	printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
@@ -502,19 +482,19 @@ int main(int argc, char * argv[])
 			if(rtabmap.getLoopClosureId())
 			{
 				printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
-						count, rtabmap.getLoopClosureId(), rtabmap.getLcHypValue(), rtabmapTime, iterationTime);
+						count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime);
 			}
-			else if(rtabmap.getRetrievedId())
+			else if(rtabmap.getHighestHypothesisId())
 			{
 				printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
-						count, rtabmap.getRetrievedId(), rtabmap.getLcHypValue(), rtabmapTime, iterationTime);
+						count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime);
 			}
 			else
 			{
 				printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
 			}
 
-			if(timeThreshold && rtabmapTime > timeThreshold*100.0f)
+			if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f)
 			{
 				printf(" ERROR,  there is  problem, too much time taken... %fs", rtabmapTime);
 				break; // there is  problem, don't continue

tools/DataRecorder/main.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/core/CameraRGBD.h>
 #include <rtabmap/core/Camera.h>
@@ -91,7 +92,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				rate = std::atof(argv[i]);
+				rate = uStr2Float(argv[i]);
 				if(rate < 0.0f)
 				{
 					showUsage();

tools/ExtractObject/main.cpp

@@ -72,7 +72,7 @@ int main(int argc, char *argv[])
 			++i;
 			if(i < argc)
 			{
-				planeDistanceThreshold = std::atof(argv[i]);
+				planeDistanceThreshold = uStr2Float(argv[i]);
 				if(planeDistanceThreshold < 0.0f)
 				{
 					showUsage();
@@ -89,7 +89,7 @@ int main(int argc, char *argv[])
 			++i;
 			if(i < argc)
 			{
-				planeEpsAngle = std::atof(argv[i]);
+				planeEpsAngle = uStr2Float(argv[i]);
 				if(planeEpsAngle < 0.0f)
 				{
 					showUsage();
@@ -106,7 +106,7 @@ int main(int argc, char *argv[])
 			++i;
 			if(i < argc)
 			{
-				clusterTolerance = std::atof(argv[i]);
+				clusterTolerance = uStr2Float(argv[i]);
 				if(clusterTolerance <= 0.0f)
 				{
 					showUsage();

tools/ImagesJoiner/main.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
@@ -35,97 +36,70 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 void showUsage()
 {
 	printf("Usage:\n"
-			"imagesJoiner.exe \"# (see below)\" \"type\" [option]\n"
+			"imagesJoiner.exe [option] path\n"
-			"  # :      Name pattern is how the file names are formatted (in number).\n"
-			"           Examples: 4 for pictures with 0001.jpg, 0002.jpg, ..., 0010.jpg, 0100.jpg, 1000.jpg, ...\n"
-			"                     1 for pictures with 1.jpg, 2.jpg, ..., 10.jpg, 100.jpg, 1000.jpg, ...\n"
-			"  type :   is the extension (jpg, bmp, png...)\n"
 			"  Options:\n"
-			"    -inv       option for copying odd images on the right\n"
+			"    -inv       option for copying odd images on the right\n\n");
-			"    -d #       destination filename size\n");
 	exit(1);
 }
 
 int main(int argc, char * argv[])
 {
-	if(argc < 3)
+	if(argc < 2)
 	{
 		showUsage();
 	}
 
 	bool inv = false;
-	unsigned int sizeFileName = std::atoi(argv[1]);
+	for(int i=1; i<argc-1; ++i)
-	int sizeTargetFileName = sizeFileName;
-
-	std::string type = argv[2];
-	for(int i=3; i<argc; ++i)
 	{
 		if(strcmp(argv[i], "-inv") == 0)
 		{
 			inv = true;
 			printf(" Inversing option activated...\n");
+			continue;
 		}
-		if(strcmp(argv[i], "-d") == 0 && i+1<argc)
+		showUsage();
-		{
+		printf(" Not recognized option: \"%s\"\n", argv[i]);
-			sizeTargetFileName = std::atoi(argv[i+1]);
-			if(sizeTargetFileName < 0)
-			{
-				showUsage();
-			}
-			printf(" Target size file name option activated=%d\n",sizeTargetFileName);
-			++i;
-		}
 	}
 
-	printf(" Format = %s\n", argv[1]);
+	std::string path = argv[argc-1];
-	printf(" Type = %s\n", argv[2]);
+	printf(" Path = %s\n", path.c_str());
 
-	std::string targetDirectory = "imagesJoined/";
+	UDirectory dir(path, "jpg bmp png tiff jpeg");
-	UDirectory::makeDir((UDirectory::currentDir(true) + "imagesJoined").c_str());
+	if(!dir.isValid())
+	{
+		printf("Path invalid!\n");
+		exit(-1);
+	}
 
-	int counterJoined = 1;
+	std::string targetDirectory = path+"_joined";
-	int counterImages = 1;
+	UDirectory::makeDir(targetDirectory);
-	bool imagesExist = true;
+	printf(" Creating directory \"%s\"\n", targetDirectory.c_str());
 
-	std::string fileNameA;
-	std::string fileNameB;
 
-	while(imagesExist)
+	std::string fileNameA = dir.getNextFilePath();
+	std::string fileNameB = dir.getNextFilePath();
+
+	int i=1;
+	while(!fileNameA.empty() && !fileNameB.empty())
 	{
-		std::string fileNameTarget = uNumber2Str(counterJoined);
 		if(inv)
 		{
-			fileNameA =  uNumber2Str(counterImages+1);
+			std::string tmp = fileNameA;
-			fileNameB =  uNumber2Str(counterImages);
+			fileNameA = fileNameB;
-		}
+			fileNameB = tmp;
-		else
-		{
-			fileNameA =  uNumber2Str(counterImages);
-			fileNameB =  uNumber2Str(counterImages+1);
 		}
 
-		while(fileNameA.size() < sizeFileName)
+		std::string ext = UFile::getExtension(fileNameA);
-		{
-			fileNameA.insert(0, "0");
-		}
 
-		while(fileNameB.size() < sizeFileName)
+		std::string targetFilePath = targetDirectory+UDirectory::separator()+uNumber2Str(i++)+"."+ext;
-		{
-			fileNameB.insert(0, "0");
-		}
-
-		while(fileNameTarget.size() < (unsigned int)sizeTargetFileName)
-		{
-			fileNameTarget.insert(0, "0");
-		}
-
-		(fileNameTarget.insert(0, targetDirectory) += ".") += type;
-		(fileNameA += ".") += type;
-		(fileNameB += ".") += type;
 
 		IplImage * imageA = cvLoadImage(fileNameA.c_str(), CV_LOAD_IMAGE_COLOR);
 		IplImage * imageB = cvLoadImage(fileNameB.c_str(), CV_LOAD_IMAGE_COLOR);
 
+		fileNameA.clear();
+		fileNameB.clear();
+
 		if(imageA && imageB)
 		{
 			CvSize sizeA = cvGetSize(imageA);
@@ -142,26 +116,28 @@ int main(int argc, char * argv[])
 				cvCopy( imageB, targetImage );
 				cvResetImageROI( targetImage );
 
-				if(!cvSaveImage(fileNameTarget.c_str(), targetImage))
+				if(!cvSaveImage(targetFilePath.c_str(), targetImage))
 				{
-					printf("Error : saving to \"%s\" goes wrong...\n", fileNameTarget.c_str());
+					printf("Error : saving to \"%s\" goes wrong...\n", targetFilePath.c_str());
 				}
 				else
 				{
-					printf("Saved \"%s\" \n", fileNameTarget.c_str());
+					printf("Saved \"%s\" \n", targetFilePath.c_str());
 				}
 
 				cvReleaseImage(&targetImage);
+
+				fileNameA = dir.getNextFilePath();
+				fileNameB = dir.getNextFilePath();
 			}
 			else
 			{
 				printf("Error : can't allocated the target image with size (%d,%d)\n", targetSize.width, targetSize.height);
-				imagesExist = false;
 			}
 		}
 		else
 		{
-			imagesExist = false;
+			printf("Error: loading images failed!\n");
 		}
 
 		if(imageA)
@@ -172,13 +148,8 @@ int main(int argc, char * argv[])
 		{
 			cvReleaseImage(&imageB);
 		}
-
-		counterJoined++;
-		counterImages += 2;
 	}
-
+	printf("%d files processed\n", i-1);
-
-
 
 	return 0;
 }

tools/OdometryViewer/main.cpp

@@ -175,7 +175,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				nndr = std::atof(argv[i]);
+				nndr = uStr2Float(argv[i]);
 				if(nndr < 0.0f)
 				{
 					showUsage();
@@ -192,7 +192,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				rate = std::atof(argv[i]);
+				rate = uStr2Float(argv[i]);
 				if(rate < 0)
 				{
 					showUsage();
@@ -244,7 +244,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				sec = std::atof(argv[i]);
+				sec = uStr2Float(argv[i]);
 				if(sec < 0.0f)
 				{
 					showUsage();
@@ -261,7 +261,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				distance = std::atof(argv[i]);
+				distance = uStr2Float(argv[i]);
 				if(distance <= 0)
 				{
 					showUsage();
@@ -312,7 +312,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				maxDepth = std::atof(argv[i]);
+				maxDepth = uStr2Float(argv[i]);
 				if(maxDepth < 0)
 				{
 					showUsage();
@@ -380,7 +380,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				voxel = std::atof(argv[i]);
+				voxel = uStr2Float(argv[i]);
 				if(voxel < 0)
 				{
 					showUsage();
@@ -414,7 +414,7 @@ int main (int argc, char * argv[])
 			++i;
 			if(i < argc)
 			{
-				ratio = std::atof(argv[i]);
+				ratio = uStr2Float(argv[i]);
 				if(ratio < 0.0f)
 				{
 					showUsage();

tools/VocabularyComparison/main.cpp

@@ -117,7 +117,7 @@ int main(int argc, char * argv[])
 					//get descriptor
 					for(;i<dimension && iter != strList.end(); ++i, ++iter)
 					{
-						descriptor[i] = std::atof(iter->c_str());
+						descriptor[i] = uStr2Float(*iter);
 					}
 					if(i != dimension)
 					{

utilite/include/rtabmap/utilite/UConversion.h

@@ -117,6 +117,22 @@ std::string UTILITE_EXP uNumber2Str(float number);
  */
 std::string UTILITE_EXP uNumber2Str(double number);
 
+
+/**
+ * Convert a string to a float independent of the locale (comma/dot).
+ * @param the string
+ * @return the number
+ */
+float UTILITE_EXP uStr2Float(const std::string & str);
+
+/**
+ * Convert a string to a double independent of the locale (comma/dot).
+ * @param the string
+ * @return the number
+ */
+double UTILITE_EXP uStr2Double(const std::string & str);
+
+
 /**
  * Convert a bool to a string.
  * The format used is "true" and "false".

utilite/include/rtabmap/utilite/UDirectory.h

@@ -115,6 +115,12 @@ public:
 	 */
 	std::string getNextFileName();
 
+	/**
+	 * Get the next file path.
+	 * @return the next file path
+	 */
+	std::string getNextFilePath();
+
 	/**
 	 * Get all file names.
 	 * @see UDirectory()

utilite/include/rtabmap/utilite/UMath.h

@@ -533,18 +533,68 @@ inline T uVariance(const std::vector<T> & v, const T & m)
 }
 
 /**
- * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
+ * Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3 + ...
- * @return the norm of the vector
+ * @return the squared norm of the vector
  */
 template<class T>
-inline T uNorm(const std::vector<T> & v)
+inline T uNormSquared(const std::vector<T> & v)
 {
 	float sum = 0.0f;
 	for(unsigned int i=0; i<v.size(); ++i)
 	{
 		sum += v[i]*v[i];
 	}
-	return std::sqrt(sum);
+	return sum;
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const std::vector<T> & v)
+{
+	return std::sqrt(uNormSquared(v));
+}
+
+/**
+ * Get the squared norm of the vector : return x1*x1 + x2*x2
+ * @return the squared norm of the vector
+ */
+template<class T>
+inline T uNormSquared(const T & x1, const T & x2)
+{
+	return x1*x1 + x2*x2;
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const T & x1, const T & x2)
+{
+	return std::sqrt(uNormSquared(x1, x2));
+}
+
+/**
+ * Get the squared norm of the vector : return x1*x1 + x2*x2 + x3*x3
+ * @return the squared norm of the vector
+ */
+template<class T>
+inline T uNormSquared(const T & x1, const T & x2, const T & x3)
+{
+	return x1*x1 + x2*x2 + x3*x3;
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const T & x1, const T & x2, const T & x3)
+{
+	return std::sqrt(uNormSquared(x1, x2, x3));
 }
 
 /**

utilite/src/UConversion.cpp

@@ -113,6 +113,24 @@ std::string uNumber2Str(double number)
 	return s.str();
 }
 
+float uStr2Float(const std::string & str)
+{
+	float value = 0.0f;
+	std::istringstream istr(uReplaceChar(str, ',', '.').c_str());
+	istr.imbue(std::locale("C"));
+	istr >> value;
+	return value;
+}
+
+double uStr2Double(const std::string & str)
+{
+	double value = 0.0;
+	std::istringstream istr(uReplaceChar(str, ',', '.').c_str());
+	istr.imbue(std::locale("C"));
+	istr >> value;
+	return value;
+}
+
 std::string uBool2Str(bool boolean)
 {
 	std::string s;

utilite/src/UDirectory.cpp

@@ -229,6 +229,17 @@ std::string UDirectory::getNextFileName()
 	return fileName;
 }
 
+std::string UDirectory::getNextFilePath()
+{
+	std::string filePath;
+	if(iFileName_ != fileNames_.end())
+	{
+		filePath = path_+separator()+*iFileName_;
+		++iFileName_;
+	}
+	return filePath;
+}
+
 void UDirectory::rewind()
 {
 	iFileName_ = fileNames_.begin();