OpenNI2: added parameters to control autoWhiteBalance and autoExposure

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

corelib/include/rtabmap/core/CameraRGBD.h

@@ -196,6 +196,7 @@ class RTABMAP_EXP CameraOpenNI2 :
 
 public:
 	static bool available();
+	static bool exposureGainAvailable();
 
 public:
 	CameraOpenNI2(float imageRate = 0,
@@ -208,6 +209,11 @@ public:
 
 	virtual bool init();
 
+	bool setAutoWhiteBalance(bool enabled);
+	bool setAutoExposure(bool enabled);
+	bool setExposure(int value);
+	bool setGain(int value);
+
 protected:
 	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
 

corelib/include/rtabmap/core/CameraThread.h

@@ -59,6 +59,9 @@ public:
 	bool isCapturing() const {return this->isRunning();}
 	void setImageRate(float imageRate);
 
+	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
+	CameraRGBD * cameraRGBD() {return _cameraRGBD;} // return null if not set, valid until CameraThread is deleted
+
 private:
 	virtual void mainLoop();
 

corelib/src/CameraRGBD.cpp

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 
 #ifdef WITH_OPENNI2
+#include <OniVersion.h>
 #include <OpenNI.h>
 #endif
 
@@ -336,6 +337,15 @@ bool CameraOpenNI2::available()
 #endif
 }
 
+bool CameraOpenNI2::exposureGainAvailable()
+{
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+	return true;
+#else
+	return false;
+#endif
+}
+
 CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) :
 	CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
 #ifdef WITH_OPENNI2
@@ -368,6 +378,66 @@ CameraOpenNI2::~CameraOpenNI2()
 #endif
 }
 
+bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
+{
+#ifdef WITH_OPENNI2
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setAutoExposure(bool enabled)
+{
+#ifdef WITH_OPENNI2
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setExposure(int value)
+{
+#ifdef WITH_OPENNI2
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
+#endif
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setGain(int value)
+{
+#ifdef WITH_OPENNI2
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
+#endif
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
 bool CameraOpenNI2::init()
 {
 #ifdef WITH_OPENNI2
@@ -470,6 +540,16 @@ bool CameraOpenNI2::init()
 			_depth->getHorizontalFieldOfView(),
 			_depth->getVerticalFieldOfView());
 
+	if(_color->getCameraSettings())
+	{
+		UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled());
+		UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled());
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+		UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
+		UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
+#endif
+	}
+
 	bool registered = true;
 	if(registered)
 	{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -172,6 +172,10 @@ public:
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
 	Src getSourceRGBD() const; 			// Openni group
+	bool getSourceOpenni2AutoWhiteBalance() const;  //Openni group
+	bool getSourceOpenni2AutoExposure() const;  //Openni group
+	int getSourceOpenni2Exposure() const;  //Openni group
+	int getSourceOpenni2Gain() const;   //Openni group
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
 	float getSourceOpenniFx() const; // Openni group
@@ -234,6 +238,7 @@ private slots:
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
+	void showOpenNI2GroupBox(bool);
 	void cleanOdometryTest();
 	void testOdometry();
 	void cleanRGBDCameraTest();

guilib/src/MainWindow.cpp

@@ -1629,6 +1629,17 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		if(_camera)
 		{
 			_camera->setImageRate(_preferencesDialog->getGeneralInputRate());
+
+			if(_camera->cameraRGBD() && dynamic_cast<CameraOpenNI2*>(_camera->cameraRGBD()) != 0)
+			{
+				((CameraOpenNI2*)_camera->cameraRGBD())->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
+				((CameraOpenNI2*)_camera->cameraRGBD())->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
+				if(CameraOpenNI2::exposureGainAvailable())
+				{
+					((CameraOpenNI2*)_camera->cameraRGBD())->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
+					((CameraOpenNI2*)_camera->cameraRGBD())->setGain(_preferencesDialog->getSourceOpenni2Gain());
+				}
+			}
 		}
 		if(_dbReader)
 		{
@@ -2227,6 +2238,13 @@ void MainWindow::startDetection()
 			}
 			return;
 		}
+		else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
+		{
+			((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance());
+			((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure());
+			((CameraOpenNI2*)camera)->setExposure(_preferencesDialog->getSourceOpenni2Exposure());
+			((CameraOpenNI2*)camera)->setGain(_preferencesDialog->getSourceOpenni2Gain());
+		}
 
 		_camera = new CameraThread(camera);
 		if(_odomThread)

guilib/src/PreferencesDialog.cpp

@@ -114,6 +114,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #endif
 
 	_ui->predictionPlot->showLegend(false);
+	_ui->groupBox_openni2->setVisible(false);
 
 	QButtonGroup * buttonGroup = new QButtonGroup(this);
 	buttonGroup->addButton(_ui->radioButton_basic);
@@ -252,7 +253,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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->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()));
+	connect(_ui->openni2_gain, SIGNAL(valueChanged(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()));
@@ -856,6 +864,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->radioButton_openni2->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);
+		_ui->openni2_gain->setValue(100);
 		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
 		_ui->doubleSpinBox_openniFx->setValue(0.0);
@@ -1111,6 +1123,10 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_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->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());
+	_ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt());
 	_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());
