Refactored Camera classes and Preferences->Source menu

corelib/include/rtabmap/core/Camera.h

@@ -50,22 +50,20 @@ class RTABMAP_EXP Camera
 {
 public:
 	virtual ~Camera();
-	cv::Mat takeImage();
+	SensorData takeImage();
-	virtual bool init() = 0;
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	virtual bool isCalibrated() const = 0;
+	virtual std::string getSerial() const = 0;
+	int getNextSeqID() {return ++_seq;}
 
 	//getters
-	void getImageSize(unsigned int & width, unsigned int & height);
 	float getImageRate() const {return _imageRate;}
-	bool isMirroringEnabled() const {return _mirroring;}
+	const Transform & getLocalTransform() const {return _localTransform;}
 
 	//setters
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setImageSize(unsigned int width, unsigned int height);
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
-
-	void setCalibration(const std::string & fileName);
-	void setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients);
-	void resetCalibration();
 
 protected:
 	/**
@@ -73,95 +71,19 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0,
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
 
-	virtual cv::Mat captureImage() = 0;
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureImage() = 0;
 
 private:
 	float _imageRate;
-	unsigned int _imageWidth;
+	Transform _localTransform;
-	unsigned int _imageHeight;
+	cv::Size _targetImageSize;
-	bool _mirroring;
 	UTimer * _frameRateTimer;
-	cv::Mat _k; // camera_matrix
+	int _seq;
-	cv::Mat _d; // distorsion_coefficients
-};
-
-
-/////////////////////////
-// CameraImages
-/////////////////////////
-class RTABMAP_EXP CameraImages :
-	public Camera
-{
-public:
-	CameraImages(const std::string & path,
-			int startAt = 1,
-			bool refreshDir = false,
-			float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	virtual ~CameraImages();
-
-	virtual bool init();
-	std::string getPath() const {return _path;}
-	unsigned int imagesCount() const;
-
-protected:
-	virtual cv::Mat captureImage();
-
-private:
-	std::string _path;
-	int _startAt;
-	// If the list of files in the directory is refreshed
-	// on each call of takeImage()
-	bool _refreshDir;
-	int _count;
-	UDirectory * _dir;
-	std::string _lastFileName;
-};
-
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-class RTABMAP_EXP CameraVideo :
-	public Camera
-{
-public:
-	enum Source{kVideoFile, kUsbDevice};
-
-public:
-	CameraVideo(int usbDevice = 0,
-			float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	CameraVideo(const std::string & filePath,
-			float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	virtual ~CameraVideo();
-
-	virtual bool init();
-	int getUsbDevice() const {return _usbDevice;}
-	const std::string & getFilePath() const {return _filePath;}
-
-protected:
-	virtual cv::Mat captureImage();
-
-private:
-	// File type
-	std::string _filePath;
-
-	cv::VideoCapture _capture;
-	Source _src;
-
-	// Usb camera
-	int _usbDevice;
 };
 
 

corelib/include/rtabmap/core/CameraEvent.h

@@ -38,14 +38,13 @@ class CameraEvent :
 {
 public:
 	enum Code {
-		kCodeImage,
+		kCodeData,
-		kCodeImageDepth,
 		kCodeNoMoreImages
 	};
 
 public:
 	CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") :
-		UEvent(kCodeImage),
+		UEvent(kCodeData),
 		data_(image, seq, stamp),
 		cameraName_(cameraName)
 	{
@@ -57,7 +56,7 @@ public:
 	}
 
 	CameraEvent(const SensorData & data, const std::string & cameraName = "") :
-		UEvent(kCodeImageDepth),
+		UEvent(kCodeData),
 		data_(data),
 		cameraName_(cameraName)
 	{

corelib/include/rtabmap/core/CameraRGB.h

@@ -0,0 +1,124 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include "rtabmap/core/Camera.h"
+#include <set>
+#include <stack>
+#include <list>
+#include <vector>
+
+class UDirectory;
+class UTimer;
+
+namespace rtabmap
+{
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+class RTABMAP_EXP CameraImages :
+	public Camera
+{
+public:
+	CameraImages(const std::string & path,
+			int startAt = 1,
+			bool refreshDir = false,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	std::string getPath() const {return _path;}
+	unsigned int imagesCount() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	std::string _path;
+	int _startAt;
+	// If the list of files in the directory is refreshed
+	// on each call of takeImage()
+	bool _refreshDir;
+	int _count;
+	UDirectory * _dir;
+	std::string _lastFileName;
+};
+
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+class RTABMAP_EXP CameraVideo :
+	public Camera
+{
+public:
+	enum Source{kVideoFile, kUsbDevice};
+
+public:
+	CameraVideo(int usbDevice = 0,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraVideo(const std::string & filePath,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraVideo();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	int getUsbDevice() const {return _usbDevice;}
+	const std::string & getFilePath() const {return _filePath;}
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	// File type
+	std::string _filePath;
+
+	cv::VideoCapture _capture;
+	Source _src;
+
+	// Usb camera
+	int _usbDevice;
+	std::string _guid;
+
+	CameraModel _model;
+};
+
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraRGBD.h

@@ -29,24 +29,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/SensorData.h"
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/USemaphore.h"
 #include "rtabmap/core/CameraModel.h"
-#include <set>
+#include "rtabmap/core/Camera.h"
-#include <stack>
-#include <list>
-#include <vector>
 
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 
 #include <boost/signals2/connection.hpp>
 
-class UDirectory;
-class UTimer;
-
 namespace openni
 {
 class Device;
@@ -67,70 +59,17 @@ class Registration;
 class PacketPipeline;
 }
 
-namespace FlyCapture2
-{
-class Camera;
-}
-
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;
 
 namespace rtabmap
 {
 
-/**
- * Class CameraRGBD
- *
- */
-class RTABMAP_EXP CameraRGBD
-{
-public:
-	virtual ~CameraRGBD();
-	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
-
-	virtual bool init(const std::string & calibrationFolder = ".") = 0;
-	virtual bool isCalibrated() const = 0;
-	virtual std::string getSerial() const = 0;
-
-	//getters
-	float getImageRate() const {return _imageRate;}
-	const Transform & getLocalTransform() const {return _localTransform;}
-	bool isMirroringEnabled() const {return _mirroring;}
-	bool isColorOnly() const {return _colorOnly;}
-
-	//setters
-	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-	void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
-	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
-
-protected:
-	/**
-	 * Constructor
-	 *
-	 * @param imageRate : image/second , 0 for fast as the camera can
-	 */
-	CameraRGBD(float imageRate = 0,
-				const Transform & localTransform = Transform::getIdentity());
-
-	/**
-	 * returned rgb and depth images should be already rectified
-	 */
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp) = 0;
-
-private:
-	float _imageRate;
-	Transform _localTransform;
-	bool _mirroring;
-	bool _colorOnly;
-	UTimer * _frameRateTimer;
-};
-
 /////////////////////////
 // CameraOpenNIPCL
 /////////////////////////
 class RTABMAP_EXP CameraOpenni :
