Refactoring Registration: added RegistrationInfo class, pipeline registration. Camera source: create laser scan from depth image. Removed OdometryICP as the same behavior can be achieved with OdometryF2F and "Reg/Strategy" = 1 (ICP) or 2 (Vis+ICP).

guilib/include/rtabmap/gui/CameraViewer.h

@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 
 class QSpinBox;
+class QCheckBox;
+class QPushButton;
 
 namespace rtabmap {
 
@@ -59,6 +61,9 @@ private:
 	bool processingImages_;
 	QSpinBox * decimationSpin_;
 	int validDecimationValue_;
+	QPushButton * pause_;
+	QCheckBox * showCloudCheckbox_;
+	QCheckBox * showScanCheckbox_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -193,9 +193,11 @@ public:
 	bool getSourceDatabaseStampsUsed() const;//Database group
 	bool isSourceRGBDColorOnly() const;
 	bool isSourceStereoDepthGenerated() const;
+	bool isSourceScanFromDepth() const;
+	int getSourceScanFromDepthDecimation() const;
+	double getSourceScanFromDepthMaxDepth() const;
 	Transform getSourceLocalTransform() const;    //Openni group
-	Transform getStereoLaserLocalTransform() const; // stereo images
-	Transform getRGBDLaserLocalTransform() const; // rgbd images
+	Transform getLaserLocalTransform() const; // directory images
 	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
@@ -259,25 +261,19 @@ private slots:
 	void openDatabaseViewer();
 	void selectSourceDatabase();
 	void selectCalibrationPath();
-	void selectSourceRGBDImagesStamps();
+	void selectSourceImagesStamps();
 	void selectSourceRGBDImagesPathRGB();
 	void selectSourceRGBDImagesPathDepth();
-	void selectSourceRGBDImagesPathScans();
-	void selectSourceRGBDImagesPathGt();
-	void selectSourceStereoImagesStamps();
+	void selectSourceImagesPathScans();
+	void selectSourceImagesPathGt();
 	void selectSourceStereoImagesPathLeft();
 	void selectSourceStereoImagesPathRight();
-	void selectSourceStereoImagesPathScans();
-	void selectSourceStereoImagesPathGt();
 	void selectSourceImagesPath();
 	void selectSourceVideoPath();
 	void selectSourceStereoVideoPath();
 	void selectSourceOniPath();
 	void selectSourceOni2Path();
 	void updateSourceGrpVisibility();
-	void updateRGBDCameraGroupBoxVisibility();
-	void updateRGBCameraGroupBoxVisibility();
-	void updateStereoCameraGroupBoxVisibility();
 	void testOdometry();
 	void testCamera();
 

guilib/src/CameraViewer.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraEvent.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util2d.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/gui/ImageView.h>
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/utilite/UCv2Qt.h>
@@ -40,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QLabel>
 #include <QSpinBox>
 #include <QDialogButtonBox>
+#include <QCheckBox>
+#include <QPushButton>
 
 namespace rtabmap {
 
@@ -66,13 +69,24 @@ CameraViewer::CameraViewer(QWidget * parent) :
 	decimationSpin_->setMaximum(16);
 	decimationSpin_->setValue(1);
 
+	pause_ = new QPushButton("Pause", this);
+	pause_->setCheckable(true);
+	showCloudCheckbox_ = new QCheckBox("Show RGB-D cloud", this);
+	showCloudCheckbox_->setEnabled(false);
+	showCloudCheckbox_->setChecked(true);
+	showScanCheckbox_ = new QCheckBox("Show scan", this);
+	showScanCheckbox_->setEnabled(false);
+
 	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
 	buttonBox->setStandardButtons(QDialogButtonBox::Close);
 	connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
 
 	QHBoxLayout * layout2 = new QHBoxLayout();
+	layout2->addWidget(pause_);
 	layout2->addWidget(decimationLabel);
 	layout2->addWidget(decimationSpin_);
+	layout2->addWidget(showCloudCheckbox_);
+	layout2->addWidget(showScanCheckbox_);
 	layout2->addStretch(1);
 	layout2->addWidget(buttonBox);
 
@@ -123,42 +137,68 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	{
 		imageView_->setImageDepth(uCvMat2QImage(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_)));
 	}
-	if((data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
+
+	if(!data.depthOrRightRaw().empty() &&
+	   (data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
 	{
-		if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
+		if(showCloudCheckbox_->isChecked())
 		{
-			cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
-			cloudView_->setVisible(true);
-			cloudView_->update();
-		}
-		else if(!data.depthOrRightRaw().empty())
-		{
-			cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
-			cloudView_->setVisible(true);
-			cloudView_->update();
+			if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
+			{
+				showCloudCheckbox_->setEnabled(true);
+				cloudView_->addOrUpdateCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
+			}
+			else if(!data.depthOrRightRaw().empty())
+			{
+				showCloudCheckbox_->setEnabled(true);
+				cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
+			}
 		}
 	}
-	else
+
+	if(!data.laserScanRaw().empty())
 	{
-		cloudView_->setVisible(false);
+		showScanCheckbox_->setEnabled(true);
+		if(showScanCheckbox_->isChecked())
+		{
+			cloudView_->addOrUpdateCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), validDecimationValue_), Transform::getIdentity(), Qt::yellow);
+		}
 	}
+
+	cloudView_->setVisible(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled());
+	if(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled())
+	{
+		cloudView_->update();
+	}
+	if(cloudView_->getAddedClouds().contains("cloud"))
+	{
+		cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
+	}
+	if(cloudView_->getAddedClouds().contains("scan"))
+	{
+		cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
+	}
+
 	processingImages_ = false;
 }
 
 void CameraViewer::handleEvent(UEvent * event)
 {
-	if(event->getClassName().compare("CameraEvent") == 0)
+	if(!pause_->isChecked())
 	{
-		CameraEvent * camEvent = (CameraEvent*)event;
-		if(camEvent->getCode() == CameraEvent::kCodeData)
+		if(event->getClassName().compare("CameraEvent") == 0)
 		{
-			if(camEvent->data().isValid())
+			CameraEvent * camEvent = (CameraEvent*)event;
+			if(camEvent->getCode() == CameraEvent::kCodeData)
 			{
-				if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+				if(camEvent->data().isValid())
 				{
-					processingImages_ = true;
-					QMetaObject::invokeMethod(this, "showImage",
-							Q_ARG(rtabmap::SensorData, camEvent->data()));
+					if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
+					{
+						processingImages_ = true;
+						QMetaObject::invokeMethod(this, "showImage",
+								Q_ARG(rtabmap::SensorData, camEvent->data()));
+					}
 				}
 			}
 		}

guilib/src/DatabaseViewer.cpp

@@ -3544,9 +3544,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 		}
 	}
 
-
-	float variance = -1.0f;
 	Transform transform;
+	RegistrationInfo info;
 
 	SensorData dataFrom, dataTo;
 	dbDriver_->getNodeData(currentLink.from(), dataFrom);
@@ -3582,12 +3581,12 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	UINFO("Uncompress time: %f s", timer.ticks());
 
 	RegistrationIcp registration(parameters);
-	transform = registration.computeTransformation(dataFrom, dataTo, t, 0, 0, &variance);
+	transform = registration.computeTransformation(dataFrom, dataTo, t, &info);
 	UINFO("Icp time: %f s", timer.ticks());
 
 	if(!transform.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, variance, variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.variance, info.variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -3652,9 +3651,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 
 	Transform t;
-	std::string rejectedMsg;
-	float variance = -1.0f;
-	std::vector<int> inliers;
+	RegistrationInfo info;
 
 	// Add sensor data to generate features
 	SensorData dataFrom;
@@ -3669,7 +3666,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	RegistrationVis reg(parameters);
 	Signature fromS(dataFrom);
 	Signature toS(dataTo);
-	t = reg.computeTransformationMod(fromS, toS, currentLink.transform(), &rejectedMsg, &inliers, &variance);
+	t = reg.computeTransformationMod(fromS, toS, currentLink.transform(), &info);
 	UDEBUG("");
 
 	if(!silent)
@@ -3681,7 +3678,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 
 	if(!t.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, info.variance, info.variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -3714,7 +3711,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	{
 		QMessageBox::warning(this,
 				tr("Add link"),
-				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
+				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg_.c_str()));
 	}
 }
 
@@ -3743,9 +3740,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 
 		Transform t;
-		std::string rejectedMsg;
-		float variance = -1.0f;
-		std::vector<int> inliers;
+		RegistrationInfo info;
 
 		// Add sensor data to generate features
 		SensorData dataFrom;
@@ -3760,7 +3755,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		RegistrationVis reg(parameters);
 		Signature fromS(dataFrom);
 		Signature toS(dataTo);
-		t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
+		t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &info);
 		UDEBUG("");
 
 		if(!silent)
