GUI: Added new Freenect2 and StereoDC1394 source options.

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include <QDialog>
+#include <QSettings>
 #include <opencv2/opencv.hpp>
 
 #include <rtabmap/core/CameraModel.h>
@@ -46,16 +47,18 @@ class RTABMAPGUI_EXP CalibrationDialog  : public QDialog, public UEventsHandler
 	Q_OBJECT;
 
 public:
-	CalibrationDialog(bool stereo = false, QWidget * parent = 0);
+	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", QWidget * parent = 0);
 	virtual ~CalibrationDialog();
 
 	bool isCalibrated() const {return calibrated_;}
-	const cv::Mat & cameraMatrix() const {return model_.K();} // Matrix K
-	const cv::Mat & distCoeffs() const {return model_.D();} // Matrix D
-	float fx() const {return model_.fx();} // K(0)
-	float fy() const {return model_.fy();} // K(4)
-	float cx() const {return model_.cx();} // K(2)
-	float cy() const {return model_.cy();} // K(5)
+	const rtabmap::CameraModel & getCameraModel() const {return model_;}
+	const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
+
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	void setStereoMode(bool stereo);
+	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 
 public slots:
 	void setBoardWidth(int width);
@@ -66,7 +69,7 @@ private slots:
 	void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
 	void restart();
 	void calibrate();
-	void save();
+	bool save();
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);
@@ -85,13 +88,13 @@ private:
 
 private:
 	// parameters
-	cv::Size boardSize_; // innner squares
-	float squareSize_; // m
 	bool stereo_;
+	QString savingDirectory_;
 
 	QString cameraName_;
 	bool calibrated_;
 	bool processingData_;
+	bool savedCalibration_;
 
 	std::vector<std::vector<std::vector<cv::Point2f> > > imagePoints_;
 	std::vector<std::vector<std::vector<float> > > imageParams_;

guilib/include/rtabmap/gui/CameraViewer.h

@@ -0,0 +1,61 @@
+/*
+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 CAMERAVIEWER_H_
+#define CAMERAVIEWER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QWidget>
+#include <rtabmap/core/SensorData.h>
+
+namespace rtabmap {
+
+class ImageView;
+class CloudViewer;
+
+class RTABMAPGUI_EXP CameraViewer : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	CameraViewer(QWidget * parent = 0);
+	virtual ~CameraViewer();
+
+public slots:
+	void showImage(const rtabmap::SensorData & data);
+protected:
+	void handleEvent(UEvent * event);
+
+private:
+	ImageView* imageView_;
+	CloudViewer* cloudView_;
+	bool processingImages_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAVIEWER_H_ */

guilib/include/rtabmap/gui/MainWindow.h

@@ -148,6 +148,8 @@ private slots:
 	void selectOpenniCv();
 	void selectOpenniCvAsus();
 	void selectOpenni2();
+	void selectFreenect2();
+	void selectStereoDC1394();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void downloadAllClouds();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -61,6 +61,8 @@ class OdometryThread;
 class CameraThread;
 class Signature;
 class LoopClosureViewer;
+class CameraRGBD;
+class CalibrationDialog;
 
 class RTABMAPGUI_EXP PreferencesDialog : public QDialog
 {
@@ -87,7 +89,9 @@ public:
 		kSrcFreenect,
 		kSrcOpenNI_CV,
 		kSrcOpenNI_CV_ASUS,
-		kSrcOpenNI2
+		kSrcOpenNI2,
+		kSrcFreenect2,
+		kSrcStereoDC1394
 	};
 
 public:
@@ -159,7 +163,7 @@ public:
 	bool isSourceMirroring() const;
 	bool isSourceImageUsed() const;
 	bool isSourceDatabaseUsed() const;
-	bool isSourceOpenniUsed() const;
+	bool isSourceRGBDUsed() const;
 	PreferencesDialog::Src getSourceImageType() const;
 	QString getSourceImageTypeStr() const;
 	int getSourceWidth() const;
@@ -182,10 +186,7 @@ public:
 	bool getSourceOpenni2Mirroring() const; //Openni group
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
-	float getSourceOpenniFx() const; // Openni group
-	float getSourceOpenniFy() const; // Openni group
-	float getSourceOpenniCx() const; // Openni group
-	float getSourceOpenniCy() const; // Openni group
+	CameraRGBD * createCameraRGBD() const; // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
 
@@ -200,6 +201,7 @@ public:
 	double getLoopThr() const;
 	double getVpThr() const;
 	int getOdomStrategy() const;
+	QString getCameraInfoDir() const; // "workinfDir/camera_info"
 
 	//
 	void setMonitoringState(bool monitoringState) {_monitoringState = monitoringState;}
@@ -216,6 +218,7 @@ public slots:
 	void selectSourceImage(Src src = kSrcUndef);
 	void selectSourceDatabase(bool user = false);
 	void selectSourceRGBD(Src src = kSrcUndef);
+	void calibrate();
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -246,8 +249,6 @@ private slots:
 	void testOdometry();
 	void cleanRGBDCameraTest();
 	void testRGBDCamera();
-	void calibrate();
-	void resetCalibration();
 
 protected:
 	virtual void showEvent ( QShowEvent * event );
@@ -302,6 +303,9 @@ private:
 	CameraThread * _cameraThread;
 	OdometryThread * _odomThread;
 
+	//calibration
+	CalibrationDialog * _calibrationDialog;
+
 	QVector<QCheckBox*> _3dRenderingShowClouds;
 	QVector<QDoubleSpinBox*> _3dRenderingVoxelSize;
 	QVector<QSpinBox*> _3dRenderingDecimation;

guilib/src/CMakeLists.txt

