CameraRealSense2: added stereo ir support for D400 cameras, added publishInterIMU parameter (required for VINS fusion).

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

@@ -73,8 +73,9 @@ public:
 	// parameters are set during initialization
 	// D400 series
 	void setEmitterEnabled(bool enabled);
-	void setIRDepthFormat(bool enabled);
+	void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
 	void setResolution(int width, int height, int fps = 30);
+	void publishInterIMU(bool enabled);
 	// T265 related parameters
 	void setImagesRectified(bool enabled);
 	void setOdomProvided(bool enabled);
@@ -116,14 +117,17 @@ private:
 	UMutex imuMutex_;
 	double hostStartStamp_;
 	double cameraStartStamp_;
+	double lastImuStamp_;
 
 	bool emitterEnabled_;
+	bool ir_;
 	bool irDepth_;
 	bool rectifyImages_;
 	bool odometryProvided_;
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;
+	bool publishInterIMU_;
 
 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;

corelib/src/OdometryThread.cpp

@@ -159,11 +159,6 @@ void OdometryThread::addData(const SensorData & data)
 				_dataBuffer.erase(_dataBuffer.begin());
 				notify = false;
 			}
-			if(notify && _imuEstimatedDelay>0.0 && data.stamp() > (_lastImuStamp+_imuEstimatedDelay))
-			{
-				// Don't notify if IMU data before this image has not been received yet
-				notify = false;
-			}
 		}
 		else
 		{
@@ -189,19 +184,16 @@ bool OdometryThread::getData(SensorData & data)
 	_dataAdded.acquire();
 	_dataMutex.lock();
 	{
-		if(!_dataBuffer.empty() || !_imuBuffer.empty())
+		if(!_dataBuffer.empty())
 		{
-			if(_dataBuffer.empty() ||
-				(!_dataBuffer.empty() && !_imuBuffer.empty() && _imuBuffer.front().stamp() < _dataBuffer.front().stamp()))
+			while(!_imuBuffer.empty() && _imuBuffer.front().stamp() <= _dataBuffer.front().stamp())
 			{
-				data = _imuBuffer.front();
+				_odometry->process(_imuBuffer.front());
 				_imuBuffer.pop_front();
 			}
-			else
-			{
-				data = _dataBuffer.front();
-				_dataBuffer.pop_front();
-			}
+
+			data = _dataBuffer.front();
+			_dataBuffer.pop_front();
 			dataFilled = true;
 		}
 	}

corelib/src/camera/CameraRealSense2.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UEventsManager.h>
 #include <opencv2/imgproc/types_c.h>
 
 #ifdef RTABMAP_REALSENSE2
@@ -66,13 +67,16 @@ CameraRealSense2::CameraRealSense2(
 	depthToRGBExtrinsics_(new rs2_extrinsics),
 	hostStartStamp_(0.0),
 	cameraStartStamp_(0.0),
+	lastImuStamp_(0.0),
 	emitterEnabled_(true),
-	irDepth_(false),
+	ir_(false),
+	irDepth_(true),
 	rectifyImages_(true),
 	odometryProvided_(false),
 	cameraWidth_(640),
 	cameraHeight_(480),
-	cameraFps_(30)
+	cameraFps_(30),
+	publishInterIMU_(false)
 #endif
 {
 	UDEBUG("");
@@ -540,18 +544,13 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		{
 			sensors[1] = elem;
 			sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
-			if(irDepth_)
-			{
-				sensors[0] = elem;
-			}
 		}
 		else if ("Coded-Light Depth Sensor" == module_name)
 		{
-			sensors[1] = elem;
 		}
 		else if ("RGB Camera" == module_name)
 		{
-			if(!irDepth_)
+			if(!ir_)
 			{
 				sensors[0] = elem;
 			}
@@ -586,6 +585,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
+		if(i==0 && ir_ && !stereo)
+		{
+			continue;
+		}
 		UINFO("Sensor %d \"%s\"", (int)i, sensors[i].get_info(RS2_CAMERA_INFO_NAME));
 		auto profiles = sensors[i].get_stream_profiles();
 		bool added = false;
@@ -595,11 +598,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UINFO("%s %d %d %d", rs2_format_to_string(
+				UINFO("%s %d %d %d %d", rs2_format_to_string(
 						video_profile.format()),
 						video_profile.width(),
 						video_profile.height(),
-						video_profile.fps());
+						video_profile.fps(),
+						video_profile.stream_index());
 			}
 		}
 		int pi = 0;
@@ -609,35 +613,50 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			if(!stereo)
 			{
 				//D400 series:
-				if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
-					video_profile.width()  == cameraWidth_ &&
+				if (video_profile.width()  == cameraWidth_ &&
 					video_profile.height() == cameraHeight_ &&
 					video_profile.fps()    == cameraFps_)
 				{
-					if(irDepth_ && i==0)
-					{
-						profilesPerSensor[1].push_back(profile.clone(profile.stream_type(), 1, profile.format()));
-					}
-					else
-					{
-						profilesPerSensor[i].push_back(profile);
-					}
 					auto intrinsic = video_profile.get_intrinsics();
-					if(i==1)
+
+					// rgb or ir left
+					if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8) ||
+					  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 1))
 					{
-						depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), CV_16UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						*depthIntrinsics_ = intrinsic;
-					}
-					else
-					{
-						rgbBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
+						if(!profilesPerSensor[i].empty())
+						{
+							// IR right already there, push ir left front
+							profilesPerSensor[i].push_back(profilesPerSensor[i].back());
+							profilesPerSensor[i].front() = profile;
+						}
+						else
+						{
+							profilesPerSensor[i].push_back(profile);
+						}
+						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));
 						rgbStreamProfile = profile;
 						*rgbIntrinsics_ = intrinsic;