@@ -3775,11 +3770,11 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			// transform is valid, make a link
 			if(from>to)
 			{
-				linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance, variance)));
+				linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
 			}
 			else
 			{
-				linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), variance, variance)));
+				linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), info.variance, info.variance)));
 			}
 			updateSlider = true;
 		}
@@ -3787,7 +3782,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		{
 			QMessageBox::warning(this,
 					tr("Add link"),
-					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
+					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg_.c_str()));
 		}
 	}
 	else if(containsLink(linksRemoved_, from, to))

guilib/src/MainWindow.cpp

@@ -735,9 +735,10 @@ void MainWindow::handleEvent(UEvent* anEvent)
 
 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
 {
-	_ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id_, (float)info.timeCapture_*1000.0);
-	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id_, (float)info.timeDisparity_*1000.0);
-	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id_, (float)info.timeMirroring_*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id, (float)info.timeCapture*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id, (float)info.timeDisparity*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id, (float)info.timeMirroring*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", (float)info.id, (float)info.timeScanFromDepth*1000.0);
 }
 
 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
@@ -3014,6 +3015,7 @@ void MainWindow::startDetection()
 		_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
 		_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 		_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
+		_camera->setScanFromDepth(_preferencesDialog->isSourceScanFromDepth(), _preferencesDialog->getSourceScanFromDepthDecimation(), _preferencesDialog->getSourceScanFromDepthMaxDepth());
 
 		//Create odometry thread if rgbd slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
