Increased ROS package version to 0.10.4

CMakeLists.txt

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

corelib/include/rtabmap/core/CameraModel.h

@@ -60,6 +60,16 @@ public:
 			double cy,
 			const Transform & localTransform = Transform::getIdentity(),
 			double Tx = 0.0f);
+	// minimal to be saved
+	CameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			const Transform & localTransform = Transform::getIdentity(),
+			double Tx = 0.0f);
+
 	virtual ~CameraModel() {}
 
 	bool isValid() const {return !K_.empty() &&
@@ -69,6 +79,7 @@ public:
 									fx()>0.0 &&
 									fy()>0.0;}
 
+	void setName(const std::string & name) {name_=name;}
 	const std::string & name() const {return name_;}
 
 	double fx() const {return P_.at<double>(0,0);}
@@ -89,8 +100,8 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageWeight() const {return imageSize_.height;}
 
-	bool load(const std::string & filePath);
-	bool save(const std::string & filePath) const;
+	bool load(const std::string & directory, const std::string & cameraName);
+	bool save(const std::string & directory) const;
 
 	void scale(double scale);
 
@@ -143,13 +154,29 @@ public:
 		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
 	{
 	}
+	//minimal to be saved
+	StereoCameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(name+"_left", fx, fy, cx, cy, localTransform),
+		right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
+		name_(name)
+	{
+	}
 	virtual ~StereoCameraModel() {}
 
 	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
+
+	void setName(const std::string & name);
 	const std::string & name() const {return name_;}
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
-	bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true) const;
+	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
 
 	double baseline() const {return -right_.Tx()/right_.fx();}
 

corelib/include/rtabmap/core/CameraRGB.h

@@ -53,6 +53,7 @@ public:
 			int startAt = 1,
 			bool refreshDir = false,
 			bool rectifyImages = false,
+			bool isDepth = false,
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraImages();
@@ -62,6 +63,7 @@ public:
 	virtual std::string getSerial() const;
 	std::string getPath() const {return _path;}
 	unsigned int imagesCount() const;
+	std::vector<std::string> filenames() const;
 
 protected:
 	virtual SensorData captureImage();
@@ -73,6 +75,7 @@ private:
 	// on each call of takeImage()
 	bool _refreshDir;
 	bool _rectifyImages;
+	bool _isDepth;
 	int _count;
 	UDirectory * _dir;
 	std::string _lastFileName;

corelib/include/rtabmap/core/CameraRGBD.h

@@ -248,4 +248,44 @@ private:
 	libfreenect2::Registration * reg_;
 };
 
+
+/////////////////////////
+// CameraRGBDImages
+/////////////////////////
+class CameraImages;
+class RTABMAP_EXP CameraRGBDImages :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraRGBDImages(
+			const std::string & pathRGBImages,
+			const std::string & pathDepthImages,
+			double depthScaleFactor = 1.0,
+			bool filenamesAreTimestamps = false,
+			const std::string & timestampsPath = "", // "times.txt"
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRGBDImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	CameraImages * cameraRGB_;
+	CameraImages * cameraDepth_;
+	double depthScaleFactor_;
+	bool filenamesAreTimestamps_;
+	std::string timestampsPath_;
+	std::list<double> stamps_;
+	CameraModel cameraModel_;
+	std::string cameraName_;
+};
+
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -105,7 +105,16 @@ public:
 
 public:
 	CameraStereoImages(
-			const std::string & path,
+			const std::string & pathLeftImages,
+			const std::string & pathRightImages,
+			bool filenamesAreTimestamps = false,
+			const std::string & timestampsPath = "", // "times.txt"
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoImages(
+			const std::string & pathLeftRightImages,
+			bool filenamesAreTimestamps = false,
 			const std::string & timestampsPath = "", // "times.txt"
 			bool rectifyImages = false,
 			float imageRate=0.0f,
@@ -122,6 +131,7 @@ protected:
 private:
 	CameraImages * camera_;
 	CameraImages * camera2_;
+	bool filenamesAreTimestamps_;
 	std::string timestampsPath_;
 	bool rectifyImages_;
 	std::list<double> stamps_;

corelib/include/rtabmap/core/Odometry.h

@@ -109,7 +109,7 @@ public:
 	virtual ~OdometryBOW();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	const std::multimap<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
+	const std::map<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
 	const Memory * getMemory() const {return _memory;}
 
 private:
@@ -121,7 +121,7 @@ private:
 	std::string _fixedLocalMapPath;
 
 	Memory * _memory;
-	std::multimap<int, pcl::PointXYZ> localMap_;
+	std::map<int, pcl::PointXYZ> localMap_;
 };
 
 class RTABMAP_EXP OdometryOpticalFlow : public Odometry
@@ -195,7 +195,7 @@ private:
 
 	cv::Mat refDepthOrRight_;
 	std::map<int, cv::Point2f> cornersMap_;
-	std::multimap<int, cv::Point3f> localMap_;
+	std::map<int, cv::Point3f> localMap_;
 	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
 	float maxVariance_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -68,7 +68,7 @@ public:
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
-	std::multimap<int, cv::Point3f> localMap;
+	std::map<int, cv::Point3f> localMap;
 
 	// Optical Flow odometry
 	std::vector<cv::Point2f> refCorners;

corelib/include/rtabmap/core/Rtabmap.h

@@ -108,12 +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;
 	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,73 @@ 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
+			kCmdResume,
+			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel}; // // params: label or location ID
+			kCmdLabel             // params: [string] label, [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/include/rtabmap/core/util3d_correspondences.h

@@ -57,6 +57,14 @@ void RTABMAP_EXP findCorrespondences(
 		float maxDepth,
 		std::vector<int> * uniqueCorrespondences = 0);
 
+void RTABMAP_EXP findCorrespondences(
+		const std::map<int, pcl::PointXYZ> & words1,
+		const std::map<int, pcl::PointXYZ> & words2,
+		pcl::PointCloud<pcl::PointXYZ> & inliers1,
+		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		float maxDepth,
+		std::vector<int> * correspondences = 0);
+
 // remove depth by z axis
 void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
 									  const std::multimap<int, pcl::PointXYZ> & words2,

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -41,23 +41,23 @@ namespace rtabmap
 namespace util3d
 {
 
-Transform estimateMotion3DTo2D(
-			const std::multimap<int, pcl::PointXYZ> & words3A,
-			const std::multimap<int, cv::KeyPoint> & words2B,
+Transform RTABMAP_EXP estimateMotion3DTo2D(
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
 			const CameraModel & cameraModel,
 			int minInliers = 10,
 			int iterations = 100,
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			const Transform & guess = Transform::getIdentity(),
-			const std::multimap<int, pcl::PointXYZ> & words3B = std::multimap<int, pcl::PointXYZ>(),
+			const std::map<int, pcl::PointXYZ> & words3B = std::map<int, pcl::PointXYZ>(),
 			double * varianceOut = 0,
 			std::vector<int> * matchesOut = 0,
 			std::vector<int> * inliersOut = 0);
 
-Transform estimateMotion3DTo3D(
-			const std::multimap<int, pcl::PointXYZ> & words3A,
-			const std::multimap<int, pcl::PointXYZ> & words3B,
+Transform RTABMAP_EXP estimateMotion3DTo3D(
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, pcl::PointXYZ> & words3B,
 			int minInliers = 10,
 			double inliersDistance = 0.1,
 			int iterations = 100,

corelib/src/CameraModel.cpp

@@ -97,7 +97,38 @@ CameraModel::CameraModel(
 	K_.at<double>(1,2) = cy;
 }
 
-bool CameraModel::load(const std::string & filePath)
+CameraModel::CameraModel(
+		const std::string & name,
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		const Transform & localTransform,
+		double Tx) :
+		name_(name),
+		K_(cv::Mat::eye(3, 3, CV_64FC1)),
+		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
+		R_(cv::Mat::eye(3, 3, CV_64FC1)),
+		P_(cv::Mat::eye(3, 4, CV_64FC1)),
+		localTransform_(localTransform)
+{
+	UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
+	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	P_.at<double>(0,0) = fx;
+	P_.at<double>(1,1) = fy;
+	P_.at<double>(0,2) = cx;
+	P_.at<double>(1,2) = cy;
+	P_.at<double>(0,3) = Tx;
+
+	K_.at<double>(0,0) = fx;
+	K_.at<double>(1,1) = fy;
+	K_.at<double>(0,2) = cx;
+	K_.at<double>(1,2) = cy;
+}
+
+bool CameraModel::load(const std::string & directory, const std::string & cameraName)
 {
 	K_ = cv::Mat();
 	D_ = cv::Mat();
@@ -106,6 +137,7 @@ bool CameraModel::load(const std::string & filePath)
 	mapX_ = cv::Mat();
 	mapY_ = cv::Mat();
 
+	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
 		UINFO("Reading calibration file \"%s\"", filePath.c_str());
@@ -115,8 +147,8 @@ bool CameraModel::load(const std::string & filePath)
 		imageSize_.width = (int)fs["image_width"];
 		imageSize_.height = (int)fs["image_height"];
 		UASSERT(!name_.empty());
-		UASSERT(imageSize_.width > 0);
-		UASSERT(imageSize_.height > 0);
+		//UASSERT(imageSize_.width > 0);
+		//UASSERT(imageSize_.height > 0);
 
 		// import from ROS calibration format
 		cv::FileNode n = fs["camera_matrix"];
@@ -157,9 +189,12 @@ bool CameraModel::load(const std::string & filePath)
 
 		fs.release();
 
+		if(imageSize_.height > 0 && imageSize_.width > 0)
+		{
 			// init rectification map
 			UINFO("Initialize rectify map");
 			cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+		}
 
 		return true;
 	}
@@ -170,8 +205,9 @@ bool CameraModel::load(const std::string & filePath)
 	return false;
 }
 
-bool CameraModel::save(const std::string & filePath) const
+bool CameraModel::save(const std::string & directory) const
 {
+	std::string filePath = directory+"/"+name_+".yaml";
 	if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty())
 	{
 		UINFO("Saving calibration to file \"%s\"", filePath.c_str());
@@ -240,6 +276,7 @@ cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 	}
 	else
 	{
+		UERROR("Cannot rectify image because the rectify map is not initialized.");
 		return raw.clone();
 	}
 }
@@ -299,10 +336,17 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 //
 //StereoCameraModel
 //
+void StereoCameraModel::setName(const std::string & name)
+{
+	name_=name;
+	left_.setName(name_+"_left");
+	right_.setName(name_+"_right");
+}
+
 bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
 {
 	name_ = cameraName;
-	if(left_.load(directory+"/"+cameraName+"_left.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml"))
+	if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
 	{
 		if(ignoreStereoTransform)
 		{
@@ -368,15 +412,15 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 	}
 	return false;
 }
-bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) const
+bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
 {
-	if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
+	if(left_.save(directory) && right_.save(directory))
 	{
 		if(ignoreStereoTransform)
 		{
 			return true;
 		}
-		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
+		std::string filePath = directory+"/"+name_+"_pose.yaml";
 		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
 		{
 			UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());

corelib/src/CameraRGB.cpp

@@ -51,6 +51,7 @@ CameraImages::CameraImages(const std::string & path,
 					 int startAt,
 					 bool refreshDir,
 					 bool rectifyImages,
+					 bool isDepth,
 					 float imageRate,
 					 const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -58,6 +59,7 @@ CameraImages::CameraImages(const std::string & path,
 	_startAt(startAt),
 	_refreshDir(refreshDir),
 	_rectifyImages(rectifyImages),
+	_isDepth(isDepth),
 	_count(0),
 	_dir(0)
 {
@@ -106,7 +108,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
-		if(!_model.load(calibrationFolder + "/" + cameraName + ".yaml"))
+		if(!_model.load(calibrationFolder, cameraName))
 		{
 			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 					cameraName.c_str(), calibrationFolder.c_str());
@@ -150,6 +152,15 @@ unsigned int CameraImages::imagesCount() const
 	return 0;
 }
 
+std::vector<std::string> CameraImages::filenames() const
+{
+	if(_dir)
+	{
+		return uListToVector(_dir->getFileNames());
+	}
+	return std::vector<std::string>();
+}
+
 SensorData CameraImages::captureImage()
 {
 	cv::Mat img;
@@ -197,6 +208,18 @@ SensorData CameraImages::captureImage()
 					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
 							img.cols, img.rows, img.channels(), img.elemSize(), img.total());
 
+					if(_isDepth)
+					{
+						if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
+						{
+							UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
+									"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
+									fileName.c_str());
+							img = cv::Mat();
+						}
+					}
+					else
+					{
 #if CV_MAJOR_VERSION < 3
 						// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
 						if(img.depth() != CV_8U)
@@ -219,6 +242,7 @@ SensorData CameraImages::captureImage()
 					}
 				}
 			}
+		}
 
 		if(!img.empty() && _model.isValid() && _rectifyImages)
 		{
@@ -230,6 +254,10 @@ SensorData CameraImages::captureImage()
 		UWARN("Directory is not set, camera must be initialized.");
 	}
 
+	if(_isDepth)
+	{
+		return SensorData(cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
+	}
 	return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
 }
 
@@ -307,7 +335,7 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 		// look for calibration files
 		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
 		{
-			if(!_model.load(calibrationFolder + "/" + (cameraName.empty()?_guid:cameraName) + ".yaml"))
+			if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName)))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 						cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());

