Updated how RtabmapCmdEvent should be called (using UVariant). MainWindow: added new actions to export poses (KITTI, RGBD-SLAM and TORO formats)

corelib/include/rtabmap/core/Rtabmap.h

@@ -108,15 +108,15 @@ public:
 	bool labelLocation(int id, const std::string & label);
 	bool setUserData(int id, const cv::Mat & data);
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
-	void generateTOROGraph(const std::string & path, bool optimized, bool global);
-	void exportPoses(const std::string & path, bool optimized, bool global);
+	void exportPoses(
+			const std::string & path,
+			bool optimized,
+			bool global,
+			int type // 0=raw/KITTI format, 1=rgbd-slam format, 2=TORO
+	);
 	void resetMemory();
 	void dumpPrediction() const;
 	void dumpData() const;
-	void dumpPoses(
-			const std::string & path,
-			const std::map<int, Transform> & poses,
-			const std::map<int, double> & stamps = std::map<int, double>()) const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
 	void rejectLoopClosure(int oldId, int newId);

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UVariant.h>
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Parameters.h"
 
@@ -58,53 +59,72 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
-			kCmdInit,
+			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
 			kCmdClose,
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
-			kCmdGenerateDOTGraph, // params: path
-			kCmdGenerateDOTLocalGraph, // params: path, id, margin
-			kCmdGenerateTOROGraphLocal, // params: path, optimized
-			kCmdGenerateTOROGraphGlobal, // params: path, optimized
-			kCmdExportPosesGlobal,
-			kCmdExportPosesLocal,
+			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
+			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO)
 			kCmdCleanDataBuffer,
-			kCmdPublish3DMapLocal, // params: optimized
-			kCmdPublish3DMapGlobal, // params: optimized
-			kCmdPublishTOROGraphGlobal, // params: optimized
-			kCmdPublishTOROGraphLocal, // params: optimized
+			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
 			kCmdTriggerNewMap,
 			kCmdPause,
-			kCmdGoal, // params: label or location ID
+			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel}; // // params: label or location ID
+			kCmdLabel             // params: [string] label or [int] location ID
+	};
 public:
-	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
+	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),
-			_cmd(cmd),
-			_strValue(strValue),
-			_intValue(intValue),
-			_parameters(parameters){}
+			cmd_(cmd),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const UVariant & value4, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			value4_(value4),
+			parameters_(parameters){}
 
 	virtual ~RtabmapEventCmd() {}
-	Cmd getCmd() const {return _cmd;}
+	Cmd getCmd() const {return cmd_;}
 
-	void setStr(const std::string & str) {_strValue = str;}
-	const std::string & getStr() const {return _strValue;}
+	const UVariant & value1() const {return value1_;}
+	const UVariant & value2() const {return value2_;}
+	const UVariant & value3() const {return value3_;}
+	const UVariant & value4() const {return value4_;}
 
-	void setInt(int v) {_intValue = v;}
-	int getInt() const {return _intValue;}
-
-	const ParametersMap & getParameters() const {return _parameters;}
+	const ParametersMap & getParameters() const {return parameters_;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
 
 private:
-	Cmd _cmd;
-	std::string _strValue;
-	int _intValue;
-	ParametersMap _parameters;
+	Cmd cmd_;
+	UVariant value1_;
+	UVariant value2_;
+	UVariant value3_;
+	UVariant value4_;
+	ParametersMap parameters_;
 };
 
 class RtabmapEventInit : public UEvent

corelib/include/rtabmap/core/RtabmapThread.h

@@ -61,17 +61,10 @@ public:
 		kStateChangingParameters,
 		kStateDumpingMemory,
 		kStateDumpingPrediction,
-		kStateGeneratingDOTGraph,
-		kStateGeneratingDOTLocalGraph,
-		kStateGeneratingTOROGraphLocal,
-		kStateGeneratingTOROGraphGlobal,
-		kStateExportingPosesLocal,
-		kStateExportingPosesGlobal,
+		kStateExportingDOTGraph,
+		kStateExportingPoses,
 		kStateCleanDataBuffer,
-		kStatePublishingMapLocal,
-		kStatePublishingMapGlobal,
-		kStatePublishingTOROGraphLocal,
-		kStatePublishingTOROGraphGlobal,
+		kStatePublishingMap,
 		kStateTriggeringMap,
 		kStateAddingUserData,
 		kStateSettingGoal,
@@ -99,8 +92,7 @@ private:
 	void addData(const OdometryEvent & odomEvent);
 	bool getData(OdometryEvent & data);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
-	void publishMap(bool optimized, bool full) const;
-	void publishGraph(bool optimized, bool full) const;
+	void publishMap(bool optimized, bool full, bool graphOnly) const;
 
 private:
 	UMutex _stateMutex;

corelib/src/Rtabmap.cpp

@@ -718,7 +718,7 @@ void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
 	}
 }
 