@@ -3527,18 +3529,16 @@ void MainWindow::postProcessing()
 						}
 
 						Transform transform;
-						std::string rejectedMsg;
-						float variance = -1.0f;
+						RegistrationInfo info;
 						RegistrationVis registration(parameters);
-						std::vector<int> inliers;
-						transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &rejectedMsg, &inliers, &variance);
+						transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info);
 
 						if(!transform.isNull())
 						{
 							UINFO("Added new loop closure between %d and %d.", from, to);
 							addedLinks.insert(from);
 							addedLinks.insert(to);
-							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance, variance)));
+							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
 							++loopClosuresAdded;
 							_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
 						}
@@ -3618,18 +3618,17 @@ void MainWindow::postProcessing()
 					if(!signatureFrom.sensorData().laserScanRaw().empty() &&
 					   !signatureTo.sensorData().laserScanRaw().empty())
 					{
-						std::string rejectedMsg;
-						float variance = -1.0f;
-						Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &rejectedMsg, 0, &variance);
+						RegistrationInfo info;
+						Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
 
 						if(!transform.isNull())
 						{
-							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
+							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), info.variance, info.variance);
 							iter->second = newLink;
 						}
 						else
 						{
-							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(rejectedMsg.c_str());
+							QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(info.rejectedMsg_.c_str());
 							_initProgressDialog->appendText(str, Qt::darkYellow);
 							UWARN("%s", str.toStdString().c_str());
 							warn = true;

guilib/src/PreferencesDialog.cpp

@@ -210,9 +210,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// Default Driver
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
-	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
-	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBCameraGroupBoxVisibility()));
-	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoCameraGroupBoxVisibility()));
+	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
+	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
+	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	this->resetSettings(_ui->groupBox_source0);
 
 	_ui->predictionPlot->showLegend(false);
@@ -388,37 +388,27 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_freenect2EdgeAwareFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
-	connect(_ui->toolButton_cameraRGBDImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesStamps()));
-	connect(_ui->lineEdit_cameraRGBDImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->toolButton_cameraImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceImagesStamps()));
+	connect(_ui->lineEdit_cameraImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_cameraRGBDImages_path_rgb, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathRGB()));
 	connect(_ui->toolButton_cameraRGBDImages_path_depth, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathDepth()));
-	connect(_ui->toolButton_cameraRGBDImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathScans()));
-	connect(_ui->toolButton_cameraRGBDImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathGt()));
+	connect(_ui->toolButton_cameraImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathScans()));
+	connect(_ui->toolButton_cameraImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathGt()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->checkBox_cameraRGBDImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_cameraImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraRGBDImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraRGBDImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->spinBox_cameraRGBDImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraRGBDImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->comboBox_cameraRGBDImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
-	connect(_ui->toolButton_cameraStereoImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesStamps()));
-	connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_cameraStereoImages_path_left, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathLeft()));
 	connect(_ui->toolButton_cameraStereoImages_path_right, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathRight()));
