Moved some util3d methods to Transform.h, Graph.h and Compression.h

Added computePath() method implementating A star on graph
GUI: SetWindowModified() and user should explicitly save the GUI config to keep them
Calibration: added mirror checkbox, added device id argument

app/src/main.cpp

@@ -52,7 +52,7 @@ int main(int argc, char* argv[])
 	UEventsManager::addHandler(mainWindow);
 
 	/* Start thread's task */
-	mainWindow->showNormal();
+	mainWindow->show();
 
 	RtabmapThread * rtabmap = new RtabmapThread(new Rtabmap());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui

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/Graph.h

@@ -0,0 +1,96 @@
+/*
+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 {
+
+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);
+
+std::vector<int> RTABMAP_EXP computePath(
+			const std::map<int, rtabmap::Transform> & poses,
+			const std::multimap<int, int> & links,
+			int from,
+			int to);
+
+} /* namespace rtabmap */
+#endif /* GRAPH_H_ */

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()
-	{
-		return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
-	}
+	Eigen::Matrix4f toEigen4f() const;
+	Eigen::Matrix4d toEigen4d() const;
+	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/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/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/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);
-			util3d::CompressionThread ctDepth(depthBytes, true);
-			util3d::CompressionThread ctLaserScan(laserScanBytes, false);
+			rtabmap::CompressionThread ctImage(imageBytes, true);
+			rtabmap::CompressionThread ctDepth(depthBytes, true);
+			rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
 			ctImage.start();
 			ctDepth.start();
 			ctLaserScan.start();

corelib/src/Graph.cpp

