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 save(const std::string & filePath) const;
+	bool load(const std::string & directory, const std::string & cameraName);
+	bool save(const std::string & directory) const;
 
 	void scale(double scale);
 
@@ -143,13 +154,29 @@ public:
 		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
 	{
 	}
+	//minimal to be saved
+	StereoCameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(name+"_left", fx, fy, cx, cy, localTransform),
+		right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
+		name_(name)
+	{
+	}
 	virtual ~StereoCameraModel() {}
 
 	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
+
+	void setName(const std::string & name);
 	const std::string & name() const {return name_;}
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
-	bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true) const;
+	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
 
 	double baseline() const {return -right_.Tx()/right_.fx();}
 

corelib/include/rtabmap/core/CameraRGB.h

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

corelib/include/rtabmap/core/CameraRGBD.h

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

corelib/include/rtabmap/core/CameraStereo.h

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

corelib/include/rtabmap/core/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);