-	connect(_ui->toolButton_cameraStereoImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathScans()));
-	connect(_ui->toolButton_cameraStereoImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathGt()));
 	connect(_ui->lineEdit_cameraStereoImages_path_left, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraStereoImages_path_right, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraStereoImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraStereoImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->spinBox_cameraStereoImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->checkBox_stereoImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->lineEdit_cameraStereoImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->comboBox_cameraStereoImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->toolButton_cameraStereoVideo_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath()));
 	connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -433,7 +423,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_openniOniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openni2OniPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 
-
+	connect(_ui->groupBox_scanFromDepth, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraScanFromDepth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -637,7 +629,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kVisInlierDistance().c_str());
 	_ui->loopClosure_bowIterations->setObjectName(Parameters::kVisIterations().c_str());
 	_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kVisRefineIterations().c_str());
-	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kVisForce2D().c_str());
+	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kRegForce3DoF().c_str());
 	_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
 	connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
 	_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
@@ -646,7 +638,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
 	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
+
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
+	_ui->comboBox_registrationStrategy->setObjectName(Parameters::kRegStrategy().c_str());
 
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
 	_ui->loopClosure_correspondencesType->setObjectName(Parameters::kVisCorType().c_str());
@@ -663,10 +657,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
 	_ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str());
 
-	_ui->loopClosure_icp->setObjectName(Parameters::kRGBDLoopClosureLinkRefining().c_str());
 	_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
 	_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
-	_ui->loopClosure_icp2D->setObjectName(Parameters::kIcp2D().c_str());
 
 	_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str());
 	_ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str());
@@ -1193,28 +1185,26 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_openni2OniPath->clear();
 		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
 		_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
-		_ui->checkBox_cameraRGBDImages_timestamps->setChecked(false);
 		_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
-		_ui->lineEdit_cameraRGBDImages_timestamps->setText("");
-		_ui->lineEdit_cameraRGBDImages_path_scans->setText("");
-		_ui->lineEdit_cameraRGBDImages_laser_transform->setText("0 0 0 0 0 0");
-		_ui->spinBox_cameraRGBDImages_max_scan_pts->setValue(0);
-		_ui->lineEdit_cameraRGBDImages_gt->setText("");
-		_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(0);
 
 		_ui->source_comboBox_image_type->setCurrentIndex(kSrcDC1394-kSrcDC1394);
-		_ui->lineEdit_cameraStereoImages_timestamps->setText("");
 		_ui->lineEdit_cameraStereoImages_path_left->setText("");
 		_ui->lineEdit_cameraStereoImages_path_right->setText("");
-		_ui->lineEdit_cameraStereoImages_path_scans->setText("");
-		_ui->lineEdit_cameraStereoImages_laser_transform->setText("0 0 0 0 0 0");
-		_ui->spinBox_cameraStereoImages_max_scan_pts->setValue(0);
-		_ui->checkBox_stereoImages_timestamps->setChecked(false);
 		_ui->checkBox_stereoImages_rectify->setChecked(false);
 		_ui->lineEdit_cameraStereoVideo_path->setText("");
 		_ui->checkBox_stereoVideo_rectify->setChecked(false);
-		_ui->lineEdit_cameraStereoImages_gt->setText("");
-		_ui->comboBox_cameraStereoImages_gtFormat->setCurrentIndex(0);
+
+		_ui->checkBox_cameraImages_timestamps->setChecked(false);
+		_ui->lineEdit_cameraImages_timestamps->setText("");
+		_ui->lineEdit_cameraImages_path_scans->setText("");
+		_ui->lineEdit_cameraImages_laser_transform->setText("0 0 0 0 0 0");
+		_ui->spinBox_cameraImages_max_scan_pts->setValue(0);
+		_ui->lineEdit_cameraImages_gt->setText("");
+		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
+
+		_ui->groupBox_scanFromDepth->setChecked(false);
+		_ui->spinBox_cameraScanFromDepth_decimation->setValue(8);
+		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1491,27 +1481,13 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("RGBDImages");
 	_ui->lineEdit_cameraRGBDImages_path_rgb->setText(settings.value("path_rgb", _ui->lineEdit_cameraRGBDImages_path_rgb->text()).toString());
 	_ui->lineEdit_cameraRGBDImages_path_depth->setText(settings.value("path_depth", _ui->lineEdit_cameraRGBDImages_path_depth->text()).toString());
-	_ui->checkBox_cameraRGBDImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraRGBDImages_timestamps->isChecked()).toBool());
-	_ui->lineEdit_cameraRGBDImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraRGBDImages_timestamps->text()).toString());
 	_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(settings.value("scale", _ui->doubleSpinBox_cameraRGBDImages_scale->value()).toDouble());
