ZED driver: added option to use visual odometry approach from zed sdk. RtabmapThread: fixed thread state change on new map trigger on Odometry init (variance=9999). Odometry: on init, verify that the first frame is ok before sending first pose. Parameters: Mem/SaveDepth16Format is now false by default

corelib/include/rtabmap/core/Camera.h

@@ -56,6 +56,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
 
 	//getters
 	float getImageRate() const {return _imageRate;}

corelib/include/rtabmap/core/CameraStereo.h

@@ -118,6 +118,7 @@ public:
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 1,// 0=FULL, 1=RAW
 			int confidenceThr = 100,
+			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoZed(
@@ -125,6 +126,7 @@ public:
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 1,// 0=FULL, 1=RAW
 			int confidenceThr = 100,
+			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoZed();
@@ -132,6 +134,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return computeOdometry_; }
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -146,6 +149,8 @@ private:
 	int quality_;
 	int sensingMode_;
 	int confidenceThr_;
+	bool computeOdometry_;
+	bool lost_;
 };
 
 /////////////////////////

corelib/include/rtabmap/core/DBReader.h

@@ -67,8 +67,7 @@ public:
 
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-
+	virtual bool odometryProvided() const {return !_odometryIgnored;}
-	bool isOdometryIgnored() const {return _odometryIgnored;}
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);

corelib/include/rtabmap/core/Parameters.h

