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

2026-09-02 01:20:25 +08:00 · 2015-07-30 14:17:29 -04:00
parent 38807bf12e
commit 2877a14360
24 changed files with 1315 additions and 237 deletions
--- a/corelib/include/rtabmap/core/CameraModel.h
+++ b/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();}

--- a/corelib/include/rtabmap/core/CameraRGB.h
+++ b/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;
--- a/corelib/include/rtabmap/core/CameraRGBD.h
+++ b/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
--- a/corelib/include/rtabmap/core/CameraStereo.h
+++ b/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_;
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/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);
--- a/corelib/src/CameraModel.cpp
+++ b/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();

-		// init rectification map
-		UINFO("Initialize rectify map");
-		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+		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());
--- a/corelib/src/CameraRGB.cpp
+++ b/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,24 +208,37 @@ 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 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)
+					if(_isDepth)
 					{
-						// The depth should be 8U
-						UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
-						IplImage * i = cvLoadImage(fullPath.c_str());
-						img = cv::Mat(i, true);
-						cvReleaseImage(&i);
+						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)
+						{
+							// The depth should be 8U
+							UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
+							IplImage * i = cvLoadImage(fullPath.c_str());
+							img = cv::Mat(i, true);
+							cvReleaseImage(&i);
+						}
 #endif

-					if(img.channels()>3)
-					{
-						UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
-						cv::Mat out;
-						cv::cvtColor(img, out, CV_BGRA2BGR);
-						img = out;
+						if(img.channels()>3)
+						{
+							UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
+							cv::Mat out;
+							cv::cvtColor(img, out, CV_BGRA2BGR);
+							img = out;
+						}
 					}
 				}
 			}
@@ -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());
--- a/corelib/src/CameraRGBD.cpp
+++ b/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
--- a/corelib/src/CameraStereo.cpp
+++ b/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,30 +851,63 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	}

 	stamps_.clear();
-	if(success && timestampsPath_.size())
+	if(success)
 	{
-		FILE * file = 0;
-#ifdef _MSC_VER
-		fopen_s(&file, timestampsPath_.c_str(), "r");
-#else
-		file = fopen(timestampsPath_.c_str(), "r");
-#endif
-		if(file)
+		if(filenamesAreTimestamps_)
 		{
-			char line[16];
-			while ( fgets (line , 16 , file) != NULL )
+			std::vector<std::string> filenames = camera_->filenames();
+			for(unsigned int i=0; i<filenames.size(); ++i)
 			{
-				stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
+				// 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;
 			}
-			fclose(file);
 		}
-		if(stamps_.size() != camera_->imagesCount())
+		else if(timestampsPath_.size())
 		{
-			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(), camera_->imagesCount(), timestampsPath_.c_str());
-			stamps_.clear();
-			success = false;
+			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() != camera_->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(), camera_->imagesCount(), timestampsPath_.c_str());
+				stamps_.clear();
+				success = false;
+			}
 		}
 	}

--- a/corelib/src/Rtabmap.cpp
+++ b/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]);
-			for(int i=1; i<(*iter).second.size(); i++)
+			int index = 0;
+			if(stamps.size() == poses.size())
+			{
+				UASSERT(uContains(stamps, iter->first));
+				fprintf(fout, "%f", stamps.at(iter->first));
+			}
+			else
+			{
+				fprintf(fout, "%f", p[index++]);
+			}
+			for(int i=index; i<(*iter).second.size(); i++)
 			{
 				fprintf(fout, " %f", p[i]);
 			}