Refactoring CameraRGBD class (now including OpenNI, OpenNI2, OpenNI from OpenCV and Freenect)

Added tool to test RGB-D camera: rtabmap-rgbd_camera
Fixed Freenect corrupted depth image (after some time)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1366 f169173b-cf89-36c8-b27e-44dbe73f0c83

cmake_modules/FindFreenect.cmake

@@ -6,7 +6,7 @@
 #  Freenect_INCLUDE_DIRS - The Freenect include directory.
 #  Freenect_LIBRARIES     - The Freenect library to link against.
 
-FIND_PATH(Freenect_INCLUDE_DIRS libfreenect.h PATH_SUFFIXES libfreenect)
+FIND_PATH(Freenect_INCLUDE_DIRS libfreenect-registration.h PATH_SUFFIXES libfreenect)
 
 FIND_LIBRARY(Freenect_LIBRARY NAMES freenect)
 FIND_LIBRARY(Freenect_sync_LIBRARY NAMES freenect_sync)

corelib/include/rtabmap/core/Camera.h

@@ -31,12 +31,6 @@
 class UDirectory;
 class UTimer;
 
-namespace openni
-{
-class Device;
-class VideoStream;
-}
-
 namespace rtabmap
 {
 
@@ -48,20 +42,16 @@ class RTABMAP_EXP Camera
 {
 public:
 	virtual ~Camera();
-	cv::Mat takeImage(); // backward compatibility
-	void takeImage(cv::Mat & rgb);
-	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	cv::Mat takeImage();
 	virtual bool init() = 0;
 
 	//getters
 	void getImageSize(unsigned int & width, unsigned int & height);
 	float getImageRate() const {return _imageRate;}
-	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;}
 
 protected:
 	/**
@@ -71,17 +61,14 @@ protected:
 	 */
 	Camera(float imageRate = 0,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0);
+			unsigned int imageHeight = 0);
 
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
+	virtual cv::Mat captureImage() = 0;
 
 private:
 	float _imageRate;
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
-	unsigned int _framesDropped;
-	Transform _localTransform;
 	UTimer * _frameRateTimer;
 };
 
@@ -98,15 +85,14 @@ public:
 			bool refreshDir = false,
 			float imageRate = 0,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0);
+			unsigned int imageHeight = 0);
 	virtual ~CameraImages();
 
 	virtual bool init();
 	std::string getPath() const {return _path;}
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	virtual cv::Mat captureImage();
 
 private:
 	std::string _path;
@@ -135,13 +121,11 @@ public:
 	CameraVideo(int usbDevice = 0,
 			float imageRate = 0,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0);
+			unsigned int imageHeight = 0);
 	CameraVideo(const std::string & filePath,
 			float imageRate = 0,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0);
+			unsigned int imageHeight = 0);
 	virtual ~CameraVideo();
 
 	virtual bool init();
@@ -149,7 +133,7 @@ public:
 	const std::string & getFilePath() const {return _filePath;}
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+	virtual cv::Mat captureImage();
 
 private:
 	// File type
@@ -163,58 +147,4 @@ private:
 };
 
 
-
-/////////////////////////
-// CameraRGBD
-/////////////////////////
-class RTABMAP_EXP CameraRGBD :
-	public Camera
-{
-
-public:
-	static bool available();
-
-public:
-	CameraRGBD(float imageRate = 0,
-				bool asus = false);
-	virtual ~CameraRGBD();
-
-	virtual bool init();
-
-protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
-
-private:
-	bool _asus;
-	cv::VideoCapture _capture;
-	float _depthFocal;
-};
-
-/////////////////////////
-// CameraOpenNI2
-/////////////////////////
-class RTABMAP_EXP CameraOpenNI2 :
-	public Camera
-{
-
-public:
-	static bool available();
-
-public:
-	CameraOpenNI2(float imageRate = 0);
-	virtual ~CameraOpenNI2();
-
-	virtual bool init();
-
-protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
-
-private:
-	openni::Device * _device;
-	openni::VideoStream * _color;
-	openni::VideoStream * _depth;
-	float _depthFocal;
-};
-
-
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraFreenect.h

@@ -1,98 +0,0 @@
-/*
- * CameraFreenect.h
- *
- *  Created on: 2014-06-02
- *      Author: Mathieu
- */
-
-#ifndef CAMERAFREENECT_H_
-#define CAMERAFREENECT_H_
-
-#include <rtabmap/core/RtabmapExp.h>
-
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/UEventsSender.h>
-#include <rtabmap/utilite/UThread.h>
-
-#include <opencv2/opencv.hpp>
-#include <stdint.h>
-
-class UTimer;
-typedef struct _freenect_context freenect_context;
-typedef struct _freenect_device freenect_device;
-
-namespace rtabmap {
-
-class RTABMAP_EXP FreenectDevice {
-  public:
-	FreenectDevice(freenect_context *ctx, int index);
-	virtual ~FreenectDevice();
-	void startVideo();
-	void stopVideo();
-	void startDepth();
-	void stopDepth();
-
-	bool init();
-
-	cv::Mat getRgb();
-	cv::Mat getDepth();
-	float getDepthFocal() const {return depthFocal_;}
-
-	// Do not call directly even in child
-	void VideoCallback(void *video, uint32_t timestamp);
-	// Do not call directly even in child
-	void DepthCallback(void *depth, uint32_t timestamp);
-  private:
-	static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp);
-	static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp);
-
-	//noncopyable
-	FreenectDevice( const FreenectDevice& );
-	const FreenectDevice& operator=( const FreenectDevice& );
-
-  private:
-	int index_;
-	freenect_context * ctx_;
-	freenect_device * device_;
-	cv::Mat depthMat_;
-	cv::Mat rgbMat_;
-	UMutex depthMutex_;
-	UMutex rgbMutex_;
-	bool depthReady_;
-	bool rgbReady_;
-	float depthFocal_;
-};
-
-class RTABMAP_EXP CameraFreenect : public UEventsSender, public UThread
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect(int deviceId= 0,
-			float rate=0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraFreenect();
-
-    bool init();
-
-    void setFrameRate(float rate);
-
-private:
-    virtual void mainLoopBegin();
-    virtual void mainLoop();
-    virtual void mainLoopEnd();
-
-private:
-    int deviceId_;
-    float rate_;
-    UTimer * frameRateTimer_;
-    Transform localTransform_; // transform from camera_optical_link to base_link
-    int seq_;
-    freenect_context * ctx_;
-    FreenectDevice * freenectDevice_;
-};
-
-} /* namespace rtabmap */
-#endif /* CAMERAFREENECT_H_ */

corelib/include/rtabmap/core/CameraOpenni.h

@@ -1,63 +0,0 @@
-/*
- * CameraOpenni.h
- *
- *  Created on: 2013-08-22
- *      Author: Mathieu
- */
-
-#ifndef CAMERAOPENNI_H_
-#define CAMERAOPENNI_H_
-
-#include <rtabmap/core/RtabmapExp.h>
-
-#include <pcl/io/openni_camera/openni_depth_image.h>
-#include <pcl/io/openni_camera/openni_image.h>
-
-#include <boost/signals2/connection.hpp>
-
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/UEventsSender.h>
-
-class UTimer;
-
-namespace pcl
-{
-	class Grabber;
-}
-
-namespace rtabmap {
-
-class RTABMAP_EXP CameraOpenni : public UEventsSender
-{
-public:
-	// default local transform z in, x right, y down));
-	CameraOpenni(const std::string & deviceId="",
-			float rate=0,
-			const Transform & localTRansform = Transform::getIdentity());
-	virtual ~CameraOpenni();
-
-    void image_cb (
-    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
-			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
-			float constant);
-
-    bool init();
-    void start();
-    void pause();
-    void kill();
-    bool isRunning();
-
-    void setFrameRate(float rate);
-
-private:
-    pcl::Grabber* interface_;
-    std::string deviceId_;
-    float rate_;
-    UTimer * frameRateTimer_;
-    Transform localTransform_; // transform from camera_optical_link to base_link
-    int seq_;
-    boost::signals2::connection connection_;
-};
-
-} /* namespace rtabmap */
-#endif /* CAMERAOPENNI_H_ */

corelib/include/rtabmap/core/CameraRGBD.h

