StereoCameraModel: baseline computed with Tx of left camera too. Fixed QMultiComboBox not showing on windows. Avoid updating grid view if there are no local grids in signatrues. Clear cached data of StatsItem when detecting smaller timestamp than last ones.

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -97,7 +97,7 @@ public:
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
 	bool saveStereoTransform(const std::string & directory) const;
 
-	double baseline() const {return right_.fx()!=0.0?-right_.Tx()/right_.fx():0.0;}
+	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
 
 	float computeDepth(float disparity) const;
 	float computeDisparity(float depth) const; // m

corelib/src/Rtabmap.cpp

@@ -2918,13 +2918,19 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
 					}
 				}
 			}
-			UASSERT_MSG(path.size(), uFormat("nearestId=%d ids=%d", nearestId, (int)ids.size()).c_str());
+			if (path.size())
-			if(maxGraphDepth > 0 && !_memory->isGraphReduced() && (int)path.size() > maxGraphDepth*2+1)
 			{
-				UWARN("%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?",
+				if (maxGraphDepth > 0 && !_memory->isGraphReduced() && (int)path.size() > maxGraphDepth * 2 + 1)
-					Parameters::kMemReduceGraph().c_str(), (int)path.size(), maxGraphDepth, nearestId, (int)ids.size());
+				{
+					UWARN("%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?",
+						Parameters::kMemReduceGraph().c_str(), (int)path.size(), maxGraphDepth, nearestId, (int)ids.size());
+				}
+				paths.insert(std::make_pair(nearestId, path));
+			}
+			else
+			{
+				UWARN(uFormat("path.size()=0!? nearestId=%d ids=%d", (int)path.size(), nearestId, (int)ids.size()).c_str());
 			}
-			paths.insert(std::make_pair(nearestId, path));
 		}
 
 	}

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -151,6 +151,7 @@ public:
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
 	bool isOdomDisabled() const;
+	bool isGroundTruthAligned() const;
 
 	bool isGraphsShown() const;
 	bool isLabelsShown() const;

guilib/include/rtabmap/gui/StatsToolBox.h

@@ -55,6 +55,7 @@ public:
 	std::vector<float> xValues() const {return _x;}
 	std::vector<float> yValues() const {return _y;}
 	void setCacheOn(bool on);
+	void clearCache();
 
 public slots:
 	void updateMenu(const QMenu * menu);
@@ -106,6 +107,7 @@ public slots:
 	void updateStat(const QString & statFullName, float y, bool cacheOn);
 	void updateStat(const QString & statFullName, float x, float y, bool cacheOn);
 	void updateStat(const QString & statFullName, const std::vector<float> & x, const std::vector<float> & y, bool cacheOn);
+	void clear();
 
 signals:
 	void menuChanged(const QMenu *);

guilib/src/3rdParty/QMultiComboBox.cpp

@@ -13,6 +13,7 @@
 #include <QCoreApplication>
 #include <QDesktopWidget>
 
+#include "rtabmap/utilite/ULogger.h"
 
 QMultiComboBox::QMultiComboBox(QWidget *widget ) :
     QComboBox(widget),
@@ -81,7 +82,6 @@ void QMultiComboBox::paintEvent(QPaintEvent *e)
 
 void QMultiComboBox::showPopup()
 {
-
     QRect rec = QRect(geometry());
 
     //QPoint p = this->mapToGlobal(QPoint(0,rec.height()));
@@ -94,11 +94,14 @@ void QMultiComboBox::showPopup()
     QPoint below = this->mapToGlobal(QPoint(0,rec.height()));
     int belowHeight = screen.bottom() - below.y();
 
+	// compute width
+	int textWidth = vlist_.sizeHint().width();
+
     // first activate it with height 1px to get all the items initialized
     QRect rec2;
     rec2.setTopLeft(below);
-    rec2.setWidth(rec.width());
+    rec2.setWidth(textWidth>rec.width()? textWidth:rec.width());
-    rec.setHeight(1);
+    rec2.setHeight(rec.height());
     popframe_.setGeometry(rec2);
     popframe_.raise();
     popframe_.show();
@@ -108,7 +111,6 @@ void QMultiComboBox::showPopup()
     int contheight = vlist_.count()*vlist_.sizeHintForRow(0) + 4; // +4 - should be determined by margins?
     belowHeight = min(abs(belowHeight)-screenbound_, contheight);
     aboveHeight = min(abs(aboveHeight)-screenbound_, contheight);
-
     if (popheight_ > 0) // fixed
     {
         rec2.setHeight(popheight_);
@@ -127,7 +129,6 @@ void QMultiComboBox::showPopup()
             rec2.setHeight(aboveHeight);
         }
     }