@@ -17,6 +17,7 @@ SET(headers_ui
 	../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
 	../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
 	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
+	../include/${PROJECT_PREFIX}/gui/CameraViewer.h
 	../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
 	./ExportDialog.h
 	./PostProcessingDialog.h
@@ -75,6 +76,7 @@ SET(SRC_FILES
 	./OdometryViewer.cpp
 	./LoopClosureViewer.cpp
 	./DataRecorder.cpp
+	./CameraViewer.cpp
 	./CalibrationDialog.cpp
 	./ExportDialog.cpp
 	./PostProcessingDialog.cpp

guilib/src/CalibrationDialog.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QFileDialog>
 #include <QMessageBox>
+#include <QCloseEvent>
 
 #include <rtabmap/core/CameraEvent.h>
 #include <rtabmap/gui/UCv2Qt.h>
@@ -47,13 +48,13 @@ namespace rtabmap {
 
 #define COUNT_MIN 40
 
-CalibrationDialog::CalibrationDialog(bool stereo, QWidget * parent) :
+CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, QWidget * parent) :
 	QDialog(parent),
-	boardSize_(8,6),
-	squareSize_(0.033),
 	stereo_(stereo),
+	savingDirectory_(savingDirectory),
 	calibrated_(false),
-	processingData_(false)
+	processingData_(false),
+	savedCalibration_(false)
 {
 	imagePoints_.resize(2);
 	imageParams_.resize(2);
@@ -64,25 +65,6 @@ CalibrationDialog::CalibrationDialog(bool stereo, QWidget * parent) :
 	ui_ = new Ui_calibrationDialog();
 	ui_->setupUi(this);
 
-	if(!stereo_)
-	{
-		ui_->progressBar_x_2->setVisible(false);
-		ui_->progressBar_y_2->setVisible(false);
-		ui_->progressBar_size_2->setVisible(false);
-		ui_->progressBar_skew_2->setVisible(false);
-		ui_->image_view_2->setVisible(false);
-		ui_->label_fx_2->setVisible(false);
-		ui_->label_fy_2->setVisible(false);
-		ui_->label_cx_2->setVisible(false);
-		ui_->label_cy_2->setVisible(false);
-		ui_->label_baseline->setVisible(false);
-		ui_->label_baseline_name->setVisible(false);
-		ui_->lineEdit_K_2->setVisible(false);
-		ui_->lineEdit_D_2->setVisible(false);
-		ui_->lineEdit_R_2->setVisible(false);
-		ui_->lineEdit_P_2->setVisible(false);
-	}
-
 	connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
 	connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
 	connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
@@ -91,15 +73,14 @@ CalibrationDialog::CalibrationDialog(bool stereo, QWidget * parent) :
 	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
 	connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
 
-	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
-	connect(ui_->buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
+	connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));
 
 	ui_->image_view->setFocus();
 
 	ui_->progressBar_count->setMaximum(COUNT_MIN);
 	ui_->progressBar_count->setFormat("%v");
 
-	this->restart();
+	this->setStereoMode(stereo_);
 }
 
 CalibrationDialog::~CalibrationDialog()
@@ -108,36 +89,120 @@ CalibrationDialog::~CalibrationDialog()
 	delete ui_;
 }
 
+void CalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("board_width", ui_->spinBox_boardWidth->value());
+	settings.setValue("board_height", ui_->spinBox_boardHeight->value());
+	settings.setValue("board_square_size", ui_->doubleSpinBox_squareSize->value());
+	settings.setValue("geometry", this->saveGeometry());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void CalibrationDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	this->setBoardWidth(settings.value("board_width", ui_->spinBox_boardWidth->value()).toInt());
+	this->setBoardHeight(settings.value("board_height", ui_->spinBox_boardHeight->value()).toInt());
+	this->setSquareSize(settings.value("board_square_size", ui_->doubleSpinBox_squareSize->value()).toDouble());
+	QByteArray bytes = settings.value("geometry", QByteArray()).toByteArray();
+	if(!bytes.isEmpty())
+	{
+		this->restoreGeometry(bytes);
+	}
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void CalibrationDialog::setStereoMode(bool stereo)
+{
+	this->restart();
+
+	stereo_ = stereo;
+	ui_->progressBar_x_2->setVisible(stereo_);
+	ui_->progressBar_y_2->setVisible(stereo_);
+	ui_->progressBar_size_2->setVisible(stereo_);
+	ui_->progressBar_skew_2->setVisible(stereo_);
+	ui_->image_view_2->setVisible(stereo_);
+	ui_->label_fx_2->setVisible(stereo_);
+	ui_->label_fy_2->setVisible(stereo_);
+	ui_->label_cx_2->setVisible(stereo_);
+	ui_->label_cy_2->setVisible(stereo_);
+	ui_->label_baseline->setVisible(stereo_);
+	ui_->label_baseline_name->setVisible(stereo_);
+	ui_->lineEdit_K_2->setVisible(stereo_);
+	ui_->lineEdit_D_2->setVisible(stereo_);
+	ui_->lineEdit_R_2->setVisible(stereo_);
+	ui_->lineEdit_P_2->setVisible(stereo_);
+}
+
 void CalibrationDialog::setBoardWidth(int width)
 {
-	if(width != boardSize_.width)
+	if(width != ui_->spinBox_boardWidth->value())
 	{
-		boardSize_.width = width;
+		ui_->spinBox_boardWidth->setValue(width);
 		this->restart();
 	}
 }
 
 void CalibrationDialog::setBoardHeight(int height)
 {
-	if(height != boardSize_.height)
+	if(height != ui_->spinBox_boardHeight->value())
 	{
-		boardSize_.height = height;
+		ui_->spinBox_boardHeight->setValue(height);
 		this->restart();
 	}
 }
 
 void CalibrationDialog::setSquareSize(double size)
 {
-	if(size != squareSize_)
+	if(size != ui_->doubleSpinBox_squareSize->value())
 	{
-		squareSize_ = size;
+		ui_->doubleSpinBox_squareSize->setValue(size);
 		this->restart();
 	}
 }
 
 void CalibrationDialog::closeEvent(QCloseEvent* event)
 {
-	this->unregisterFromEventsManager();
+	if(!savedCalibration_ && calibrated_)
+	{
+		QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
+				tr("The camera is calibrated but you didn't "
+				   "save the calibration, do you want to save it?"),
+				QMessageBox::Yes | QMessageBox::Ignore | QMessageBox::Cancel, QMessageBox::Yes);
+		event->ignore();
+		if(b == QMessageBox::Yes)
+		{
+			if(this->save())
+			{
+				event->accept();
+			}
+		}
+		else if(b == QMessageBox::Ignore)
+		{
+			event->accept();
+		}
+	}
+	else
+	{
+		event->accept();
+	}
+	if(event->isAccepted())
+	{
+		this->unregisterFromEventsManager();
+	}
 }
 
 void CalibrationDialog::handleEvent(UEvent * event)
@@ -207,6 +272,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		}
 
 		bool found = false;
+		cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
 		if(!viewGray.empty())
 		{
 			int maxScale = 1;
@@ -217,7 +283,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 					timg = viewGray;
 				else
 					cv::resize(viewGray, timg, cv::Size(), scale, scale);
-				found = cv::findChessboardCorners(timg, boardSize_, pointBuf[id],
+				found = cv::findChessboardCorners(timg, boardSize, pointBuf[id],
 						CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
 				if(found)
 				{
@@ -248,10 +314,10 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 					cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
 
 			boardAccepted[id] = true;
-			getParams(pointBuf[id], boardSize_, imageSize_[id], params[id][0], params[id][1], params[id][2], params[id][3]);
+			getParams(pointBuf[id], boardSize, imageSize_[id], params[id][0], params[id][1], params[id][2], params[id][3]);
 
 			// Draw the corners.
-			cv::drawChessboardCorners(images[id], boardSize_, cv::Mat(pointBuf[id]), found);
+			cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), found);
 		}
 	}
 