+						added = true;
+						if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
+						{
+							break;
+						}
+					}
+					// depth or ir right
+					else if(((!ir_ || irDepth_) && video_profile.format() == RS2_FORMAT_Z16) ||
+						   (ir_ && !irDepth_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 2))
+					{
+						profilesPerSensor[i].push_back(profile);
+						depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
+						depthStreamProfile = profile;
+						*depthIntrinsics_ = intrinsic;
+						added = true;
+						if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
+						{
+							break;
+						}
 					}
-					added = true;
-					break;
 				}
 				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F)
 				{
@@ -696,11 +715,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UERROR("%s %d %d %d", rs2_format_to_string(
+				UERROR("%s %d %d %d %d", rs2_format_to_string(
 						video_profile.format()),
 						video_profile.width(),
 						video_profile.height(),
-						video_profile.fps());
+						video_profile.fps(),
+						video_profile.stream_index());
 			}
 			return false;
 		}
@@ -715,11 +735,30 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 }
 		 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
 
+		 if(ir_ && !irDepth_ && profilesPerSensor.size() >= 2 && profilesPerSensor[1].size() >= 2)
+		 {
+			 rs2_extrinsics leftToRight = profilesPerSensor[1][1].get_extrinsics_to(profilesPerSensor[1][0]);
+			 Transform leftToRightT(
+					 leftToRight.rotation[0], leftToRight.rotation[1], leftToRight.rotation[2], leftToRight.translation[0],
+					 leftToRight.rotation[3], leftToRight.rotation[4], leftToRight.rotation[5], leftToRight.translation[1],
+					 leftToRight.rotation[6], leftToRight.rotation[7], leftToRight.rotation[8], leftToRight.translation[2]);
+
+			 UINFO("left to right transform = %s", leftToRightT.prettyPrint().c_str());
+
+			 // Create stereo camera model from left and right ir of D435
+			 stereoModel_ = StereoCameraModel(model_.fx(), model_.fy(), model_.cx(), model_.cy(), leftToRightT.x(), model_.localTransform(), model_.imageSize());
+			 UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+						stereoModel_.left().fx(),
+						stereoModel_.left().cx(),
+						stereoModel_.left().cy(),
+						stereoModel_.baseline());
+		 }
+
 		 if(profilesPerSensor.size() == 3)
 		 {
 			 if(!profilesPerSensor[2].empty() && !profilesPerSensor[0].empty())
 			 {
-				 rs2_extrinsics leftToIMU = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[2][0]);
+				 rs2_extrinsics leftToIMU = profilesPerSensor[2][0].get_extrinsics_to(profilesPerSensor[0][0]);
 				 Transform leftToIMUT(
 						 leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
 						 leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
@@ -729,17 +768,18 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 }
 			 else if(!profilesPerSensor[2].empty() && !profilesPerSensor[1].empty())
 			 {
-				 rs2_extrinsics leftToIMU = profilesPerSensor[1][0].get_extrinsics_to(profilesPerSensor[2][0]);
+				 rs2_extrinsics leftToIMU = profilesPerSensor[2][0].get_extrinsics_to(profilesPerSensor[1][0]);
 				 Transform leftToIMUT(
 						 leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
 						 leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
 						 leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
+
 				 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
 				 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
 			 }
 		 }
 	 }
