Added CameraRGBDImages class (read RGB-D images from a folder)

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 load(const std::string & directory, const std::string & cameraName);
-	bool save(const std::string & filePath) const;
+	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/Rtabmap.h

@@ -113,7 +113,10 @@ public:
 	void resetMemory();
 	void dumpPrediction() const;
 	void dumpData() const;
-	void dumpPoses(const std::string & path, const std::map<int, Transform> & poses) const;
+	void dumpPoses(
+			const std::string & path,
+			const std::map<int, Transform> & poses,
+			const std::map<int, double> & stamps = std::map<int, double>()) const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
 	void rejectLoopClosure(int oldId, int newId);

corelib/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_.width > 0);
-		UASSERT(imageSize_.height > 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/Rtabmap.cpp

@@ -756,7 +756,19 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global)
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
-		this->dumpPoses(path, poses);
+		//get timestamps
+		std::map<int, double> stamps;
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			Transform o;
+			int m, w;
+			std::string l;
+			double stamp = 0.0;
+			_memory->getNodeInfo(iter->first, o, m, w, l, stamp, true);
+			stamps.insert(std::make_pair(iter->first, stamp));
+		}
+
+		this->dumpPoses(path, poses, stamps);
 	}
 }
 
@@ -2466,9 +2478,11 @@ void Rtabmap::dumpData() const
 
 void Rtabmap::dumpPoses(
 		const std::string & path,
-		const std::map<int, Transform> & poses) const
+		const std::map<int, Transform> & poses,
+		const std::map<int, double> & stamps) const
 {
 	UDEBUG("");
+	UASSERT(stamps.size()== 0 || stamps.size() == poses.size());
 	FILE* fout = 0;
 #ifdef _MSC_VER
 	fopen_s(&fout, path.c_str(), "w");
@@ -2482,8 +2496,17 @@ void Rtabmap::dumpPoses(
 			// in camera frame
 			const float * p = (const float *)(*iter).second.data();
 
-			fprintf(fout, "%f", p[0]);
+			int index = 0;
-			for(int i=1; i<(*iter).second.size(); i++)
+			if(stamps.size() == poses.size())
+			{
+				UASSERT(uContains(stamps, iter->first));
+				fprintf(fout, "%f", stamps.at(iter->first));
+			}
+			else
+			{
+				fprintf(fout, "%f", p[index++]);
+			}
+			for(int i=index; i<(*iter).second.size(); i++)
 			{
 				fprintf(fout, " %f", p[i]);
 			}

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

@@ -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());
 					}

guilib/src/MainWindow.cpp

@@ -2220,22 +2220,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)
 		{
@@ -2768,6 +2752,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();

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->toolButton_cameraStereoImages_path_left, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathLeft()));
-	connect(_ui->lineEdit_cameraStereoImages_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	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(),
+			_ui->source_video_lineEdit_path->text().toStdString(),
-			this->getSourceVideoRectify(),
+			_ui->checkBox_rgbVideo_rectify->isChecked(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
 	else if(driver == kSrcImages)
 	{
 		camera = new CameraImages(
-			this->getSourceImagesPath().toStdString(),
+			_ui->source_images_lineEdit_path->text().toStdString(),
-			this->getSourceImagesStartPos(),
+			_ui->source_images_spinBox_startPos->value(),
-			this->getSourceImagesRefreshDir(),
+			_ui->source_images_refreshDir->isChecked(),
-			this->getSourceVideoRectify(),
+			_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)->setAutoWhiteBalance(_ui->openni2_autoWhiteBalance->isChecked());
-				((CameraOpenNI2*)camera)->setAutoExposure(this->getSourceOpenni2AutoExposure());
+				((CameraOpenNI2*)camera)->setAutoExposure(_ui->openni2_autoExposure->isChecked());
-				((CameraOpenNI2*)camera)->setMirroring(this->getSourceOpenni2Mirroring());
+				((CameraOpenNI2*)camera)->setMirroring(_ui->openni2_mirroring->isChecked());
 				if(CameraOpenNI2::exposureGainAvailable())
 				{
-					((CameraOpenNI2*)camera)->setExposure(this->getSourceOpenni2Exposure());
+					((CameraOpenNI2*)camera)->setExposure(_ui->openni2_exposure->value());
-					((CameraOpenNI2*)camera)->setGain(this->getSourceOpenni2Gain());
+					((CameraOpenNI2*)camera)->setGain(_ui->openni2_gain->value());
 				}
 			}
 		}
@@ -3705,8 +3760,8 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testOdometry(int type)
 {
-	DBReader dbReader(this->getSourceDatabasePath().toStdString(),
+	DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
-					 this->getSourceDatabaseStampsUsed()?-1:this->getGeneralInputRate(),
+					 _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(),
+		DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
-						 this->getSourceDatabaseStampsUsed()?-1:this->getGeneralInputRate(),
+						  _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/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-625</y>
+            <y>-399</y>
             <width>760</width>
             <height>1570</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>3</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>

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
 	{

utilite/include/rtabmap/utilite/UStl.h

@@ -548,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