-	public CameraRGBD
+	public Camera
 {
 public:
 	static bool available() {return true;}
@@ -147,12 +86,12 @@ public:
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
 
-    virtual bool init(const std::string & calibrationFolder = ".");
+    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
     virtual bool isCalibrated() const;
     virtual std::string getSerial() const;
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
+	virtual SensorData captureImage();
 
 private:
     pcl::Grabber* interface_;
@@ -169,7 +108,7 @@ private:
 // CameraOpenNICV
 /////////////////////////
 class RTABMAP_EXP CameraOpenNICV :
-	public CameraRGBD
+	public Camera
 {
 
 public:
@@ -181,12 +120,12 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNICV();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const {return "";} // unknown with OpenCV
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
+	virtual SensorData captureImage();
 
 private:
 	bool _asus;
@@ -198,7 +137,7 @@ private:
 // CameraOpenNI2
 /////////////////////////
 class RTABMAP_EXP CameraOpenNI2 :
-	public CameraRGBD
+	public Camera
 {
 
 public:
@@ -211,7 +150,7 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
@@ -222,7 +161,7 @@ public:
 	bool setMirroring(bool enabled);
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
+	virtual SensorData captureImage();
 
 private:
 	openni::Device * _device;
@@ -240,7 +179,7 @@ private:
 class FreenectDevice;
 
 class RTABMAP_EXP CameraFreenect :
-	public CameraRGBD
+	public Camera
 {
 public:
 	static bool available();
@@ -252,12 +191,12 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
+	virtual SensorData captureImage();
 
 private:
 	int deviceId_;
@@ -270,7 +209,7 @@ private:
 /////////////////////////
 
 class RTABMAP_EXP CameraFreenect2 :
-	public CameraRGBD
+	public Camera
 {
 public:
 	static bool available();
@@ -290,12 +229,12 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect2();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp);
+	virtual SensorData captureImage();
 
 private:
 	int deviceId_;
@@ -308,91 +247,4 @@ private:
 	libfreenect2::Registration * reg_;
 };
 
-/////////////////////////
-// CameraStereoDC1394
-/////////////////////////
-class DC1394Device;
-
-class RTABMAP_EXP CameraStereoDC1394 :
-	public CameraRGBD
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoDC1394();
-
-	virtual bool init(const std::string & calibrationFolder = ".");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
-
-private:
-	DC1394Device *device_;
-	StereoCameraModel stereoModel_;
-};
-
-/////////////////////////
-// CameraStereoFlyCapture2
-/////////////////////////
-class RTABMAP_EXP CameraStereoFlyCapture2 :
-	public CameraRGBD
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoFlyCapture2();
-
-	virtual bool init(const std::string & calibrationFolder = ".");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
-
-private:
-	FlyCapture2::Camera * camera_;
-	void * triclopsCtx_; // TriclopsContext
-};
-
-/////////////////////////
-// CameraStereoImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoImages :
-	public CameraRGBD
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoImages(
-			const std::string & path,
-			const std::string & cameraName = "stereo_images", // calibration file name
-			const std::string & timestampsPath = "", // "times.txt"
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp);
-
-private:
-	CameraImages * camera_;
-	CameraImages * camera2_;
-	std::string cameraName_;
-	std::string timestampsPath_;
-	std::list<double> stamps_;
-	StereoCameraModel stereoModel_;
-};
-
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -0,0 +1,130 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include <list>
+
+namespace FlyCapture2
+{
+class Camera;
+}
+
+namespace rtabmap
+{
+
+/////////////////////////
+// CameraStereoDC1394
+/////////////////////////
+class DC1394Device;
+
+class RTABMAP_EXP CameraStereoDC1394 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoDC1394();
+
+	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:
+	DC1394Device *device_;
+	StereoCameraModel stereoModel_;
+};
+
+/////////////////////////
+// CameraStereoFlyCapture2
+/////////////////////////
+class RTABMAP_EXP CameraStereoFlyCapture2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoFlyCapture2();
+
+	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:
+	FlyCapture2::Camera * camera_;
+	void * triclopsCtx_; // TriclopsContext
+};
+
+/////////////////////////
+// CameraStereoImages
+/////////////////////////
+class CameraImages;
+class RTABMAP_EXP CameraStereoImages :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoImages(
+			const std::string & path,
+			const std::string & timestampsPath = "", // "times.txt"
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoImages();
+
+	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 * camera_;
+	CameraImages * camera2_;
+	std::string timestampsPath_;
+	std::list<double> stamps_;
+	StereoCameraModel stereoModel_;
+	std::string cameraName_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -36,7 +36,6 @@ namespace rtabmap
 {
 
 class Camera;
-class CameraRGBD;
 
 /**
  * Class CameraThread
@@ -49,10 +48,10 @@ class RTABMAP_EXP CameraThread :
 public:
 	// ownership transferred
 	CameraThread(Camera * camera);
-	CameraThread(CameraRGBD * camera);
 	virtual ~CameraThread();
 
-	bool init(); // call camera->init()
+	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -60,15 +59,14 @@ public:
 	void setImageRate(float imageRate);
 
 	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
-	CameraRGBD * cameraRGBD() {return _cameraRGBD;} // return null if not set, valid until CameraThread is deleted
 
 private:
 	virtual void mainLoop();
 
 private:
 	Camera * _camera;
-	CameraRGBD * _cameraRGBD;
+	bool _mirroring;
-	int _seq;
+	bool _colorOnly;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/SensorData.h

@@ -157,6 +157,9 @@ public:
 	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
 	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
 	void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;}
+	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
+	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
+	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
 
 	//for convenience
 	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}

corelib/src/CMakeLists.txt

@@ -13,7 +13,9 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
+    CameraRGB.cpp
     CameraRGBD.cpp
+    CameraStereo.cpp
     CameraModel.cpp
     
     EpipolarGeometry.cpp

corelib/src/Camera.cpp

@@ -44,14 +44,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-Camera::Camera(float imageRate,
+Camera::Camera(float imageRate, const Transform & localTransform) :
-		unsigned int imageWidth,
-		unsigned int imageHeight) :
 	_imageRate(imageRate),
-	_imageWidth(imageWidth),
+	_localTransform(localTransform),
-	_imageHeight(imageHeight),
+	_targetImageSize(0,0),
-	_mirroring(false),
+	_frameRateTimer(new UTimer()),
-	_frameRateTimer(new UTimer())
+	_seq(0)
 {
 }
 
@@ -63,397 +61,46 @@ Camera::~Camera()
 	}
 }
 
-void Camera::setImageSize(unsigned int width, unsigned int height)
+SensorData Camera::takeImage()
 {
-	_imageWidth = width;
+	bool warnFrameRateTooHigh = false;
-	_imageHeight = height;
+	float actualFrameRate = 0;
-}
+	if(_imageRate>0)
-
-void Camera::getImageSize(unsigned int & width, unsigned int & height)
 	{
-	width = _imageWidth;
+		int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
-	height = _imageHeight;
-}
-
-void Camera::setCalibration(const std::string & fileName)
-{
-	if(UFile::getExtension(fileName).compare("yaml") == 0)
-	{
-		cv::FileStorage fs;
-		fs.open(fileName, cv::FileStorage::READ);
-
-		if (!fs.isOpened())
-		{
-			UERROR("Failed to open file \"%s\"", fileName.c_str());
-			return;
-		}
-
-		cv::Mat k,d;
-
-		cv::FileNode n = fs["camera_matrix"];
-		int rows = n["rows"];
-		int cols = n["cols"];
-		std::vector<double> data;
-		n["data"] >> data;
-		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
-		{
-			k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-		}
-
-		cv::FileNode nd = fs["distortion_coefficients"];
-		rows = nd["rows"];
-		cols = nd["cols"];
-		data.clear();
-		nd["data"] >> data;
-		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
-		{
-			d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-		}
-
-		if(k.empty())
-		{
-			UERROR("Failed to load \"camera_matrix\" matrix.");
-		}
-		if(d.empty())
-		{
-			UERROR("Failed to load \"distortion_coefficients\" matrix.");
-		}
-		if(!k.empty() && !d.empty())
-		{
-			this->setCalibration(k, d);
-		}
-	}
-	else
-	{
-		UERROR("Calibration file must be in \"*.yaml\" format");
-	}
-}
-
-void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients)
-{
-	UASSERT(cameraMatrix.type() == CV_64FC1 &&
-			cameraMatrix.rows == 3 &&
-			cameraMatrix.cols == 3);
-	UASSERT(distorsionCoefficients.type() == CV_64FC1 &&
-			distorsionCoefficients.rows ==1 &&
-			(distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8));
-
-	_k = cameraMatrix;
-	_d = distorsionCoefficients;
-}
-
-void Camera::resetCalibration()
-{
-	_k = cv::Mat();
-	_d = cv::Mat();
-}
-
-cv::Mat Camera::takeImage()
-{
-	cv::Mat img;
-	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
-	if(imageRate>0)
-	{
-		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
 		if(sleepTime > 2)
 		{
 			uSleep(sleepTime-2);
 		}
+		else if(sleepTime < 0)
+		{
+			warnFrameRateTooHigh = true;
+			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
+		}
 
 		// Add precision at the cost of a small overhead
-		while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
+		while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
 		{
 			//
 		}
 
 		double slept = _frameRateTimer->getElapsedTime();
 		_frameRateTimer->start();
-		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
+		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
 	}
 
 	UTimer timer;
-	img = this->captureImage();
+	SensorData data  = this->captureImage();
-	if(!img.empty() && !_k.empty() && !_d.empty())
+	if(warnFrameRateTooHigh)
 	{
-		cv::Mat temp = img.clone();
+		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
-		cv::undistort(temp, img, _k, _d);
+				_imageRate, actualFrameRate, timer.ticks());
 	}
-	if(!img.empty() && _mirroring)
+	else
 	{
-		cv::flip(img,img,1);
-	}
 		UDEBUG("Time capturing image = %fs", timer.ticks());
-	return img;
 	}
-
+	return data;
-/////////////////////////
-// CameraImages
-/////////////////////////
-CameraImages::CameraImages(const std::string & path,
-					 int startAt,
-					 bool refreshDir,
-					 float imageRate,
-					 unsigned int imageWidth,
-					 unsigned int imageHeight) :
-	Camera(imageRate, imageWidth, imageHeight),
-	_path(path),
-	_startAt(startAt),
-	_refreshDir(refreshDir),
-	_count(0),
-	_dir(0)
-{
-
-}
-
-CameraImages::~CameraImages(void)
-{
-	if(_dir)
-	{
-		delete _dir;
-	}
-}
-
-bool CameraImages::init()
-{
-	UDEBUG("");
-	if(_dir)
-	{
-		_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
-	}
-	else
-	{
-		_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
-	}
-	_count = 0;
-	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
-	{
-		_path.append("/");
-	}
-	if(!_dir->isValid())
-	{
-		ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
-	}
-	else if(_dir->getFileNames().size() == 0)
-	{
-		UWARN("Directory is empty \"%s\"", _path.c_str());
-	}
-	else
-	{
-		UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
-	}
-	return _dir->isValid();
-}
-
-unsigned int CameraImages::imagesCount() const
-{
-	if(_dir)
-	{
-		return _dir->getFileNames().size();
-	}
-	return 0;
-}
-
-cv::Mat CameraImages::captureImage()
-{
-	cv::Mat img;
-	UDEBUG("");
-	if(_dir->isValid())
-	{
-		if(_refreshDir)
-		{
-			_dir->update();
-		}
-		if(_startAt == 0)
-		{
-			const std::list<std::string> & fileNames = _dir->getFileNames();
-			if(fileNames.size())
-			{
-				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
-				{
-					_lastFileName = *fileNames.rbegin();
-					std::string fullPath = _path + _lastFileName;
-					img = cv::imread(fullPath.c_str());
-				}
-			}
-		}
-		else
-		{
-			std::string fileName;
-			std::string fullPath;
-			fileName = _dir->getNextFileName();
-			if(fileName.size())
-			{
-				fullPath = _path + fileName;
-				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
-				{
-					fullPath = _path + fileName;
-				}
-				if(fileName.size())
-				{
-					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
-
-#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
-#else
-					img = cv::imread(fullPath.c_str(), -1);
-#endif
-					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)
-					{
-						// 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;
-					}
-				}
-			}
-		}
-	}
-	else
-	{
-		UWARN("Directory is not set, camera must be initialized.");
-	}
-
-	unsigned int w;
-	unsigned int h;
-	this->getImageSize(w, h);
-
-	if(!img.empty() &&
-	   w &&
-	   h &&
-	   w != (unsigned int)img.cols &&
-	   h != (unsigned int)img.rows)
-	{
-		cv::Mat resampled;
-		cv::resize(img, resampled, cv::Size(w, h));
-		img = resampled;
-	}
-	return img;
-}
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-CameraVideo::CameraVideo(int usbDevice,
-						 float imageRate,
-						 unsigned int imageWidth,
-						 unsigned int imageHeight) :
-	Camera(imageRate, imageWidth, imageHeight),
-	_src(kUsbDevice),
-	_usbDevice(usbDevice)
-{
-
-}
-
-CameraVideo::CameraVideo(const std::string & filePath,
-						   float imageRate,
-						   unsigned int imageWidth,
-						   unsigned int imageHeight) :
-	Camera(imageRate, imageWidth, imageHeight),
-	_filePath(filePath),
-	_src(kVideoFile),
-	_usbDevice(0)
-{
-}
-
-CameraVideo::~CameraVideo()
-{
-	_capture.release();
-}
-
-bool CameraVideo::init()
-{
-	if(_capture.isOpened())
-	{
-		_capture.release();
-	}
-
-	if(_src == kUsbDevice)
-	{
-		unsigned int w;
-		unsigned int h;
-		this->getImageSize(w, h);
-
-		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, w, h);
-		_capture.open(_usbDevice);
-
-		if(w && h)
-		{
-			_capture.set(CV_CAP_PROP_FRAME_WIDTH, double(w));
-			_capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(h));
-		}
-	}
-	else if(_src == kVideoFile)
-	{
-		ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
-		_capture.open(_filePath.c_str());
-	}
-	else
-	{
-		ULOGGER_ERROR("Camera: Unknown source...");
-	}
-	if(!_capture.isOpened())
-	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
-		_capture.release();
-		return false;
-	}
-	return true;
-}
-
-cv::Mat CameraVideo::captureImage()
-{
-	cv::Mat img;
-	if(_capture.isOpened())
-	{
-		if(_capture.read(img))
-		{
-			unsigned int w;
-			unsigned int h;
-			this->getImageSize(w, h);
-
-			if(!img.empty() &&
-			   w &&
-			   h &&
-			   w != (unsigned int)img.cols &&
-			   h != (unsigned int)img.rows)
-			{
-				cv::Mat resampled;
-				cv::resize(img, resampled, cv::Size(w, h));
-				img = resampled;
-			}
-			else
-			{
-				// clone required
-				img = img.clone();
-			}
-		}
-		else if(_usbDevice)
-		{
-			UERROR("Camera has been disconnected!");
-		}
-	}
-	else
-	{
-		ULOGGER_WARN("The camera must be initialized before requesting an image.");
-	}
-	return img;
 }
 
 } // namespace rtabmap

corelib/src/CameraRGB.cpp

@@ -0,0 +1,328 @@
+/*
+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 "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/DBDriver.h"
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <iostream>
+#include <cmath>
+
+namespace rtabmap
+{
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+CameraImages::CameraImages(const std::string & path,
+					 int startAt,
+					 bool refreshDir,
+					 float imageRate,
+					 const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_path(path),
+	_startAt(startAt),
+	_refreshDir(refreshDir),
+	_count(0),
+	_dir(0)
+{
+
+}
+
+CameraImages::~CameraImages(void)
+{
+	if(_dir)
+	{
+		delete _dir;
+	}
+}
+
+bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+	if(_dir)
+	{
+		_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
+	}
+	else
+	{
+		_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
+	}
+	_count = 0;
+	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
+	{
+		_path.append("/");
+	}
+	if(!_dir->isValid())
+	{
+		ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
+	}
+	else if(_dir->getFileNames().size() == 0)
+	{
+		UWARN("Directory is empty \"%s\"", _path.c_str());
+	}
+	else
+	{
+		UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
+	}
+	return _dir->isValid();
+}
+
+bool CameraImages::isCalibrated() const
+{
+	return false;
+}
+
+std::string CameraImages::getSerial() const
+{
+	return "";
+}
+
+unsigned int CameraImages::imagesCount() const
+{
+	if(_dir)
+	{
+		return _dir->getFileNames().size();
+	}
+	return 0;
+}
+
+SensorData CameraImages::captureImage()
+{
+	cv::Mat img;
+	UDEBUG("");
+	if(_dir->isValid())
+	{
+		if(_refreshDir)
+		{
+			_dir->update();
+		}
+		if(_startAt == 0)
+		{
+			const std::list<std::string> & fileNames = _dir->getFileNames();
+			if(fileNames.size())
+			{
+				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
+				{
+					_lastFileName = *fileNames.rbegin();
+					std::string fullPath = _path + _lastFileName;
+					img = cv::imread(fullPath.c_str());
+				}
+			}
+		}
+		else
+		{
+			std::string fileName;
+			std::string fullPath;
+			fileName = _dir->getNextFileName();
+			if(fileName.size())
+			{
+				fullPath = _path + fileName;
+				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
+				{
+					fullPath = _path + fileName;
+				}
+				if(fileName.size())
+				{
+					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
+
+#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
+#else
+					img = cv::imread(fullPath.c_str(), -1);
+#endif
+					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)
+					{
+						// 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;
+					}
+				}
+			}
+		}
+	}
+	else
+	{
+		UWARN("Directory is not set, camera must be initialized.");
+	}
+
+	return SensorData(img);
+}
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+CameraVideo::CameraVideo(int usbDevice,
+						 float imageRate,
+						 const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_src(kUsbDevice),
+	_usbDevice(usbDevice)
+{
+
+}
+
+CameraVideo::CameraVideo(const std::string & filePath,
+						   float imageRate,
+						   const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_filePath(filePath),
+	_src(kVideoFile),
+	_usbDevice(0)
+{
+}
+
+CameraVideo::~CameraVideo()
+{
+	_capture.release();
+}
+
+bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	_guid.clear();
+	if(_capture.isOpened())
+	{
+		_capture.release();
+	}
+
+	if(_src == kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
+		_capture.open(_usbDevice);
+	}
+	else if(_src == kVideoFile)
+	{
+		ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
+		_capture.open(_filePath.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("Camera: Unknown source...");
+	}
+	if(!_capture.isOpened())
+	{
+		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		_capture.release();
+		return false;
+	}
+	else
+	{
+		uint32_t guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+		if(guid != 0 && guid != 0xffffffff)
+		{
+			_guid = uFormat("%08x", guid);
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
+		{
+			if(!_model.load(calibrationFolder + "/" + (cameraName.empty()?_guid:cameraName) + ".yaml"))
+			{
+				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());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f cx=%f cy=%f cy=%f",
+						_model.fx(),
+						_model.cx(),
+						_model.cy(),
+						_model.cy());
+			}
+		}
+	}
+	return true;
+}
+
+bool CameraVideo::isCalibrated() const
+{
+	return _model.isValid();
+}
+
+std::string CameraVideo::getSerial() const
+{
+	return _guid;
+}
+
+SensorData CameraVideo::captureImage()
+{
+	cv::Mat img;
+	if(_capture.isOpened())
+	{
+		if(_capture.read(img))
+		{
+			if(_model.isValid())
+			{
+				img = _model.rectifyImage(img);
+			}
+			else
+			{
+				// clone required
+				img = img.clone();
+			}
+		}
+		else if(_usbDevice)
+		{
+			UERROR("Camera has been disconnected!");
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+
+	return SensorData(img);
+}
+
+} // namespace rtabmap

corelib/src/CameraStereo.cpp

@@ -0,0 +1,910 @@
+/*
+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 "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/CameraRGB.h"
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMath.h>
+
+#ifdef WITH_DC1394
+#include <dc1394/dc1394.h>
+#endif
+
+#ifdef WITH_FLYCAPTURE2
+#include <triclops.h>
+#include <fc2triclops.h>
+#endif
+
+namespace rtabmap
+{
+
+//
+// CameraStereoDC1394
+// Inspired from ROS camera1394stereo package
+//
+
+#ifdef WITH_DC1394
+class DC1394Device
+{
+public:
+	DC1394Device() :
+		camera_(0),
+		context_(0)
+	{
+
+	}
+	~DC1394Device()
+	{
+		if (camera_)
+		{
+			if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
+				DC1394_SUCCESS != dc1394_capture_stop(camera_))
+			{
+				UWARN("unable to stop camera");
+			}
+
+			// Free resources
+			dc1394_capture_stop(camera_);
+			dc1394_camera_free(camera_);
+			camera_ = NULL;
+		}
+		if(context_)
+		{
+			dc1394_free(context_);
+			context_ = NULL;
+		}
+	}
+
+	const std::string & guid() const {return guid_;}
+
+	bool init()
+	{
+		if(camera_)
+		{
+			// Free resources
+			dc1394_capture_stop(camera_);
+			dc1394_camera_free(camera_);
+			camera_ = NULL;
+		}
+
+		// look for a camera
+		int err;
+		if(context_ == NULL)
+		{
+			context_ = dc1394_new ();
+			if (context_ == NULL)
+			{
+				UERROR(    "Could not initialize dc1394_context.\n"
+				"Make sure /dev/raw1394 exists, you have access permission,\n"
+				"and libraw1394 development package is installed.");
+				return false;
+			}
+		}
+
+		dc1394camera_list_t *list;
+		err = dc1394_camera_enumerate(context_, &list);
+		if (err != DC1394_SUCCESS)
+		{
+			UERROR("Could not get camera list");
+			return false;
+		}
+
+		if (list->num == 0)
+		{
+			UERROR("No cameras found");
+			dc1394_camera_free_list (list);
+			return false;
+		}
+		uint64_t guid = list->ids[0].guid;
+		dc1394_camera_free_list (list);
+
+		// Create a camera
+		camera_ = dc1394_camera_new (context_, guid);
+		if (!camera_)
+		{
+			UERROR("Failed to initialize camera with GUID [%016lx]", guid);
+			return false;
+		}
+
+		uint32_t value[3];
+		value[0]= camera_->guid & 0xffffffff;
+		value[1]= (camera_->guid >>32) & 0x000000ff;
+		value[2]= (camera_->guid >>40) & 0xfffff;
+		guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
+
+		UINFO("camera model: %s %s", camera_->vendor, camera_->model);
+
+		// initialize camera
+		// Enable IEEE1394b mode if the camera and bus support it
+		bool bmode = camera_->bmode_capable;
+		if (bmode
+			&& (DC1394_SUCCESS !=
+			dc1394_video_set_operation_mode(camera_,
+									DC1394_OPERATION_MODE_1394B)))
+		{
+			bmode = false;
+			UWARN("failed to set IEEE1394b mode");
+		}
+
+		// start with highest speed supported
+		dc1394speed_t request = DC1394_ISO_SPEED_3200;
+		int rate = 3200;
+		if (!bmode)
+		{
+			// not IEEE1394b capable: so 400Mb/s is the limit
+			request = DC1394_ISO_SPEED_400;
+			rate = 400;
+		}
+
+		// round requested speed down to next-lower defined value
+		while (rate > 400)
+		{
+			if (request <= DC1394_ISO_SPEED_MIN)
+			{
+				// get current ISO speed of the device
+				dc1394speed_t curSpeed;
+				if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
+				{
+					// Translate curSpeed back to an int for the parameter
+					// update, works as long as any new higher speeds keep
+					// doubling.
+					request = curSpeed;
+					rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
+				}
+				else
+				{
+					UWARN("Unable to get ISO speed; assuming 400Mb/s");
+					rate = 400;
+					request = DC1394_ISO_SPEED_400;
+				}
+				break;
+			}
+			// continue with next-lower possible value
+			request = (dc1394speed_t) ((int) request - 1);
+			rate = rate / 2;
+		}
+
+		// set the requested speed
+		if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
+		{
+			UERROR("Failed to set iso speed");
+			return false;
+		}
+
+		// set video mode
+		dc1394video_modes_t vmodes;
+		err = dc1394_video_get_supported_modes(camera_, &vmodes);
+		if (err != DC1394_SUCCESS)
+		{
+			UERROR("unable to get supported video modes");
+			return (dc1394video_mode_t) 0;
+		}
+
+		// see if requested mode is available
+		bool found = false;
+		dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee
+		for (uint32_t i = 0; i < vmodes.num; ++i)
+		{
+			if (vmodes.modes[i] == videoMode)
+			{
+				found = true;
+			}
+		}
+		if(!found)
+		{
+			UERROR("unable to get video mode %d", videoMode);
+			return false;
+		}
+
+		if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
+		{
+			UERROR("Failed to set video mode %d", videoMode);
+			return false;
+		}
+
+		// special handling for Format7 modes
+		if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
+		{
+			if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
+			{
+				UERROR("Could not set color coding");
+				return false;
+			}
+			uint32_t packetSize;
+			if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
+			{
+				UERROR("Could not get default packet size");
+				return false;
+			}
+
+			if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
+			{
+				UERROR("Could not set packet size");
+				return false;
+			}
+		}
+		else
+		{
+			UERROR("Video is not in mode scalable");
+		}
+
+		// start the device streaming data
+		// Set camera to use DMA, improves performance.
+		if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
+		{
+			UERROR("Failed to open device!");
+			return false;
+		}
+
+		// Start transmitting camera data
+		if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
+		{
+			UERROR("Failed to start device!");
+			return false;
+		}
+
+		return true;
+	}
+
+	bool getImages(cv::Mat & left, cv::Mat & right)
+	{
+		if(camera_)
+		{
+			dc1394video_frame_t * frame = NULL;
+			UDEBUG("[%016lx] waiting camera", camera_->guid);
+			dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
+			if (!frame)
+			{
+				UERROR("Unable to capture frame");
+				return false;
+			}
+			dc1394video_frame_t frame1 = *frame;
+			// deinterlace frame into two imagesCount one on top the other
+			size_t frame1_size = frame->total_bytes;
+			frame1.image = (unsigned char *) malloc(frame1_size);
+			frame1.allocated_image_bytes = frame1_size;
+			frame1.color_coding = DC1394_COLOR_CODING_RAW8;
+			int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
+			if (err != DC1394_SUCCESS)
+			{
+				free(frame1.image);
+				dc1394_capture_enqueue(camera_, frame);
+				UERROR("Could not extract stereo frames");
+				return false;
+			}
+
+			uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
+			UASSERT(capture_buffer);
+
+			cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
+			cv::Mat image2 = image.clone();
+
+			//DC1394_COLOR_CODING_RAW16:
+			//DC1394_COLOR_FILTER_BGGR
+			cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
+			cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
+
+			dc1394_capture_enqueue(camera_, frame);
+
+			free(frame1.image);
+
+			return true;
+		}
+		return false;
+	}
+
+private:
+	dc1394camera_t *camera_;
+	dc1394_t *context_;
+	std::string guid_;
+};
+#endif
+
+bool CameraStereoDC1394::available()
+{
+#ifdef WITH_DC1394
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		device_(0)
+{
+#ifdef WITH_DC1394
+	device_ = new DC1394Device();
+#endif
+}
+
+CameraStereoDC1394::~CameraStereoDC1394()
+{
+#ifdef WITH_DC1394
+	if(device_)
+	{
+		delete device_;
+	}
+#endif
+}
+
+bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef WITH_DC1394
+	if(device_)
+	{
+		bool ok = device_->init();
+		if(ok)
+		{
+			// look for calibration files
+			if(!calibrationFolder.empty())
+			{
+				if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName))
+				{
+					UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+							cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
+				}
+				else
+				{
+					UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+							stereoModel_.left().fx(),
+							stereoModel_.left().cx(),
+							stereoModel_.left().cy(),
+							stereoModel_.baseline());
+				}
+			}
+		}
+		return ok;
+	}
+#else
+	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
+#endif
+	return false;
+}
+
+bool CameraStereoDC1394::isCalibrated() const
+{
+	return stereoModel_.isValid();
+}
+
+std::string CameraStereoDC1394::getSerial() const
+{
+#ifdef WITH_DC1394
+	if(device_)
+	{
+		return device_->guid();
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoDC1394::captureImage()
+{
+	SensorData data;
+#ifdef WITH_DC1394
+	if(device_)
+	{
+		cv::Mat left, right;
+		device_->getImages(left, right);
+
+		if(!left.empty() && !right.empty())
+		{
+			// Rectification
+			left = stereoModel_.left().rectifyImage(left);
+			right = stereoModel_.right().rectifyImage(right);
+			StereoCameraModel model(
+					stereoModel_.left().fx(), //fx
+					stereoModel_.left().fy(), //fy
+					stereoModel_.left().cx(), //cx
+					stereoModel_.left().cy(), //cy
+					stereoModel_.baseline(),
+					this->getLocalTransform());
+			data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
+		}
+	}
+#else
+	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
+#endif
+	return data;
+}
+
+//
+// CameraTriclops
+//
+CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		camera_(0),
+		triclopsCtx_(0)
+{
+#ifdef WITH_FLYCAPTURE2
+	camera_ = new FlyCapture2::Camera();
+#endif
+}
+
+CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
+{
+#ifdef WITH_FLYCAPTURE2
+	// Close the camera
+	camera_->StopCapture();
+	camera_->Disconnect();
+
+	// Destroy the Triclops context
+	triclopsDestroyContext( triclopsCtx_ ) ;
+
+	delete camera_;
+#endif
+}
+
+bool CameraStereoFlyCapture2::available()
+{
+#ifdef WITH_FLYCAPTURE2
+	return true;
+#else
+	return false;
+#endif
+}
+
+bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef WITH_FLYCAPTURE2
+	if(camera_)
+	{
+		// Close the camera
+		camera_->StopCapture();
+		camera_->Disconnect();
+	}
+	if(triclopsCtx_)
+	{
+		triclopsDestroyContext(triclopsCtx_);
+		triclopsCtx_ = 0;
+	}
+
+	// connect camera
+	FlyCapture2::Error fc2Error = camera_->Connect();
+	if(fc2Error != FlyCapture2::PGRERROR_OK)
+	{
+		UERROR("Failed to connect the camera.");
+		return false;
+	}
+
+	// configure camera
+	Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
+	if(Fc2Triclops::setStereoMode(*camera_, mode ))
+	{
+		UERROR("Failed to set stereo mode.");
+		return false;
+	}
+
+	// generate the Triclops context
+	FlyCapture2::CameraInfo camInfo;
+	if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
+	{
+		UERROR("Failed to get camera info.");
+		return false;
+	}
+
+	float dummy;
+	unsigned packetSz;
+	FlyCapture2::Format7ImageSettings imageSettings;
+	int maxWidth = 640;
+	int maxHeight = 480;
+	if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
+	{
+		maxHeight = imageSettings.height;
+		maxWidth = imageSettings.width;
+	}
+
+	// Get calibration from th camera
+	if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
+	{
+		UERROR("Failed to get calibration from the camera.");
+		return false;
+	}
+
+	float fx, cx, cy, baseline;
+	triclopsGetFocalLength(triclopsCtx_, &fx);
+	triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+	triclopsGetBaseline(triclopsCtx_, &baseline);
+	UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
+
+	triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
+	UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
+
+	if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
+	{
+		UERROR("Failed to start capture.");
+		return false;
+	}
+
+	return true;
+#else
+	UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
+#endif
+	return false;
+}
+
+bool CameraStereoFlyCapture2::isCalibrated() const
+{
+#ifdef WITH_FLYCAPTURE2
+	if(triclopsCtx_)
+	{
+		float fx, cx, cy, baseline;
+		triclopsGetFocalLength(triclopsCtx_, &fx);
+		triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+		triclopsGetBaseline(triclopsCtx_, &baseline);
+		return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
+	}
+#endif
+	return false;
+}
+
+std::string CameraStereoFlyCapture2::getSerial() const
+{
+#ifdef WITH_FLYCAPTURE2
+	if(camera_ && camera_->IsConnected())
+	{
+		FlyCapture2::CameraInfo camInfo;
+		if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
+		{
+			return uNumber2Str(camInfo.serialNumber);
+		}
+	}
+#endif
+	return "";
+}
+
+// struct containing image needed for processing
+#ifdef WITH_FLYCAPTURE2
+struct ImageContainer
+{
+	FlyCapture2::Image tmp[2];
+    FlyCapture2::Image unprocessed[2];
+} ;
+#endif
+
+SensorData CameraStereoFlyCapture2::captureImage()
+{
+	SensorData data;
+#ifdef WITH_FLYCAPTURE2
+	if(camera_ && triclopsCtx_ && camera_->IsConnected())
+	{
+		// grab image from camera.
+		// this image contains both right and left imagesCount
+		FlyCapture2::Image grabbedImage;
+		if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
+		{
+			stamp = UTimer::now();
+
+			// right and left image extracted from grabbed image
+			ImageContainer imageCont;
+
+			// generate triclops input from grabbed image
+			FlyCapture2::Image imageRawRight;
+			FlyCapture2::Image imageRawLeft;
+			FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
+
+			// Convert the pixel interleaved raw data to de-interleaved and color processed data
+			if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
+										   grabbedImage,
+										   true /*assume little endian*/,
+										   imageRawLeft /* right */,
+										   imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK)
+			{
+				// convert to color
+				FlyCapture2::Image srcImgRightRef(imageRawRight);
+				FlyCapture2::Image srcImgLeftRef(imageRawLeft);
+
+				bool ok = true;;
+				if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
+				     srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
+				{
+					ok = false;
+				}
+
+				if(ok)
+				{
+					FlyCapture2::Image imageColorRight;
+					FlyCapture2::Image imageColorLeft;
+					if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
+						 srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
+					{
+						ok = false;
+					}
+
+					if(ok)
+					{
+						//RECTIFY RIGHT
+						TriclopsInput triclopsColorInputs;
+						triclopsBuildRGBTriclopsInput(
+							grabbedImage.GetCols(),
+							grabbedImage.GetRows(),
+							imageColorRight.GetStride(),
+							(unsigned long)grabbedImage.GetTimeStamp().seconds,
+							(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
+							imageColorRight.GetData(),
+							imageColorRight.GetData(),
+							imageColorRight.GetData(),
+							&triclopsColorInputs);
+
+						triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
+						// Retrieve the rectified image from the triclops context
+						TriclopsImage rectifiedImage;
+						triclopsGetImage( triclopsCtx_,
+							TriImg_RECTIFIED,
+							TriCam_REFERENCE,
+							&rectifiedImage );
+
+						cv::Mat left,right;
+						right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
+
+						//RECTIFY LEFT COLOR
+						triclopsBuildPackedTriclopsInput(
+							grabbedImage.GetCols(),
+							grabbedImage.GetRows(),
+							imageColorLeft.GetStride(),
+							(unsigned long)grabbedImage.GetTimeStamp().seconds,
+							(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
+							imageColorLeft.GetData(),
+							&triclopsColorInputs );
+
+						cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
+						TriclopsPackedColorImage colorImage;
+						triclopsSetPackedColorImageBuffer(
+							triclopsCtx_,
+							TriCam_LEFT,
+							(TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
+
+						triclopsRectifyPackedColorImage(
+							triclopsCtx_,
+							TriCam_LEFT,
+							&triclopsColorInputs,
+							&colorImage );
+
+						cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
+
+						// Set calibration stuff
+						float fx, cy, cx, baseline;
+						triclopsGetFocalLength(triclopsCtx_, &fx);
+						triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+						triclopsGetBaseline(triclopsCtx_, &baseline);
+
+						StereoCameraModel model(
+								fx
+								fx,
+								cx
+								cy
+								baseline,
+								this->getLocalTransform());
+						data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
+					}
+				}
+			}
+		}
+	}
+
+#else
+	UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
+#endif
+	return data;
+}
+
+//
+// CameraStereoImages
+//
+bool CameraStereoImages::available()
+{
+	return true;
+}
+
+CameraStereoImages::CameraStereoImages(
+		const std::string & path,
+		const std::string & timestampsPath,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		camera_(0),
+		camera2_(0),
+		timestampsPath_(timestampsPath)
+{
+	std::vector<std::string> paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';'));
+	if(paths.size() >= 1)
+	{
+		camera_ = new CameraImages(paths[0]);
+
+		if(paths.size() >= 2)
+		{
+			camera2_ = new CameraImages(paths[1]);
+		}
+	}
+	else
+	{
+		UERROR("The path is empty!");
+	}
+}
+
+CameraStereoImages::~CameraStereoImages()
+{
+	if(camera_)
+	{
+		delete camera_;
+	}
+	if(camera2_)
+	{
+		delete camera2_;
+	}
+}
+
+bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	// look for calibration files
+	cameraName_.clear();
+	if(!calibrationFolder.empty() && !cameraName.empty())
+	{
+		cameraName_ = cameraName;
+		if(!stereoModel_.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("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+					stereoModel_.left().fx(),
+					stereoModel_.left().cx(),
+					stereoModel_.left().cy(),
+					stereoModel_.baseline());
+		}
+	}
+	bool success = false;
+	if(camera_ == 0)
+	{
+		UERROR("Cannot initialize the camera.");
+	}
+	else if(camera_->init())
+	{
+		if(camera2_)
+		{
+			if(camera2_->init())
+			{
+				if(camera_->imagesCount() == camera2_->imagesCount())
+				{
+					success = true;
+				}
+				else
+				{
+					UERROR("Cameras don't have the same number of images (%d vs %d)",
+							camera_->imagesCount(), camera2_->imagesCount());
+				}
+			}
+			else
+			{
+				UERROR("Cannot initialize the second camera.");
+			}
+		}
+		else
+		{
+			success = true;
+		}
+	}
+
+	stamps_.clear();
+	if(success && 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() != 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;
+		}
+	}
+
+	return success;
+}
+
+bool CameraStereoImages::isCalibrated() const
+{
+	return stereoModel_.isValid();
+}
+
+std::string CameraStereoImages::getSerial() const
+{
+	return cameraName_;
+}
+
+SensorData CameraStereoImages::captureImage()
+{
+	SensorData data;
+	if(camera_)
+	{
+		double stamp;
+		if(stamps_.size())
+		{
+			stamp = stamps_.front();
+			stamps_.pop_front();
+		}
+		else
+		{
+			stamp = UTimer::now();
+		}
+		SensorData left, right;
+		left = camera_->takeImage();
+		if(!left.imageRaw().empty())
+		{
+			if(camera2_)
+			{
+				right = camera2_->takeImage();
+			}
+			else
+			{
+				right = camera_->takeImage();
+			}
+
+			if(!right.imageRaw().empty())
+			{
+				// Rectification
+				//left = stereoModel_.left().rectifyImage(left);
+				//right = stereoModel_.right().rectifyImage(right);
+				StereoCameraModel model(
+						stereoModel_.left().fx(), //fx
+						stereoModel_.left().fy(), //fy
+						stereoModel_.left().cx(), //cx
+						stereoModel_.left().cy(), //cy
+						stereoModel_.baseline(),
+						this->getLocalTransform());
+				data = SensorData(left.imageRaw(), right.imageRaw(), model, this->getNextSeqID(), stamp);
+			}
+		}
+	}
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/CameraThread.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraEvent.h"
 
 #include <rtabmap/utilite/UTimer.h>
@@ -39,21 +38,12 @@ namespace rtabmap
 // ownership transferred
 CameraThread::CameraThread(Camera * camera) :
 		_camera(camera),
-		_cameraRGBD(0),
+		_mirroring(false),
-		_seq(0)
+		_colorOnly(false)
 {
 	UASSERT(_camera != 0);
 }
 
-// ownership transferred
-CameraThread::CameraThread(CameraRGBD * camera) :
-		_camera(0),
-		_cameraRGBD(camera),
-		_seq(0)
-{
-	UASSERT(_cameraRGBD != 0);
-}
-
 CameraThread::~CameraThread()
 {
 	join(true);
@@ -61,10 +51,6 @@ CameraThread::~CameraThread()
 	{
 		delete _camera;
 	}
-	if(_cameraRGBD)
-	{
-		delete _cameraRGBD;
-	}
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -73,86 +59,48 @@ void CameraThread::setImageRate(float imageRate)
 	{
 		_camera->setImageRate(imageRate);
 	}
-	if(_cameraRGBD)
-	{
-		_cameraRGBD->setImageRate(imageRate);
-	}
-}
-
-bool CameraThread::init()
-{
-	if(!this->isRunning())
-	{
-		_seq = 0;
-		if(_cameraRGBD)
-		{
-			return _cameraRGBD->init();
-		}
-		else
-		{
-			return _camera->init();
-		}
-
-		// Added sleep time to ignore first frames (which are darker)
-		uSleep(1000);
-	}
-	else
-	{
-		UERROR("Cannot initialize the camera because it is already running...");
-	}
-	return false;
 }
 
 void CameraThread::mainLoop()
 {
 	UTimer timer;
 	UDEBUG("");
-	cv::Mat rgb, depth;
+	SensorData data = _camera->takeImage();
-	float fx = 0.0f;
+
-	float fyOrBaseline = 0.0f;
+	if(!data.imageRaw().empty())
-	float cx = 0.0f;
-	float cy = 0.0f;
-	double stamp = UTimer::now();
-	if(_cameraRGBD)
 	{
-		_cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp);
+		if(_colorOnly && !data.depthRaw().empty())
+		{
+			data.setDepthOrRightRaw(cv::Mat());
 		}
-	else
+		if(_mirroring && data.cameraModels().size() == 1)
 		{
-		rgb = _camera->takeImage();
+			cv::Mat tmpRgb;
+			cv::flip(data.imageRaw(), tmpRgb, 1);
+			data.setImageRaw(tmpRgb);
+			if(data.cameraModels()[0].cx())
+			{
+				CameraModel tmpModel(
+						data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy(),
+						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy(),
+						data.cameraModels()[0].localTransform());
+				data.setCameraModel(tmpModel);
+			}
+			if(!data.depthRaw().empty())
+			{
+				cv::Mat tmpDepth;
+				cv::flip(data.depthRaw(), tmpDepth, 1);
+				data.setDepthOrRightRaw(tmpDepth);
+			}
 		}
 
-	if(!rgb.empty())
+		this->post(new CameraEvent(data, _camera->getSerial()));
-	{
-		if(_cameraRGBD)
-		{
-			SensorData data;
-			if(dynamic_cast<CameraStereoFlyCapture2*>(_cameraRGBD) ||
-				dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) ||
-				dynamic_cast<CameraStereoImages*>(_cameraRGBD))
-			{
-				//stereo
-				data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
-				UASSERT(data.stereoCameraModel().isValid());
-			}
-			else
-			{
-				data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
-				UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid());
-			}
-			this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
-		}
-		else
-		{
-			this->post(new CameraEvent(rgb, ++_seq, stamp));
-		}
 	}
 	else if(!this->isKilled())
