Added message box when calibration cannot be done on a specified driver. Added HD option in Source panel for Freenect 2.

corelib/src/CameraRGBD.cpp

@@ -367,7 +367,7 @@ bool CameraOpenNICV::init(const std::string & calibrationFolder)
 
 bool CameraOpenNICV::isCalibrated() const
 {
-	return _depthFocal > 0.0f;
+	return true;
 }
 
 void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
@@ -697,7 +697,7 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder)
 
 bool CameraOpenNI2::isCalibrated() const
 {
-	return _depthFx > 0.0f && _depthFy > 0.0f;
+	return true;
 }
 
 std::string CameraOpenNI2::getSerial() const
@@ -1048,10 +1048,7 @@ bool CameraFreenect::init(const std::string & calibrationFolder)
 
 bool CameraFreenect::isCalibrated() const
 {
-#ifdef WITH_FREENECT
+	return true;
-	return freenectDevice_ != 0 && freenectDevice_->getDepthFocal() > 0.0f;
-#endif
-	return false;
 }
 
 std::string CameraFreenect::getSerial() const

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -186,10 +186,11 @@ public:
 	int getSourceOpenni2Exposure() const;  //Openni group
 	int getSourceOpenni2Gain() const;   //Openni group
 	bool getSourceOpenni2Mirroring() const; //Openni group
+	int getSourceFreenect2Format() const; //Openni group
 	bool isSourceRGBDColorOnly() const;
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
-	CameraRGBD * createCameraRGBD() const; // return camera should be deleted if not null
+	CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
 
@@ -247,7 +248,7 @@ private slots:
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
-	void updateOpenNI2GroupBoxVisibility();
+	void updateRGBDCameraGroupBoxVisibility();
 	void testOdometry();
 	void testRGBDCamera();
 

guilib/src/PreferencesDialog.cpp

@@ -167,7 +167,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
 	// Default Driver
-	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOpenNI2GroupBoxVisibility()));
+	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
 	this->resetSettings(_ui->groupBox_source0);
 
 	_ui->predictionPlot->showLegend(false);
@@ -310,6 +310,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rgbd_colorOnly, 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()));
@@ -970,6 +971,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->openni2_exposure->setValue(0);
 		_ui->openni2_gain->setValue(100);
 		_ui->openni2_mirroring->setChecked(false);
+		_ui->comboBox_freenect2Format->setCurrentIndex(0);
 		_ui->checkbox_rgbd_colorOnly->setChecked(false);
 		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
@@ -1234,6 +1236,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->openni2_exposure->setValue(settings.value("openni2Exposure", _ui->openni2_exposure->value()).toInt());
 	_ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt());
 	_ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->isChecked()).toBool());
+	_ui->comboBox_freenect2Format->setCurrentIndex(settings.value("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()).toInt());
 	_ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->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());
@@ -1503,6 +1506,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("openni2Exposure", 		_ui->openni2_exposure->value());
 	settings.setValue("openni2Gain", 			_ui->openni2_gain->value());
 	settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked());
+	settings.setValue("freenect2Format", _ui->comboBox_freenect2Format->currentIndex());
 	settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
@@ -2829,9 +2833,10 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }
 
-void PreferencesDialog::updateOpenNI2GroupBoxVisibility()
+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);
 }
 
 /*** GETTERS ***/
@@ -3104,6 +3109,11 @@ bool PreferencesDialog::getSourceOpenni2Mirroring() const
 {
 	return _ui->openni2_mirroring->isChecked();
 }
+int PreferencesDialog::getSourceFreenect2Format() const
+{
+	return _ui->comboBox_freenect2Format->currentIndex();
+}
+
 bool PreferencesDialog::isSourceRGBDColorOnly() const
 {
 	return _ui->checkbox_rgbd_colorOnly->isChecked();
@@ -3144,42 +3154,82 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
 	return t;
 }
 