corelib/src/CameraRGBD.cpp

@@ -1508,4 +1508,187 @@ SensorData CameraFreenect2::captureImage()
 	return data;
 }
 
+//
+// CameraRGBDImages
+//
+bool CameraRGBDImages::available()
+{
+	return true;
+}
+
+CameraRGBDImages::CameraRGBDImages(
+		const std::string & pathRGBImages,
+		const std::string & pathDepthImages,
+		double depthScaleFactor,
+		bool filenamesAreTimestamps,
+		const std::string & timestampsPath,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		cameraRGB_(0),
+		cameraDepth_(0),
+		depthScaleFactor_(depthScaleFactor),
+		filenamesAreTimestamps_(filenamesAreTimestamps),
+		timestampsPath_(timestampsPath)
+{
+	UASSERT(depthScaleFactor >= 1.0);
+	cameraRGB_ = new CameraImages(pathRGBImages);
+	cameraDepth_ = new CameraImages(pathDepthImages, 1, false, false, true);
+}
+
+CameraRGBDImages::~CameraRGBDImages()
+{
+	if(cameraRGB_)
+	{
+		delete cameraRGB_;
+	}
+	if(cameraDepth_)
+	{
+		delete cameraDepth_;
+	}
+}
+
+bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	// look for calibration files
+	cameraName_ = cameraName;
+	if(!calibrationFolder.empty() && !cameraName.empty())
+	{
+		if(!cameraModel_.load(calibrationFolder, cameraName))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+					cameraName.c_str(), calibrationFolder.c_str());
+		}
+		else
+		{
+			UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+					cameraModel_.fx(),
+					cameraModel_.fy(),
+					cameraModel_.cx(),
+					cameraModel_.cy());
+		}
+	}
+	cameraModel_.setLocalTransform(this->getLocalTransform());
+
+	bool success = false;
+	if(cameraRGB_->init() && cameraDepth_->init())
+	{
+		if(cameraRGB_->imagesCount() == cameraDepth_->imagesCount())
+		{
+			success = true;
+		}
+		else
+		{
+			UERROR("Cameras don't have the same number of images (%d vs %d)",
+					cameraRGB_->imagesCount(), cameraDepth_->imagesCount());
+		}
+	}
+
+	stamps_.clear();
+	if(success)
+	{
+		if(filenamesAreTimestamps_)
+		{
+			std::vector<std::string> filenames = cameraRGB_->filenames();
+			for(unsigned int i=0; i<filenames.size(); ++i)
+			{
+				// format is 12234456.12334.png
+				std::list<std::string> list = uSplit(filenames.at(i), '.');
+				if(list.size() == 3)
+				{
+					list.pop_back(); // remove extension
+					double stamp = uStr2Double(uJoin(list, "."));
+					if(stamp > 0.0)
+					{
+						stamps_.push_back(stamp);
+					}
+					else
+					{
+						UERROR("Conversion filename to timestamp failed! (filename=%s)", filenames.at(i).c_str());
+					}
+				}
+			}
+			if(stamps_.size() != cameraRGB_->imagesCount())
+			{
+				UERROR("The stamps count is not the same as the images (%d vs %d)! "
+					   "Converting filenames to timestamps is activated.",
+						(int)stamps_.size(), cameraRGB_->imagesCount());
+				stamps_.clear();
+				success = false;
+			}
+		}
+		else if(timestampsPath_.size())
+		{
+			FILE * file = 0;
+#ifdef _MSC_VER
+			fopen_s(&file, timestampsPath_.c_str(), "r");
+#else
+			file = fopen(timestampsPath_.c_str(), "r");
+#endif
+			if(file)
+			{
+				char line[16];
+				while ( fgets (line , 16 , file) != NULL )
+				{
+					stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
+				}
+				fclose(file);
+			}
+			if(stamps_.size() != cameraRGB_->imagesCount())
+			{
+				UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
+						"the timestamps file path if you don't want to use them (current file path=%s).",
+						(int)stamps_.size(), cameraRGB_->imagesCount(), timestampsPath_.c_str());
+				stamps_.clear();
+				success = false;
+			}
+		}
+	}
+
+	return success;
+}
+
+bool CameraRGBDImages::isCalibrated() const
+{
+	return cameraModel_.isValid();
+}
+
+std::string CameraRGBDImages::getSerial() const
+{
+	return cameraName_;
+}
+
+SensorData CameraRGBDImages::captureImage()
+{
+	SensorData data;
+
+	double stamp;
+	if(stamps_.size())
+	{
+		stamp = stamps_.front();
+		stamps_.pop_front();
+	}
+	else
+	{
+		stamp = UTimer::now();
+	}
+	SensorData rgb, depth;
+	rgb = cameraRGB_->takeImage();
+	if(!rgb.imageRaw().empty())
+	{
+		depth = cameraDepth_->takeImage();
+		if(!depth.depthRaw().empty())
+		{
+			cv::Mat depthScaled = depth.depthRaw();
+			if(depthScaleFactor_ > 1.0)
+			{
+				depthScaled /= depthScaleFactor_;
+			}
+			data = SensorData(rgb.imageRaw(), depthScaled, cameraModel_, this->getNextSeqID(), stamp);
+		}
+	}
+	return data;
+}
+
+
 } // namespace rtabmap

corelib/src/CameraStereo.cpp

