Added Freenect camera source

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

CMakeLists.txt

@@ -136,6 +136,7 @@ FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL 1.7 REQUIRED)
 FIND_PACKAGE(VTK REQUIRED)
 FIND_PACKAGE(ZLIB REQUIRED)
+FIND_PACKAGE(Freenect REQUIRED)
 
 # If Qt is here, the GUI will be built
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) 

cmake_modules/FindFreenect.cmake

@@ -0,0 +1,30 @@
+# - Find Freenect alias libfreenect
+# This module finds an installed Freenect package.
+#
+# It sets the following variables:
+#  Freenect_FOUND       - Set to false, or undefined, if Freenect isn't found.
+#  Freenect_INCLUDE_DIRS - The Freenect include directory.
+#  Freenect_LIBRARIES     - The Freenect library to link against.
+
+FIND_PATH(Freenect_INCLUDE_DIRS libfreenect.hpp PATH_SUFFIXES libfreenect)
+
+FIND_LIBRARY(Freenect_LIBRARY NAMES freenect)
+FIND_LIBRARY(Freenect_sync_LIBRARY NAMES freenect_sync)
+
+IF (Freenect_INCLUDE_DIRS AND Freenect_LIBRARY AND Freenect_sync_LIBRARY)
+   SET(Freenect_FOUND TRUE)
+ENDIF (Freenect_INCLUDE_DIRS AND Freenect_LIBRARY AND Freenect_sync_LIBRARY)
+
+IF (Freenect_FOUND)
+   # show which Freenect was found only if not quiet
+   SET(Freenect_LIBRARIES ${Freenect_LIBRARY} ${Freenect_sync_LIBRARY})
+   IF (NOT Freenect_FIND_QUIETLY)
+      MESSAGE(STATUS "Found Freenect")
+   ENDIF (NOT Freenect_FIND_QUIETLY)
+ELSE (Freenect_FOUND)
+   # fatal error if Freenect is required but not found
+   IF (Freenect_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find Freenect (libfreenect)")
+   ENDIF (Freenect_FIND_REQUIRED)
+ENDIF (Freenect_FOUND)
+

corelib/include/rtabmap/core/CameraFreenect.h

@@ -0,0 +1,91 @@
+/*
+ * 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 <libfreenect.h>
+#include <opencv2/opencv.hpp>
+
+class UTimer;
+
+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();
+
+	// 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_;
+};
+
+class RTABMAP_EXP CameraFreenect : public UEventsSender, public UThread
+{
+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/src/CMakeLists.txt

@@ -14,6 +14,7 @@ SET(SRC_FILES
     Camera.cpp
     CameraThread.cpp
     CameraOpenni.cpp
+    CameraFreenect.cpp
     
     EpipolarGeometry.cpp
 	VisualWord.cpp
@@ -43,6 +44,7 @@ SET(INCLUDE_DIRS
     ${OpenCV_INCLUDE_DIRS}
 	${PCL_INCLUDE_DIRS}
 	${ZLIB_INCLUDE_DIRS}
+	${Freenect_INCLUDE_DIRS}
 )
 
 ####################################
@@ -81,7 +83,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 # Add binary that is built from the source file "main.cpp".
 # The extension is automatically found.
 ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
-TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${OpenCV_LIBS} ${PCL_LIBRARIES} ${ZLIB_LIBRARIES})
+TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${OpenCV_LIBS} ${PCL_LIBRARIES} ${ZLIB_LIBRARIES} ${Freenect_LIBRARIES})
 
 INSTALL(TARGETS rtabmap_core
         RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime

corelib/src/CameraFreenect.cpp

@@ -0,0 +1,252 @@
+/*
+ * 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>
+
+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)
+{
+	UASSERT(ctx_ != 0);
+}
+
+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("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);
+	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);
+}
+
+// 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;
+}
+
+
+//
+// CameraOpenKinect
+//
+CameraFreenect::CameraFreenect(int deviceId, float inputRate, const Transform & localTransform) :
+		deviceId_(deviceId),
+		rate_(inputRate),
+		frameRateTimer_(new UTimer()),
+		localTransform_(localTransform),
+		seq_(0),
+		ctx_(0),
+		freenectDevice_(0)
+{
+	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));
+}
+
+CameraFreenect::~CameraFreenect()
+{
+	UDEBUG("");
+	join(true);
+	if(freenectDevice_)
+	{
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+	if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
+	delete frameRateTimer_;
+}
+
+bool CameraFreenect::init()
+{
+	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("CameraOpenKinect: No devices connected!");
+		}
+	}
+	else
+	{
+		UERROR("CameraOpenKinect: Cannot initialize the camera because it is already running...");
+	}
+	return false;
+}
+
+
+void CameraFreenect::setFrameRate(float rate)
+{
+	rate_ = rate;
+}
+
+void CameraFreenect::mainLoopBegin()
+{
+	if(freenectDevice_)
+	{
+		freenectDevice_->startDepth();
+		freenectDevice_->startVideo();
+		frameRateTimer_->start();
+	}
+	else
+	{
+		UERROR("CameraOpenKinect: init should be called before starting the camera.");
+	}
+}
+
+void CameraFreenect::mainLoop()
+{
+	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("CameraOpenKinect: Depth not ready! Try to reduce the image rate to avoid this warning...");
+				return;
+			}
+
+			if(rgb.empty())
+			{
+				UWARN("CameraOpenKinect: Rgb not ready! Try to reduce the image rate to avoid this warning...");
+				return;
+			}
+
+			float constant = 0.001905f;
+			this->post(new CameraEvent(rgb, depth, constant, localTransform_, ++seq_));
+		}
+	}
+}
+
+void CameraFreenect::mainLoopEnd()
+{
+	if(freenectDevice_)
+	{
+		freenectDevice_->stopDepth();
+		freenectDevice_->stopVideo();
+	}
+}
+
+} /* namespace rtabmap */

corelib/src/EpipolarGeometry.cpp

@@ -49,19 +49,9 @@ EpipolarGeometry::~EpipolarGeometry() {
 
 void EpipolarGeometry::parseParameters(const ParametersMap & parameters)
 {
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
-	{
-		_matchCountMinAccepted = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
-	{
-		_ransacParam1 = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
-	{
-		_ransacParam2 = std::atof((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kVhEpMatchCountMin(), _matchCountMinAccepted);
+	Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), _ransacParam1);
+	Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), _ransacParam2);
 }
 
 bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)

corelib/src/Features2d.cpp

@@ -27,9 +27,9 @@
 #include "rtabmap/utilite/UTimer.h"
 #include <opencv2/imgproc/imgproc_c.h>
 #include <opencv2/gpu/gpu.hpp>
-#include <opencv2/nonfree/gpu.hpp>
 #include <opencv2/core/version.hpp>
 #if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
+#include <opencv2/nonfree/gpu.hpp>
 #include <opencv2/nonfree/features2d.hpp>
 #endif
 

examples/RGBDMapping/main.cpp

@@ -20,6 +20,7 @@
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/core/CameraOpenni.h"
+#include "rtabmap/core/CameraFreenect.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/utilite/UEventsManager.h"
 #include <QtGui/QApplication>
@@ -43,6 +44,8 @@ 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())
 	{
 		UERROR("Camera init failed!");

guilib/include/rtabmap/gui/MainWindow.h

@@ -37,6 +37,7 @@ namespace rtabmap {
 class CameraThread;
 class DBReader;
 class CameraOpenni;
+class CameraFreenect;
 class OdometryThread;
 class CloudViewer;
 }
@@ -113,6 +114,7 @@ private slots:
 	void selectStream();
 	void selectDatabase();
 	void selectOpenni();
+	void selectFreenect();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void downloadAllClouds();
@@ -175,7 +177,7 @@ private:
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceImageMenu(int type);
 	void updateSelectSourceDatabase(bool used);
-	void updateSelectSourceOpenni(bool used);
+	void updateSelectSourceOpenniMenu(bool used, bool openni);
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud();
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
@@ -201,6 +203,7 @@ private:
 	rtabmap::CameraThread * _camera;
 	rtabmap::DBReader * _dbReader;
 	rtabmap::CameraOpenni * _cameraOpenni;
+	rtabmap::CameraFreenect * _cameraOpenKinect;
 	rtabmap::OdometryThread * _odomThread;
 
 	SrcType _srcType;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -50,6 +50,7 @@ class QDoubleSpinBox;
 namespace rtabmap {
 
 class CameraOpenni;
+class CameraFreenect;
 class OdometryThread;
 class Signature;
 class LoopClosureViewer;
@@ -155,6 +156,7 @@ public:
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
+	bool getSourceFreenect() const; 			// Openni group
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
 
@@ -188,7 +190,7 @@ public slots:
 	void setSLAMMode(bool enabled);
 	void selectSourceImage(Src src = kSrcUndef);
 	void selectSourceDatabase(bool user = false);
-	void selectSourceOpenni(bool user = false);
+	void selectSourceOpenni(bool openni);
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -266,7 +268,8 @@ private:
 	QProgressDialog * _progressDialog;
 
 	//Odometry test
-	CameraOpenni * _odomCamera;
+	CameraOpenni * _odomCameraOpenNI;
+	CameraFreenect * _odomCameraFreenect;
 	OdometryThread * _odomThread;
 
 	QVector<QCheckBox*> _3dRenderingShowClouds;

guilib/src/MainWindow.cpp

@@ -66,6 +66,7 @@
 
 //RGB-D stuff
 #include "rtabmap/core/CameraOpenni.h"
+#include "rtabmap/core/CameraFreenect.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
@@ -97,6 +98,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_camera(0),
 	_dbReader(0),
 	_cameraOpenni(0),
+	_cameraOpenKinect(0),
 	_odomThread(0),
 	_srcType(kSrcUndefined),
 	_preferencesDialog(0),
@@ -280,15 +282,17 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	selectSourceImageGrp->addAction(_ui->actionUsbCamera);
 	selectSourceImageGrp->addAction(_ui->actionImageFiles);
 	selectSourceImageGrp->addAction(_ui->actionVideo);
-	selectSourceImageGrp->addAction(_ui->actionOpenni_RGBD);
+	selectSourceImageGrp->addAction(_ui->actionOpenNI);
+	selectSourceImageGrp->addAction(_ui->actionFreenect);
 	this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
 	connect(_ui->actionImageFiles, SIGNAL(triggered()), this, SLOT(selectImages()));
 	connect(_ui->actionVideo, SIGNAL(triggered()), this, SLOT(selectVideo()));
 	connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
 	this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
 	connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase()));