-void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool global)
+void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int type)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -735,40 +735,70 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
-		graph::TOROOptimizer::saveGraph(path, poses, constraints);
-	}
-}
-
-void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global)
-{
-	if(_memory && _memory->getLastWorkingSignature())
-	{
-		std::map<int, Transform> poses;
-		std::multimap<int, Link> constraints;
-
-		if(optimized)
+		if(type==2) // TORO
 		{
-			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+			graph::TOROOptimizer::saveGraph(path, poses, constraints);
 		}
 		else
 		{
-			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
-		}
+			//get timestamps
+			std::map<int, double> stamps;
+			if(type == 1)
+			{
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					Transform o;
+					int m, w;
+					std::string l;
+					double stamp = 0.0;
+					_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
+					stamps.insert(std::make_pair(iter->first, stamp));
+				}
+				UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
+			}
 
-		//get timestamps
-		std::map<int, double> stamps;
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-		{
-			Transform o;
-			int m, w;
-			std::string l;
-			double stamp = 0.0;
-			_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
-			stamps.insert(std::make_pair(iter->first, stamp));
-		}
+			FILE* fout = 0;
+#ifdef _MSC_VER
+			fopen_s(&fout, path.c_str(), "w");
+#else
+			fout = fopen(path.c_str(), "w");
+#endif
+			if(fout)
+			{
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					if(type == 1) // rgbd-slam format
+					{
+						// Format: stamp x y z qw qx qy qz
+						Eigen::Quaternionf q = (*iter).second.getQuaternionf();
 
-		this->dumpPoses(path, poses, stamps);
+						UASSERT(uContains(stamps, iter->first));
+						fprintf(fout, "%f %f %f %f %f %f %f %f\n",
+								stamps.at(iter->first),
+								(*iter).second.x(),
+								(*iter).second.y(),
+								(*iter).second.z(),
+								q.w(),
+								q.x(),
+								q.y(),
+								q.z());
+					}
+					else // default / KITTI format
+					{
+						// Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+						const float * p = (const float *)(*iter).second.data();
+
+						fprintf(fout, "%f", p[0]);
+						for(int i=1; i<(*iter).second.size(); i++)
+						{
+							fprintf(fout, " %f", p[i]);
+						}
+						fprintf(fout, "\n");
+					}
+				}
+				fclose(fout);
+			}
+		}
 	}
 }
 
@@ -2476,46 +2506,6 @@ void Rtabmap::dumpData() const
 	}
 }
 
-void Rtabmap::dumpPoses(
-		const std::string & path,
-		const std::map<int, Transform> & poses,
-		const std::map<int, double> & stamps) const
-{
-	UDEBUG("");
-	UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
-	FILE* fout = 0;
-#ifdef _MSC_VER
-	fopen_s(&fout, path.c_str(), "w");
-#else
-	fout = fopen(path.c_str(), "w");
-#endif
-	if(fout)
-	{
-		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-		{
-			// in camera frame
-			const float * p = (const float *)(*iter).second.data();
-
-			int index = 0;
-			if(stamps.size() == poses.size())
-			{
-				UASSERT(uContains(stamps, iter->first));
-				fprintf(fout, "%f", stamps.at(iter->first));
-			}
-			else
-			{
-				fprintf(fout, "%f", p[index++]);
-			}
-			for(int i=index; i<(*iter).second.size(); i++)
-			{
-				fprintf(fout, " %f", p[i]);
-			}
-			fprintf(fout, "\n");
-		}
-		fclose(fout);
-	}
-}
-
 // fromId must be in _memory and in _optimizedPoses
 // Get poses in front of the robot, return optimized poses
 std::map<int, Transform> Rtabmap::getForwardWMPoses(

corelib/src/RtabmapThread.cpp

@@ -117,7 +117,7 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
 	enabled = _createIntermediateNodes;
 }
 
-void RtabmapThread::publishMap(bool optimized, bool full) const
+void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 {
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
@@ -127,11 +127,23 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, std::string> labels;
 	std::map<int, std::vector<unsigned char> > userDatas;
 
-	_rtabmap->get3DMap(signatures,
-			poses,
-			constraints,
-			optimized,
-			full);
+	if(graphOnly)
+	{
+		_rtabmap->getGraph(poses,
+				constraints,
+				optimized,
+				full,
+				&signatures);
+	}
+	else
+	{
+		_rtabmap->get3DMap(
+				signatures,
+				poses,
+				constraints,
+				optimized,
+				full);
+	}
 
 	this->post(new RtabmapEvent3DMap(
 			signatures,
@@ -139,29 +151,6 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 			constraints));
 }
 