@@ -0,0 +1,755 @@
+/*
+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 <pcl/search/kdtree.h>
+#include <pcl/common/eigen.h>
+#include <pcl/common/common.h>
+#include <set>
+#include <queue>
+#include "toro3d/treeoptimizer3.hh"
+
+namespace rtabmap {
+
+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!
+			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 = 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 = 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 = 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 = 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;
+			}
+			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)
+				{
+					UERROR("newCostSoFar > previous cost (%f vs %f)", newCostSoFar, nodeIter->second.costSoFar());
+				}
+			}
+		}
+	}
+	return uListToVector(path);
+}
+
+
+} /* 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>
@@ -1658,7 +1659,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)
@@ -1923,7 +1924,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);
@@ -2040,7 +2041,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
 			{
@@ -2059,7 +2060,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)
@@ -3302,9 +3303,9 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 		{
 			depthOrRightImage = data.rightImage();
 		}
-		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
-		util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
-		util3d::CompressionThread ctDepth2d(data.laserScan());
+		rtabmap::CompressionThread ctImage(data.image(), std::string(".jpg"));
+		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
+		rtabmap::CompressionThread ctDepth2d(data.laserScan());
 		ctImage.start();
 		ctDepth.start();
 		ctDepth2d.start();
@@ -3333,7 +3334,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			words,
 			words3D,
 			data.pose(),
-			util3d::compressData2(data.laserScan()));
+			rtabmap::compressData2(data.laserScan()));
 	}
 	if(this->isRawDataKept())
 	{

corelib/src/Rtabmap.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/Version.h"
 #include "rtabmap/core/Features2d.h"
-
+#include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Signature.h"
 
 #include "rtabmap/core/EpipolarGeometry.h"
@@ -646,7 +646,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 		}
 
-		util3d::saveTOROGraph(path, poses, constraints);
+		rtabmap::saveTOROGraph(path, poses, constraints);
 	}
 }
 
@@ -1264,6 +1264,8 @@ bool Rtabmap::process(const SensorData & data)
 				uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
 				uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
 				uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+				uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
+				uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
 				uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
 
 				//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
@@ -2010,7 +2012,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		else
 		{
-			util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
+			rtabmap::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
 		}
 	}
 }

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>
@@ -283,9 +283,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()))
 	{
-		util3d::CompressionThread ctImage(_imageCompressed, true);
-		util3d::CompressionThread ctDepth(_depthCompressed, true);
-		util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
+		rtabmap::CompressionThread ctImage(_imageCompressed, true);
+		rtabmap::CompressionThread ctDepth(_depthCompressed, true);
+		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 util3d::transformFromEigen4f(m.inverse());
+	return fromEigen4f(toEigen4f().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);
-	Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
-	return util3d::transformFromEigen4f(m1*m2);
+	return fromEigen4f(toEigen4f()*t.toEigen4f());
 }
 
 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/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,

guilib/include/rtabmap/gui/CloudViewer.h

@@ -185,6 +185,9 @@ public slots:
 	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);

guilib/include/rtabmap/gui/ImageView.h

@@ -75,6 +75,9 @@ public:
 	void clearLines();
 	void clear();
 
+signals:
+	void configChanged();
+
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * e);
 	virtual void wheelEvent(QWheelEvent * e);

guilib/include/rtabmap/gui/MainWindow.h

@@ -109,11 +109,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();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -97,12 +97,12 @@ public:
 	virtual QString getIniFilePath() const;
 	void init();
 
-	void saveWindowGeometry(const QString & windowName, const QWidget * window);
-	void loadWindowGeometry(const QString & windowName, QWidget * window);
+	void saveWindowGeometry(const QWidget * window);
+	void loadWindowGeometry(QWidget * window);
 	void saveMainWindowState(const QMainWindow * mainWindow);
 	void loadMainWindowState(QMainWindow * mainWindow);
-	void saveWidgetState(const QString & name, const QWidget * widget);
-	void loadWidgetState(const QString & name, QWidget * widget);
+	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);

guilib/src/CalibrationDialog.cpp

@@ -245,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()

guilib/src/CloudViewer.cpp

@@ -97,6 +97,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 
 	//setup menu/actions
 	createMenu();
+
+	setMouseTracking(false);
 }
 
 CloudViewer::~CloudViewer()
@@ -171,7 +173,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;
@@ -254,7 +256,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;
@@ -334,7 +336,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;
 		}
@@ -352,7 +354,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;
 		}
@@ -580,7 +582,7 @@ 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);
@@ -1044,6 +1046,8 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 
 		render();
+
+		emit configChanged();
 	}
 	else
 	{
@@ -1082,6 +1086,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 
 	}
+	emit configChanged();
 }
 
 void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
@@ -1090,6 +1095,7 @@ void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
 	if(a)
 	{
 		handleAction(a);
+		emit configChanged();
 	}
 }
 

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,7 +241,7 @@ 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::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());
@@ -373,7 +375,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());
 			}
@@ -569,7 +571,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::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
 		}
 	}
 }
@@ -801,7 +803,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::radiusPosesClustering(
 				optimizedPoses,
 				ui_->doubleSpinBox_detectMore_radius->value(),
 				ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
@@ -1216,7 +1218,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
@@ -1413,7 +1415,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::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
 	if(iter != linksRefined_.end())
 	{
@@ -1696,7 +1698,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::findLink(linksRefined_, currentLink.from(), currentLink.to());
 		if(iter != linksRefined_.end())
 		{
 			currentLink = iter->second;
@@ -1726,7 +1728,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 +1788,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);
-		util3d::optimizeTOROGraph(
+		std::map<int, int> depthGraph = rtabmap::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
+		rtabmap::optimizeTOROGraph(
 				depthGraph,
 				poses_,
 				links, finalPoses,
@@ -1813,21 +1815,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::findLink(linksRefined_, from ,to);
 	if(findIter != linksRefined_.end())
 	{
 		link = findIter->second;
 	}
 	else
 	{
-		findIter = util3d::findLink(linksAdded_, from ,to);
+		findIter = rtabmap::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::findLink(links_, from ,to);
 			if(findIter != links_.end())
 			{
 				link = findIter->second;
@@ -1839,7 +1841,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::findLink(links, from, to) != links.end();
 }
 
 void DatabaseViewer::refineConstraint()
@@ -1897,8 +1899,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 newLaserScan = util3d::uncompressData(dataTo.getLaserScanCompressed());
+		cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed());
+		cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed());
 
 		if(!oldLaserScan.empty() && !newLaserScan.empty())
 		{
@@ -1928,13 +1930,13 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 	else
 	{
 		//3D
-		cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepthCompressed());
-		cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepthCompressed());
+		cv::Mat depthA = rtabmap::uncompressImage(dataFrom.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 +1969,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 +2268,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 +2279,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::findLink(linksRemoved_, from, to));
 		updateSlider = true;
 	}
 
@@ -2310,19 +2312,19 @@ void DatabaseViewer::resetConstraint()
 	}
 
 
-	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, from, to);
+	std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 		this->updateGraphView();
 	}
 
-	iter = util3d::findLink(links_, from, to);
+	iter = rtabmap::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		this->updateConstraintView(iter->second);
 	}
-	iter = util3d::findLink(linksAdded_, from, to);
+	iter = rtabmap::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		this->updateConstraintView(iter->second);
@@ -2350,7 +2352,7 @@ void DatabaseViewer::rejectConstraint()
 
 	// find the original one
 	std::multimap<int, Link>::iterator iter;
-	iter = util3d::findLink(links_, from, to);
+	iter = rtabmap::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		if(iter->second.type() == Link::kNeighbor)
@@ -2363,13 +2365,13 @@ void DatabaseViewer::rejectConstraint()
 	}
 
 	// remove from refined and added
-	iter = util3d::findLink(linksRefined_, from, to);
+	iter = rtabmap::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 		removed = true;
 	}
-	iter = util3d::findLink(linksAdded_, from, to);
+	iter = rtabmap::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		linksAdded_.erase(iter);
@@ -2392,7 +2394,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::findLink(linksRemoved_, iter->second.from(), iter->second.to());
 		if(findIter != linksRemoved_.end())
 		{
 			if(!(iter->second.from() == findIter->second.from() &&
@@ -2410,7 +2412,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			}
 		}
 
-		findIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
+		findIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
 		if(findIter!=linksRefined_.end())
 		{
 			if(iter->second.from() == findIter->second.from() &&

guilib/src/ExportCloudsDialog.cpp

@@ -39,6 +39,19 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	_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()

guilib/src/ExportCloudsDialog.h

@@ -76,6 +76,9 @@ public:
 	void setMeshNormalKSearch(int k);
 	void setMeshGp3Radius(double radius);
 
+signals:
+	void configChanged();
+
 public slots:
 	void restoreDefaults();
 

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/ImageView.cpp

@@ -162,23 +162,28 @@ 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();
 	}
 
 	if(action == _showImage || action ==_showImageDepth)
 	{
 		this->updateOpacity();
+		emit configChanged();
 	}
 }
 

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>
@@ -128,7 +129,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_rawLikelihoodCurve(0),
 	_autoScreenCaptureOdomSync(false)
 {
-	ULOGGER_DEBUG("");
+	UDEBUG("");
 
 	initGuiResource();
 
@@ -140,6 +141,7 @@ 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);
@@ -148,9 +150,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_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()))
@@ -179,10 +182,15 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	{
 		_preferencesDialog = new PreferencesDialog(this);
 	}
+	_preferencesDialog->setObjectName("PreferencesDialog");
 	_preferencesDialog->init();
 
 	// Restore window geometry
 	_preferencesDialog->loadMainWindowState(this);
+	_preferencesDialog->loadWindowGeometry(_preferencesDialog);
+	_preferencesDialog->loadWindowGeometry(_exportDialog);
+	_preferencesDialog->loadWindowGeometry(_postProcessingDialog);
+	_preferencesDialog->loadWindowGeometry(_aboutDialog);
 	setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
 
 	// Timer
@@ -198,9 +206,9 @@ 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));
-	_preferencesDialog->loadWidgetState(_ui->imageView_source->objectName(), _ui->imageView_source);
-	_preferencesDialog->loadWidgetState(_ui->imageView_loopClosure->objectName(), _ui->imageView_loopClosure);
-	_preferencesDialog->loadWidgetState(_ui->imageView_odometry->objectName(), _ui->imageView_odometry);
+	_preferencesDialog->loadWidgetState(_ui->imageView_source);
+	_preferencesDialog->loadWidgetState(_ui->imageView_loopClosure);
+	_preferencesDialog->loadWidgetState(_ui->imageView_odometry);
 
 	_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
 	_ui->posteriorPlot->addCurve(_posteriorCurve, false);
@@ -255,6 +263,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()));
@@ -295,6 +304,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);
@@ -349,6 +359,21 @@ 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->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()));
@@ -375,11 +400,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->objectName(), _ui->widget_cloudViewer);
+	_preferencesDialog->loadWidgetState(_ui->widget_cloudViewer);
 
 	//dialog states
-	_preferencesDialog->loadWidgetState(_exportDialog->objectName(), _exportDialog);
-	_preferencesDialog->loadWidgetState(_postProcessingDialog->objectName(), _postProcessingDialog);
+	_preferencesDialog->loadWidgetState(_exportDialog);
+	_preferencesDialog->loadWidgetState(_postProcessingDialog);
 
 	if(_ui->statsToolBox->findChildren<StatItem*>().size() == 0)
 	{
@@ -404,6 +429,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	splash.close();
 
 	this->setFocus();
+
+	UDEBUG("");
 }
 
 MainWindow::~MainWindow()
@@ -454,16 +481,32 @@ void MainWindow::closeEvent(QCloseEvent* event)
 
 	if(processStopped)
 	{
+		//write settings before quit?
+		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();
 
-		//write settings before quit?
-		_preferencesDialog->saveMainWindowState(this);
-		_preferencesDialog->saveWidgetState(_ui->widget_cloudViewer->objectName(), _ui->widget_cloudViewer);
-		_preferencesDialog->saveWidgetState(_ui->imageView_source->objectName(), _ui->imageView_source);
-		_preferencesDialog->saveWidgetState(_ui->imageView_loopClosure->objectName(), _ui->imageView_loopClosure);
-		_preferencesDialog->saveWidgetState(_ui->imageView_odometry->objectName(), _ui->imageView_odometry);
-		_preferencesDialog->saveWidgetState(_exportDialog->objectName(), _exportDialog);
-		_preferencesDialog->saveWidgetState(_postProcessingDialog->objectName(), _postProcessingDialog);
 		_ui->dockWidget_imageView->close();
 		_ui->dockWidget_likelihood->close();
 		_ui->dockWidget_rawlikelihood->close();
@@ -1131,7 +1174,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::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)
@@ -2027,6 +2070,25 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 	}
 }
 
+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);
@@ -2035,6 +2097,10 @@ 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(bool used, PreferencesDialog::Src src)
@@ -2108,7 +2174,26 @@ void MainWindow::beep()
 	QApplication::beep();
 }
 
+void MainWindow::configGUIModified()
+{
+	this->setWindowModified(true);
+}
+
 //ACTIONS
+void MainWindow::saveConfigGUI()
+{
+	_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);
+	this->setWindowModified(false);
+}
+
 void MainWindow::newDatabase()
 {
 	if(_state != MainWindow::kIdle)
@@ -2871,7 +2956,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::radiusPosesClustering(
 					_currentPosesMap,
 					clusterRadius,
 					clusterAngle*CV_PI/180.0);
@@ -2893,7 +2978,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::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
 				{
 					if(!_cachedSignatures.contains(from))
 					{
@@ -2976,8 +3061,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);
-				util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
+				std::map<int, int> depthGraph = rtabmap::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+				rtabmap::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 				_currentPosesMap = optimizedPoses;
 				_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
 			}
@@ -3131,8 +3216,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);
-	util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
+	std::map<int, int> depthGraph = rtabmap::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+	rtabmap::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
 	_initProgressDialog->incrementStep();
 

guilib/src/PostProcessingDialog.cpp

@@ -42,6 +42,14 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 	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()

guilib/src/PostProcessingDialog.h

@@ -62,6 +62,9 @@ public:
 	void setRefineNeighborLinks(bool on);
 	void setRefineLoopClosureLinks(bool on);
 
+signals:
+	void configChanged();
+
 public slots:
 	void restoreDefaults();
 

guilib/src/PreferencesDialog.cpp

@@ -543,7 +543,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 }
 
 PreferencesDialog::~PreferencesDialog() {
-	this->saveWindowGeometry("PreferencesDialog", this);
 	delete _ui;
 }
 
@@ -559,8 +558,6 @@ void PreferencesDialog::init()
 	this->readSettings();
 	this->writeSettings();// This will create the ini file if not exist
 
-	this->loadWindowGeometry("PreferencesDialog", this);
-
 	_initialized = true;
 }
 
@@ -1638,229 +1635,250 @@ 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);
-	settings.beginGroup("Gui");
-	settings.beginGroup(windowName);
-	settings.setValue("geometry", window->saveGeometry());
-	settings.endGroup(); // "windowName"
-	settings.endGroup(); // rtabmap
+	if(!window->objectName().isNull())
+	{
+		if(!window->isMaximized())
+		{
+			QSettings settings(getIniFilePath(), QSettings::IniFormat);
+			settings.beginGroup("Gui");
+			settings.beginGroup(window->objectName());
+			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;
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
-	settings.beginGroup("Gui");
-	settings.beginGroup(windowName);
-	bytes = settings.value("geometry", QByteArray()).toByteArray();
-	if(!bytes.isEmpty())
+	if(!window->objectName().isNull())
 	{
-		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);
-	settings.beginGroup("Gui");
-	settings.beginGroup("MainWindow");
-	settings.setValue("state", mainWindow->saveState());
-	settings.endGroup(); // "MainWindow"
-	settings.endGroup(); // rtabmap
+	if(!mainWindow->objectName().isNull())
+	{
+		saveWindowGeometry(mainWindow);
 
-	saveWindowGeometry("MainWindow", mainWindow);
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(mainWindow->objectName());
+		settings.setValue("state", mainWindow->saveState());
+		settings.endGroup(); // "MainWindow"
+		settings.endGroup(); // rtabmap
+	}
 }
 
 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())
+	if(!mainWindow->objectName().isNull())
 	{
-		mainWindow->restoreState(bytes);
-	}
-	settings.endGroup(); // "MainWindow"
-	settings.endGroup(); // rtabmap
+		loadWindowGeometry(mainWindow);
 
-	loadWindowGeometry("MainWindow", 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);
+		}
+		settings.endGroup(); // "MainWindow"
+		settings.endGroup(); // rtabmap
+	}
 }
 
-void PreferencesDialog::saveWidgetState(const QString & name, const QWidget * widget)
+void PreferencesDialog::saveWidgetState(const QWidget * widget)
 {
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
-	settings.beginGroup("Gui");
-	settings.beginGroup(name);
-
-	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);
-
-	if(cloudViewer)
+	if(!widget->objectName().isNull())
 	{
-		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())
+		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);
+
+		if(cloudViewer)
 		{
-			// make camera position relative to target
-			Transform T = cloudViewer->getTargetPose();
-			if(cloudViewer->isCameraTargetLocked())
+			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())
 			{
-				T = Transform(T.x(), T.y(), T.z(), 0,0,0);
+				// 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());
 			}
-			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());
+		}
+		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
+		{
+			UERROR("Widget \"%s\" cannot be exported in config file.", widget->objectName().toStdString().c_str());
 		}
-		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());
+		settings.endGroup(); // "name"
+		settings.endGroup(); // Gui
 	}
-	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());
-	}
-	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
-	{
-		UERROR("Widget \"%s\" cannot be exported in config file.", widget->objectName().toStdString().c_str());
-	}
-
-	settings.endGroup(); // "name"
-	settings.endGroup(); // Gui
 }
 
-void PreferencesDialog::loadWidgetState(const QString & name, QWidget * widget)
+void PreferencesDialog::loadWidgetState(QWidget * widget)
 {
-	QByteArray bytes;
-	QSettings settings(getIniFilePath(), QSettings::IniFormat);
-	settings.beginGroup("Gui");
-	settings.beginGroup(name);
-
-	CloudViewer * cloudViewer = qobject_cast<CloudViewer*>(widget);
-	ImageView * imageView = qobject_cast<ImageView*>(widget);
-	ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
-	PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
-
-	if(cloudViewer)
+	if(!widget->objectName().isNull())
 	{
-		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());
+		QByteArray bytes;
+		QSettings settings(getIniFilePath(), QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(widget->objectName());
 
-		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 * cloudViewer = qobject_cast<CloudViewer*>(widget);
+		ImageView * imageView = qobject_cast<ImageView*>(widget);
+		ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
+		PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
 
-		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())
+		if(cloudViewer)
 		{
-			cloudViewer->setCameraFree();
+			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());
+		}
+		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
+		{
+			UERROR("Widget \"%s\" cannot be loaded from config file.", widget->objectName().toStdString().c_str());
 		}
-		cloudViewer->setCameraLockZ(settings.value("camera_lockZ", cloudViewer->isCameraLockZ()).toBool());
 
-		cloudViewer->setBackgroundColor(settings.value("bg_color", cloudViewer->getBackgroundColor()).value<QColor>());
+		settings.endGroup(); //"name"
+		settings.endGroup(); // Gui
 	}
-	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());
-	}
-	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
-	{
-		UERROR("Widget \"%s\" cannot be loaded from config file.", widget->objectName().toStdString().c_str());
-	}
-
-	settings.endGroup(); //"name"
-	settings.endGroup(); // Gui
 }
 
 

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">
-         <property name="text">
-          <string>Show rectified</string>
-         </property>
-        </widget>
+        <layout class="QHBoxLayout" name="horizontalLayout_2">
+         <item>
+          <widget class="QCheckBox" name="checkBox_rectified">
+           <property name="text">
+            <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>

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>

tools/Calibration/main.cpp

@@ -29,14 +29,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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"
-			"  driver       Driver number to use: 0=USB camera, 1=OpenNI-PCL, 2=OpenNI2, 3=Freenect, 4=OpenNI-CV, 5=OpenNI-CV-ASUS\n\n");
+			"rtabmap-calibration [options]\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);
 }
 
@@ -46,31 +50,70 @@ 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();
-	}
-	else
-	{
-		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 5)
+		if(strcmp(argv[i], "--driver") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				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 5.");
 			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)
 	{
-		cameraUsb = new rtabmap::CameraVideo(0, imageRate);
+		cameraUsb = new rtabmap::CameraVideo(device, imageRate);
 	}
 	else if(driver == 1)
 	{
-		camera = new rtabmap::CameraOpenni("", imageRate);
+		camera = new rtabmap::CameraOpenni(uNumber2Str(device), imageRate);
 	}
 	else if(driver == 2)
 	{
@@ -88,7 +131,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect(0, imageRate);
+		camera = new rtabmap::CameraFreenect(device, imageRate);
 	}
 	else if(driver == 4)
 	{