Added CameraRGBD with openni from OpenCV, removed feature2D stuff from Camera

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

corelib/include/rtabmap/core/Camera.h

@@ -22,10 +22,7 @@
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <opencv2/highgui/highgui.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Image.h"
-#include "rtabmap/core/Features2d.h"
 #include <set>
 #include <stack>
 #include <list>
@@ -37,9 +34,6 @@ class UTimer;
 namespace rtabmap
 {
 
-class KeypointDetector;
-class KeypointDescriptor;
-
 /**
  * Class Camera
  *
@@ -48,24 +42,20 @@ class RTABMAP_EXP Camera
 {
 public:
 	virtual ~Camera();
-	cv::Mat takeImage();
+	cv::Mat takeImage(); // backward compatibility
-	cv::Mat takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints);
+	void takeImage(cv::Mat & rgb);
+	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
 	virtual bool init() = 0;
 
 	//getters
 	void getImageSize(unsigned int & width, unsigned int & height);
 	float getImageRate() const {return _imageRate;}
-	bool isFeaturesExtracted() const {return _featuresExtracted;}
+	const Transform & getLocalTransform() const {return _localTransform;}
 
 	//setters
-	void setFeaturesExtracted(bool featuresExtracted,
-			KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef,
-			KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef);
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setImageSize(unsigned int width, unsigned int height);
-
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-
-	virtual void parseParameters(const ParametersMap & parameters);
 
 protected:
 	/**
@@ -78,18 +68,15 @@ protected:
 			unsigned int imageHeight = 0,
 			unsigned int framesDropped = 0);
 
-	virtual cv::Mat captureImage() = 0;
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0;
 
 private:
 	float _imageRate;
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
 	unsigned int _framesDropped;
+	Transform _localTransform;
 	UTimer * _frameRateTimer;
-
-	bool _featuresExtracted;
-	KeypointDetector * _keypointDetector;
-	KeypointDescriptor * _keypointDescriptor;
 };
 
 
@@ -113,7 +100,7 @@ public:
 	std::string getPath() const {return _path;}
 
 protected:
-	virtual cv::Mat captureImage();
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
 
 private:
 	std::string _path;
@@ -156,7 +143,7 @@ public:
 	const std::string & getFilePath() const {return _filePath;}
 
 protected:
-	virtual cv::Mat captureImage();
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant);
 
 private:
 	// File type
@@ -170,4 +157,28 @@ private:
 };
 
 
+
+/////////////////////////
+// CameraRGBD
+/////////////////////////
+class RTABMAP_EXP CameraRGBD :
+	public Camera
+{
+
+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;
+};
+
+
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -39,11 +39,8 @@ class Camera;
  */
 class RTABMAP_EXP CameraThread :
 	public UThreadNode,
-	public UEventsHandler
+	public UEventsSender
 {
-public:
-	enum State {kStateCapturing, kStateChangingParameters};
-
 public:
 	// ownership transferred
 	CameraThread(Camera * camera, bool autoRestart = false);
@@ -57,19 +54,11 @@ public:
 	void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
 	Camera * getCamera() {return _camera;}
 
-protected:
-	virtual void handleEvent(UEvent* anEvent);
-
 private:
 	virtual void mainLoop();
-	void process();
-	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 
 private:
 	Camera * _camera;
-	UMutex _stateMutex;
-	std::stack<State> _state;
-	std::stack<ParametersMap> _stateParam;
 	bool _autoRestart;
 	int _seq;
 };

corelib/src/Camera.cpp

@@ -19,7 +19,6 @@
 
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/DBDriver.h"
-#include "rtabmap/core/Features2d.h"
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -31,6 +30,8 @@
 
 #include <opencv2/imgproc/imgproc.hpp>
 
+#include <iostream>
+
 namespace rtabmap
 {
 
@@ -42,40 +43,19 @@ Camera::Camera(float imageRate,
 	_imageWidth(imageWidth),
 	_imageHeight(imageHeight),
 	_framesDropped(framesDropped),
-	_frameRateTimer(new UTimer()),
+	_localTransform(Transform::getIdentity()),
-	_featuresExtracted(false),
+	_frameRateTimer(new UTimer())
-	_keypointDetector(0),
-	_keypointDescriptor(0)
 {
 }
 
 Camera::~Camera()
 {
-	if(_keypointDetector)
-	{
-		delete _keypointDetector;
-	}
-	if(_keypointDescriptor)
-	{
-		delete _keypointDescriptor;
-	}
 	if(_frameRateTimer)
 	{
 		delete _frameRateTimer;
 	}
 }
 
-void Camera::setFeaturesExtracted(bool featuresExtracted, KeypointDetector::DetectorType detector, KeypointDescriptor::DescriptorType descriptor)
-{
-	_featuresExtracted = featuresExtracted;
-	if(detector != KeypointDetector::kDetectorUndef || descriptor != KeypointDescriptor::kDescriptorUndef)
-	{
-		ParametersMap pm;
-		pm.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str((int)detector)));
-		this->parseParameters(pm);
-	}
-}
-
 void Camera::setImageSize(unsigned int width, unsigned int height)
 {
 	_imageWidth = width;
@@ -88,71 +68,23 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
 	height = _imageHeight;
 }
 
-void Camera::parseParameters(const ParametersMap & parameters)
-{
-	UDEBUG("");
-	ParametersMap::const_iterator iter;
-	//Keypoint detector
-	KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef;
-	if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
-	{
-		detector = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
-	}
-
-	if(detector!=KeypointDetector::kDetectorUndef)
-	{
-		ULOGGER_DEBUG("new detector strategy %d", int(detector));
-		if(_keypointDetector)
-		{
-			delete _keypointDetector;
-			_keypointDetector = 0;
-		}
-		if(_keypointDescriptor)
-		{
-			delete _keypointDescriptor;
-			_keypointDescriptor = 0;
-		}
-		switch(detector)
-		{
-		case KeypointDetector::kDetectorSift:
-			_keypointDetector = new SIFTDetector(parameters);
-			_keypointDescriptor = new SIFTDescriptor(parameters);
-			break;
-		case KeypointDetector::kDetectorSurf:
-		default:
-			_keypointDetector = new SURFDetector(parameters);
-			_keypointDescriptor = new SURFDescriptor(parameters);
-			break;
-		}
-	}
-	else
-	{
-		if(_keypointDetector)
-		{
-			_keypointDetector->parseParameters(parameters);
-		}
-		if(_keypointDescriptor)
-		{
-			_keypointDescriptor->parseParameters(parameters);
-		}
-	}
-}
-
 cv::Mat Camera::takeImage()
 {
-	cv::Mat descriptors;
+	cv::Mat rgb, depth;
-	std::vector<cv::KeyPoint> keypoints;
+	float depthConstant = 0.0f;
-	bool tmp = _featuresExtracted;
+	takeImage(rgb, depth, depthConstant);
-	_featuresExtracted = false; // no need to extract descriptors/keypoints in this function
+	return rgb;
-	cv::Mat img = takeImage(descriptors, keypoints);
-	_featuresExtracted = tmp;
-	return img;
 }
 
-cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints)
+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)
 {
-	descriptors = cv::Mat();
-	keypoints.clear();
 	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
 	if(imageRate>0)
 	{
@@ -173,35 +105,22 @@ cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & key
 		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
 	}
 
-	cv::Mat img;
 	UTimer timer;
-	img = this->captureImage();
+	this->captureImage(rgb, depth, depthConstant);
 	UDEBUG("Time capturing image = %fs", timer.ticks());
 
-	if(!img.empty())
+	if(!rgb.empty())
 	{
-		if(img.depth() != CV_8U)
+		UASSERT(rgb.depth() == CV_8U);
-		{
-			UWARN("Images should have already 8U depth !?");
-			cv::Mat tmp = img;
-			img = cv::Mat();
-			tmp.convertTo(img, CV_8U);
-			UDEBUG("Time converting image to 8U = %fs", timer.ticks());
-		}
-
-		if(_featuresExtracted && _keypointDetector && _keypointDescriptor)
-		{
-			keypoints = _keypointDetector->generateKeypoints(img);
-			descriptors = _keypointDescriptor->generateDescriptors(img, keypoints);
-			UDEBUG("Post treatment time = %fs", timer.ticks());
-		}
 
 		if(_framesDropped)
 		{
 			unsigned int count = 0;
 			while(count++ < _framesDropped)
 			{
-				cv::Mat tmp = this->captureImage();
+				cv::Mat tmp,tmp2;
+				float tmpf;
+				this->captureImage(tmp, tmp2, tmpf);
 				if(!tmp.empty())
 				{
 					UDEBUG("frame dropped (%d/%d)", (int)count, (int)_framesDropped);
@@ -214,7 +133,6 @@ cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & key
 			UDEBUG("Frames dropped time = %fs", timer.ticks());
 		}
 	}
-	return img;
 }
 
 /////////////////////////
@@ -272,10 +190,9 @@ bool CameraImages::init()
 	return _dir->isValid();
 }
 
-cv::Mat CameraImages::captureImage()
+void CameraImages::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
 {
 	UDEBUG("");
-	cv::Mat img;
 	if(_dir->isValid())
 	{
 		if(_refreshDir)
@@ -291,7 +208,7 @@ cv::Mat CameraImages::captureImage()
 				{
 					_lastFileName = *fileNames.rbegin();
 					std::string fullPath = _path + _lastFileName;
-					img = cv::imread(fullPath.c_str());
+					rgb = cv::imread(fullPath.c_str());
 				}
 			}
 		}
@@ -311,19 +228,19 @@ cv::Mat CameraImages::captureImage()
 				{
 					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
 #if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
-					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
+					rgb = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
 #else
-					img = cv::imread(fullPath.c_str(), -1);
+					rgb = cv::imread(fullPath.c_str(), -1);
 #endif
-					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", img.cols, img.rows, img.channels(), img.elemSize(), img.total());
+					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d", rgb.cols, rgb.rows, rgb.channels(), rgb.elemSize(), rgb.total());
 
 					// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
-					if(img.depth() != CV_8U)
+					if(rgb.depth() != CV_8U)
 					{
 						// The depth should be 8U
 						UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
 						IplImage * i = cvLoadImage(fullPath.c_str());
-						img = cv::Mat(i, true);
+						rgb = cv::Mat(i, true);
 						cvReleaseImage(&i);
 					}
 				}
@@ -339,18 +256,16 @@ cv::Mat CameraImages::captureImage()
 	unsigned int h;
 	this->getImageSize(w, h);
 
-	if(!img.empty() &&
+	if(!rgb.empty() &&
 	   w &&
 	   h &&
-	   w != (unsigned int)img.cols &&
+	   w != (unsigned int)rgb.cols &&
-	   h != (unsigned int)img.rows)
+	   h != (unsigned int)rgb.rows)
 	{
 		cv::Mat resampled;
-		cv::resize(img, resampled, cv::Size(w, h));
+		cv::resize(rgb, resampled, cv::Size(w, h));
-		img = resampled;
+		rgb = resampled;
 	}
-
-	return img;
 }
 
 