-	 else
+	 else // T265
 	 {
 
 		// look for calibration files
@@ -887,10 +927,11 @@ void CameraRealSense2::setEmitterEnabled(bool enabled)
 #endif
 }
 
-void CameraRealSense2::setIRDepthFormat(bool enabled)
+void CameraRealSense2::setIRFormat(bool enabled, bool useDepthInsteadOfRightImage)
 {
 #ifdef RTABMAP_REALSENSE2
-	irDepth_ = enabled;
+	ir_ = enabled;
+	irDepth_ = useDepthInsteadOfRightImage;
 #endif
 }
 
@@ -903,6 +944,13 @@ void CameraRealSense2::setResolution(int width, int height, int fps)
 #endif
 }
 
+void CameraRealSense2::publishInterIMU(bool enabled)
+{
+#ifdef RTABMAP_REALSENSE2
+	publishInterIMU_ = enabled;
+#endif
+}
+
 void CameraRealSense2::setImagesRectified(bool enabled)
 {
 #ifdef RTABMAP_REALSENSE2
@@ -955,8 +1003,25 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				auto stream_type = f.get_profile().stream_type();
 				if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
 				{
-					rgb_frame = f;
-					is_rgb_arrived = true;
+					if(ir_ && !irDepth_)
+					{
+						//stereo D435
+						if(!is_depth_arrived)
+						{
+							depth_frame = f; // right image
+							is_depth_arrived = true;
+						}
+						else
+						{
+							rgb_frame = f; // left image
+							is_rgb_arrived = true;
+						}
+					}
+					else
+					{
+						rgb_frame = f;
+						is_rgb_arrived = true;
+					}
 				}
 				else if (stream_type == RS2_STREAM_DEPTH)
 				{
@@ -982,7 +1047,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 			{
 				auto from_image_frame = depth_frame.as<rs2::video_frame>();
 				cv::Mat depth;
-				if(irDepth_)
+				if(ir_)
 				{
 					depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
 				}
@@ -1005,7 +1070,15 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					bgr = rgb.clone();
 				}
 
-				data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
+				if(ir_ && !irDepth_)
+				{
+					//stereo D435
+					data = SensorData(bgr, depth, stereoModel_, this->getNextSeqID(), stamp);
+				}
+				else
+				{
+					data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
+				}
 			}
 			else if(is_left_fisheye_arrived && is_right_fisheye_arrived)
 			{
@@ -1036,7 +1109,8 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 
 			IMU imu;
 			unsigned int confidence = 0;
-			getPoseAndIMU(frameset.get_timestamp()> UTimer::now()+1000000000?stamp*1000.0:frameset.get_timestamp(), info->odomPose, confidence, imu);
+			double imuStamp = frameset.get_timestamp()> UTimer::now()+1000000000?stamp*1000.0:frameset.get_timestamp();
+			getPoseAndIMU(imuStamp, info->odomPose, confidence, imu);
 
 			if(odometryProvided_ && !info->odomPose.isNull())
 			{
@@ -1047,6 +1121,33 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 			if(!imu.empty())
 			{
 				data.setIMU(imu);
+
+				if(publishInterIMU_ && lastImuStamp_ > 0.0)
+				{
+					UASSERT(imuStamp > lastImuStamp_);
+					imuMutex_.lock();
+					std::map<double, cv::Vec3f>::iterator iterA = gyroBuffer_.upper_bound(lastImuStamp_);
+					std::map<double, cv::Vec3f>::iterator iterB = gyroBuffer_.lower_bound(imuStamp);
+					if(iterA != gyroBuffer_.end())
+					{
+						++iterA;
+					}
+					if(iterA != iterB)
+					{
+						for(;iterA != iterB;++iterA)
+						{
+							Transform tmp;
+							IMU imuTmp;
+							getPoseAndIMU(iterA->first, tmp, confidence, imuTmp);
+							if(!imuTmp.empty())
+							{
+								UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
+							}
+						}
+					}
+					imuMutex_.unlock();
+				}
+				lastImuStamp_ = imuStamp;
 			}
 		}
 		else

corelib/src/odometry/OdometryVINS.cpp

@@ -104,6 +104,18 @@ public:
 			UWARN("Images are rectified but received calibration cannot be "
 					"used, make sure calibration in config file doesn't have "
 					"distortion or send raw images to VINS odometry.");
+			if(!featureTracker.m_camera.empty())
+			{
+				if(featureTracker.m_camera.front()->imageWidth() != model.left().imageWidth() ||
+				   featureTracker.m_camera.front()->imageHeight() != model.left().imageHeight())
+				{
+					UERROR("Received images don't have same size (%dx%d) than in the config file (%dx%d)!",
+							model.left().imageWidth(),
+							model.left().imageHeight(),
+							featureTracker.m_camera.front()->imageWidth(),
+							featureTracker.m_camera.front()->imageHeight());
+				}
+			}
 		}
 
 		Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
@@ -472,6 +484,14 @@ Transform OdometryVINS::computeTransform(
 			UWARN("VINS not yet initialized... waiting to get enough IMU messages");
 		}
 	}