@@ -0,0 +1,215 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include "rtabmap/core/Image.h"
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include <set>
+#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;
+class VideoStream;
+}
+
+namespace pcl
+{
+	class Grabber;
+}
+
+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 & depthConstant);
+	virtual bool init() = 0;
+
+	//getters
+	float getImageRate() const {return _imageRate;}
+	const Transform & getLocalTransform() const {return _localTransform;}
+
+	//setters
+	void setImageRate(float imageRate) {_imageRate = imageRate;}
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
+
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param imageRate : image/second , 0 for fast as the camera can
+	 */
+	CameraRGBD(float imageRate = 0,
+				const Transform & localTransform = Transform::getIdentity());
+
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
+
+private:
+	float _imageRate;
+	Transform _localTransform;
+	UTimer * _frameRateTimer;
+};
+
+/////////////////////////
+// CameraOpenNIPCL
+/////////////////////////
+class RTABMAP_EXP CameraOpenni :
+	public CameraRGBD
+{
+public:
+	static bool available() {return true;}
+
+public:
+	// default local transform z in, x right, y down));
+	CameraOpenni(const std::string & deviceId="",
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraOpenni();
+
+    void image_cb (
+    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+			float constant);
+
+    bool init();
+
+protected:
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+
+private:
+    pcl::Grabber* interface_;
+    std::string deviceId_;
+    boost::signals2::connection connection_;
+    cv::Mat depth_;
+    cv::Mat rgb_;
+    float depthConstant_;
+    UMutex dataMutex_;
+    USemaphore dataReady_;
+};
+
+/////////////////////////
+// CameraOpenNICV
+/////////////////////////
+class RTABMAP_EXP CameraOpenNICV :
+	public CameraRGBD
+{
+
+public:
+	static bool available();
+
+public:
+	CameraOpenNICV(bool asus = false,
+					float imageRate = 0,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraOpenNICV();
+
+	virtual bool init();
+
+protected:
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+
+private:
+	bool _asus;
+	cv::VideoCapture _capture;
+	float _depthFocal;
+};
+
+/////////////////////////
+// CameraOpenNI2
+/////////////////////////
+class RTABMAP_EXP CameraOpenNI2 :
+	public CameraRGBD
+{
+
+public:
+	static bool available();
+
+public:
+	CameraOpenNI2(float imageRate = 0,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraOpenNI2();
+
+	virtual bool init();
+
+protected:
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+
+private:
+	openni::Device * _device;
+	openni::VideoStream * _color;
+	openni::VideoStream * _depth;
+	float _depthFocal;
+};
+
+
+/////////////////////////
+// CameraFreenect
+/////////////////////////
+class FreenectDevice;
+
+class RTABMAP_EXP CameraFreenect :
+	public CameraRGBD
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraFreenect(int deviceId= 0,
+					float imageRate=0.0f,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraFreenect();
+
+	bool init();
+
+protected:
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
+
+private:
+	int deviceId_;
+	freenect_context * ctx_;
+	FreenectDevice * freenectDevice_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -21,29 +21,27 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <rtabmap/utilite/UThreadNode.h>
-#include <rtabmap/utilite/UEventsHandler.h>
-
-#include "rtabmap/core/Parameters.h"
-
-#include <stack>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
 
 namespace rtabmap
 {
 
 class Camera;
+class CameraRGBD;
 
 /**
  * Class CameraThread
  *
  */
 class RTABMAP_EXP CameraThread :
-	public UThreadNode,
+	public UThread,
 	public UEventsSender
 {
 public:
 	// ownership transferred
-	CameraThread(Camera * camera, bool autoRestart = false);
+	CameraThread(Camera * camera);
+	CameraThread(CameraRGBD * camera);
 	virtual ~CameraThread();
 
 	bool init(); // call camera->init()
@@ -51,15 +49,14 @@ public:
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
-	void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
-	Camera * getCamera() {return _camera;}
+	void setImageRate(float imageRate);
 
 private:
 	virtual void mainLoop();
 
 private:
 	Camera * _camera;
-	bool _autoRestart;
+	CameraRGBD * _cameraRGBD;
 	int _seq;
 };
 

corelib/src/CMakeLists.txt

@@ -13,8 +13,7 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
-    CameraOpenni.cpp
-    CameraFreenect.cpp
+    CameraRGBD.cpp
     
     EpipolarGeometry.cpp
 	VisualWord.cpp

corelib/src/Camera.cpp

@@ -33,23 +33,15 @@
 #include <iostream>
 #include <cmath>
 
-#ifdef WITH_OPENNI2
-
-#endif
-#include <OpenNI.h>
-
 namespace rtabmap
 {
 
 Camera::Camera(float imageRate,
 		unsigned int imageWidth,
-		unsigned int imageHeight,
-		unsigned int framesDropped) :
+		unsigned int imageHeight) :
 	_imageRate(imageRate),
 	_imageWidth(imageWidth),
 	_imageHeight(imageHeight),
-	_framesDropped(framesDropped),
-	_localTransform(Transform::getIdentity()),
 	_frameRateTimer(new UTimer())
 {
 }
@@ -76,21 +68,7 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
 
 cv::Mat Camera::takeImage()
 {
-	cv::Mat rgb, depth;
-	float depthConstant = 0.0f;
-	takeImage(rgb, depth, depthConstant);
-	return rgb;
-}
-
-void Camera::takeImage(cv::Mat & rgb)
-{
-	cv::Mat depth;
-	float depthConstant = 0.0f;
-	takeImage(rgb, depth, depthConstant);
-}
-
-void Camera::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
-{
+	cv::Mat img;
 	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
 	if(imageRate>0)
 	{
@@ -112,33 +90,9 @@ void Camera::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
 	}
 
 	UTimer timer;
-	this->captureImage(rgb, depth, depthConstant);
+	img = this->captureImage();
 	UDEBUG("Time capturing image = %fs", timer.ticks());
-
-	if(!rgb.empty())
-	{
-		UASSERT(rgb.depth() == CV_8U);
-
-		if(_framesDropped)
-		{
-			unsigned int count = 0;
-			while(count++ < _framesDropped)
-			{
-				cv::Mat tmp,tmp2;
-				float tmpf;
-				this->captureImage(tmp, tmp2, tmpf);
-				if(!tmp.empty())
-				{
-					UDEBUG("frame dropped (%d/%d)", (int)count, (int)_framesDropped);
-				}
-				else
-				{
-					break;
-				}
-			}
-			UDEBUG("Frames dropped time = %fs", timer.ticks());
-		}
-	}
+	return img;
 }
 
 /////////////////////////
@@ -149,9 +103,8 @@ CameraImages::CameraImages(const std::string & path,
 					 bool refreshDir,
 					 float imageRate,
 					 unsigned int imageWidth,
-					 unsigned int imageHeight,
-					 unsigned int framesDropped) :
-	Camera(imageRate, imageWidth, imageHeight, framesDropped),
+					 unsigned int imageHeight) :
+	Camera(imageRate, imageWidth, imageHeight),
 	_path(path),
 	_startAt(startAt),
 	_refreshDir(refreshDir),
@@ -196,8 +149,9 @@ bool CameraImages::init()
 	return _dir->isValid();
 }
 
-void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+cv::Mat CameraImages::captureImage()
 {
+	cv::Mat img;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
@@ -214,7 +168,7 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
 				{
 					_lastFileName = *fileNames.rbegin();
 					std::string fullPath = _path + _lastFileName;
-					rgb = cv::imread(fullPath.c_str());
+					img = cv::imread(fullPath.c_str());
 				}
 			}
 		}
@@ -234,19 +188,20 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
 				{
 					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
 #if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
-					rgb = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
+					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
 #else
-					rgb = cv::imread(fullPath.c_str(), -1);
+					img = cv::imread(fullPath.c_str(), -1);
 #endif
-					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", rgb.cols, rgb.rows, rgb.channels(), rgb.elemSize(), rgb.total());
+					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
+							img.cols, img.rows, img.channels(), img.elemSize(), img.total());
 
 					// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
-					if(rgb.depth() != CV_8U)
+					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());
-						rgb = cv::Mat(i, true);
+						img = cv::Mat(i, true);
 						cvReleaseImage(&i);
 					}
 				}
@@ -262,16 +217,17 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
 	unsigned int h;
 	this->getImageSize(w, h);
 
-	if(!rgb.empty() &&
+	if(!img.empty() &&
 	   w &&
 	   h &&
-	   w != (unsigned int)rgb.cols &&
-	   h != (unsigned int)rgb.rows)
+	   w != (unsigned int)img.cols &&
+	   h != (unsigned int)img.rows)
 	{
 		cv::Mat resampled;
-		cv::resize(rgb, resampled, cv::Size(w, h));
-		rgb = resampled;
+		cv::resize(img, resampled, cv::Size(w, h));
+		img = resampled;
 	}
+	return img;
 }
 
 
@@ -282,9 +238,8 @@ void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCon
 CameraVideo::CameraVideo(int usbDevice,
 						 float imageRate,
 						 unsigned int imageWidth,
-						 unsigned int imageHeight,
-						 unsigned int framesDropped) :
-	Camera(imageRate, imageWidth, imageHeight, framesDropped),
+						 unsigned int imageHeight) :
+	Camera(imageRate, imageWidth, imageHeight),
 	_src(kUsbDevice),
 	_usbDevice(usbDevice)
 {
@@ -294,9 +249,8 @@ CameraVideo::CameraVideo(int usbDevice,
 CameraVideo::CameraVideo(const std::string & filePath,
 						   float imageRate,
 						   unsigned int imageWidth,
-						   unsigned int imageHeight,
-						   unsigned int framesDropped) :
-	Camera(imageRate, imageWidth, imageHeight, framesDropped),
+						   unsigned int imageHeight) :
+	Camera(imageRate, imageWidth, imageHeight),
 	_filePath(filePath),
 	_src(kVideoFile),
 	_usbDevice(0)
@@ -348,30 +302,31 @@ bool CameraVideo::init()
 	return true;
 }
 