@@ -427,42 +342,115 @@ bool CameraVideo::init()
 	return true;
 }
 
-cv::Mat CameraVideo::captureImage()
+void CameraVideo::captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant)
 {
-	cv::Mat img;  // Null image
 	if(_capture.isOpened())
 	{
-		if(!_capture.read(img))
+		if(_capture.read(rgb))
 		{
-			if(_usbDevice)
+			unsigned int w;
+			unsigned int h;
+			this->getImageSize(w, h);
+
+			if(!rgb.empty() &&
+			   w &&
+			   h &&
+			   w != (unsigned int)rgb.cols &&
+			   h != (unsigned int)rgb.rows)
 			{
-				UERROR("Camera has been disconnected!");
+				cv::Mat resampled;
+				cv::resize(rgb, resampled, cv::Size(w, h));
+				rgb = resampled;
 			}
+			else
+			{
+				// clone required
+				rgb = rgb.clone();
+			}
+		}
+		else if(_usbDevice)
+		{
+			UERROR("Camera has been disconnected!");
 		}
 	}
 	else
 	{
 		ULOGGER_WARN("The camera must be initialized before requesting an image.");
 	}
+}
 
-	unsigned int w;
+/////////////////////////
-	unsigned int h;
+// CameraRGBD
-	this->getImageSize(w, h);
+/////////////////////////
+CameraRGBD::CameraRGBD(float imageRate, bool asus) :
+	Camera(imageRate),
+	_asus(asus)
+{
 
-	if(!img.empty() &&
+}
-	   w &&
+
-	   h &&
+CameraRGBD::~CameraRGBD()
-	   w != (unsigned int)img.cols &&
+{
-	   h != (unsigned int)img.rows)
+	_capture.release();
+}
+
+bool CameraRGBD::init()
+{
+	if(_capture.isOpened())
 	{
-		cv::Mat resampled;
+		_capture.release();
-		cv::resize(img, resampled, cv::Size(w, h));
+	}
-		return resampled;
+
+	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 );
+		// Print some avalible device settings.
+		UINFO("Depth generator output mode:");
+		UINFO("FRAME_WIDTH        %d", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
+		UINFO("FRAME_HEIGHT       %d", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
+		UINFO("FRAME_MAX_DEPTH    %d mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
+		UINFO("FPS                %d", _capture.get( CV_CAP_PROP_FPS ));
+		UINFO("REGISTRATION       %d", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
+		if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
+		{
+			UINFO("Image generator output mode:");
+			UINFO("FRAME_WIDTH    %d", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
+			UINFO("FRAME_HEIGHT   %d", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
+			UINFO("FPS            %d", _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
 	{
-		// clone required
+		ULOGGER_ERROR("CameraRGBD: Failed to create a capture object!");
-		return img.clone();
+		_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();
+		depthConstant = 0.001905f;
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
 	}
 }
 

corelib/src/CameraThread.cpp

@@ -39,7 +39,6 @@ CameraThread::CameraThread(Camera * camera, bool autoRestart) :
 
 CameraThread::~CameraThread()
 {
-	UEventsManager::removeHandler(this);
 	join(true);
 	delete _camera;
 }
@@ -66,79 +65,17 @@ bool CameraThread::init()
 }
 
 void CameraThread::mainLoop()
-{
-	State state = kStateCapturing;
-	ParametersMap parameters;
-
-	_stateMutex.lock();
-	{
-		if(!_state.empty() && !_stateParam.empty())
-		{
-			state = _state.top();
-			_state.pop();
-			parameters = _stateParam.top();
-			_stateParam.pop();
-		}
-	}
-	_stateMutex.unlock();
-
-	if(state == kStateCapturing)
-	{
-		process();
-	}
-	else if(state == kStateChangingParameters)
-	{
-		_camera->parseParameters(parameters);
-	}
-}
-
-void CameraThread::pushNewState(State newState, const ParametersMap & parameters)
-{
-	ULOGGER_DEBUG("to %d", newState);
-
-	_stateMutex.lock();
-	{
-		_state.push(newState);
-		_stateParam.push(parameters);
-	}
-	_stateMutex.unlock();
-}
-
-void CameraThread::handleEvent(UEvent* anEvent)
-{
-	if(anEvent->getClassName().compare("ParamEvent") == 0)
-	{
-		if(this->isIdle())
-		{
-			_stateMutex.lock();
-			_camera->parseParameters(((ParamEvent*)anEvent)->getParameters());
-			_stateMutex.unlock();
-		}
-		else
-		{
-			ULOGGER_DEBUG("changing parameters");
-			pushNewState(kStateChangingParameters, ((ParamEvent*)anEvent)->getParameters());
-		}
-	}
-}
-
-void CameraThread::process()
 {
 	UTimer timer;
 	ULOGGER_DEBUG("Camera::process()");
 	cv::Mat descriptors;
 	std::vector<cv::KeyPoint> keypoints;
-	cv::Mat img = _camera->takeImage(descriptors, keypoints);
+	cv::Mat rgb, depth;
-	if(!img.empty() && !this->isKilled())
+	float depthConstant = 0.0f;
+	 _camera->takeImage(rgb, depth, depthConstant);
+	if(!rgb.empty() && !this->isKilled())
 	{
-		if(_camera->isFeaturesExtracted())
+		this->post(new CameraEvent(rgb, depth, depthConstant, _camera->getLocalTransform(), ++_seq));
-		{
-			this->post(new CameraEvent(descriptors, keypoints, img, ++_seq));
-		}
-		else
-		{
-			this->post(new CameraEvent(img, ++_seq));
-		}
 	}
 	else if(!this->isKilled())
 	{

guilib/include/rtabmap/gui/MainWindow.h

@@ -115,6 +115,8 @@ private slots:
 	void selectDatabase();
 	void selectOpenni();
 	void selectFreenect();
+	void selectOpenniCv();
+	void selectOpenniCvAsus();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void downloadAllClouds();
@@ -177,7 +179,7 @@ private:
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceImageMenu(int type);
 	void updateSelectSourceDatabase(bool used);
-	void updateSelectSourceOpenniMenu(bool used, bool openni);
+	void updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src);
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud();
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -52,6 +52,7 @@ namespace rtabmap {
 class CameraOpenni;
 class CameraFreenect;
 class OdometryThread;
+class CameraThread;
 class Signature;
 class LoopClosureViewer;
 
@@ -75,7 +76,11 @@ public:
 		kSrcUndef,
 		kSrcUsbDevice,
 		kSrcImages,
-		kSrcVideo
+		kSrcVideo,
+		kSrcOpenNI_PCL,
+		kSrcFreenect,
+		kSrcOpenNI_CV,
+		kSrcOpenNI_CV_ASUS
 	};
 
 	enum OdomType {
@@ -146,7 +151,6 @@ public:
 	bool isSourceOpenniUsed() const;
 	OdomType getOdometryType() const;
 	bool getGeneralAutoRestart() const;
-	bool getGeneralCameraKeypoints() const;
 	int getSourceImageType() const;
 	QString getSourceImageTypeStr() const;
 	int getSourceWidth() const;
@@ -162,7 +166,7 @@ public:
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
-	bool getSourceFreenect() const; 			// Openni group
+	Src getSourceRGBD() const; 			// Openni group
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
 
@@ -196,7 +200,7 @@ public slots:
 	void setSLAMMode(bool enabled);
 	void selectSourceImage(Src src = kSrcUndef);
 	void selectSourceDatabase(bool user = false);
-	void selectSourceOpenni(bool openni);
+	void selectSourceRGBD(Src src = kSrcUndef);
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -276,6 +280,7 @@ private:
 	//Odometry test
 	CameraOpenni * _odomCameraOpenNI;
 	CameraFreenect * _odomCameraFreenect;
+	CameraThread * _odomCameraOpenNICv;
 	OdometryThread * _odomThread;
 
 	QVector<QCheckBox*> _3dRenderingShowClouds;

guilib/src/MainWindow.cpp

@@ -290,9 +290,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionUsbCamera, SIGNAL(triggered()), this, SLOT(selectStream()));
 	this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
 	connect(_ui->actionDatabase, SIGNAL(triggered()), this, SLOT(selectDatabase()));
-	this->updateSelectSourceOpenniMenu(_preferencesDialog->isSourceOpenniUsed(), !_preferencesDialog->getSourceFreenect());
+	this->updateSelectSourceRGBDMenu(_preferencesDialog->isSourceOpenniUsed(), _preferencesDialog->getSourceRGBD());
 	connect(_ui->actionOpenNI, SIGNAL(triggered()), this, SLOT(selectOpenni()));
 	connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
+	connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
+	connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
 
 	connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
 	connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));
@@ -1514,7 +1516,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		_ui->doubleSpinBox_stats_imgRate->setValue(_preferencesDialog->getGeneralInputRate());
 		this->updateSelectSourceImageMenu(_preferencesDialog->getSourceImageType());
 		this->updateSelectSourceDatabase(_preferencesDialog->isSourceDatabaseUsed());
-		this->updateSelectSourceOpenniMenu(_preferencesDialog->isSourceOpenniUsed(), !_preferencesDialog->getSourceFreenect());
+		this->updateSelectSourceRGBDMenu(_preferencesDialog->isSourceOpenniUsed(), _preferencesDialog->getSourceRGBD());
 		QString src;
 		if(_preferencesDialog->isSourceImageUsed())
 		{
@@ -1830,10 +1832,12 @@ void MainWindow::updateSelectSourceDatabase(bool used)
 	_ui->actionDatabase->setChecked(used);
 }
 
-void MainWindow::updateSelectSourceOpenniMenu(bool used, bool openni)
+void MainWindow::updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src)
 {
-	_ui->actionOpenNI->setChecked(used && openni);
+	_ui->actionOpenNI->setChecked(used && src == PreferencesDialog::kSrcOpenNI_PCL);
-	_ui->actionFreenect->setChecked(used && !openni);
+	_ui->actionFreenect->setChecked(used && src == PreferencesDialog::kSrcFreenect);
+	_ui->actionOpenNI_CV->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV);
+	_ui->actionOpenNI_CV_ASUS->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV_ASUS);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -2023,7 +2027,7 @@ void MainWindow::startDetection()
 			_odomThread->start();
 		}
 
-		if(_preferencesDialog->getSourceFreenect())
+		if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
 		{
 			_cameraOpenKinect = new CameraFreenect(
 				_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
@@ -2060,6 +2064,37 @@ void MainWindow::startDetection()
 				UEventsManager::createPipe(_cameraOpenKinect, _odomThread, "CameraEvent");
 			}
 		}
+		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
+				_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
+		{
+			Camera * 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");
+			}
+		}
 		else
 		{
 			_cameraOpenni = new CameraOpenni(
@@ -2176,7 +2211,7 @@ void MainWindow::startDetection()
 						_preferencesDialog->getSourceHeight(),
 						_preferencesDialog->getFramesDropped());
 			}
-			else if(sourceType == 0)
+			else //if(sourceType == 0)
 			{
 				camera = new CameraVideo(
 						_preferencesDialog->getSourceUsbDeviceId(),
@@ -2185,21 +2220,6 @@ void MainWindow::startDetection()
 						_preferencesDialog->getSourceHeight(),
 						_preferencesDialog->getFramesDropped());
 			}
-			else
-			{
-				QMessageBox::warning(this,
-										   tr("RTAB-Map"),
-										   tr("Source type is not supported..."));
-				ULOGGER_WARN("iSource type not supported...");
-				emit stateChanged(kIdle);
-				return;
-			}
-
-			if(_preferencesDialog->getGeneralCameraKeypoints())
-			{
-				camera->setFeaturesExtracted(true);
-				camera->parseParameters(_preferencesDialog->getAllParameters());
-			}
 
 			if(!camera->init())
 			{
@@ -2214,7 +2234,6 @@ void MainWindow::startDetection()
 			}
 
 			_camera = new CameraThread(camera);
-			UEventsManager::addHandler(_camera); //thread
 		}
 	}
 