-void RtabmapThread::publishGraph(bool optimized, bool full) const
-{
-	std::map<int, Signature> signatures;
-	std::map<int, Transform> poses;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-
-	_rtabmap->getGraph(poses,
-			constraints,
-			optimized,
-			full,
-			&signatures);
-
-	this->post(new RtabmapEvent3DMap(
-			signatures,
-			poses,
-			constraints));
-}
-
-
 void RtabmapThread::mainLoopKill()
 {
 	this->clearBufferedData();
@@ -229,38 +218,27 @@ void RtabmapThread::mainLoop()
 	case kStateDumpingPrediction:
 		_rtabmap->dumpPrediction();
 		break;
-	case kStateGeneratingDOTGraph:
-		_rtabmap->generateDOTGraph(parameters.at("path"));
+	case kStateExportingDOTGraph:
+		_rtabmap->generateDOTGraph(
+				parameters.at("path"),
+				atoi(parameters.at("id").c_str()),
+				atoi(parameters.at("margin").c_str()));
 		break;
-	case kStateGeneratingDOTLocalGraph:
-		_rtabmap->generateDOTGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
-		break;
-	case kStateGeneratingTOROGraphLocal:
-		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStateGeneratingTOROGraphGlobal:
-		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
-		break;
-	case kStateExportingPosesLocal:
-		_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStateExportingPosesGlobal:
-		_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
+	case kStateExportingPoses:
+		_rtabmap->exportPoses(
+				parameters.at("path"),
+				uStr2Bool(parameters.at("optimized")),
+				uStr2Bool(parameters.at("global")),
+				atoi(parameters.at("type").c_str()));
 		break;
 	case kStateCleanDataBuffer:
 		this->clearBufferedData();
 		break;
-	case kStatePublishingMapLocal:
-		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStatePublishingMapGlobal:
-		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
-		break;
-	case kStatePublishingTOROGraphLocal:
-		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStatePublishingTOROGraphGlobal:
-		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
+	case kStatePublishingMap:
+		this->publishMap(
+				uStr2Bool(parameters.at("optimized")),
+				uStr2Bool(parameters.at("global")),
+				uStr2Bool(parameters.at("graph_only")));
 		break;
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
@@ -275,10 +253,10 @@ void RtabmapThread::mainLoop()
 		_rtabmap->setUserData(0, userData);
 		break;
 	case kStateSettingGoal:
-		id = atoi(parameters.at("goal_id").c_str());
-		if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory())
+		id = atoi(parameters.at("id").c_str());
+		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
 		{
-			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
 		}
 		if(id <= 0 || !_rtabmap->computePath(id, true))
 		{
@@ -360,8 +338,8 @@ void RtabmapThread::handleEvent(UEvent* event)
 		{
 			ULOGGER_DEBUG("CMD_INIT");
 			ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
-			UASSERT(!rtabmapEvent->getStr().empty());
-			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
+			UASSERT(rtabmapEvent->value1().isStr());
+			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
 			pushNewState(kStateInit, parameters);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdClose)
@@ -386,68 +364,30 @@ void RtabmapThread::handleEvent(UEvent* event)
 		}
 		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
 		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
 			ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isStr());
+			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
+			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			pushNewState(kStateGeneratingDOTGraph, param);
+			param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
+			param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
+			pushNewState(kStateExportingDOTGraph, param);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph)
+		else if(cmd == RtabmapEventCmd::kCmdExportPoses)
 		{
-			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
-			UASSERT(values.size() == 3);
-
-			ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH");
+			ULOGGER_DEBUG("CMD_EXPORT_POSES");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isBool());
+			UASSERT(rtabmapEvent->value3().isStr());
+			UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("path", *values.begin()));
-			param.insert(ParametersPair("id", *(++values.begin())));
-			param.insert(ParametersPair("margin", *values.rbegin()));
-			pushNewState(kStateGeneratingDOTLocalGraph, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateGeneratingTOROGraphLocal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateGeneratingTOROGraphGlobal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdExportPosesLocal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_EXPORT_POSES_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateExportingPosesLocal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdExportPosesGlobal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_EXPORT_POSES_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateExportingPosesGlobal, param);
+			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
+			param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
+			pushNewState(kStateExportingPoses, param);
 
 		}
 		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
@@ -455,33 +395,17 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
 			pushNewState(kStateCleanDataBuffer);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal)
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
 		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL");
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isBool());
+			UASSERT(rtabmapEvent->value3().isBool());
 			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingMapLocal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingMapGlobal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingTOROGraphLocal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingTOROGraphGlobal, param);
+			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
+			param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
+			pushNewState(kStatePublishingMap, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
 		{
@@ -496,9 +420,10 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdGoal)
 		{
 			ULOGGER_DEBUG("CMD_GOAL");
+			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
+			param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
 			pushNewState(kStateSettingGoal, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
@@ -509,9 +434,10 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdLabel)
 		{
 			ULOGGER_DEBUG("CMD_LABEL");
+			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("label", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("id", uNumber2Str(rtabmapEvent->getInt())));
+			param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
 			pushNewState(kStateLabelling, param);
 		}
 		else

guilib/include/rtabmap/gui/MainWindow.h

@@ -126,10 +126,10 @@ private slots:
 	void stopDetection();
 	void notifyNoMoreImages();
 	void printLoopClosureIds();
-	void generateMap();
-	void generateLocalMap();
-	void generateTOROMap();
-	void exportPoses();
+	void generateGraphDOT();
+	void exportPosesKITTI();
+	void exportPosesRGBDSLAM();
+	void exportPosesTORO();
 	void postProcessing();
 	void deleteMemory();
 	void openWorkingDirectory();
@@ -223,6 +223,7 @@ private:
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
 	void saveFigures();
 	void loadFigures();
+	void exportPoses(int format);
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
 			const std::map<int, Transform> & poses,
@@ -296,8 +297,7 @@ private:
 	DetailedProgressDialog * _initProgressDialog;
 
 	QString _graphSavingFileName;
-	QString _toroSavingFileName;
-	QString _posesSavingFileName;
+	QMap<int, QString> _exportPosesFileName;
 	bool _autoScreenCaptureOdomSync;
 
 	QVector<int> _refIds;

guilib/src/MainWindow.cpp

@@ -299,10 +299,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
 	connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
 	connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
-	connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateMap()));
-	connect(_ui->actionGenerate_local_map, SIGNAL(triggered()), this, SLOT(generateLocalMap()));
-	connect(_ui->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this , SLOT(generateTOROMap()));
-	connect(_ui->actionExport_poses_txt, SIGNAL(triggered()), this , SLOT(exportPoses()));
+	connect(_ui->actionGenerate_map, SIGNAL(triggered()), this , SLOT(generateGraphDOT()));
+	connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
+	connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
+	connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
 	connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory()));
 	connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
 	connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
@@ -2643,7 +2643,7 @@ void MainWindow::newDatabase()
 		}
 	}
 	_newDatabasePath = databasePath.c_str();