-void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+cv::Mat CameraVideo::captureImage()
 {
+	cv::Mat img;
 	if(_capture.isOpened())
 	{
-		if(_capture.read(rgb))
+		if(_capture.read(img))
 		{
 			unsigned int w;
 			unsigned int h;
 			this->getImageSize(w, h);
 
-			if(!rgb.empty() &&
+			if(!img.empty() &&
 			   w &&
 			   h &&
-			   w != (unsigned int)rgb.cols &&
-			   h != (unsigned int)rgb.rows)
+			   w != (unsigned int)img.cols &&
+			   h != (unsigned int)img.rows)
 			{
 				cv::Mat resampled;
-				cv::resize(rgb, resampled, cv::Size(w, h));
-				rgb = resampled;
+				cv::resize(img, resampled, cv::Size(w, h));
+				img = resampled;
 			}
 			else
 			{
 				// clone required
-				rgb = rgb.clone();
+				img = img.clone();
 			}
 		}
 		else if(_usbDevice)
@@ -383,297 +338,7 @@ void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthCons
 	{
 		ULOGGER_WARN("The camera must be initialized before requesting an image.");
 	}
-}
-
-/////////////////////////
-// CameraRGBD
-/////////////////////////
-bool CameraRGBD::available()
-{
-	return cv::getBuildInformation().find("OpenNI:                      YES") != std::string::npos;
-}
-
-CameraRGBD::CameraRGBD(float imageRate, bool asus) :
-	Camera(imageRate),
-	_asus(asus),
-	_depthFocal(0.0f)
-{
-
-}
-
-CameraRGBD::~CameraRGBD()
-{
-	_capture.release();
-}
-
-bool CameraRGBD::init()
-{
-	if(_capture.isOpened())
-	{
-		_capture.release();
-	}
-
-	ULOGGER_DEBUG("CameraRGBD::init()");
-	_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
-	if(_capture.isOpened())
-	{
-		_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
-		_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
-		// Print some avalible device settings.
-		UINFO("Depth generator output mode:");
-		UINFO("FRAME_WIDTH        %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
-		UINFO("FRAME_HEIGHT       %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
-		UINFO("FRAME_MAX_DEPTH    %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
-		UINFO("BASELINE           %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
-		UINFO("FPS                %f", _capture.get( CV_CAP_PROP_FPS ));
-		UINFO("Focal              %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
-		UINFO("REGISTRATION       %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
-		if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
-		{
-			UERROR("Depth registration is not activated on this device!");
-		}
-		if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
-		{
-			UINFO("Image generator output mode:");
-			UINFO("FRAME_WIDTH    %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
-			UINFO("FRAME_HEIGHT   %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
-			UINFO("FPS            %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
-		}
-		else
-		{
-			UERROR("CameraRGBD: Device doesn't contain image generator.");
-			_capture.release();
-			return false;
-		}
-	}
-	else
-	{
-		ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!");
-		_capture.release();
-		return false;
-	}
-	return true;
-}
-
-void CameraRGBD::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
-{
-	if(_capture.isOpened())
-	{
-		_capture.grab();
-		_capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP );
-		_capture.retrieve( rgb, CV_CAP_OPENNI_BGR_IMAGE );
-
-		depth = depth.clone();
-		rgb = rgb.clone();
-		UASSERT(_depthFocal > 0.0f);
-		depthConstant = 1.0f/_depthFocal;
-	}
-	else
-	{
-		ULOGGER_WARN("The camera must be initialized before requesting an image.");
-	}
-}
-
-
-/////////////////////////
-// CameraOpenNI2
-/////////////////////////
-bool CameraOpenNI2::available()
-{
-#ifdef WITH_OPENNI2
-	return true;
-#else
-	return false;
-#endif
-}
-
-CameraOpenNI2::CameraOpenNI2(float imageRate) :
-	Camera(imageRate),
-	_device(new openni::Device()),
-	_color(new openni::VideoStream()),
-	_depth(new openni::VideoStream()),
-	_depthFocal(0.0f)
-{
-}
-
-CameraOpenNI2::~CameraOpenNI2()
-{
-	_color->stop();
-	_color->destroy();
-	_depth->stop();
-	_depth->destroy();
-	_device->close();
-	openni::OpenNI::shutdown();
-
-	delete _device;
-	delete _color;
-	delete _depth;
-}
-
-bool CameraOpenNI2::init()
-{
-	openni::OpenNI::initialize();
-
-	if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK)
-	{
-		UERROR("CameraOpenNI2: Cannot open device.");
-		_device->close();
-		openni::OpenNI::shutdown();
-		return false;
-	}
-
-	if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
-	{
-		UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
-		_device->close();
-		openni::OpenNI::shutdown();
-		return false;
-	}
-
-	if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
-	  _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
-	{
-		UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
-		_device->close();
-		openni::OpenNI::shutdown();
-		return false;
-	}
-
-	if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
-	{
-		UERROR("CameraOpenNI2: Cannot create depth stream.");
-		_device->close();
-		openni::OpenNI::shutdown();
-		return false;
-	}
-
-	if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
-	{
-		UERROR("CameraOpenNI2: Cannot create color stream.");
-		_depth->destroy();
-		_device->close();
-		openni::OpenNI::shutdown();
-		return false;
-	}
-
-	if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
-	{
-		UERROR("CameraOpenNI2: Failed to set depth/color registration.");
-	}
-
-	_depth->setMirroringEnabled(false);
-	_color->setMirroringEnabled(false);
-
-	const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
-	for(int i=0; i<depthVideoModes.getSize(); ++i)
-	{
-		UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
-				i,
-				depthVideoModes[i].getFps(),
-				depthVideoModes[i].getPixelFormat(),
-				depthVideoModes[i].getResolutionX(),
-				depthVideoModes[i].getResolutionY());
-	}
-
-	const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
-	for(int i=0; i<colorVideoModes.getSize(); ++i)
-	{
-		UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
-				i,
-				colorVideoModes[i].getFps(),
-				colorVideoModes[i].getPixelFormat(),
-				colorVideoModes[i].getResolutionX(),
-				colorVideoModes[i].getResolutionY());
-	}
-
-	openni::VideoMode mMode;
-	mMode.setFps(30);
-	mMode.setResolution(640,480);
-	mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
-	_depth->setVideoMode(mMode);
-
-	openni::VideoMode mModeColor;
-	mModeColor.setFps(30);
-	mModeColor.setResolution(640,480);
-	mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
-	_color->setVideoMode(mModeColor);
-
-	UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
-			_depth->getVideoMode().getFps(),
-			_depth->getVideoMode().getPixelFormat(),
-			_depth->getVideoMode().getResolutionX(),
-			_depth->getVideoMode().getResolutionY(),
-			_depth->getHorizontalFieldOfView(),
-			_depth->getVerticalFieldOfView());
-
-	bool registered = true;
-	if(registered)
-	{
-		_depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
-	}
-	else
-	{
-		_depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
-	}
-	UINFO("depth focal = %f", _depthFocal);
-
-	UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
-			_color->getVideoMode().getFps(),
-			_color->getVideoMode().getPixelFormat(),
-			_color->getVideoMode().getResolutionX(),
-			_color->getVideoMode().getResolutionY(),
-			_color->getHorizontalFieldOfView(),
-			_color->getVerticalFieldOfView());
-
-	if(_depth->start() != openni::STATUS_OK ||
-	   _color->start() != openni::STATUS_OK)
-	{
-		UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
-		_depth->stop();
-		_color->stop();
-		_depth->destroy();
-		_color->destroy();
-		_device->close();
-		openni::OpenNI::shutdown();
-		return false;
-	}
-
-	uSleep(1000); // just to make sure the sensor is correctly initialized
-
-	return true;
-}
-
-void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
-{
-	if(_device->isValid() &&
-		_depth->isValid() &&
-		_color->isValid() &&
-		_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
-		_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
-	{
-		openni::VideoFrameRef depthFrame, colorFrame;
-
-		_depth->readFrame(&depthFrame);
-		_color->readFrame(&colorFrame);
-
-		if(depthFrame.isValid() && colorFrame.isValid())
-		{
-			int h=depthFrame.getHeight();
-			int w=depthFrame.getWidth();
-			depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
-
-			h=colorFrame.getHeight();
-			w=colorFrame.getWidth();
-			cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
-			cv::cvtColor(tmp, rgb, CV_RGB2BGR);
-		}
-		UASSERT(_depthFocal != 0.0f);
-		depthConstant = 1.0f/_depthFocal;
-	}
-	else
-	{
-		ULOGGER_WARN("The camera must be initialized before requesting an image.");
-	}
+	return img;
 }
 
 } // namespace rtabmap

corelib/src/CameraFreenect.cpp

@@ -1,316 +0,0 @@
-/*
- * CameraFreenect.cpp
- *
- *  Created on: 2014-06-02
- *      Author: Mathieu
- */
-
-#include "rtabmap/core/CameraFreenect.h"
-#include "rtabmap/core/CameraEvent.h"
-#include "rtabmap/utilite/ULogger.h"
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UEventsManager.h>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#ifdef WITH_FREENECT
-#include <libfreenect.h>
-#include <libfreenect-registration.h>
-#endif
-
-namespace rtabmap {
-
-//
-// FreenectDevice
-//
-FreenectDevice::FreenectDevice(freenect_context * ctx, int index) :
-	index_(index),
-	ctx_(ctx),
-	device_(0),
-	depthMat_(cv::Size(640,480),CV_16UC1),
-	rgbMat_(cv::Size(640,480), CV_8UC3, cv::Scalar(0)),
-	depthReady_(false),
-	rgbReady_(false),
-	depthFocal_(0.0f)
-{
-	UASSERT(ctx_ != 0);
-}
-
-#ifdef WITH_FREENECT
-FreenectDevice::~FreenectDevice() {
-	if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
-}
-void FreenectDevice::startVideo() {
-	if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
-}
-void FreenectDevice::stopVideo() {
-	if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
-}
-void FreenectDevice::startDepth() {
-	if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
-}
-void FreenectDevice::stopDepth() {
-	if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
-}
-
-bool FreenectDevice::init()
-{
-	if(freenect_open_device(ctx_, &device_, index_) < 0)
-	{
-		UERROR("FreenectDevice: Cannot open Kinect");
-		return false;
-	}
-	freenect_set_user(device_, this);
-	freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
-	freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
-	freenect_set_depth_callback(device_, freenect_depth_callback);
-	freenect_set_video_callback(device_, freenect_video_callback);
-
-	bool registered = true;
-	float rgb_focal_length_sxga = 1050.0f;
-	float width_sxga = 1280.0f;
-	float width = freenect_get_current_depth_mode(device_).width;
-	float scale = width / width_sxga;
-	if(registered)
-	{
-		depthFocal_ =  rgb_focal_length_sxga * scale;
-	}
-	else
-	{
-		freenect_registration reg = freenect_copy_registration(device_);
-		float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
-		freenect_destroy_registration(&reg);
-
-		depthFocal_ =  depth_focal_length_sxga * scale;
-	}
-
-	UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
-
-
-	return true;
-}
-
-void FreenectDevice::freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
-	FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
-	device->DepthCallback(depth, timestamp);
-}
-void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
-	FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
-	device->VideoCallback(video, timestamp);
-}
-#else
-FreenectDevice::~FreenectDevice() {}
-void FreenectDevice::startVideo() {}
-void FreenectDevice::stopVideo() {}
-void FreenectDevice::startDepth() {}
-void FreenectDevice::stopDepth() {}
-
-bool FreenectDevice::init()
-{
-	UERROR("RTAB-Map is not built with Freenect support!");
-	return false;
-}
-void FreenectDevice::freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {}
-void FreenectDevice::freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {}
-#endif
-
-// Do not call directly even in child
-void FreenectDevice::VideoCallback(void* _rgb, uint32_t timestamp)
-{
-	rgbMutex_.lock();
-	uint8_t* rgb = static_cast<uint8_t*>(_rgb);
-	rgbMat_.data = rgb;
-	rgbReady_ = true;
-	rgbMutex_.unlock();
-}
-
-// Do not call directly even in child
-void FreenectDevice::DepthCallback(void* _depth, uint32_t timestamp)
-{
-	depthMutex_.lock();
-	uint16_t* depth = static_cast<uint16_t*>(_depth);
-	depthMat_.data = (uchar*) depth;
-	depthReady_ = true;
-	depthMutex_.unlock();
-}
-
-cv::Mat FreenectDevice::getRgb()
-{
-	cv::Mat out;
-	rgbMutex_.lock();
-	if(rgbReady_)
-	{
-		cv::cvtColor(rgbMat_, out, CV_RGB2BGR);
-		rgbReady_ = false;
-	}
-	rgbMutex_.unlock();
-	return out;
-}
-
-cv::Mat FreenectDevice::getDepth()
-{
-	cv::Mat out;
-	depthMutex_.lock();
-	if(depthReady_)
-	{
-		depthMat_.copyTo(out);
-		depthReady_ = false;
-	}
-	depthMutex_.unlock();
-	return out;
-}
-
-
-//
-// CameraFreenect
-//
-bool CameraFreenect::available()
-{
-#ifdef WITH_FREENECT
-	return true;
-#else
-	return false;
-#endif
-}
-
-CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform & localTransform) :
-		deviceId_(deviceId),
-		rate_(inputRate),
-		frameRateTimer_(new UTimer()),
-		localTransform_(localTransform),
-		seq_(0),
-		ctx_(0),
-		freenectDevice_(0)
-{
-#ifdef WITH_FREENECT
-	if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
-	// We claim both the motor and camera devices, since this class exposes both.
-	// It does not support audio, so we do not claim it.
-	freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
-#endif
-}
-
-CameraFreenect::~CameraFreenect()
-{
-	UDEBUG("");
-	join(true);
-	if(freenectDevice_)
-	{
-		delete freenectDevice_;
-		freenectDevice_ = 0;
-	}
-#ifdef WITH_FREENECT
-	if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
-#endif
-	delete frameRateTimer_;
-}
-
-bool CameraFreenect::init()
-{
-#ifdef WITH_FREENECT
-	if(!this->isRunning())
-	{
-		if(freenectDevice_)
-		{
-			delete freenectDevice_;
-			freenectDevice_ = 0;
-		}
-
-		seq_ = 0;
-
-		if(freenect_num_devices(ctx_) > 0)
-		{
-			freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
-			if(freenectDevice_->init())
-			{
-				return true;
-			}
-			delete freenectDevice_;
-			freenectDevice_ = 0;
-		}
-		else
-		{
-			UERROR("CameraFreenect: No devices connected!");
-		}
-	}
-	else
-	{
-		UERROR("CameraFreenect: Cannot initialize the camera because it is already running...");
-	}
-#else
-	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
-#endif
-	return false;
-}
-
-
-void CameraFreenect::setFrameRate(float rate)
-{
-	rate_ = rate;
-}
-
-void CameraFreenect::mainLoopBegin()
-{
-	if(freenectDevice_)
-	{
-		freenectDevice_->startDepth();
-		freenectDevice_->startVideo();
-		frameRateTimer_->start();
-	}
-	else
-	{
-		UERROR("CameraFreenect: init should be called before starting the camera.");
-		this->kill();
-	}
-}
-
-void CameraFreenect::mainLoop()
-{
-#ifdef WITH_FREENECT
-	timeval t;
-	t.tv_sec = 0;
-	t.tv_usec = 10000;
-	if(freenect_process_events_timeout(ctx_, &t) < 0) UERROR("Cannot process freenect events");
-
-	if(freenectDevice_ && !this->isKilled())
-	{
-		float imageRate = rate_==0.0f?33.0f:rate_; // limit to 33Hz if infinity
-
-		if(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));
-
-			cv::Mat depth = freenectDevice_->getDepth();
-			cv::Mat rgb = freenectDevice_->getRgb();
-
-			if(depth.empty())
-			{
-				UWARN("CameraFreenect: Depth not ready! Try to reduce the image rate to avoid this warning...");
-				return;
-			}
-
-			if(rgb.empty())
-			{
-				UWARN("CameraFreenect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
-				return;
-			}
-			UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
-			float constant = 1.0f/freenectDevice_->getDepthFocal();
-			this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
-		}
-	}
-#endif
-}
-
-void CameraFreenect::mainLoopEnd()
-{
-	if(freenectDevice_)
-	{
-		freenectDevice_->stopDepth();
-		freenectDevice_->stopVideo();
-	}
-}
-
-} /* namespace rtabmap */

corelib/src/CameraOpenni.cpp