-	{
-		if(_cameraRGBD)
 	{
 		UWARN("no more images...");
-		}
 		this->kill();
 		this->post(new CameraEvent());
 	}

corelib/src/DBReader.cpp

@@ -51,7 +51,8 @@ DBReader::DBReader(const std::string & databasePath,
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
-	_currentId(_ids.end())
+	_currentId(_ids.end()),
+	_previousStamp(0)
 {
 }
 
@@ -64,7 +65,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
-	_currentId(_ids.end())
+	_currentId(_ids.end()),
+	_previousStamp(0)
 {
 }
 

corelib/src/OdometryThread.cpp

@@ -60,7 +60,7 @@ void OdometryThread::handleEvent(UEvent * event)
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			CameraEvent * cameraEvent = (CameraEvent*)event;
-			if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
+			if(cameraEvent->getCode() == CameraEvent::kCodeData)
 			{
 				this->addData(cameraEvent->data());
 			}

corelib/src/RtabmapThread.cpp

@@ -302,7 +302,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 	{
 		UDEBUG("CameraEvent");
 		CameraEvent * e = (CameraEvent*)event;
-		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
+		if(e->getCode() == CameraEvent::kCodeData)
 		{
 			this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
 		}

examples/BOWMapping/main.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/Rtabmap.h"
-#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include <opencv2/core/core.hpp>
 #include "rtabmap/utilite/UFile.h"
 #include <stdio.h>
@@ -118,12 +118,12 @@ int main(int argc, char * argv[])
 
 	int countLoopDetected=0;
 	int i=0;
-	cv::Mat img = camera.takeImage();
+	rtabmap::SensorData data = camera.takeImage();
 	int nextIndex = rtabmap.getLastLocationId()+1;
-	while(!img.empty())
+	while(!data.imageRaw().empty())
 	{
 		// Process image : Main loop of RTAB-Map
-		rtabmap.process(img, nextIndex);
+		rtabmap.process(data.imageRaw(), nextIndex);
 
 		// Check if a loop closure is detected and print some info
 		if(rtabmap.getLoopClosureId())
@@ -157,7 +157,7 @@ int main(int argc, char * argv[])
 		++nextIndex;
 
 		//Get next image
-		img = camera.takeImage();
+		data = camera.takeImage();
 	}
 
 	printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);

examples/RGBDMapping/main.cpp

@@ -71,7 +71,7 @@ int main(int argc, char * argv[])
 
 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
-	CameraRGBD * camera = 0;
+	Camera * camera = 0;
 	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 1)
 	{
@@ -114,13 +114,14 @@ int main(int argc, char * argv[])
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
 	}
 
