diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h
index 038a5da3..c21246f4 100644
--- a/guilib/include/rtabmap/gui/CameraViewer.h
+++ b/guilib/include/rtabmap/gui/CameraViewer.h
@@ -31,7 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include <rtabmap/utilite/UEventsHandler.h>
-#include <QWidget>
+#include <QDialog>
 #include <rtabmap/core/SensorData.h>
 
 namespace rtabmap {
@@ -39,7 +39,7 @@ namespace rtabmap {
 class ImageView;
 class CloudViewer;
 
-class RTABMAPGUI_EXP CameraViewer : public QWidget, public UEventsHandler
+class RTABMAPGUI_EXP CameraViewer : public QDialog, public UEventsHandler
 {
 	Q_OBJECT
 public:
@@ -49,7 +49,7 @@ public:
 public slots:
 	void showImage(const rtabmap::SensorData & data);
 protected:
-	void handleEvent(UEvent * event);
+	virtual void handleEvent(UEvent * event);
 
 private:
 	ImageView* imageView_;
diff --git a/guilib/include/rtabmap/gui/OdometryViewer.h b/guilib/include/rtabmap/gui/OdometryViewer.h
index 5d7a1f04..ffec951b 100644
--- a/guilib/include/rtabmap/gui/OdometryViewer.h
+++ b/guilib/include/rtabmap/gui/OdometryViewer.h
@@ -31,47 +31,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include "rtabmap/core/SensorData.h"
-#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include <QDialog>
 #include "rtabmap/utilite/UEventsHandler.h"
-#include "rtabmap/utilite/UTimer.h"
-#include "rtabmap/utilite/UMutex.h"
+
+class QSpinBox;
+class QDoubleSpinBox;
 
 namespace rtabmap {
 
-class RTABMAPGUI_EXP OdometryViewer : public CloudViewer, public UEventsHandler
+class ImageView;
+class CloudViewer;
+
+class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler
 {
 	Q_OBJECT
 
 public:
 	OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, int qualityWarningThr=0, QWidget * parent = 0);
-	virtual ~OdometryViewer() {}
+	virtual ~OdometryViewer();
 
 public slots:
 	virtual void clear();
 
 protected:
-	void handleAction(QAction * a);
 	virtual void handleEvent(UEvent * event);
 
 private slots:
-	void processData();
+	void processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
 
 private:
-	UMutex dataMutex_;
-	std::list<rtabmap::SensorData> data_;
-	int dataQuality_;
+	ImageView* imageView_;
+	CloudViewer* cloudView_;
+	bool processingData_;
+	bool odomImageShow_;
+	bool odomImageDepthShow_;
+
 	Transform lastOdomPose_;
-	UTimer timer_;
-	int maxClouds_;
-	float voxelSize_;
-	int decimation_;
 	int qualityWarningThr_;
 	int id_;
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
-	QAction * _aSetVoxelSize;
-	QAction * _aSetDecimation;
-	QAction * _aSetCloudHistorySize;
-	QAction * _aPause;
+
+	QSpinBox * maxCloudsSpin_;
+	QDoubleSpinBox * voxelSpin_;
+	QSpinBox * decimationSpin_;
+	int validDecimationValue_;
 };
 
 } /* namespace rtabmap */
diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h
index 0f5bbb16..371f16f2 100644
--- a/guilib/include/rtabmap/gui/PreferencesDialog.h
+++ b/guilib/include/rtabmap/gui/PreferencesDialog.h
@@ -57,8 +57,6 @@ class QDoubleSpinBox;
 
 namespace rtabmap {
 
-class OdometryThread;
-class CameraThread;
 class Signature;
 class LoopClosureViewer;
 class CameraRGBD;
@@ -245,9 +243,7 @@ private slots:
 	void updateBasicParameter();
 	void openDatabaseViewer();
 	void showOpenNI2GroupBox(bool);
-	void cleanOdometryTest();
 	void testOdometry();
-	void cleanRGBDCameraTest();
 	void testRGBDCamera();
 
 protected:
@@ -299,10 +295,6 @@ private:
 
 	QProgressDialog * _progressDialog;
 
-	//Odometry test
-	CameraThread * _cameraThread;
-	OdometryThread * _odomThread;
-
 	//calibration
 	CalibrationDialog * _calibrationDialog;
 
diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp
index c1f3add9..4ca160a2 100644
--- a/guilib/src/CameraViewer.cpp
+++ b/guilib/src/CameraViewer.cpp
@@ -34,12 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/gui/UCv2Qt.h>
 #include <QtCore/QMetaType>
 #include <QHBoxLayout>
+#include <QVBoxLayout>
+#include <QDialogButtonBox>
 
 namespace rtabmap {
 
 
 CameraViewer::CameraViewer(QWidget * parent) :
-		QWidget(parent),
+		QDialog(parent),
 	imageView_(new ImageView(this)),
 	cloudView_(new CloudViewer(this)),
 	processingImages_(false)
@@ -48,13 +50,22 @@ CameraViewer::CameraViewer(QWidget * parent) :
 
 	imageView_->setImageDepthShown(true);
 	imageView_->setMinimumSize(320, 240);
-	QHBoxLayout * layout = new QHBoxLayout(this);
+	QHBoxLayout * layout = new QHBoxLayout();
 	layout->setMargin(0);
-	layout->addWidget(imageView_);
-	layout->addWidget(cloudView_);
-	layout->setStretch(0, 1);
-	layout->setStretch(1, 1);
-	this->setLayout(layout);
+	layout->addWidget(imageView_,1);
+	layout->addWidget(cloudView_,1);
+
+	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
+	buttonBox->setStandardButtons(QDialogButtonBox::Close);
+	connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
+
+	QVBoxLayout * vlayout = new QVBoxLayout(this);
+	vlayout->setMargin(0);
+	vlayout->setSpacing(0);
+	vlayout->addLayout(layout, 1);
+	vlayout->addWidget(buttonBox);
+
+	this->setLayout(vlayout);
 }
 
 CameraViewer::~CameraViewer()
diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp
index 641d5af9..f02e0184 100644
--- a/guilib/src/OdometryViewer.cpp
+++ b/guilib/src/OdometryViewer.cpp
@@ -31,200 +31,320 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UConversion.h"
-#include <pcl/common/transforms.h>
-#include <pcl/io/pcd_io.h>
-#include <QInputDialog>
-#include <QAction>
-#include <QMenu>
-#include <QtGui/QKeyEvent>
+#include "rtabmap/gui/UCv2Qt.h"
+
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/gui/CloudViewer.h"
+
+#include <QPushButton>
+#include <QSpinBox>
+#include <QDoubleSpinBox>
+#include <QLabel>
+#include <QHBoxLayout>
+#include <QVBoxLayout>
 
 namespace rtabmap {
 
-
 OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) :
-		CloudViewer(parent),
-		dataQuality_(-1),
+		QDialog(parent),
+		imageView_(new ImageView(this)),
+		cloudView_(new CloudViewer(this)),
+		processingData_(false),
+		odomImageShow_(true),
+		odomImageDepthShow_(true),
 		lastOdomPose_(Transform::getIdentity()),
-		maxClouds_(maxClouds),
-		voxelSize_(voxelSize),
-		decimation_(decimation),
 		qualityWarningThr_(qualityWarningThr),
 		id_(0),
-		_aSetVoxelSize(0),
-		_aSetDecimation(0),
-		_aSetCloudHistorySize(0),
-		_aPause(0)
+		validDecimationValue_(1)
 {
 
-	//add actions to CloudViewer menu
-	_aSetVoxelSize = new QAction("Set voxel size...", this);
-	_aSetDecimation = new QAction("Set depth image decimation...", this);
-	_aSetCloudHistorySize = new QAction("Set cloud history size...", this);
-	_aPause = new QAction("Pause", this);
-	_aPause->setCheckable(true);
-	menu()->addAction(_aSetVoxelSize);
-	menu()->addAction(_aSetDecimation);
-	menu()->addAction(_aSetCloudHistorySize);
-	menu()->addAction(_aPause);
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
+
+	imageView_->setImageDepthShown(true);
+	imageView_->setMinimumSize(320, 240);
+
+	cloudView_->setCameraFree();
+	cloudView_->setGridShown(true);
+
+	QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
+	QLabel * voxelLabel = new QLabel("Voxel", this);
+	QLabel * decimationLabel = new QLabel("Decimation", this);
+	maxCloudsSpin_ = new QSpinBox(this);
+	maxCloudsSpin_->setMinimum(0);
+	maxCloudsSpin_->setMaximum(100);
+	maxCloudsSpin_->setValue(maxClouds);
+	voxelSpin_ = new QDoubleSpinBox(this);
+	voxelSpin_->setMinimum(0);
+	voxelSpin_->setMaximum(1);
+	voxelSpin_->setDecimals(3);
+	voxelSpin_->setSingleStep(0.01);
+	voxelSpin_->setSuffix(" m");
+	voxelSpin_->setValue(voxelSize);
+	decimationSpin_ = new QSpinBox(this);
+	decimationSpin_->setMinimum(1);
+	decimationSpin_->setMaximum(16);
+	decimationSpin_->setValue(decimation);
+	QPushButton * clearButton = new QPushButton("clear", this);
+	QPushButton * closeButton = new QPushButton("close", this);
+	connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
+	connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
+
+	//layout
+	QHBoxLayout * layout = new QHBoxLayout();
+	layout->setMargin(0);
+	layout->setSpacing(0);
+	layout->addWidget(imageView_,1);
+	layout->addWidget(cloudView_,1);
+
+	QHBoxLayout * hlayout2 = new QHBoxLayout();
+	hlayout2->setMargin(0);
+	hlayout2->addWidget(maxCloudsLabel);
+	hlayout2->addWidget(maxCloudsSpin_);
+	hlayout2->addWidget(voxelLabel);
+	hlayout2->addWidget(voxelSpin_);
+	hlayout2->addWidget(decimationLabel);
+	hlayout2->addWidget(decimationSpin_);
+	hlayout2->addStretch(1);
+	hlayout2->addWidget(clearButton);
+	hlayout2->addWidget(closeButton);
+
+	QVBoxLayout * vlayout = new QVBoxLayout(this);
+	vlayout->setMargin(0);
+	vlayout->setSpacing(0);
+	vlayout->addLayout(layout, 1);
+	vlayout->addLayout(hlayout2);
+
+	this->setLayout(vlayout);
+}
+
+OdometryViewer::~OdometryViewer()
+{
+	this->unregisterFromEventsManager();
+	this->clear();
 }
 
 void OdometryViewer::clear()
 {
-	dataMutex_.lock();
-	data_.clear();
-	dataMutex_.unlock();
-	clouds_.clear();
-	CloudViewer::clear();
+	cloudView_->clear();
 }
 
-void OdometryViewer::processData()
+void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
 {
-	rtabmap::SensorData data;
-	int quality = -1;
-	dataMutex_.lock();
-	if(data_.size())
-	{
-		data = data_.back();
-		data_.clear();
-		quality = dataQuality_;
-		dataQuality_ = -1;
-	}
-	dataMutex_.unlock();
+	processingData_ = true;
+	int quality = info.inliers;
 
-	if(!data.image().empty() && !data.depth().empty() && data.fx()>0.0f && data.fy()>0.0f && this->isVisible())
+	bool lost = false;
+	bool lostStateChanged = false;
+
+	if(data.pose().isNull())
+	{
+		UDEBUG("odom lost"); // use last pose
+		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkRed);
+		cloudView_->setBackgroundColor(Qt::darkRed);
+
+		lost = true;
+	}
+	else if(info.inliers>0 &&
+			qualityWarningThr_ &&
+			info.inliers < qualityWarningThr_)
+	{
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, qualityWarningThr_);
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkYellow);
+		cloudView_->setBackgroundColor(Qt::darkYellow);
+	}
+	else
+	{
+		UDEBUG("odom ok");
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
+		cloudView_->setBackgroundColor(Qt::black);
+	}
+
+
+	if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f)
 	{
 		UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
 
+		if(data.image().cols % decimationSpin_->value() == 0 &&
+		   data.image().rows % decimationSpin_->value() == 0)
+		{
+			validDecimationValue_ = decimationSpin_->value();
+		}
+		else
+		{
+			UWARN("Decimation (%d) must be a denominator of the width and height of "
+					"the image (%d/%d). Using last valid decimation value (%d).",
+					decimationSpin_->value(),
+					data.image().cols,
+					data.image().rows,
+					validDecimationValue_);
+		}
+
+
 		// visualization: buffering the clouds
 		// Create the new cloud
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		if(data.depth().type() == CV_8UC1)
-		{
-			cloud = util3d::cloudFromStereoImages(
-					data.image(),
-					data.depth(),
-					data.cx(), data.cy(),
-					data.fx(), data.fy(),
-					decimation_);
-		}
-		else
+		if(!data.depth().empty())
 		{
 			cloud = util3d::cloudFromDepthRGB(
 					data.image(),
 					data.depth(),
 					data.cx(), data.cy(),
 					data.fx(), data.fy(),
-					decimation_);
+					validDecimationValue_);
 		}