@@ -731,7 +731,9 @@ bool CameraStereoImages::available()
 }
 
 CameraStereoImages::CameraStereoImages(
-		const std::string & path,
+		const std::string & pathLeftImages,
+		const std::string & pathRightImages,
+		bool filenamesAreTimestamps,
 		const std::string & timestampsPath,
 		bool rectifyImages,
 		float imageRate,
@@ -739,10 +741,29 @@ CameraStereoImages::CameraStereoImages(
 		Camera(imageRate, localTransform),
 		camera_(0),
 		camera2_(0),
+		filenamesAreTimestamps_(filenamesAreTimestamps),
 		timestampsPath_(timestampsPath),
 		rectifyImages_(rectifyImages)
 {
-	std::vector<std::string> paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';'));
+	camera_ = new CameraImages(pathLeftImages);
+	camera2_ = new CameraImages(pathRightImages);
+}
+
+CameraStereoImages::CameraStereoImages(
+		const std::string & pathLeftRightImages,
+		bool filenamesAreTimestamps,
+		const std::string & timestampsPath,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		camera_(0),
+		camera2_(0),
+		filenamesAreTimestamps_(filenamesAreTimestamps),
+		timestampsPath_(timestampsPath),
+		rectifyImages_(rectifyImages)
+{
+	std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
 	if(paths.size() >= 1)
 	{
 		camera_ = new CameraImages(paths[0]);
@@ -830,7 +851,39 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	}
 
 	stamps_.clear();
-	if(success && timestampsPath_.size())
+	if(success)
+	{
+		if(filenamesAreTimestamps_)
+		{
+			std::vector<std::string> filenames = camera_->filenames();
+			for(unsigned int i=0; i<filenames.size(); ++i)
+			{
+				// format is 12234456.12334.png
+				std::list<std::string> list = uSplit(filenames.at(i), '.');
+				if(list.size() == 3)
+				{
+					list.pop_back(); // remove extension
+					double stamp = uStr2Double(uJoin(list, "."));
+					if(stamp > 0.0)
+					{
+						stamps_.push_back(stamp);
+					}
+					else
+					{
+						UERROR("Conversion filename to timestamp failed! (filename=%s)", filenames.at(i).c_str());
+					}
+				}
+			}
+			if(stamps_.size() != camera_->imagesCount())
+			{
+				UERROR("The stamps count is not the same as the images (%d vs %d)! "
+					   "Converting filenames to timestamps is activated.",
+						(int)stamps_.size(), camera_->imagesCount());
+				stamps_.clear();
+				success = false;
+			}
+		}
+		else if(timestampsPath_.size())
 		{
 			FILE * file = 0;
 #ifdef _MSC_VER
@@ -856,6 +909,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 				success = false;
 			}
 		}
+	}
 
 	return success;
 }

corelib/src/Memory.cpp

@@ -2102,15 +2102,15 @@ Transform Memory::computeVisualTransform(
 
 				std::vector<int> inliersV;
 				transform = util3d::estimateMotion3DTo2D(
-						oldS.getWords3(),
-						newS.getWords(),
+						uMultimapToMap(oldS.getWords3()),
+						uMultimapToMap(newS.getWords()),
 						cameraModel,
 						_bowMinInliers,
 						_bowIterations,
 						_bowPnPReprojError,
 						_bowPnPFlags,
 						Transform::getIdentity(),
-						newS.getWords3(),
+						uMultimapToMap(newS.getWords3()),
 						&variance,
 						0,
 						&inliersV);
@@ -2143,8 +2143,8 @@ Transform Memory::computeVisualTransform(
 		{
 			std::vector<int> inliersV;
 			transform = util3d::estimateMotion3DTo3D(
-					oldS.getWords3(),
-					newS.getWords3(),
+					uMultimapToMap(oldS.getWords3()),
+					uMultimapToMap(newS.getWords3()),
 					_bowMinInliers,
 					_bowInlierDistance,
 					_bowIterations,

corelib/src/OdometryBOW.cpp

@@ -247,14 +247,14 @@ Transform OdometryBOW::computeTransform(
 						UDEBUG("");
 						t = util3d::estimateMotion3DTo2D(
 								localMap_,
-								newSignature->getWords(),
+								uMultimapToMap(newSignature->getWords()),
 								cameraModel,
 								this->getMinInliers(),
 								this->getIterations(),
 								this->getPnPReprojError(),
 								this->getPnPFlags(),
 								this->getPose(),
-								newSignature->getWords3(),
+								uMultimapToMap(newSignature->getWords3()),
 								&variance,
 								&matches,
 								&inliers);
@@ -271,7 +271,7 @@ Transform OdometryBOW::computeTransform(
 					{
 						t = util3d::estimateMotion3DTo3D(
 								localMap_,
-								newSignature->getWords3(),
+								uMultimapToMap(newSignature->getWords3()),
 								this->getMinInliers(),
 								this->getInlierDistance(),
 								this->getIterations(),

corelib/src/OdometryMono.cpp

@@ -416,7 +416,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							UDEBUG("cameraTransform guess=  %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
 							if(cameraTransform.getNorm() < minTranslation_)
 							{
-								UWARN("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
+								UINFO("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
 										cameraTransform.getNorm(), minTranslation_);
 							}
 							else
@@ -730,6 +730,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 
 							pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
 
+							if(!refDepthOrRight_.empty())
+							{
 								if(refDepthOrRight_.type() == CV_8UC1)
 								{
 									 newCorners3D = util3d::generateKeypoints3DStereo(
@@ -757,10 +759,11 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 											 refDepthOrRight_,
 											 m);
 								}
-							else if(!refDepthOrRight_.empty())
+								else
 								{
 									UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type());
 								}
+							}
 
 							for(unsigned int i=0; i<cloud->size(); ++i)
 							{
@@ -833,11 +836,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									scale = scales.begin()->second;
 									UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size());
 
-									maxVariance_ = 0.01;
-									UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
-									if(scales.begin()->first > 0.01)
+									UDEBUG("Max noise variance = %f current variance=%f", maxVariance_, scales.begin()->first);
+									if(scales.begin()->first > maxVariance_)
 									{
-										UWARN("Too high variance %f (should be < 0.01)", scales.begin()->first);
+										UWARN("Too high variance %f (should be < %f)", scales.begin()->first, maxVariance_);
 										reject = true; // 20 cm for good initialization
 									}
 								}
@@ -849,7 +851,6 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								Eigen::Vector4f centroid;
 								pcl::compute3DCentroid(*inliersRef, centroid);
 								scale = 1.0f / centroid[2];
-								maxVariance_ = 0.01;
 							}
 							else
 							{
@@ -959,9 +960,12 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 			if((int)words.size() > this->getMinInliers())
 			{
 				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+				{
+					if(words.count(iter->first) == 1)
 					{
 						cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
 					}
+				}
 				refDepthOrRight_ = data.depthOrRightRaw().clone();
 				keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
 			}

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,28 +735,70 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
+		if(type==2) // TORO
+		{
 			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)
-		{
-			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, 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());
 			}
 
-		this->dumpPoses(path, poses);
+			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();
+
+						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);
+			}
+		}
 	}
 }
 
@@ -2464,35 +2506,6 @@ void Rtabmap::dumpData() const
 	}
 }
 
