OctoMap::createCloud() added cloud probability output option.

CameraRealSense2: added new warnings to debug easier timestamps problems.
GUI: added gravity visualization.
DbViewer: empty clouds in OctoMap mode have color depending on probability of occupancy.
Brought general changes from local xvision branch.

guilib/src/CameraViewer.cpp

@@ -52,7 +52,6 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	imageView_(new ImageView(this)),
 	cloudView_(new CloudViewer(this)),
 	processingImages_(false),
-	validDecimationValue_(2),
 	parameters_(parameters)
 {
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
@@ -66,7 +65,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 
 	QLabel * decimationLabel = new QLabel("Decimation", this);
 	decimationSpin_ = new QSpinBox(this);
-	decimationSpin_->setMinimum(1);
+	decimationSpin_->setMinimum(-16);
 	decimationSpin_->setMaximum(16);
 	decimationSpin_->setValue(2);
 
@@ -78,6 +77,8 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	showScanCheckbox_ = new QCheckBox("Show scan", this);
 	showScanCheckbox_->setEnabled(false);
 
+	imageSizeLabel_ = new QLabel(this);
+
 	QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
 	buttonBox->setStandardButtons(QDialogButtonBox::Close);
 	connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
@@ -88,6 +89,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	layout2->addWidget(decimationSpin_);
 	layout2->addWidget(showCloudCheckbox_);
 	layout2->addWidget(showScanCheckbox_);
+	layout2->addWidget(imageSizeLabel_);
 	layout2->addStretch(1);
 	layout2->addWidget(buttonBox);
 
@@ -107,37 +109,19 @@ CameraViewer::~CameraViewer()
 
 void CameraViewer::showImage(const rtabmap::SensorData & data)
 {
-	if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty())
-	{
-		if(data.imageRaw().cols % decimationSpin_->value() == 0 &&
-		   data.imageRaw().rows % decimationSpin_->value() == 0 &&
-		   data.depthOrRightRaw().cols % decimationSpin_->value() == 0 &&
-		   data.depthOrRightRaw().rows % decimationSpin_->value() == 0)
-		{
-			validDecimationValue_ = decimationSpin_->value();
-		}
-		else
-		{
-			UWARN("Decimation (%d) must be a denominator of the width and height of "
-					"the image (color=%d/%d depth=%d/%d). Using last valid decimation value (%d).",
-					decimationSpin_->value(),
-					data.imageRaw().cols,
-					data.imageRaw().rows,
-					data.depthOrRightRaw().cols,
-					data.depthOrRightRaw().rows,
-					validDecimationValue_);
-		}
-	}
-
 	processingImages_ = true;
+	QString sizes;
 	if(!data.imageRaw().empty())
 	{
-		imageView_->setImage(uCvMat2QImage(util2d::decimate(data.imageRaw(), validDecimationValue_)));
+		imageView_->setImage(uCvMat2QImage(data.imageRaw()));
+		sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows));
 	}
 	if(!data.depthOrRightRaw().empty())
 	{
-		imageView_->setImageDepth(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_));
+		imageView_->setImageDepth(data.depthOrRightRaw());
+		sizes.append(QString(" Depth=%1x%2").arg(data.depthOrRightRaw().cols).arg(data.depthOrRightRaw().rows));
 	}
+	imageSizeLabel_->setText(sizes);
 
 	if(!data.depthOrRightRaw().empty() &&
 	   (data.stereoCameraModel().isValidForProjection() || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
@@ -147,12 +131,12 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 			if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
+				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
 			}
 			else if(!data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
+				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
 			}
 		}
 	}
@@ -162,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()), validDecimationValue_), Transform::getIdentity(), Qt::yellow);
+			cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), Transform::getIdentity(), Qt::yellow);
 		}
 	}
 

guilib/src/DatabaseViewer.cpp