-
-		if(voxelSize_ > 0.0f)
+		else if(!data.rightImage().empty())
 		{
-			cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSize_);
+			cloud = util3d::cloudFromStereoImages(
+					data.image(),
+					data.rightImage(),
+					data.cx(), data.cy(),
+					data.fx(), data.baseline(),
+					validDecimationValue_);
 		}
 
-		cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
+		if(voxelSpin_->value() > 0.0f && cloud->size())
+		{
+			cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSpin_->value());
+		}
+
+		if(cloud->size())
+		{
+			cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
+		}
 
 		if(!data.pose().isNull())
 		{
 			lastOdomPose_ = data.pose();
-			if(this->getAddedClouds().contains("cloudtmp"))
+			if(cloudView_->getAddedClouds().contains("cloudtmp"))
 			{
-				this->removeCloud("cloudtmp");
+				cloudView_->removeCloud("cloudtmp");
 			}
 
 			data.id()?id_=data.id():++id_;
 
-			clouds_.insert(std::make_pair(id_, cloud));
-
-			while(maxClouds_>0 && (int)clouds_.size() > maxClouds_)
+			while(maxCloudsSpin_->value()>0 && (int)cloudView_->getAddedClouds().size() > maxCloudsSpin_->value())
 			{
-				this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
-				clouds_.erase(clouds_.begin());
+				cloudView_->removeCloud(cloudView_->getAddedClouds().begin().key());
 			}
 
-			if(clouds_.size())
-			{
-				this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
-			}
+			cloudView_->addOrUpdateCloud(uFormat("cloud%d", id_), cloud, data.pose());
 
-			this->updateCameraTargetPosition(data.pose());
-
-			if(qualityWarningThr_ && quality>=0 && quality < qualityWarningThr_)
-			{
-				this->setBackgroundColor(Qt::darkYellow);
-			}
-			else
-			{
-				this->setBackgroundColor(this->getDefaultBackgroundColor());
-			}
+			cloudView_->updateCameraTargetPosition(data.pose());
 		}
 		else
 		{
-			this->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
-			this->setBackgroundColor(Qt::darkRed);
+			cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
+		}
+	}
+
+	if(!data.image().empty())
+	{
+		if(info.type == 0)
+		{
+			imageView_->setFeatures(info.words, Qt::yellow);
+		}
+		else if(info.type == 1)
+		{
+			imageView_->setFeatures(info.refCorners, Qt::red);
 		}
 
-		this->update();
+		imageView_->clearLines();
+		if(lost)
+		{
+			if(lostStateChanged)
+			{
+				// save state
+				odomImageShow_ = imageView_->isImageShown();
+				odomImageDepthShow_ = imageView_->isImageDepthShown();
+			}
+			imageView_->setImageDepth(uCvMat2QImage(data.image()));
+			imageView_->setImageShown(true);
+			imageView_->setImageDepthShown(true);
+		}
+		else
+		{
+			if(lostStateChanged)
+			{
+				// restore state
+				imageView_->setImageShown(odomImageShow_);
+				imageView_->setImageDepthShown(odomImageDepthShow_);
+			}
+
+			imageView_->setImage(uCvMat2QImage(data.image()));
+			if(imageView_->isImageDepthShown())
+			{
+				imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+			}
+
+			if(info.type == 0)
+			{
+				if(imageView_->isFeaturesShown())
+				{
+					for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+					{
+						imageView_->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
+					}
+					for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+					{
+						imageView_->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
+					}
+				}
+			}
+			else if(info.type == 1)
+			{
+				if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
+				{
+					//draw lines
+					UASSERT(info.refCorners.size() == info.newCorners.size());
+					for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
+					{
+						if(imageView_->isFeaturesShown())
+						{
+							imageView_->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
+						}
+						if(imageView_->isLinesShown())
+						{
+							imageView_->addLine(
+									info.refCorners[info.cornerInliers[i]].pt.x,
+									info.refCorners[info.cornerInliers[i]].pt.y,
+									info.newCorners[info.cornerInliers[i]].pt.x,
+									info.newCorners[info.cornerInliers[i]].pt.y,
+									Qt::blue);
+						}
+					}
+					imageView_->update();
+				}
+			}
+
+		}
+		if(!data.image().empty())
+		{
+			imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
+		}
 	}