-	_ui->lineEdit_cameraRGBDImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraRGBDImages_path_scans->text()).toString());
-	_ui->lineEdit_cameraRGBDImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraRGBDImages_laser_transform->text()).toString());
-	_ui->spinBox_cameraRGBDImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraRGBDImages_max_scan_pts->value()).toInt());
-	_ui->lineEdit_cameraRGBDImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraRGBDImages_gt->text()).toString());
-	_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // RGBDImages
 
 	settings.beginGroup("StereoImages");
-	_ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString());
 	_ui->lineEdit_cameraStereoImages_path_left->setText(settings.value("path_left", _ui->lineEdit_cameraStereoImages_path_left->text()).toString());
 	_ui->lineEdit_cameraStereoImages_path_right->setText(settings.value("path_right", _ui->lineEdit_cameraStereoImages_path_right->text()).toString());
-	_ui->lineEdit_cameraStereoImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraStereoImages_path_scans->text()).toString());
-	_ui->lineEdit_cameraStereoImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraStereoImages_laser_transform->text()).toString());
-	_ui->spinBox_cameraStereoImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraStereoImages_max_scan_pts->value()).toInt());
-	_ui->checkBox_stereoImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_stereoImages_timestamps->isChecked()).toBool());
 	_ui->checkBox_stereoImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoImages_rectify->isChecked()).toBool());
-	_ui->lineEdit_cameraStereoImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraStereoImages_gt->text()).toString());
-	_ui->comboBox_cameraStereoImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraStereoImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // StereoImages
 
 	settings.beginGroup("StereoVideo");
@@ -1524,6 +1500,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
 	_ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool());
 	_ui->checkBox_rgbImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbImages_rectify->isChecked()).toBool());
+
+	_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
+	_ui->lineEdit_cameraImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraImages_timestamps->text()).toString());
+	_ui->lineEdit_cameraImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraImages_path_scans->text()).toString());
+	_ui->lineEdit_cameraImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraImages_laser_transform->text()).toString());
+	_ui->spinBox_cameraImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value()).toInt());
+	_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
+	_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // images
 
 	settings.beginGroup("Video");
@@ -1531,6 +1515,12 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->checkBox_rgbVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbVideo_rectify->isChecked()).toBool());
 	settings.endGroup(); // video
 
+	settings.beginGroup("ScanFromDepth");
+	_ui->groupBox_scanFromDepth->setChecked(settings.value("enabled", _ui->groupBox_scanFromDepth->isChecked()).toBool());
+	_ui->spinBox_cameraScanFromDepth_decimation->setValue(settings.value("decimation", _ui->spinBox_cameraScanFromDepth_decimation->value()).toInt());
+	_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(settings.value("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()).toDouble());
+	settings.endGroup();//ScanFromDepth
+
 	settings.beginGroup("Database");
 	_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
@@ -1842,27 +1832,13 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.beginGroup("RGBDImages");
 	settings.setValue("path_rgb",            _ui->lineEdit_cameraRGBDImages_path_rgb->text());
 	settings.setValue("path_depth",          _ui->lineEdit_cameraRGBDImages_path_depth->text());
-	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraRGBDImages_timestamps->isChecked());
-	settings.setValue("stamps",              _ui->lineEdit_cameraRGBDImages_timestamps->text());
 	settings.setValue("scale",               _ui->doubleSpinBox_cameraRGBDImages_scale->value());
-	settings.setValue("path_scans",          _ui->lineEdit_cameraRGBDImages_path_scans->text());
-	settings.setValue("scan_transform",      _ui->lineEdit_cameraRGBDImages_laser_transform->text());
-	settings.setValue("scan_max_pts",        _ui->spinBox_cameraRGBDImages_max_scan_pts->value());
-	settings.setValue("gt_path",             _ui->lineEdit_cameraRGBDImages_gt->text());
-	settings.setValue("gt_format",           _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex());
 	settings.endGroup(); // RGBDImages
 
 	settings.beginGroup("StereoImages");
-	settings.setValue("stamps",         _ui->lineEdit_cameraStereoImages_timestamps->text());
 	settings.setValue("path_left",      _ui->lineEdit_cameraStereoImages_path_left->text());
 	settings.setValue("path_right",     _ui->lineEdit_cameraStereoImages_path_right->text());
-	settings.setValue("path_scans",     _ui->lineEdit_cameraStereoImages_path_scans->text());
-	settings.setValue("scan_transform", _ui->lineEdit_cameraStereoImages_laser_transform->text());
-	settings.setValue("scan_max_pts", _ui->spinBox_cameraStereoImages_max_scan_pts->value());
-	settings.setValue("filenames_as_stamps", _ui->checkBox_stereoImages_timestamps->isChecked());
 	settings.setValue("rectify", 	    _ui->checkBox_stereoImages_rectify->isChecked());
-	settings.setValue("gt_path",        _ui->lineEdit_cameraStereoImages_gt->text());
-	settings.setValue("gt_format",      _ui->comboBox_cameraStereoImages_gtFormat->currentIndex());
 	settings.endGroup(); // StereoImages
 
 	settings.beginGroup("StereoVideo");
@@ -1875,6 +1851,13 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
 	settings.setValue("refreshDir", 	_ui->source_images_refreshDir->isChecked());
 	settings.setValue("rectify", 	    _ui->checkBox_rgbImages_rectify->isChecked());
+	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
+	settings.setValue("stamps",              _ui->lineEdit_cameraImages_timestamps->text());
+	settings.setValue("path_scans",          _ui->lineEdit_cameraImages_path_scans->text());
+	settings.setValue("scan_transform",      _ui->lineEdit_cameraImages_laser_transform->text());
+	settings.setValue("scan_max_pts",        _ui->spinBox_cameraImages_max_scan_pts->value());
+	settings.setValue("gt_path",             _ui->lineEdit_cameraImages_gt->text());
+	settings.setValue("gt_format",           _ui->comboBox_cameraImages_gtFormat->currentIndex());
 	settings.endGroup(); // images
 
 	settings.beginGroup("Video");
@@ -1882,6 +1865,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("rectify", 	    _ui->checkBox_rgbVideo_rectify->isChecked());
 	settings.endGroup(); // video
 
+	settings.beginGroup("ScanFromDepth");
+	settings.setValue("enabled", 			_ui->groupBox_scanFromDepth->isChecked());
+	settings.setValue("decimation", 		_ui->spinBox_cameraScanFromDepth_decimation->value());
+	settings.setValue("maxDepth", 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
+	settings.endGroup();
+
 	settings.beginGroup("Database");
 	settings.setValue("path", 			   _ui->source_database_lineEdit_path->text());
 	settings.setValue("ignoreOdometry",    _ui->source_checkBox_ignoreOdometry->isChecked());
@@ -2454,9 +2443,9 @@ void PreferencesDialog::selectCalibrationPath()
 	}
 }
 