-void Rtabmap::dumpPoses(
-		const std::string & path,
-		const std::map<int, Transform> & poses) const
-{
-	UDEBUG("");
-	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();
-
-			fprintf(fout, "%f", p[0]);
-			for(int i=1; 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,29 +117,7 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
 	enabled = _createIntermediateNodes;
 }
 
-void RtabmapThread::publishMap(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->get3DMap(signatures,
-			poses,
-			constraints,
-			optimized,
-			full);
-
-	this->post(new RtabmapEvent3DMap(
-			signatures,
-			poses,
-			constraints));
-}
-
-void RtabmapThread::publishGraph(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;
@@ -149,11 +127,23 @@ void RtabmapThread::publishGraph(bool optimized, bool full) const
 	std::map<int, std::string> labels;
 	std::map<int, std::vector<unsigned char> > userDatas;
 
+	if(graphOnly)
+	{
 		_rtabmap->getGraph(poses,
 				constraints,
 				optimized,
 				full,
 				&signatures);
+	}
+	else
+	{
+		_rtabmap->get3DMap(
+				signatures,
+				poses,
+				constraints,
+				optimized,
+				full);
+	}
 
 	this->post(new RtabmapEvent3DMap(
 			signatures,
@@ -161,7 +151,6 @@ void RtabmapThread::publishGraph(bool optimized, bool full) const
 			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,11 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdLabel)
 		{
 			ULOGGER_DEBUG("CMD_LABEL");
+			UASSERT(rtabmapEvent->value1().isStr());
+			UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("label", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("id", uNumber2Str(rtabmapEvent->getInt())));
+			param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
 			pushNewState(kStateLabelling, param);
 		}
 		else

corelib/src/util3d_correspondences.cpp

@@ -314,41 +314,6 @@ void findCorrespondences(
 	}
 }
 
-
-std::list<std::pair<cv::Point2f, cv::Point2f> > findCorrespondences(
-		const std::multimap<int, cv::KeyPoint> & words1,
-		const std::multimap<int, cv::KeyPoint> & words2)
-{
-	std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences;
-
-	// Find pairs
-	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
-	rtabmap::EpipolarGeometry::findPairsUnique(words1, words2, pairs);
-
-	if(pairs.size() > 7) // 8 min?
-	{
-		// Find fundamental matrix
-		std::vector<uchar> status;
-		cv::Mat fundamentalMatrix = rtabmap::EpipolarGeometry::findFFromWords(pairs, status);
-		//ROS_INFO("inliers = %d/%d", uSum(status), pairs.size());
-		if(!fundamentalMatrix.empty())
-		{
-			int i = 0;
-			//int goodCount = 0;
-			for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
-			{
-				if(status[i])
-				{
-					correspondences.push_back(std::pair<cv::Point2f, cv::Point2f>(iter->second.first.pt, iter->second.second.pt));
-					//ROS_INFO("inliers kpts %f %f vs %f %f", iter->second.first.pt.x, iter->second.first.pt.y, iter->second.second.pt.x, iter->second.second.pt.y);
-				}
-				++i;
-			}
-		}
-	}
-	return correspondences;
-}
-
 void findCorrespondences(
 		const std::multimap<int, pcl::PointXYZ> & words1,
 		const std::multimap<int, pcl::PointXYZ> & words2,
@@ -395,6 +360,52 @@ void findCorrespondences(
 	}
 }
 
+void findCorrespondences(
+		const std::map<int, pcl::PointXYZ> & words1,
+		const std::map<int, pcl::PointXYZ> & words2,
+		pcl::PointCloud<pcl::PointXYZ> & inliers1,
+		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		float maxDepth,
+		std::vector<int> * correspondences)
+{
+	std::vector<int> ids = uKeys(words1);
+	// Find pairs
+	inliers1.resize(ids.size());
+	inliers2.resize(ids.size());
+	if(correspondences)
+	{
+		correspondences->resize(ids.size());
+	}
+
+	int oi=0;
+	for(std::vector<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+	{
+		if(words2.find(*iter) != words2.end())
+		{
+			inliers1[oi] = words1.find(*iter)->second;
+			inliers2[oi] = words2.find(*iter)->second;
+			if(pcl::isFinite(inliers1[oi]) &&
+			   pcl::isFinite(inliers2[oi]) &&
+			   (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
+			   (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
+			   (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
+			{
+				if(correspondences)
+				{
+					correspondences->at(oi) = *iter;
+				}
+				++oi;
+			}
+		}
+	}
+	inliers1.resize(oi);
+	inliers2.resize(oi);
+	if(correspondences)
+	{
+		correspondences->resize(oi);
+	}
+}
+
 }
 
 }

corelib/src/util3d_motion_estimation.cpp

@@ -40,15 +40,15 @@ namespace util3d
 {
 
 Transform estimateMotion3DTo2D(
-			const std::multimap<int, pcl::PointXYZ> & words3A,
-			const std::multimap<int, cv::KeyPoint> & words2B,
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
 			const CameraModel & cameraModel,
 			int minInliers,
 			int iterations,
 			double reprojError,
 			int flagsPnP,
 			const Transform & guess,
-			const std::multimap<int, pcl::PointXYZ> & words3B,
+			const std::map<int, pcl::PointXYZ> & words3B,
 			double * varianceOut,
 			std::vector<int> * matchesOut,
 			std::vector<int> * inliersOut)
@@ -63,14 +63,14 @@ Transform estimateMotion3DTo2D(
 	}
 
 	// find correspondences
-	std::vector<int> ids = uListToVector(uUniqueKeys(words2B));
+	std::vector<int> ids = uKeys(words2B);
 	std::vector<cv::Point3f> objectPoints(ids.size());
 	std::vector<cv::Point2f> imagePoints(ids.size());
 	int oi=0;
 	matches.resize(ids.size());
 	for(unsigned int i=0; i<ids.size(); ++i)
 	{
-		if(words3A.count(ids[i]) == 1)
+		if(words3A.find(ids[i]) != words3A.end())
 		{
 			pcl::PointXYZ pt = words3A.find(ids[i])->second;
 			objectPoints[oi].x = pt.x;
@@ -170,8 +170,8 @@ Transform estimateMotion3DTo2D(
 }
 
 Transform estimateMotion3DTo3D(
-			const std::multimap<int, pcl::PointXYZ> & words3A,
-			const std::multimap<int, pcl::PointXYZ> & words3B,
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, pcl::PointXYZ> & words3B,
 			int minInliers,
 			double inliersDistance,
 			int iterations,

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();
@@ -168,7 +168,6 @@ private slots:
 	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
 	void changeMappingMode();
-	void captureScreen();
 	void setAspectRatio(int w, int h);
 	void setAspectRatio16_9();
 	void setAspectRatio16_10();
@@ -223,6 +222,8 @@ private:
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
 	void saveFigures();
 	void loadFigures();
+	void exportPoses(int format);
+	QString captureScreen();
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
 			const std::map<int, Transform> & poses,
@@ -262,6 +263,7 @@ private:
 	QSet<int> _lastIds;
 	int _lastId;
 	bool _processingStatistics;
+	bool _processingDownloadedMap;
 	bool _odometryReceived;
 	QString _newDatabasePath;
 	QString _newDatabasePathOutput;
@@ -296,8 +298,7 @@ private:
 	DetailedProgressDialog * _initProgressDialog;
 
 	QString _graphSavingFileName;
-	QString _toroSavingFileName;
-	QString _posesSavingFileName;
+	QMap<int, QString> _exportPosesFileName;
 	bool _autoScreenCaptureOdomSync;
 
 	QVector<int> _refIds;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -88,6 +88,7 @@ public:
 		kSrcOpenNI_CV_ASUS = 3,
 		kSrcOpenNI2        = 4,
 		kSrcFreenect2      = 5,
+		kSrcRGBDImages     = 6,
 
 		kSrcStereo         = 100,
 		kSrcDC1394         = 100,
@@ -181,27 +182,11 @@ public:
 	QString getSourceDriverStr() const;
 	QString getSourceDevice() const;
 
-	QString getSourceImagesPath() const;	//Images group
-	QString getSourceImagesSuffix() const;	//Images group
-	int getSourceImagesSuffixIndex() const;	//Images group
-	int getSourceImagesStartPos() const;	//Images group
-	bool getSourceImagesRefreshDir() const;	//Images group
-	bool getSourceImagesRectify() const; //Images group
-	QString getSourceVideoPath() const;	//Video group
-	bool getSourceVideoRectify() const; //Video group
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	bool getSourceDatabaseGoalDelayIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
 	bool getSourceDatabaseStampsUsed() const;//Database group
-	bool getSourceOpenni2AutoWhiteBalance() const;  //Openni group
-	bool getSourceOpenni2AutoExposure() const;  //Openni group
-	int getSourceOpenni2Exposure() const;  //Openni group
-	int getSourceOpenni2Gain() const;   //Openni group
-	bool getSourceOpenni2Mirroring() const; //Openni group
-	int getSourceFreenect2Format() const; //Openni group
-	bool getSourceStereoImagesRectify() const;
-	bool getSourceStereoVideoRectify() const;
 	bool isSourceRGBDColorOnly() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
@@ -236,6 +221,7 @@ public slots:
 	void setSLAMMode(bool enabled);
 	void selectSourceDriver(Src src);
 	void calibrate();
+	void calibrateSimple();
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -263,8 +249,12 @@ private slots:
 	void updateBasicParameter();
 	void openDatabaseViewer();
 	void selectSourceDatabase();
+	void selectSourceRGBDImagesStamps();
+	void selectSourceRGBDImagesPathRGB();
+	void selectSourceRGBDImagesPathDepth();
 	void selectSourceStereoImagesStamps();
-	void selectSourceStereoImagesPath();
+	void selectSourceStereoImagesPathLeft();
+	void selectSourceStereoImagesPathRight();
 	void selectSourceImagesPath();
 	void selectSourceVideoPath();
 	void selectSourceStereoVideoPath();

guilib/src/CMakeLists.txt

@@ -24,6 +24,7 @@ SET(headers_ui
 	./ExportCloudsDialog.h
 	./MapVisibilityWidget.h
 	./GraphViewer.h
+	./CreateSimpleCalibrationDialog.h
 )
 
 SET(uis
@@ -37,6 +38,7 @@ SET(uis
     ./ui/postProcessingDialog.ui
     ./ui/exportCloudsDialog.ui
     ./ui/calibrationDialog.ui
+    ./ui/createSimpleCalibrationDialog.ui
 )
 
 SET(qrc 
@@ -83,6 +85,7 @@ SET(SRC_FILES
 	./ExportCloudsDialog.cpp
 	./MapVisibilityWidget.cpp
 	./GraphViewer.cpp
+	./CreateSimpleCalibrationDialog.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}

guilib/src/CalibrationDialog.cpp

@@ -877,7 +877,10 @@ bool CalibrationDialog::save()
 
 		if(!filePath.isEmpty())
 		{
-			if(models_[0].save(filePath.toStdString()))
+			QString name = QFileInfo(filePath).baseName();
+			QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+			models_[0].setName(name.toStdString());
+			if(models_[0].save(dir.toStdString()))
 			{
 				QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
 				UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
@@ -901,11 +904,12 @@ bool CalibrationDialog::save()
 		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
 		if(!name.isEmpty())
 		{
+			stereoModel_.setName(name.toStdString());
 			std::string base = (dir+QDir::separator()+name).toStdString();
 			std::string leftPath = base+"_left.yaml";
 			std::string rightPath = base+"_right.yaml";
 			std::string posePath = base+"_pose.yaml";
-			if(stereoModel_.save(dir.toStdString(), name.toStdString(), false))
+			if(stereoModel_.save(dir.toStdString(), false))
 			{
 				QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
 						arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));

guilib/src/CameraViewer.cpp

@@ -85,7 +85,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	{
 		imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
 	}
-	if((data.stereoCameraModel().isValid() || data.cameraModels().size()))
+	if((data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
 	{
 		if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
 		{

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -0,0 +1,135 @@
+/*
+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 "CreateSimpleCalibrationDialog.h"
+#include "ui_createSimpleCalibrationDialog.h"
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/utilite/ULogger.h"
+
+#include <QFileDialog>
+#include <QPushButton>
+#include <QMessageBox>
+
+namespace rtabmap {
+
+CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
+		const QString & savingFolder,
+		const QString & cameraName,
+		QWidget * parent) :
+	QDialog(parent),
+	savingFolder_(savingFolder),
+	cameraName_(cameraName)
+{
+	ui_ = new Ui_createSimpleCalibrationDialog();
+	ui_->setupUi(this);
+
+	connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
+	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+
+	connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+	connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
+
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
+}
+
+CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
+{
+	delete ui_;
+}
+
+void CreateSimpleCalibrationDialog::updateSaveStatus()
+{
+	ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(ui_->doubleSpinBox_fx->value() > 0.0 && ui_->doubleSpinBox_fy->value() > 0.0);
+}
+
+void CreateSimpleCalibrationDialog::saveCalibration()
+{
+	if(ui_->doubleSpinBox_baseline->value()==0)
+	{
+		QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
+		QString name = QFileInfo(filePath).baseName();
+		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+		if(!filePath.isEmpty())
+		{
+			cameraName_ = name;
+			CameraModel model(
+					name.toStdString(),
+					ui_->doubleSpinBox_fx->value(),
+					ui_->doubleSpinBox_fy->value(),
+					ui_->doubleSpinBox_cx->value(),
+					ui_->doubleSpinBox_cy->value());
+			UASSERT(model.isValid());
+			if(model.save(dir.toStdString()))
+			{
+				QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\"").arg(filePath));
+			}
+		}
+	}
+	else
+	{
+		QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_ + "/" + cameraName_, "*.yaml");
+		QString name = QFileInfo(filePath).baseName();
+		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+		if(!name.isEmpty())
+		{
+			std::string base = (dir+QDir::separator()+name).toStdString();
+			std::string leftPath = base+"_left.yaml";
+			std::string rightPath = base+"_right.yaml";
+			std::string posePath = base+"_pose.yaml";
+
+			StereoCameraModel model(
+					name.toStdString(),
+					ui_->doubleSpinBox_fx->value(),
+					ui_->doubleSpinBox_fy->value(),
+					ui_->doubleSpinBox_cx->value(),
+					ui_->doubleSpinBox_cy->value(),
+					ui_->doubleSpinBox_baseline->value());
+			UASSERT(model.left().isValid() &&
+					model.right().isValid()&&
+					model.baseline() > 0.0);
+
+			if(model.save(dir.toStdString(), true))
+			{
+				QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
+						arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
+				this->accept();
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
+			}
+		}
+	}
+}
+
+}

guilib/src/CreateSimpleCalibrationDialog.h

@@ -0,0 +1,62 @@
+/*
+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 CREATESIMPLECALIBRATIONDIALOG_H_
+#define CREATESIMPLECALIBRATIONDIALOG_H_
+
+#include <QDialog>
+#include <QSettings>
+
+class Ui_createSimpleCalibrationDialog;
+
+namespace rtabmap {
+
+class CreateSimpleCalibrationDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	CreateSimpleCalibrationDialog(
+			const QString & savingFolder = ".",
+			const QString & cameraName = "",
+			QWidget * parent = 0);
+
+	virtual ~CreateSimpleCalibrationDialog();
+
+private slots:
+	void updateSaveStatus();
+	void saveCalibration();
+
+private:
+	Ui_createSimpleCalibrationDialog * ui_;
+	QString savingFolder_;
+	QString cameraName_;
+};
+
+}
+
+#endif /* CREATESIMPLECALIBRATIONDIALOG_H_ */

guilib/src/DatabaseViewer.cpp

@@ -895,7 +895,7 @@ void DatabaseViewer::extractImages()
 				{
 					UERROR("Cannot save calibration file, database name is empty!");
 				}
-				else
+				else if(data.stereoCameraModel().isValid())
 				{
 					std::string cameraName = uSplit(databaseFileName_, '.').front();
 					StereoCameraModel model(
@@ -915,7 +915,7 @@ void DatabaseViewer::extractImages()
 							data.stereoCameraModel().E(),
 							data.stereoCameraModel().F(),
 							data.stereoCameraModel().left().localTransform());
-					if(model.save(path.toStdString(), cameraName))
+					if(model.save(path.toStdString()))
 					{
 						UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
 					}
@@ -925,6 +925,43 @@ void DatabaseViewer::extractImages()
 					}
 				}
 			}
+			else if(!data.imageRaw().empty())
+			{
+				if(!data.depthRaw().empty())
+				{
+					QDir dir;
+					dir.mkdir(QString("%1/rgb").arg(path));
+					dir.mkdir(QString("%1/depth").arg(path));
+				}
+
+				if(databaseFileName_.empty())
+				{
+					UERROR("Cannot save calibration file, database name is empty!");
+				}
+				else if(data.cameraModels().size() > 1)
+				{
+					UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
+				}
+				else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValid())
+				{
+					std::string cameraName = uSplit(databaseFileName_, '.').front();
+					CameraModel model(cameraName,
+							data.imageRaw().size(),
+							data.cameraModels().front().K(),
+							data.cameraModels().front().D(),
+							data.cameraModels().front().R(),
+							data.cameraModels().front().P(),
+							data.cameraModels().front().localTransform());
+					if(model.save(path.toStdString()))
+					{
+						UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+					}
+					else
+					{
+						UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+					}
+				}
+			}
 		}
 
 		for(int i=0; i<ids_.size(); ++i)
@@ -937,6 +974,12 @@ void DatabaseViewer::extractImages()
 				cv::imwrite(QString("%1/right/%2.jpg").arg(path).arg(id).toStdString(), data.rightRaw());
 				UINFO(QString("Saved left/%1.jpg and right/%1.jpg").arg(id).toStdString().c_str());
 			}
+			else if(!data.imageRaw().empty() && !data.depthRaw().empty())
+			{
+				cv::imwrite(QString("%1/rgb/%2.jpg").arg(path).arg(id).toStdString(), data.imageRaw());
+				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw());
+				UINFO(QString("Saved rgb/%1.jpg and depth/%1.png").arg(id).toStdString().c_str());
+			}
 			else if(!data.imageRaw().empty())
 			{
 				cv::imwrite(QString("%1/%2.jpg").arg(path).arg(id).toStdString(), data.imageRaw());

guilib/src/MainWindow.cpp

@@ -125,6 +125,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_dataRecorder(0),
 	_lastId(0),
 	_processingStatistics(false),
+	_processingDownloadedMap(false),
 	_odometryReceived(false),
 	_newDatabasePath(""),
 	_newDatabasePathOutput(""),
@@ -299,10 +300,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()));
@@ -622,7 +623,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		   (localLoopClosureId > 0 &&
 		    _ui->actionPause_on_local_loop_detection->isChecked()))
 		{
-			if(_state != kPaused && _state != kMonitoringPaused)
+			if(_state != kPaused && _state != kMonitoringPaused && !_processingDownloadedMap)
 			{
 				if(_preferencesDialog->beepOnPause())
 				{
@@ -631,9 +632,13 @@ void MainWindow::handleEvent(UEvent* anEvent)
 				this->pauseDetection();
 			}
 		}
+
+		if(!_processingDownloadedMap)
+		{
 			_processingStatistics = true;
 			emit statsReceived(stats);
 		}
+	}
 	else if(anEvent->getClassName().compare("RtabmapEventInit") == 0)
 	{
 		RtabmapEventInit * rtabmapEventInit = (RtabmapEventInit*)anEvent;
@@ -848,8 +853,15 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 				}
 			}
 
+			//detect if it is OdometryMono intitialization
+			bool monoInitialization = false;
+			if(_preferencesDialog->getOdomStrategy() == 2 && odom.info().type == 1)
+			{
+				monoInitialization = true;
+			}
+
 			_ui->imageView_odometry->clearLines();
-			if(lost)
+			if(lost && !monoInitialization)
 			{
 				if(lostStateChanged)
 				{
@@ -2087,6 +2099,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 	else
 	{
 
+		_processingDownloadedMap = true;
 		UINFO("Received map!");
 		_initProgressDialog->appendText(tr(" poses = %1").arg(event.getPoses().size()));
 		_initProgressDialog->appendText(tr(" constraints = %1").arg(event.getConstraints().size()));
@@ -2142,6 +2155,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		{
 			_cachedSignatures.clear();
 		}
+		_processingDownloadedMap = false;
 	}
 	_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
 }
@@ -2213,22 +2227,6 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		if(_camera)
 		{
 			_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
-
-			if(_camera->camera() && dynamic_cast<CameraOpenNI2*>(_camera->camera()) != 0)
-			{
-				((CameraOpenNI2*)_camera->camera())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
-				((CameraOpenNI2*)_camera->camera())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
-				if(CameraOpenNI2::exposureGainAvailable())
-				{
-					((CameraOpenNI2*)_camera->camera())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
-					((CameraOpenNI2*)_camera->camera())->setGain(_preferencesDialog->getSourceOpenni2Gain());
-				}
-			}
-			if(_camera)
-			{
-				_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
-				_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
-			}
 		}
 		if(_dbReader)
 		{
@@ -2557,7 +2555,7 @@ void MainWindow::changeMappingMode()
 	emit mappingModeChanged(_ui->actionSLAM_mode->isChecked());
 }
 
-void MainWindow::captureScreen()
+QString MainWindow::captureScreen()
 {
 	QString targetDir = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "ScreensCaptured";
 	QDir dir;
@@ -2579,6 +2577,8 @@ void MainWindow::captureScreen()
 	QString msg = tr("Screen captured \"%1\"").arg(targetDir + name);
 	_ui->statusbar->showMessage(msg, _preferencesDialog->getTimeLimit()*500);
 	_ui->widget_console->appendMsg(msg);
+
+	return targetDir + name;
 }
 
 void MainWindow::beep()
@@ -2652,7 +2652,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);
 }
 
@@ -2761,6 +2761,9 @@ void MainWindow::startDetection()
 	// verify source with input rates
 	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
 	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
+	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages ||
+	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
+	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
 	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
 	{
 		float inputRate = _preferencesDialog->getGeneralInputRate();
@@ -3121,57 +3124,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);
+	bool ok;
+	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID (0=full map)"), 0, 0, 999999, 0, &ok);
 	if(ok)
 	{
-				loopId = val;
-			}
-			ok = false;
-		}
-	}
-	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), loopId, 1, 999999, 1, &ok);
-	if(ok)
+		int margin = 0;
+		if(id > 0)
 		{
-		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+			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));
@@ -3180,13 +3156,21 @@ void MainWindow::generateLocalMap()
 	}
 }
 
