CameraMyntEye/GUI: added manual/auto exposure option (default auto). Rtabmap: refactored warning when RGBD/OptimizeFromGraphEnd changes state.

corelib/include/rtabmap/core/Rtabmap.h

@@ -284,6 +284,7 @@ private:
 	double _lastProcessTime;
 	bool _someNodesHaveBeenTransferred;
 	float _distanceTravelled;
+	bool _optimizeFromGraphEndChanged;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.

corelib/include/rtabmap/core/camera/CameraMyntEye.h

@@ -61,6 +61,7 @@ public:
 	virtual bool odomProvided() const { return false; }
 
 	void publishInterIMU(bool enabled);
+	void setAutoExposure(bool enabled, int manualGain=24, int manualBrightness=120, int manualConstrast=116);
 
 protected:
 	/**
@@ -80,6 +81,10 @@ private:
 	std::string deviceName_;
 	bool apiRectification_;
 	bool apiDepth_;
+	bool autoExposure_;
+	int gain_;
+	int brightness_;
+	int contrast_;
 	USemaphore dataReady_;
 	UMutex dataMutex_;
 	cv::Mat leftFrameBuffer_;
@@ -94,7 +99,7 @@ private:
 
 	double softTimeBegin_;
 	std::uint64_t hardTimeBegin_;
-	std::uint64_t unitHardTime;
+	std::uint64_t unitHardTime_;
 	std::vector<std::uint64_t> lastHardTimes_;
 	std::vector<std::uint64_t> acc_;
 #endif

corelib/src/Rtabmap.cpp

@@ -133,6 +133,7 @@ Rtabmap::Rtabmap() :
 	_lastProcessTime(0.0),
 	_someNodesHaveBeenTransferred(false),
 	_distanceTravelled(0.0f),
+	_optimizeFromGraphEndChanged(false),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
 	_graphOptimizer(0),
@@ -375,6 +376,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
 	_distanceTravelled = 0.0f;
+	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
 	_gpsGeocentricCache.clear();
 	_currentSessionHasGPS = false;
@@ -478,7 +480,12 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_proximityAngle *= M_PI/180.0f;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
+	bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
+	if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd)
+	{
+		_optimizeFromGraphEndChanged = true;
+	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
 	if(_optimizationMaxError > 0.0 && _optimizationMaxError < 1.0)
 	{
@@ -887,6 +894,7 @@ void Rtabmap::resetMemory()
 	_odomCachePoses.clear();
 	_odomCacheConstraints.clear();
 	_distanceTravelled = 0.0f;
+	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
 
 	if(_memory)
@@ -2874,23 +2882,12 @@ bool Rtabmap::process(
 			std::map<int, Transform> poses = _optimizedPoses;
 
 			// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
-			float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
+			if(_optimizeFromGraphEndChanged)
-			if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) ||
-				(normMapCorrection < 0.001f && !_optimizeFromGraphEnd))
 			{
-				for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
+				UWARN("Optimization: clearing guess poses as %s has changed state, now %s",
-				{
+						Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false");
-					if( iter->second.type() != Link::kNeighbor &&
+				poses.clear();
-						iter->second.type() != Link::kVirtualClosure &&
+				_optimizeFromGraphEndChanged = false;
-						iter->second.type() != Link::kLandmark &&
-						iter->second.type() != Link::kGravity &&
-						iter->second.type() != Link::kPosePrior)
-					{
-						UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection);
-						poses.clear();
-						break;
-					}
-				}
 			}
 
 			std::multimap<int, Link> constraints;

corelib/src/camera/CameraMyntEye.cpp

@@ -45,13 +45,17 @@ CameraMyntEye::CameraMyntEye(const std::string & device, bool apiRectification,
 			deviceName_(device),
 			apiRectification_(apiRectification),
 			apiDepth_(apiDepth),
+			autoExposure_(true),
+			gain_(24),
+			brightness_(120),
+			contrast_(116),
 			dataReady_(0),
 			lastFramesStamp_(0.0),
 			stamp_(0),
 			publishInterIMU_(false),
 			softTimeBegin_(0.0),
 			hardTimeBegin_(0),
-			unitHardTime(std::numeric_limits<std::uint32_t>::max()*10)
+			unitHardTime_(std::numeric_limits<std::uint32_t>::max()*10)
 #endif
 {
 #ifdef RTABMAP_MYNTEYE
@@ -133,13 +137,13 @@ inline bool is_overflow(std::uint64_t now, std::uint64_t pre, std::uint64_t unit
 
 double CameraMyntEye::checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream) {
 	UASSERT(stream < (std::uint8_t)mynteye::Stream::LAST+1);
-	if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime)) {
+	if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime_)) {
 		acc_[stream]++;
 	}
 
 	lastHardTimes_[stream] = _hard_time;
 
-	return hardTimeToSoftTime(acc_[stream] * unitHardTime + _hard_time);
+	return hardTimeToSoftTime(acc_[stream] * unitHardTime_ + _hard_time);
 }
 #endif
 
@@ -150,6 +154,16 @@ void CameraMyntEye::publishInterIMU(bool enabled)
 #endif
 }
 
+void CameraMyntEye::setAutoExposure(bool enabled, int manualGain, int manualBrightness, int manualConstrast)
+{
+#ifdef RTABMAP_MYNTEYE
+	autoExposure_ = enabled;
+	gain_ = manualGain;
+	brightness_ = manualBrightness;
+	contrast_ = manualConstrast;
+#endif
+}
+
 bool CameraMyntEye::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 #ifdef RTABMAP_MYNTEYE
@@ -430,6 +444,14 @@ bool CameraMyntEye::init(const std::string & calibrationFolder, const std::strin
 
 		});
 
+		api_->SetOptionValue(mynteye::Option::EXPOSURE_MODE, autoExposure_?0:1);
+		if(!autoExposure_)
+		{
+			api_->SetOptionValue(mynteye::Option::GAIN, gain_);
+			api_->SetOptionValue(mynteye::Option::BRIGHTNESS, brightness_);
+			api_->SetOptionValue(mynteye::Option::CONTRAST, contrast_);
+		}
+
 		api_->Start(mynteye::Source::ALL);
 		uSleep(500); // To buffer some imus before sending images
 		return true;

guilib/src/PreferencesDialog.cpp

@@ -733,6 +733,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->checkbox_stereoMyntEye_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_stereoMyntEye_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_stereoMyntEye_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_brightness, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_stereoMyntEye_contrast, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1910,6 +1914,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkbox_stereoRealSense2_odom->setChecked(false);
 		_ui->checkbox_stereoMyntEye_rectify->setChecked(false);
 		_ui->checkbox_stereoMyntEye_depth->setChecked(false);
+		_ui->checkbox_stereoMyntEye_autoExposure->setChecked(true);
+		_ui->spinBox_stereoMyntEye_gain->setValue(24);
+		_ui->spinBox_stereoMyntEye_brightness->setValue(120);
+		_ui->spinBox_stereoMyntEye_contrast->setValue(116);
 
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
@@ -2358,7 +2366,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("MyntEye");
 	_ui->checkbox_stereoMyntEye_rectify->setChecked(settings.value("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()).toBool());
 	_ui->checkbox_stereoMyntEye_depth->setChecked(settings.value("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()).toBool());
-	settings.endGroup(); // StereoRealSense2
+	_ui->checkbox_stereoMyntEye_autoExposure->setChecked(settings.value("auto_exposure", _ui->checkbox_stereoMyntEye_autoExposure->isChecked()).toBool());
+	_ui->spinBox_stereoMyntEye_gain->setValue(settings.value("gain", _ui->spinBox_stereoMyntEye_gain->value()).toInt());
+	_ui->spinBox_stereoMyntEye_brightness->setValue(settings.value("brightness", _ui->spinBox_stereoMyntEye_brightness->value()).toInt());
+	_ui->spinBox_stereoMyntEye_contrast->setValue(settings.value("contrast", _ui->spinBox_stereoMyntEye_contrast->value()).toInt());
+	settings.endGroup(); // MyntEye
 
 	settings.beginGroup("Images");
 	_ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString());
@@ -2828,8 +2840,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.endGroup(); // StereoRealSense2
 
 	settings.beginGroup("MyntEye");
-	settings.setValue("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked());
+	settings.setValue("rectify",       _ui->checkbox_stereoMyntEye_rectify->isChecked());
-	settings.setValue("depth", _ui->checkbox_stereoMyntEye_depth->isChecked());
+	settings.setValue("depth",         _ui->checkbox_stereoMyntEye_depth->isChecked());
+	settings.setValue("auto_exposure", _ui->checkbox_stereoMyntEye_autoExposure->isChecked());
+	settings.setValue("gain",          _ui->spinBox_stereoMyntEye_gain->value());
+	settings.setValue("brightness",    _ui->spinBox_stereoMyntEye_brightness->value());
+	settings.setValue("contrast",      _ui->spinBox_stereoMyntEye_contrast->value());
 	settings.endGroup(); // MyntEye
 
 	settings.beginGroup("Images");
@@ -5678,6 +5694,11 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 			((CameraMyntEye*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
+			((CameraMyntEye*)camera)->setAutoExposure(
+					_ui->checkbox_stereoMyntEye_autoExposure->isChecked(),
+					_ui->spinBox_stereoMyntEye_gain->value(),
+					_ui->spinBox_stereoMyntEye_brightness->value(),
+					_ui->spinBox_stereoMyntEye_contrast->value());
 		}
 	}
 	else if(driver == kSrcRGBDImages)

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-687</y>
             <width>680</width>
-            <height>2107</height>
+            <height>3270</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>28</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -5339,6 +5339,98 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="4" column="0">
+                                   <widget class="QSpinBox" name="spinBox_stereoMyntEye_brightness">
+                                    <property name="maximum">
+                                     <number>240</number>
+                                    </property>
+                                    <property name="value">
+                                     <number>120</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="0">
+                                   <widget class="QSpinBox" name="spinBox_stereoMyntEye_gain">
+                                    <property name="maximum">
+                                     <number>48</number>
+                                    </property>
+                                    <property name="value">
+                                     <number>24</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="0">
+                                   <widget class="QCheckBox" name="checkbox_stereoMyntEye_autoExposure">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="1">
+                                   <widget class="QLabel" name="label_595">
+                                    <property name="text">
+                                     <string>Image gain, valid if manual-exposure.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLabel" name="label_596">
+                                    <property name="text">
+                                     <string>Image brightness, valid if manual-exposure.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="1">
+                                   <widget class="QLabel" name="label_594">
+                                    <property name="text">
+                                     <string>Auto-exposure.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="1">
+                                   <widget class="QLabel" name="label_597">
+                                    <property name="text">
+                                     <string>Image contrast, valid if manual-exposure.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="0">
+                                   <widget class="QSpinBox" name="spinBox_stereoMyntEye_contrast">
+                                    <property name="maximum">
+                                     <number>254</number>
+                                    </property>
+                                    <property name="value">
+                                     <number>116</number>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>