From 835155e6ae6d180ad3df7bc8a7078b25467d9c66 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 15 Oct 2015 16:40:49 -0400 Subject: [PATCH] CameraOpenNI2: Added OpenNI2 stamps/Ids option --- corelib/include/rtabmap/core/CameraRGBD.h | 2 ++ corelib/src/CameraRGBD.cpp | 12 ++++++-- guilib/src/PreferencesDialog.cpp | 5 ++++ guilib/src/ui/preferencesDialog.ui | 34 ++++++++++++++++++----- 4 files changed, 44 insertions(+), 9 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index adbac999..548c2e4a 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -160,6 +160,7 @@ public: bool setExposure(int value); bool setGain(int value); bool setMirroring(bool enabled); + void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;} protected: virtual SensorData captureImage(); @@ -171,6 +172,7 @@ private: float _depthFx; float _depthFy; std::string _deviceId; + bool _openNI2StampsAndIDsUsed; }; diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index e6dc5aa6..2070bef1 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -390,7 +390,8 @@ CameraOpenNI2::CameraOpenNI2( #endif _depthFx(0.0f), _depthFy(0.0f), - _deviceId(deviceId) + _deviceId(deviceId), + _openNI2StampsAndIDsUsed(false) { } @@ -715,7 +716,14 @@ SensorData CameraOpenNI2::captureImage() float(rgb.cols/2) - 0.5f, //cx float(rgb.rows/2) - 0.5f, //cy this->getLocalTransform()); - data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + if(_openNI2StampsAndIDsUsed) + { + data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0); + } + else + { + data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + } } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index c7f92241..9417206a 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -377,6 +377,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->openni2_stampsIdsUsed, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_cameraRGBDImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesStamps())); @@ -1134,6 +1135,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->openni2_exposure->setValue(0); _ui->openni2_gain->setValue(100); _ui->openni2_mirroring->setChecked(false); + _ui->openni2_stampsIdsUsed->setChecked(false); _ui->comboBox_freenect2Format->setCurrentIndex(0); _ui->lineEdit_openniOniPath->clear(); _ui->lineEdit_openni2OniPath->clear(); @@ -1408,6 +1410,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->openni2_exposure->setValue(settings.value("exposure", _ui->openni2_exposure->value()).toInt()); _ui->openni2_gain->setValue(settings.value("gain", _ui->openni2_gain->value()).toInt()); _ui->openni2_mirroring->setChecked(settings.value("mirroring", _ui->openni2_mirroring->isChecked()).toBool()); + _ui->openni2_stampsIdsUsed->setChecked(settings.value("stampsIdsUsed", _ui->openni2_stampsIdsUsed->isChecked()).toBool()); _ui->lineEdit_openni2OniPath->setText(settings.value("oniPath", _ui->lineEdit_openni2OniPath->text()).toString()); settings.endGroup(); // Openni2 @@ -1721,6 +1724,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("exposure", _ui->openni2_exposure->value()); settings.setValue("gain", _ui->openni2_gain->value()); settings.setValue("mirroring", _ui->openni2_mirroring->isChecked()); + settings.setValue("stampsIdsUsed", _ui->openni2_stampsIdsUsed->isChecked()); settings.setValue("oniPath", _ui->lineEdit_openni2OniPath->text()); settings.endGroup(); // Openni2 @@ -3733,6 +3737,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) ((CameraOpenNI2*)camera)->setAutoWhiteBalance(_ui->openni2_autoWhiteBalance->isChecked()); ((CameraOpenNI2*)camera)->setAutoExposure(_ui->openni2_autoExposure->isChecked()); ((CameraOpenNI2*)camera)->setMirroring(_ui->openni2_mirroring->isChecked()); + ((CameraOpenNI2*)camera)->setOpenNI2StampsAndIDsUsed(_ui->openni2_stampsIdsUsed->isChecked()); if(CameraOpenNI2::exposureGainAvailable()) { ((CameraOpenNI2*)camera)->setExposure(_ui->openni2_exposure->value()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 43397344..76447d56 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -1003 + -315 760 1705 @@ -86,7 +86,7 @@ QFrame::Raised - 19 + 3 @@ -1769,7 +1769,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 3 + 0 @@ -1874,7 +1874,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 6 + 4 @@ -2009,7 +2009,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Qt::Vertical @@ -2042,7 +2042,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -2052,7 +2052,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Mirroring. @@ -2086,6 +2086,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Use timestamps and frame IDs from OpenNI2. + + + true + + + + + + + + + + false + + +