+	else if(!data.imageRaw().empty() && !data.depthRaw().empty())
+	{
+		UERROR("VINS-Fusion doesn't work with RGB-D data, stereo images are required!");
+	}
+	else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
+	{
+		UERROR("VINS-Fusion requires stereo images!");
+	}
 #else
 	UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach.");
 #endif

guilib/src/PreferencesDialog.cpp

@@ -626,6 +626,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkbox_realsenseDepthScaledToRGBSize, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_realsenseRGBSource, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_emitter, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_rs2_irMode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_rs2_width, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_rs2_height, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -714,6 +715,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_imuFilter, SLOT(setCurrentIndex(int)));
 	_ui->stackedWidget_imuFilter->setCurrentIndex(_ui->comboBox_imuFilter_strategy->currentIndex());
+	connect(_ui->checkbox_publishInterIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1790,7 +1792,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(false);
 		_ui->comboBox_realsenseRGBSource->setCurrentIndex(0);
 		_ui->checkbox_rs2_emitter->setChecked(true);
-		_ui->checkbox_rs2_irDepth->setChecked(false);
+		_ui->checkbox_rs2_irMode->setChecked(false);
+		_ui->checkbox_rs2_irDepth->setChecked(true);
 		_ui->spinBox_rs2_width->setValue(848);
 		_ui->spinBox_rs2_height->setValue(480);
 		_ui->spinBox_rs2_rate->setValue(60);
@@ -1849,6 +1852,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setValue(Parameters::defaultImuFilterComplementaryBiasAlpha());
 		_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setChecked(Parameters::defaultImuFilterComplementaryDoAdpativeGain());
 		_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setChecked(Parameters::defaultImuFilterComplementaryDoBiasEstimation());
+		_ui->checkbox_publishInterIMU->setChecked(false);
 
 		_ui->checkBox_source_scanFromDepth->setChecked(false);
 		_ui->spinBox_source_scanDownsampleStep->setValue(1);
@@ -2217,6 +2221,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 
 	settings.beginGroup("RealSense2");
 	_ui->checkbox_rs2_emitter->setChecked(settings.value("emitter", _ui->checkbox_rs2_emitter->isChecked()).toBool());
+	_ui->checkbox_rs2_irMode->setChecked(settings.value("ir", _ui->checkbox_rs2_irMode->isChecked()).toBool());
 	_ui->checkbox_rs2_irDepth->setChecked(settings.value("irdepth", _ui->checkbox_rs2_irDepth->isChecked()).toBool());
 	_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());
@@ -2299,6 +2304,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setValue(settings.value("complementary_bias_alpha", _ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->value()).toDouble());
 	_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setChecked(settings.value("complementary_adaptive_gain", _ui->checkBox_imuFilterComplementaryDoAdaptiveGain->isChecked()).toBool());
 	_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setChecked(settings.value("complementary_biais_estimation", _ui->checkBox_imuFilterComplementaryDoBiasEstimation->isChecked()).toBool());
+	_ui->checkbox_publishInterIMU->setChecked(settings.value("publish_inter_imu", _ui->checkbox_publishInterIMU->isChecked()).toBool());
 	settings.endGroup();//IMU
 
 	settings.beginGroup("Scan");
@@ -2673,6 +2679,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 
 	settings.beginGroup("RealSense2");
 	settings.setValue("emitter",                _ui->checkbox_rs2_emitter->isChecked());