@@ -392,6 +458,7 @@ void CalibrationDialog::restart()
 {
 	// restart
 	calibrated_ = false;
+	savedCalibration_ = false;
 	imagePoints_[0].clear();
 	imagePoints_[1].clear();
 	imageParams_[0].clear();
@@ -402,7 +469,6 @@ void CalibrationDialog::restart()
 
 	ui_->pushButton_calibrate->setEnabled(false);
 	ui_->pushButton_save->setEnabled(false);
-	ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
 	ui_->checkBox_rectified->setEnabled(false);
 
 	ui_->progressBar_count->reset();
@@ -442,11 +508,23 @@ void CalibrationDialog::calibrate()
 {
 	UINFO("Calibration...");
 	processingData_ = true;
+	savedCalibration_ = false;
+
+	QMessageBox mb(QMessageBox::Information,
+			tr("Calibrating..."),
+			tr("Operation in progress. You have time to take a coffee!"));
+	mb.show();
+	QApplication::processEvents();
+	uSleep(100); // hack make sure the text in the QMessageBox is shown...
+	QApplication::processEvents();
+
 	std::vector<std::vector<cv::Point3f> > objectPoints(1);
+	cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
+	float squareSize = ui_->doubleSpinBox_squareSize->value();
 	// compute board corner positions
-	for( int i = 0; i < boardSize_.height; ++i )
-		for( int j = 0; j < boardSize_.width; ++j )
-			objectPoints[0].push_back(cv::Point3f(float( j*squareSize_ ), float( i*squareSize_ ), 0));
+	for( int i = 0; i < boardSize.height; ++i )
+		for( int j = 0; j < boardSize.width; ++j )
+			objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
 
 	objectPoints.resize(imagePoints_[0].size(),objectPoints[0]);
 
@@ -523,7 +601,6 @@ void CalibrationDialog::calibrate()
 			ui_->checkBox_rectified->setEnabled(true);
 			ui_->checkBox_rectified->setChecked(true);
 
-			ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
 			ui_->pushButton_save->setEnabled(true);
 		}
 	}
@@ -631,24 +708,23 @@ void CalibrationDialog::calibrate()
 			ui_->checkBox_rectified->setEnabled(true);
 			ui_->checkBox_rectified->setChecked(true);
 
-			ui_->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
 			ui_->pushButton_save->setEnabled(true);
 		}
 	}
 
 	UINFO("End calibration");
-
 	processingData_ = false;
 }
 
-void CalibrationDialog::save()
+bool CalibrationDialog::save()
 {
+	bool saved = false;
 	processingData_ = true;
 	if(!stereo_)
 	{
 		UASSERT(model_.isValid());
 		QString cameraName = model_.name().c_str();
-		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), cameraName+".yaml", "*.yaml");
+		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
 
 		if(!filePath.isEmpty())
 		{
@@ -656,6 +732,8 @@ void CalibrationDialog::save()
 			{
 				QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
 				UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
+				savedCalibration_ = true;
+				saved = true;
 			}
 			else
 			{
@@ -667,7 +745,7 @@ void CalibrationDialog::save()
 	{
 		UASSERT(stereoModel_.isValid());
 		QString cameraName = stereoModel_.name().c_str();
-		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), cameraName, "*.yaml");
+		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
 		std::string name = UFile::getName(filePath.toStdString());
 		std::string dir = UDirectory::getDir(filePath.toStdString());
 		if(!name.empty())
@@ -680,6 +758,8 @@ void CalibrationDialog::save()
 				QMessageBox::information(this, tr("Export"), tr("Calibration files saved to \"%1\" and \"%2\".").
 						arg(leftPath.c_str()).arg(rightPath.c_str()));
 				UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
+				savedCalibration_ = true;
+				saved = true;
 			}
 			else
 			{
@@ -688,6 +768,7 @@ void CalibrationDialog::save()
 		}
 	}
 	processingData_ = false;
+	return saved;
 }
 
 float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)

guilib/src/CameraViewer.cpp

@@ -0,0 +1,111 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/gui/CameraViewer.h"
+
+#include <rtabmap/core/CameraEvent.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/CloudViewer.h>
+#include <rtabmap/gui/UCv2Qt.h>
+#include <QtCore/QMetaType>
+#include <QHBoxLayout>
+
+namespace rtabmap {
+
+
+CameraViewer::CameraViewer(QWidget * parent) :
+		QWidget(parent),
+	imageView_(new ImageView(this)),
+	cloudView_(new CloudViewer(this)),
+	processingImages_(false)
+{
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+
+	imageView_->setImageDepthShown(true);
+	imageView_->setMinimumSize(320, 240);
+	QHBoxLayout * layout = new QHBoxLayout(this);
+	layout->setMargin(0);
+	layout->addWidget(imageView_);
+	layout->addWidget(cloudView_);
+	layout->setStretch(0, 1);
+	layout->setStretch(1, 1);
+	this->setLayout(layout);
+}
+
+CameraViewer::~CameraViewer()
+{
+	this->unregisterFromEventsManager();
+}
+
+void CameraViewer::showImage(const rtabmap::SensorData & data)
+{
+	processingImages_ = true;
+	imageView_->setImage(uCvMat2QImage(data.image()));
+	imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+	if(!data.depth().empty() && data.fx() && data.fy())
+	{
+		cloudView_->addOrUpdateCloud("cloud",
+				util3d::cloudFromDepthRGB(data.image(), data.depth(), data.cx(), data.cy(), data.fx(), data.fy()),
+				data.localTransform());
+	}
+	else if(!data.rightImage().empty() && data.fx() && data.baseline())
+	{
+		cloudView_->addOrUpdateCloud("cloud",
+				util3d::cloudFromStereoImages(data.image(), data.rightImage(), data.cx(), data.cy(), data.fx(), data.baseline()),
+				data.localTransform());
+	}
+	else
+	{
+		cloudView_->setVisible(false);
+	}
+	cloudView_->update();
+	processingImages_ = false;
+}
+
+void CameraViewer::handleEvent(UEvent * event)
+{
+	if(event->getClassName().compare("CameraEvent") == 0)
+	{
+		CameraEvent * camEvent = (CameraEvent*)event;
+		if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
+		   camEvent->getCode() == CameraEvent::kCodeImage)
+		{
+			if(camEvent->data().isValid())
+			{
+				if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+				{
+					processingImages_ = true;
+					QMetaObject::invokeMethod(this, "showImage",
+							Q_ARG(rtabmap::SensorData, camEvent->data()));
+				}
+			}
+		}
+	}
+}
+
+} /* namespace rtabmap */

guilib/src/CloudViewer.cpp

@@ -193,7 +193,7 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 
 	settings.setValue("grid", this->isGridShown());
 	settings.setValue("grid_cell_count", this->getGridCellCount());
-	settings.setValue("grid_cell_size", this->getGridCellSize());
+	settings.setValue("grid_cell_size", (double)this->getGridCellSize());
 
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());

guilib/src/DataRecorder.cpp

@@ -177,7 +177,7 @@ void DataRecorder::handleEvent(UEvent * event)
 						processingImages_ = true;
 						QMetaObject::invokeMethod(this, "showImage",
 								Q_ARG(cv::Mat, camEvent->data().image()),
-								Q_ARG(cv::Mat, camEvent->data().depth()));
+								Q_ARG(cv::Mat, camEvent->data().depthOrRightImage()));
 					}
 				}
 			}