-	this->updateSelectSourceOpenni(_preferencesDialog->isSourceOpenniUsed());
-	connect(_ui->actionOpenni_RGBD, SIGNAL(triggered()), this, SLOT(selectOpenni()));
+	this->updateSelectSourceOpenniMenu(_preferencesDialog->isSourceOpenniUsed(), !_preferencesDialog->getSourceFreenect());
+	connect(_ui->actionOpenNI, SIGNAL(triggered()), this, SLOT(selectOpenni()));
+	connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
 
 	connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
 	connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));
@@ -418,6 +422,12 @@ void MainWindow::closeEvent(QCloseEvent* event)
 			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!");
@@ -1475,7 +1485,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		_ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
 		this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
 		this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
-		this->updateSelectSourceOpenni(_preferencesDialog->isSourceOpenniUsed());
+		this->updateSelectSourceOpenniMenu(_preferencesDialog->isSourceOpenniUsed(), !_preferencesDialog->getSourceFreenect());
 		QString src;
 		if(_preferencesDialog->isSourceImageUsed())
 		{
@@ -1501,6 +1511,11 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		{
 			_cameraOpenni->setFrameRate(_preferencesDialog->getGeneralInputRate());
 		}
+
+		if(_cameraOpenKinect)
+		{
+			_cameraOpenKinect->setFrameRate(_preferencesDialog->getGeneralInputRate());
+		}
 	}
 
 	if(flags & PreferencesDialog::kPanelGeneral)
