diff --git a/corelib/include/rtabmap/core/CameraInfo.h b/corelib/include/rtabmap/core/CameraInfo.h index 0b3681543c..9243947152 100644 --- a/corelib/include/rtabmap/core/CameraInfo.h +++ b/corelib/include/rtabmap/core/CameraInfo.h @@ -45,6 +45,7 @@ class CameraInfo timeMirroring(0.0f), timeStereoExposureCompensation(0.0f), timeImageDecimation(0.0f), + timeHistogramEqualization(0.0f), timeScanFromDepth(0.0f), timeUndistortDepth(0.0f), timeBilateralFiltering(0.0f), @@ -62,6 +63,7 @@ class CameraInfo float timeMirroring; float timeStereoExposureCompensation; float timeImageDecimation; + float timeHistogramEqualization; float timeScanFromDepth; float timeUndistortDepth; float timeBilateralFiltering; diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 8dd40469ce..514fea9f58 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -50,7 +50,7 @@ class IMUFilter; /** * Class CameraThread - * + * */ class RTABMAP_CORE_EXPORT CameraThread : public UThread, @@ -80,6 +80,7 @@ class RTABMAP_CORE_EXPORT CameraThread : void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;} void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} void setImageDecimation(int decimation) {_imageDecimation = decimation;} + void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;} void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} void setImageRate(float imageRate); void setDistortionModel(const std::string & path); @@ -134,6 +135,7 @@ class RTABMAP_CORE_EXPORT CameraThread : bool _stereoExposureCompensation; bool _colorOnly; int _imageDecimation; + int _histogramMethod; bool _stereoToDepth; bool _scanFromDepth; int _scanDownsampleStep; diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 68abc3b498..2f56d4b768 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -57,6 +57,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) : _stereoExposureCompensation(false), _colorOnly(false), _imageDecimation(1), + _histogramMethod(0), _stereoToDepth(false), _scanFromDepth(false), _scanDownsampleStep(1), @@ -96,6 +97,7 @@ CameraThread::CameraThread( _stereoExposureCompensation(false), _colorOnly(false), _imageDecimation(1), + _histogramMethod(0), _stereoToDepth(false), _scanFromDepth(false), _scanDownsampleStep(1), @@ -134,6 +136,7 @@ CameraThread::CameraThread( _stereoExposureCompensation(false), _colorOnly(false), _imageDecimation(1), + _histogramMethod(0), _stereoToDepth(false), _scanFromDepth(false), _scanDownsampleStep(1), @@ -458,6 +461,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const } if(info) info->timeImageDecimation = timer.ticks(); } + if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1) { if(data.cameraModels().size() == 1) @@ -493,6 +497,50 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const } } + if(_histogramMethod && !data.imageRaw().empty()) + { + if(data.imageRaw().type() == CV_8UC1) + { + UDEBUG(""); + UTimer timer; + cv::Mat image; + if(_histogramMethod == 1) + { + cv::equalizeHist(data.imageRaw(), image); + if(!data.depthRaw().empty()) + { + data.setRGBDImage(image, data.depthRaw(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + cv::Mat right; + cv::equalizeHist(data.rightRaw(), right); + data.setStereoImage(image, right, data.stereoCameraModels()[0]); + } + } + else if(_histogramMethod == 2) + { + cv::Ptr clahe = cv::createCLAHE(3.0); + clahe->apply(data.imageRaw(), image); + if(!data.depthRaw().empty()) + { + data.setRGBDImage(image, data.depthRaw(), data.cameraModels()); + } + else if(!data.rightRaw().empty()) + { + cv::Mat right; + clahe->apply(data.rightRaw(), right); + data.setStereoImage(image, right, data.stereoCameraModels()[0]); + } + } + if(info) info->timeHistogramEqualization = timer.ticks(); + } + else + { + UWARN("Histogram equalization only supports grayscale images..."); + } + } + if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty()) { if(data.stereoCameraModels().size()==1) diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 8a9348f9dd..75f5156f6b 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -264,6 +264,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog double getBilateralSigmaS() const; double getBilateralSigmaR() const; int getSourceImageDecimation() const; + int getSourceHistogramMethod() const; bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; bool isSourceScanFromDepth() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 950babffe9..e5894b5c2d 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -598,10 +598,15 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Planning/Length/m", false); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false); + _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", false); + _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", false); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false); _ui->statsToolBox->updateStat("Camera/Time disparity/ms", false); _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", false); + _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", false); + _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", false); _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", false); + _ui->statsToolBox->updateStat("Camera/Time total/ms", false); _ui->statsToolBox->updateStat("Odometry/ID/", false); _ui->statsToolBox->updateStat("Odometry/Features/", false); @@ -988,15 +993,16 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info) } if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible()) { - _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time histogram equalization/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeHistogramEqualization*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time total/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeTotal*1000.0f, _preferencesDialog->isCacheSavedInFigures()); } Q_EMIT(cameraInfoProcessed()); @@ -5758,6 +5764,7 @@ void MainWindow::startDetection() _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation()); + _camera->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod()); _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); _camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation()); _camera->setScanParameters( diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 90a3372872..e09f7875c0 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -816,6 +816,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_source_histogramMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_stereo_exposureCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate())); @@ -1985,6 +1986,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkbox_rgbd_colorOnly->setChecked(false); _ui->spinBox_source_imageDecimation->setValue(1); + _ui->comboBox_source_histogramMethod->setCurrentIndex(0); _ui->checkbox_stereo_depthGenerated->setChecked(false); _ui->checkBox_stereo_exposureCompensation->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); @@ -2419,6 +2421,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString()); _ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString()); _ui->spinBox_source_imageDecimation->setValue(settings.value("imageDecimation",_ui->spinBox_source_imageDecimation->value()).toInt()); + _ui->comboBox_source_histogramMethod->setCurrentIndex(settings.value("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()).toInt()); // Backward compatibility if(_ui->lineEdit_sourceLocalTransform->text().compare("0 0 1 -1 0 0 0 -1 0") == 0) { @@ -2957,6 +2960,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("device", _ui->lineEdit_sourceDevice->text()); settings.setValue("localTransform", _ui->lineEdit_sourceLocalTransform->text()); settings.setValue("imageDecimation", _ui->spinBox_source_imageDecimation->value()); + settings.setValue("histogramMethod", _ui->comboBox_source_histogramMethod->currentIndex()); settings.beginGroup("rgbd"); settings.setValue("driver", _ui->comboBox_cameraRGBD->currentIndex()); @@ -5921,6 +5925,10 @@ int PreferencesDialog::getSourceImageDecimation() const { return _ui->spinBox_source_imageDecimation->value(); } +int PreferencesDialog::getSourceHistogramMethod() const +{ + return _ui->comboBox_source_histogramMethod->currentIndex(); +} bool PreferencesDialog::isSourceStereoDepthGenerated() const { return _ui->checkbox_stereo_depthGenerated->isChecked(); @@ -6725,6 +6733,7 @@ void PreferencesDialog::testOdometry() cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); + cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); cameraThread.setScanParameters( @@ -6796,6 +6805,7 @@ void PreferencesDialog::testCamera() cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); + cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex()); cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked()); cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked()); cameraThread.setScanParameters( diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index c4cf93b253..870a60bb38 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -2955,7 +2955,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. @@ -2968,7 +2968,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -3068,7 +3068,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -3081,7 +3081,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -3094,7 +3094,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Calibration files are saved in "camera_info" folder of the working directory. @@ -3114,7 +3114,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + @@ -3127,7 +3127,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. @@ -3161,6 +3161,44 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + Equalizes the histogram of grayscale images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + Histogram + + + + + CLAHE + + + + + Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. @@ -3173,7 +3211,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index d467630724..5eb29e0f95 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -148,6 +148,7 @@ int main (int argc, char * argv[]) cam->setMirroringEnabled(dialog.isSourceMirroring()); cam->setColorOnly(dialog.isSourceRGBDColorOnly()); cam->setImageDecimation(dialog.getSourceImageDecimation()); + cam->setHistogramMethod(dialog.getSourceHistogramMethod()); cam->setStereoToDepth(dialog.isSourceStereoDepthGenerated()); cam->setStereoExposureCompensation(dialog.isSourceStereoExposureCompensation()); cam->setScanParameters( diff --git a/tools/EurocDataset/main.cpp b/tools/EurocDataset/main.cpp index 75ab880acb..52071e23b2 100644 --- a/tools/EurocDataset/main.cpp +++ b/tools/EurocDataset/main.cpp @@ -486,6 +486,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index fe30590e21..7d3ed21175 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -491,6 +491,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f)); diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index b8c0e75750..144cf6796e 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -319,6 +319,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f)); externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f)); externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f)); + externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f)); externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f)); externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f)); externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));