-void MainWindow::generateTOROMap()
+void MainWindow::exportPosesKITTI()
 {
-	if(_toroSavingFileName.isEmpty())
-	{
-		_toroSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "toro.graph";
-	}
+	exportPoses(0);
+}
+void MainWindow::exportPosesRGBDSLAM()
+{
+	exportPoses(1);
+}
+void MainWindow::exportPosesTORO()
+{
+	exportPoses(2);
+}
 
+void MainWindow::exportPoses(int format)
+{
 	QStringList items;
 	items.append("Local map optimized");
 	items.append("Local map not optimized");
@@ -3219,82 +3203,30 @@ void MainWindow::generateTOROMap()
 			UFATAL("Item \"%s\" not found?!?", item.toStdString().c_str());
 		}
 
-		QString path = QFileDialog::getSaveFileName(this, tr("Save File"), _toroSavingFileName, tr("TORO file (*.graph)"));
+		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())
 		{
-			_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));
-			}
+			_exportPosesFileName[format] = path;
+
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdExportPoses, global, optimized, path.toStdString(), format));
 
 			_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));
-		}
-
-	}
-}
-
-void MainWindow::exportPoses()
-{
-	if(_posesSavingFileName.isEmpty())
-	{
-		_posesSavingFileName = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "poses.txt";
-	}
-
-	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"), _posesSavingFileName, 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));
-			}
-
-			_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]));
 		}
 
 	}