-
     popframe_.setGeometry(rec2);
     popframe_.raise();
     popframe_.show();
@@ -149,6 +150,7 @@ void QMultiComboBox::addItem ( const QString & text, const QVariant & userData)
     else
         wi->setCheckState(Qt::Unchecked);
     vlist_.addItem(wi);
+	vlist_.setMinimumWidth(vlist_.sizeHintForColumn(0));
 }
 
 

guilib/src/GraphViewer.cpp

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UCv2Qt.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 
 namespace rtabmap {
 
@@ -277,6 +278,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::multimap<int, Link> & constraints,
 				 const std::map<int, int> & mapIds)
 {
+	UTimer timer;
 	bool wasVisible = _graphRoot->isVisible();
 	_graphRoot->show();
 
@@ -488,11 +490,12 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 
 	_graphRoot->setVisible(wasVisible);
 
-	UDEBUG("_nodeItems=%d, _linkItems=%d", _nodeItems.size(), _linkItems.size());
+	UDEBUG("_nodeItems=%d, _linkItems=%d, timer=%fs", _nodeItems.size(), _linkItems.size(), timer.ticks());
 }
 
 void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 {
+	UTimer timer;
 	bool wasVisible = _gtGraphRoot->isVisible();
 	_gtGraphRoot->show();
 	bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
@@ -613,7 +616,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 
 	_gtGraphRoot->setVisible(wasVisible);
 
-	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d", _gtNodeItems.size(), _gtLinkItems.size());
+	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks());
 }
 
 void GraphViewer::updateReferentialPosition(const Transform & t)

guilib/src/MainWindow.cpp

