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((normMapCorrection > 0.001f && _optimizeFromGraphEnd) ||
-				(normMapCorrection < 0.001f && !_optimizeFromGraphEnd))
+			if(_optimizeFromGraphEndChanged)
 			{
-				for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
-				{
-					if( iter->second.type() != Link::kNeighbor &&
-						iter->second.type() != Link::kVirtualClosure &&
-						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;
-					}
-				}
+				UWARN("Optimization: clearing guess poses as %s has changed state, now %s",
+						Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false");
+				poses.clear();
+				_optimizeFromGraphEndChanged = false;
 			}
 
 			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;