-	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, 0, _preferencesDialog->getAllParameters()));
+	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, _preferencesDialog->getAllParameters()));
 	applyPrefSettings(_preferencesDialog->getAllParameters(), false);
 }
 
@@ -3115,57 +3115,30 @@ void MainWindow::printLoopClosureIds()
 	_ui->widget_console->appendMsg(QString("LoopIDs = [%1];").arg(msgLoop));
 }
 
-void MainWindow::generateMap()
-{
-	if(_graphSavingFileName.isEmpty())
-	{
-		_graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
-	}
-	QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
-	if(!path.isEmpty())
-	{
-		_graphSavingFileName = path;
-		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, path.toStdString())); // The event is automatically deleted by the EventsManager...
-
-		_ui->dockWidget_console->show();
-		_ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName));
-	}
-}
-
-void MainWindow::generateLocalMap()
+void MainWindow::generateGraphDOT()
 {
 	if(_graphSavingFileName.isEmpty())
 	{
 		_graphSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "Graph.dot";
 	}
 
-	bool ok = false;
-	int loopId = 1;
-	if(_ui->label_matchId->text().size())
-	{
-		std::list<std::string> values = uSplitNumChar(_ui->label_matchId->text().toStdString());
-		if(values.size() > 1)
-		{
-			int val = QString((++values.begin())->c_str()).toInt(&ok);
-			if(ok)
-			{
-				loopId = val;
-			}
-			ok = false;
-		}
-	}
-	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), loopId, 1, 999999, 1, &ok);
+	bool ok;
+	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
 	if(ok)
 	{
-		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+		int margin = 0;
+		if(id > 0)
+		{
+			margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+		}
+
 		if(ok)
 		{
 			QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _graphSavingFileName, tr("Graphiz file (*.dot)"));
 			if(!path.isEmpty())
 			{
 				_graphSavingFileName = path;
-				QString str = path + QString(";") + QString::number(id) + QString(";") + QString::number(margin);
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTLocalGraph, str.toStdString())); // The event is automatically deleted by the EventsManager...
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateDOTGraph, false, path.toStdString(), id, margin));
 
 				_ui->dockWidget_console->show();
 				_ui->widget_console->appendMsg(QString("Graph saved... Tip:\nneato -Tpdf \"%1\" -o out.pdf").arg(_graphSavingFileName).arg(_graphSavingFileName));
@@ -3174,73 +3147,21 @@ void MainWindow::generateLocalMap()
 	}
 }
 
-void MainWindow::generateTOROMap()
+void MainWindow::exportPosesKITTI()
 {
-	if(_toroSavingFileName.isEmpty())
-	{
-		_toroSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "toro.graph";
-	}
-
-	QStringList items;
-	items.append("Local map optimized");
-	items.append("Local map not optimized");
-	items.append("Global map optimized");
-	items.append("Global map not optimized");
-	bool ok;
-	QString item = QInputDialog::getItem(this, tr("Parameters"), tr("Options:"), items, 2, false, &ok);
-	if(ok)
-	{
-		bool optimized=false, global=false;
-		if(item.compare("Local map optimized") == 0)
-		{
-			optimized = true;
-		}
-		else if(item.compare("Local map not optimized") == 0)
-		{
-
-		}
-		else if(item.compare("Global map optimized") == 0)
-		{
-			global=true;
-			optimized=true;
-		}
-		else if(item.compare("Global map not optimized") == 0)
-		{
-			global=true;
-		}
-		else
-		{
-			UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
-		}
-
-		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _toroSavingFileName, tr("TORO file (*.graph)"));
-		if(!path.isEmpty())
-		{
-			_toroSavingFileName = path;
-			if(global)
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphGlobal, path.toStdString(), optimized?1:0));
-			}
-			else
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGenerateTOROGraphLocal, path.toStdString(), optimized?1:0));
-			}
-
-			_ui->dockWidget_console->show();
-			_ui->widget_console->appendMsg(QString("TORO Graph saved (global=%1, optimized=%2)... %3")
-					.arg(global?"true":"false").arg(optimized?"true":"false").arg(_toroSavingFileName));
-		}
-
-	}
+	exportPoses(0);
+}
+void MainWindow::exportPosesRGBDSLAM()
+{
+	exportPoses(1);
+}
+void MainWindow::exportPosesTORO()
+{
+	exportPoses(2);
 }
 
-void MainWindow::exportPoses()
+void MainWindow::exportPoses(int format)
 {
-	if(_posesSavingFileName.isEmpty())
-	{
-		_posesSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "poses.txt";
-	}
-
 	QStringList items;
 	items.append("Local map optimized");
 	items.append("Local map not optimized");
@@ -3273,22 +3194,30 @@ void MainWindow::exportPoses()
 			UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
 		}
 
-		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _posesSavingFileName, tr("Text file (*.txt)"));
+		if(_exportPosesFileName[format].isEmpty())
+		{
+			_exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==2?"toro.graph":"poses.txt");
+		}
+
+		QString path = QFileDialog::getSaveFileName(
+				this,
+				tr("Save File"),
+				_exportPosesFileName[format],
+				format == 2?tr("TORO file (*.graph)"):tr("Text file (*.txt)"));
+
 		if(!path.isEmpty())
 		{
-			_posesSavingFileName = path;
-			if(global)
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesGlobal, path.toStdString(), optimized?1:0));
-			}
-			else
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPosesLocal, path.toStdString(), optimized?1:0));
-			}
+			_exportPosesFileName[format] = path;
+
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPoses, global, optimized, path.toStdString(), format));
 
 			_ui->dockWidget_console->show();