@@ -942,126 +942,112 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			   (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
 			   _preferencesDialog->isCloudsShown(1))
 			{
-
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-				if(odom.data().imageRaw().cols % _preferencesDialog->getCloudDecimation(1) != 0 ||
+				pcl::IndicesPtr indices(new std::vector<int>);
-				   odom.data().imageRaw().rows % _preferencesDialog->getCloudDecimation(1) != 0)
+				cloud = util3d::cloudRGBFromSensorData(odom.data(),
-				{
+						_preferencesDialog->getCloudDecimation(1),
-					UERROR("Decimation (%d) is not modulo of the image resolution (%dx%d)! The cloud cannot be "
+						_preferencesDialog->getCloudMaxDepth(1),
-							"created. Go to Preferences->3D Rendering under \"Odom\" column to modify this parameter.",
+						_preferencesDialog->getCloudMinDepth(1),
-							_preferencesDialog->getCloudDecimation(1),
+						indices.get(),
-							odom.data().imageRaw().cols,
+						_preferencesDialog->getAllParameters());
-							odom.data().imageRaw().rows);
+				if(indices->size())
-				}
-				else
 				{
+					cloud = util3d::transformPointCloud(cloud, pose);
 
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+					if(_preferencesDialog->isCloudMeshing())
-					pcl::IndicesPtr indices(new std::vector<int>);
-					cloud = util3d::cloudRGBFromSensorData(odom.data(),
-							_preferencesDialog->getCloudDecimation(1),
-							_preferencesDialog->getCloudMaxDepth(1),
-							_preferencesDialog->getCloudMinDepth(1),
-							indices.get(),
-							_preferencesDialog->getAllParameters());
-					if(indices->size())
 					{
-						cloud = util3d::transformPointCloud(cloud, pose);
+						// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+						output = util3d::extractIndices(cloud, indices, false, true);
 
-						if(_preferencesDialog->isCloudMeshing())
+						// Fast organized mesh
+						Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
+						if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
 						{
-							// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
+							viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
-							pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+							viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
-							output = util3d::extractIndices(cloud, indices, false, true);
+							viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
-
+						}
-							// Fast organized mesh
+						else if(!odom.data().stereoCameraModel().localTransform().isNull())
-							Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
+						{
-							if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
+							viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
+							viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
+							viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
+						}
+						std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
+								output,
+								_preferencesDialog->getCloudMeshingAngle(),
+								_preferencesDialog->isCloudMeshingQuad(),
+								_preferencesDialog->getCloudMeshingTriangleSize(),
+								Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
+						if(polygons.size())
+						{
+							if(_preferencesDialog->isCloudMeshingTexture() && !odom.data().imageRaw().empty())
 							{
-								viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
+								pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
-								viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
+								pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
-								viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
+								textureMesh->tex_polygons.push_back(polygons);
-							}
+								int w = cloud->width;
-							else if(!odom.data().stereoCameraModel().localTransform().isNull())
+								int h = cloud->height;
-							{
+								UASSERT(w > 1 && h > 1);
-								viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
+								textureMesh->tex_coordinates.resize(1);
-								viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
+								int nPoints = textureMesh->cloud.data.size()/textureMesh->cloud.point_step;
-								viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
+								textureMesh->tex_coordinates[0].resize(nPoints);
-							}
+								for(int i=0; i<nPoints; ++i)
-							std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
-									output,
-									_preferencesDialog->getCloudMeshingAngle(),
-									_preferencesDialog->isCloudMeshingQuad(),
-									_preferencesDialog->getCloudMeshingTriangleSize(),
-									Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
-							if(polygons.size())
-							{
-								if(_preferencesDialog->isCloudMeshingTexture() && !odom.data().imageRaw().empty())
 								{
-									pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
+									//uv
-									pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
+									textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
-									textureMesh->tex_polygons.push_back(polygons);
+											float(i % w) / float(w),      // u
-									int w = cloud->width;
+											float(h - i / w) / float(h)); // v
-									int h = cloud->height;
-									UASSERT(w > 1 && h > 1);
-									textureMesh->tex_coordinates.resize(1);
-									int nPoints = textureMesh->cloud.data.size()/textureMesh->cloud.point_step;
-									textureMesh->tex_coordinates[0].resize(nPoints);
-									for(int i=0; i<nPoints; ++i)
-									{
-										//uv
-										textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
-												float(i % w) / float(w),      // u
-												float(h - i / w) / float(h)); // v
-									}
-
-									pcl::TexMaterial mesh_material;
-									mesh_material.tex_d = 1.0f;
-									mesh_material.tex_Ns = 75.0f;
-									mesh_material.tex_illum = 1;
-
-									mesh_material.tex_name = "material_odom";
-
-									QDir dir(_preferencesDialog->getWorkingDirectory());
-									ExportCloudsDialog::removeDirRecursively(_preferencesDialog->getWorkingDirectory()+QDir::separator()+"tmp_textures");
-									dir.mkdir("tmp_textures");
-
-									std::string tmpDirectory = dir.filePath("tmp_textures").toStdString();
-									mesh_material.tex_file = uFormat("%s/%s.png", tmpDirectory.c_str(), "texture_odom");
-									if(!cv::imwrite(mesh_material.tex_file, odom.data().imageRaw()))
-									{
-										UERROR("Cannot save texture of image odom");
-									}
-									else
-									{
-										UINFO("Saved temporary texture: \"%s\"", mesh_material.tex_file.c_str());
-									}
-
-									textureMesh->tex_materials.push_back(mesh_material);
-
-									if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, _odometryCorrection))
-									{
-										UERROR("Adding cloudOdom to viewer failed!");
-									}
 								}
-								else if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
+
+								pcl::TexMaterial mesh_material;
+								mesh_material.tex_d = 1.0f;
+								mesh_material.tex_Ns = 75.0f;
+								mesh_material.tex_illum = 1;
+
+								mesh_material.tex_name = "material_odom";
+
+								QDir dir(_preferencesDialog->getWorkingDirectory());
+								ExportCloudsDialog::removeDirRecursively(_preferencesDialog->getWorkingDirectory()+QDir::separator()+"tmp_textures");
+								dir.mkdir("tmp_textures");
+
+								std::string tmpDirectory = dir.filePath("tmp_textures").toStdString();
+								mesh_material.tex_file = uFormat("%s/%s.png", tmpDirectory.c_str(), "texture_odom");
+								if(!cv::imwrite(mesh_material.tex_file, odom.data().imageRaw()))
+								{
+									UERROR("Cannot save texture of image odom");
+								}
+								else
+								{
+									UINFO("Saved temporary texture: \"%s\"", mesh_material.tex_file.c_str());
+								}
+
+								textureMesh->tex_materials.push_back(mesh_material);
+
+								if(!_cloudViewer->addCloudTextureMesh("cloudOdom", textureMesh, _odometryCorrection))
 								{
 									UERROR("Adding cloudOdom to viewer failed!");
 								}
 							}