@@ -1786,9 +1801,10 @@ void MainWindow::updateSelectSourceDatabase(bool used)
 	_ui->actionDatabase->setChecked(used);
 }
 
-void MainWindow::updateSelectSourceOpenni(bool used)
+void MainWindow::updateSelectSourceOpenniMenu(bool used, bool openni)
 {
-	_ui->actionOpenni_RGBD->setChecked(used);
+	_ui->actionOpenNI->setChecked(used && openni);
+	_ui->actionFreenect->setChecked(used && !openni);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -1923,6 +1939,15 @@ void MainWindow::startDetection()
 		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() &&
@@ -1969,31 +1994,61 @@ void MainWindow::startDetection()
 			_odomThread->start();
 		}
 
-		// With odomtry, go as fast as we can
-		_cameraOpenni = new CameraOpenni(
-			_preferencesDialog->getSourceOpenniDevice().toStdString(),
-			_preferencesDialog->getGeneralInputRate(),
-			_preferencesDialog->getSourceOpenniLocalTransform());
-
-		if(!_cameraOpenni->init())
+		if(_preferencesDialog->getSourceFreenect())
 		{
-			ULOGGER_WARN("init CameraOpenni failed... ");
-			QMessageBox::warning(this,
-								   tr("RTAB-Map"),
-								   tr("Openni camera initialization failed..."));
-			emit stateChanged(kIdle);
-			delete _cameraOpenni;
-			_cameraOpenni = 0;
+			_cameraOpenKinect = new CameraFreenect(
+				_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
+				_preferencesDialog->getGeneralInputRate(),
+				_preferencesDialog->getSourceOpenniLocalTransform());
+
+			if(!_cameraOpenKinect->init())
+			{
+				ULOGGER_WARN("init CameraOpenKinect failed... ");
+				QMessageBox::warning(this,
+									   tr("RTAB-Map"),
+									   tr("OpenKinect camera initialization failed..."));
+				emit stateChanged(kIdle);
+				delete _cameraOpenKinect;
+				_cameraOpenKinect = 0;
+				if(_odomThread)
+				{
+					delete _odomThread;
+					_odomThread = 0;
+				}
+				return;
+			}
 			if(_odomThread)
 			{
-				delete _odomThread;
-				_odomThread = 0;
+				UEventsManager::createPipe(_cameraOpenKinect, _odomThread, "CameraEvent");
 			}
-			return;
 		}