-			_ui->widget_console->appendMsg(QString("Poses saved (global=%1, optimized=%2)... %3")
-					.arg(global?"true":"false").arg(optimized?"true":"false").arg(_posesSavingFileName));
+			_ui->widget_console->appendMsg(
+					QString("%1 saved (global=%2, optimized=%3)... %4")
+					.arg(format == 2?"TORO graph":"Poses")
+					.arg(global?"true":"false")
+					.arg(optimized?"true":"false")
+					.arg(_exportPosesFileName[format]));
 		}
 
 	}
@@ -3957,14 +3886,7 @@ void MainWindow::downloadAllClouds()
 		_initProgressDialog->show();
 		_initProgressDialog->appendText(tr("Downloading the map (global=%1 ,optimized=%2)...")
 				.arg(global?"true":"false").arg(optimized?"true":"false"));
-		if(global)
-		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapGlobal, "", optimized?1:0));
-		}
-		else
-		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMapLocal, "", optimized?1:0));
-		}
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, false));
 	}
 }
 
@@ -4008,14 +3930,8 @@ void MainWindow::downloadPoseGraph()
 		_initProgressDialog->show();
 		_initProgressDialog->appendText(tr("Downloading the graph (global=%1 ,optimized=%2)...")
 				.arg(global?"true":"false").arg(optimized?"true":"false"));
-		if(global)
-		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphGlobal, "", optimized?1:0));
-		}
-		else
-		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublishTOROGraphLocal, "", optimized?1:0));
-		}
+
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPublish3DMap, global, optimized, true));
 	}
 }
 
@@ -5261,9 +5177,7 @@ void MainWindow::changeState(MainWindow::State newState)
 	_ui->actionDump_the_memory->setVisible(!monitoring);
 	_ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
 	_ui->actionGenerate_map->setVisible(!monitoring);
-	_ui->actionGenerate_local_map->setVisible(!monitoring);
-	_ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring);
-	_ui->actionExport_poses_txt->setVisible(!monitoring);
+	_ui->menuExport_poses->setEnabled(!monitoring);
 	_ui->actionOpen_working_directory->setVisible(!monitoring);
 	_ui->actionData_recorder->setVisible(!monitoring);
 	_ui->menuSelect_source->menuAction()->setVisible(!monitoring);
@@ -5345,9 +5259,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(false);
 		_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 		_ui->actionGenerate_map->setEnabled(false);
-		_ui->actionGenerate_local_map->setEnabled(false);
-		_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
-		_ui->actionExport_poses_txt->setEnabled(false);
+		_ui->menuExport_poses->setEnabled(false);
 		_ui->actionDownload_all_clouds->setEnabled(false);
 		_ui->actionDownload_graph->setEnabled(false);
 		_ui->menuSelect_source->setEnabled(false);
@@ -5393,9 +5305,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(true);
 		_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 		_ui->actionGenerate_map->setEnabled(true);
-		_ui->actionGenerate_local_map->setEnabled(true);
-		_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
-		_ui->actionExport_poses_txt->setEnabled(true);
+		_ui->menuExport_poses->setEnabled(true);
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
 		_ui->menuSelect_source->setEnabled(true);
@@ -5430,9 +5340,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDelete_memory->setEnabled(false);
 		_ui->actionPost_processing->setEnabled(false);
 		_ui->actionGenerate_map->setEnabled(false);
-		_ui->actionGenerate_local_map->setEnabled(false);
-		_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
-		_ui->actionExport_poses_txt->setEnabled(false);
+		_ui->menuExport_poses->setEnabled(false);
 		_ui->actionDownload_all_clouds->setEnabled(false);
 		_ui->actionDownload_graph->setEnabled(false);
 		_ui->menuSelect_source->setEnabled(false);
@@ -5469,9 +5377,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->actionDelete_memory->setEnabled(false);
 			_ui->actionPost_processing->setEnabled(false);
 			_ui->actionGenerate_map->setEnabled(false);
-			_ui->actionGenerate_local_map->setEnabled(false);
-			_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
-			_ui->actionExport_poses_txt->setEnabled(false);
+			_ui->menuExport_poses->setEnabled(false);
 			_ui->actionDownload_all_clouds->setEnabled(false);
 			_ui->actionDownload_graph->setEnabled(false);
 			_state = kDetecting;
@@ -5498,9 +5404,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->actionDelete_memory->setEnabled(false);
 			_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 			_ui->actionGenerate_map->setEnabled(true);
-			_ui->actionGenerate_local_map->setEnabled(true);
-			_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
-			_ui->actionExport_poses_txt->setEnabled(true);
+			_ui->menuExport_poses->setEnabled(true);
 			_ui->actionDownload_all_clouds->setEnabled(true);
 			_ui->actionDownload_graph->setEnabled(true);
 			_state = kPaused;

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -55,13 +55,19 @@
      <property name="title">
       <string>Advanced</string>
      </property>
+     <widget class="QMenu" name="menuExport_poses">
+      <property name="title">
+       <string>Export poses...</string>
+      </property>
+      <addaction name="actionKITTI_format_txt"/>
+      <addaction name="actionRGBD_SLAM_format_txt"/>
+      <addaction name="actionTORO_graph"/>
+     </widget>
      <addaction name="actionOpen_working_directory"/>
      <addaction name="actionDump_the_memory"/>
      <addaction name="actionDump_the_prediction_matrix"/>
      <addaction name="actionGenerate_map"/>