guilib/src/GraphViewer.cpp

@@ -492,8 +492,8 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	{
 		settings.beginGroup(group);
 	}
-	settings.setValue("node_radius", this->getNodeRadius());
-	settings.setValue("link_width", this->getLinkWidth());
+	settings.setValue("node_radius", (double)this->getNodeRadius());
+	settings.setValue("link_width", (double)this->getLinkWidth());
 	settings.setValue("node_color", this->getNodeColor());
 	settings.setValue("neighbor_color", this->getNeighborColor());
 	settings.setValue("global_color", this->getGlobalLoopClosureColor());

guilib/src/GuiLib.qrc

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

guilib/src/MainWindow.cpp

@@ -346,17 +346,21 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionOpenNI_PCL, SIGNAL(triggered()), this, SLOT(selectOpenni()));
 	connect(_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenni()));
 	connect(_ui->actionFreenect, SIGNAL(triggered()), this, SLOT(selectFreenect()));
-	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	connect(_ui->actionOpenNI_CV, SIGNAL(triggered()), this, SLOT(selectOpenniCv()));
 	connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
+	connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
+	connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
+	connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
+	connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
+	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
+	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
-	connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	_ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
-	connect(_ui->actionOpenNI2_Sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
-	_ui->actionOpenNI2_Sense->setEnabled(CameraOpenNI2::available());
-	connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	_ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
+	_ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
+	_ui->actionFreenect2->setEnabled(CameraFreenect2::available());
+	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
 	this->updateSelectSourceMenu();
 
 	connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
@@ -2221,14 +2225,16 @@ void MainWindow::updateSelectSourceMenu()
 
 	_ui->actionDatabase->setChecked(_preferencesDialog->isSourceDatabaseUsed());
 
-	_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
-	_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
-	_ui->actionFreenect->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect);
-	_ui->actionOpenNI_CV->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV);
-	_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
-	_ui->actionOpenNI2->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
-	_ui->actionOpenNI2_Sense->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
-	_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->isSourceOpenniUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
+	_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL);
+	_ui->actionFreenect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect);
+	_ui->actionOpenNI_CV->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV);
+	_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
+	_ui->actionOpenNI2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionFreenect2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect2);
+	_ui->actionStereoDC1394->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoDC1394);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -2555,7 +2561,7 @@ void MainWindow::startDetection()
 	// Adjust pre-requirements
 	if( !_preferencesDialog->isSourceImageUsed() &&
 		!_preferencesDialog->isSourceDatabaseUsed() &&
-		!_preferencesDialog->isSourceOpenniUsed())
+		!_preferencesDialog->isSourceRGBDUsed())
 	{
 		QMessageBox::warning(this,
 				 tr("RTAB-Map"),
@@ -2565,83 +2571,11 @@ void MainWindow::startDetection()
 		return;
 	}
 
-	if(_preferencesDialog->isSourceOpenniUsed())
+	if(_preferencesDialog->isSourceRGBDUsed())
 	{
-		//Create odometry thread if rgbd slam
-		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
-		{
-			if(_odomThread)
-			{
-				UERROR("OdomThread must be already deleted here?!");
-				delete _odomThread;
-			}
-			Odometry * odom;
-			if(_preferencesDialog->getOdomStrategy() == 1)
-			{
-				odom = new OdometryOpticalFlow(parameters);
-			}
-			else
-			{
-				odom = new OdometryBOW(parameters);
-			}
-			_odomThread = new OdometryThread(odom);
+		CameraRGBD * camera = _preferencesDialog->createCameraRGBD();
 
-			UEventsManager::addHandler(_odomThread);
-			_odomThread->start();
-		}
-
-		CameraRGBD * camera = 0;
-		if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL)
-		{
-			camera = new CameraOpenni(
-					_preferencesDialog->getSourceOpenniDevice().toStdString(),
-					_preferencesDialog->getGeneralInputRate(),
-					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFx(),
-					_preferencesDialog->getSourceOpenniFy(),
-					_preferencesDialog->getSourceOpenniCx(),
-					_preferencesDialog->getSourceOpenniCy());
-		}
-		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
-		{
-			camera = new CameraOpenNI2(
-					_preferencesDialog->getSourceOpenniDevice().toStdString(),
-					_preferencesDialog->getGeneralInputRate(),
-					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFx(),
-					_preferencesDialog->getSourceOpenniFy(),
-					_preferencesDialog->getSourceOpenniCx(),
-					_preferencesDialog->getSourceOpenniCy());
-		}
-		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
-		{
-			camera = new CameraFreenect(
-					_preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()),
-					_preferencesDialog->getGeneralInputRate(),
-					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFx(),
-					_preferencesDialog->getSourceOpenniFy(),
-					_preferencesDialog->getSourceOpenniCx(),
-					_preferencesDialog->getSourceOpenniCy());
-		}
-		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
-				_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
-		{
-			camera = new CameraOpenNICV(
-					_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
-					_preferencesDialog->getGeneralInputRate(),
-					_preferencesDialog->getSourceOpenniLocalTransform(),
-					_preferencesDialog->getSourceOpenniFx(),
-					_preferencesDialog->getSourceOpenniFy(),
-					_preferencesDialog->getSourceOpenniCx(),
-					_preferencesDialog->getSourceOpenniCy());
-		}
-		else
-		{
-			UFATAL("RGBD Source type undefined!");
-		}
-
-		if(!camera->init())
+		if(!camera->init(_preferencesDialog->getCameraInfoDir().toStdString()))
 		{
 			ULOGGER_WARN("init camera failed... ");
 			QMessageBox::warning(this,
@@ -2657,7 +2591,7 @@ void MainWindow::startDetection()
 			}
 			return;
 		}