@@ -3902,7 +3834,7 @@ void MainWindow::sendGoal()
 	{
 		_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
 		UINFO("Posting event with goal %d", id);
-		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", id));
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
 	}
 }
 
@@ -3963,14 +3895,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));
 	}
 }
 
@@ -4014,14 +3939,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));
 	}
 }
 
@@ -4189,7 +4108,7 @@ void MainWindow::selectScreenCaptureFormat(bool checked)
 
 void MainWindow::takeScreenshot()
 {
-	this->captureScreen();
+	QDesktopServices::openUrl(QUrl::fromLocalFile(this->captureScreen()));
 }
 
 void MainWindow::setAspectRatio(int w, int h)
@@ -5267,9 +5186,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->menuAction()->setVisible(!monitoring);
 	_ui->actionOpen_working_directory->setVisible(!monitoring);
 	_ui->actionData_recorder->setVisible(!monitoring);
 	_ui->menuSelect_source->menuAction()->setVisible(!monitoring);
@@ -5351,9 +5268,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);
@@ -5399,9 +5314,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);
@@ -5436,9 +5349,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);
@@ -5475,9 +5386,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;
@@ -5504,9 +5413,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;
@@ -5541,7 +5448,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_state = newState;
 		_elapsedTime->start();
 		_oneSecondTimer->start();
-		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 0));
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResume));
 		break;
 	case kMonitoringPaused:
 		_ui->actionPause->setToolTip(tr("Continue"));
@@ -5559,7 +5466,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->statusbar->showMessage(tr("Monitoring paused..."));
 		_state = newState;
 		_oneSecondTimer->stop();
-		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 1));
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause));
 		break;
 	default:
 		break;

guilib/src/OdometryViewer.cpp

@@ -265,7 +265,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(odom.info().localMap.size());
 		int i=0;
-		for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
+		for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
 		{
 			(*cloud)[i].x = iter->second.x;
 			(*cloud)[i].y = iter->second.y;

guilib/src/PreferencesDialog.cpp

@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "GraphViewer.h"
 #include "ExportCloudsDialog.h"
 #include "PostProcessingDialog.h"
+#include "CreateSimpleCalibrationDialog.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -366,16 +367,32 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->openni2_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
+	connect(_ui->toolButton_cameraRGBDImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesStamps()));
+	connect(_ui->lineEdit_cameraRGBDImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->toolButton_cameraRGBDImages_path_rgb, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathRGB()));
+	connect(_ui->toolButton_cameraRGBDImages_path_depth, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathDepth()));
+	connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_RGBDImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+
 	connect(_ui->toolButton_cameraStereoImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesStamps()));
 	connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->toolButton_cameraStereoImages_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPath()));
-	connect(_ui->lineEdit_cameraStereoImages_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->toolButton_cameraStereoImages_path_left, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathLeft()));
+	connect(_ui->toolButton_cameraStereoImages_path_right, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathRight()));
+	connect(_ui->lineEdit_cameraStereoImages_path_left, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraStereoImages_path_right, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_stereoImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
 	connect(_ui->toolButton_cameraStereoVideo_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath()));
 	connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
+	connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
 	connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
 	connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path()));
 	connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1094,10 +1111,17 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_freenect2Format->setCurrentIndex(0);
 		_ui->lineEdit_openniOniPath->clear();
 		_ui->lineEdit_openni2OniPath->clear();
+		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
+		_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
+		_ui->checkBox_RGBDImages_timestamps->setChecked(false);
+		_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
+		_ui->lineEdit_cameraRGBDImages_timestamps->setText("");
 
 		_ui->source_comboBox_image_type->setCurrentIndex(kSrcDC1394-kSrcDC1394);
 		_ui->lineEdit_cameraStereoImages_timestamps->setText("");
-		_ui->lineEdit_cameraStereoImages_path->setText("");
+		_ui->lineEdit_cameraStereoImages_path_left->setText("");
+		_ui->lineEdit_cameraStereoImages_path_right->setText("");
+		_ui->checkBox_stereoImages_timestamps->setChecked(false);
 		_ui->checkBox_stereoImages_rectify->setChecked(false);
 		_ui->lineEdit_cameraStereoVideo_path->setText("");
 		_ui->checkBox_stereoVideo_rectify->setChecked(false);
@@ -1363,9 +1387,19 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_freenect2Format->setCurrentIndex(settings.value("format", _ui->comboBox_freenect2Format->currentIndex()).toInt());
 	settings.endGroup(); // Freenect2
 
+	settings.beginGroup("RGBDImages");
+	_ui->lineEdit_cameraRGBDImages_path_rgb->setText(settings.value("path_rgb", _ui->lineEdit_cameraRGBDImages_path_rgb->text()).toString());
+	_ui->lineEdit_cameraRGBDImages_path_depth->setText(settings.value("path_depth", _ui->lineEdit_cameraRGBDImages_path_depth->text()).toString());
+	_ui->checkBox_RGBDImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_RGBDImages_timestamps->isChecked()).toBool());
+	_ui->lineEdit_cameraRGBDImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraRGBDImages_timestamps->text()).toString());
+	_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(settings.value("scale", _ui->doubleSpinBox_cameraRGBDImages_scale->value()).toDouble());
+	settings.endGroup(); // RGBDImages
+
 	settings.beginGroup("StereoImages");
 	_ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString());
-	_ui->lineEdit_cameraStereoImages_path->setText(settings.value("path", _ui->lineEdit_cameraStereoImages_path->text()).toString());
+	_ui->lineEdit_cameraStereoImages_path_left->setText(settings.value("path_left", _ui->lineEdit_cameraStereoImages_path_left->text()).toString());
+	_ui->lineEdit_cameraStereoImages_path_right->setText(settings.value("path_right", _ui->lineEdit_cameraStereoImages_path_right->text()).toString());
+	_ui->checkBox_stereoImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_stereoImages_timestamps->isChecked()).toBool());
 	_ui->checkBox_stereoImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoImages_rectify->isChecked()).toBool());
 	settings.endGroup(); // StereoImages
 
@@ -1663,9 +1697,19 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("format",         _ui->comboBox_freenect2Format->currentIndex());
 	settings.endGroup(); // Freenect2
 
+	settings.beginGroup("RGBDImages");
+	settings.setValue("path_rgb",            _ui->lineEdit_cameraRGBDImages_path_rgb->text());
+	settings.setValue("path_depth",          _ui->lineEdit_cameraRGBDImages_path_depth->text());
+	settings.setValue("filenames_as_stamps", _ui->checkBox_RGBDImages_timestamps->isChecked());
+	settings.setValue("stamps",              _ui->lineEdit_cameraRGBDImages_timestamps->text());
+	settings.setValue("scale",               _ui->doubleSpinBox_cameraRGBDImages_scale->value());
+	settings.endGroup(); // RGBDImages
+
 	settings.beginGroup("StereoImages");
 	settings.setValue("stamps",         _ui->lineEdit_cameraStereoImages_timestamps->text());
-	settings.setValue("path", _ui->lineEdit_cameraStereoImages_path->text());
+	settings.setValue("path_left",      _ui->lineEdit_cameraStereoImages_path_left->text());
+	settings.setValue("path_right",     _ui->lineEdit_cameraStereoImages_path_right->text());
+	settings.setValue("filenames_as_stamps", _ui->checkBox_stereoImages_timestamps->isChecked());
 	settings.setValue("rectify", 	    _ui->checkBox_stereoImages_rectify->isChecked());
 	settings.endGroup(); // StereoImages
 
@@ -2202,6 +2246,48 @@ void PreferencesDialog::openDatabaseViewer()
 	}
 }
 
+void PreferencesDialog::selectSourceRGBDImagesStamps()
+{
+	QString dir = _ui->lineEdit_cameraRGBDImages_timestamps->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)"));
+	if(path.size())
+	{
+		_ui->lineEdit_cameraRGBDImages_timestamps->setText(path);
+	}
+}
+
+void PreferencesDialog::selectSourceRGBDImagesPathRGB()
+{
+	QString dir = _ui->lineEdit_cameraRGBDImages_path_rgb->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select RGB images directory"), dir);
+	if(path.size())
+	{
+		_ui->lineEdit_cameraRGBDImages_path_rgb->setText(path);
+	}
+}
+
+void PreferencesDialog::selectSourceRGBDImagesPathDepth()
+{
+	QString dir = _ui->lineEdit_cameraRGBDImages_path_depth->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select depth images directory"), dir);
+	if(path.size())
+	{
+		_ui->lineEdit_cameraRGBDImages_path_depth->setText(path);
+	}
+}
+
 void PreferencesDialog::selectSourceStereoImagesStamps()
 {
 	QString dir = _ui->lineEdit_cameraStereoImages_timestamps->text();
@@ -2216,17 +2302,31 @@ void PreferencesDialog::selectSourceStereoImagesStamps()
 	}
 }
 