+
+	cloudView_->update();
+	processingData_ = false;
 }
 
 void OdometryViewer::handleEvent(UEvent * event)
 {
-	if(!_aPause->isChecked())
+	if(!processingData_ && this->isVisible())
 	{
 		if(event->getClassName().compare("OdometryEvent") == 0)
 		{
 			rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
-
-			bool empty = false;
-			dataMutex_.lock();
-			if(data_.empty())
+			if(odomEvent->data().isValid())
 			{
-				data_.push_back(odomEvent->data());
-				empty= true;
+				processingData_ = true;
+				QMetaObject::invokeMethod(this, "processData",
+						Q_ARG(rtabmap::SensorData, odomEvent->data()),
+						Q_ARG(rtabmap::OdometryInfo, odomEvent->info()));
 			}
-			else
-			{
-				data_.back() = odomEvent->data();
-			}
-			dataQuality_ = odomEvent->info().inliers;
-			dataMutex_.unlock();
-			if(empty)
-			{
-				QMetaObject::invokeMethod(this, "processData");
-			}
-		}
-	}
-}
-
-void OdometryViewer::handleAction(QAction * a)
-{
-	CloudViewer::handleAction(a);
-	if(a == _aSetVoxelSize)
-	{
-		bool ok;
-		double value = QInputDialog::getDouble(this, tr("Set voxel size"), tr("Size (0=disabled)"), voxelSize_, 0.0, 0.1, 2, &ok);
-		if(ok)
-		{
-			voxelSize_ = value;
-		}
-	}
-	else if(a == _aSetCloudHistorySize)
-	{
-		bool ok;
-		int value = QInputDialog::getInt(this, tr("Set cloud history size"), tr("Size (0=infinite)"), maxClouds_, 0, 100, 1, &ok);
-		if(ok)
-		{
-			maxClouds_ = value;
-		}
-	}
-	else if(a == _aSetDecimation)
-	{
-		bool ok;
-		int value = QInputDialog::getInt(this, tr("Set depth image decimation"), tr("Decimation"), decimation_, 1, 8, 1, &ok);
-		if(ok)
-		{
-			decimation_ = value;
 		}
 	}
 }
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index 6543609d..6217b245 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -81,11 +81,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui(0),
 	_indexModel(0),
 	_initialized(false),