-		if(_odomThread)
+		else
 		{
-			UEventsManager::createPipe(_cameraOpenni, _odomThread, "CameraEvent");
+			_cameraOpenni = new CameraOpenni(
+				_preferencesDialog->getSourceOpenniDevice().toStdString(),
+				_preferencesDialog->getGeneralInputRate(),
+				_preferencesDialog->getSourceOpenniLocalTransform());
+
+			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(_odomThread)
+			{
+				UEventsManager::createPipe(_cameraOpenni, _odomThread, "CameraEvent");
+			}
 		}
 	}
 	else if(_preferencesDialog->isSourceDatabaseUsed())
@@ -2149,7 +2204,7 @@ void MainWindow::startDetection()
 // Could not be in the main thread here! (see handleEvents())
 void MainWindow::pauseDetection()
 {
-	if(_camera || _dbReader || _cameraOpenni)
+	if(_camera || _dbReader || _cameraOpenni || _cameraOpenKinect)
 	{
 		if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier))
 		{
@@ -2183,7 +2238,7 @@ void MainWindow::pauseDetection()
 
 void MainWindow::stopDetection()
 {
-	if(_state == kIdle || (!_camera && !_dbReader && !_cameraOpenni))
+	if(_state == kIdle || (!_camera && !_dbReader && !_cameraOpenni && !_cameraOpenKinect))
 	{
 		return;
 	}
@@ -2191,7 +2246,8 @@ void MainWindow::stopDetection()
 	if(_state == kDetecting &&
 			( (_camera && _camera->isRunning()) ||
 			  (_dbReader && _dbReader->isRunning()) ||
-			  (_cameraOpenni && _cameraOpenni->isRunning())) )
+			  (_cameraOpenni && _cameraOpenni->isRunning()) ||
+			  (_cameraOpenKinect && _cameraOpenKinect->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);
 
@@ -2218,6 +2274,11 @@ void MainWindow::stopDetection()
 		_cameraOpenni->kill();
 	}
 
+	if(_cameraOpenKinect)
+	{
+		_cameraOpenKinect->join(true);
+	}
+
 	if(_odomThread)
 	{
 		_ui->actionReset_Odometry->setEnabled(false);
@@ -2240,6 +2301,11 @@ void MainWindow::stopDetection()
 		delete _cameraOpenni;
 		_cameraOpenni = 0;
 	}
+	if(_cameraOpenKinect)
+	{
+		delete _cameraOpenKinect;
+		_cameraOpenKinect = 0;
+	}
 	if(_odomThread)
 	{
 		delete _odomThread;
@@ -2479,6 +2545,11 @@ void MainWindow::selectOpenni()
 	_preferencesDialog->selectSourceOpenni(true);
 }
 
+void MainWindow::selectFreenect()
+{
+	_preferencesDialog->selectSourceOpenni(false);
+}
+
 
 void MainWindow::dumpTheMemory()
 {
@@ -3552,6 +3623,11 @@ void MainWindow::changeState(MainWindow::State newState)
 		{
 			_cameraOpenni->start();
 		}
+
+		if(_cameraOpenKinect)
+		{
+			_cameraOpenKinect->start();
+		}
 		break;
 
 	case kPaused:
@@ -3585,6 +3661,11 @@ void MainWindow::changeState(MainWindow::State newState)
 			{
 				_cameraOpenni->start();
 			}
+
+			if(_cameraOpenKinect)
+			{
+				_cameraOpenKinect->start();
+			}
 		}
 		else if(_state == kDetecting)
 		{
@@ -3616,6 +3697,11 @@ void MainWindow::changeState(MainWindow::State newState)
 			{
 				_cameraOpenni->pause();
 			}
+
+			if(_cameraOpenKinect)
+			{
+				_cameraOpenKinect->join(true);
+			}
 		}
 		break;
 	case kMonitoring:

guilib/src/PreferencesDialog.cpp

@@ -39,6 +39,7 @@
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/CameraOpenni.h"
+#include "rtabmap/core/CameraFreenect.h"
 #include "rtabmap/core/Memory.h"
 
 #include "rtabmap/gui/LoopClosureViewer.h"
@@ -61,7 +62,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui(0),
 	_indexModel(0),
 	_initialized(false),
-	_odomCamera(0),
+	_odomCameraOpenNI(0),
+	_odomCameraFreenect(0),
 	_odomThread(0)
 {
 	ULOGGER_DEBUG("");
@@ -208,6 +210,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	//openni group
 	connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->radioButton_openni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->radioButton_freenect, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -735,7 +739,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 
 		_ui->groupBox_sourceOpenni->setChecked(true);
-		_ui->lineEdit_openniDevice->setText("#1");
+		_ui->radioButton_openni->setChecked(true);
+		_ui->radioButton_freenect->setChecked(false);
+		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
@@ -969,6 +975,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 
 	settings.beginGroup("Openni");
 	_ui->groupBox_sourceOpenni->setChecked(settings.value("openniUsed", _ui->groupBox_sourceOpenni->isChecked()).toBool());
+	_ui->radioButton_openni->setChecked(settings.value("openniType", _ui->radioButton_openni->isChecked()).toBool());
+	_ui->radioButton_freenect->setChecked(settings.value("freenectType", _ui->radioButton_freenect->isChecked()).toBool());
 	_ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString());
 	_ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString());
 	settings.endGroup(); // Openni