@@ -5002,15 +5002,22 @@ void DatabaseViewer::updateConstraintView()
 		Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
 		if(link.isValid())
 		{
-			if(link.type() == Link::kNeighbor ||
-			   link.type() == Link::kNeighborMerged)
+			UDEBUG("link %d->%d type=%d", link.from(), link.to(), link.type());
+			if((link.type() == Link::kNeighbor ||
+			   link.type() == Link::kNeighborMerged) &&
+					ui_->horizontalSlider_neighbors->value() >= 0 && ui_->horizontalSlider_neighbors->value() < neighborLinks_.size())
 			{
 				this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
 			}
-			else
+			else if((link.type() != Link::kPosePrior || link.type() != Link::kGravity) &&
+					ui_->horizontalSlider_loops->value() >= 0 && ui_->horizontalSlider_loops->value() < loopLinks_.size())
 			{
 				this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
 			}
+			else
+			{
+				this->updateConstraintView(link, false);
+			}
 		}
 	}
 }
@@ -5047,6 +5054,13 @@ void DatabaseViewer::updateConstraintView(
 		}
 	}
 	rtabmap::Transform t = link.transform();
+	if(link.type() == Link::kGravity)
+	{
+		// remove yaw, keep only roll and pitch
+		float roll, pitch, yaw;
+		t.getEulerAngles(roll, pitch, yaw);
+		t = Transform(0,0,0,roll,pitch,0);
+	}
 
 	ui_->label_constraint->clear();
 	ui_->label_constraint_opt->clear();
@@ -6369,7 +6383,15 @@ void DatabaseViewer::updateOctomapView()
 					pcl::IndicesPtr obstacles(new std::vector<int>);
 					pcl::IndicesPtr empty(new std::vector<int>);
 					pcl::IndicesPtr ground(new std::vector<int>);
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
+					std::vector<double> prob;
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(
+							ui_->spinBox_grid_depth->value(),
+							obstacles.get(),
+							empty.get(),
+							ground.get(),
+							true,
+							0,
+							&prob);
 
 					if(octomap_->hasColor())
 					{
@@ -6398,11 +6420,35 @@ void DatabaseViewer::updateOctomapView()
 
 					if(ui_->checkBox_grid_empty->isChecked())
 					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
-						occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
-						occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
-						occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
+						if(prob.size()==cloud->size())
+						{
+							float occThr = Parameters::defaultGridGlobalOccupancyThr();
+							Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occThr);
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+							emptyCloud->resize(empty->size());
+							for(unsigned int i=0;i<empty->size(); ++i)
+							{
+								emptyCloud->points.at(i).x = cloud->points.at(empty->at(i)).x;
+								emptyCloud->points.at(i).y = cloud->points.at(empty->at(i)).y;
+								emptyCloud->points.at(i).z = cloud->points.at(empty->at(i)).z;
+								QColor hsv = QColor::fromHsv(int(prob.at(empty->at(i))/occThr*240.0), 255, 255, 255);
+								QRgb color = hsv.rgb();
+								emptyCloud->points.at(i).r = qRed(color);
+								emptyCloud->points.at(i).g = qGreen(color);
+								emptyCloud->points.at(i).b = qBlue(color);
+							}
+							occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
+							occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
+							occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
+						}
+						else
+						{
+							pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
+							pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
+							occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
+							occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
+							occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
+						}
 					}
 				}
 				occupancyGridViewer_->update();

guilib/src/ImageView.cpp

