L515 refactoring (realsense v2.41.0, firmware 1.5.3): added IR-only mode support, fixed support with latest firmware, T265+L515 working, related to #574 #614 #629. MainWindow: Selecting RealSense2, ZED sdk, K4A, Mynteye drivers automatically enable gravity optimization (with IMU filtering).

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

@@ -75,6 +75,7 @@ public:
 	void setEmitterEnabled(bool enabled);
 	void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
 	void setResolution(int width, int height, int fps = 30);
+	void setDepthResolution(int width, int height, int fps = 30);
 	void setGlobalTimeSync(bool enabled);
 	void publishInterIMU(bool enabled);
 	void setDualMode(bool enabled, const Transform & extrinsics);
@@ -132,13 +133,15 @@ private:
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;
+	int cameraDepthWidth_;
+	int cameraDepthHeight_;
+	int cameraDepthFps_;
 	bool globalTimeSync_;
 	bool publishInterIMU_;
 	bool dualMode_;
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
 	bool closing_;
-	bool isL500_;
 
 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;

corelib/src/camera/CameraRealSense2.cpp

@@ -77,11 +77,13 @@ CameraRealSense2::CameraRealSense2(
 	cameraWidth_(640),
 	cameraHeight_(480),
 	cameraFps_(30),
+	cameraDepthWidth_(640),
+	cameraDepthHeight_(480),
+	cameraDepthFps_(30),
 	globalTimeSync_(true),
 	publishInterIMU_(false),
 	dualMode_(false),
-	closing_(false),
+	closing_(false)
-	isL500_(false)
 #endif
 {
 	UDEBUG("");
@@ -283,13 +285,30 @@ void CameraRealSense2::getPoseAndIMU(
 				pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
 				poseConfidence = iterA->second.second;
 			}
-			else if(stamp < iterA->first)
+			else
 			{
-				UWARN("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+				if(!imuGlobalSyncWarningShown_)
+				{
+					if(stamp < iterA->first)
+					{
+						UWARN("Could not find pose data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
 					}
 					else
 					{
-				UWARN("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", stamp, iterA->first, iterB->first);
+						UWARN("Could not find pose data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+					}
+				}
+				if(!globalTimeSync_)
+				{
+					if(!imuGlobalSyncWarningShown_)
+					{
+						UWARN("As globalTimeSync option is off, the received pose, gyro and accelerometer will be re-stamped with image time. This message is only shown once.");
+						imuGlobalSyncWarningShown_ = true;
+					}
+					std::map<double, std::pair<Transform, unsigned int> >::const_reverse_iterator iterC = poseBuffer_.rbegin();
+					pose = iterC->second.first;
+					poseConfidence = iterC->second.second;
+				}
 			}
 		}
 		poseMutex_.unlock();
@@ -606,7 +625,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UINFO("Device Sensors: ");
 	std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth 2=(pose in dualMode_)
 	bool stereo = false;
-	isL500_ = false;
+	bool isL500 = false;
 	for(auto&& elem : dev_sensors)
 	{
 		std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
@@ -654,7 +673,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		else if ("L500 Depth Sensor" == module_name)
 		{
 			sensors[1] = elem;
-			isL500_ = true;
+			isL500 = true;
+			if(ir_)
+			{
+				cameraWidth_ = cameraDepthWidth_;
+				cameraHeight_ = cameraDepthHeight_;
+				cameraFps_ = cameraDepthFps_;
+			}
+			irDepth_ = true;
 		}
 		else
 		{
@@ -680,7 +706,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		auto profiles = sensors[i].get_stream_profiles();
 		bool added = false;
 		UINFO("profiles=%d", (int)profiles.size());
-		if(ULogger::level()>=ULogger::kInfo)
+		if(ULogger::level()<ULogger::kWarning)
 		{
 			for (auto& profile : profiles)
 			{
@@ -701,44 +727,19 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			auto video_profile = profile.as<rs2::video_stream_profile>();
 			if(!stereo)
 			{
-				if(isL500_ &&
+				if( (video_profile.width()  == cameraWidth_ &&
-						(video_profile.width()  == 640 &&
-						 video_profile.height() == 480 &&
-						 video_profile.fps()    == 30))
-				{
-					if( i==0 // rgb
-						&& video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR)
-					{
-						auto intrinsic = video_profile.get_intrinsics();
-						profilesPerSensor[i].push_back(profile);
-						rgbBuffer_ = cv::Mat(cv::Size(video_profile.width(), video_profile.height()), CV_8UC3, cv::Scalar(0, 0, 0));
-						model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
-						rgbStreamProfile = profile;
-						*rgbIntrinsics_ = intrinsic;
-						added = true;
-					}
-					else if( i==1 // depth
-						 && video_profile.format() == RS2_FORMAT_Z16 && video_profile.stream_type() == RS2_STREAM_DEPTH)
-					{
-						auto intrinsic = video_profile.get_intrinsics();
-						profilesPerSensor[i].push_back(profile);
-						depthBuffer_ = cv::Mat(cv::Size(video_profile.width(), video_profile.height()), CV_16UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						*depthIntrinsics_ = intrinsic;
-						added = true;
-					}
-				}
-				//D400 series:
-				else if (!isL500_ &&
-						(video_profile.width()  == cameraWidth_ &&
 					 video_profile.height() == cameraHeight_ &&
-						 video_profile.fps()    == cameraFps_))
+					 video_profile.fps()    == cameraFps_) ||
+						(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
+							video_profile.width()  == cameraDepthWidth_ &&
+							video_profile.height() == cameraDepthHeight_ &&
+							video_profile.fps()    == cameraDepthFps_))
 				{
 					auto intrinsic = video_profile.get_intrinsics();
 
 					// rgb or ir left
 					if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
-					  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 1))
+					  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && (video_profile.stream_index() == 1 || isL500)))
 					{
 						if(!profilesPerSensor[i].empty())
 						{
@@ -752,6 +753,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 						}
 						rgbBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_8UC3, ir_?cv::Scalar(0):cv::Scalar(0, 0, 0));
 						model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
+						UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f %f",
+								intrinsic.width, intrinsic.height,
+								intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
+								intrinsic.model,
+								intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
 						rgbStreamProfile = profile;
 						*rgbIntrinsics_ = intrinsic;
 						added = true;
@@ -875,6 +881,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 if(!model_.isValidForProjection())
 		 {
 			 UERROR("Calibration info not valid!");
+			 std::cout<< model_ << std::endl;
 			 return false;
 		 }
 		 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
@@ -1133,6 +1140,15 @@ void CameraRealSense2::setResolution(int width, int height, int fps)
 #endif
 }
 
+void CameraRealSense2::setDepthResolution(int width, int height, int fps)
+{
+#ifdef RTABMAP_REALSENSE2
+	cameraDepthWidth_ = width;
+	cameraDepthHeight_ = height;
+	cameraDepthFps_ = fps;
+#endif
+}
+
 void CameraRealSense2::setGlobalTimeSync(bool enabled)
 {
 #ifdef RTABMAP_REALSENSE2
@@ -1195,8 +1211,6 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 		auto frameset = syncer_->wait_for_frames(5000);
 		UTimer timer;
 		int desiredFramesetSize = 2;
-		if(isL500_ && globalTimeSync_)
-			desiredFramesetSize = 3;
 		while ((int)frameset.size() != desiredFramesetSize && timer.elapsed() < 2.0)
 		{
 			// maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
@@ -1222,15 +1236,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				auto stream_type = f.get_profile().stream_type();
 				if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
 				{
-					if(isL500_)
+					if(ir_ && !irDepth_)
-					{
-						if(stream_type == RS2_STREAM_COLOR)
-						{
-							rgb_frame = f;
-							is_rgb_arrived = true;
-						}
-					} 
-					else if(ir_ && !irDepth_)
 					{
 						//stereo D435
 						if(!is_depth_arrived)
@@ -1422,10 +1428,6 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				lastImuStamp_ = imuStamp;
 			}
 		}
-		else if(isL500_ && globalTimeSync_)
-		{
-			UERROR("Missing frames (received %d, needed=%d). L500 camera is used and global time sync is enabled, try disabling global time sync for the RealSense2 driver.", (int)frameset.size(), desiredFramesetSize);
-		}
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);

guilib/include/rtabmap/gui/MainWindow.h

@@ -175,6 +175,7 @@ protected Q_SLOTS:
 	void selectK4A();
 	void selectRealSense();
 	void selectRealSense2();
+	void selectRealSense2L515();
 	void selectRealSense2Stereo();
 	void selectStereoDC1394();
 	void selectStereoFlyCapture2();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -308,7 +308,7 @@ public Q_SLOTS:
 	void setDetectionRate(double value);
 	void setTimeLimit(float value);
 	void setSLAMMode(bool enabled);
-	void selectSourceDriver(Src src);
+	void selectSourceDriver(Src src, int variant = 0);
 	void calibrate();
 	void calibrateSimple();
 

guilib/src/CameraViewer.cpp

@@ -146,7 +146,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		showScanCheckbox_->setEnabled(true);
 		if(showScanCheckbox_->isChecked())
 		{
-			cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), Transform::getIdentity(), Qt::yellow);
+			cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
 		}
 	}
 

guilib/src/MainWindow.cpp

@@ -435,9 +435,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense2_SR300, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
-	connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
+	connect(_ui->actionRealSense2_D400, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
-	connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
+	connect(_ui->actionRealSense2_L515, SIGNAL(triggered()), this, SLOT(selectRealSense2L515()));
-	connect(_ui->actionRealSense2_L515, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
 	connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
 	connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
@@ -457,8 +456,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense2_SR300->setEnabled(CameraRealSense2::available());
-	_ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
+	_ui->actionRealSense2_D400->setEnabled(CameraRealSense2::available());
-	_ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_L515->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_T265->setEnabled(CameraRealSense2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
@@ -4731,8 +4729,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
 	_ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
 	_ui->actionRealSense2_SR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
-	_ui->actionRealSense2_D415->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
+	_ui->actionRealSense2_D400->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
-	_ui->actionRealSense2_D435->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionRealSense2_L515->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
 	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
@@ -6484,6 +6481,10 @@ void MainWindow::selectRealSense2()
 {
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2);
 }
+void MainWindow::selectRealSense2L515()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2, 1);
+}
 
 void MainWindow::selectRealSense2Stereo()
 {

guilib/src/PreferencesDialog.cpp

@@ -682,6 +682,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_rs2_width, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_rs2_height, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_rs2_rate, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_rs2_width_depth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_rs2_height_depth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_rs2_rate_depth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_globalTimeStync, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_dualMode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_rs2_dualModeExtrinsics, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1908,6 +1911,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_rs2_width->setValue(848);
 		_ui->spinBox_rs2_height->setValue(480);
 		_ui->spinBox_rs2_rate->setValue(60);
+		_ui->spinBox_rs2_width_depth->setValue(640);
+		_ui->spinBox_rs2_height_depth->setValue(480);
+		_ui->spinBox_rs2_rate_depth->setValue(30);
 		_ui->checkbox_rs2_globalTimeStync->setChecked(true);
 		_ui->checkbox_rs2_dualMode->setChecked(false);
 		_ui->lineEdit_rs2_dualModeExtrinsics->setText("0.009 0.021 0.027 0 -0.018 0.005");
@@ -2364,6 +2370,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->spinBox_rs2_width->setValue(settings.value("width", _ui->spinBox_rs2_width->value()).toInt());
 	_ui->spinBox_rs2_height->setValue(settings.value("height", _ui->spinBox_rs2_height->value()).toInt());
 	_ui->spinBox_rs2_rate->setValue(settings.value("rate", _ui->spinBox_rs2_rate->value()).toInt());
+	_ui->spinBox_rs2_width_depth->setValue(settings.value("width_depth", _ui->spinBox_rs2_width_depth->value()).toInt());
+	_ui->spinBox_rs2_height_depth->setValue(settings.value("height_depth", _ui->spinBox_rs2_height_depth->value()).toInt());
+	_ui->spinBox_rs2_rate_depth->setValue(settings.value("rate_depth", _ui->spinBox_rs2_rate_depth->value()).toInt());
 	_ui->checkbox_rs2_globalTimeStync->setChecked(settings.value("global_time_sync", _ui->checkbox_rs2_globalTimeStync->isChecked()).toBool());
 	_ui->checkbox_rs2_dualMode->setChecked(settings.value("dual_mode", _ui->checkbox_rs2_dualMode->isChecked()).toBool());
 	_ui->lineEdit_rs2_dualModeExtrinsics->setText(settings.value("dual_mode_extrinsics", _ui->lineEdit_rs2_dualModeExtrinsics->text()).toString());
@@ -2847,6 +2856,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("width",                  _ui->spinBox_rs2_width->value());
 	settings.setValue("height",                 _ui->spinBox_rs2_height->value());
 	settings.setValue("rate",                   _ui->spinBox_rs2_rate->value());
+	settings.setValue("width_depth",            _ui->spinBox_rs2_width_depth->value());
+	settings.setValue("height_depth",           _ui->spinBox_rs2_height_depth->value());
+	settings.setValue("rate_depth",             _ui->spinBox_rs2_rate_depth->value());
 	settings.setValue("global_time_sync",       _ui->checkbox_rs2_globalTimeStync->isChecked());
 	settings.setValue("dual_mode",              _ui->checkbox_rs2_dualMode->isChecked());
 	settings.setValue("dual_mode_extrinsics",   _ui->lineEdit_rs2_dualModeExtrinsics->text());
@@ -3708,8 +3720,11 @@ void PreferencesDialog::updateParameters(const ParametersMap & parameters, bool
 	}
 }
 
-void PreferencesDialog::selectSourceDriver(Src src)
+void PreferencesDialog::selectSourceDriver(Src src, int variant)
 {
+	_ui->comboBox_imuFilter_strategy->setCurrentIndex(0);
+	this->setParameter(Parameters::kOptimizerGravitySigma(), uNumber2Str(Parameters::defaultOptimizerGravitySigma()));
+
 	if(src >= kSrcRGBD && src<kSrcStereo)
 	{
 		_ui->comboBox_sourceType->setCurrentIndex(0);
@@ -3725,12 +3740,60 @@ void PreferencesDialog::selectSourceDriver(Src src)
 		else if (src == kSrcK4A)
 		{
 			_ui->lineEdit_k4a_mkv->clear();
+			if(Optimizer::isAvailable(Optimizer::kTypeG2O) || Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				// enable IMU
+				_ui->comboBox_imuFilter_strategy->setCurrentIndex(1);
+				this->setParameter(Parameters::kOptimizerGravitySigma(), "0.3");
+			}
+		}
+		else if (src == kSrcRealSense2)
+		{
+			if(variant > 0) // L515
+			{
+				_ui->spinBox_rs2_width->setValue(1280);
+				_ui->spinBox_rs2_height->setValue(720);
+				_ui->spinBox_rs2_rate->setValue(30);
+			}
+			else
+			{
+				_ui->spinBox_rs2_width->setValue(848);
+				_ui->spinBox_rs2_height->setValue(480);
+				_ui->spinBox_rs2_rate->setValue(60);
+			}
+			if(Optimizer::isAvailable(Optimizer::kTypeG2O) || Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+			{
+				// enable IMU
+				_ui->comboBox_imuFilter_strategy->setCurrentIndex(1);
+				this->setParameter(Parameters::kOptimizerGravitySigma(), "0.3");
+			}
 		}
 	}
 	else if(src >= kSrcStereo && src<kSrcRGB)
 	{
 		_ui->comboBox_sourceType->setCurrentIndex(1);
 		_ui->comboBox_cameraStereo->setCurrentIndex(src - kSrcStereo);
+		if(Optimizer::isAvailable(Optimizer::kTypeG2O) || Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+		{
+			if(src == kSrcStereoZed) // Zedm, Zed2
+			{
+				// enable IMU (zed sends already quaternion)
+				_ui->comboBox_imuFilter_strategy->setCurrentIndex(0);
+				this->setParameter(Parameters::kOptimizerGravitySigma(), "0.3");
+			}
+			else if(src == kSrcStereoRealSense2) // T265
+			{
+				// enable IMU
+				_ui->comboBox_imuFilter_strategy->setCurrentIndex(1);
+				this->setParameter(Parameters::kOptimizerGravitySigma(), "0.3");
+			}
+			else if(src == kSrcStereoMyntEye)
+			{
+				// enable IMU
+				_ui->comboBox_imuFilter_strategy->setCurrentIndex(1);
+				this->setParameter(Parameters::kOptimizerGravitySigma(), "0.3");
+			}
+		}
 	}
 	else if(src >= kSrcRGB && src<kSrcDatabase)
 	{
@@ -5738,6 +5801,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked());
 				((CameraRealSense2*)camera)->setIRFormat(_ui->checkbox_rs2_irMode->isChecked(), _ui->checkbox_rs2_irDepth->isChecked());
 				((CameraRealSense2*)camera)->setResolution(_ui->spinBox_rs2_width->value(), _ui->spinBox_rs2_height->value(), _ui->spinBox_rs2_rate->value());
+				((CameraRealSense2*)camera)->setDepthResolution(_ui->spinBox_rs2_width_depth->value(), _ui->spinBox_rs2_height_depth->value(), _ui->spinBox_rs2_rate_depth->value());
 				((CameraRealSense2*)camera)->setGlobalTimeSync(_ui->checkbox_rs2_globalTimeStync->isChecked());
 				((CameraRealSense2*)camera)->setDualMode(_ui->checkbox_rs2_dualMode->isChecked(), Transform::fromString(_ui->lineEdit_rs2_dualModeExtrinsics->text().toStdString()));
 				((CameraRealSense2*)camera)->setJsonConfig(_ui->lineEdit_rs2_jsonFile->text().toStdString());

guilib/src/ui/mainWindow.ui

@@ -200,23 +200,13 @@
       </widget>
       <widget class="QMenu" name="menuRealSense_D435">
        <property name="title">
-        <string>RealSense D435</string>
+        <string>RealSense D400</string>
        </property>
        <property name="icon">
         <iconset resource="../GuiLib.qrc">
          <normaloff>:/images/d435.png</normaloff>:/images/d435.png</iconset>
        </property>
-       <addaction name="actionRealSense2_D435"/>
+       <addaction name="actionRealSense2_D400"/>
-      </widget>
-      <widget class="QMenu" name="menuRealSense_D415">
-       <property name="title">
-        <string>RealSense D415</string>
-       </property>
-       <property name="icon">
-        <iconset resource="../GuiLib.qrc">
-         <normaloff>:/images/d415.png</normaloff>:/images/d415.png</iconset>
-       </property>
-       <addaction name="actionRealSense2_D415"/>
       </widget>
       <widget class="QMenu" name="menuRealSense_SR300">
        <property name="title">
@@ -246,7 +236,6 @@
       <addaction name="menuRealSense_R200"/>
       <addaction name="menuRealSense_ZR300"/>
       <addaction name="menuRealSense_SR300"/>
-      <addaction name="menuRealSense_D415"/>
       <addaction name="menuRealSense_D435"/>
       <addaction name="menuRealSense_L515"/>
      </widget>
@@ -1572,15 +1561,7 @@
     <string>Kinect for Azure</string>
    </property>
   </action>
-  <action name="actionRealSense2_D435">
+  <action name="actionRealSense2_D400">
-   <property name="checkable">
-    <bool>true</bool>
-   </property>
-   <property name="text">
-    <string>RealSense2</string>
-   </property>
-  </action>
-  <action name="actionRealSense2_D415">
    <property name="checkable">
     <bool>true</bool>
    </property>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-677</y>
+            <y>-687</y>
             <width>686</width>
-            <height>3286</height>
+            <height>3357</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3220,7 +3220,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>10</number>
+                              <number>9</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4173,13 +4173,33 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>RealSense2</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_100" columnstretch="0,0,1">
-                                  <item row="9" column="1">
+                                  <item row="6" column="1">
+                                   <widget class="QSpinBox" name="spinBox_rs2_width_depth">
+                                    <property name="suffix">
+                                     <string> pix</string>
+                                    </property>
+                                    <property name="maximum">
+                                     <number>9999</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="7" column="1">
+                                   <widget class="QSpinBox" name="spinBox_rs2_height_depth">
+                                    <property name="suffix">
+                                     <string> pix</string>
+                                    </property>
+                                    <property name="maximum">
+                                     <number>9999</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="12" column="1">
                                    <widget class="QLineEdit" name="lineEdit_rs2_jsonFile"/>
                                   </item>
-                                  <item row="7" column="2">
+                                  <item row="10" column="2">
                                    <widget class="QLabel" name="label_565">
                                     <property name="text">
-                                     <string>Dual Mode (D400+T265): Odometry is computed by T265 and RGB-D frames are from D400.</string>
+                                     <string>Dual Mode (D400/L500+T265): Odometry is computed by T265 and RGB-D frames are from D400/L500.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -4192,7 +4212,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <item row="3" column="2">
                                    <widget class="QLabel" name="label_549">
                                     <property name="text">
-                                     <string>Stream width.</string>
+                                     <string>RGB/IR stream width. Set 1280 for L515.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -4281,7 +4301,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <item row="4" column="2">
                                    <widget class="QLabel" name="label_550">
                                     <property name="text">
-                                     <string>Stream height.</string>
+                                     <string>RGB/IR stream height. Set 720 for L515.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -4291,7 +4311,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="13" column="1">
                                    <spacer name="verticalSpacer_71">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -4307,7 +4327,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <item row="5" column="2">
                                    <widget class="QLabel" name="label_551">
                                     <property name="text">
-                                     <string>Stream rate.</string>
+                                     <string>RGB/IR stream rate. Set 30 for L515.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -4340,7 +4360,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="10" column="1">
                                    <widget class="QCheckBox" name="checkbox_rs2_dualMode">
                                     <property name="text">
                                      <string/>
@@ -4350,7 +4370,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="8" column="1">
+                                  <item row="11" column="1">
                                    <widget class="QLineEdit" name="lineEdit_rs2_dualModeExtrinsics">
                                     <property name="toolTip">
                                      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0&lt;br/&gt;KITTI: /base_footprint to /gray_camera = 0 0 1 0 -1 0 0 0 0 -1 0 1.67&lt;br/&gt;KITTI: /base_footprint to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 1.67&lt;/p&gt;&lt;p&gt;EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = -0.0257742 0.00375623 0.999661 0.00981073 -0.999557 -0.0149672 -0.0257155 0.064677 0.0148655 -0.999881 0.00414038 -0.0216401&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -4360,23 +4380,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="8" column="2">
+                                  <item row="12" column="2">
-                                   <widget class="QLabel" name="label_566">
-                                    <property name="text">
-                                     <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Dual Mode extrinsics (T265's pose frame to D400's left IR camera). Default extrinsics match the 3D printed bracket &lt;a href=&quot; https://www.intelrealsense.com/depth-and-tracking-combined-get-started/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;here&lt;/span&gt;&lt;/a&gt; (&lt;a href=&quot;https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;stl&lt;/span&gt;&lt;/a&gt;).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="openExternalLinks">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="9" column="2">
                                    <widget class="QLabel" name="label_571">
                                     <property name="text">
                                      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;D400 Series Visual Presets. See this &lt;a href=&quot;https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;page&lt;/span&gt;&lt;/a&gt;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -4392,14 +4396,14 @@ 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="12" column="0">
                                    <widget class="QToolButton" name="toolButton_rs2_jsonFile">
                                     <property name="text">
                                      <string>...</string>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="2">
+                                  <item row="9" column="2">
                                    <widget class="QLabel" name="label_606">
                                     <property name="text">
                                      <string>Global time sync. This will make sure IMU data is interpolated at image timestamp, otherwise latest received IMU is synchronized to current frame. Set to off if your firmware of your camera cannot synchronize timestamps.</string>
@@ -4412,7 +4416,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="1">
+                                  <item row="9" column="1">
                                    <widget class="QCheckBox" name="checkbox_rs2_globalTimeStync">
                                     <property name="text">
                                      <string/>
@@ -4422,6 +4426,71 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="6" column="2">
+                                   <widget class="QLabel" name="label_611">
+                                    <property name="text">
+                                     <string>(L515) Depth stream width.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="8" column="1">
+                                   <widget class="QSpinBox" name="spinBox_rs2_rate_depth">
+                                    <property name="suffix">
+                                     <string> Hz</string>
+                                    </property>
+                                    <property name="maximum">
+                                     <number>999</number>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="7" column="2">
+                                   <widget class="QLabel" name="label_612">
+                                    <property name="text">
+                                     <string>(L515) Depth stream height.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="8" column="2">
+                                   <widget class="QLabel" name="label_613">
+                                    <property name="text">
+                                     <string>(L515) Depth stream rate.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="11" column="2">
+                                   <widget class="QLabel" name="label_566">
+                                    <property name="text">
+                                     <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Dual Mode extrinsics (T265's pose frame to D400's left IR camera). Default extrinsics match the 3D printed bracket &lt;a href=&quot; https://www.intelrealsense.com/depth-and-tracking-combined-get-started/&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;here&lt;/span&gt;&lt;/a&gt; (&lt;a href=&quot;https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;stl&lt;/span&gt;&lt;/a&gt;).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="openExternalLinks">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>