-void PreferencesDialog::selectSourceStereoImagesPath()
+void PreferencesDialog::selectSourceStereoImagesPathLeft()
 {
-	QString dir = _ui->lineEdit_cameraStereoImages_path->text();
+	QString dir = _ui->lineEdit_cameraStereoImages_path_left->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getExistingDirectory(this, tr("Select stereo images directory"), dir);
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select left images directory"), dir);
 	if(path.size())
 	{
-		_ui->lineEdit_cameraStereoImages_path->setText(path);
+		_ui->lineEdit_cameraStereoImages_path_left->setText(path);
+	}
+}
+
+void PreferencesDialog::selectSourceStereoImagesPathRight()
+{
+	QString dir = _ui->lineEdit_cameraStereoImages_path_right->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getExistingDirectory(this, tr("Select right images directory"), dir);
+	if(path.size())
+	{
+		_ui->lineEdit_cameraStereoImages_path_right->setText(path);
 	}
 }
 
@@ -3315,30 +3415,6 @@ Transform PreferencesDialog::getSourceLocalTransform() const
 	return t;
 }
 
-QString PreferencesDialog::getSourceImagesPath() const
-{
-	return _ui->source_images_lineEdit_path->text();
-}
-int PreferencesDialog::getSourceImagesStartPos() const
-{
-	return _ui->source_images_spinBox_startPos->value();
-}
-bool PreferencesDialog::getSourceImagesRefreshDir() const
-{
-	return _ui->source_images_refreshDir->isChecked();
-}
-bool PreferencesDialog::getSourceImagesRectify() const
-{
-	return _ui->checkBox_rgbImages_rectify->isChecked();
-}
-QString PreferencesDialog::getSourceVideoPath() const
-{
-	return _ui->source_video_lineEdit_path->text();
-}
-bool PreferencesDialog::getSourceVideoRectify() const
-{
-	return _ui->checkBox_rgbVideo_rectify->isChecked();
-}
 QString PreferencesDialog::getSourceDatabasePath() const
 {
 	return _ui->source_database_lineEdit_path->text();
@@ -3359,46 +3435,11 @@ bool PreferencesDialog::getSourceDatabaseStampsUsed() const
 {
 	return _ui->source_checkBox_useDbStamps->isChecked();
 }
-
-bool PreferencesDialog::getSourceOpenni2AutoWhiteBalance() const
-{
-	return _ui->openni2_autoWhiteBalance->isChecked();
-}
-bool PreferencesDialog::getSourceOpenni2AutoExposure() const
-{
-	return _ui->openni2_autoExposure->isChecked();
-}
-int PreferencesDialog::getSourceOpenni2Exposure() const
-{
-	return _ui->openni2_exposure->value();
-}
-int PreferencesDialog::getSourceOpenni2Gain() const
-{
-	return _ui->openni2_gain->value();
-}
-bool PreferencesDialog::getSourceOpenni2Mirroring() const
-{
-	return _ui->openni2_mirroring->isChecked();
-}
-int PreferencesDialog::getSourceFreenect2Format() const
-{
-	return _ui->comboBox_freenect2Format->currentIndex();
-}
-bool PreferencesDialog::getSourceStereoImagesRectify() const
-{
-	return _ui->checkBox_stereoImages_rectify->isChecked();
-}
-bool PreferencesDialog::getSourceStereoVideoRectify() const
-{
-	return _ui->checkBox_stereoVideo_rectify->isChecked();
-}
-
 bool PreferencesDialog::isSourceRGBDColorOnly() const
 {
 	return _ui->checkbox_rgbd_colorOnly->isChecked();
 }
 
-
 Camera * PreferencesDialog::createCamera(bool useRawImages)
 {
 	Src driver = this->getSourceDriver();
@@ -3476,7 +3517,18 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	{
 		camera = new CameraFreenect2(
 			this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
-			useRawImages?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)getSourceFreenect2Format(),
+			useRawImages?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)_ui->comboBox_freenect2Format->currentIndex(),
+			this->getGeneralInputRate(),
+			this->getSourceLocalTransform());
+	}
+	else if(driver == kSrcRGBDImages)
+	{
+		camera = new CameraRGBDImages(
+			_ui->lineEdit_cameraRGBDImages_path_rgb->text().append(QDir::separator()).toStdString(),
+			_ui->lineEdit_cameraRGBDImages_path_depth->text().append(QDir::separator()).toStdString(),
+			_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
+			_ui->checkBox_RGBDImages_timestamps->isChecked(),
+			_ui->lineEdit_cameraRGBDImages_timestamps->text().toStdString(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
@@ -3505,7 +3557,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	else if(driver == kSrcStereoImages)
 	{
 		camera = new CameraStereoImages(
-			_ui->lineEdit_cameraStereoImages_path->text().append(QDir::separator()).toStdString(),
+			_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
+			_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
+			_ui->checkBox_stereoImages_timestamps->isChecked(),
 			_ui->lineEdit_cameraStereoImages_timestamps->text().toStdString(),
 			_ui->checkBox_stereoImages_rectify->isChecked(),
 			this->getGeneralInputRate(),
@@ -3529,18 +3583,19 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 	else if(driver == kSrcVideo)
 	{
 		camera = new CameraVideo(
-			this->getSourceVideoPath().toStdString(),
-			this->getSourceVideoRectify(),
+			_ui->source_video_lineEdit_path->text().toStdString(),
+			_ui->checkBox_rgbVideo_rectify->isChecked(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
 	else if(driver == kSrcImages)
 	{
 		camera = new CameraImages(
-			this->getSourceImagesPath().toStdString(),
-			this->getSourceImagesStartPos(),
-			this->getSourceImagesRefreshDir(),
-			this->getSourceVideoRectify(),
+			_ui->source_images_lineEdit_path->text().toStdString(),
+			_ui->source_images_spinBox_startPos->value(),
+			_ui->source_images_refreshDir->isChecked(),
+			_ui->checkBox_rgbImages_rectify->isChecked(),
+			false,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
@@ -3571,13 +3626,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			//should be after initialization
 			if(driver == kSrcOpenNI2)
 			{
-				((CameraOpenNI2*)camera)->setAutoWhiteBalance(this->getSourceOpenni2AutoWhiteBalance());
-				((CameraOpenNI2*)camera)->setAutoExposure(this->getSourceOpenni2AutoExposure());
-				((CameraOpenNI2*)camera)->setMirroring(this->getSourceOpenni2Mirroring());
+				((CameraOpenNI2*)camera)->setAutoWhiteBalance(_ui->openni2_autoWhiteBalance->isChecked());
+				((CameraOpenNI2*)camera)->setAutoExposure(_ui->openni2_autoExposure->isChecked());
+				((CameraOpenNI2*)camera)->setMirroring(_ui->openni2_mirroring->isChecked());
 				if(CameraOpenNI2::exposureGainAvailable())
 				{
-					((CameraOpenNI2*)camera)->setExposure(this->getSourceOpenni2Exposure());
-					((CameraOpenNI2*)camera)->setGain(this->getSourceOpenni2Gain());
+					((CameraOpenNI2*)camera)->setExposure(_ui->openni2_exposure->value());
+					((CameraOpenNI2*)camera)->setGain(_ui->openni2_gain->value());
 				}
 			}
 		}
@@ -3705,8 +3760,8 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testOdometry(int type)
 {
-	DBReader dbReader(this->getSourceDatabasePath().toStdString(),
-					 this->getSourceDatabaseStampsUsed()?-1:this->getGeneralInputRate(),
+	DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
+					 _ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
 					 true,
 					 true);
 	Camera * camera = 0;
@@ -3762,7 +3817,7 @@ void PreferencesDialog::testOdometry(int type)
 	{
 		CameraThread cameraThread(camera); // take ownership of camera
 		cameraThread.setMirroringEnabled(isSourceMirroring());
-		cameraThread.setColorOnly(isSourceRGBDColorOnly());
+		cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 		UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
 		UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
 		UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
@@ -3802,8 +3857,8 @@ void PreferencesDialog::testCamera()
 
 	if(this->getSourceType() == kSrcDatabase)
 	{
-		DBReader dbReader(this->getSourceDatabasePath().toStdString(),
-						 this->getSourceDatabaseStampsUsed()?-1:this->getGeneralInputRate(),
+		DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
+						  _ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
 						 true,
 						 true);
 		if(!dbReader.init())
@@ -3828,7 +3883,7 @@ void PreferencesDialog::testCamera()
 		{
 			CameraThread cameraThread(camera);
 			cameraThread.setMirroringEnabled(isSourceMirroring());
-			cameraThread.setColorOnly(isSourceRGBDColorOnly());
+			cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 			UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
 
 			cameraThread.start();
@@ -3887,5 +3942,11 @@ void PreferencesDialog::calibrate()
 	cameraThread.join(true);
 }
 
+void PreferencesDialog::calibrateSimple()
+{
+	CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), _ui->lineEdit_calibrationName->text(), this);
+	dialog.exec();
+}
+
 }
 

guilib/src/ui/createSimpleCalibrationDialog.ui

@@ -0,0 +1,156 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>createSimpleCalibrationDialog</class>
+ <widget class="QDialog" name="createSimpleCalibrationDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>306</width>
+    <height>212</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Create simple calibration</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <layout class="QFormLayout" name="formLayout">
+     <item row="0" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_fx">
+       <property name="decimals">
+        <number>4</number>
+       </property>
+       <property name="maximum">
+        <double>99999.000000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="0" column="1">
+      <widget class="QLabel" name="label">
+       <property name="text">
+        <string>fx</string>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_fy">
+       <property name="decimals">
+        <number>4</number>
+       </property>
+       <property name="maximum">
+        <double>99999.000000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="1" column="1">
+      <widget class="QLabel" name="label_2">
+       <property name="text">
+        <string>fy</string>
+       </property>
+      </widget>
+     </item>
+     <item row="2" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_cx">
+       <property name="decimals">
+        <number>4</number>
+       </property>
+       <property name="maximum">
+        <double>99999.000000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="2" column="1">
+      <widget class="QLabel" name="label_3">
+       <property name="text">
+        <string>cx</string>
+       </property>
+      </widget>
+     </item>
+     <item row="3" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_cy">
+       <property name="decimals">
+        <number>4</number>
+       </property>
+       <property name="maximum">
+        <double>99999.000000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="3" column="1">
+      <widget class="QLabel" name="label_4">
+       <property name="text">
+        <string>cy</string>
+       </property>
+      </widget>
+     </item>
+     <item row="4" column="0">
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_baseline">
+       <property name="decimals">
+        <number>4</number>
+       </property>
+       <property name="minimum">
+        <double>0.000000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>99999.000000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item row="4" column="1">
+      <widget class="QLabel" name="label_5">
+       <property name="text">
+        <string>baseline (only for stereo)</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Save</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>accepted()</signal>
+   <receiver>createSimpleCalibrationDialog</receiver>
+   <slot>accept()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>248</x>
+     <y>254</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>157</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>rejected()</signal>
+   <receiver>createSimpleCalibrationDialog</receiver>
+   <slot>reject()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>316</x>
+     <y>260</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>286</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+ </connections>
+</ui>

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>-625</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>1</number>
+                <number>4</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1651,7 +1651,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </item>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                        <widget class="QPushButton" name="pushButton_test_camera">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -1697,6 +1697,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="0">
+                       <widget class="QPushButton" name="pushButton_calibrate_simple">
+                        <property name="sizePolicy">
+                         <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+                          <horstretch>0</horstretch>
+                          <verstretch>0</verstretch>
+                         </sizepolicy>
+                        </property>
+                        <property name="text">
+                         <string>Simple calibration</string>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_244">
+                        <property name="text">
+                         <string>Create a simple calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of a source of rectified or registered images.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -1765,6 +1791,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>Freenect2</string>
                                 </property>
                                </item>
+                               <item>
+                                <property name="text">
+                                 <string>Images</string>
+                                </property>
+                               </item>
                               </widget>
                              </item>
                              <item row="0" column="2">
@@ -1802,7 +1833,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>4</number>
+                              <number>6</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -2086,6 +2117,165 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                               </layout>
                              </widget>
+                             <widget class="QWidget" name="page_47">
+                              <layout class="QVBoxLayout" name="verticalLayout_79">
+                               <item>
+                                <widget class="QGroupBox" name="groupBox_cameraStereoImages_3">
+                                 <property name="sizePolicy">
+                                  <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
+                                   <horstretch>0</horstretch>
+                                   <verstretch>0</verstretch>
+                                  </sizepolicy>
+                                 </property>
+                                 <property name="title">
+                                  <string>RGB-D Images</string>
+                                 </property>
+                                 <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,0,1">
+                                  <item row="0" column="2">
+                                   <widget class="QLabel" name="label_252">
+                                    <property name="text">
+                                     <string>Path to directory containing RGB images.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="2">
+                                   <widget class="QLabel" name="label_251">
+                                    <property name="text">
+                                     <string>Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use RGB file names as timestamps&quot; above is checked. </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_path_depth">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_path_depth">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="1">
+                                   <widget class="QCheckBox" name="checkBox_RGBDImages_timestamps">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="2">
+                                   <widget class="QLabel" name="label_255">
+                                    <property name="text">
+                                     <string>Use RGB file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_timestamps">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="0" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraRGBDImages_path_rgb">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="0">
+                                   <spacer name="verticalSpacer_40">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                  <item row="0" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_path_rgb">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraRGBDImages_timestamps">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="2">
+                                   <widget class="QLabel" name="label_254">
+                                    <property name="text">
+                                     <string>Path to directory containing depth images. The directory should have the same size has the RGB directory. The depth images should be already registered to RGB images. Assume that UINT16 images are in mm and FLOAT32 images are in m.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="2">
+                                   <widget class="QLabel" name="label_257">
+                                    <property name="text">
+                                     <string>Depth scale factor (depth = pixel value / factor).</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="1">
+                                   <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraRGBDImages_scale">
+                                    <property name="decimals">
+                                     <number>1</number>
+                                    </property>
+                                    <property name="minimum">
+                                     <double>1.000000000000000</double>
+                                    </property>
+                                    <property name="maximum">
+                                     <double>999999.000000000000000</double>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                 </layout>
+                                </widget>
+                               </item>
+                              </layout>
+                             </widget>
                             </widget>
                            </item>
                           </layout>
@@ -2179,14 +2369,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>Stereo Images</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_61" columnstretch="0,0,1">
-                                  <item row="1" column="1">
+                                  <item row="3" column="1">
                                    <widget class="QLineEdit" name="lineEdit_cameraStereoImages_timestamps">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="1" column="0">
+                                  <item row="3" column="0">
                                    <widget class="QToolButton" name="toolButton_cameraStereoImages_timestamps">
                                     <property name="text">
                                      <string>...</string>
@@ -2194,20 +2384,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </widget>
                                   </item>
                                   <item row="0" column="1">
-                                   <widget class="QLineEdit" name="lineEdit_cameraStereoImages_path">
+                                   <widget class="QLineEdit" name="lineEdit_cameraStereoImages_path_left">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
                                   <item row="0" column="0">
-                                   <widget class="QToolButton" name="toolButton_cameraStereoImages_path">
+                                   <widget class="QToolButton" name="toolButton_cameraStereoImages_path_left">
                                     <property name="text">
                                      <string>...</string>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="0">
+                                  <item row="5" column="0">
                                    <spacer name="verticalSpacer_38">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -2220,10 +2410,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </spacer>
                                   </item>
-                                  <item row="1" column="2">
+                                  <item row="3" column="2">
                                    <widget class="QLabel" name="label_248">
                                     <property name="text">
-                                     <string>Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. </string>
+                                     <string>Optional timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use left file names as timestamps&quot; above is checked.  </string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -2236,7 +2426,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <item row="0" column="2">
                                    <widget class="QLabel" name="label_249">
                                     <property name="text">
-                                     <string>Path to directory containing stereo images. The images order should be left/right/left/right... and so on. You can also set two directories (separated by ';'), one for left images and one for right images.</string>
+                                     <string>Path to directory containing left images.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -2246,7 +2436,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="2">
+                                  <item row="4" column="2">
                                    <widget class="QLabel" name="label_250">
                                     <property name="text">
                                      <string>Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified.</string>
@@ -2259,13 +2449,60 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="1">
+                                  <item row="4" column="1">
                                    <widget class="QCheckBox" name="checkBox_stereoImages_rectify">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="1" column="2">
+                                   <widget class="QLabel" name="label_253">
+                                    <property name="text">
+                                     <string>Path to directory containing right images. The directory should have the same size has the left images directory. </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="1">
+                                   <widget class="QLineEdit" name="lineEdit_cameraStereoImages_path_right">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="0">
+                                   <widget class="QToolButton" name="toolButton_cameraStereoImages_path_right">
+                                    <property name="text">
+                                     <string>...</string>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="1">
+                                   <widget class="QCheckBox" name="checkBox_stereoImages_timestamps">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="2">
+                                   <widget class="QLabel" name="label_256">
+                                    <property name="text">
+                                     <string>Use left file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -2759,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>
@@ -3097,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>

package.xml

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

tools/Camera/main.cpp

@@ -149,7 +149,7 @@ int main(int argc, char * argv[])
 		}
 		else if(UDirectory::exists(path))
 		{
-			camera = new rtabmap::CameraImages(path, rate);
+			camera = new rtabmap::CameraImages(path, 1, false, false, false, rate);
 		}
 		else
 		{

tools/ConsoleApp/main.cpp

@@ -302,7 +302,7 @@ int main(int argc, char * argv[])
 	Camera * camera = 0;
 	if(UDirectory::exists(path))
 	{
-		camera = new CameraImages(path, startAt, false, false, 1.0f/rate);
+		camera = new CameraImages(path, startAt, false, false, false, 1.0f/rate);
 	}
 	else
 	{

tools/OdometryViewer/main.cpp

@@ -807,6 +807,7 @@ int main (int argc, char * argv[])
 			if(camera->isCalibrated())
 			{
 				rtabmap::CameraThread cameraThread(camera);
+				cameraThread.setColorOnly(true);
 
 				odomThread.start();
 				cameraThread.start();

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

@@ -470,6 +470,15 @@ inline std::list<V> uVectorToList(const std::vector<V> & v)
 	return std::list<V>(v.begin(), v.end());
 }
 
+/**
+ * Convert a std::multimap to a std::map
+ */
+template<class K, class V>
+inline std::map<K, V> uMultimapToMap(const std::multimap<K, V> & m)
+{
+	return std::map<K, V>(m.begin(), m.end());
+}
+
 /**
  * Append a list to another list.
  * @param list the list on which the other list will be appended
@@ -539,6 +548,34 @@ inline std::list<std::string> uSplit(const std::string & str, char separator = '
 	return v;
 }
 
+/**
+ * Join multiple strings into one string with optional separator.
+ * Example:
+ * @code
+ *      std::list<std::string> v;
+ *      v.push_back("Hello");
+ *      v.push_back("world!");
+ * 	    std::string joined = split(v, " ");
+ * @endcode
+ * The output string is "Hello world!"
+ * @param strings a list of strings
+ * @param separator the separator string
+ * @return the joined string
+ */
+inline std::string uJoin(const std::list<std::string> & strings, const std::string & separator = "")
+{
+	std::string out;
+	for(std::list<std::string>::const_iterator iter = strings.begin(); iter!=strings.end(); ++iter)
+	{
+		if(iter!=strings.begin() && !separator.empty())
+		{
+			out += separator;
+		}
+		out+=*iter;
+	}
+	return out;
+}
+
 /**
  * Check if a character is a digit.
  * @param c the character

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,643 @@
+/*
+*  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"
+#include <limits>
+#include <string.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 <= (unsigned int)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 <= (unsigned int)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 <= (unsigned int)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;
+}