-		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
+		else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 		{
 			((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
 			((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
@@ -2671,9 +2605,50 @@ void MainWindow::startDetection()
 		camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
 
 		_camera = new CameraThread(camera);
-		if(_odomThread)
+
+		//Create odometry thread if rgbd slam
+		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
 		{
-			UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
+			// Require calibrated camera
+			if(!camera->isCalibrated())
+			{
+				UWARN("Camera is not calibrated!");
+				emit stateChanged(kInitialized);
+				delete _camera;
+				_camera = 0;
+
+				int button = QMessageBox::question(this,
+						tr("Camera is not calibrated!"),
+						tr("RTAB-Map cannot run with an uncalibrated camera. Do you want to calibrate the camera now?"),
+						 QMessageBox::Yes | QMessageBox::No);
+				if(button == QMessageBox::Yes)
+				{
+					QTimer::singleShot(0, _preferencesDialog, SLOT(calibrate()));
+				}
+				return;
+			}
+			else
+			{
+				if(_odomThread)
+				{
+					UERROR("OdomThread must be already deleted here?!");
+					delete _odomThread;
+				}
+				Odometry * odom;
+				if(_preferencesDialog->getOdomStrategy() == 1)
+				{
+					odom = new OdometryOpticalFlow(parameters);
+				}
+				else
+				{
+					odom = new OdometryBOW(parameters);
+				}
+				_odomThread = new OdometryThread(odom);
+
+				UEventsManager::addHandler(_odomThread);
+				UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
+				_odomThread->start();
+			}
 		}
 	}
 	else if(_preferencesDialog->isSourceDatabaseUsed())
@@ -3590,6 +3565,16 @@ void MainWindow::selectOpenni2()
 	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2);
 }
 
+void MainWindow::selectFreenect2()
+{
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect2);
+}
+
+void MainWindow::selectStereoDC1394()
+{
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoDC1394);
+}
+
 
 void MainWindow::dumpTheMemory()
 {

guilib/src/PreferencesDialog.cpp

@@ -56,7 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Graph.h"
 
 #include "rtabmap/gui/LoopClosureViewer.h"
-#include "rtabmap/gui/DataRecorder.h"
+#include "rtabmap/gui/CameraViewer.h"
 #include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/gui/ImageView.h"
 #include "GraphViewer.h"
@@ -82,7 +82,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_indexModel(0),
 	_initialized(false),
 	_cameraThread(0),
-	_odomThread(0)
+	_odomThread(0),
+	_calibrationDialog(new CalibrationDialog(false, ".", this))
 {
 	ULOGGER_DEBUG("");
 
@@ -144,6 +145,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
 	}
+	if(!CameraFreenect::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(1, 0, Qt::UserRole - 1);
+	}
+	if(!CameraOpenNICV::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(2, 0, Qt::UserRole - 1);
+		_ui->comboBox_cameraRGBD->setItemData(3, 0, Qt::UserRole - 1);
+	}
+	if(!CameraOpenNI2::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(4, 0, Qt::UserRole - 1);
+	}
+	if(!CameraFreenect2::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(5, 0, Qt::UserRole - 1);
+	}
+	if(!CameraStereoDC1394::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1);
+	}
+	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
+	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
 	_ui->predictionPlot->showLegend(false);
 