@@ -2580,12 +2599,22 @@ void MainWindow::selectDatabase()
 
 void MainWindow::selectOpenni()
 {
-	_preferencesDialog->selectSourceOpenni(true);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL);
 }
 
 void MainWindow::selectFreenect()
 {
-	_preferencesDialog->selectSourceOpenni(false);
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect);
+}
+
+void MainWindow::selectOpenniCv()
+{
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV);
+}
+
+void MainWindow::selectOpenniCvAsus()
+{
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS);
 }
 
 

guilib/src/PreferencesDialog.cpp

@@ -40,6 +40,8 @@
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/CameraOpenni.h"
 #include "rtabmap/core/CameraFreenect.h"
+#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Memory.h"
 
 #include "rtabmap/gui/LoopClosureViewer.h"
@@ -64,6 +66,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_initialized(false),
 	_odomCameraOpenNI(0),
 	_odomCameraFreenect(0),
+	_odomCameraOpenNICv(0),
 	_odomThread(0)
 {
 	ULOGGER_DEBUG("");
@@ -208,7 +211,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	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()));
-	connect(_ui->general_checkBox_cameraKeypoints, 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()));
@@ -231,8 +233,10 @@ 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_opennipcl, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->radioButton_freenect, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->radioButton_opennicv, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->radioButton_opennicvasus, 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()));
 