@@ -1171,6 +1179,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
 
 	settings.beginGroup("Openni");
 	settings.setValue("openniUsed", 	_ui->groupBox_sourceOpenni->isChecked());
+	settings.setValue("openniType", 	_ui->radioButton_openni->isChecked());
+	settings.setValue("freenectType", 	_ui->radioButton_freenect->isChecked());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
 	settings.endGroup();
@@ -1533,7 +1543,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
 	}
 }
 
-void PreferencesDialog::selectSourceOpenni(bool user)
+void PreferencesDialog::selectSourceOpenni(bool openni)
 {
 	ULOGGER_DEBUG("");
 
@@ -1541,8 +1551,8 @@ void PreferencesDialog::selectSourceOpenni(bool user)
 	{
 		int button = QMessageBox::information(this,
 				tr("Activate RGB-D SLAM?"),
-				tr("You've selected OpenNI camera as source input, "
-				   "would you want to activate RGB-D SLAM mode?"),
+				tr("You've selected %1 camera as source input, "
+				   "would you want to activate RGB-D SLAM mode?").arg(openni?"OpenNI":"Freenect"),
 				QMessageBox::Yes | QMessageBox::No);
 		if(button & QMessageBox::Yes)
 		{
@@ -1550,13 +1560,11 @@ void PreferencesDialog::selectSourceOpenni(bool user)
 		}
 	}
 
-	if(user)
-	{
-		// from user
-		_ui->groupBox_sourceOpenni->setChecked(true);
-	}
+	_ui->groupBox_sourceOpenni->setChecked(true);
+	_ui->radioButton_openni->setChecked(openni);
+	_ui->radioButton_freenect->setChecked(!openni);
 
-	if(user && _obsoletePanels)
+	if(_obsoletePanels)
 	{
 		_ui->groupBox_sourceImage->setChecked(false);
 		_ui->groupBox_sourceDatabase->setChecked(false);
@@ -2442,6 +2450,10 @@ int PreferencesDialog::getSourceDatabaseStartPos() const
 {
 	return _ui->source_spinBox_databaseStartPos->value();
 }
+bool PreferencesDialog::getSourceFreenect() const
+{
+	return _ui->radioButton_freenect->isChecked();
+}
 QString PreferencesDialog::getSourceOpenniDevice() const
 {
 	return _ui->lineEdit_openniDevice->text();
@@ -2630,14 +2642,34 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testOdometry(OdomType type)
 {
-	UASSERT(_odomCamera == 0 && _odomThread == 0);
+	UASSERT(_odomCameraOpenNI == 0 && _odomCameraFreenect == 0 && _odomThread == 0);
 
-	_odomCamera = new CameraOpenni(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform());
+	if(this->getSourceFreenect())
+	{
+		_odomCameraFreenect = new CameraFreenect(
+				this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+				this->getGeneralInputRate(),
+				this->getSourceOpenniLocalTransform());
+		if(!_odomCameraFreenect->init())
+		{
+			delete _odomCameraFreenect;
+			_odomCameraFreenect = 0;
+		}
+	}
+	else
+	{
+		_odomCameraOpenNI = new CameraOpenni(
+				this->getSourceOpenniDevice().toStdString(),
+				this->getGeneralInputRate(),
+				this->getSourceOpenniLocalTransform());
+		if(!_odomCameraOpenNI->init())
+		{
+			delete _odomCameraOpenNI;
+			_odomCameraOpenNI = 0;
+		}
+	}
 
-	if(_odomCamera->init())
+	if(_odomCameraOpenNI || _odomCameraFreenect)
 	{
 		Odometry * odometry;
 		ParametersMap parameters = this->getAllParameters();
@@ -2673,7 +2705,14 @@ void PreferencesDialog::testOdometry(OdomType type)
 		UEventsManager::addHandler(_odomThread);
 		UEventsManager::addHandler(odomViewer);
 
-		UEventsManager::createPipe(_odomCamera, _odomThread, "CameraEvent");
+		if(_odomCameraFreenect)
+		{
+			UEventsManager::createPipe(_odomCameraFreenect, _odomThread, "CameraEvent");
+		}
+		else
+		{
+			UEventsManager::createPipe(_odomCameraOpenNI, _odomThread, "CameraEvent");
+		}
 		UEventsManager::createPipe(_odomThread, odomViewer, "OdometryEvent");
 
 		window->showNormal();
@@ -2683,24 +2722,35 @@ void PreferencesDialog::testOdometry(OdomType type)
 		QApplication::processEvents();
 
 		_odomThread->start();
-		_odomCamera->start();
+		if(_odomCameraFreenect)
+		{
+			_odomCameraFreenect->start();
+		}
+		else
+		{
+			_odomCameraOpenNI->start();
+		}
 	}
 	else
 	{
-		QMessageBox::warning(this, "Initialization failed!", tr("Openni camera initialization failed!"));
-		delete _odomCamera;
-		_odomCamera = 0;
+		QMessageBox::warning(this, "Initialization failed!", tr("RGB-D camera initialization failed!"));
 	}
 }
 
 void PreferencesDialog::cleanOdometryTest()
 {
 	UDEBUG("");
-	if(_odomCamera)
+	if(_odomCameraOpenNI)
 	{
-		_odomCamera->kill();
-		delete _odomCamera;
-		_odomCamera = 0;
+		_odomCameraOpenNI->kill();
+		delete _odomCameraOpenNI;
+		_odomCameraOpenNI = 0;
+	}
+	if(_odomCameraFreenect)
+	{
+		_odomCameraFreenect->join(true);
+		delete _odomCameraFreenect;
+		_odomCameraFreenect = 0;
 	}
 	if(_odomThread)
 	{

guilib/src/ui/mainWindow.ui

@@ -86,9 +86,16 @@
       <addaction name="actionImageFiles"/>
       <addaction name="actionVideo"/>
      </widget>
+     <widget class="QMenu" name="menuRGB_D_camera">
+      <property name="title">
+       <string>RGB-D camera</string>
+      </property>
+      <addaction name="actionOpenNI"/>
+      <addaction name="actionFreenect"/>
+     </widget>
      <addaction name="menuImage"/>
      <addaction name="actionDatabase"/>
-     <addaction name="actionOpenni_RGBD"/>
+     <addaction name="menuRGB_D_camera"/>
     </widget>
     <addaction name="menuSelect_source"/>
     <addaction name="separator"/>
@@ -890,14 +897,6 @@
     <string>Print loop closure IDs to console</string>
    </property>
   </action>
-  <action name="actionOpenni_RGBD">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>Openni (RGBD)</string>
-   </property>
-  </action>
   <action name="actionSave_point_cloud">
    <property name="text">
     <string>Save high-res point clouds (*.pcd *.ply *.vtk)...</string>
@@ -979,6 +978,22 @@
     <string>Take a screenshot</string>
    </property>
   </action>
+  <action name="actionOpenNI">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>OpenNI</string>
+   </property>
+  </action>
+  <action name="actionFreenect">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Freenect</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>859</width>
-    <height>390</height>
+    <width>1009</width>
+    <height>612</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
-            <width>560</width>
-            <height>632</height>
+            <y>-271</y>
+            <width>711</width>
+            <height>795</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>9</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1521,7 +1521,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QGroupBox" name="groupBox_sourceOpenni">
                       <property name="title">
-                       <string>OpenNI camera (RGB-D)</string>
+                       <string>RGB-D camera</string>
                       </property>
                       <property name="checkable">
                        <bool>true</bool>
@@ -1533,13 +1533,44 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item>
                         <widget class="QLabel" name="label_40">
                          <property name="text">
-                          <string>Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live).</string>
+                          <string>Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
                          </property>
                         </widget>
                        </item>
+                       <item>
+                        <layout class="QHBoxLayout" name="horizontalLayout_7">
+                         <item>
+                          <widget class="QRadioButton" name="radioButton_openni">
+                           <property name="text">
+                            <string>OpenNI</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item>
+                          <widget class="QRadioButton" name="radioButton_freenect">
+                           <property name="text">
+                            <string>Feeenect</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item>
+                          <spacer name="horizontalSpacer">
+                           <property name="orientation">
+                            <enum>Qt::Horizontal</enum>
+                           </property>
+                           <property name="sizeHint" stdset="0">
+                            <size>
+                             <width>40</width>
+                             <height>20</height>
+                            </size>
+                           </property>
+                          </spacer>
+                         </item>
+                        </layout>
+                       </item>
                        <item>
                         <layout class="QFormLayout" name="formLayout">
                          <property name="fieldGrowthPolicy">
@@ -1548,7 +1579,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          <item row="0" column="0">
                           <widget class="QLineEdit" name="lineEdit_openniDevice">
                            <property name="text">
-                            <string>#1</string>
+                            <string/>
                            </property>
                           </widget>
                          </item>