@@ -293,7 +293,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	this->setGraphicsViewMode(settings.value("graphics_view", this->isGraphicsViewMode()).toBool());
 	this->setGraphicsViewScaled(settings.value("graphics_view_scale", this->isGraphicsViewScaled()).toBool());
 	this->setGraphicsViewScaledToHeight(settings.value("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight()).toBool());
-	int colorMap = settings.value("colormap", 0).toInt();
+	int colorMap = settings.value("colormap", 2).toInt();
 	_colorMapWhiteToBlack->setChecked(colorMap==0);
 	_colorMapBlackToWhite->setChecked(colorMap==1);
 	_colorMapRedToBlue->setChecked(colorMap==2);

guilib/src/MainWindow.cpp

@@ -806,7 +806,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
 {
 	if(anEvent->getClassName().compare("IMUEvent") == 0)
 	{
-		// IMU events are published at high frequency, exit now
+		// IMU events are published at high frequency, early exit
 		return false;
 	}
 	else if(anEvent->getClassName().compare("RtabmapEvent") == 0)
@@ -1028,6 +1028,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		bool cloudUpdated = false;
 		bool scanUpdated = false;
 		bool featuresUpdated = false;
+		bool filteredGravityUpdated = false;
+		bool accelerationUpdated = false;
 		if(!pose.isNull())
 		{
 			// 3d cloud
@@ -1267,6 +1269,33 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 						}
 					}
 				}
+
+				if(  _preferencesDialog->isIMUGravityShown(1) &&
+					(odom.data().imu().orientation().val[0]!=0 ||
+					odom.data().imu().orientation().val[1]!=0 ||
+					odom.data().imu().orientation().val[2]!=0 ||
+					odom.data().imu().orientation().val[3]!=0))
+				{
+					Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
+					Transform orientation(0,0,0, odom.data().imu().orientation()[0], odom.data().imu().orientation()[1], odom.data().imu().orientation()[2], odom.data().imu().orientation()[3]);
+					gravity = (orientation* odom.data().imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
+					_cloudViewer->addOrUpdateLine("odom_imu_orientation", _odometryCorrection*pose, (_odometryCorrection*pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, true);
+					filteredGravityUpdated = true;
+				}
+				if( _preferencesDialog->isIMUAccShown() &&
+					(odom.data().imu().linearAcceleration().val[0]!=0 ||
+					odom.data().imu().linearAcceleration().val[1]!=0 ||
+					odom.data().imu().linearAcceleration().val[2]!=0))
+				{
+					Eigen::Vector3f gravity(
+							-odom.data().imu().linearAcceleration().val[0],
+							-odom.data().imu().linearAcceleration().val[1],
+							-odom.data().imu().linearAcceleration().val[2]);
+					gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
+					gravity = odom.data().imu().localTransform().toEigen3f()*gravity;
+					_cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
+					accelerationUpdated = true;
+				}
 			}
 		}
 		if(!dataIgnored)
@@ -1287,6 +1316,14 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			{
 				_cloudViewer->setCloudVisibility("featuresOdom", false);
 			}
+			if(!filteredGravityUpdated && _cloudViewer->getAddedLines().find("odom_imu_orientation") != _cloudViewer->getAddedLines().end())
+			{
+				_cloudViewer->removeLine("odom_imu_orientation");
+			}
+			if(!accelerationUpdated && _cloudViewer->getAddedLines().find("odom_imu_acc") != _cloudViewer->getAddedLines().end())
+			{
+				_cloudViewer->removeLine("odom_imu_acc");
+			}
 		}
 	}
 
@@ -2235,6 +2272,7 @@ void MainWindow::updateMapCloud(
 	// Map updated! regenerate the assembled cloud, last pose is the new one
 	UDEBUG("Update map with %d locations", poses.size());
 	QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
+	std::set<std::string> viewerLines = _cloudViewer->getAddedLines();
 	int i=1;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
@@ -2407,6 +2445,29 @@ void MainWindow::updateMapCloud(
 				_cloudViewer->setCloudVisibility(featuresName.c_str(), false);
 			}
 
+			// Gravity arrows
+			std::string gravityName = uFormat("gravity%d", iter->first);
+			if(iter->first == 0)
+			{
+				viewerLines.erase(gravityName);
+				_cloudViewer->removeLine(gravityName);
+			}
+			if(_cloudViewer->isVisible() && _preferencesDialog->isIMUGravityShown(0))
+			{
+				std::multimap<int, Link>::const_iterator linkIter = graph::findLink(constraints, iter->first, iter->first, false, Link::kGravity);
+				if(linkIter != constraints.end())
+				{
+					Transform gravityT = linkIter->second.transform();
+					Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(0));
+					gravity = (gravityT.rotation()*(iter->second).rotation().inverse()).toEigen3f()*gravity;
+					_cloudViewer->addOrUpdateLine(gravityName, iter->second, (iter->second).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*iter->second.rotation().inverse(), Qt::yellow, false, false);
+				}
+			}
+			else if(viewerLines.find(gravityName)!=viewerLines.end())
+			{
+				_cloudViewer->removeLine(gravityName.c_str());
+			}
+
 			if(verboseProgress)
 			{
 				_progressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
@@ -2443,6 +2504,21 @@ void MainWindow::updateMapCloud(
 			}
 		}
 	}