-CameraRGBD * PreferencesDialog::createCameraRGBD() const
+CameraRGBD * PreferencesDialog::createCameraRGBD(bool forCalibration)
 {
 	if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL)
 	{
-		return new CameraOpenni(
+		if(forCalibration)
-				this->getSourceOpenniDevice().toStdString(),
+		{
-				this->getGeneralInputRate(),
+			QMessageBox::warning(this, tr("Calibration"),
-				this->getSourceOpenniLocalTransform());
+					tr("RTAB-Map calibration for \"OpenNI\" driver is not yet supported. "
+					    "Factory calibration loaded from OpenNI is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			return new CameraOpenni(
+					this->getSourceOpenniDevice().toStdString(),
+					this->getGeneralInputRate(),
+					this->getSourceOpenniLocalTransform());
+		}
 	}
 	else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
 	{
-		return new CameraOpenNI2(
+		if(forCalibration)
-				this->getSourceOpenniDevice().toStdString(),
+	{
-				this->getGeneralInputRate(),
+		QMessageBox::warning(this, tr("Calibration"),
-				this->getSourceOpenniLocalTransform());
+				tr("RTAB-Map calibration for \"OpenNI2\" driver is not yet supported. "
+					"Factory calibration loaded from OpenNI2 is used."), QMessageBox::Ok);
+		return 0;
+	}
+		else
+		{
+			return new CameraOpenNI2(
+					this->getSourceOpenniDevice().toStdString(),
+					this->getGeneralInputRate(),
+					this->getSourceOpenniLocalTransform());
+		}
 	}
 	else if(this->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
 	{
-		return new CameraFreenect(
+		if(forCalibration)
-				this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+		{
-				this->getGeneralInputRate(),
+			QMessageBox::warning(this, tr("Calibration"),
-				this->getSourceOpenniLocalTransform());
+					tr("RTAB-Map calibration for \"Freenect\" driver is not yet supported. "
+						"Factory calibration loaded from Freenect is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			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(
+		if(forCalibration)
-				this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
+		{
-				this->getGeneralInputRate(),
+			QMessageBox::warning(this, tr("Calibration"),
-				this->getSourceOpenniLocalTransform());
+					tr("RTAB-Map calibration for \"OpenNI\" driver is not yet supported. "
+						"Factory calibration loaded from OpenNI is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			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()),
-			CameraFreenect2::kTypeRGBDepthSD,
+			forCalibration?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)getSourceFreenect2Format(),
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform());
 	}
@@ -3429,12 +3479,17 @@ void PreferencesDialog::testRGBDCamera()
 
 void PreferencesDialog::calibrate()
 {
-	CameraRGBD * camera = this->createCameraRGBD();
+	CameraRGBD * camera = this->createCameraRGBD(true);
 	if(camera == 0 || !camera->init("")) // don't set calibration folder to use raw images
 	{
-		QMessageBox::warning(this,
+		if(camera != 0)
-			   tr("RTAB-Map"),
+		{
-			   tr("RGBD camera initialization failed!"));
+			QMessageBox::warning(this,
+				   tr("RTAB-Map"),
+				   tr("RGBD camera initialization failed!"));
+		}
+		//else already warned
+
 		if(camera)
 		{
 			delete camera;
@@ -3467,7 +3522,7 @@ void PreferencesDialog::calibrate()
 			}
 		}
 	}
-	_calibrationDialog->setStereoMode(dynamic_cast<CameraStereoDC1394*>(camera)!=0);
+	_calibrationDialog->setStereoMode(true);
 	_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 	_calibrationDialog->registerToEventsManager();
 

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-485</y>
             <width>760</width>
-            <height>1029</height>
+            <height>1132</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1943,6 +1943,42 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </layout>
                         </widget>
                        </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_freenect2">
+                         <property name="title">
+                          <string>Freenect2</string>
+                         </property>
+                         <layout class="QGridLayout" name="gridLayout_56" columnstretch="0,1">
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_230">
+                            <property name="text">
+                             <string>Format.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="0">
+                           <widget class="QComboBox" name="comboBox_freenect2Format">
+                            <property name="sizeAdjustPolicy">
+                             <enum>QComboBox::AdjustToContents</enum>
+                            </property>
+                            <item>
+                             <property name="text">
+                              <string>RGB+Depth SD</string>
+                             </property>
+                            </item>
+                            <item>
+                             <property name="text">
+                              <string>RGB+Depth HD</string>
+                             </property>
+                            </item>
+                           </widget>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
                        <item>
                         <layout class="QFormLayout" name="formLayout">
                          <property name="fieldGrowthPolicy">