-	_cameraThread(0),
-	_odomThread(0),
 	_calibrationDialog(new CalibrationDialog(false, ".", this))
 {
 	ULOGGER_DEBUG("");
+	_calibrationDialog->setWindowFlags(Qt::Window);
 
 	_ui = new Ui_preferencesDialog();
 	_ui->setupUi(this);
@@ -3254,25 +3253,18 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testOdometry(int type)
 {
-	if(_cameraThread)
-	{
-		QMessageBox::warning(this,
-			   tr("RTAB-Map"),
-			   tr("A camera is already running!"));
-		return;
-	}
-
-	UASSERT(_odomThread == 0 && _cameraThread == 0);
-
 	CameraRGBD * camera = this->createCameraRGBD();
 
-	if(!camera->init(this->getCameraInfoDir().toStdString()))
+	if(camera == 0 || !camera->init(this->getCameraInfoDir().toStdString()))
 	{
 		QMessageBox::warning(this,
 			   tr("RTAB-Map"),
 			   tr("RGBD camera initialization failed!"));
-		delete camera;
-		camera = 0;
+		if(camera)
+		{
+			delete camera;
+		}
+		return;
 	}
 	else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 	{
@@ -3287,110 +3279,56 @@ void PreferencesDialog::testOdometry(int type)
 	}
 	camera->setMirroringEnabled(isSourceMirroring());
 
-
-	if(camera)
+	ParametersMap parameters = this->getAllParameters();
+	Odometry * odometry;
+	if(this->getOdomStrategy() == 1)
 	{
-		ParametersMap parameters = this->getAllParameters();
-		Odometry * odometry;
-		if(this->getOdomStrategy() == 1)
-		{
-			odometry = new OdometryOpticalFlow(parameters);
-		}
-		else
-		{
-			odometry = new OdometryBOW(parameters);
-		}
-
-		_odomThread = new OdometryThread(odometry); // take ownership of odometry
-
-		QDialog * window = new QDialog(this);
-		window->setWindowModality(Qt::WindowModal);
-		window->setWindowTitle(tr("Odometry viewer"));
-		window->setMinimumWidth(800);
-		window->setMinimumHeight(600);
-		connect( window, SIGNAL(finished(int)), this, SLOT(cleanOdometryTest()) );
-
-		OdometryViewer * odomViewer = new OdometryViewer(10,
-				_ui->spinBox_decimation_odom->value(),
-				_ui->doubleSpinBox_voxelSize_odom->value(),
-				this->getOdomQualityWarnThr(),
-				window);
-		connect( window, SIGNAL(finished(int)), odomViewer, SLOT(clear()) );
-
-		odomViewer->setCameraFree();
-		odomViewer->setGridShown(true);
-
-		QVBoxLayout *layout = new QVBoxLayout();
-		layout->addWidget(odomViewer);
-		window->setLayout(layout);
-
-		UEventsManager::addHandler(_odomThread);
-		UEventsManager::addHandler(odomViewer);
-
-		_cameraThread = new CameraThread(camera);
-		UEventsManager::createPipe(_cameraThread, _odomThread, "CameraEvent");
-		UEventsManager::createPipe(_odomThread, odomViewer, "OdometryEvent");
-
-		window->showNormal();
-
-		_ui->pushButton_testOdometry->setEnabled(false);
-
-		QApplication::processEvents();
-		uSleep(500);
-		QApplication::processEvents();
-
-		_odomThread->start();
-		_cameraThread->start();
+		odometry = new OdometryOpticalFlow(parameters);
 	}
-}
-
-void PreferencesDialog::cleanOdometryTest()
-{
-	UDEBUG("");
-	if(_cameraThread)
+	else
 	{
-		_cameraThread->join(true);
-		delete _cameraThread;
-		_cameraThread = 0;
+		odometry = new OdometryBOW(parameters);
 	}
-	if(_odomThread)
-	{
-		_odomThread->join(true);
-		delete _odomThread;
-		_odomThread = 0;
-	}
-	_ui->pushButton_testOdometry->setEnabled(true);
-}
 
-void PreferencesDialog::cleanRGBDCameraTest()
-{
-	UDEBUG("");
-	if(_cameraThread)
-	{
-		_cameraThread->join(true);
-		delete _cameraThread;
-		_cameraThread = 0;
-	}
-	_ui->pushButton_test_rgbd_camera->setEnabled(true);
+	OdometryThread odomThread(odometry); // take ownership of odometry
+	odomThread.registerToEventsManager();
+
+	OdometryViewer * odomViewer = new OdometryViewer(10,
+					_ui->spinBox_decimation_odom->value(),
+					_ui->doubleSpinBox_voxelSize_odom->value(),
+					this->getOdomQualityWarnThr(),
+					this);
+	odomViewer->setWindowTitle(tr("Odometry viewer"));
+	odomViewer->resize(1280, 480+QPushButton().minimumHeight());
+	odomViewer->registerToEventsManager();
+
+	CameraThread cameraThread(camera); // take ownership of camera
+	UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
+	UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
+
+	odomThread.start();
+	cameraThread.start();
+
+	odomViewer->exec();
+	delete odomViewer;
+
+	cameraThread.join(true);
+	odomThread.join(true);
 }
 
 void PreferencesDialog::testRGBDCamera()
 {
-	if(_cameraThread)
-	{
-		QMessageBox::warning(this,
-			   tr("RTAB-Map"),
-			   tr("A camera is already running!"));
-	}
-
 	CameraRGBD * camera = this->createCameraRGBD();
-	if(!camera->init(this->getCameraInfoDir().toStdString()))
+	if(camera == 0 || !camera->init(this->getCameraInfoDir().toStdString()))
 	{
 		QMessageBox::warning(this,
 			   tr("RTAB-Map"),
 			   tr("RGBD camera initialization failed!"));
-		delete camera;
-		camera = 0;
+		if(camera)
+		{
+			delete camera;
+		}
+		return;
 	}
 	else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 	{
@@ -3405,41 +3343,34 @@ void PreferencesDialog::testRGBDCamera()
 	}
 	camera->setMirroringEnabled(isSourceMirroring());
 
+	// Create DataRecorder without init it, just to show images...
+	CameraViewer * window = new CameraViewer(this);
+	window->setWindowTitle(tr("RGBD camera viewer"));
+	window->resize(1280, 480+QPushButton().minimumHeight());
+	window->registerToEventsManager();
 
-	if(camera)
-	{
-		_ui->pushButton_test_rgbd_camera->setEnabled(false);
+	CameraThread cameraThread(camera);
+	UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
 
-		// Create DataRecorder without init it, just to show images...
-		CameraViewer * window = new CameraViewer(this);
-		window->setWindowModality(Qt::WindowModal);
-		window->setAttribute(Qt::WA_DeleteOnClose);
-		window->setWindowFlags(Qt::Dialog);
-		window->setWindowTitle(tr("RGBD camera viewer"));
-		window->setMinimumWidth(1280);
-		window->setMinimumHeight(480);
-		connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanRGBDCameraTest()) );
-		window->registerToEventsManager();
-
-		_cameraThread = new CameraThread(camera);
-		UEventsManager::createPipe(_cameraThread, window, "CameraEvent");
-
-		window->showNormal();
-
-		_cameraThread->start();
-	}
+	cameraThread.start();
+	window->exec();
+	delete window;
+	cameraThread.join(true);
 }
 
 void PreferencesDialog::calibrate()
 {
 	CameraRGBD * camera = this->createCameraRGBD();
-	if(!camera->init("")) // don't set calibration folder to use raw images
+	if(camera == 0 || !camera->init("")) // don't set calibration folder to use raw images
 	{
 		QMessageBox::warning(this,
 			   tr("RTAB-Map"),
 			   tr("RGBD camera initialization failed!"));
-		delete camera;
-		camera = 0;
+		if(camera)
+		{
+			delete camera;
+		}
+		return;
 	}
 	else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 	{
@@ -3455,34 +3386,31 @@ void PreferencesDialog::calibrate()
 	camera->setMirroringEnabled(isSourceMirroring());
 
 
-	if(camera)
+	if(!this->getCameraInfoDir().isEmpty())
 	{
-		if(!this->getCameraInfoDir().isEmpty())
+		QDir dir(this->getCameraInfoDir());
+		if (!dir.exists())
 		{
-			QDir dir(this->getCameraInfoDir());
-			if (!dir.exists())
+			UINFO("Creating camera_info directory: \"%s\"", this->getCameraInfoDir().toStdString().c_str());
+			if(!dir.mkpath(this->getCameraInfoDir()))
 			{
-				UINFO("Creating camera_info directory: \"%s\"", this->getCameraInfoDir().toStdString().c_str());
-				if(!dir.mkpath(this->getCameraInfoDir()))
-				{
-					UWARN("Could create camera_info directory: \"%s\"", this->getCameraInfoDir().toStdString().c_str());
-				}
+				UWARN("Could create camera_info directory: \"%s\"", this->getCameraInfoDir().toStdString().c_str());
 			}
 		}
-		_calibrationDialog->setStereoMode(dynamic_cast<CameraStereoDC1394*>(camera)!=0);
-		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
-		_calibrationDialog->registerToEventsManager();
-
-		CameraThread cameraThread(camera);
-		UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent");
-
-		cameraThread.start();
-
-		_calibrationDialog->exec();
-		_calibrationDialog->unregisterFromEventsManager();
-
-		cameraThread.join(true);
 	}
+	_calibrationDialog->setStereoMode(dynamic_cast<CameraStereoDC1394*>(camera)!=0);
+	_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
+	_calibrationDialog->registerToEventsManager();
+
+	CameraThread cameraThread(camera);
+	UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent");
+
+	cameraThread.start();
+
+	_calibrationDialog->exec();
+	_calibrationDialog->unregisterFromEventsManager();
+
+	cameraThread.join(true);
 }
 
 }