@@ -758,7 +762,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_spinBox_imgheight->setValue(0);
 		_ui->source_spinBox_framesDropped->setValue(0);
 		_ui->general_checkBox_autoRestart->setChecked(false);
-		_ui->general_checkBox_cameraKeypoints->setChecked(false);
 		_ui->source_images_spinBox_startPos->setValue(1);
 		_ui->source_images_refreshDir->setChecked(false);
 
@@ -767,8 +770,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 
 		_ui->groupBox_sourceOpenni->setChecked(true);
-		_ui->radioButton_openni->setChecked(true);
+		_ui->radioButton_opennipcl->setChecked(true);
 		_ui->radioButton_freenect->setChecked(false);
+		_ui->radioButton_opennicv->setChecked(false);
+		_ui->radioButton_opennicvasus->setChecked(false);
 		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
 	}
@@ -978,7 +983,6 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_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->general_checkBox_cameraKeypoints->setChecked(settings.value("cameraKeypoints", _ui->general_checkBox_cameraKeypoints->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());
@@ -1008,8 +1012,10 @@ 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_opennipcl->setChecked(settings.value("openniType", _ui->radioButton_opennipcl->isChecked()).toBool());
 	_ui->radioButton_freenect->setChecked(settings.value("freenectType", _ui->radioButton_freenect->isChecked()).toBool());
+	_ui->radioButton_opennicv->setChecked(settings.value("openniCvType", _ui->radioButton_opennicv->isChecked()).toBool());
+	_ui->radioButton_opennicvasus->setChecked(settings.value("openniCvAsusType", _ui->radioButton_opennicvasus->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
@@ -1186,7 +1192,6 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
 	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("cameraKeypoints", _ui->general_checkBox_cameraKeypoints->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());
@@ -1217,8 +1222,10 @@ 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("openniType", 	_ui->radioButton_opennipcl->isChecked());
 	settings.setValue("freenectType", 	_ui->radioButton_freenect->isChecked());
+	settings.setValue("openniCvType", 	_ui->radioButton_opennicv->isChecked());
+	settings.setValue("openniCvAsusType", 	_ui->radioButton_opennicvasus->isChecked());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
 	settings.endGroup();
@@ -1581,7 +1588,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
 	}
 }
 
-void PreferencesDialog::selectSourceOpenni(bool openni)
+void PreferencesDialog::selectSourceRGBD(Src src)
 {
 	ULOGGER_DEBUG("");
 
@@ -1589,8 +1596,8 @@ void PreferencesDialog::selectSourceOpenni(bool openni)
 	{
 		int button = QMessageBox::information(this,
 				tr("Activate RGB-D SLAM?"),
-				tr("You've selected %1 camera as source input, "
+				tr("You've selected RGB-D camera as source input, "
-				   "would you want to activate RGB-D SLAM mode?").arg(openni?"OpenNI":"Freenect"),
+				   "would you want to activate RGB-D SLAM mode?"),
 				QMessageBox::Yes | QMessageBox::No);
 		if(button & QMessageBox::Yes)
 		{
@@ -1599,8 +1606,10 @@ void PreferencesDialog::selectSourceOpenni(bool openni)
 	}
 
 	_ui->groupBox_sourceOpenni->setChecked(true);
-	_ui->radioButton_openni->setChecked(openni);
+	_ui->radioButton_opennipcl->setChecked(src == kSrcOpenNI_PCL);
-	_ui->radioButton_freenect->setChecked(!openni);
+	_ui->radioButton_freenect->setChecked(src == kSrcFreenect);
+	_ui->radioButton_opennicv->setChecked(src == kSrcOpenNI_CV);
+	_ui->radioButton_opennicvasus->setChecked(src == kSrcOpenNI_CV_ASUS);
 
 	if(_obsoletePanels)
 	{
@@ -2456,10 +2465,6 @@ bool PreferencesDialog::getGeneralAutoRestart() const
 {
 	return _ui->general_checkBox_autoRestart->isChecked();
 }
-bool PreferencesDialog::getGeneralCameraKeypoints() const
-{
-	return _ui->general_checkBox_cameraKeypoints->isChecked();
-}
 int PreferencesDialog::getSourceImageType() const
 {
 	return _ui->source_comboBox_image_type->currentIndex();
@@ -2512,9 +2517,25 @@ int PreferencesDialog::getSourceDatabaseStartPos() const
 {
 	return _ui->source_spinBox_databaseStartPos->value();
 }
-bool PreferencesDialog::getSourceFreenect() const
+
+PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
 {
-	return _ui->radioButton_freenect->isChecked();
+	if(_ui->radioButton_freenect->isChecked())
+	{
+		return kSrcFreenect;
+	}
+	else if(_ui->radioButton_opennicv->isChecked())
+	{
+		return kSrcOpenNI_CV;
+	}
+	else if (_ui->radioButton_opennicvasus->isChecked())
+	{
+		return kSrcOpenNI_CV_ASUS;
+	}
+	else
+	{
+		return kSrcOpenNI_PCL;
+	}
 }
 QString PreferencesDialog::getSourceOpenniDevice() const
 {
@@ -2704,9 +2725,9 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testOdometry(OdomType type)
 {
-	UASSERT(_odomCameraOpenNI == 0 && _odomCameraFreenect == 0 && _odomThread == 0);
+	UASSERT(_odomCameraOpenNI == 0 && _odomCameraFreenect == 0 && _odomThread == 0 && _odomCameraOpenNICv == 0);
 
-	if(this->getSourceFreenect())
+	if(this->getSourceRGBD() == kSrcFreenect)
 	{
 		_odomCameraFreenect = new CameraFreenect(
 				this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
@@ -2730,6 +2751,20 @@ void PreferencesDialog::testOdometry(OdomType type)
 			_odomCameraFreenect = 0;
 		}
 	}
+	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
+	{
+		_odomCameraOpenNICv = new CameraThread(new CameraRGBD(
+				this->getGeneralInputRate(),
+				this->getSourceRGBD() == kSrcOpenNI_CV_ASUS));
+		if(!_odomCameraOpenNICv->init())
+		{
+			QMessageBox::warning(this,
+				   tr("RTAB-Map"),
+				   tr("OpenNI-CV camera initialization failed!"));
+			delete _odomCameraOpenNICv;
+			_odomCameraOpenNICv = 0;
+		}
+	}
 	else
 	{
 		_odomCameraOpenNI = new CameraOpenni(
@@ -2746,7 +2781,7 @@ void PreferencesDialog::testOdometry(OdomType type)
 		}
 	}
 
-	if(_odomCameraOpenNI || _odomCameraFreenect)
+	if(_odomCameraOpenNI || _odomCameraFreenect || _odomCameraOpenNICv)
 	{
 		Odometry * odometry;
 		ParametersMap parameters = this->getAllParameters();
@@ -2786,6 +2821,10 @@ void PreferencesDialog::testOdometry(OdomType type)
 		{
 			UEventsManager::createPipe(_odomCameraFreenect, _odomThread, "CameraEvent");
 		}
+		else if(_odomCameraOpenNICv)
+		{
+			UEventsManager::createPipe(_odomCameraOpenNICv, _odomThread, "CameraEvent");
+		}
 		else
 		{
 			UEventsManager::createPipe(_odomCameraOpenNI, _odomThread, "CameraEvent");
@@ -2803,6 +2842,10 @@ void PreferencesDialog::testOdometry(OdomType type)
 		{
 			_odomCameraFreenect->start();
 		}
+		else if(_odomCameraOpenNICv)
+		{
+			_odomCameraOpenNICv->start();
+		}
 		else
 		{
 			_odomCameraOpenNI->start();
@@ -2825,6 +2868,12 @@ void PreferencesDialog::cleanOdometryTest()
 		delete _odomCameraFreenect;
 		_odomCameraFreenect = 0;
 	}
+	if(_odomCameraOpenNICv)
+	{
+		_odomCameraOpenNICv->join(true);
+		delete _odomCameraOpenNICv;
+		_odomCameraOpenNICv = 0;
+	}
 	if(_odomThread)
 	{
 		_odomThread->join(true);

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>947</width>
-     <height>25</height>
+     <height>21</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -92,6 +92,8 @@
       </property>
       <addaction name="actionOpenNI"/>
       <addaction name="actionFreenect"/>
+      <addaction name="actionOpenNI_CV"/>
+      <addaction name="actionOpenNI_CV_ASUS"/>
      </widget>
      <addaction name="menuImage"/>
      <addaction name="actionDatabase"/>
@@ -983,7 +985,7 @@
     <bool>true</bool>
    </property>
    <property name="text">
-    <string>OpenNI</string>
+    <string>OpenNI-PCL</string>
    </property>
   </action>
   <action name="actionFreenect">
@@ -994,6 +996,22 @@
     <string>Freenect</string>
    </property>
   </action>
+  <action name="actionOpenNI_CV">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>OpenNI-CV</string>
+   </property>
+  </action>
+  <action name="actionOpenNI_CV_ASUS">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>OpenNI-CV-ASUS</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-142</y>
-            <width>729</width>
+            <width>728</width>
-            <height>836</height>
+            <height>721</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1736,9 +1736,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item>
                         <layout class="QHBoxLayout" name="horizontalLayout_7">
                          <item>
-                          <widget class="QRadioButton" name="radioButton_openni">
+                          <widget class="QRadioButton" name="radioButton_opennipcl">
                            <property name="text">
-                            <string>OpenNI</string>
+                            <string>OpenNI-PCL</string>
                            </property>
                           </widget>
                          </item>
@@ -1749,6 +1749,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item>
+                          <widget class="QRadioButton" name="radioButton_opennicv">
+                           <property name="text">
+                            <string>OpenNI-CV</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item>
+                          <widget class="QRadioButton" name="radioButton_opennicvasus">
+                           <property name="text">
+                            <string>OpenNI-CV-ASUS</string>
+                           </property>
+                          </widget>
+                         </item>
                          <item>
                           <spacer name="horizontalSpacer">
                            <property name="orientation">

tools/ConsoleApp/main.cpp

@@ -422,20 +422,14 @@ int main(int argc, char * argv[])
 	}
 	printf("\nProcessing images...\n");
 
-	//setup camera
-	camera->setFeaturesExtracted(true);
-	camera->parseParameters(pm);
-
 	UTimer iterationTimer;
 	UTimer rtabmapTimer;
 	int imagesProcessed = 0;
 	std::list<std::vector<float> > teleopActions;
 	while(loopDataset <= repeat && g_forever)
 	{
-		cv::Mat descriptors;
+		cv::Mat cvImg = camera->takeImage();
-		std::vector<cv::KeyPoint> keypoints;
+		Image img(cvImg, 0);
-		cv::Mat cvImg = camera->takeImage(descriptors, keypoints);
-		Image img(cvImg, 0, descriptors, keypoints);
 		int i=0;
 		double maxIterationTime = 0.0;
 		int maxIterationTimeId = 0;
@@ -451,8 +445,8 @@ int main(int argc, char * argv[])
 			{
 				++countLoopDetected;
 			}
-			cvImg = camera->takeImage(descriptors, keypoints);
+			cvImg = camera->takeImage();
-			img = Image(cvImg, 0, descriptors, keypoints);
+			img = Image(cvImg, 0);
 			if(++count % 100 == 0)
 			{
 				printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",