-	CameraThread cameraThread(camera);
+	if(!camera->init())
-	if(!cameraThread.init())
 	{
 		UERROR("Camera init failed!");
-		exit(1);
 	}
 
+	CameraThread cameraThread(camera);
+
+
 	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
 	// We give it the camera so the GUI can pause/resume the camera
 	QApplication app(argc, argv);

examples/WifiMapping/main.cpp

@@ -109,7 +109,7 @@ int main(int argc, char * argv[])
 
 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
-	CameraRGBD * camera = 0;
+	Camera * camera = 0;
 	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 1)
 	{
@@ -152,17 +152,17 @@ int main(int argc, char * argv[])
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
 	}
 
-	if(mirroring)
-	{
-		camera->setMirroringEnabled(true);
-	}
 
-	CameraThread cameraThread(camera);
+	if(!camera->init())
-	if(!cameraThread.init())
 	{
 		UERROR("Camera init failed!");
 		//exit(1);
 	}
+	CameraThread cameraThread(camera);
+	if(mirroring)
+	{
+		cameraThread.setMirroringEnabled(true);
+	}
 
 	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
 	// We give it the camera so the GUI can pause/resume the camera

guilib/include/rtabmap/gui/MainWindow.h

@@ -86,13 +86,6 @@ public:
 		kMonitoringPaused
 	};
 