+	settings.setValue("ir",                     _ui->checkbox_rs2_irMode->isChecked());
 	settings.setValue("irdepth",                _ui->checkbox_rs2_irDepth->isChecked());
 	settings.setValue("width",                  _ui->spinBox_rs2_width->value());
 	settings.setValue("height",                 _ui->spinBox_rs2_height->value());
@@ -2753,6 +2760,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("complementary_bias_alpha",       _ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->value());
 	settings.setValue("complementary_adaptive_gain",    _ui->checkBox_imuFilterComplementaryDoAdaptiveGain->isChecked());
 	settings.setValue("complementary_biais_estimation", _ui->checkBox_imuFilterComplementaryDoBiasEstimation->isChecked());
+	settings.setValue("publish_inter_imu",              _ui->checkbox_publishInterIMU->isChecked());
 	settings.endGroup();//IMU
 
 	settings.beginGroup("Scan");
@@ -5410,6 +5418,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getSourceDevice().toStdString(),
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
+			((CameraRealSense2*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
 			if(driver == kSrcStereoRealSense2)
 			{
 				((CameraRealSense2*)camera)->setImagesRectified(_ui->checkBox_stereo_rectify->isChecked() && !useRawImages);
@@ -5418,7 +5427,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			else
 			{
 				((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked());
-				((CameraRealSense2*)camera)->setIRDepthFormat(_ui->checkbox_rs2_irDepth->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());
 			}
 		}

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
-            <width>681</width>
-            <height>3071</height>
+            <y>-2150</y>
+            <width>680</width>
+            <height>3083</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>16</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3096,7 +3096,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">
@@ -4062,7 +4062,7 @@ 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,1">
-                                  <item row="2" column="1">
+                                  <item row="3" column="1">
                                    <widget class="QLabel" name="label_549">
                                     <property name="text">
                                      <string>Stream width.</string>
@@ -4072,7 +4072,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="0">
+                                  <item row="4" column="0">
                                    <widget class="QSpinBox" name="spinBox_rs2_height">
                                     <property name="suffix">
                                      <string> pix</string>
@@ -4082,7 +4082,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="4" column="0">
+                                  <item row="5" column="0">
                                    <widget class="QSpinBox" name="spinBox_rs2_rate">
                                     <property name="suffix">
                                      <string> Hz</string>
@@ -4113,7 +4113,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </widget>
                                   </item>
                                   <item row="1" column="0">
-                                   <widget class="QCheckBox" name="checkbox_rs2_irDepth">
+                                   <widget class="QCheckBox" name="checkbox_rs2_irMode">
                                     <property name="text">
                                      <string/>
                                     </property>
@@ -4125,14 +4125,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <item row="1" column="1">
                                    <widget class="QLabel" name="label_471">
                                     <property name="text">
-                                     <string>Use IR for RGB image. Tracking will be more accurate (field-of-view of the IR camera is larger with less motion blur). Make sure to disable IR emitter. </string>
+                                     <string>IR mode: use IR camera instead of RGB camera. Tracking will be more accurate (field-of-view of the IR camera is larger with less motion blur). Make sure to disable IR emitter. </string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="0">
+                                  <item row="3" column="0">
                                    <widget class="QSpinBox" name="spinBox_rs2_width">
                                     <property name="suffix">
                                      <string> pix</string>
@@ -4142,7 +4142,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="1">
+                                  <item row="4" column="1">
                                    <widget class="QLabel" name="label_550">
                                     <property name="text">
                                      <string>Stream height.</string>
@@ -4152,7 +4152,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="0">
+                                  <item row="6" column="0">
                                    <spacer name="verticalSpacer_71">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -4165,7 +4165,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </spacer>
                                   </item>
-                                  <item row="4" column="1">
+                                  <item row="5" column="1">
                                    <widget class="QLabel" name="label_551">
                                     <property name="text">
                                      <string>Stream rate.</string>
@@ -4175,6 +4175,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="2" column="1">
+                                   <widget class="QLabel" name="label_562">
+                                    <property name="text">
+                                     <string>Use depth image in IR mode (instead of right image).</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="0">
+                                   <widget class="QCheckBox" name="checkbox_rs2_irDepth">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -6310,6 +6330,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="0">
+                          <widget class="QCheckBox" name="checkbox_publishInterIMU">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_563">
+                           <property name="text">
+                            <string>Publish inter IMU messages from the camera. IMU received between images will be published as separate topic.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>