-						}
+							else if(!_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
-						else
-						{
-							if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
 							{
 								UERROR("Adding cloudOdom to viewer failed!");
 							}
 						}
-						_cloudViewer->setCloudVisibility("cloudOdom", true);
-						_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
-						_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
-
-						cloudUpdated = true;
 					}
+					else
+					{
+						if(!_cloudViewer->addCloud("cloudOdom", cloud, _odometryCorrection))
+						{
+							UERROR("Adding cloudOdom to viewer failed!");
+						}
+					}
+					_cloudViewer->setCloudVisibility("cloudOdom", true);
+					_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
+					_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
+
+					cloudUpdated = true;
 				}
 			}
 
@@ -1436,7 +1422,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			signature = stat.getSignatures().at(stat.refImageId());
 			signature.sensorData().uncompressData(); // make sure data are uncompressed
 
-			if(!smallMovement && uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())))
+			if(!smallMovement && 
+			   uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
+				signature.getWeight()>=0) // ignore intermediate nodes for the cache
 			{
 				_cachedSignatures.insert(signature.id(), signature);
 				_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
@@ -2132,14 +2120,17 @@ void MainWindow::updateMapCloud(
 					{
 						cv::Mat ground;
 						cv::Mat obstacles;
-						jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+						if (jter->sensorData().gridCellSize() > 0.0f)
-						_gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
-						_gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint()));
-						_cachedGridsMemoryUsage += ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize();
-
-						if(ground.cols || obstacles.cols)
 						{
-							_occupancyGrid->addToCache(iter->first, ground, obstacles);
+							jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+							_gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+							_gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint()));
+							_cachedGridsMemoryUsage += ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize();
+
+							if (ground.cols || obstacles.cols)
+							{
+								_occupancyGrid->addToCache(iter->first, ground, obstacles);
+							}
 						}
 					}
 #ifdef RTABMAP_OCTOMAP