@@ -1,152 +0,0 @@
-/*
- * CameraOpenni.cpp
- *
- *  Created on: 2013-08-22
- *      Author: Mathieu
- */
-
-#include "rtabmap/core/CameraOpenni.h"
-#include "rtabmap/core/CameraEvent.h"
-#include "rtabmap/utilite/ULogger.h"
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UEventsManager.h>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <pcl/io/openni_grabber.h>
-
-namespace rtabmap {
-
-CameraOpenni::CameraOpenni(const std::string & deviceId, float inputRate, const Transform & localTransform) :
-		interface_(0),
-		deviceId_(deviceId),
-		rate_(inputRate),
-		frameRateTimer_(new UTimer()),
-		localTransform_(localTransform),
-		seq_(0)
-{
-}
-
-CameraOpenni::~CameraOpenni()
-{
-	UDEBUG("");
-	kill();
-	delete frameRateTimer_;
-	if(interface_)
-	{
-		uSleep(100); // make sure it is stopped
-		delete interface_;
-		interface_ = 0;
-	}
-}
-
-void CameraOpenni::image_cb (
-		const boost::shared_ptr<openni_wrapper::Image>& rgb,
-		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
-		float constant)
-{
-	if(rate_>0.0f)
-	{
-		if(frameRateTimer_->getElapsedTime() < 1.0f/rate_)
-		{
-			return;
-		}
-	}
-	frameRateTimer_->start();
-
-	UTimer t;
-
-	cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
-	rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
-	cv::Mat bgrFrame;
-	cv::cvtColor(rgbFrame, bgrFrame, CV_RGB2BGR);
-
-	cv::Mat depthFrame(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
-	depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depthFrame.data);
-
-	UINFO("constant=%f focal=%f", constant, 1.0f/constant);
-
-	this->post(new CameraEvent(bgrFrame, depthFrame, constant, localTransform_, ++seq_));
-}
-
-bool CameraOpenni::init()
-{
-	if(interface_ && interface_->isRunning())
-	{
-		UERROR("Already started!!!\n");
-		return false;
-	}
-	else if(interface_)
-	{
-		delete interface_;
-		interface_ = 0;
-	}
-
-	seq_ = 0;
-	try
-	{
-		interface_ = new pcl::OpenNIGrabber(deviceId_);
-	}
-	catch(const pcl::IOException& ex)
-	{
-		UERROR("OpenNI exception: %s", ex.what());
-		if(interface_)
-		{
-			delete interface_;
-			interface_ = 0;
-		}
-		return false;
-	}
-
-	frameRateTimer_->start();
-	return true;
-}
-
-void CameraOpenni::start()
-{
-	if(interface_)
-	{
-		if(!connection_.connected())
-		{
-			boost::function<void (
-					const boost::shared_ptr<openni_wrapper::Image>&,
-					const boost::shared_ptr<openni_wrapper::DepthImage>&,
-					float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
-			connection_ = interface_->registerCallback (f);
-		}
-
-		if(!interface_->isRunning())
-		{
-			interface_->start ();
-		}
-	}
-}
-
-void CameraOpenni::pause()
-{
-	if(connection_.connected())
-	{
-		connection_.disconnect();
-	}
-}
-
-void CameraOpenni::kill()
-{
-	UDEBUG("");
-	if(interface_)
-	{
-		interface_->stop();
-	}
-}
-
-bool CameraOpenni::isRunning()
-{
-	return (interface_ && interface_->isRunning());
-}
-
-void CameraOpenni::setFrameRate(float rate)
-{
-	rate_ = rate;
-}
-
-} /* namespace rtabmap */

corelib/src/CameraRGBD.cpp

@@ -0,0 +1,791 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/CameraRGBD.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 <pcl/io/openni_grabber.h>
+
+#include <cmath>
+
+#ifdef WITH_FREENECT
+#include <libfreenect.h>
+#include <libfreenect-registration.h>
+#endif
+
+#ifdef WITH_OPENNI2
+#include <OpenNI.h>
+#endif
+
+namespace rtabmap
+{
+
+CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) :
+	_imageRate(imageRate),
+	_localTransform(localTransform),
+	_frameRateTimer(new UTimer())
+{
+}
+
+CameraRGBD::~CameraRGBD()
+{
+	if(_frameRateTimer)
+	{
+		delete _frameRateTimer;
+	}
+}
+
+void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+{
+	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);
+		}
+
+		// Add precision at the cost of a small overhead
+		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));
+	}
+
+	UTimer timer;
+	this->captureImage(rgb, depth, depthConstant);
+	UDEBUG("Time capturing image = %fs", timer.ticks());
+}
+
+/////////////////////////
+// CameraOpenNIPCL
+/////////////////////////
+CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
+		CameraRGBD(imageRate, localTransform),
+		interface_(0),
+		deviceId_(deviceId)
+{
+}
+
+CameraOpenni::~CameraOpenni()
+{
+	UDEBUG("");
+	if(connection_.connected())
+	{
+		connection_.disconnect();
+	}
+
+	if(interface_)
+	{
+		interface_->stop();
+		uSleep(1000); // make sure it is stopped
+		delete interface_;
+		interface_ = 0;
+	}
+}
+
+void CameraOpenni::image_cb (
+		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+		float constant)
+{
+	UScopeMutex s(dataMutex_);
+
+	cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
+	rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
+	cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
+
+	depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
+	depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
+
+	depthConstant_ = constant;
+
+	if(dataReady_.value() <= 0)
+	{
+		dataReady_.release();
+	}
+}
+
+bool CameraOpenni::init()
+{
+	if(interface_)
+	{
+		interface_->stop();
+		uSleep(100); // make sure it is stopped
+		delete interface_;
+		interface_ = 0;
+	}
+
+	try
+	{
+		interface_ = new pcl::OpenNIGrabber(deviceId_);
+
+		boost::function<void (
+				const boost::shared_ptr<openni_wrapper::Image>&,
+				const boost::shared_ptr<openni_wrapper::DepthImage>&,
+				float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
+		connection_ = interface_->registerCallback (f);
+
+		interface_->start ();
+	}
+	catch(const pcl::IOException& ex)
+	{
+		UERROR("OpenNI exception: %s", ex.what());
+		if(interface_)
+		{
+			delete interface_;
+			interface_ = 0;
+		}
+		return false;
+	}
+	return true;
+}
+
+void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+{
+	if(interface_ && interface_->isRunning())
+	{
+		dataReady_.acquire();
+		UScopeMutex s(dataMutex_);
+		depth = depth_;
+		rgb = rgb_;
+		depthConstant = depthConstant_;
+
+		depth_ = cv::Mat();
+		rgb_ = cv::Mat();
+		depthConstant_ = 0;
+	}
+}
+
+
+
+/////////////////////////
+// CameraOpenNICV
+/////////////////////////
+bool CameraOpenNICV::available()
+{
+	return cv::getBuildInformation().find("OpenNI:                      YES") != std::string::npos;
+}
+
+CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
+	CameraRGBD(imageRate, localTransform),
+	_asus(asus),
+	_depthFocal(0.0f)
+{
+
+}
+
+CameraOpenNICV::~CameraOpenNICV()
+{
+	_capture.release();
+}
+
+bool CameraOpenNICV::init()
+{
+	if(_capture.isOpened())
+	{
+		_capture.release();
+	}
+
+	ULOGGER_DEBUG("CameraRGBD::init()");
+	_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
+	if(_capture.isOpened())
+	{
+		_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
+		_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
+		// Print some avalible device settings.
+		UINFO("Depth generator output mode:");
+		UINFO("FRAME_WIDTH        %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
+		UINFO("FRAME_HEIGHT       %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
+		UINFO("FRAME_MAX_DEPTH    %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
+		UINFO("BASELINE           %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
+		UINFO("FPS                %f", _capture.get( CV_CAP_PROP_FPS ));
+		UINFO("Focal              %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
+		UINFO("REGISTRATION       %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
+		if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
+		{
+			UERROR("Depth registration is not activated on this device!");
+		}
+		if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
+		{
+			UINFO("Image generator output mode:");
+			UINFO("FRAME_WIDTH    %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
+			UINFO("FRAME_HEIGHT   %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
+			UINFO("FPS            %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
+		}
+		else
+		{
+			UERROR("CameraRGBD: Device doesn't contain image generator.");
+			_capture.release();
+			return false;
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!");
+		_capture.release();
+		return false;
+	}
+	return true;
+}
+
+void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+{
+	if(_capture.isOpened())
+	{
+		_capture.grab();
+		_capture.retrieve( depth, CV_CAP_OPENNI_DEPTH_MAP );
+		_capture.retrieve( rgb, CV_CAP_OPENNI_BGR_IMAGE );
+
+		depth = depth.clone();
+		rgb = rgb.clone();
+		UASSERT(_depthFocal > 0.0f);
+		depthConstant = 1.0f/_depthFocal;
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+}
+
+
+/////////////////////////
+// CameraOpenNI2
+/////////////////////////
+bool CameraOpenNI2::available()
+{
+#ifdef WITH_OPENNI2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform) :
+	CameraRGBD(imageRate, localTransform),
+#ifdef WITH_OPENNI2
+	_device(new openni::Device()),
+	_color(new openni::VideoStream()),
+	_depth(new openni::VideoStream()),
+#else
+	_device(0),
+	_color(0),
+	_depth(0),
+#endif
+	_depthFocal(0.0f)
+{
+}
+
+CameraOpenNI2::~CameraOpenNI2()
+{
+#ifdef WITH_OPENNI2
+	_color->stop();
+	_color->destroy();
+	_depth->stop();
+	_depth->destroy();
+	_device->close();
+	openni::OpenNI::shutdown();
+
+	delete _device;
+	delete _color;
+	delete _depth;
+#endif
+}
+
+bool CameraOpenNI2::init()
+{
+#ifdef WITH_OPENNI2
+	openni::OpenNI::initialize();
+
+	if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot open device.");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
+	{
+		UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
+	  _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
+	{
+		UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot create depth stream.");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot create color stream.");
+		_depth->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Failed to set depth/color registration.");
+	}
+
+	_depth->setMirroringEnabled(false);
+	_color->setMirroringEnabled(false);
+
+	const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
+	for(int i=0; i<depthVideoModes.getSize(); ++i)
+	{
+		UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+				i,
+				depthVideoModes[i].getFps(),
+				depthVideoModes[i].getPixelFormat(),
+				depthVideoModes[i].getResolutionX(),
+				depthVideoModes[i].getResolutionY());
+	}
+
+	const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
+	for(int i=0; i<colorVideoModes.getSize(); ++i)
+	{
+		UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+				i,
+				colorVideoModes[i].getFps(),
+				colorVideoModes[i].getPixelFormat(),
+				colorVideoModes[i].getResolutionX(),
+				colorVideoModes[i].getResolutionY());
+	}
+
+	openni::VideoMode mMode;
+	mMode.setFps(30);
+	mMode.setResolution(640,480);
+	mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
+	_depth->setVideoMode(mMode);
+
+	openni::VideoMode mModeColor;
+	mModeColor.setFps(30);
+	mModeColor.setResolution(640,480);
+	mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
+	_color->setVideoMode(mModeColor);
+
+	UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+			_depth->getVideoMode().getFps(),
+			_depth->getVideoMode().getPixelFormat(),
+			_depth->getVideoMode().getResolutionX(),
+			_depth->getVideoMode().getResolutionY(),
+			_depth->getHorizontalFieldOfView(),
+			_depth->getVerticalFieldOfView());
+
+	bool registered = true;
+	if(registered)
+	{
+		_depthFocal = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
+	}
+	else
+	{
+		_depthFocal = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+	}
+	UINFO("depth focal = %f", _depthFocal);
+
+	UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+			_color->getVideoMode().getFps(),
+			_color->getVideoMode().getPixelFormat(),
+			_color->getVideoMode().getResolutionX(),
+			_color->getVideoMode().getResolutionY(),
+			_color->getHorizontalFieldOfView(),
+			_color->getVerticalFieldOfView());
+
+	if(_depth->start() != openni::STATUS_OK ||
+	   _color->start() != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
+		_depth->stop();
+		_color->stop();
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	uSleep(1000); // just to make sure the sensor is correctly initialized
+
+	return true;
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+	return false;
+#endif
+}
+
+void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+{
+#ifdef WITH_OPENNI2
+	if(_device->isValid() &&
+		_depth->isValid() &&
+		_color->isValid() &&
+		_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
+		_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
+	{
+		openni::VideoFrameRef depthFrame, colorFrame;
+
+		_depth->readFrame(&depthFrame);
+		_color->readFrame(&colorFrame);
+
+		if(depthFrame.isValid() && colorFrame.isValid())
+		{
+			int h=depthFrame.getHeight();
+			int w=depthFrame.getWidth();
+			depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
+
+			h=colorFrame.getHeight();
+			w=colorFrame.getWidth();
+			cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
+			cv::cvtColor(tmp, rgb, CV_RGB2BGR);
+		}
+		UASSERT(_depthFocal != 0.0f);
+		depthConstant = 1.0f/_depthFocal;
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+}
+
+#ifdef WITH_FREENECT
+//
+// FreenectDevice
+//
+class FreenectDevice : public UThread {
+  public:
+	FreenectDevice(freenect_context * ctx, int index) :
+		index_(index),
+		ctx_(ctx),
+		device_(0),
+		depthFocal_(0.0f)
+	{
+		UASSERT(ctx_ != 0);
+	}
+
+	~FreenectDevice()
+	{
+		this->join(true);
+		if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
+	}
+
+	bool init()
+	{
+		if(freenect_open_device(ctx_, &device_, index_) < 0)
+		{
+			UERROR("FreenectDevice: Cannot open Kinect");
+			return false;
+		}
+		freenect_set_user(device_, this);
+		freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
+		freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
+		depthBuffer_ = cv::Mat(cv::Size(640,480),CV_16UC1);
+		rgbBuffer_ = cv::Mat(cv::Size(640,480), CV_8UC3);
+		freenect_set_depth_buffer(device_, depthBuffer_.data);
+		freenect_set_video_buffer(device_, rgbBuffer_.data);
+		freenect_set_depth_callback(device_, freenect_depth_callback);
+		freenect_set_video_callback(device_, freenect_video_callback);
+
+		bool registered = true;
+		float rgb_focal_length_sxga = 1050.0f;
+		float width_sxga = 1280.0f;
+		float width = freenect_get_current_depth_mode(device_).width;
+		float scale = width / width_sxga;
+		if(registered)
+		{
+			depthFocal_ =  rgb_focal_length_sxga * scale;
+		}
+		else
+		{
+			freenect_registration reg = freenect_copy_registration(device_);
+			float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
+			freenect_destroy_registration(&reg);
+
+			depthFocal_ =  depth_focal_length_sxga * scale;
+		}
+
+		UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
+		return true;
+	}
+
+	float getDepthFocal() const {return depthFocal_;}
+
+	void getData(cv::Mat & rgb, cv::Mat & depth)
+	{
+		if(this->isRunning())
+		{
+			dataReady_.acquire();
+			{
+				UScopeMutex s1(rgbMutex_);
+				if(!rgbLastFrame_.empty())
+				{
+					rgb = rgbLastFrame_;
+					rgbLastFrame_ = cv::Mat();
+				}
+			}
+			{
+				UScopeMutex s2(depthMutex_);
+				if(!depthLastFrame_.empty())
+				{
+					depth = depthLastFrame_;
+					depthLastFrame_= cv::Mat();
+				}
+			}
+		}
+	}
+
+private:
+	// Do not call directly even in child
+	void VideoCallback(void* rgb)
+	{
+		UASSERT(rgbBuffer_.data == rgb);
+		UScopeMutex s1(rgbMutex_);
+		cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
+		if(!depthLastFrame_.empty() && dataReady_.value() <= 0)
+		{
+			dataReady_.release();
+		}
+	}
+
+	// Do not call directly even in child
+	void DepthCallback(void* depth)
+	{
+		UASSERT(depthBuffer_.data == depth);
+		UScopeMutex s2(depthMutex_);
+		depthLastFrame_ = depthBuffer_.clone();
+		if(!rgbLastFrame_.empty() && dataReady_.value() <= 0)
+		{
+			dataReady_.release();
+		}
+	}
+
+	void startVideo() {
+		if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
+	}
+	void stopVideo() {
+		if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
+	}
+	void startDepth() {
+		if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
+	}
+	void stopDepth() {
+		if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
+	}
+
+	virtual void mainLoopBegin()
+	{
+		this->startDepth();
+		this->startVideo();
+	}
+
+	virtual void mainLoop()
+	{
+		timeval t;
+		t.tv_sec = 0;
+		t.tv_usec = 10000;
+		if(freenect_process_events_timeout(ctx_, &t) < 0)
+		{
+			UERROR("FreenectDevice: Cannot process freenect events");
+			this->kill();
+		}
+	}
+
+	virtual void mainLoopEnd()
+	{
+		this->stopDepth();
+		this->stopVideo();
+		dataReady_.release();
+	}
+
+	static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
+		FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
+		device->DepthCallback(depth);
+	}
+	static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
+		FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
+		device->VideoCallback(video);
+	}
+
+	//noncopyable
+	FreenectDevice( const FreenectDevice& );
+	const FreenectDevice& operator=( const FreenectDevice& );
+
+  private:
+	int index_;
+	freenect_context * ctx_;
+	freenect_device * device_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbBuffer_;
+	UMutex depthMutex_;
+	UMutex rgbMutex_;
+	cv::Mat depthLastFrame_;
+	cv::Mat rgbLastFrame_;
+	float depthFocal_;
+	USemaphore dataReady_;
+};
+#endif
+
+//
+// CameraFreenect
+//
+bool CameraFreenect::available()
+{
+#ifdef WITH_FREENECT
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
+		CameraRGBD(imageRate, localTransform),
+		deviceId_(deviceId),
+		ctx_(0),
+		freenectDevice_(0)
+{
+#ifdef WITH_FREENECT
+	if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
+	// We claim both the motor and camera devices, since this class exposes both.
+	// It does not support audio, so we do not claim it.
+	freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
+#endif
+}
+
+CameraFreenect::~CameraFreenect()
+{
+#ifdef WITH_FREENECT
+	if(freenectDevice_)
+	{
+		freenectDevice_->join(true);
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+	if(ctx_)
+	{
+		if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
+	}
+#endif
+}
+
+bool CameraFreenect::init()
+{
+#ifdef WITH_FREENECT
+	if(freenectDevice_)
+	{
+		freenectDevice_->join(true);
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+
+	if(ctx_ && freenect_num_devices(ctx_) > 0)
+	{
+		freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
+		if(freenectDevice_->init())
+		{
+			freenectDevice_->start();
+			uSleep(3000);
+			return true;
+		}
+		else
+		{
+			UERROR("CameraFreenect: Init failed!");
+		}
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+	else
+	{
+		UERROR("CameraFreenect: No devices connected!");
+	}
+#else
+	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
+#endif
+	return false;
+}
+
+void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
+{
+#ifdef WITH_FREENECT
+	if(ctx_ && freenectDevice_)
+	{
+		if(freenectDevice_->isRunning())
+		{
+			freenectDevice_->getData(rgb, depth);
+			UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
+			depthConstant = 1.0f/freenectDevice_->getDepthFocal();
+
+			if(depth.empty())
+			{
+				UWARN("CameraFreenect: Data not ready! Try to reduce the image rate to avoid this warning...");
+			}
+		}
+		else
+		{
+			UERROR("CameraFreenect: Re-initialization needed!");
+			delete freenectDevice_;
+			freenectDevice_ = 0;
+		}
+
+		if(depth.empty() || rgb.empty())
+		{
+			rgb = cv::Mat();
+			depth = cv::Mat();
+			depthConstant = 0.0f;
+		}
+	}
+#else
+	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
+#endif
+}
+
+} // namespace rtabmap

corelib/src/CameraThread.cpp

@@ -19,42 +19,70 @@
 
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraEvent.h"
-#include "rtabmap/core/ParamEvent.h"
 
-#include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap
 {
 
 // ownership transferred
-CameraThread::CameraThread(Camera * camera, bool autoRestart) :
+CameraThread::CameraThread(Camera * camera) :
 		_camera(camera),
-		_autoRestart(autoRestart),
+		_cameraRGBD(0),
 		_seq(0)
 {
 	UASSERT(_camera != 0);
 }
 
+// ownership transferred
+CameraThread::CameraThread(CameraRGBD * camera) :
+		_camera(0),
+		_cameraRGBD(camera),
+		_seq(0)
+{
+	UASSERT(_cameraRGBD != 0);
+}
+
 CameraThread::~CameraThread()
 {
 	join(true);
-	delete _camera;
+	if(_camera)
+	{
+		delete _camera;
+	}
+	if(_cameraRGBD)
+	{
+		delete _cameraRGBD;
+	}
+}
+
+void CameraThread::setImageRate(float imageRate)
+{
+	if(_camera)
+	{
+		_camera->setImageRate(imageRate);
+	}
+	if(_cameraRGBD)
+	{
+		_cameraRGBD->setImageRate(imageRate);
+	}
 }
 
 bool CameraThread::init()
 {
 	if(!this->isRunning())
 	{
-		if(_camera)
+		_seq = 0;
+		if(_cameraRGBD)
 		{
-			_seq = 0;
-			return _camera->init();
+			return _cameraRGBD->init();
 		}
 		else
 		{
-			UERROR("Cannot initialize the camera because the camera object is null...");
+			return _camera->init();
 		}
 	}
 	else
@@ -67,29 +95,37 @@ bool CameraThread::init()
 void CameraThread::mainLoop()
 {
 	UTimer timer;
-	ULOGGER_DEBUG("Camera::process()");
+	UDEBUG("");
 	cv::Mat descriptors;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat rgb, depth;
 	float depthConstant = 0.0f;
-	 _camera->takeImage(rgb, depth, depthConstant);
+	if(_cameraRGBD)
+	{
+		_cameraRGBD->takeImage(rgb, depth, depthConstant);
+	}
+	else
+	{
+		rgb = _camera->takeImage();
+	}
+
 	if(!rgb.empty() && !this->isKilled())
 	{
-		this->post(new CameraEvent(rgb, depth, depthConstant, _camera->getLocalTransform(), ++_seq));
-	}
-	else if(!this->isKilled())
-	{
-		if(_autoRestart)
+		if(_cameraRGBD)
 		{
-			_camera->init();
+			this->post(new CameraEvent(rgb, depth, depthConstant, _cameraRGBD->getLocalTransform(), ++_seq));
 		}
 		else
 		{
-			ULOGGER_DEBUG("Camera::process() : no more images...");
-			this->kill();
-			this->post(new CameraEvent());
+			this->post(new CameraEvent(rgb, ++_seq));
 		}
 	}
+	else if(!this->isKilled())
+	{
+		UDEBUG("no more images...");
+		this->kill();
+		this->post(new CameraEvent());
+	}
 }
 
 } // namespace rtabmap

corelib/src/Features2d.cpp

@@ -54,7 +54,7 @@ void filterKeypointsByDepth(
 		float depthConstant,
 		float maxDepth)
 {
-	if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == keypoints.size()))
+	if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
 	{
 		std::vector<cv::KeyPoint> output(keypoints.size());
 		std::vector<int> indexes(keypoints.size(), 0);
@@ -71,7 +71,7 @@ void filterKeypointsByDepth(
 		output.resize(oi);
 		keypoints = output;
 
-		if(!descriptors.empty() && keypoints.size() != descriptors.rows)
+		if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
 		{
 			if(keypoints.size() == 0)
 			{

examples/BOWMapping/CMakeLists.txt

@@ -7,8 +7,6 @@ SET(LIBRARIES
 	${OpenCV_LIBRARIES} 
 )
 
-add_definitions(${PCL_DEFINITIONS})
-
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(bow_mapping main.cpp)

examples/CMakeLists.txt

@@ -7,5 +7,6 @@ ELSE()
    MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping example will not be built...")
 ENDIF()
 
+ADD_SUBDIRECTORY( CameraRGBD )
 
 

examples/RGBDMapping/main.cpp

@@ -19,8 +19,8 @@
 
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/RtabmapThread.h"
-#include "rtabmap/core/CameraOpenni.h"
-#include "rtabmap/core/CameraFreenect.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/utilite/UEventsManager.h"
 #include <QtGui/QApplication>
@@ -43,10 +43,10 @@ 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
-	CameraOpenni camera("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
-	//CameraOpenKinect camera(0, 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
-
-	if(!camera.init())
+	CameraRGBD * camera = new CameraOpenni("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+	//CameraRGBD * camera = new CameraFreenect(0, 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+	CameraThread cameraThread(camera);
+	if(!cameraThread.init())
 	{
 		UERROR("Camera init failed!");
 		exit(1);
@@ -72,12 +72,12 @@ int main(int argc, char * argv[])
 	// only the odometry will receive CameraEvent from that camera. RTAB-Map is
 	// also subscribed to OdometryEvent by default, so no need to create a pipe between
 	// odometry and RTAB-Map.
-	UEventsManager::createPipe(&camera, &odomThread, "CameraEvent");
+	UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
 
 	// Let's start the threads
 	rtabmapThread.start();
 	odomThread.start();
-	camera.start();
+	cameraThread.start();
 
 	mapBuilder.show();
 	app.exec(); // main loop
@@ -88,7 +88,7 @@ int main(int argc, char * argv[])
 	odomThread.unregisterFromEventsManager();
 
 	// Kill all threads
-	camera.kill();
+	cameraThread.kill();
 	odomThread.join(true);
 	rtabmapThread.join(true);
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -205,8 +205,6 @@ private:
 	State _state;
 	rtabmap::CameraThread * _camera;
 	rtabmap::DBReader * _dbReader;
-	rtabmap::CameraOpenni * _cameraOpenni;
-	rtabmap::CameraFreenect * _cameraOpenKinect;
 	rtabmap::OdometryThread * _odomThread;
 
 	SrcType _srcType;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -49,8 +49,6 @@ class QDoubleSpinBox;
 
 namespace rtabmap {
 
-class CameraOpenni;
-class CameraFreenect;
 class OdometryThread;
 class CameraThread;
 class Signature;
@@ -151,12 +149,10 @@ public:
 	bool isSourceDatabaseUsed() const;
 	bool isSourceOpenniUsed() const;
 	OdomType getOdometryType() const;
-	bool getGeneralAutoRestart() const;
 	int getSourceImageType() const;
 	QString getSourceImageTypeStr() const;
 	int getSourceWidth() const;
 	int getSourceHeight() const;
-	int getFramesDropped() const;
 	QString getSourceImagesPath() const;	//Images group
 	QString getSourceImagesSuffix() const;	//Images group
 	int getSourceImagesSuffixIndex() const;	//Images group
@@ -196,7 +192,6 @@ public slots:
 	void setInputRate(double value);
 	void setDetectionRate(double value);
 	void setHardThr(int value);
-	void setAutoRestart(bool value);
 	void setTimeLimit(float value);
 	void setSLAMMode(bool enabled);
 	void selectSourceImage(Src src = kSrcUndef);
@@ -279,9 +274,7 @@ private:
 	QProgressDialog * _progressDialog;
 
 	//Odometry test
-	CameraOpenni * _odomCameraOpenNI;
-	CameraFreenect * _odomCameraFreenect;
-	CameraThread * _odomCameraOpenNICv;
+	CameraThread * _cameraThread;
 	OdometryThread * _odomThread;
 
 	QVector<QCheckBox*> _3dRenderingShowClouds;

guilib/src/MainWindow.cpp

@@ -65,8 +65,7 @@
 #include <QtGui/QInputDialog>
 
 //RGB-D stuff
-#include "rtabmap/core/CameraOpenni.h"
-#include "rtabmap/core/CameraFreenect.h"
+#include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
@@ -97,8 +96,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_state(kIdle),
 	_camera(0),
 	_dbReader(0),
-	_cameraOpenni(0),
-	_cameraOpenKinect(0),
 	_odomThread(0),
 	_srcType(kSrcUndefined),
 	_preferencesDialog(0),
@@ -291,8 +288,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
 	connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
-	_ui->actionOpenNI_CV->setEnabled(CameraRGBD::available());
-	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraRGBD::available());
+	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
+	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
 	connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	_ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
 
@@ -420,18 +417,6 @@ void MainWindow::closeEvent(QCloseEvent* event)
 			delete _dbReader;
 			_dbReader = 0;
 		}
-		if(_cameraOpenni)
-		{
-			UERROR("CameraOpenni must be already deleted here!");
-			delete _cameraOpenni;
-			_cameraOpenni = 0;
-		}
-		if(_cameraOpenKinect)
-		{
-			UERROR("CameraOpenKinect must be already deleted here!");
-			delete _cameraOpenKinect;
-			_cameraOpenKinect = 0;
-		}
 		if(_odomThread)
 		{
 			UERROR("OdomThread must be already deleted here!");
@@ -1530,23 +1515,12 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 
 		if(_camera)
 		{
-			_camera->getCamera()->setImageRate(_preferencesDialog->getGeneralInputRate());
-			_camera->setAutoRestart(_preferencesDialog->getGeneralAutoRestart());
+			_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
 		}
 		if(_dbReader)
 		{
 			_dbReader->setFrameRate(_preferencesDialog->getGeneralInputRate());
 		}
-
-		if(_cameraOpenni)
-		{
-			_cameraOpenni->setFrameRate(_preferencesDialog->getGeneralInputRate());
-		}
-
-		if(_cameraOpenKinect)
-		{
-			_cameraOpenKinect->setFrameRate(_preferencesDialog->getGeneralInputRate());
-		}
 	}
 
 	if(flags & PreferencesDialog::kPanelGeneral)
@@ -1965,24 +1939,6 @@ void MainWindow::startDetection()
 		emit stateChanged(kIdle);
 		return;
 	}
-	if(_cameraOpenni != 0)
-	{
-		QMessageBox::warning(this,
-							 tr("RTAB-Map"),
-							 tr("An Openni camera is running, stop it first."));
-		UWARN("_cameraOpenni is not null... it must be stopped first");
-		emit stateChanged(kIdle);
-		return;
-	}
-	if(_cameraOpenKinect != 0)
-	{
-		QMessageBox::warning(this,
-							 tr("RTAB-Map"),
-							 tr("A Freenect camera is running, stop it first."));
-		UWARN("_cameraOpenKinect is not null... it must be stopped first");
-		emit stateChanged(kIdle);
-		return;
-	}
 
 	// Adjust pre-requirements
 	if( !_preferencesDialog->isSourceImageUsed() &&
@@ -2029,112 +1985,61 @@ void MainWindow::startDetection()
 			_odomThread->start();
 		}
 
-		if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
+		CameraRGBD * camera = 0;
+		if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL)
 		{
-			_cameraOpenKinect = new CameraFreenect(
-				_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
-				_preferencesDialog->getGeneralInputRate(),
-				_preferencesDialog->getSourceOpenniLocalTransform());
-
-			if(!_cameraOpenKinect->init())
-			{
-				ULOGGER_WARN("init CameraFreenect failed... ");
-				if(!_cameraOpenKinect->available())
-				{
-					QMessageBox::warning(this,
-					   tr("RTAB-Map"),
-					   tr("Freenect camera unavailable! RTAB-Map is not built with Freenect support (libfreenect)."));
-				}
-				else
-				{
-					QMessageBox::warning(this,
-					   tr("RTAB-Map"),
-					   tr("Freenect camera initialization failed!"));
-				}
-				emit stateChanged(kIdle);
-				delete _cameraOpenKinect;
-				_cameraOpenKinect = 0;
-				if(_odomThread)
-				{
-					delete _odomThread;
-					_odomThread = 0;
-				}
-				return;
-			}
-			if(_odomThread)
-			{
-				UEventsManager::createPipe(_cameraOpenKinect, _odomThread, "CameraEvent");
-			}
+			camera = new CameraOpenni(
+					_preferencesDialog->getSourceOpenniDevice().toStdString(),
+					_preferencesDialog->getGeneralInputRate(),
+					_preferencesDialog->getSourceOpenniLocalTransform());
+		}
+		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
+		{
+			camera = new CameraOpenNI2(
+					_preferencesDialog->getGeneralInputRate(),
+					_preferencesDialog->getSourceOpenniLocalTransform());
+		}
+		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
+		{
+			camera = new CameraFreenect(
+					_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
+					_preferencesDialog->getGeneralInputRate(),
+					_preferencesDialog->getSourceOpenniLocalTransform());
 		}
 		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
-				_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS ||
-				_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
+				_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
 		{
-			Camera * camera = 0;
-			if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
-			{
-				camera = new CameraOpenNI2(
-						_preferencesDialog->getGeneralInputRate());
-			}
-			else
-			{
-				camera = new CameraRGBD(
-						_preferencesDialog->getGeneralInputRate(),
-						_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
-			}
-
-			camera->setLocalTransform(_preferencesDialog->getSourceOpenniLocalTransform());
-
-			if(!camera->init())
-			{
-				ULOGGER_WARN("init camera failed... ");
-				QMessageBox::warning(this,
-									   tr("RTAB-Map"),
-									   tr("Camera initialization failed..."));
-				emit stateChanged(kIdle);
-				delete camera;
-				camera = 0;
-				if(_odomThread)
-				{
-					delete _odomThread;
-					_odomThread = 0;
-				}
-				return;
-			}
-
-			_camera = new CameraThread(camera);
-			if(_odomThread)
-			{
-				UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
-			}
+			camera = new CameraOpenNICV(
+					_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
+					_preferencesDialog->getGeneralInputRate(),
+					_preferencesDialog->getSourceOpenniLocalTransform());
 		}
 		else
 		{
-			_cameraOpenni = new CameraOpenni(
-				_preferencesDialog->getSourceOpenniDevice().toStdString(),
-				_preferencesDialog->getGeneralInputRate(),
-				_preferencesDialog->getSourceOpenniLocalTransform());
+			UFATAL("RGBD Source type undefined!");
+		}
 
-			if(!_cameraOpenni->init())
-			{
-				ULOGGER_WARN("init CameraOpenni failed... ");
-				QMessageBox::warning(this,
-									   tr("RTAB-Map"),
-									   tr("Openni camera initialization failed!"));
-				emit stateChanged(kIdle);
-				delete _cameraOpenni;
-				_cameraOpenni = 0;
-				if(_odomThread)
-				{
-					delete _odomThread;
-					_odomThread = 0;
-				}
-				return;
-			}
+		if(!camera->init())
+		{
+			ULOGGER_WARN("init camera failed... ");
+			QMessageBox::warning(this,
+								   tr("RTAB-Map"),
+								   tr("Camera initialization failed..."));
+			emit stateChanged(kIdle);
+			delete camera;
+			camera = 0;
 			if(_odomThread)
 			{
-				UEventsManager::createPipe(_cameraOpenni, _odomThread, "CameraEvent");
+				delete _odomThread;
+				_odomThread = 0;
 			}
+			return;
+		}
+
+		_camera = new CameraThread(camera);
+		if(_odomThread)
+		{
+			UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
 		}
 	}
 	else if(_preferencesDialog->isSourceDatabaseUsed())
@@ -2211,9 +2116,7 @@ void MainWindow::startDetection()
 						_preferencesDialog->getSourceImagesRefreshDir(),
 						_preferencesDialog->getGeneralInputRate(),
 						_preferencesDialog->getSourceWidth(),
-						_preferencesDialog->getSourceHeight(),
-						_preferencesDialog->getFramesDropped()
-						);
+						_preferencesDialog->getSourceHeight());
 			}
 			else if(sourceType == 2)
 			{
@@ -2221,8 +2124,7 @@ void MainWindow::startDetection()
 						_preferencesDialog->getSourceVideoPath().toStdString(),
 						_preferencesDialog->getGeneralInputRate(),
 						_preferencesDialog->getSourceWidth(),
-						_preferencesDialog->getSourceHeight(),
-						_preferencesDialog->getFramesDropped());
+						_preferencesDialog->getSourceHeight());
 			}
 			else //if(sourceType == 0)
 			{
@@ -2230,8 +2132,7 @@ void MainWindow::startDetection()
 						_preferencesDialog->getSourceUsbDeviceId(),
 						_preferencesDialog->getGeneralInputRate(),
 						_preferencesDialog->getSourceWidth(),
-						_preferencesDialog->getSourceHeight(),
-						_preferencesDialog->getFramesDropped());
+						_preferencesDialog->getSourceHeight());
 			}
 
 			if(!camera->init())
@@ -2274,7 +2175,7 @@ void MainWindow::startDetection()
 // Could not be in the main thread here! (see handleEvents())
 void MainWindow::pauseDetection()
 {
-	if(_camera || _dbReader || _cameraOpenni || _cameraOpenKinect)
+	if(_camera || _dbReader)
 	{
 		if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
 		{
@@ -2308,16 +2209,14 @@ void MainWindow::pauseDetection()
 
 void MainWindow::stopDetection()
 {
-	if(_state == kIdle || (!_camera && !_dbReader && !_cameraOpenni && !_cameraOpenKinect))
+	if(_state == kIdle || (!_camera && !_dbReader))
 	{
 		return;
 	}
 
 	if(_state == kDetecting &&
 			( (_camera && _camera->isRunning()) ||
-			  (_dbReader && _dbReader->isRunning()) ||
-			  (_cameraOpenni && _cameraOpenni->isRunning()) ||
-			  (_cameraOpenKinect && _cameraOpenKinect->isRunning()) ) )
+			  (_dbReader && _dbReader->isRunning()) ) )
 	{
 		QMessageBox::StandardButton button = QMessageBox::question(this, tr("Stopping process..."), tr("Are you sure you want to stop the process?"), QMessageBox::Yes|QMessageBox::No, QMessageBox::No);
 
@@ -2339,16 +2238,6 @@ void MainWindow::stopDetection()
 		_dbReader->join(true);
 	}
 
-	if(_cameraOpenni)
-	{
-		_cameraOpenni->kill();
-	}
-
-	if(_cameraOpenKinect)
-	{
-		_cameraOpenKinect->join(true);
-	}
-
 	if(_odomThread)
 	{
 		_ui->actionReset_Odometry->setEnabled(false);
@@ -2366,16 +2255,6 @@ void MainWindow::stopDetection()
 		delete _dbReader;
 		_dbReader = 0;
 	}
-	if(_cameraOpenni)
-	{
-		delete _cameraOpenni;
-		_cameraOpenni = 0;
-	}
-	if(_cameraOpenKinect)
-	{
-		delete _cameraOpenKinect;
-		_cameraOpenKinect = 0;
-	}
 	if(_odomThread)
 	{
 		delete _odomThread;
@@ -3815,16 +3694,6 @@ void MainWindow::changeState(MainWindow::State newState)
 		{
 			_dbReader->start();
 		}
-
-		if(_cameraOpenni)
-		{
-			_cameraOpenni->start();
-		}
-
-		if(_cameraOpenKinect)
-		{
-			_cameraOpenKinect->start();
-		}
 		break;
 
 	case kPaused:
@@ -3853,16 +3722,6 @@ void MainWindow::changeState(MainWindow::State newState)
 			{
 				_dbReader->start();
 			}
-
-			if(_cameraOpenni)
-			{
-				_cameraOpenni->start();
-			}
-
-			if(_cameraOpenKinect)
-			{
-				_cameraOpenKinect->start();
-			}
 		}
 		else if(_state == kDetecting)
 		{
@@ -3889,16 +3748,6 @@ void MainWindow::changeState(MainWindow::State newState)
 			{
 				_dbReader->join(true);
 			}
-
-			if(_cameraOpenni)
-			{
-				_cameraOpenni->pause();
-			}
-
-			if(_cameraOpenKinect)
-			{
-				_cameraOpenKinect->join(true);
-			}
 		}
 		break;
 	case kMonitoring:

guilib/src/OdometryViewer.cpp

@@ -49,7 +49,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, i
 void OdometryViewer::processData()
 {
 	rtabmap::Image data;
-	int quality;
+	int quality = 0;
 	dataMutex_.lock();
 	if(data_.size())
 	{

guilib/src/PreferencesDialog.cpp

@@ -38,8 +38,7 @@
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Odometry.h"
-#include "rtabmap/core/CameraOpenni.h"
-#include "rtabmap/core/CameraFreenect.h"
+#include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Memory.h"
@@ -64,9 +63,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui(0),
 	_indexModel(0),
 	_initialized(false),
-	_odomCameraOpenNI(0),
-	_odomCameraFreenect(0),
-	_odomCameraOpenNICv(0),
+	_cameraThread(0),
 	_odomThread(0)
 {
 	ULOGGER_DEBUG("");
@@ -210,12 +207,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stackedWidget_image->setCurrentIndex(_ui->source_comboBox_image_type->currentIndex());
 	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_image, SLOT(setCurrentIndex(int)));
 	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->general_checkBox_autoRestart, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	//usbDevice group
 	connect(_ui->source_usbDevice_spinBox_id, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_imgWidth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_imgheight, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->source_spinBox_framesDropped, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	//images group
 	connect(_ui->source_images_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImage()));
 	connect(_ui->source_images_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -238,8 +233,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->radioButton_freenect->setEnabled(CameraFreenect::available());
 	connect(_ui->radioButton_opennicv, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->radioButton_opennicvasus, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
-	_ui->radioButton_opennicv->setEnabled(CameraRGBD::available());
-	_ui->radioButton_opennicvasus->setEnabled(CameraRGBD::available());
+	_ui->radioButton_opennicv->setEnabled(CameraOpenNICV::available());
+	_ui->radioButton_opennicvasus->setEnabled(CameraOpenNICV::available());
 	connect(_ui->radioButton_openni2, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	_ui->radioButton_openni2->setEnabled(CameraOpenNI2::available());
 	connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -765,8 +760,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->groupBox_sourceImage->setChecked(false);
 		_ui->source_spinBox_imgWidth->setValue(0);
 		_ui->source_spinBox_imgheight->setValue(0);
-		_ui->source_spinBox_framesDropped->setValue(0);
-		_ui->general_checkBox_autoRestart->setChecked(false);
 		_ui->source_images_spinBox_startPos->setValue(1);
 		_ui->source_images_refreshDir->setChecked(false);
 
@@ -987,11 +980,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("Camera");
 	_ui->groupBox_sourceImage->setChecked(settings.value("imageUsed", _ui->groupBox_sourceImage->isChecked()).toBool());
 	_ui->general_doubleSpinBox_imgRate->setValue(settings.value("imgRate", _ui->general_doubleSpinBox_imgRate->value()).toDouble());
-	_ui->general_checkBox_autoRestart->setChecked(settings.value("autoRestart", _ui->general_checkBox_autoRestart->isChecked()).toBool());
 	_ui->source_comboBox_image_type->setCurrentIndex(settings.value("type", _ui->source_comboBox_image_type->currentIndex()).toInt());
 	_ui->source_spinBox_imgWidth->setValue(settings.value("imgWidth",_ui->source_spinBox_imgWidth->value()).toInt());
 	_ui->source_spinBox_imgheight->setValue(settings.value("imgHeight",_ui->source_spinBox_imgheight->value()).toInt());
-	_ui->source_spinBox_framesDropped->setValue(settings.value("framesDropped",_ui->source_spinBox_framesDropped->value()).toInt());
 	//usbDevice group
 	settings.beginGroup("usbDevice");
 	_ui->source_usbDevice_spinBox_id->setValue(settings.value("id",_ui->source_usbDevice_spinBox_id->value()).toInt());
@@ -1196,11 +1187,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
 	settings.beginGroup("Camera");
 	settings.setValue("imageUsed", 		_ui->groupBox_sourceImage->isChecked());
 	settings.setValue("imgRate", 		_ui->general_doubleSpinBox_imgRate->value());
-	settings.setValue("autoRestart", 	_ui->general_checkBox_autoRestart->isChecked());
 	settings.setValue("type", 			_ui->source_comboBox_image_type->currentIndex());
 	settings.setValue("imgWidth", 		_ui->source_spinBox_imgWidth->value());
 	settings.setValue("imgHeight", 		_ui->source_spinBox_imgheight->value());
-	settings.setValue("framesDropped",  _ui->source_spinBox_framesDropped->value());
 	//usbDevice group
 	settings.beginGroup("usbDevice");
 	settings.setValue("id", 			_ui->source_usbDevice_spinBox_id->value());
@@ -2467,10 +2456,6 @@ PreferencesDialog::OdomType PreferencesDialog::getOdometryType() const
 	return kOdomICP;
 }
 
-bool PreferencesDialog::getGeneralAutoRestart() const
-{
-	return _ui->general_checkBox_autoRestart->isChecked();
-}
 int PreferencesDialog::getSourceImageType() const
 {
 	return _ui->source_comboBox_image_type->currentIndex();
@@ -2487,10 +2472,6 @@ int PreferencesDialog::getSourceHeight() const
 {
 	return _ui->source_spinBox_imgheight->value();
 }
-int PreferencesDialog::getFramesDropped() const
-{
-	return _ui->source_spinBox_framesDropped->value();
-}
 QString PreferencesDialog::getSourceImagesPath() const
 {
 	return _ui->source_images_lineEdit_path->text();
@@ -2666,23 +2647,6 @@ void PreferencesDialog::setDetectionRate(double value)
 	}
 }
 
-void PreferencesDialog::setAutoRestart(bool value)
-{
-	ULOGGER_DEBUG("autoRestart=%d", value);
-	if(_ui->general_checkBox_autoRestart->isChecked() != value)
-	{
-		_ui->general_checkBox_autoRestart->setChecked(value);
-		if(validateForm())
-		{
-			this->writeSettings();
-		}
-		else
-		{
-			this->readSettingsBegin();
-		}
-	}
-}
-
 void PreferencesDialog::setTimeLimit(float value)
 {
 	ULOGGER_DEBUG("timeLimit=%fs", value);
@@ -2735,77 +2699,53 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testOdometry(OdomType type)
 {
-	UASSERT(_odomCameraOpenNI == 0 && _odomCameraFreenect == 0 && _odomThread == 0 && _odomCameraOpenNICv == 0);
+	UASSERT(_odomThread == 0 && _cameraThread == 0);
 
-	if(this->getSourceRGBD() == kSrcFreenect)
+	CameraRGBD * camera = 0;
+	if(this->getSourceRGBD() == kSrcOpenNI_PCL)
 	{
-		_odomCameraFreenect = new CameraFreenect(
-				this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
-				this->getGeneralInputRate(),
-				this->getSourceOpenniLocalTransform());
-		if(!_odomCameraFreenect->init())
-		{
-			if(!_odomCameraFreenect->available())
-			{
-				QMessageBox::warning(this,
-				   tr("RTAB-Map"),
-				   tr("Freenect camera unavailable! RTAB-Map is not built with Freenect support (libfreenect)."));
-			}
-			else
-			{
-				QMessageBox::warning(this,
-				   tr("RTAB-Map"),
-				   tr("Freenect camera initialization failed!"));
-			}
-			delete _odomCameraFreenect;
-			_odomCameraFreenect = 0;
-		}
+		camera = new CameraOpenni(
+			this->getSourceOpenniDevice().toStdString(),
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
 	}
-	else if(this->getSourceRGBD() == kSrcOpenNI_CV ||
-			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS ||
-			this->getSourceRGBD() == kSrcOpenNI2)
+	else if(this->getSourceRGBD() == kSrcOpenNI2)
 	{
-		Camera * camera = 0;
-		if(this->getSourceRGBD() == kSrcOpenNI2)
-		{
-			camera = new CameraOpenNI2(
-				this->getGeneralInputRate());
+		camera = new CameraOpenNI2(
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
 
-		}
-		else
-		{
-			camera = new CameraRGBD(
-				this->getGeneralInputRate(),
-				this->getSourceRGBD() == kSrcOpenNI_CV_ASUS);
-		}
-		camera->setLocalTransform(this->getSourceOpenniLocalTransform());
-		_odomCameraOpenNICv = new CameraThread(camera);
-		if(!_odomCameraOpenNICv->init())
-		{
-			QMessageBox::warning(this,
-				   tr("RTAB-Map"),
-				   tr("OpenNI-CV camera initialization failed!"));
-			delete _odomCameraOpenNICv;
-			_odomCameraOpenNICv = 0;
-		}
+	}
+	else if(this->getSourceRGBD() == kSrcFreenect)
+	{
+		camera = new CameraFreenect(
+			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
+	}
+	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
+	{
+		camera = new CameraOpenNICV(
+			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS,
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
 	}
 	else
 	{
-		_odomCameraOpenNI = new CameraOpenni(
-				this->getSourceOpenniDevice().toStdString(),
-				this->getGeneralInputRate(),
-				this->getSourceOpenniLocalTransform());
-		if(!_odomCameraOpenNI->init())
-		{
-			QMessageBox::warning(this,
-				   tr("RTAB-Map"),
-				   tr("OpenNI camera initialization failed!"));
-			delete _odomCameraOpenNI;
-			_odomCameraOpenNI = 0;
-		}
+		UFATAL("RGBD Source type undefined!");
 	}
 
-	if(_odomCameraOpenNI || _odomCameraFreenect || _odomCameraOpenNICv)
+	if(!camera->init())
+	{
+		QMessageBox::warning(this,
+			   tr("RTAB-Map"),
+			   tr("RGBD camera initialization failed!"));
+		delete camera;
+		camera = 0;
+	}
+
+
+	if(camera)
 	{
 		Odometry * odometry;
 		ParametersMap parameters = this->getAllParameters();
@@ -2841,18 +2781,8 @@ void PreferencesDialog::testOdometry(OdomType type)
 		UEventsManager::addHandler(_odomThread);
 		UEventsManager::addHandler(odomViewer);
 
-		if(_odomCameraFreenect)
-		{
-			UEventsManager::createPipe(_odomCameraFreenect, _odomThread, "CameraEvent");
-		}
-		else if(_odomCameraOpenNICv)
-		{
-			UEventsManager::createPipe(_odomCameraOpenNICv, _odomThread, "CameraEvent");
-		}
-		else
-		{
-			UEventsManager::createPipe(_odomCameraOpenNI, _odomThread, "CameraEvent");
-		}
+		_cameraThread = new CameraThread(camera);
+		UEventsManager::createPipe(_cameraThread, _odomThread, "CameraEvent");
 		UEventsManager::createPipe(_odomThread, odomViewer, "OdometryEvent");
 
 		window->showNormal();
@@ -2862,41 +2792,18 @@ void PreferencesDialog::testOdometry(OdomType type)
 		QApplication::processEvents();
 
 		_odomThread->start();
-		if(_odomCameraFreenect)
-		{
-			_odomCameraFreenect->start();
-		}
-		else if(_odomCameraOpenNICv)
-		{
-			_odomCameraOpenNICv->start();
-		}
-		else
-		{
-			_odomCameraOpenNI->start();
-		}
+		_cameraThread->start();
 	}
 }
 
 void PreferencesDialog::cleanOdometryTest()
 {
 	UDEBUG("");
-	if(_odomCameraOpenNI)
+	if(_cameraThread)
 	{
-		_odomCameraOpenNI->kill();
-		delete _odomCameraOpenNI;
-		_odomCameraOpenNI = 0;
-	}
-	if(_odomCameraFreenect)
-	{
-		_odomCameraFreenect->join(true);
-		delete _odomCameraFreenect;
-		_odomCameraFreenect = 0;
-	}
-	if(_odomCameraOpenNICv)
-	{
-		_odomCameraOpenNICv->join(true);
-		delete _odomCameraOpenNICv;
-		_odomCameraOpenNICv = 0;
+		_cameraThread->join(true);
+		delete _cameraThread;
+		_cameraThread = 0;
 	}
 	if(_odomThread)
 	{

guilib/src/ui/preferencesDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>908</width>
-    <height>406</height>
+    <height>494</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-331</y>
-            <width>609</width>
-            <height>657</height>
+            <y>0</y>
+            <width>707</width>
+            <height>752</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -1363,21 +1363,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="0">
-                          <widget class="QCheckBox" name="general_checkBox_autoRestart">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="1" column="1">
-                          <widget class="QLabel" name="label_6">
-                           <property name="text">
-                            <string>Auto restart.</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="1">
                           <widget class="QLabel" name="label_10">
                            <property name="text">
                             <string>Image width (set to 0 to use the default size of the source).</string>
@@ -1387,7 +1373,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
+                         <item row="2" column="1">
                           <widget class="QLabel" name="label_11">
                            <property name="text">
                             <string>Image height (set to 0 to use the default size of the source).</string>
@@ -1397,7 +1383,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="1" column="0">
                           <widget class="QSpinBox" name="source_spinBox_imgWidth">
                            <property name="minimum">
                             <number>0</number>
@@ -1413,7 +1399,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
+                         <item row="2" column="0">
                           <widget class="QSpinBox" name="source_spinBox_imgheight">
                            <property name="minimum">
                             <number>0</number>
@@ -1429,46 +1415,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="0">
-                          <widget class="QCheckBox" name="general_checkBox_cameraKeypoints">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="5" column="1">
-                          <widget class="QLabel" name="label_13">
-                           <property name="text">
-                            <string>Keypoints extraction post threatement.</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="4" column="1">
-                          <widget class="QLabel" name="label_128">
-                           <property name="text">
-                            <string>Frames dropped (ignore X frames after each frame acquired, used to decrease source framerate)</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="4" column="0">
-                          <widget class="QSpinBox" name="source_spinBox_framesDropped">
-                           <property name="minimum">
-                            <number>0</number>
-                           </property>
-                           <property name="maximum">
-                            <number>999999999</number>
-                           </property>
-                           <property name="singleStep">
-                            <number>1</number>
-                           </property>
-                           <property name="value">
-                            <number>0</number>
-                           </property>
-                          </widget>
-                         </item>
                         </layout>
                        </item>
                        <item>

tools/ConsoleApp/main.cpp

@@ -50,7 +50,6 @@ void showUsage()
 			"                                   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"
-			"  -frames_dropped #               Frames dropped from source (Default 0: no frames dropped)\n"
 			"  -start_at #                     When \"path\" is a directory of images, set this parameter\n"
 			"                                   to start processing at image # (default 1)."
 			"  -\"parameter name\" \"value\"       Overwrite a specific RTAB-Map's parameter :\n"
@@ -114,7 +113,6 @@ int main(int argc, char * argv[])
 	int imageWidth = 0;
 	int imageHeight = 0;
 	int startAt = 1;
-	int framesDropped = 0;
 	ParametersMap pm;
 	ULogger::Level logLevel = ULogger::kError;
 	ULogger::Level exitLevel = ULogger::kFatal;
@@ -239,23 +237,6 @@ int main(int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-frames_dropped") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				framesDropped = std::atoi(argv[i]);
-				if(framesDropped < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
 		if(strcmp(argv[i], "-start_at") == 0)
 		{
 			++i;
@@ -357,11 +338,11 @@ int main(int argc, char * argv[])
 	Camera * camera = 0;
 	if(UDirectory::exists(path))
 	{
-		camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight, framesDropped);
+		camera = new CameraImages(path, startAt, false, 1/rate, imageWidth, imageHeight);
 	}
 	else
 	{
-		camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight, framesDropped);
+		camera = new CameraVideo(path, 1/rate, imageWidth, imageHeight);
 	}
 
 	if(!camera || !camera->init())
@@ -402,7 +383,6 @@ int main(int argc, char * argv[])
 	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);
-	printf(" Camera frames dropped %d (default 0)\n", framesDropped);
 	if(createGT)
 	{
 		printf(" Creating the ground truth matrix.\n");

tools/DataRecorder/main.cpp

@@ -1,7 +1,8 @@
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
-#include <rtabmap/core/CameraOpenni.h>
+#include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/CameraRGBD.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/gui/DataRecorder.h>
@@ -22,17 +23,12 @@ void showUsage()
 	exit(1);
 }
 
-rtabmap::CameraOpenni * openniCamera = 0;
 rtabmap::CameraThread * cam = 0;
 QApplication * app = 0;
 // catch ctrl-c
 void sighandler(int sig)
 {
 	printf("\nSignal %d caught...\n", sig);
-	if(openniCamera)
-	{
-		openniCamera->kill();
-	}
 	if(cam)
 	{
 		cam->join(true);
@@ -119,7 +115,7 @@ int main (int argc, char * argv[])
 
 	if(openni)
 	{
-		openniCamera = new rtabmap::CameraOpenni("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+		cam = new rtabmap::CameraThread(new rtabmap::CameraOpenni("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0)));
 	}
 	else
 	{
@@ -140,9 +136,9 @@ int main (int argc, char * argv[])
 			app->processEvents();
 		}
 
-		if(openni?openniCamera->init():cam->init())
+		if(cam->init())
 		{
-			openni?openniCamera->start():cam->start();
+			cam->start();
 
 			app->exec();
 
@@ -160,10 +156,6 @@ int main (int argc, char * argv[])
 		UERROR("Cannot initialize the recorder! Maybe the path is wrong: \"%s\"", fileName.toStdString().c_str());
 	}
 
-	if(openniCamera)
-	{
-		delete openniCamera;
-	}
 	if(cam)
 	{
 		delete cam;

tools/OdometryViewer/main.cpp

@@ -4,7 +4,8 @@
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/core/Odometry.h>
 #include <rtabmap/gui/OdometryViewer.h>
-#include <rtabmap/core/CameraOpenni.h>
+#include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/CameraRGBD.h>
 #include <rtabmap/core/DBReader.h>
 #include <QtGui/QApplication>
 
@@ -555,7 +556,7 @@ int main (int argc, char * argv[])
 	}
 	else
 	{
-		rtabmap::CameraOpenni camera("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+		rtabmap::CameraThread camera(new rtabmap::CameraOpenni("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0)));
 		if(camera.init())
 		{
 			odomThread.start();