-     <addaction name="actionGenerate_local_map"/>
-     <addaction name="actionGenerate_TORO_graph_graph"/>
-     <addaction name="actionExport_poses_txt"/>
+     <addaction name="menuExport_poses"/>
      <addaction name="actionPrint_loop_closure_IDs_to_console"/>
      <addaction name="separator"/>
      <addaction name="actionSend_goal"/>
@@ -908,7 +914,7 @@
   </action>
   <action name="actionGenerate_map">
    <property name="text">
-    <string>Generate graph map (*.dot)...</string>
+    <string>Generate graph (*.dot)...</string>
    </property>
   </action>
   <action name="actionDelete_memory">
@@ -973,11 +979,6 @@
     <string>Usb camera</string>
    </property>
   </action>
-  <action name="actionGenerate_local_map">
-   <property name="text">
-    <string>Generate graph local map (*.dot)...</string>
-   </property>
-  </action>
   <action name="actionPrint_loop_closure_IDs_to_console">
    <property name="text">
     <string>Print loop closure IDs to console</string>
@@ -1043,11 +1044,6 @@
     <string>Trigger a new map</string>
    </property>
   </action>
-  <action name="actionGenerate_TORO_graph_graph">
-   <property name="text">
-    <string>Generate TORO graph (*.graph)...</string>
-   </property>
-  </action>
   <action name="actionDownload_graph">
    <property name="text">
     <string>Download graph only</string>
@@ -1213,11 +1209,6 @@
     <string>Send a goal...</string>
    </property>
   </action>
-  <action name="actionExport_poses_txt">
-   <property name="text">
-    <string>Export poses (*.txt)...</string>
-   </property>
-  </action>
   <action name="actionCancel_goal">
    <property name="text">
     <string>Cancel goal</string>
@@ -1241,6 +1232,21 @@
     <string>Label current location...</string>
    </property>
   </action>
+  <action name="actionKITTI_format_txt">
+   <property name="text">
+    <string>KITTI format (*.txt)</string>
+   </property>
+  </action>
+  <action name="actionRGBD_SLAM_format_txt">
+   <property name="text">
+    <string>RGBD-SLAM format (*.txt)</string>
+   </property>
+  </action>
+  <action name="actionTORO_graph">
+   <property name="text">
+    <string>TORO (*.graph)</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-399</y>
-            <width>760</width>
-            <height>1570</height>
+            <y>0</y>
+            <width>755</width>
+            <height>1591</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>4</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2996,7 +2996,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string> Hz</string>
                       </property>
                       <property name="decimals">
-                       <number>1</number>
+                       <number>3</number>
                       </property>
                       <property name="value">
                        <double>1.000000000000000</double>
@@ -3334,7 +3334,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <string> Hz</string>
                         </property>
                         <property name="decimals">
-                         <number>1</number>
+                         <number>3</number>
                         </property>
                         <property name="value">
                          <double>1.000000000000000</double>

utilite/include/rtabmap/utilite/UConversion.h

@@ -148,6 +148,7 @@ std::string UTILITE_EXP uBool2Str(bool boolean);
  * @return the boolean
  */
 bool UTILITE_EXP uStr2Bool(const char * str);
+bool UTILITE_EXP uStr2Bool(const std::string & str);
 
 /**
  * Convert a string to an array of bytes including the null character ('\0').

utilite/include/rtabmap/utilite/UVariant.h

@@ -20,47 +20,73 @@
 #ifndef UVARIANT_H
 #define UVARIANT_H
 
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
 #include <string>
+#include <vector>
 
 /**
  * Experimental class...
  */
-class UVariant
+class UTILITE_EXP UVariant
 {
 public:
+	enum Type{
+		kBool,
+		kChar,
+		kUChar,
+		kShort,
+		kUShort,
+		kInt,
+		kUInt,
+		kFloat,
+		kDouble,
+		kStr,
+		kUndef
+		};
+public:
+	UVariant();
+	UVariant(const bool & value);
+	UVariant(const char & value);
+	UVariant(const unsigned char & value);
+	UVariant(const short & value);
+	UVariant(const unsigned short & value);
+	UVariant(const int & value);
+	UVariant(const unsigned int & value);
+	UVariant(const float & value);
+	UVariant(const double & value);
+	UVariant(const char * value);
+	UVariant(const std::string & value);
+
+	Type type() const {return type_;}
+
+	bool isUndef() const {return type_ == kUndef;}
+	bool isBool() const {return type_ == kBool;}
+	bool isChar() const {return type_ == kChar;}
+	bool isUChar() const {return type_ == kUChar;}
+	bool isShort() const {return type_ == kShort;}
+	bool isUShort() const {return type_ == kUShort;}
+	bool isInt() const {return type_ == kInt;}
+	bool isUInt() const {return type_ == kUInt;}
+	bool isFloat() const {return type_ == kFloat;}
+	bool isDouble() const {return type_ == kDouble;}
+	bool isStr() const {return type_ == kStr;}
+
+	bool toBool() const;
+	char toChar(bool * ok = 0) const;
+	unsigned char toUChar(bool * ok = 0) const;
+	short toShort(bool * ok = 0) const;
+	unsigned short toUShort(bool * ok = 0) const;
+	int toInt(bool * ok = 0) const;
+	unsigned int toUInt(bool * ok = 0) const;
+	float toFloat(bool * ok = 0) const;
+	double toDouble(bool * ok = 0) const;
+	std::string toStr(bool * ok = 0) const;
+
 	virtual ~UVariant() {}
 
-	virtual std::string className() const = 0;
-
-	template<class T>
-	const T * data() const {
-		if(data_)
-			return (T*)data_;
-		return (const T*)constData_;
-	}
-
-	template<class T>
-	T * takeDataOwnership() {
-		T * data = (T*)data_;
-		constData_ = 0;
-		data_=0;
-		return data;
-	}
-
-protected:
-	UVariant(void * data) :
-		data_(data),
-		constData_(0)
-	{}
-
-	UVariant(const void * data) :
-		data_(0),
-		constData_(data)
-	{}
-
-protected:
-	void * data_;
-	const void * constData_;
+private:
+	Type type_;
+	std::vector<unsigned char> data_;
 };
 
 #endif /* UVARIANT_H */