@@ -3002,7 +2993,7 @@ Transform MainWindow::alignPosesToGroundTruth(
 		const std::map<int, Transform> & groundTruth)
 {
 	Transform t = Transform::getIdentity();
-	if(groundTruth.size() && poses.size())
+	if(groundTruth.size() && poses.size() && _preferencesDialog->isGroundTruthAligned())
 	{
 		unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
 		pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
@@ -5301,6 +5292,7 @@ void MainWindow::clearTheCache()
 	_currentLabels.clear();
 	_odometryCorrection = Transform::getIdentity();
 	_lastOdomPose.setNull();
+	_ui->statsToolBox->clear();
 	//disable save cloud action
 	_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
 	_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);

guilib/src/PreferencesDialog.cpp

@@ -292,6 +292,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->checkbox_groundTruthAlign, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
 	// Cloud rendering panel
 	_3dRenderingShowClouds.resize(2);
@@ -1212,6 +1213,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_odomQualityWarnThr->setValue(50);
 		_ui->checkBox_posteriorGraphView->setChecked(true);
 		_ui->checkbox_odomDisabled->setChecked(false);
+		_ui->checkbox_groundTruthAlign->setChecked(true);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_cloudRendering1->objectName())
 	{
@@ -1595,6 +1597,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
+	_ui->checkbox_groundTruthAlign->setChecked(settings.value("gtAlign", _ui->checkbox_groundTruthAlign->isChecked()).toBool());
 
 	for(int i=0; i<2; ++i)
 	{
@@ -1973,6 +1976,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("odomQualityThr",       _ui->spinBox_odomQualityWarnThr->value());
 	settings.setValue("posteriorGraphView",   _ui->checkBox_posteriorGraphView->isChecked());
 	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
+	settings.setValue("gtAlign",              _ui->checkbox_groundTruthAlign->isChecked());
 
 	for(int i=0; i<2; ++i)
 	{
@@ -3784,6 +3788,10 @@ bool PreferencesDialog::isOdomDisabled() const
 {
 	return _ui->checkbox_odomDisabled->isChecked();
 }
+bool PreferencesDialog::isGroundTruthAligned() const
+{
+	return _ui->checkbox_groundTruthAlign->isChecked();
+}
 
 bool PreferencesDialog::isCloudsShown(int index) const
 {

guilib/src/StatsToolBox.cpp

@@ -77,6 +77,12 @@ StatItem::~StatItem()
 
 }
 
+void StatItem::clearCache()
+{
+	_x.clear();
+	_y.clear();
+}
+
 void StatItem::addValue(float y)
 {
 	if(_cacheOn)
@@ -95,6 +101,11 @@ void StatItem::addValue(float x, float y)
 {
 	if(_cacheOn)
 	{
+		if (_x.size() && x <_x.back())
+		{
+			clearCache();
+		}
+
 		if(_y.size() % 100)
 		{
 			_y.reserve(_y.size()+100);
@@ -511,6 +522,28 @@ void StatsToolBox::figureDeleted(QObject * obj)
 	}
 }
 
+void StatsToolBox::clear()
+{
+	for (QMap<QString, QWidget*>::iterator i = _figures.begin(); i != _figures.end(); ++i)
+	{
+		QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
+		if (plots.size() == 1)
+		{
+			QStringList names = plots[0]->curveNames();
+			plots[0]->clearData();
+		}
+		else
+		{
+			UERROR("");
+		}
+	}
+	QList<StatItem*> items = _statBox->currentWidget()->findChildren<StatItem*>();
+	for (int i = 0; i<items.size(); ++i)
+	{
+		items[i]->clearCache();
+	}
+}
+
 void StatsToolBox::contextMenuEvent(QContextMenuEvent * event)
 {
 	QMenu topMenu(this);
@@ -524,19 +557,7 @@ void StatsToolBox::contextMenuEvent(QContextMenuEvent * event)
 	{
 		if(action == aClearFigures)
 		{
-			for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
+			this->clear();
-			{
-				QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
-				if(plots.size() == 1)
-				{
-					QStringList names = plots[0]->curveNames();
-					plots[0]->clearData();
-				}
-				else
-				{
-					UERROR("");
-				}
-			}
 		}
 		else
 		{

guilib/src/ui/preferencesDialog.ui

@@ -64,15 +64,24 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>673</width>
+            <width>677</width>
-            <height>2520</height>
+            <height>2435</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="margin">
+           <property name="leftMargin">
+            <number>0</number>
+           </property>
+           <property name="topMargin">
+            <number>0</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -98,16 +107,10 @@
                    <layout class="QVBoxLayout" name="verticalLayout_28">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_39" columnstretch="0,1">
-                      <item row="3" column="1">
+                      <item row="2" column="0">
-                       <widget class="QLabel" name="label_247">
+                       <widget class="QCheckBox" name="checkBox_beep">
                         <property name="text">
-                         <string>Use time for figures' x-axis. Otherwise, sensor data IDs are used for camera/odometry info and for all mapping statistics, node IDs are used.</string>
+                         <string/>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                         </property>
                        </widget>
                       </item>
@@ -121,6 +124,19 @@
                         </property>
                        </widget>
                       </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_247">
+                        <property name="text">
+                         <string>Use time for figures' x-axis. Otherwise, sensor data IDs are used for camera/odometry info and for all mapping statistics, node IDs are used.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="1" column="1">
                        <widget class="QLabel" name="label_313">
                         <property name="text">
@@ -134,6 +150,19 @@
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_102">
+                        <property name="text">
+                         <string>Notify when a new global path is received.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="general_checkBox_imagesKept">
                         <property name="text">
@@ -144,19 +173,6 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_102">
-                        <property name="text">
-                         <string>Notify when a new global path is received.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="1" column="0">
                        <widget class="QCheckBox" name="general_checkBox_cloudsKept">
                         <property name="text">
@@ -167,39 +183,6 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="0" column="1">
-                       <widget class="QLabel" name="label_59">
-                        <property name="text">
-                         <string>Insert new data received in the GUI cache. Used to show the loop closure image and the 3D Map.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="0">
-                       <widget class="QCheckBox" name="checkBox_beep">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_88">
-                        <property name="text">
-                         <string>Beep! on special events (finished processing the data set, an error has occured, ...).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="3" column="0">
                        <widget class="QCheckBox" name="checkBox_stamps">
                         <property name="text">
@@ -220,8 +203,34 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
+                      <item row="2" column="1">
-                       <widget class="QCheckBox" name="checkBox_cacheStatistics">
+                       <widget class="QLabel" name="label_88">
+                        <property name="text">
+                         <string>Beep! on special events (finished processing the data set, an error has occured, ...).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_59">
+                        <property name="text">
+                         <string>Insert new data received in the GUI cache. Used to show the loop closure image and the 3D Map.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="checkbox_odomDisabled">
                         <property name="text">
                          <string/>
                         </property>
@@ -240,8 +249,28 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
+                      <item row="4" column="0">
-                       <widget class="QCheckBox" name="checkbox_odomDisabled">
+                       <widget class="QCheckBox" name="checkBox_cacheStatistics">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_347">
+                        <property name="text">
+                         <string>When a ground truth is provided, align the map with it.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="checkbox_groundTruthAlign">
                         <property name="text">
                          <string/>
                         </property>
@@ -4406,7 +4435,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="margin">
+                       <property name="leftMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="topMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="rightMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -10376,7 +10414,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                       <widget class="QWidget" name="page_54">
                        <layout class="QVBoxLayout" name="verticalLayout_85">
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -10516,7 +10563,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </widget>
                       <widget class="QWidget" name="page_55">
                        <layout class="QVBoxLayout" name="verticalLayout_86">
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -10674,7 +10730,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -10754,7 +10819,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -10866,7 +10940,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>

guilib/src/utilite/UPlot.cpp

@@ -430,8 +430,8 @@ void UPlotCurve::addValue(UPlotItem * data)
 void UPlotCurve::addValue(float x, float y)
 {
 	if(_items.size() &&
-		dynamic_cast<UPlotItem*>(_items.back()) &&
+		_minMax[0] != _minMax[1] &&
-		x < ((UPlotItem*)_items.back())->data().x())
+		x < _minMax[1])
 	{
 		UWARN("New value (%f) added to curve \"%s\" is smaller "
 			  "than the last added (%f). Clearing the curve.",