-	enum SrcType {
-		kSrcUndefined,
-		kSrcVideo,
-		kSrcImages,
-		kSrcStream
-	};
-
 public:
 	/**
 	 * @param prefDialog If NULL, a default dialog is created. This
@@ -141,10 +134,7 @@ private slots:
 	void deleteMemory();
 	void openWorkingDirectory();
 	void updateEditMenu();
-	void selectImages();
-	void selectVideo();
 	void selectStream();
-	void selectDatabase();
 	void selectOpenni();
 	void selectFreenect();
 	void selectOpenniCv();
@@ -161,6 +151,7 @@ private slots:
 	void downloadPoseGraph();
 	void clearTheCache();
 	void openPreferences();
+	void openPreferencesSource();
 	void setDefaultViews();
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
@@ -252,9 +243,6 @@ private:
 	rtabmap::DBReader * _dbReader;
 	rtabmap::OdometryThread * _odomThread;
 
-	SrcType _srcType;
-	QString _srcPath;
-
 	//Dialogs
 	PreferencesDialog * _preferencesDialog;
 	AboutDialog * _aboutDialog;

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -58,6 +58,7 @@ protected:
 	virtual void handleEvent(UEvent * event);
 
 private slots:
+	void reset();
 	void processData(const rtabmap::OdometryEvent & odom);
 
 private:

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -59,7 +59,7 @@ namespace rtabmap {
 
 class Signature;
 class LoopClosureViewer;
-class CameraRGBD;
+class Camera;
 class CalibrationDialog;
 
 class RTABMAPGUI_EXP PreferencesDialog : public QDialog
@@ -79,19 +79,27 @@ public:
 	Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
 
 	enum Src {
-		kSrcUndef,
+		kSrcUndef = -1,
-		kSrcUsbDevice,
+
-		kSrcImages,
+		kSrcRGBD           = 0,
-		kSrcVideo,
+		kSrcOpenNI_PCL     = 0,
-		kSrcOpenNI_PCL,
+		kSrcFreenect       = 1,
-		kSrcFreenect,
+		kSrcOpenNI_CV      = 2,
-		kSrcOpenNI_CV,
+		kSrcOpenNI_CV_ASUS = 3,
-		kSrcOpenNI_CV_ASUS,
+		kSrcOpenNI2        = 4,
-		kSrcOpenNI2,
+		kSrcFreenect2      = 5,
-		kSrcFreenect2,
+
-		kSrcStereoDC1394,
+		kSrcStereo         = 100,
-		kSrcStereoFlyCapture2,
+		kSrcDC1394         = 100,
-		kSrcStereoImages
+		kSrcFlyCapture2    = 101,
+		kSrcStereoImages   = 102,
+
+		kSrcRGB            = 200,
+		kSrcUsbDevice      = 200,
+		kSrcImages         = 201,
+		kSrcVideo          = 202,
+
+		kSrcDatabase       = 300
 	};
 
 public:
@@ -100,6 +108,7 @@ public:
 
 	virtual QString getIniFilePath() const;
 	void init();
+	void setCurrentPanelToSource();
 
 	// save stuff
 	void saveSettings();
@@ -162,26 +171,23 @@ public:
 	// source panel
 	double getGeneralInputRate() const;
 	bool isSourceMirroring() const;
-	bool isSourceImageUsed() const;
+	QString getCalibrationName() const;
-	bool isSourceDatabaseUsed() const;
+	PreferencesDialog::Src getSourceType() const;
-	bool isSourceRGBDUsed() const;
+	PreferencesDialog::Src getSourceDriver() const;
-	PreferencesDialog::Src getSourceImageType() const;
+	QString getSourceDriverStr() const;
-	QString getSourceImageTypeStr() const;
+	QString getSourceDevice() const;
-	int getSourceWidth() const;
+
-	int getSourceHeight() 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
 	QString getSourceVideoPath() const;	//Video group
-	int getSourceUsbDeviceId() const;		//UsbDevice 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
-	Src getSourceRGBD() const; 			// Openni group
 	bool getSourceOpenni2AutoWhiteBalance() const;  //Openni group
 	bool getSourceOpenni2AutoExposure() const;  //Openni group
 	int getSourceOpenni2Exposure() const;  //Openni group
@@ -189,9 +195,8 @@ public:
 	bool getSourceOpenni2Mirroring() const; //Openni group
 	int getSourceFreenect2Format() const; //Openni group
 	bool isSourceRGBDColorOnly() const;
-	QString getSourceOpenniDevice() const;            //Openni group
+	Transform getSourceLocalTransform() const;    //Openni group
-	Transform getSourceOpenniLocalTransform() const;    //Openni group
+	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
-	CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
 
@@ -221,9 +226,7 @@ public slots:
 	void setDetectionRate(double value);
 	void setTimeLimit(float value);
 	void setSLAMMode(bool enabled);
-	void selectSourceImage(Src src = kSrcUndef);
+	void selectSourceDriver(Src src);
-	void selectSourceDatabase(bool user = false);
-	void selectSourceRGBD(Src src = kSrcUndef);
 	void calibrate();
 
 private slots:
@@ -251,11 +254,19 @@ private slots:
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
+	void selectSourceDatabase();
 	void selectSourceStereoImagesStamps();
 	void selectSourceStereoImagesPath();
+	void selectSourceImagesPath();
+	void selectSourceVideoPath();
+	void selectSourceOniPath();
+	void selectSourceOni2Path();
+	void updateSourceGrpVisibility();
 	void updateRGBDCameraGroupBoxVisibility();
+	void updateRGBCameraGroupBoxVisibility();
+	void updateStereoCameraGroupBoxVisibility();
 	void testOdometry();
-	void testRGBDCamera();
+	void testCamera();
 
 protected:
 	virtual void showEvent ( QShowEvent * event );

guilib/src/AboutDialog.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "AboutDialog.h"
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/Graph.h"
 #include "ui_aboutDialog.h"
 #include <opencv2/core/version.hpp>

guilib/src/CalibrationDialog.cpp

@@ -167,6 +167,7 @@ void CalibrationDialog::setStereoMode(bool stereo)
 	ui_->lineEdit_R_2->setVisible(stereo_);
 	ui_->lineEdit_P_2->setVisible(stereo_);
 	ui_->radioButton_stereoRectified->setVisible(stereo_);
+	ui_->checkBox_switchImages->setVisible(stereo_);
 }
 
 void CalibrationDialog::setBoardWidth(int width)
@@ -234,8 +235,7 @@ void CalibrationDialog::handleEvent(UEvent * event)
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
-			if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
+			if(e->getCode() == rtabmap::CameraEvent::kCodeData)
-			   e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
 			{
 				processingData_ = true;
 				QMetaObject::invokeMethod(this, "processImages",

guilib/src/CameraViewer.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/gui/ImageView.h>
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/gui/UCv2Qt.h>
+#include <rtabmap/utilite/ULogger.h>
 #include <QtCore/QMetaType>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
@@ -76,17 +77,33 @@ CameraViewer::~CameraViewer()
 void CameraViewer::showImage(const rtabmap::SensorData & data)
 {
 	processingImages_ = true;
+	if(!data.imageRaw().empty())
+	{
 		imageView_->setImage(uCvMat2QImage(data.imageRaw()));
+	}
+	if(!data.depthOrRightRaw().empty())
+	{
 		imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
-	if(!data.depthOrRightRaw().empty() && (data.stereoCameraModel().isValid() || data.cameraModels().size()))
+	}
+	if((data.stereoCameraModel().isValid() || data.cameraModels().size()))
+	{
+		if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
+		{
+			cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data));
+			cloudView_->setVisible(true);
+			cloudView_->update();
+		}
+		else if(!data.depthOrRightRaw().empty())
 		{
 			cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data));
+			cloudView_->setVisible(true);
+			cloudView_->update();
+		}
 	}
 	else
 	{
 		cloudView_->setVisible(false);
 	}
-	cloudView_->update();
 	processingImages_ = false;
 }
 
@@ -95,8 +112,7 @@ void CameraViewer::handleEvent(UEvent * event)
 	if(event->getClassName().compare("CameraEvent") == 0)
 	{
 		CameraEvent * camEvent = (CameraEvent*)event;
-		if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
+		if(camEvent->getCode() == CameraEvent::kCodeData)
-		   camEvent->getCode() == CameraEvent::kCodeImage)
 		{
 			if(camEvent->data().isValid())
 			{

guilib/src/DataRecorder.cpp

@@ -171,8 +171,7 @@ void DataRecorder::handleEvent(UEvent * event)
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			CameraEvent * camEvent = (CameraEvent*)event;
-			if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
+			if(camEvent->getCode() == CameraEvent::kCodeData)
-			   camEvent->getCode() == CameraEvent::kCodeImage)
 			{
 				if(camEvent->data().isValid())
 				{

guilib/src/GuiLib.qrc

@@ -29,5 +29,6 @@
         <file>images/sense.png</file>
         <file>images/xtion_pro_live.png</file>
         <file>images/bumblebee2.png</file>
+	<file>images/webcam.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -29,7 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "ui_mainWindow.h"
 
-#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/DBReader.h"
@@ -118,7 +119,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_camera(0),
 	_dbReader(0),
 	_odomThread(0),
-	_srcType(kSrcUndefined),
 	_preferencesDialog(0),
 	_aboutDialog(0),
 	_exportDialog(0),
@@ -338,7 +338,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionPost_processing->setEnabled(false);
 
 	QToolButton* toolButton = new QToolButton(this);
-	toolButton->setMenu(_ui->menuRGB_D_camera);
+	toolButton->setMenu(_ui->menuSelect_source);
 	toolButton->setPopupMode(QToolButton::InstantPopup);
 	toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
 	toolButton->setToolTip("Select sensor driver");
@@ -351,10 +351,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 #endif
 
 	//Settings menu
-	connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages()));
+	connect(_ui->actionMore_options, SIGNAL(triggered()), this, SLOT(openPreferencesSource()));
-	connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo()));
 	connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
-	connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase()));
 	connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
 	connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
 	connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
@@ -2113,39 +2111,26 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		// Camera settings...
 		_ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
 		this->updateSelectSourceMenu();
-		QString src;
+		_ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr());
-		if(_preferencesDialog->isSourceImageUsed())
-		{
-			src = _preferencesDialog->getSourceImageTypeStr();
-		}
-		else if(_preferencesDialog->isSourceDatabaseUsed())
-		{
-			src = "Database";
-		}
-		_ui->label_stats_source->setText(src);
 
 		if(_camera)
 		{
 			_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
 
-			if(_camera->cameraRGBD() && dynamic_cast<CameraOpenNI2*>(_camera->cameraRGBD()) != 0)
+			if(_camera->camera() && dynamic_cast<CameraOpenNI2*>(_camera->camera()) != 0)
 			{
-				((CameraOpenNI2*)_camera->cameraRGBD())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
+				((CameraOpenNI2*)_camera->camera())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
-				((CameraOpenNI2*)_camera->cameraRGBD())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
+				((CameraOpenNI2*)_camera->camera())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
 				if(CameraOpenNI2::exposureGainAvailable())
 				{
-					((CameraOpenNI2*)_camera->cameraRGBD())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
+					((CameraOpenNI2*)_camera->camera())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
-					((CameraOpenNI2*)_camera->cameraRGBD())->setGain(_preferencesDialog->getSourceOpenni2Gain());
+					((CameraOpenNI2*)_camera->camera())->setGain(_preferencesDialog->getSourceOpenni2Gain());
 				}
 			}
-			if(_camera->camera())
+			if(_camera)
 			{
-				_camera->camera()->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
+				_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
-			}
+				_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
-			if(_camera->cameraRGBD())
-			{
-				_camera->cameraRGBD()->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
-				_camera->cameraRGBD()->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 			}
 		}
 		if(_dbReader)
@@ -2433,23 +2418,25 @@ bool MainWindow::eventFilter(QObject *obj, QEvent *event)
 
 void MainWindow::updateSelectSourceMenu()
 {
-	_ui->actionUsbCamera->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcUsbDevice);
+	_ui->actionUsbCamera->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUsbDevice);
-	_ui->actionImageFiles->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages);
-	_ui->actionVideo->setChecked(_preferencesDialog->isSourceImageUsed() && _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo);
 
-	_ui->actionDatabase->setChecked(_preferencesDialog->isSourceDatabaseUsed());
+	_ui->actionMore_options->setChecked(
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages);
 
-	_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
+	_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
-	_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
+	_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
-	_ui->actionFreenect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect);
+	_ui->actionFreenect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect);
-	_ui->actionOpenNI_CV->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV);
+	_ui->actionOpenNI_CV->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV);
-	_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
+	_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
-	_ui->actionOpenNI2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
-	_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
-	_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
-	_ui->actionFreenect2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect2);
+	_ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
-	_ui->actionStereoDC1394->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoDC1394);
+	_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
-	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoFlyCapture2);
+	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -2674,11 +2661,9 @@ void MainWindow::startDetection()
 {
 	ParametersMap parameters = _preferencesDialog->getAllParameters();
 	// verify source with input rates
-	if((_preferencesDialog->isSourceImageUsed() &&
+	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
-			(_preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcImages ||
+	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
-			 _preferencesDialog->getSourceImageType() == PreferencesDialog::kSrcVideo))
+	   _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
-	   ||
-	   _preferencesDialog->isSourceDatabaseUsed())
 	{
 		float inputRate = _preferencesDialog->getGeneralInputRate();
 		float detectionRate = uStr2Float(parameters.at(Parameters::kRtabmapDetectionRate()));
@@ -2748,9 +2733,7 @@ void MainWindow::startDetection()
 	}
 
 	// Adjust pre-requirements
-	if( !_preferencesDialog->isSourceImageUsed() &&
+	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef)
-		!_preferencesDialog->isSourceDatabaseUsed() &&
-		!_preferencesDialog->isSourceRGBDUsed())
 	{
 		QMessageBox::warning(this,
 				 tr("RTAB-Map"),
@@ -2760,41 +2743,19 @@ void MainWindow::startDetection()
 		return;
 	}
 
-	if(_preferencesDialog->isSourceRGBDUsed())
-	{
-		CameraRGBD * camera = _preferencesDialog->createCameraRGBD();
 
-		if(!camera->init(_preferencesDialog->getCameraInfoDir().toStdString()))
+	if(_preferencesDialog->getSourceDriver() < PreferencesDialog::kSrcDatabase)
+	{
+		Camera * camera = _preferencesDialog->createCamera();
+		if(!camera)
 		{
-			ULOGGER_WARN("init camera failed... ");
-			QMessageBox::warning(this,
-								   tr("RTAB-Map"),
-								   tr("Camera initialization failed..."));
 			emit stateChanged(kInitialized);
-			delete camera;
-			camera = 0;
-			if(_odomThread)
-			{
-				delete _odomThread;
-				_odomThread = 0;
-			}
 			return;
 		}
-		else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
-		{
-			((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
-			((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
-			((CameraOpenNI2*)camera)->setMirroring(_preferencesDialog->getSourceOpenni2Mirroring());
-			if(CameraOpenNI2::exposureGainAvailable())
-			{
-				((CameraOpenNI2*)camera)->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
-				((CameraOpenNI2*)camera)->setGain(_preferencesDialog->getSourceOpenni2Gain());
-			}
-		}
-		camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
-		camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 
 		_camera = new CameraThread(camera);
+		_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
+		_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 
 		//Create odometry thread if rgbd slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
@@ -2823,6 +2784,7 @@ void MainWindow::startDetection()
 				{
 					UERROR("OdomThread must be already deleted here?!");
 					delete _odomThread;
+					_odomThread = 0;
 				}
 				Odometry * odom;
 				if(_preferencesDialog->getOdomStrategy() == 1)
@@ -2845,7 +2807,7 @@ void MainWindow::startDetection()
 			}
 		}
 	}
-	else if(_preferencesDialog->isSourceDatabaseUsed())
+	else if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase)
 	{
 		_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
 								 _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
@@ -2902,58 +2864,6 @@ void MainWindow::startDetection()
 			UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent");
 		}
 	}
-	else
-	{
-		if(_preferencesDialog->isSourceImageUsed())
-		{
-			Camera * camera = 0;
-			// Change type of the camera...
-			//
-			int sourceType = _preferencesDialog->getSourceImageType();
-			UASSERT(sourceType >= PreferencesDialog::kSrcUsbDevice && sourceType <= PreferencesDialog::kSrcVideo);
-			if(sourceType == PreferencesDialog::kSrcImages) //Images
-			{
-				camera = new CameraImages(
-						_preferencesDialog->getSourceImagesPath().append(QDir::separator()).toStdString(),
-						_preferencesDialog->getSourceImagesStartPos(),
-						_preferencesDialog->getSourceImagesRefreshDir(),
-						_preferencesDialog->getGeneralInputRate(),
-						_preferencesDialog->getSourceWidth(),
-						_preferencesDialog->getSourceHeight());
-			}
-			else if(sourceType == PreferencesDialog::kSrcVideo)
-			{
-				camera = new CameraVideo(
-						_preferencesDialog->getSourceVideoPath().toStdString(),
-						_preferencesDialog->getGeneralInputRate(),
-						_preferencesDialog->getSourceWidth(),
-						_preferencesDialog->getSourceHeight());
-			}
-			else //if(sourceType == PreferencesDialog::kSrcUsbDevice)
-			{
-				camera = new CameraVideo(
-						_preferencesDialog->getSourceUsbDeviceId(),
-						_preferencesDialog->getGeneralInputRate(),
-						_preferencesDialog->getSourceWidth(),
-						_preferencesDialog->getSourceHeight());
-			}
-
-			if(!camera->init())
-			{
-				ULOGGER_WARN("init camera failed... ");
-				QMessageBox::warning(this,
-									   tr("RTAB-Map"),
-									   tr("Camera initialization failed..."));
-				emit stateChanged(kInitialized);
-				delete camera;
-				camera = 0;
-				return;
-			}
-			camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
-
-			_camera = new CameraThread(camera);
-		}
-	}
 
 	if(_dataRecorder)
 	{
@@ -3810,64 +3720,49 @@ void MainWindow::updateEditMenu()
 	}
 }
 
-void MainWindow::selectImages()
-{
-	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcImages);
-}
-
-void MainWindow::selectVideo()
-{
-	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcVideo);
-}
-
 void MainWindow::selectStream()
 {
-	_preferencesDialog->selectSourceImage(PreferencesDialog::kSrcUsbDevice);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcUsbDevice);
-}
-
-void MainWindow::selectDatabase()
-{
-	_preferencesDialog->selectSourceDatabase(true);
 }
 
 void MainWindow::selectOpenni()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_PCL);
 }
 
 void MainWindow::selectFreenect()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect);
 }
 
 void MainWindow::selectOpenniCv()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV);
 }
 
 void MainWindow::selectOpenniCvAsus()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI_CV_ASUS);
 }
 
 void MainWindow::selectOpenni2()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcOpenNI2);
 }
 
 void MainWindow::selectFreenect2()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect2);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFreenect2);
 }
 
 void MainWindow::selectStereoDC1394()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoDC1394);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);
 }
 
 void MainWindow::selectStereoFlyCapture2()
 {
-	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoFlyCapture2);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
 }
 
 
@@ -4116,6 +4011,13 @@ void MainWindow::openPreferences()
 	_preferencesDialog->exec();
 }
 
+void MainWindow::openPreferencesSource()
+{
+	_preferencesDialog->setCurrentPanelToSource();
+	openPreferences();
+	this->updateSelectSourceMenu();
+}
+
 void MainWindow::setDefaultViews()
 {
 	_ui->dockWidget_posterior->setVisible(false);

guilib/src/OdometryViewer.cpp

@@ -97,8 +97,10 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
 	decimationSpin_->setMaximum(16);
 	decimationSpin_->setValue(decimation);
 	timeLabel_ = new QLabel(this);
+	QPushButton * resetButton = new QPushButton("reset", this);
 	QPushButton * clearButton = new QPushButton("clear", this);
 	QPushButton * closeButton = new QPushButton("close", this);
+	connect(resetButton, SIGNAL(clicked()), this, SLOT(reset()));
 	connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
 	connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
 
@@ -121,6 +123,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
 	hlayout2->addWidget(decimationSpin_);
 	hlayout2->addWidget(timeLabel_);
 	hlayout2->addStretch(1);
+	hlayout2->addWidget(resetButton);
 	hlayout2->addWidget(clearButton);
 	hlayout2->addWidget(closeButton);
 
@@ -140,6 +143,11 @@ OdometryViewer::~OdometryViewer()
 	UDEBUG("");
 }
 
+void OdometryViewer::reset()
+{
+	this->post(new OdometryResetEvent());
+}
+
 void OdometryViewer::clear()
 {
 	addedClouds_.clear();

guilib/src/ui/mainWindow.ui

@@ -95,16 +95,22 @@
      </property>
      <widget class="QMenu" name="menuImage">
       <property name="title">
-       <string>Image-only</string>
+       <string>RGB camera</string>
+      </property>
+      <property name="icon">
+       <iconset resource="../GuiLib.qrc">
+        <normaloff>:/images/webcam.png</normaloff>:/images/webcam.png</iconset>
       </property>
       <addaction name="actionUsbCamera"/>
-      <addaction name="actionImageFiles"/>
-      <addaction name="actionVideo"/>
      </widget>
      <widget class="QMenu" name="menuRGB_D_camera">
       <property name="title">
        <string>RGB-D camera</string>
       </property>
+      <property name="icon">
+       <iconset resource="../GuiLib.qrc">
+        <normaloff>:/images/kinect_xbox_360.png</normaloff>:/images/kinect_xbox_360.png</iconset>
+      </property>
       <widget class="QMenu" name="menuKinect_for_Xbox_360">
        <property name="title">
         <string>Kinect</string>
@@ -150,7 +156,20 @@
        </property>
        <addaction name="actionFreenect2"/>
       </widget>
-      <widget class="QMenu" name="menuBumblebee2">
+      <addaction name="menuKinect_for_Xbox_360"/>
+      <addaction name="menuXtion_PRO_LIVE"/>
+      <addaction name="menuSense_3D_scanner"/>
+      <addaction name="menuKinect_v2"/>
+     </widget>
+     <widget class="QMenu" name="menuStereo_camera">
+      <property name="title">
+       <string>Stereo camera</string>
+      </property>
+      <property name="icon">
+       <iconset resource="../GuiLib.qrc">
+        <normaloff>:/images/bumblebee2.png</normaloff>:/images/bumblebee2.png</iconset>
+      </property>
+      <widget class="QMenu" name="menuBumblebee2_2">
        <property name="title">
         <string>Bumblebee2</string>
        </property>
@@ -161,15 +180,12 @@
        <addaction name="actionStereoDC1394"/>
        <addaction name="actionStereoFlyCapture2"/>
       </widget>
-      <addaction name="menuKinect_for_Xbox_360"/>
+      <addaction name="menuBumblebee2_2"/>
-      <addaction name="menuXtion_PRO_LIVE"/>
-      <addaction name="menuSense_3D_scanner"/>
-      <addaction name="menuKinect_v2"/>
-      <addaction name="menuBumblebee2"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
+     <addaction name="menuStereo_camera"/>
      <addaction name="menuImage"/>
-     <addaction name="actionDatabase"/>
+     <addaction name="actionMore_options"/>
     </widget>
     <addaction name="menuSelect_source"/>
     <addaction name="separator"/>
@@ -946,34 +962,14 @@
    <property name="checkable">
     <bool>true</bool>
    </property>
+   <property name="icon">
+    <iconset resource="../GuiLib.qrc">
+     <normaloff>:/images/webcam.png</normaloff>:/images/webcam.png</iconset>
+   </property>
    <property name="text">
     <string>Usb camera</string>
    </property>
   </action>
-  <action name="actionImageFiles">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Images...</string>
-   </property>
-  </action>
-  <action name="actionVideo">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Video...</string>
-   </property>
-  </action>
-  <action name="actionDatabase">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Database...</string>
-   </property>
-  </action>
   <action name="actionGenerate_local_map">
    <property name="text">
     <string>Generate graph local map (*.dot)...</string>
@@ -1203,7 +1199,7 @@
   </action>
   <action name="actionStereoFlyCapture2">
    <property name="text">
-    <string>StereoFlyCapture2</string>
+    <string>FlyCapture2</string>
    </property>
   </action>
   <action name="actionSend_goal">
@@ -1226,6 +1222,14 @@
     <string>Default views</string>
    </property>
   </action>
+  <action name="actionMore_options">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>More options...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

tools/Calibration/main.cpp

@@ -25,8 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UConversion.h"
@@ -130,11 +131,10 @@ int main(int argc, char * argv[])
 
 	bool switchImages = false;
 
-	rtabmap::Camera * cameraUsb = 0;
+	rtabmap::Camera * camera = 0;
-	rtabmap::CameraRGBD * camera = 0;
 	if(driver == -1)
 	{
-		cameraUsb = new rtabmap::CameraVideo(device);
+		camera = new rtabmap::CameraVideo(device);
 	}
 	else if(driver == 0)
 	{
@@ -211,17 +211,7 @@ int main(int argc, char * argv[])
 
 	rtabmap::CameraThread * cameraThread = 0;
 
-	if(cameraUsb)
+	if(camera)
-	{
-		if(!cameraUsb->init())
-		{
-			printf("Camera init failed!\n");
-			delete cameraUsb;
-			exit(1);
-		}
-		cameraThread = new rtabmap::CameraThread(cameraUsb);
-	}
-	else if(camera)
 	{
 		if(!camera->init(""))
 		{

tools/Camera/main.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UFile.h"
@@ -40,7 +40,7 @@ void showUsage()
 			"rtabmap-camera [option] \n"
 			" Options:\n"
 			"    --device #            USB camera device id (default 0).\n"
-			"    --rate #              Frame rate (default 30 Hz). 0 means as fast as possible.\n"
+			"    --rate #              Frame rate (default 0 Hz). 0 means as fast as possible.\n"
 			"    --path ""             Path to a directory of images or a video file.\n"
 			"    --calibration ""      Calibration file (*.yaml).\n\n");
 	exit(1);
@@ -53,7 +53,7 @@ int main(int argc, char * argv[])
 
 	int device = 0;
 	std::string path;
-	float rate = 30.0f;
+	float rate = 0.0f;
 	std::string calibrationFile;
 	for(int i=1; i<argc; ++i)
 	{
@@ -164,18 +164,16 @@ int main(int argc, char * argv[])
 
 	if(camera)
 	{
-		if(!camera->init())
+		if(!calibrationFile.empty())
+		{
+			UINFO("Set calibration: %s", calibrationFile.c_str());
+		}
+		if(!camera->init(UDirectory::getDir(calibrationFile), UFile::getName(calibrationFile)))
 		{
 			delete camera;
 			UERROR("Cannot initialize the camera.");
 			return -1;
 		}
-
-		if(!calibrationFile.empty())
-		{
-			UINFO("Set calibration: %s", calibrationFile.c_str());
-			camera->setCalibration(calibrationFile);
-		}
 	}
 
 	if(dbReader)
@@ -189,7 +187,7 @@ int main(int argc, char * argv[])
 	}
 
 	cv::Mat rgb;
-	rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
+	rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
 	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
 	while(!rgb.empty())
 	{
@@ -199,7 +197,7 @@ int main(int argc, char * argv[])
 		if(c == 27)
 			break; // if ESC, break and quit
 
-		rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
+		rgb = camera?camera->takeImage().imageRaw():dbReader->getNextData().data().imageRaw();
 	}
 	cv::destroyWindow("Video");
 	if(camera)

tools/CameraRGBD/main.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -77,7 +78,7 @@ int main(int argc, char * argv[])
 	}
 	UINFO("Using driver %d", driver);
 
-	rtabmap::CameraRGBD * camera = 0;
+	rtabmap::Camera * camera = 0;
 	if(driver == 0)
 	{
 		camera = new rtabmap::CameraOpenni();
@@ -156,42 +157,55 @@ int main(int argc, char * argv[])
 		delete camera;
 		exit(1);
 	}
-	cv::Mat rgb, depth;
+	rtabmap::SensorData data = camera->takeImage();
-	float fx, fy, cx, cy;
+	if(data.imageRaw().cols != data.depthOrRightRaw().cols || data.imageRaw().rows != data.depthOrRightRaw().rows)
-	double stamp = 0.0;
-	camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
-	if(rgb.cols != depth.cols || rgb.rows != depth.rows)
 	{
 		UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
-				rgb.cols, rgb.rows, depth.cols, depth.rows);
+				data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
 	}
-	if(!fx || !fy)
+	if(!data.stereoCameraModel().isValid() && (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
 	{
-		UWARN("fx and/or fy are not set! The registered cloud cannot be shown.");
+		UWARN("Camera not calibrated! The registered cloud cannot be shown.");
 	}
 	pcl::visualization::CloudViewer viewer("cloud");
 	rtabmap::Transform t(1, 0, 0, 0,
 						 0, -1, 0, 0,
 						 0, 0, -1, 0);
-	while(!rgb.empty() && !viewer.wasStopped())
+	while(!data.imageRaw().empty() && !viewer.wasStopped())
 	{
-		if(depth.type() == CV_16UC1 || depth.type() == CV_32FC1)
+		cv::Mat rgb = data.imageRaw();
+		if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
 		{
 			// depth
+			cv::Mat depth = data.depthRaw();
 			if(depth.type() == CV_32FC1)
 			{
 				depth = rtabmap::util3d::cvtDepthFromFloat(depth);
 			}
 
-			if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
+			if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
+					data.cameraModels().size() &&
+					data.cameraModels()[0].isValid())
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
+						rgb, depth,
+						data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy(),
+						data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy());
 				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				viewer.showCloud(cloud, "cloud");
 			}
-			else if(!depth.empty() && fx && fy)
+			else if(!depth.empty() &&
+					data.cameraModels().size() &&
+					data.cameraModels()[0].isValid())
 			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(depth, cx, cy, fx, fy);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
+						depth,
+						data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy(),
+						data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy());
 				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				viewer.showCloud(cloud, "cloud");
 			}
@@ -204,19 +218,25 @@ int main(int argc, char * argv[])
 			cv::imshow("Video", rgb); // show frame
 			cv::imshow("Depth", tmp);
 		}
-		else
+		else if(!data.rightRaw().empty())
 		{
 			// stereo
+			cv::Mat right = data.rightRaw();
 			cv::imshow("Left", rgb); // show frame
-			cv::imshow("Right", depth);
+			cv::imshow("Right", right);
 
-			if(rgb.cols == depth.cols && rgb.rows == depth.rows && fx && fy)
+			if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModel().isValid())
 			{
-				if(depth.channels() == 3)
+				if(right.channels() == 3)
 				{
-					cv::cvtColor(depth, depth, CV_BGR2GRAY);
+					cv::cvtColor(right, right, CV_BGR2GRAY);
 				}
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(rgb, depth, cx, cy, fx, fy);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
+						rgb, right,
+						data.stereoCameraModel().left().cx(),
+						data.stereoCameraModel().left().cy(),
+						data.stereoCameraModel().left().fx(),
+						data.stereoCameraModel().baseline());
 				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				viewer.showCloud(cloud, "cloud");
 			}
@@ -226,9 +246,7 @@ int main(int argc, char * argv[])
 		if(c == 27)
 			break; // if ESC, break and quit
 
-		rgb = cv::Mat();
+		data = camera->takeImage();
-		depth = cv::Mat();
-		camera->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
 	}
 	cv::destroyWindow("Video");
 	cv::destroyWindow("Depth");

tools/ConsoleApp/main.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
 #include "rtabmap/core/Rtabmap.h"
-#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -53,10 +53,6 @@ void showUsage()
 			"  -rateHz #.##                    Acquisition rate (Hz), for convenience\n"
 			"  -repeat #                       Repeat the process on the data set # times (minimum of 1)\n"
 			"  -createGT                       Generate a ground truth file\n"
-			"  -image_width #                  Force an image width (Default 0: original size used).\n"
-			"                                   The height must be also specified if changed.\n"
-			"  -image_height #                 Force an image height (Default 0: original size used)\n"
-			"                                   The height must be also specified if changed.\n"
 			"  -start_at #                     When \"path\" is a directory of images, set this parameter\n"
 			"                                   to start processing at image # (default 1).\n"
 			"  -\"parameter name\" \"value\"       Overwrite a specific RTAB-Map's parameter :\n"
@@ -118,8 +114,6 @@ int main(int argc, char * argv[])
 	int repeat = 0;
 	bool createGT = false;
 	std::string inputDbPath;
-	int imageWidth = 0;
-	int imageHeight = 0;
 	int startAt = 1;
 	ParametersMap pm;
 	ULogger::Level logLevel = ULogger::kError;
@@ -194,40 +188,6 @@ int main(int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-image_width") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				imageWidth = std::atoi(argv[i]);
-				if(imageWidth < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-image_height") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				imageHeight = std::atoi(argv[i]);
-				if(imageHeight < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
 		if(strcmp(argv[i], "-start_at") == 0)
 		{
 			++i;
@@ -328,12 +288,6 @@ int main(int argc, char * argv[])
 		printf("Cannot create a Ground truth if repeat is on.\n");
 		showUsage();
 	}
-	else if((imageWidth && imageHeight == 0) ||
-			(imageHeight && imageWidth == 0))
-	{
-		printf("If imageWidth is set, imageHeight must be too.\n");
-		showUsage();
-	}
 
 	UTimer timer;
 	timer.start();
@@ -342,11 +296,11 @@ int main(int argc, char * argv[])
 	Camera * camera = 0;
 	if(UDirectory::exists(path))
 	{
-		camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight);
+		camera = new CameraImages(path, startAt, false, 1/rate);
 	}
 	else
 	{
-		camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight);
+		camera = new CameraVideo(path, 1/rate);
 	}
 
 	if(!camera || !camera->init())
@@ -395,7 +349,6 @@ int main(int argc, char * argv[])
 	printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
 	printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
 	printf(" Repeating data set = %s\n", repeat?"true":"false");
-	printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
 	printf(" Camera starts at image %d (default 1)\n", startAt);
 	if(createGT)
 	{
@@ -422,23 +375,23 @@ int main(int argc, char * argv[])
 	std::list<std::vector<float> > teleopActions;
 	while(loopDataset <= repeat && g_forever)
 	{
-		cv::Mat img = camera->takeImage();
+		SensorData data = camera->takeImage();
 		int i=0;
 		double maxIterationTime = 0.0;
 		int maxIterationTimeId = 0;
-		while(!img.empty() && g_forever)
+		while(!data.imageRaw().empty() && g_forever)
 		{
 			++imagesProcessed;
 			iterationTimer.start();
 			rtabmapTimer.start();
-			rtabmap.process(img);
+			rtabmap.process(data.imageRaw());
 			double rtabmapTime = rtabmapTimer.elapsed();
 			loopClosureId = rtabmap.getLoopClosureId();
 			if(rtabmap.getLoopClosureId())
 			{
 				++countLoopDetected;
 			}
-			img = camera->takeImage();
+			data = camera->takeImage();
 			if(++count % 100 == 0)
 			{
 				printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",

tools/DataRecorder/main.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/core/CameraRGBD.h>
+#include <rtabmap/core/CameraStereo.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/gui/DataRecorder.h>
@@ -174,7 +175,7 @@ int main (int argc, char * argv[])
 	signal(SIGTERM, &sighandler);
 	signal(SIGINT, &sighandler);
 
-	rtabmap::CameraRGBD * camera = 0;
+	rtabmap::Camera * camera = 0;
 	rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 0)
 	{
@@ -263,7 +264,7 @@ int main (int argc, char * argv[])
 			app->processEvents();
 		}
 
-		if(cam->init())
+		if(camera->init())
 		{
 			cam->start();
 

tools/OdometryViewer/main.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/gui/OdometryViewer.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/core/CameraRGBD.h>
+#include <rtabmap/core/CameraStereo.h>
 #include <rtabmap/core/DBReader.h>
 #include <rtabmap/core/VWDictionary.h>
 #include <QApplication>
@@ -725,7 +726,7 @@ int main (int argc, char * argv[])
 	}
 	else
 	{
-		rtabmap::CameraRGBD * camera = 0;
+		rtabmap::Camera * camera = 0;
 		rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 		if(driver == 0)
 		{