-void PreferencesDialog::selectSourceRGBDImagesStamps()
+void PreferencesDialog::selectSourceImagesStamps()
 {
-	QString dir = _ui->lineEdit_cameraRGBDImages_timestamps->text();
+	QString dir = _ui->lineEdit_cameraImages_timestamps->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
@@ -2464,7 +2453,7 @@ void PreferencesDialog::selectSourceRGBDImagesStamps()
 	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)"));
 	if(path.size())
 	{
-		_ui->lineEdit_cameraRGBDImages_timestamps->setText(path);
+		_ui->lineEdit_cameraImages_timestamps->setText(path);
 	}
 }
 
@@ -2483,9 +2472,9 @@ void PreferencesDialog::selectSourceRGBDImagesPathRGB()
 }
 
 
-void PreferencesDialog::selectSourceRGBDImagesPathScans()
+void PreferencesDialog::selectSourceImagesPathScans()
 {
-	QString dir = _ui->lineEdit_cameraRGBDImages_path_scans->text();
+	QString dir = _ui->lineEdit_cameraImages_path_scans->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
@@ -2493,7 +2482,7 @@ void PreferencesDialog::selectSourceRGBDImagesPathScans()
 	QString path = QFileDialog::getExistingDirectory(this, tr("Select scans directory"), dir);
 	if(path.size())
 	{
-		_ui->lineEdit_cameraRGBDImages_path_scans->setText(path);
+		_ui->lineEdit_cameraImages_path_scans->setText(path);
 	}
 }
 
@@ -2511,9 +2500,9 @@ void PreferencesDialog::selectSourceRGBDImagesPathDepth()
 	}
 }
 