@@ -275,18 +299,7 @@ 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_opennipcl, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->radioButton_freenect, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
-	_ui->radioButton_freenect->setEnabled(CameraFreenect::available());
-	connect(_ui->radioButton_opennicv, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->radioButton_opennicvasus, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
-	_ui->radioButton_opennicv->setEnabled(CameraOpenNICV::available());
-	_ui->radioButton_opennicvasus->setEnabled(CameraOpenNICV::available());
-	connect(_ui->radioButton_openni2, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->radioButton_openni2, SIGNAL(toggled(bool)), this, SLOT(showOpenNI2GroupBox(bool)));
-	_ui->radioButton_openni2->setEnabled(CameraOpenNI2::available());
-	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
-	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
+	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_autoWhiteBalance, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -294,12 +307,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), 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()));
-	connect(_ui->doubleSpinBox_openniFx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->doubleSpinBox_openniFy, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->doubleSpinBox_openniCx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->doubleSpinBox_openniCy, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
-	connect(_ui->pushButton_calibrate_reset, SIGNAL(clicked()), this, SLOT(resetCalibration()));
 
 
 	//Rtabmap basic
@@ -918,15 +926,17 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->groupBox_sourceOpenni->setChecked(true);
 #ifdef _WIN32
-		_ui->radioButton_openni2->setChecked(true);
-		_ui->radioButton_opennipcl->setChecked(false);
+		_ui->comboBox_cameraRGBD->setCurrentIndex(4); // openni2
 #else
-		_ui->radioButton_opennipcl->setChecked(true);
-		_ui->radioButton_openni2->setChecked(false);
+		if(CameraFreenect::available())
+		{
+			_ui->comboBox_cameraRGBD->setCurrentIndex(1); // freenect
+		}
+		else
+		{
+			_ui->comboBox_cameraRGBD->setCurrentIndex(0); // openni-pcl
+		}
 #endif
-		_ui->radioButton_freenect->setChecked(false);
-		_ui->radioButton_opennicv->setChecked(false);
-		_ui->radioButton_opennicvasus->setChecked(false);
 		_ui->openni2_autoWhiteBalance->setChecked(true);
 		_ui->openni2_autoExposure->setChecked(true);
 		_ui->openni2_exposure->setValue(0);
@@ -934,10 +944,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->openni2_mirroring->setChecked(false);
 		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
-		_ui->doubleSpinBox_openniFx->setValue(0.0);
-		_ui->doubleSpinBox_openniFy->setValue(0.0);
-		_ui->doubleSpinBox_openniCx->setValue(0.0);
-		_ui->doubleSpinBox_openniCy->setValue(0.0);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1190,11 +1196,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 
 	settings.beginGroup("Openni");
 	_ui->groupBox_sourceOpenni->setChecked(settings.value("openniUsed", _ui->groupBox_sourceOpenni->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_openni2->setChecked(settings.value("openni2", _ui->radioButton_openni2->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->comboBox_cameraRGBD->setCurrentIndex(settings.value("cameraRGBDType", _ui->comboBox_cameraRGBD->currentIndex()).toInt());
 	_ui->openni2_autoWhiteBalance->setChecked(settings.value("openni2AutoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked()).toBool());
 	_ui->openni2_autoExposure->setChecked(settings.value("openni2AutoExposure", _ui->openni2_autoExposure->isChecked()).toBool());
 	_ui->openni2_exposure->setValue(settings.value("openni2Exposure", _ui->openni2_exposure->value()).toInt());
@@ -1202,10 +1204,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->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());
-	_ui->doubleSpinBox_openniFx->setValue(settings.value("fx", _ui->doubleSpinBox_openniFx->value()).toDouble());
-	_ui->doubleSpinBox_openniFy->setValue(settings.value("fy", _ui->doubleSpinBox_openniFy->value()).toDouble());
-	_ui->doubleSpinBox_openniCx->setValue(settings.value("cx", _ui->doubleSpinBox_openniCx->value()).toDouble());
-	_ui->doubleSpinBox_openniCy->setValue(settings.value("cy", _ui->doubleSpinBox_openniCy->value()).toDouble());
+	_calibrationDialog->loadSettings(settings, "CalibrationDialog");
 	settings.endGroup(); // Openni
 }
 
@@ -1462,11 +1461,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 
 	settings.beginGroup("Openni");
 	settings.setValue("openniUsed", 	_ui->groupBox_sourceOpenni->isChecked());
-	settings.setValue("openniType", 	_ui->radioButton_opennipcl->isChecked());
-	settings.setValue("freenectType", 	_ui->radioButton_freenect->isChecked());
-	settings.setValue("openni2", 		_ui->radioButton_openni2->isChecked());
-	settings.setValue("openniCvType", 	_ui->radioButton_opennicv->isChecked());
-	settings.setValue("openniCvAsusType", 	_ui->radioButton_opennicvasus->isChecked());
+	settings.setValue("cameraRGBDType", 	_ui->comboBox_cameraRGBD->currentIndex());
 	settings.setValue("openni2AutoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked());
 	settings.setValue("openni2AutoExposure", 	_ui->openni2_autoExposure->isChecked());
 	settings.setValue("openni2Exposure", 		_ui->openni2_exposure->value());
@@ -1474,11 +1469,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
-	settings.setValue("fx", _ui->doubleSpinBox_openniFx->value());
-	settings.setValue("fy", _ui->doubleSpinBox_openniFy->value());
-	settings.setValue("cx", _ui->doubleSpinBox_openniCx->value());
-	settings.setValue("cy", _ui->doubleSpinBox_openniCy->value());
-	settings.endGroup();
+	_calibrationDialog->saveSettings(settings, "CalibrationDialog");
+	settings.endGroup(); // Openni
 }
 
 void PreferencesDialog::writeCoreSettings(const QString & filePath) const
@@ -1795,6 +1787,7 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
 		const ExportCloudsDialog * exportCloudsDialog = qobject_cast<const ExportCloudsDialog*>(widget);
 		const PostProcessingDialog * postProcessingDialog = qobject_cast<const PostProcessingDialog *>(widget);
 		const GraphViewer * graphViewer = qobject_cast<const GraphViewer *>(widget);
+		const CalibrationDialog * calibrationDialog = qobject_cast<const CalibrationDialog *>(widget);
 
 		if(cloudViewer)
 		{
@@ -1816,6 +1809,10 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
 		{
 			graphViewer->saveSettings(settings);
 		}
+		else if(calibrationDialog)
+		{
+			calibrationDialog->saveSettings(settings);
+		}
 		else
 		{
 			UERROR("Widget \"%s\" cannot be exported in config file.", widget->objectName().toStdString().c_str());
@@ -1840,6 +1837,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
 		ExportCloudsDialog * exportCloudsDialog = qobject_cast<ExportCloudsDialog*>(widget);
 		PostProcessingDialog * postProcessingDialog = qobject_cast<PostProcessingDialog *>(widget);
 		GraphViewer * graphViewer = qobject_cast<GraphViewer *>(widget);
+		CalibrationDialog * calibrationDialog = qobject_cast<CalibrationDialog *>(widget);
 
 		if(cloudViewer)
 		{
@@ -1861,6 +1859,10 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
 		{
 			graphViewer->loadSettings(settings);
 		}
+		else if(calibrationDialog)
+		{
+			calibrationDialog->loadSettings(settings);
+		}
 		else
 		{
 			UERROR("Widget \"%s\" cannot be loaded from config file.", widget->objectName().toStdString().c_str());
@@ -2059,36 +2061,12 @@ void PreferencesDialog::selectSourceRGBD(Src src)
 	}
 
 	_ui->groupBox_sourceOpenni->setChecked(true);
-	_ui->radioButton_opennipcl->setChecked(src == kSrcOpenNI_PCL);
-	_ui->radioButton_freenect->setChecked(src == kSrcFreenect);
-	_ui->radioButton_opennicv->setChecked(src == kSrcOpenNI_CV);
-	_ui->radioButton_opennicvasus->setChecked(src == kSrcOpenNI_CV_ASUS);
-	_ui->radioButton_openni2->setChecked(src == kSrcOpenNI2);
+	_ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcOpenNI_PCL);
 
 	if(_ui->groupBox_sourceOpenni->isChecked())
 	{
 		_ui->groupBox_sourceImage->setChecked(false);
 		_ui->groupBox_sourceDatabase->setChecked(false);
-
-		if(_ui->doubleSpinBox_openniFx->value() != 0 ||
-			_ui->doubleSpinBox_openniFy->value() != 0 ||
-			_ui->doubleSpinBox_openniCx->value() != 0 ||
-			_ui->doubleSpinBox_openniCy->value() != 0 )
-		{
-			int button = QMessageBox::information(this,
-					tr("Calibration detected"),
-					tr("Some calibration values (fx=%1, fy=%2, cx=%3, cy=%4) are set.\n"
-					   "Do you want to reset them to factory defaults?")
-					   .arg(_ui->doubleSpinBox_openniFx->value())
-					   .arg(_ui->doubleSpinBox_openniFy->value())
-					   .arg(_ui->doubleSpinBox_openniCx->value())
-					   .arg(_ui->doubleSpinBox_openniCy->value()),
-					QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
-			if(button & QMessageBox::Yes)
-			{
-				this->resetCalibration();
-			}
-		}
 	}
 
 	if(validateForm())
@@ -2984,7 +2962,7 @@ bool PreferencesDialog::isSourceDatabaseUsed() const
 {
 	return _ui->groupBox_sourceDatabase->isChecked();
 }
-bool PreferencesDialog::isSourceOpenniUsed() const
+bool PreferencesDialog::isSourceRGBDUsed() const
 {
 	return _ui->groupBox_sourceOpenni->isChecked();
 }
@@ -3052,26 +3030,7 @@ int PreferencesDialog::getSourceDatabaseStartPos() const
 
 PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
 {
-	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 if (_ui->radioButton_openni2->isChecked())
-	{
-		return kSrcOpenNI2;
-	}
-	else
-	{
-		return kSrcOpenNI_PCL;
-	}
+	return (PreferencesDialog::Src)(_ui->comboBox_cameraRGBD->currentIndex()+kSrcOpenNI_PCL);
 }
 bool PreferencesDialog::getSourceOpenni2AutoWhiteBalance() const
 {
@@ -3129,21 +3088,55 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
 	return t;
 }
 
-float PreferencesDialog::getSourceOpenniFx() const
+CameraRGBD * PreferencesDialog::createCameraRGBD() const
 {
-	return _ui->doubleSpinBox_openniFx->value();
-}
-float PreferencesDialog::getSourceOpenniFy() const
-{
-	return _ui->doubleSpinBox_openniFy->value();
-}
-float PreferencesDialog::getSourceOpenniCx() const
-{
-	return _ui->doubleSpinBox_openniCx->value();
-}
-float PreferencesDialog::getSourceOpenniCy() const
-{
-	return _ui->doubleSpinBox_openniCy->value();
+	if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL)
+	{
+		return new CameraOpenni(
+				this->getSourceOpenniDevice().toStdString(),
+				this->getGeneralInputRate(),
+				this->getSourceOpenniLocalTransform());
+	}
+	else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
+	{
+		return new CameraOpenNI2(
+				this->getSourceOpenniDevice().toStdString(),
+				this->getGeneralInputRate(),
+				this->getSourceOpenniLocalTransform());
+	}
+	else if(this->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
+	{
+		return new CameraFreenect(
+				this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+				this->getGeneralInputRate(),
+				this->getSourceOpenniLocalTransform());
+	}
+	else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
+			this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
+	{
+		return new CameraOpenNICV(
+				this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
+				this->getGeneralInputRate(),
+				this->getSourceOpenniLocalTransform());
+	}
+	else if(this->getSourceRGBD() == kSrcFreenect2)
+	{
+		return new CameraFreenect2(
+			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
+	}
+	else if(this->getSourceRGBD() == kSrcStereoDC1394)
+	{
+		return new CameraStereoDC1394(
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
+	}
+	else
+	{
+		UFATAL("RGBD Source type undefined!");
+	}
+	return 0;
 }
 
 bool PreferencesDialog::isStatisticsPublished() const
@@ -3163,6 +3156,11 @@ int PreferencesDialog::getOdomStrategy() const
 	return _ui->odom_strategy->currentIndex();
 }
 
+QString PreferencesDialog::getCameraInfoDir() const
+{
+	return (this->getWorkingDirectory().isEmpty()?".":this->getWorkingDirectory())+"/camera_info";
+}
+
 bool PreferencesDialog::isImagesKept() const
 {
 	return _ui->general_checkBox_imagesKept->isChecked();
@@ -3266,58 +3264,9 @@ void PreferencesDialog::testOdometry(int type)
 
 	UASSERT(_odomThread == 0 && _cameraThread == 0);
 
-	CameraRGBD * camera = 0;
-	if(this->getSourceRGBD() == kSrcOpenNI_PCL)
-	{
-		camera = new CameraOpenni(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else if(this->getSourceRGBD() == kSrcOpenNI2)
-	{
-		camera = new CameraOpenNI2(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
+	CameraRGBD * camera = this->createCameraRGBD();
 
-	}
-	else if(this->getSourceRGBD() == kSrcFreenect)
-	{
-		camera = new CameraFreenect(
-			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
-	{
-		camera = new CameraOpenNICV(
-			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS,
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else
-	{
-		UFATAL("RGBD Source type undefined!");
-	}
-
-	if(!camera->init())
+	if(!camera->init(this->getCameraInfoDir().toStdString()))
 	{
 		QMessageBox::warning(this,
 			   tr("RTAB-Map"),
@@ -3325,7 +3274,7 @@ void PreferencesDialog::testOdometry(int type)
 		delete camera;
 		camera = 0;
 	}
-	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 	{
 		((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance());
 		((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure());
@@ -3434,58 +3383,8 @@ void PreferencesDialog::testRGBDCamera()
 			   tr("A camera is already running!"));
 	}
 
-	CameraRGBD * camera = 0;
-	if(this->getSourceRGBD() == kSrcOpenNI_PCL)
-	{
-		camera = new CameraOpenni(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else if(this->getSourceRGBD() == kSrcOpenNI2)
-	{
-		camera = new CameraOpenNI2(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-
-	}
-	else if(this->getSourceRGBD() == kSrcFreenect)
-	{
-		camera = new CameraFreenect(
-			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
-	{
-		camera = new CameraOpenNICV(
-			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS,
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else
-	{
-		UFATAL("RGBD Source type undefined!");
-	}
-
-	if(!camera->init())
+	CameraRGBD * camera = this->createCameraRGBD();
+	if(!camera->init(this->getCameraInfoDir().toStdString()))
 	{
 		QMessageBox::warning(this,
 			   tr("RTAB-Map"),
@@ -3493,7 +3392,7 @@ void PreferencesDialog::testRGBDCamera()
 		delete camera;
 		camera = 0;
 	}
-	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 	{
 		((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance());
 		((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure());
@@ -3512,13 +3411,13 @@ void PreferencesDialog::testRGBDCamera()
 		_ui->pushButton_test_rgbd_camera->setEnabled(false);
 
 		// Create DataRecorder without init it, just to show images...
-		DataRecorder * window = new DataRecorder(this);
+		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(800);
-		window->setMinimumHeight(600);
+		window->setMinimumWidth(1280);
+		window->setMinimumHeight(480);
 		connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanRGBDCameraTest()) );
 		window->registerToEventsManager();
 
@@ -3533,58 +3432,8 @@ void PreferencesDialog::testRGBDCamera()
 
 void PreferencesDialog::calibrate()
 {
-	CameraRGBD * camera = 0;
-	if(this->getSourceRGBD() == kSrcOpenNI_PCL)
-	{
-		camera = new CameraOpenni(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else if(this->getSourceRGBD() == kSrcOpenNI2)
-	{
-		camera = new CameraOpenNI2(
-			this->getSourceOpenniDevice().toStdString(),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-
-	}
-	else if(this->getSourceRGBD() == kSrcFreenect)
-	{
-		camera = new CameraFreenect(
-			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS)
-	{
-		camera = new CameraOpenNICV(
-			this->getSourceRGBD() == kSrcOpenNI_CV_ASUS,
-			this->getGeneralInputRate(),
-			this->getSourceOpenniLocalTransform(),
-			this->getSourceOpenniFx(),
-			this->getSourceOpenniFy(),
-			this->getSourceOpenniCx(),
-			this->getSourceOpenniCy());
-	}
-	else
-	{
-		UFATAL("RGBD Source type undefined!");
-	}
-
-	if(!camera->init())
+	CameraRGBD * camera = this->createCameraRGBD();
+	if(!camera->init("")) // don't set calibration folder to use raw images
 	{
 		QMessageBox::warning(this,
 			   tr("RTAB-Map"),
@@ -3592,7 +3441,7 @@ void PreferencesDialog::calibrate()
 		delete camera;
 		camera = 0;
 	}
-	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	else if(dynamic_cast<CameraOpenNI2*>(camera) != 0)
 	{
 		((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance());
 		((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure());
@@ -3608,33 +3457,32 @@ void PreferencesDialog::calibrate()
 
 	if(camera)
 	{
-		CalibrationDialog * dialog = new CalibrationDialog(this);
-		dialog->registerToEventsManager();
+		if(!this->getCameraInfoDir().isEmpty())
+		{
+			QDir dir(this->getCameraInfoDir());
+			if (!dir.exists())
+			{
+				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());
+				}
+			}
+		}
+		_calibrationDialog->setStereoMode(dynamic_cast<CameraStereoDC1394*>(camera)!=0);
+		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
+		_calibrationDialog->registerToEventsManager();
 
 		CameraThread cameraThread(camera);
-		UEventsManager::createPipe(&cameraThread, dialog, "CameraEvent");
+		UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent");
 
 		cameraThread.start();
 
-		if(dialog->exec() == QDialog::Accepted)
-		{
-			_ui->doubleSpinBox_openniFx->setValue(dialog->fx());
-			_ui->doubleSpinBox_openniFy->setValue(dialog->fy());
-			_ui->doubleSpinBox_openniCx->setValue(dialog->cx());
-			_ui->doubleSpinBox_openniCy->setValue(dialog->cy());
-		}
+		_calibrationDialog->exec();
+		_calibrationDialog->unregisterFromEventsManager();
 
 		cameraThread.join(true);
-		delete dialog;
 	}
 }
 
-void PreferencesDialog::resetCalibration()
-{
-	_ui->doubleSpinBox_openniFx->setValue(0);
-	_ui->doubleSpinBox_openniFy->setValue(0);
-	_ui->doubleSpinBox_openniCx->setValue(0);
-	_ui->doubleSpinBox_openniCy->setValue(0);
-}
-
 }

guilib/src/ui/calibrationDialog.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>875</width>
+    <width>1310</width>
     <height>834</height>
    </rect>
   </property>
@@ -586,7 +586,7 @@
    <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+      <set>QDialogButtonBox::Close</set>
      </property>
     </widget>
    </item>

guilib/src/ui/mainWindow.ui

@@ -135,11 +135,33 @@
         <iconset resource="../GuiLib.qrc">
          <normaloff>:/images/sense.png</normaloff>:/images/sense.png</iconset>
        </property>
-       <addaction name="actionOpenNI2_Sense"/>
+       <addaction name="actionOpenNI2_sense"/>
+      </widget>
+      <widget class="QMenu" name="menuKinect_v2">
+       <property name="title">
+        <string>Kinect v2</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/kinect_xbox_one.png</normaloff>:/images/kinect_xbox_one.png</iconset>
+       </property>
+       <addaction name="actionFreenect2"/>
+      </widget>
+      <widget class="QMenu" name="menuBumblebee2">
+       <property name="title">
+        <string>Bumblebee2</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/bumblebee2.png</normaloff>:/images/bumblebee2.png</iconset>
+       </property>
+       <addaction name="actionStereoDC1394"/>
       </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
       <addaction name="menuSense_3D_scanner"/>
+      <addaction name="menuKinect_v2"/>
+      <addaction name="menuBumblebee2"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
      <addaction name="menuImage"/>
@@ -1113,14 +1135,6 @@
     <string>View scans...</string>
    </property>
   </action>
-  <action name="actionOpenNI2_Sense">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>OpenNI2</string>
-   </property>
-  </action>
   <action name="actionOpen_database">
    <property name="icon">
     <iconset resource="../GuiLib.qrc">
@@ -1163,6 +1177,22 @@
     <string>Save config</string>
    </property>
   </action>
+  <action name="actionFreenect2">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Freenect2</string>
+   </property>
+  </action>
+  <action name="actionStereoDC1394">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>StereoDC1394</string>
+   </property>
+  </action>
   <action name="actionOpenNI2_kinect">
    <property name="checkable">
     <bool>true</bool>
@@ -1171,6 +1201,14 @@
     <string>OpenNI2</string>
    </property>
   </action>
+  <action name="actionOpenNI2_sense">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>OpenNI2</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>744</width>
-            <height>1110</height>
+            <width>737</width>
+            <height>982</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>23</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1687,65 +1687,83 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </widget>
                        </item>
                        <item>
-                        <layout class="QHBoxLayout" name="horizontalLayout_7">
-                         <item>
-                          <widget class="QRadioButton" name="radioButton_opennipcl">
-                           <property name="text">
-                            <string>OpenNI-PCL</string>
+                        <layout class="QGridLayout" name="gridLayout_55" columnstretch="0,0,1">
+                         <item row="0" column="1">
+                          <widget class="QComboBox" name="comboBox_cameraRGBD">
+                           <property name="sizeAdjustPolicy">
+                            <enum>QComboBox::AdjustToContents</enum>
                            </property>
-                           <property name="checked">
+                           <item>
+                            <property name="text">
+                             <string>OpenNI-PCL</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Freenect</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>OpenNI-CV</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>OpenNI-CV-ASUS</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>OpenNI2</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Freenect2</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>StereoDC1394</string>
+                            </property>
+                           </item>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QPushButton" name="pushButton_calibrate">
+                           <property name="text">
+                            <string>Calibrate</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="2">
+                          <widget class="QLabel" name="label_46">
+                           <property name="text">
+                            <string>On initialization, calibration files are loaded from the &quot;camera_info&quot; folder in the working directory.</string>
+                           </property>
+                           <property name="wordWrap">
                             <bool>true</bool>
                            </property>
                           </widget>
                          </item>
-                         <item>
-                          <widget class="QRadioButton" name="radioButton_freenect">
-                           <property name="text">
-                            <string>Feeenect</string>
-                           </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>
-                          <widget class="QRadioButton" name="radioButton_openni2">
-                           <property name="text">
-                            <string>OpenNI2</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>
-                         <item>
+                         <item row="2" column="1">
                           <widget class="QPushButton" name="pushButton_test_rgbd_camera">
                            <property name="text">
                             <string>Test</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="0" column="2">
+                          <widget class="QLabel" name="label_228">
+                           <property name="text">
+                            <string>Driver</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>
@@ -1895,116 +1913,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </item>
                         </layout>
                        </item>
-                       <item>
-                        <widget class="QGroupBox" name="groupBox_3">
-                         <property name="title">
-                          <string>Calibration</string>
-                         </property>
-                         <layout class="QVBoxLayout" name="verticalLayout_41">
-                          <item>
-                           <widget class="QLabel" name="label_160">
-                            <property name="text">
-                             <string>Camera intrinsic parameters. While the default values are good, a calibration is recommended to increase the precision of the 3D clouds. Set values to 0 to use the default parameters from the camera. For example, the focal length is ~525 for Kinect and ~545 for Xtion Pro Live. The button &quot;Calibrate&quot; below can be used to calibrate the camera with a chessboard pattern.</string>
-                            </property>
-                            <property name="wordWrap">
-                             <bool>true</bool>
-                            </property>
-                           </widget>
-                          </item>
-                          <item>
-                           <layout class="QGridLayout" name="gridLayout_55" columnstretch="0,1">
-                            <item row="0" column="0">
-                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniFx">
-                              <property name="maximum">
-                               <double>1000.000000000000000</double>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="0" column="1">
-                             <widget class="QLabel" name="label_161">
-                              <property name="text">
-                               <string>fx=K[0]</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="1" column="0">
-                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniFy">
-                              <property name="maximum">
-                               <double>1000.000000000000000</double>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="1" column="1">
-                             <widget class="QLabel" name="label_162">
-                              <property name="text">
-                               <string>fy=K[4]</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="2" column="0">
-                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniCx">
-                              <property name="maximum">
-                               <double>1000.000000000000000</double>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="2" column="1">
-                             <widget class="QLabel" name="label_166">
-                              <property name="text">
-                               <string>cx=K[2]</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="3" column="0">
-                             <widget class="QDoubleSpinBox" name="doubleSpinBox_openniCy">
-                              <property name="maximum">
-                               <double>1000.000000000000000</double>
-                              </property>
-                             </widget>
-                            </item>
-                            <item row="3" column="1">
-                             <widget class="QLabel" name="label_165">
-                              <property name="text">
-                               <string>cy=K[5]</string>
-                              </property>
-                             </widget>
-                            </item>
-                           </layout>
-                          </item>
-                          <item>
-                           <layout class="QHBoxLayout" name="horizontalLayout_8">
-                            <item>
-                             <widget class="QPushButton" name="pushButton_calibrate">
-                              <property name="text">
-                               <string>Calibrate</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item>
-                             <widget class="QPushButton" name="pushButton_calibrate_reset">
-                              <property name="text">
-                               <string>Reset</string>
-                              </property>
-                             </widget>
-                            </item>
-                            <item>
-                             <spacer name="horizontalSpacer_3">
-                              <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>
-                         </layout>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>