add histogram equalization (#1137)

introlab · Sep 21, 2023 · afd10b1 · afd10b1
1 parent f1cd819
commit afd10b1
Show file tree

Hide file tree

Showing 11 changed files with 122 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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<cv::CLAHE> 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
@@ -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
@@ -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
@@ -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
@@ -2955,7 +2955,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="1">
+                      <item row="11" column="1">
                        <widget class="QLabel" name="label_244">
                         <property name="text">
                          <string>Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images.</string>
@@ -2968,7 +2968,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                        <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="0,1">
                         <item>
                          <widget class="QToolButton" name="toolButton_source_path_calibration">
@@ -3068,7 +3068,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </item>
                        </widget>
                       </item>
-                      <item row="11" column="0">
+                      <item row="12" column="0">
                        <widget class="QPushButton" name="pushButton_test_camera">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -3081,7 +3081,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                        <widget class="QPushButton" name="pushButton_calibrate">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -3094,7 +3094,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_24">
                         <property name="text">
                          <string>Calibration files are saved in &quot;camera_info&quot; folder of the working directory.</string>
@@ -3114,7 +3114,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                        <widget class="QPushButton" name="pushButton_calibrate_simple">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -3127,7 +3127,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_18">
                         <property name="text">
                          <string>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.</string>
@@ -3161,6 +3161,44 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </widget>
                       </item>
                       <item row="6" column="1">
+                       <widget class="QLabel" name="label_681">
+                        <property name="text">
+                         <string>Equalizes the histogram of grayscale images.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QComboBox" name="comboBox_source_histogramMethod">
+                        <property name="currentIndex">
+                         <number>0</number>
+                        </property>
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContents</enum>
+                        </property>
+                        <item>
+                         <property name="text">
+                          <string>None</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>Histogram</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>CLAHE</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
+                      <item row="7" column="1">
                        <widget class="QLabel" name="label_229">
                         <property name="text">
                          <string>Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published.</string>
@@ -3173,7 +3211,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
+                      <item row="7" column="0">
                        <widget class="QCheckBox" name="checkbox_rgbd_colorOnly">
                         <property name="text">
                          <string/>

diff --git 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(