@@ -1354,6 +1370,10 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath)
 	settings.setValue("openni2", 		_ui->radioButton_openni2->isChecked());
 	settings.setValue("openniCvType", 	_ui->radioButton_opennicv->isChecked());
 	settings.setValue("openniCvAsusType", 	_ui->radioButton_opennicvasus->isChecked());
+	settings.setValue("openni2AutoWhiteBalance", _ui->openni2_autoWhiteBalance->isChecked());
+	settings.setValue("openni2AutoExposure", 	_ui->openni2_autoExposure->isChecked());
+	settings.setValue("openni2Exposure", 		_ui->openni2_exposure->value());
+	settings.setValue("openni2Gain", 			_ui->openni2_gain->value());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
 	settings.setValue("fx", _ui->doubleSpinBox_openniFx->value());
@@ -2489,6 +2509,11 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }
 
+void PreferencesDialog::showOpenNI2GroupBox(bool shown)
+{
+	_ui->groupBox_openni2->setVisible(shown);
+}
+
 /*** GETTERS ***/
 //General
 int PreferencesDialog::getGeneralLoggerLevel() const
@@ -2739,6 +2764,22 @@ PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
 		return kSrcOpenNI_PCL;
 	}
 }
+bool PreferencesDialog::getSourceOpenni2AutoWhiteBalance() const
+{
+	return _ui->openni2_autoWhiteBalance->isChecked();
+}
+bool PreferencesDialog::getSourceOpenni2AutoExposure() const
+{
+	return _ui->openni2_autoExposure->isChecked();
+}
+int PreferencesDialog::getSourceOpenni2Exposure() const
+{
+	return _ui->openni2_exposure->value();
+}
+int PreferencesDialog::getSourceOpenni2Gain() const
+{
+	return _ui->openni2_gain->value();
+}
 QString PreferencesDialog::getSourceOpenniDevice() const
 {
 	return _ui->lineEdit_openniDevice->text();
@@ -2988,6 +3029,16 @@ void PreferencesDialog::testOdometry(int type)
 		delete camera;
 		camera = 0;
 	}
+	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	{
+		((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance());
+		((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure());
+		if(CameraOpenNI2::exposureGainAvailable())
+		{
+			((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure());
+			((CameraOpenNI2*)camera)->setGain(getSourceOpenni2Gain());
+		}
+	}
 
 
 	if(camera)
@@ -3143,6 +3194,16 @@ void PreferencesDialog::testRGBDCamera()
 		delete camera;
 		camera = 0;
 	}
+	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	{
+		((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance());
+		((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure());
+		if(CameraOpenNI2::exposureGainAvailable())
+		{
+			((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure());
+			((CameraOpenNI2*)camera)->setGain(getSourceOpenni2Gain());
+		}
+	}
 
 
 	if(camera)
@@ -3229,6 +3290,16 @@ void PreferencesDialog::calibrate()
 		delete camera;
 		camera = 0;
 	}
+	else if(this->getSourceRGBD() == kSrcOpenNI2)
+	{
+		((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance());
+		((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure());
+		if(CameraOpenNI2::exposureGainAvailable())
+		{
+			((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure());
+			((CameraOpenNI2*)camera)->setGain(getSourceOpenni2Gain());
+		}
+	}
 
 
 	if(camera)

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>744</width>
+            <width>736</width>
-            <height>1161</height>
+            <height>909</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1786,6 +1786,92 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </item>
                         </layout>
                        </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_openni2">
+                         <property name="title">
+                          <string>OpenNI 2</string>
+                         </property>
+                         <layout class="QFormLayout" name="formLayout_6">
+                          <item row="3" column="0">
+                           <widget class="QSpinBox" name="openni2_gain">
+                            <property name="maximum">
+                             <number>1000</number>
+                            </property>
+                            <property name="value">
+                             <number>100</number>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="2" column="0">
+                           <widget class="QSpinBox" name="openni2_exposure">
+                            <property name="maximum">
+                             <number>65535</number>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="0">
+                           <widget class="QCheckBox" name="openni2_autoExposure">
+                            <property name="text">
+                             <string/>
+                            </property>
+                            <property name="checked">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="0">
+                           <widget class="QCheckBox" name="openni2_autoWhiteBalance">
+                            <property name="text">
+                             <string/>
+                            </property>
+                            <property name="checked">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_217">
+                            <property name="text">
+                             <string>Auto white balance.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="1">
+                           <widget class="QLabel" name="label_218">
+                            <property name="text">
+                             <string>Auto exposure.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="2" column="1">
+                           <widget class="QLabel" name="label_219">
+                            <property name="text">
+                             <string>Exposure.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="3" column="1">
+                           <widget class="QLabel" name="label_220">
+                            <property name="text">
+                             <string>Gain.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
                        <item>
                         <layout class="QFormLayout" name="formLayout">
                          <property name="fieldGrowthPolicy">