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

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;
+}