@@ -194,7 +194,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
 	RTABMAP_PARAM(Mem, RawDescriptorsKept, 		bool, true, 	"Raw descriptors kept in memory.");
 	RTABMAP_PARAM(Mem, MapLabelsAdded, 		    bool, true, 	"Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
-	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, false, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory,       bool, true, 	"SLAM mode, otherwise it is Localization mode.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -70,6 +70,7 @@ public:
 	void close(bool databaseSaved = true);
 
 	const std::string & getWorkingDir() const {return _wDir;}
+	bool isRGBDMode() const { return _rgbdSlamMode; }
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
 	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
 	int getHighestHypothesisId() const {return _highestHypothesis.first;}

corelib/src/CameraStereo.cpp

@@ -753,6 +753,7 @@ CameraStereoZed::CameraStereoZed(
 		int quality,
 		int sensingMode,
 		int confidenceThr,
+		bool computeOdometry,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -763,7 +764,9 @@ CameraStereoZed::CameraStereoZed(
 	resolution_(resolution),
 	quality_(quality),
 	sensingMode_(sensingMode),
-	confidenceThr_(confidenceThr)
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
 {
 #ifdef RTABMAP_ZED
 	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
@@ -778,6 +781,7 @@ CameraStereoZed::CameraStereoZed(
 		int quality,
 		int sensingMode,
 		int confidenceThr,
+		bool computeOdometry,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -788,7 +792,9 @@ CameraStereoZed::CameraStereoZed(
 	resolution_(2),
 	quality_(quality),
 	sensingMode_(sensingMode),
-	confidenceThr_(confidenceThr)
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
 {
 #ifdef RTABMAP_ZED
 	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
@@ -817,7 +823,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		zed_ = 0;
 	}
 	
-
+	lost_ = true;
 	if(src_ == CameraVideo::kVideoFile)
 	{
 		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
@@ -858,6 +864,13 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 
 	zed_->setConfidenceThreshold(confidenceThr_);
 
+	if (computeOdometry_)
+	{
+		Eigen::Matrix4f initPose;
+		initPose.setIdentity(4, 4);
+		zed_->enableTracking(initPose, false);
+	}
+
 	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
 	sl::zed::resolution res = zed_->getImageSize();
 				
@@ -931,6 +944,40 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 			
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
 			}
+
+			if (computeOdometry_ && info)
+			{
+				Eigen::Matrix4f path;
+				int trackingConfidence = zed_->getTrackingConfidence();
+				if (trackingConfidence)
+				{
+					sl::zed::TRACKING_STATE track_state = zed_->getPosition(path);
+					info->odomPose = Transform::fromEigen4f(path);
+					if (!info->odomPose.isNull())
+					{
+						//transform x->forward, y->left, z->up
+						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+						info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
+					}
+					if (lost_)
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+						lost_ = false;
+						UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+					}
+					else
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+						UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+					}
+				}
+				else
+				{
+					info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+					lost_ = true;
+					UWARN("ZED lost!");
+				}
+			}
 		}
 		else if(src_ == CameraVideo::kUsbDevice)
 		{

corelib/src/OdometryF2F.cpp

@@ -176,6 +176,12 @@ Transform OdometryF2F::computeTransform(
 			}
 			else
 			{
+				if (!refFrame_.sensorData().isValid())
+				{
+					// Don't send odometry if we don't have a keyframe yet
+					output.setNull();
+				}
+
 				if(features < registrationPipeline_->getMinVisualCorrespondences())
 				{
 					UWARN("Too low 2D features (%d), keeping last key frame...", features);

corelib/src/OdometryF2M.cpp

@@ -419,51 +419,75 @@ Transform OdometryF2M::computeTransform(
 
 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
 
-			if(fixedMapPath_.empty())
+			// a very high variance tells that the new pose is not linked with the previous one
+			regInfo.variance = 9999;
+
+			bool frameValid = false;
+			Transform newFramePose = this->getPose(); // initial pose may be not identity...
+			if(regPipeline_->isImageRequired())
 			{
-				output.setIdentity();
+				if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
-				// a very high variance tells that the new pose is not linked with the previous one
-				regInfo.variance = 9999;
-
-				Transform newFramePose = this->getPose(); // initial pose may be not identity...
-				if(regPipeline_->isImageRequired() &&
-				   (int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
 				{
-					// update local map
+					frameValid = true;
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					if (fixedMapPath_.empty())
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-
-					std::multimap<int, cv::KeyPoint> words;
-					std::multimap<int, cv::Point3f> transformedPoints;
-					std::multimap<int, cv::Mat> descriptors;
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
-					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-							iter!=lastFrame_->getWords3().end();
-							++iter,++descIter,++wordsIter)
 					{
-						if(util3d::isFinite(iter->second))
+						// update local map
-						{
+						UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
-							words.insert(*wordsIter);
+						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
-							descriptors.insert(*descIter);
-						}
-					}
-					map_->setWords(words);
-					map_->setWords3(transformedPoints);
-					map_->setWordsDescriptors(descriptors);
 
-					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
+						std::multimap<int, cv::KeyPoint> words;
-					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+						std::multimap<int, cv::Point3f> transformedPoints;
+						std::multimap<int, cv::Mat> descriptors;
+						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
+						std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
+						std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
+						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+							iter != lastFrame_->getWords3().end();
+							++iter, ++descIter, ++wordsIter)
+						{
+							if (util3d::isFinite(iter->second))
+							{
+								words.insert(*wordsIter);
+								transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
+								descriptors.insert(*descIter);
+							}
+						}
+						map_->setWords(words);
+						map_->setWords3(transformedPoints);
+						map_->setWordsDescriptors(descriptors);
+
+						map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
+						map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+					}
 				}
-				if(regPipeline_->isScanRequired())
+				else
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+					UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
-					scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
-					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
 				}
 			}
+			if(regPipeline_->isScanRequired())
+			{
+				if (lastFrame_->sensorData().laserScanRaw().cols)
+				{
+					frameValid = true;
+					if (fixedMapPath_.empty())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+						scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0, 0);
+					}
+				}
+				else
+				{
+					UWARN("Mising scan to initialize odometry.");
+				}
+			}
+
+			if (frameValid)
+			{
+				// We initialized the local map
+				output.setIdentity();
+			}
 
 			if(info)
 			{

corelib/src/RtabmapThread.cpp

@@ -333,7 +333,22 @@ void RtabmapThread::handleEvent(UEvent* event)
 			CameraEvent * e = (CameraEvent*)event;
 			if(e->getCode() == CameraEvent::kCodeData)
 			{
-				this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
+				if (_rtabmap->isRGBDMode())
+				{
+					if (!e->info().odomPose.isNull())
+					{
+						this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
+					}
+					else
+					{
+						lastPose_.setNull();
+					}
+				}
+				else
+				{ 
+					this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
+				}
+				
 			}
 		}
 		else if(event->getClassName().compare("OdometryEvent") == 0)
@@ -538,21 +553,22 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
+		UScopeMutex scopeMutex(_dataMutex);
+
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
 			if((_previousStamp>0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
-			   ((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
+				((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
 			{
 				ignoreFrame = true;
 			}
 		}
-		if(_dataBufferMaxSize > 0 &&
+		if(!lastPose_.isIdentity() &&
-				(!lastPose_.isIdentity() &&
+						(odomEvent.pose().isIdentity() ||
-					   (odomEvent.pose().isIdentity() ||
 						odomEvent.info().variance>=9999 ||
 						odomEvent.rotVariance()>=9999 ||
-						odomEvent.transVariance()>=9999)))
+						odomEvent.transVariance()>=9999))
 		{
 			UWARN("Odometry is reset (identity pose or high variance >=9999 detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
@@ -585,40 +601,37 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		lastPose_ = odomEvent.pose();
 
 		bool notify = true;
-		_dataMutex.lock();
-		{
-			if(_rotVariance <= 0)
-			{
-				_rotVariance = 1.0;
-			}
-			if(_transVariance <= 0)
-			{
-				_transVariance = 1.0;
-			}
-			if(ignoreFrame)
-			{
-				// set negative id so rtabmap will detect it as an intermediate node
-				SensorData tmp = odomEvent.data();
-				tmp.setId(-1);
-				tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
-				_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
-			}
-			else
-			{
-				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
-			}
-			UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
 		
-			_rotVariance = 0;
+		if(_rotVariance <= 0)
-			_transVariance = 0;
+		{
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+			_rotVariance = 1.0;
-			{
+		}
-				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+		if(_transVariance <= 0)
-				_dataBuffer.pop_front();
+		{
-				notify = false;
+			_transVariance = 1.0;
-			}
+		}
+		if(ignoreFrame)
+		{
+			// set negative id so rtabmap will detect it as an intermediate node
+			SensorData tmp = odomEvent.data();
+			tmp.setId(-1);
+			tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
+			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
+		}
+		else
+		{
+			_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
+		}
+		UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
+
+		_rotVariance = 0;
+		_transVariance = 0;
+		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+		{
+			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+			_dataBuffer.pop_front();
+			notify = false;
 		}
-		_dataMutex.unlock();
 
 		if(notify)
 		{
@@ -636,9 +649,10 @@ bool RtabmapThread::getData(OdometryEvent & data)
 	ULOGGER_INFO("wake-up");
 
 	bool dataFilled = false;
+
 	_dataMutex.lock();
 	{
-		if(!_dataBuffer.empty())
+		if(_state.empty() && !_dataBuffer.empty())
 		{
 			data = _dataBuffer.front();
 			_dataBuffer.pop_front();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -224,6 +224,7 @@ public:
 	float getTimeLimit() const;
 	float getDetectionRate() const;
 	bool isSLAMMode() const;
+	bool isRGBDMode() const;
 
 	//specific
 	bool isStatisticsPublished() const;
@@ -270,6 +271,7 @@ private slots:
 	void updatePredictionPlot();
 	void updateKpROI();
 	void updateG2oVisibility();
+	void updateStereoDisparityVisibility();
 	void useOdomFeatures();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();

guilib/src/MainWindow.cpp

@@ -761,6 +761,25 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		else
 		{
 			emit cameraInfoReceived(cameraEvent->info());
+			if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode())
+			{
+				if (!_processingOdometry && !_processingStatistics)
+				{
+					_processingOdometry = true; // if we receive too many odometry events!
+					OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
+					emit odometryReceived(tmp, false);
+				}
+				else
+				{
+					// we receive too many odometry events! just send without data
+					SensorData data(cv::Mat(), cameraEvent->data().id(), cameraEvent->data().stamp());
+					data.setCameraModels(cameraEvent->data().cameraModels());
+					data.setStereoCameraModel(cameraEvent->data().stereoCameraModel());
+					data.setGroundTruth(cameraEvent->data().groundTruth());
+					OdometryEvent tmp(data, cameraEvent->info().odomPose, cameraEvent->info().odomCovariance);
+					emit odometryReceived(tmp, true);
+				}
+			}
 		}
 	}
 	else if(anEvent->getClassName().compare("OdometryEvent") == 0)
@@ -3861,8 +3880,7 @@ void MainWindow::startDetection()
 				_odomThread = 0;
 			}
 
-			DBReader* dbReader = dynamic_cast<DBReader*>(camera);
+			if(!camera->odomProvided())
-			if(dbReader == 0 || dbReader->isOdometryIgnored())
 			{
 				Odometry * odom = Odometry::create(parameters);
 				_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());

guilib/src/PreferencesDialog.cpp

@@ -485,8 +485,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->comboBox_stereoZed_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_stereoZed_quality, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_stereoZed_quality, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoDisparityVisibility()));
+	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoDisparityVisibility()));
 	connect(_ui->comboBox_stereoZed_sensingMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_stereoZed_confidenceThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_stereoZed_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_zedSvoPath, SIGNAL(clicked()), this, SLOT(selectSourceSvoPath()));
 	connect(_ui->lineEdit_zedSvoPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -1312,6 +1315,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_stereoZed_quality->setCurrentIndex(1);
 		_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(1);
 		_ui->spinBox_stereoZed_confidenceThr->setValue(100);
+		_ui->checkbox_stereoZed_odom->setChecked(false);
 		_ui->lineEdit_zedSvoPath->clear();
 
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
@@ -1654,6 +1658,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_stereoZed_quality->setCurrentIndex(settings.value("quality", _ui->comboBox_stereoZed_quality->currentIndex()).toInt());
 	_ui->comboBox_stereoZed_sensingMode->setCurrentIndex(settings.value("sensing_mode", _ui->comboBox_stereoZed_sensingMode->currentIndex()).toInt());
 	_ui->spinBox_stereoZed_confidenceThr->setValue(settings.value("confidence_thr", _ui->spinBox_stereoZed_confidenceThr->value()).toInt());
+	_ui->checkbox_stereoZed_odom->setChecked(settings.value("odom", _ui->checkbox_stereoZed_odom->isChecked()).toBool());
 	_ui->lineEdit_zedSvoPath->setText(settings.value("svo_path", _ui->lineEdit_zedSvoPath->text()).toString());
 	settings.endGroup(); // StereoZed
 	
@@ -2062,6 +2067,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("quality", _ui->comboBox_stereoZed_quality->currentIndex());
 	settings.setValue("sensing_mode", _ui->comboBox_stereoZed_sensingMode->currentIndex());
 	settings.setValue("confidence_thr", _ui->spinBox_stereoZed_confidenceThr->value());
+	settings.setValue("odom", _ui->checkbox_stereoZed_odom->isChecked());
 	settings.setValue("svo_path", _ui->lineEdit_zedSvoPath->text());
 	settings.endGroup(); // StereoZed
 
@@ -3447,6 +3453,17 @@ void PreferencesDialog::updateG2oVisibility()
 	_ui->groupBox_g2o->setVisible(_ui->graphOptimization_type->currentIndex() == 1);
 }
 
+void PreferencesDialog::updateStereoDisparityVisibility()
+{
+	Src driver = this->getSourceDriver();
+	_ui->checkbox_stereo_depthGenerated->setVisible(
+		driver != PreferencesDialog::kSrcStereoZed ||
+		_ui->comboBox_stereoZed_quality->currentIndex() == 0);
+	_ui->label_stereo_depthGenerated->setVisible(
+		driver != PreferencesDialog::kSrcStereoZed ||
+		_ui->comboBox_stereoZed_quality->currentIndex() == 0);
+}
+
 void PreferencesDialog::useOdomFeatures()
 {
 	if(this->isVisible() && _ui->checkBox_useOdomFeatures->isChecked())
@@ -4105,6 +4122,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 				_ui->comboBox_stereoZed_quality->currentIndex(),
 				_ui->comboBox_stereoZed_sensingMode->currentIndex(),
 				_ui->spinBox_stereoZed_confidenceThr->value(),
+				_ui->checkbox_stereoZed_odom->isChecked(),
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 		}
@@ -4116,6 +4134,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 				_ui->comboBox_stereoZed_quality->currentIndex(),
 				_ui->comboBox_stereoZed_sensingMode->currentIndex(),
 				_ui->spinBox_stereoZed_confidenceThr->value(),
+				_ui->checkbox_stereoZed_odom->isChecked(),
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 		}
@@ -4285,6 +4304,10 @@ bool PreferencesDialog::isSLAMMode() const
 {
 	return _ui->general_checkBox_SLAM_mode->isChecked();
 }
+bool PreferencesDialog::isRGBDMode() const
+{
+	return _ui->general_checkBox_activateRGBD->isChecked();
+}
 
 /*** SETTERS ***/
 void PreferencesDialog::setInputRate(double value)

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-360</y>
+            <y>0</y>
-            <width>686</width>
+            <width>685</width>
-            <height>2023</height>
+            <height>1826</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -2316,7 +2316,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>3</number>
+                       <number>1</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3104,7 +3104,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </widget>
                              </item>
                              <item row="1" column="2">
-                              <widget class="QLabel" name="label_247">
+                              <widget class="QLabel" name="label_stereo_depthGenerated">
                                <property name="text">
                                 <string>Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can found under StereoBM tab.</string>
                                </property>
@@ -3340,37 +3340,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>Zed sdk</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
-                                  <item row="2" column="0">
+                                  <item row="6" column="0">
-                                   <widget class="QComboBox" name="comboBox_stereoZed_sensingMode">
+                                   <spacer name="verticalSpacer_54">
-                                    <item>
+                                    <property name="orientation">
-                                     <property name="text">
+                                     <enum>Qt::Vertical</enum>
-                                      <string>FILL</string>
+                                    </property>
-                                     </property>
+                                    <property name="sizeHint" stdset="0">
-                                    </item>
+                                     <size>
-                                    <item>
+                                      <width>20</width>
-                                     <property name="text">
+                                      <height>13</height>
-                                      <string>STANDARD</string>
+                                     </size>
-                                     </property>
+                                    </property>
-                                    </item>
+                                   </spacer>
-                                   </widget>
                                   </item>
-                                  <item row="4" column="0">
+                                  <item row="5" column="1">
-                                   <layout class="QHBoxLayout" name="horizontalLayout_8">
+                                   <widget class="QLabel" name="label_305">
-                                    <item>
+                                    <property name="text">
-                                     <widget class="QToolButton" name="toolButton_zedSvoPath">
+                                     <string>Path to a *.SVO file.</string>
-                                      <property name="text">
+                                    </property>
-                                       <string>...</string>
+                                    <property name="wordWrap">
-                                      </property>
+                                     <bool>true</bool>
-                                     </widget>
+                                    </property>
-                                    </item>
+                                    <property name="textInteractionFlags">
-                                    <item>
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                     <widget class="QLineEdit" name="lineEdit_zedSvoPath">
+                                    </property>
-                                      <property name="text">
+                                   </widget>
-                                       <string/>
-                                      </property>
-                                     </widget>
-                                    </item>
-                                   </layout>
                                   </item>
                                   <item row="0" column="0">
                                    <widget class="QComboBox" name="comboBox_stereoZed_resolution">
@@ -3396,10 +3390,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </item>
                                    </widget>
                                   </item>
-                                  <item row="0" column="1">
+                                  <item row="2" column="1">
-                                   <widget class="QLabel" name="label_274">
+                                   <widget class="QLabel" name="label_291">
                                     <property name="text">
-                                     <string>Resolution. Not used when a SVO file is used.</string>
+                                     <string>Sensing mode.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -3409,6 +3403,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="2" column="0">
+                                   <widget class="QComboBox" name="comboBox_stereoZed_sensingMode">
+                                    <item>
+                                     <property name="text">
+                                      <string>FILL</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>STANDARD</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
                                   <item row="1" column="0">
                                    <widget class="QComboBox" name="comboBox_stereoZed_quality">
                                     <item>
@@ -3433,58 +3441,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </item>
                                    </widget>
                                   </item>
-                                  <item row="1" column="1">
-                                   <widget class="QLabel" name="label_290">
-                                    <property name="text">
-                                     <string>Quality. If NONE, the disparity is not computed on the GPU.</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_291">
-                                    <property name="text">
-                                     <string>Sensing mode.</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_305">
-                                    <property name="text">
-                                     <string>Path to a *.SVO file.</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">
-                                   <spacer name="verticalSpacer_54">
-                                    <property name="orientation">
-                                     <enum>Qt::Vertical</enum>
-                                    </property>
-                                    <property name="sizeHint" stdset="0">
-                                     <size>
-                                      <width>20</width>
-                                      <height>13</height>
-                                     </size>
-                                    </property>
-                                   </spacer>
-                                  </item>
                                   <item row="3" column="1">
                                    <widget class="QLabel" name="label_306">
                                     <property name="text">
@@ -3498,6 +3454,50 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="0" column="1">
+                                   <widget class="QLabel" name="label_274">
+                                    <property name="text">
+                                     <string>Resolution. Not used when a SVO file is used.</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">
+                                   <layout class="QHBoxLayout" name="horizontalLayout_8">
+                                    <item>
+                                     <widget class="QToolButton" name="toolButton_zedSvoPath">
+                                      <property name="text">
+                                       <string>...</string>
+                                      </property>
+                                     </widget>
+                                    </item>
+                                    <item>
+                                     <widget class="QLineEdit" name="lineEdit_zedSvoPath">
+                                      <property name="text">
+                                       <string/>
+                                      </property>
+                                     </widget>
+                                    </item>
+                                   </layout>
+                                  </item>
+                                  <item row="1" column="1">
+                                   <widget class="QLabel" name="label_290">
+                                    <property name="text">
+                                     <string>Quality. If NONE, the disparity is not computed on the GPU.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                   <item row="3" column="0">
                                    <widget class="QSpinBox" name="spinBox_stereoZed_confidenceThr">
                                     <property name="maximum">
@@ -3505,6 +3505,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLabel" name="label_316">
+                                    <property name="text">
+                                     <string>Use ZED visual odometry.</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="0">
+                                   <widget class="QCheckBox" name="checkbox_stereoZed_odom">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>