+	// remove not used gravity lines
+	for(std::set<std::string>::iterator iter = viewerLines.begin(); iter!=viewerLines.end(); ++iter)
+	{
+		std::list<std::string> splitted = uSplitNumChar(*iter);
+		int id = 0;
+		if(splitted.size() == 2)
+		{
+			id = std::atoi(splitted.back().c_str());
+			if(poses.find(id) == poses.end())
+			{
+				UDEBUG("Remove %s", iter->c_str());
+				_cloudViewer->removeLine(*iter);
+			}
+		}
+	}
 
 	UDEBUG("");
 	if(stats)

guilib/src/PreferencesDialog.cpp

@@ -480,6 +480,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingPtSizeFeatures[0] = _ui->spinBox_ptsize_features;
 	_3dRenderingPtSizeFeatures[1] = _ui->spinBox_ptsize_odom_features;
 
+	_3dRenderingGravity.resize(2);
+	_3dRenderingGravity[0] = _ui->checkBox_showIMUGravity;
+	_3dRenderingGravity[1] = _ui->checkBox_showIMUGravity_odom;
+
+	_3dRenderingGravityLength.resize(2);
+	_3dRenderingGravityLength[0] = _ui->doubleSpinBox_lengthIMUGravity;
+	_3dRenderingGravityLength[1] = _ui->doubleSpinBox_lengthIMUGravity_odom;
+
 	for(int i=0; i<2; ++i)
 	{
 		connect(_3dRenderingShowClouds[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -502,6 +510,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingOpacityScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingPtSizeScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingPtSizeFeatures[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingGravity[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingGravityLength[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+
 	}
 	connect(_ui->doubleSpinBox_voxel, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_noiseRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -519,6 +530,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_showLabels, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showLandmarks, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_landmarkSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_showIMUGravity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_showIMUAcc, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->radioButton_noFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->radioButton_nodeFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1649,6 +1662,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingOpacityScan[i]->setValue(i==0?1.0:0.5);
 			_3dRenderingPtSizeScan[i]->setValue(2);
 			_3dRenderingPtSizeFeatures[i]->setValue(3);
+			_3dRenderingGravity[i]->setChecked(false);
+			_3dRenderingGravityLength[i]->setValue(1);
 		}
 		_ui->doubleSpinBox_voxel->setValue(0);
 		_ui->doubleSpinBox_noiseRadius->setValue(0);
@@ -1668,6 +1683,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_showLabels->setChecked(false);
 		_ui->checkBox_showLandmarks->setChecked(true);
 		_ui->doubleSpinBox_landmarkSize->setValue(0);
+		_ui->checkBox_showIMUGravity->setChecked(false);
+		_ui->checkBox_showIMUAcc->setChecked(false);
 
 		_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
 		_ui->groupBox_organized->setChecked(false);
@@ -2094,6 +2111,9 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingOpacityScan[i]->setValue(settings.value(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value()).toDouble());
 		_3dRenderingPtSizeScan[i]->setValue(settings.value(QString("ptSizeScan%1").arg(i), _3dRenderingPtSizeScan[i]->value()).toInt());
 		_3dRenderingPtSizeFeatures[i]->setValue(settings.value(QString("ptSizeFeatures%1").arg(i), _3dRenderingPtSizeFeatures[i]->value()).toInt());
+		_3dRenderingGravity[i]->setChecked(settings.value(QString("gravityShown%1").arg(i), _3dRenderingGravity[i]->isChecked()).toBool());
+		_3dRenderingGravityLength[i]->setValue(settings.value(QString("gravityLength%1").arg(i), _3dRenderingGravityLength[i]->value()).toDouble());
+
 	}
 	_ui->doubleSpinBox_voxel->setValue(settings.value("cloudVoxel", _ui->doubleSpinBox_voxel->value()).toDouble());
 	_ui->doubleSpinBox_noiseRadius->setValue(settings.value("cloudNoiseRadius", _ui->doubleSpinBox_noiseRadius->value()).toDouble());
@@ -2111,6 +2131,8 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
 	_ui->checkBox_showLandmarks->setChecked(settings.value("showLandmarks", _ui->checkBox_showLandmarks->isChecked()).toBool());
 	_ui->doubleSpinBox_landmarkSize->setValue(settings.value("landmarkSize", _ui->doubleSpinBox_landmarkSize->value()).toDouble());
+	_ui->checkBox_showIMUGravity->setChecked(settings.value("showIMUGravity", _ui->checkBox_showIMUGravity->isChecked()).toBool());
+	_ui->checkBox_showIMUAcc->setChecked(settings.value("showIMUAcc", _ui->checkBox_showIMUAcc->isChecked()).toBool());
 
 	_ui->radioButton_noFiltering->setChecked(settings.value("noFiltering", _ui->radioButton_noFiltering->isChecked()).toBool());
 	_ui->radioButton_nodeFiltering->setChecked(settings.value("cloudFiltering", _ui->radioButton_nodeFiltering->isChecked()).toBool());
@@ -2551,6 +2573,8 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value());
 		settings.setValue(QString("ptSizeScan%1").arg(i), _3dRenderingPtSizeScan[i]->value());
 		settings.setValue(QString("ptSizeFeatures%1").arg(i), _3dRenderingPtSizeFeatures[i]->value());
+		settings.setValue(QString("gravityShown%1").arg(i), _3dRenderingGravity[i]->isChecked());
+		settings.setValue(QString("gravityLength%1").arg(i), _3dRenderingGravityLength[i]->value());
 	}
 	settings.setValue("cloudVoxel",             _ui->doubleSpinBox_voxel->value());
 	settings.setValue("cloudNoiseRadius",       _ui->doubleSpinBox_noiseRadius->value());
@@ -2568,6 +2592,8 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
 	settings.setValue("showLandmarks", _ui->checkBox_showLandmarks->isChecked());
 	settings.setValue("landmarkSize", _ui->doubleSpinBox_landmarkSize->value());
+	settings.setValue("showIMUGravity", _ui->checkBox_showIMUGravity->isChecked());
+	settings.setValue("showIMUAcc", _ui->checkBox_showIMUAcc->isChecked());
 
 
 	settings.setValue("noFiltering",             _ui->radioButton_noFiltering->isChecked());
@@ -4899,6 +4925,20 @@ double PreferencesDialog::landmarkVisSize() const
 {
 	return _ui->doubleSpinBox_landmarkSize->value();
 }
+bool PreferencesDialog::isIMUGravityShown(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingGravity[index]->isChecked();
+}
+double PreferencesDialog::getIMUGravityLength(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingGravityLength[index]->value();
+}
+bool PreferencesDialog::isIMUAccShown() const
+{
+	return _ui->checkBox_showIMUAcc->isChecked();
+}
 bool PreferencesDialog::isMarkerDetection() const
 {
 	return _ui->RGBDMarkerDetection->isChecked();

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-585</y>
+            <y>-1139</y>
             <width>680</width>
             <height>3083</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1899,6 +1899,107 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="8" column="2">
+                        <widget class="QLabel" name="label_567">
+                         <property name="text">
+                          <string>Show IMU filtered gravity.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="2">
+                        <widget class="QLabel" name="label_568">
+                         <property name="text">
+                          <string>Show IMU acceleration.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="1">
+                        <widget class="QCheckBox" name="checkBox_showIMUAcc">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QCheckBox" name="checkBox_showIMUGravity">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="1">
+                        <widget class="QCheckBox" name="checkBox_showIMUGravity_odom">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_lengthIMUGravity">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="minimum">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_lengthIMUGravity_odom">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="minimum">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="2">
+                        <widget class="QLabel" name="label_569">
+                         <property name="text">
+                          <string>IMU filtered gravity length.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -3058,7 +3159,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <item row="0" column="1">
                               <widget class="QLabel" name="label_228">
                                <property name="text">
-                                <string>Driver</string>
+                                <string>Driver.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>