-void PreferencesDialog::selectSourceRGBDImagesPathGt()
+void PreferencesDialog::selectSourceImagesPathGt()
 {
-	QString dir = _ui->lineEdit_cameraRGBDImages_gt->text();
+	QString dir = _ui->lineEdit_cameraImages_gt->text();
 	if(dir.isEmpty())
 	{
 		dir = getWorkingDirectory();
@@ -2522,33 +2511,19 @@ void PreferencesDialog::selectSourceRGBDImagesPathGt()
 	if(path.size())
 	{
 		QStringList list;
-		for(int i=0; i<_ui->comboBox_cameraRGBDImages_gtFormat->count(); ++i)
+		for(int i=0; i<_ui->comboBox_cameraImages_gtFormat->count(); ++i)
 		{
-			list.push_back(_ui->comboBox_cameraRGBDImages_gtFormat->itemText(i));
+			list.push_back(_ui->comboBox_cameraImages_gtFormat->itemText(i));
 		}
 		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
 		if(!item.isEmpty())
 		{
-			_ui->lineEdit_cameraRGBDImages_gt->setText(path);
-			_ui->comboBox_cameraRGBDImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraRGBDImages_gtFormat->findText(item));
+			_ui->lineEdit_cameraImages_gt->setText(path);
+			_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraImages_gtFormat->findText(item));
 		}
 	}
 }
 
-void PreferencesDialog::selectSourceStereoImagesStamps()
-{
-	QString dir = _ui->lineEdit_cameraStereoImages_timestamps->text();
-	if(dir.isEmpty())
-	{
-		dir = getWorkingDirectory();
-	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Timestamps file (*.txt)"));
-	if(path.size())
-	{
-		_ui->lineEdit_cameraStereoImages_timestamps->setText(path);
-	}
-}
-
 void PreferencesDialog::selectSourceStereoImagesPathLeft()
 {
 	QString dir = _ui->lineEdit_cameraStereoImages_path_left->text();
@@ -2577,44 +2552,6 @@ void PreferencesDialog::selectSourceStereoImagesPathRight()
 	}
 }
 
-void PreferencesDialog::selectSourceStereoImagesPathScans()
-{
-	QString dir = _ui->lineEdit_cameraStereoImages_path_scans->text();
-	if(dir.isEmpty())
-	{
-		dir = getWorkingDirectory();
-	}
-	QString path = QFileDialog::getExistingDirectory(this, tr("Select scans directory"), dir);
-	if(path.size())
-	{
-		_ui->lineEdit_cameraStereoImages_path_scans->setText(path);
-	}
-}
-
-void PreferencesDialog::selectSourceStereoImagesPathGt()
-{
-	QString dir = _ui->lineEdit_cameraStereoImages_gt->text();
-	if(dir.isEmpty())
-	{
-		dir = getWorkingDirectory();
-	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt)"));
-	if(path.size())
-	{
-		QStringList list;
-		for(int i=0; i<_ui->comboBox_cameraStereoImages_gtFormat->count(); ++i)
-		{
-			list.push_back(_ui->comboBox_cameraStereoImages_gtFormat->itemText(i));
-		}
-		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list);
-		if(!item.isEmpty())
-		{
-			_ui->lineEdit_cameraStereoImages_gt->setText(path);
-			_ui->comboBox_cameraStereoImages_gtFormat->setCurrentIndex(_ui->comboBox_cameraStereoImages_gtFormat->findText(item));
-		}
-	}
-}
-
 void PreferencesDialog::selectSourceImagesPath()
 {
 	QString dir = _ui->source_images_lineEdit_path->text();
@@ -2935,6 +2872,11 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 					this->addParameter(_ui->graphOptimization_stopEpsilon,       _ui->graphOptimization_stopEpsilon->value());
 					this->addParameter(_ui->graphOptimization_robust,            _ui->graphOptimization_robust->isChecked());
 				}
+				else if(comboBox == _ui->comboBox_registrationStrategy)
+				{
+					this->addParameters(_ui->groupBox_visualTransform2);
+					this->addParameters(_ui->groupBox_icp2);
+				}
 			}
 			// Add parameter
 			_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
@@ -2981,10 +2923,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 				this->addParameters(_ui->groupBox_visualTransform2);
 				this->addParameters(_ui->groupBox_icp2);
 			}
-			else if(value && checkbox == _ui->loopClosure_icp)
-			{
-				this->addParameters(_ui->groupBox_icp2);
-			}
 
 			if(groupBox)
 			{
@@ -3409,23 +3347,24 @@ void PreferencesDialog::updateSourceGrpVisibility()
 	_ui->groupBox_sourceStereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1);
 	_ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2);
 	_ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3);
-}
 
-void PreferencesDialog::updateRGBDCameraGroupBoxVisibility()
-{
-	_ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL);
-	_ui->groupBox_freenect2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcOpenNI_PCL);
-}
+	_ui->groupBox_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1);
 
-void PreferencesDialog::updateRGBCameraGroupBoxVisibility()
-{
-	_ui->source_groupBox_images->setVisible(_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcUsbDevice);
-	_ui->source_groupBox_video->setVisible(_ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcUsbDevice);
-}
+	_ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD));
+	_ui->groupBox_openni2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD);
+	_ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD);
 