diff --git a/guilib/src/ui/postProcessingDialog.ui b/guilib/src/ui/postProcessingDialog.ui
index 7b212418..b67a731c 100644
--- a/guilib/src/ui/postProcessingDialog.ui
+++ b/guilib/src/ui/postProcessingDialog.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>486</width>
-    <height>369</height>
+    <height>425</height>
    </rect>
   </property>
   <property name="windowTitle">
diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp
index d3e7c8ab..de2596d0 100644
--- a/tools/CameraRGBD/main.cpp
+++ b/tools/CameraRGBD/main.cpp
@@ -59,6 +59,10 @@ int main(int argc, char * argv[])
 	}
 	else
 	{
+		if(strcmp(argv[argc-1], "--help") == 0)
+		{
+			showUsage();
+		}
 		driver = atoi(argv[argc-1]);
 		if(driver < 0 || driver > 6)
 		{
@@ -134,7 +138,7 @@ int main(int argc, char * argv[])
 
 	if(!camera->init())
 	{
-		printf("Camera init failed!\n");
+		printf("Camera init failed! Please select another driver (see \"--help\").\n");
 		delete camera;
 		exit(1);
 	}
diff --git a/tools/OdometryViewer/CMakeLists.txt b/tools/OdometryViewer/CMakeLists.txt
index 1683fed5..dfa5f316 100644
--- a/tools/OdometryViewer/CMakeLists.txt
+++ b/tools/OdometryViewer/CMakeLists.txt
@@ -16,14 +16,8 @@ SET(LIBRARIES
 	${QT_LIBRARIES} 
 )
 
-IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
-    QT4_WRAP_CPP(moc_srcs OdomInfoWidget.h)
-ELSE()
-    QT5_WRAP_CPP(moc_srcs OdomInfoWidget.h)
-ENDIF()
-
 SET(SRC_FILES
-    main.cpp OdomInfoWidget.cpp ${moc_srcs}
+    main.cpp 
 )
 
 add_definitions(${PCL_DEFINITIONS})
diff --git a/tools/OdometryViewer/OdomInfoWidget.cpp b/tools/OdometryViewer/OdomInfoWidget.cpp
deleted file mode 100644
index 6e5d0873..00000000
--- a/tools/OdometryViewer/OdomInfoWidget.cpp
+++ /dev/null
@@ -1,208 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "OdomInfoWidget.h"
-
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/core/OdometryEvent.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/gui/ImageView.h>
-#include <rtabmap/gui/UCv2Qt.h>
-#include <QtCore/QMetaType>
-#include <QtGui/QCloseEvent>
-#include <QLabel>
-#include <QVBoxLayout>
-
-OdomInfoWidget::OdomInfoWidget(QWidget * parent) :
-		QWidget(parent),
-	imageView_(new rtabmap::ImageView(this)),
-	label_(new QLabel(this)),
-	processingOdomInfo_(false),
-	receivingRate_(0),
-	lastTime_(0),
-	odomImageShow_(true),
-	odomImageDepthShow_(false)
-{
-	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
-	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
-
-	imageView_->setMinimumSize(320, 240);
-	QVBoxLayout * layout = new QVBoxLayout(this);
-	layout->setMargin(0);
-	layout->addWidget(imageView_);
-	layout->addWidget(label_);
-	layout->setStretch(0,1);
-	this->setLayout(layout);
-}
-
-
-OdomInfoWidget::~OdomInfoWidget()
-{
-	this->unregisterFromEventsManager();
-}
-
-
-void OdomInfoWidget::processOdomInfo(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
-{
-	processingOdomInfo_ = true;
-
-	const rtabmap::Transform & pose = data.pose();
-	bool lost = false;
-	bool lostStateChanged = false;
-
-	if(pose.isNull())
-	{
-		// lost
-		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
-		imageView_->setBackgroundColor(Qt::darkRed);
-		lost = true;
-	}
-	else
-	{
-		// ok
-		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
-		imageView_->setBackgroundColor(Qt::black);
-	}
-
-	if(!data.image().empty())
-	{
-		if(imageView_->isFeaturesShown())
-		{
-			if(info.type == 0)
-			{
-				imageView_->setFeatures(info.words, Qt::yellow);
-			}
-			else if(info.type == 1)
-			{
-				imageView_->setFeatures(info.refCorners, Qt::red);
-			}
-		}
-
-		imageView_->clearLines();
-		if(lost)
-		{
-			if(lostStateChanged)
-			{
-				// save state
-				odomImageShow_ = imageView_->isImageShown();
-				odomImageDepthShow_ = imageView_->isImageDepthShown();
-			}
-			imageView_->setImageDepth(uCvMat2QImage(data.image()));
-			imageView_->setImageShown(true);
-			imageView_->setImageDepthShown(true);
-		}
-		else
-		{
-			if(lostStateChanged)
-			{
-				// restore state
-				imageView_->setImageShown(odomImageShow_);
-				imageView_->setImageDepthShown(odomImageDepthShow_);
-			}
-
-			imageView_->setImage(uCvMat2QImage(data.image()));
-			if(imageView_->isImageDepthShown())
-			{
-				imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
-			}
-
-			if(info.type == 0)
-			{
-				if(imageView_->isFeaturesShown())
-				{
-					for(unsigned int i=0; i<info.wordMatches.size(); ++i)
-					{
-						imageView_->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
-					}
-					for(unsigned int i=0; i<info.wordInliers.size(); ++i)
-					{
-						imageView_->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
-					}
-				}
-			}
-			else if(info.type == 1)
-			{
-				if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
-				{
-					//draw lines
-					UASSERT(info.refCorners.size() == info.newCorners.size());
-					for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
-					{
-						if(imageView_->isFeaturesShown())
-						{
-							imageView_->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
-						}
-						if(imageView_->isLinesShown())
-						{
-							imageView_->addLine(
-									info.refCorners[info.cornerInliers[i]].pt.x,
-									info.refCorners[info.cornerInliers[i]].pt.y,
-									info.newCorners[info.cornerInliers[i]].pt.x,
-									info.newCorners[info.cornerInliers[i]].pt.y,
-									Qt::blue);
-						}
-					}
-					imageView_->update();
-				}
-			}
-
-		}
-		if(!data.image().empty())
-		{
-			imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
-		}
-	}
-	label_->setText(tr("Rate=~%1 Hz").arg(receivingRate_));
-	processingOdomInfo_ = false;
-}
-
-void OdomInfoWidget::closeEvent(QCloseEvent* event)
-{
-	this->unregisterFromEventsManager();
-	event->accept();
-}
-
-void OdomInfoWidget::handleEvent(UEvent * event)
-{
-	if(event->getClassName().compare("OdometryEvent") == 0)
-	{
-		rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
-		receivingRate_ = 1.0f/timer_.ticks();
-
-		// update max 10 Hz
-		if(UTimer::now() - lastTime_ > 0.1)
-		{
-			lastTime_ = UTimer::now();
-			if(!processingOdomInfo_ && this->isVisible())
-			{
-				QMetaObject::invokeMethod(this, "processOdomInfo",
-						Q_ARG(rtabmap::SensorData, odomEvent->data()),
-						Q_ARG(rtabmap::OdometryInfo, odomEvent->info()));
-			}
-		}
-	}
-}
diff --git a/tools/OdometryViewer/OdomInfoWidget.h b/tools/OdometryViewer/OdomInfoWidget.h
deleted file mode 100644
index c30849fd..00000000
--- a/tools/OdometryViewer/OdomInfoWidget.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef ODOMINFOWIDGET_H_
-#define ODOMINFOWIDGET_H_
-
-#include <rtabmap/utilite/UEventsHandler.h>
-#include <QWidget>
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/core/SensorData.h>
-#include <rtabmap/core/OdometryInfo.h>
-
-class QLabel;
-
-namespace rtabmap{
-class ImageView;
-}
-
-class OdomInfoWidget : public QWidget, public UEventsHandler
-{
-	Q_OBJECT
-public:
-	OdomInfoWidget(QWidget * parent = 0);
-	virtual ~OdomInfoWidget();
-public slots:
-	void processOdomInfo(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
-protected:
-	virtual void closeEvent(QCloseEvent* event);
-	void handleEvent(UEvent * event);
-
-private:
-	rtabmap::ImageView* imageView_;
-	QLabel* label_;
-	UTimer timer_;
-	bool processingOdomInfo_;
-	double receivingRate_;
-	double lastTime_;
-	bool odomImageShow_;
-	bool odomImageDepthShow_;
-};
-
-#endif /* ODOMINFOWIDGET_H_ */
diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp
index ec14632c..31144c26 100644
--- a/tools/OdometryViewer/main.cpp
+++ b/tools/OdometryViewer/main.cpp
@@ -36,15 +36,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/DBReader.h>
 #include <rtabmap/core/VWDictionary.h>
 #include <QApplication>
+#include <QPushButton>
 #include <pcl/console/print.h>
-#include "OdomInfoWidget.h"
 
 void showUsage()
 {
 	printf("\nUsage:\n"
 			"odometryViewer [options]\n"
 			"Options:\n"
-			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n"
+			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394\n"
 			"  -o #                      Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
 			"  -nn #                     Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
 			"  -nndr #                   Nearest neighbor distance ratio (default 0.7)\n"
@@ -90,6 +90,8 @@ int main (int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setPrintTime(false);
+	ULogger::setPrintWhere(false);
 
 	// parse arguments
 	float rate = 0.0;
@@ -126,7 +128,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				driver = std::atoi(argv[i]);
-				if(driver < 0 || driver > 4)
+				if(driver < 0 || driver > 6)
 				{
 					showUsage();
 				}
@@ -501,6 +503,8 @@ int main (int argc, char * argv[])
 		if(strcmp(argv[i], "-debug") == 0)
 		{
 			ULogger::setLevel(ULogger::kDebug);
+			ULogger::setPrintTime(true);
+			ULogger::setPrintWhere(true);
 			continue;
 		}
 
@@ -680,23 +684,11 @@ int main (int argc, char * argv[])
 	}
 	rtabmap::OdometryThread odomThread(odom);
 	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
-	OdomInfoWidget odomInfoWidget;
 	UEventsManager::addHandler(&odomThread);
 	UEventsManager::addHandler(&odomViewer);
-	UEventsManager::addHandler(&odomInfoWidget);
 
-	odomViewer.setCameraFree();
-	odomViewer.setGridShown(true);
-
-	odomViewer.setWindowTitle("Odometry 3D view");
-	odomViewer.setMinimumWidth(800);
-	odomViewer.setMinimumHeight(500);
-	odomViewer.showNormal();
-
-	odomInfoWidget.setWindowTitle("Odometry info");
-	odomInfoWidget.showNormal();
-
-	app.processEvents();
+	odomViewer.setWindowTitle("Odometry view");
+	odomViewer.resize(1280, 480+QPushButton().minimumHeight());
 
 	if(inputDatabase.size())
 	{
@@ -756,6 +748,24 @@ int main (int argc, char * argv[])
 			}
 			camera = new rtabmap::CameraOpenNICV(true, rate, t);
 		}
+		else if(driver == 5)
+		{
+			if(!rtabmap::CameraFreenect2::available())
+			{
+				UERROR("Not built with Freenect2 support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraFreenect2(0, rate, t);
+		}
+		else if(driver == 6)
+		{
+			if(!rtabmap::CameraStereoDC1394::available())
+			{
+				UERROR("Not built with dc1394 support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraStereoDC1394(rate, t);
+		}
 		else
 		{
 			UFATAL("Camera driver (%d) not found!", driver);
@@ -763,16 +773,28 @@ int main (int argc, char * argv[])
 
 		//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
 
-		rtabmap::CameraThread cameraThread(camera);
-		if(cameraThread.init())
+		if(camera->init())
 		{
-			odomThread.start();
-			cameraThread.start();
+			if(camera->isCalibrated())
+			{
+				rtabmap::CameraThread cameraThread(camera);
 
-			app.exec();
+				odomThread.start();
+				cameraThread.start();
 
-			cameraThread.kill();
-			odomThread.join(true);
+				odomViewer.exec();
+
+				cameraThread.kill();
+				odomThread.join(true);
+			}
+			else
+			{
+				printf("The camera is not calibrated! You should calibrate the camera first.\n");
+			}
+		}
+		else
+		{
+			printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
 		}
 	}