utilite/src/CMakeLists.txt

@@ -10,6 +10,7 @@ SET(SRC_FILES
     UThread.cpp
     UTimer.cpp
     UProcessInfo.cpp
+    UVariant.cpp
 )
 
 SET(INCLUDE_DIRS

utilite/src/UConversion.cpp

@@ -150,6 +150,11 @@ bool uStr2Bool(const char * str)
 	return !(str && (strcmp(str, "false") == 0 || strcmp(str, "FALSE") == 0 || strcmp(str, "0") == 0));
 }
 
+bool uStr2Bool(const std::string & str)
+{
+	return !(str.compare("false") == 0 || str.compare("FALSE") == 0 || str.compare("0") == 0);
+}
+
 std::vector<unsigned char> uStr2Bytes(const std::string & str)
 {
 	std::vector<unsigned char> bytes(str.size()+1);

utilite/src/UVariant.cpp

@@ -0,0 +1,641 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite is distributed in the hope that it will be useful,
+*  but WITHOUT ANY WARRANTY; without even the implied warranty of
+*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+*  GNU Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "rtabmap/utilite/UVariant.h"
+#include "rtabmap/utilite/UConversion.h"
+
+UVariant::UVariant() :
+	type_(kUndef)
+{
+}
+UVariant::UVariant(const bool & value) :
+	type_(kBool),
+	data_(1)
+{
+	data_[0] = value?1:0;
+}
+UVariant::UVariant(const char & value) :
+	type_(kChar),
+	data_(sizeof(char))
+{
+	memcpy(data_.data(), &value, sizeof(char));
+}
+UVariant::UVariant(const unsigned char & value) :
+	type_(kUChar),
+	data_(sizeof(unsigned char))
+{
+	memcpy(data_.data(), &value, sizeof(unsigned char));
+}
+UVariant::UVariant(const short & value) :
+	type_(kShort),
+	data_(sizeof(short))
+{
+	memcpy(data_.data(), &value, sizeof(short));
+}
+UVariant::UVariant(const unsigned short & value) :
+	type_(kUShort),
+	data_(sizeof(unsigned short))
+{
+	memcpy(data_.data(), &value, sizeof(unsigned short));
+}
+UVariant::UVariant(const int & value) :
+	type_(kInt),
+	data_(sizeof(int))
+{
+	memcpy(data_.data(), &value, sizeof(int));
+}
+UVariant::UVariant(const unsigned int & value) :
+	type_(kUInt),
+	data_(sizeof(unsigned int))
+{
+	memcpy(data_.data(), &value, sizeof(unsigned int));
+}
+UVariant::UVariant(const float & value) :
+	type_(kFloat),
+	data_(sizeof(float))
+{
+	memcpy(data_.data(), &value, sizeof(float));
+}
+UVariant::UVariant(const double & value) :
+	type_(kDouble),
+	data_(sizeof(double))
+{
+	memcpy(data_.data(), &value, sizeof(double));
+}
+UVariant::UVariant(const char * value) :
+	type_(kStr)
+{
+	std::string str(value);
+	data_.resize(str.size()+1);
+	memcpy(data_.data(), str.data(), str.size()+1);
+}
+UVariant::UVariant(const std::string & value) :
+	type_(kStr),
+	data_(value.size()+1) // with null character
+{
+	memcpy(data_.data(), value.data(), value.size()+1);
+}
+
+bool UVariant::toBool() const
+{
+	if(type_ ==kStr)
+	{
+		return uStr2Bool(toStr().c_str());
+	}
+	else if(data_.size())
+	{
+		return memcmp(data_.data(), std::vector<unsigned char>(data_.size(), 0).data(), data_.size()) != 0;
+	}
+	return false;
+}
+
+char UVariant::toChar(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	char v = 0;
+	if(type_ == kChar)
+	{
+		memcpy(&v, data_.data(), sizeof(char));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kUChar)
+	{
+		unsigned char tmp = toUChar();
+		if(tmp <= std::numeric_limits<char>::max())
+		{
+			v = (char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kShort)
+	{
+		short tmp = toShort();
+		if(tmp >= std::numeric_limits<char>::min() && tmp <= std::numeric_limits<char>::max())
+		{
+			v = (char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUShort)
+	{
+		unsigned short tmp = toUShort();
+		if(tmp <= std::numeric_limits<char>::max())
+		{
+			v = (char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kInt)
+	{
+		int tmp = toInt();
+		if(tmp >= std::numeric_limits<char>::min() && tmp <= std::numeric_limits<char>::max())
+		{
+			v = (char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUInt)
+	{
+		unsigned int tmp = toUInt();
+		if(tmp <= std::numeric_limits<char>::max())
+		{
+			v = (char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+unsigned char UVariant::toUChar(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	unsigned char v = 0;
+	if(type_ == kUChar)
+	{
+		memcpy(&v, data_.data(), sizeof(unsigned char));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kChar)
+	{
+		char tmp = toChar();
+		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		{
+			v = (unsigned char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kShort)
+	{
+		short tmp = toShort();
+		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		{
+			v = (unsigned char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUShort)
+	{
+		unsigned short tmp = toUShort();
+		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		{
+			v = (unsigned char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kInt)
+	{
+		int tmp = toInt();
+		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		{
+			v = (unsigned char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUInt)
+	{
+		unsigned int tmp = toUInt();
+		if(tmp >= std::numeric_limits<unsigned char>::min() && tmp <= std::numeric_limits<unsigned char>::max())
+		{
+			v = (unsigned char)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+short UVariant::toShort(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	short v = 0;
+	if(type_ == kShort)
+	{
+		memcpy(&v, data_.data(), sizeof(short));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kChar)
+	{
+		v = (short)toChar();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kUChar)
+	{
+		v = (short)toUChar();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kUShort)
+	{
+		unsigned short tmp = toUShort();
+		if(tmp <= std::numeric_limits<short>::max())
+		{
+			v = (short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kInt)
+	{
+		int tmp = toInt();
+		if(tmp >= std::numeric_limits<short>::min() && tmp <= std::numeric_limits<short>::max())
+		{
+			v = (short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUInt)
+	{
+		unsigned int tmp = toUInt();
+		if(tmp <= std::numeric_limits<short>::max())
+		{
+			v = (short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+unsigned short UVariant::toUShort(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	unsigned short v = 0;
+	if(type_ == kUShort)
+	{
+		memcpy(&v, data_.data(), sizeof(unsigned short));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kChar)
+	{
+		char tmp = toChar();
+		if(tmp >= 0)
+		{
+			v = (unsigned short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUChar)
+	{
+		v = (unsigned short)toUChar();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kShort)
+	{
+		short tmp = toShort();
+		if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
+		{
+			v = (unsigned short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kInt)
+	{
+		int tmp = toInt();
+		if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
+		{
+			v = (unsigned short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUInt)
+	{
+		unsigned int tmp = toUInt();
+		if(tmp >= std::numeric_limits<unsigned short>::min() && tmp <= std::numeric_limits<unsigned short>::max())
+		{
+			v = (unsigned short)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+int UVariant::toInt(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	int v = 0;
+	if(type_ == kInt)
+	{
+		memcpy(&v, data_.data(), sizeof(int));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kChar)
+	{
+		v = (int)toChar();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kUChar)
+	{
+		v = (int)toUChar();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kShort)
+	{
+		v = (int)toShort();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kUShort)
+	{
+		v = (int)toUShort();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kUInt)
+	{
+		unsigned int tmp = toUInt();
+		if(tmp <= std::numeric_limits<int>::max())
+		{
+			v = (int)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+unsigned int UVariant::toUInt(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	unsigned int v = 0;
+	if(type_ == kUInt)
+	{
+		memcpy(&v, data_.data(), sizeof(unsigned int));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kChar)
+	{
+		char tmp = toChar();
+		if(tmp >= 0)
+		{
+			v = (unsigned int)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUChar)
+	{
+		v = (unsigned int)toUChar();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kShort)
+	{
+		short tmp = toShort();
+		if(tmp >= 0)
+		{
+			v = (unsigned int)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	else if(type_ == kUShort)
+	{
+		v = (unsigned int)toUShort();
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kInt)
+	{
+		int tmp = toInt();
+		if(tmp >= 0)
+		{
+			v = (unsigned int)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+float UVariant::toFloat(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	float v = 0;
+	if(type_ == kFloat)
+	{
+		memcpy(&v, data_.data(), sizeof(float));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kDouble)
+	{
+		double tmp = toDouble();
+		if(tmp >= std::numeric_limits<float>::min() && tmp <= std::numeric_limits<float>::max())
+		{
+			v = (float)tmp;
+			if(ok)
+			{
+				*ok = true;
+			}
+		}
+	}
+	return v;
+}
+double UVariant::toDouble(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	double v = 0;
+	if(type_ == kDouble)
+	{
+		memcpy(&v, data_.data(), sizeof(double));
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kFloat)
+	{
+		v = (double)toFloat(ok);
+	}
+	return v;
+}
+std::string UVariant::toStr(bool * ok) const
+{
+	if(ok)
+	{
+		*ok = false;
+	}
+	std::string v;
+	if(type_ == kStr)
+	{
+		v = std::string((const char *)data_.data());
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kBool)
+	{
+		v = toBool()?"true":"false";
+		if(ok)
+		{
+			*ok = true;
+		}
+	}
+	else if(type_ == kChar)
+	{
+		v = " ";
+		v.at(0) = toChar(ok);
+	}
+	else if(type_ == kUChar)
+	{
+		v = uNumber2Str(toUChar(ok));
+	}
+	else if(type_ == kShort)
+	{
+		v = uNumber2Str(toShort(ok));
+	}
+	else if(type_ == kUShort)
+	{
+		v = uNumber2Str(toUShort(ok));
+	}
+	else if(type_ == kInt)
+	{
+		v = uNumber2Str(toInt(ok));
+	}
+	else if(type_ == kUInt)
+	{
+		v = uNumber2Str(toUInt(ok));
+	}
+	else if(type_ == kFloat)
+	{
+		v = uNumber2Str(toFloat(ok));
+	}
+	else if(type_ == kDouble)
+	{
+		v = uNumber2Str(toDouble(ok));
+	}
+	return v;
+}