-void PreferencesDialog::updateStereoCameraGroupBoxVisibility()
-{
-	_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcDC1394);
+	_ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && (_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo));
+	_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo);
+
+	_ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB));
+	_ui->source_groupBox_images->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
+	_ui->source_groupBox_video->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB);
+
+	_ui->groupBox_sourceImages_optional->setVisible(
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
+			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->comboBox_sourceType->currentIndex() == kSrcImages-kSrcRGB));
 }
 
 /*** GETTERS ***/
@@ -3701,19 +3640,9 @@ Transform PreferencesDialog::getSourceLocalTransform() const
 	return t;
 }
 
-Transform PreferencesDialog::getStereoLaserLocalTransform() const
+Transform PreferencesDialog::getLaserLocalTransform() const
 {
-	Transform t = Transform::fromString(_ui->lineEdit_cameraStereoImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
-	if(t.isNull())
-	{
-		return Transform::getIdentity();
-	}
-	return t;
-}
-
-Transform PreferencesDialog::getRGBDLaserLocalTransform() const
-{
-	Transform t = Transform::fromString(_ui->lineEdit_cameraRGBDImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
+	Transform t = Transform::fromString(_ui->lineEdit_cameraImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
 	if(t.isNull())
 	{
 		return Transform::getIdentity();
@@ -3753,6 +3682,18 @@ bool PreferencesDialog::isSourceStereoDepthGenerated() const
 {
 	return _ui->checkbox_stereo_depthGenerated->isChecked();
 }
+bool PreferencesDialog::isSourceScanFromDepth() const
+{
+	return _ui->groupBox_scanFromDepth->isChecked();
+}
+int PreferencesDialog::getSourceScanFromDepthDecimation() const
+{
+	return _ui->spinBox_cameraScanFromDepth_decimation->value();
+}
+double PreferencesDialog::getSourceScanFromDepthMaxDepth() const
+{
+	return _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value();
+}
 
 Camera * PreferencesDialog::createCamera(bool useRawImages)
 {
@@ -3848,12 +3789,12 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
-		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraRGBDImages_gt->text().toStdString(), _ui->comboBox_cameraRGBDImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraRGBDImages*)camera)->setScanPath(
-						_ui->lineEdit_cameraRGBDImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraRGBDImages_path_scans->text().append(QDir::separator()).toStdString(),
-						_ui->spinBox_cameraRGBDImages_max_scan_pts->value(),
-						this->getRGBDLaserLocalTransform());
-		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraRGBDImages_timestamps->isChecked(), _ui->lineEdit_cameraRGBDImages_timestamps->text().toStdString());
+						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						this->getLaserLocalTransform());
+		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcDC1394)
 	{
@@ -3885,12 +3826,12 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->checkBox_stereoImages_rectify->isChecked(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
-		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraStereoImages_gt->text().toStdString(), _ui->comboBox_cameraStereoImages_gtFormat->currentIndex());
+		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
 		((CameraStereoImages*)camera)->setScanPath(
-						_ui->lineEdit_cameraStereoImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraStereoImages_path_scans->text().append(QDir::separator()).toStdString(),
-						_ui->spinBox_cameraStereoImages_max_scan_pts->value(),
-						this->getStereoLaserLocalTransform());
-		((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_stereoImages_timestamps->isChecked(), _ui->lineEdit_cameraStereoImages_timestamps->text().toStdString());
+						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						this->getLaserLocalTransform());
+		((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcStereoVideo)
 	{
@@ -3925,6 +3866,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
 		((CameraImages*)camera)->setDirRefreshed(_ui->source_images_refreshDir->isChecked());
 		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked());
+
+		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setScanPath(
+						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
+						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						this->getLaserLocalTransform());
+		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
 	else if(driver == kSrcDatabase)
 	{
@@ -4153,6 +4101,7 @@ void PreferencesDialog::testOdometry()
 		cameraThread.setMirroringEnabled(isSourceMirroring());
 		cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 		cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
+		cameraThread.setScanFromDepth(_ui->groupBox_scanFromDepth->isChecked(), _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
 		UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
 		UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
 		UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
@@ -4220,6 +4169,7 @@ void PreferencesDialog::testCamera()
 			cameraThread.setMirroringEnabled(isSourceMirroring());
 			cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 			cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
+			cameraThread.setScanFromDepth(_ui->groupBox_scanFromDepth->isChecked(), _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
 			UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
 
 			cameraThread.start();