0.11.1: Added "ground_truth_pose" field to database in Node table. MainWindow: alignment of the map to ground truth (if ground truth is present).

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -166,6 +166,7 @@ private:
 	std::list<std::map<int, rtabmap::Transform> > graphes_;
 	std::multimap<int, rtabmap::Link> graphLinks_;
 	std::map<int, rtabmap::Transform> poses_;
+	std::map<int, rtabmap::Transform> groundTruthPoses_;
 	std::multimap<int, rtabmap::Link> links_;
 	std::multimap<int, rtabmap::Link> linksRefined_;
 	std::multimap<int, rtabmap::Link> linksAdded_;

guilib/include/rtabmap/gui/MainWindow.h

@@ -229,9 +229,11 @@ private:
 			const std::multimap<int, Link> & constraints,
 			const std::map<int, int> & mapIds,
 			const std::map<int, std::string> & labels,
+			const std::map<int, Transform> & groundTruths,
 			bool verboseProgress = false);
 	void createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
+	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceMenu();
@@ -296,7 +298,7 @@ private:
 	std::map<int, Transform> _currentGTPosesMap; // <nodeId, pose>
 	std::multimap<int, Link> _currentLinksMap; // <nodeFromId, link>
 	std::map<int, int> _currentMapIds;   // <nodeId, mapId>
-	std::map<int, std::string> _curentLabels; // <nodeId, label>
+	std::map<int, std::string> _currentLabels; // <nodeId, label>
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -124,7 +124,7 @@ public:
 	void saveCustomConfig(const QString & section, const QString & key, const QString & value);
 	QString loadCustomConfig(const QString & section, const QString & key);
 
-	rtabmap::ParametersMap getAllParameters();
+	const rtabmap::ParametersMap & getAllParameters() const;
 
 	//General panel
 	int getGeneralLoggerLevel() const;
@@ -315,6 +315,7 @@ private:
 
 protected:
 	rtabmap::ParametersMap _parameters;
+	rtabmap::ParametersMap _modifiedParameters;
 	PANEL_FLAGS _obsoletePanels;
 
 private:

guilib/src/DatabaseViewer.cpp

@@ -333,7 +333,7 @@ QString DatabaseViewer::getIniFilePath() const
 	{
 		QDir::home().mkdir(".rtabmap");
 	}
-	return privatePath + "/dbviewer.ini";
+	return privatePath + "/rtabmap.ini";
 }
 
 void DatabaseViewer::readSettings()
@@ -515,6 +515,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			graphes_.clear();
 			graphLinks_.clear();
 			poses_.clear();
+			groundTruthPoses_.clear();
 			mapIds_.clear();
 			links_.clear();
 			linksAdded_.clear();
@@ -711,14 +712,16 @@ void DatabaseViewer::exportDatabase()
 			std::vector<double> delays(ids_.size());
 			int oi=0;
 			std::map<int, Transform> poses;
+			std::map<int, double> stamps;
+			std::map<int, Transform> groundTruths;
 			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
-				Transform odomPose;
+				Transform odomPose, groundTruth;
 				int weight = -1;
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
-				if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp))
+				if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth))
 				{
 					if(frameRate == 0 ||
 					   previousStamp == 0 ||
@@ -736,6 +739,8 @@ void DatabaseViewer::exportDatabase()
 							previousStamp = stamp;
 
 							poses.insert(std::make_pair(ids_[i], odomPose));
+							stamps.insert(std::make_pair(ids_[i], stamp));
+							groundTruths.insert(std::make_pair(ids_[i], groundTruth));
 						}
 					}
 					if(sessionExported >= 0 && mapId > sessionExported)
@@ -791,8 +796,8 @@ void DatabaseViewer::exportDatabase()
 							rgb,
 							depth,
 							data.cameraModels(),
-							data.id(),
-							data.stamp(),
+							id,
+							stamps.at(id),
 							userData);
 					}
 					else
@@ -804,10 +809,11 @@ void DatabaseViewer::exportDatabase()
 							rgb,
 							depth,
 							data.stereoCameraModel(),
-							data.id(),
-							data.stamp(),
+							id,
+							stamps.at(id),
 							userData);
 					}
+					sensorData.setGroundTruth(groundTruths.at(id));
 
 					recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
 
@@ -970,6 +976,7 @@ void DatabaseViewer::updateIds()
 	idToIndex_.clear();
 	mapIds_.clear();
 	poses_.clear();
+	groundTruthPoses_.clear();
 	links_.clear();
 	linksAdded_.clear();
 	linksRefined_.clear();
@@ -991,12 +998,12 @@ void DatabaseViewer::updateIds()
 	{
 		idToIndex_.insert(ids_[i], i);
 
-		Transform p;
+		Transform p, g;
 		int w;
 		std::string l;
 		double s;
 		int mapId;
-		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s);
+		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g);
 		mapIds_.insert(std::make_pair(ids_[i], mapId));
 
 		if(i>0)
@@ -1053,6 +1060,10 @@ void DatabaseViewer::updateIds()
 		if(addPose)
 		{
 			poses_.insert(std::make_pair(ids_[i], p));
+			if(!g.isNull())
+			{
+				groundTruthPoses_.insert(std::make_pair(ids_[i], g));
+			}
 		}
 
 		if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
@@ -1075,6 +1086,7 @@ void DatabaseViewer::updateIds()
 	ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids.size()).arg(dbDriver_->getTotalDictionarySize()));
 	ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
 	ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(poses_.size()).arg(links_.size()));
+	ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
 	ui_->textEdit_info->append("");
 	ui_->textEdit_info->append(tr("Database size:\t%1 MB").arg(dbDriver_->getMemoryUsed()/1000000));
 	ui_->textEdit_info->append(tr("Images size:\t%1 MB").arg(dbDriver_->getImagesMemoryUsed()/1000000));
@@ -1456,10 +1468,10 @@ void DatabaseViewer::view3DMap()
 						{
 							QColor color = Qt::red;
 							int mapId, weight;
-							Transform odomPose;
+							Transform odomPose, groundTruth;
 							std::string label;
 							double stamp;
-							if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp))
+							if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth))
 							{
 								color = (Qt::GlobalColor)(mapId % 12 + 7 );
 							}
@@ -1561,10 +1573,10 @@ void DatabaseViewer::view3DLaserScans()
 					{
 						QColor color = Qt::red;
 						int mapId, weight;
-						Transform odomPose;
+						Transform odomPose, groundTruth;
 						std::string label;
 						double stamp;
-						if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp))
+						if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth))
 						{
 							color = (Qt::GlobalColor)(mapId % 12 + 7 );
 						}
@@ -2103,11 +2115,11 @@ void DatabaseViewer::update(int value,
 					signatures.clear();
 				}
 
-				Transform odomPose;
+				Transform odomPose, g;
 				int w;
 				std::string l;
 				double s;
-				dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s);
+				dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g);
 
 				weight->setNum(w);
 				label->setText(l.c_str());
@@ -3211,6 +3223,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("Update local maps list... done");
 		}
 
+		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
 		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
 		if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
 		{
@@ -3256,6 +3269,7 @@ void DatabaseViewer::updateGraphView()
 {
 	ui_->label_loopClosures->clear();
 	ui_->label_poses->clear();
+
 	if(poses_.size())
 	{
 		int fromId = ui_->spinBox_optimizationsFrom->value();
@@ -3429,6 +3443,20 @@ void DatabaseViewer::updateGraphView()
 		{
 			QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
 		}
+
+		if(uContains(groundTruthPoses_, fromId) && uContains(posesOut, fromId))
+		{
+			// adjust the ground truth to fit the root
+			Transform t = posesOut.at(fromId) * groundTruthPoses_.at(fromId).inverse();
+			for(std::map<int, Transform>::iterator iter=groundTruthPoses_.begin(); iter!=groundTruthPoses_.end(); ++iter)
+			{
+				iter->second = t * iter->second;
+			}
+		}
+		else if(groundTruthPoses_.size())
+		{
+			UWARN("Could not find ground truth for root node %d", fromId);
+		}
 	}
 	if(graphes_.size())
 	{

guilib/src/GraphViewer.cpp

@@ -104,13 +104,13 @@ class LinkItem: public QGraphicsLineItem
 {
 public:
 	// in meter
-	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, Link::Type type, bool interSessionClosure) :
+	LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, const Link & link, bool interSessionClosure) :
 		QGraphicsLineItem(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x()),
 		_from(from),
 		_to(to),
 		_poseA(poseA),
 		_poseB(poseB),
-		_type(type),
+		_link(link),
 		_interSession(interSessionClosure)
 	{
 		this->setAcceptHoverEvents(true);
@@ -140,7 +140,7 @@ public:
 		return _poseB;
 	}
 
-	Link::Type linkType() const {return _type;}
+	Link::Type linkType() const {return _link.type();}
 	bool isInterSession() const {return _interSession;}
 	int from() const {return _from;}
 	int to() const {return _to;}
@@ -148,7 +148,12 @@ public:
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
 	{
-		this->setToolTip(QString("%1->%2 %3 m").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB)));
+		QString str = QString("%1->%2 (%3 m)").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB));
+		if(!_link.transform().isNull())
+		{
+			str.append(QString("\n%1\n%2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance()));
+		}
+		this->setToolTip(str);
 		QPen pen = this->pen();
 		pen.setWidthF(pen.widthF()+0.02);
 		this->setPen(pen);
@@ -168,7 +173,7 @@ private:
 	int _to;
 	Transform _poseA;
 	Transform _poseB;
-	Link::Type _type;
+	Link _link;
 	bool _interSession;
 };
 
@@ -333,7 +338,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				if(linkItem == 0)
 				{
 					//create a link item
-					linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure);
+					linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second, interSessionClosure);
 					QPen p = linkItem->pen();
 					p.setWidthF(_linkWidth);
 					linkItem->setPen(p);
@@ -526,7 +531,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 				if(linkItem == 0)
 				{
 					//create a link item
-					linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link::kUndef, 1);
+					linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1);
 					QPen p = linkItem->pen();
 					p.setWidthF(_linkWidth);
 					linkItem->setPen(p);
@@ -655,7 +660,7 @@ void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > &
 			//create a link item
 			int idFrom = globalPath[i].first;
 			int idTo = globalPath[i+1].first;
-			LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link::kUndef, false);
+			LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link(), false);
 			QPen p = item->pen();
 			p.setWidthF(_linkWidth);
 			item->setPen(p);
@@ -731,7 +736,7 @@ void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 				if(!updated)
 				{
 					//create a link item
-					LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link::kUndef, false);
+					LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link(), false);
 					QPen p = item->pen();
 					p.setWidthF(_linkWidth);
 					item->setPen(p);

guilib/src/MainWindow.cpp

@@ -1102,11 +1102,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	if(uContains(stat.getSignatures(), stat.refImageId()))
 	{
 		refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
-		if(!stat.getSignatures().at(stat.refImageId()).sensorData().groundTruth().isNull())
-		{
-			_currentGTPosesMap.insert(std::make_pair(stat.refImageId(), stat.getSignatures().at(stat.refImageId()).sensorData().groundTruth()));
-			_ui->actionAnchor_clouds_to_ground_truth->setEnabled(true);
-		}
 	}
 	int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
 	int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
@@ -1330,24 +1325,35 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			// update pose only if odometry is not received
 			std::map<int, int> mapIds;
+			std::map<int, Transform> groundTruth;
 			std::map<int, std::string> labels;
 			for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
 			{
 				mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
+				if(!iter->second.getGroundTruthPose().isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
+				}
 				if(!iter->second.getLabel().empty())
 				{
 					labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
 				}
 			}
-			updateMapCloud(stat.poses(),
-					_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
+
+			std::map<int, Transform> poses = stat.poses();
+			Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth);
+
+			updateMapCloud(
+					poses,
+					_odometryReceived||poses.size()==0?Transform():poses.rbegin()->second,
 					stat.constraints(),
 					mapIds,
-					labels);
+					labels,
+					groundTruth);
 
 			_odometryReceived = false;
 
-			_odometryCorrection = stat.mapCorrection();
+			_odometryCorrection = groundTruthOffset * stat.mapCorrection();
 
 			UDEBUG("time= %d ms", time.restart());
 			_ui->statsToolBox->updateStat("/Gui RGB-D cloud/ms", stat.refImageId(), int(timerVis.elapsed()*1000.0f));
@@ -1433,6 +1439,7 @@ void MainWindow::updateMapCloud(
 		const std::multimap<int, Link> & constraints,
 		const std::map<int, int> & mapIdsIn,
 		const std::map<int, std::string> & labels,
+		const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
 		bool verboseProgress)
 {
 	UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d currentPose=%s",
@@ -1442,12 +1449,14 @@ void MainWindow::updateMapCloud(
 		_currentPosesMap = posesIn;
 		_currentLinksMap = constraints;
 		_currentMapIds = mapIdsIn;
-		_curentLabels = labels;
+		_currentLabels = labels;
+		_currentGTPosesMap = groundTruths;
 		if(_state != kMonitoring && _state != kDetecting)
 		{
 			_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 			_ui->menuExport_poses->setEnabled(!_currentPosesMap.empty());
 		}
+		_ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
 	}
 
 	// filter duplicated poses
@@ -1492,7 +1501,7 @@ void MainWindow::updateMapCloud(
 	}
 	_ui->widget_mapVisibility->setMap(posesIn, posesMask);
 
-	if(_ui->actionAnchor_clouds_to_ground_truth->isChecked() && _currentGTPosesMap.size())
+	if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
 	{
 		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
@@ -2063,6 +2072,55 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 	}
 }
 
+Transform MainWindow::alignPosesToGroundTruth(
+		std::map<int, Transform> & poses,
+		const std::map<int, Transform> & groundTruth)
+{
+	Transform t = Transform::getIdentity();
+	if(groundTruth.size())
+	{
+		if(uContains(_preferencesDialog->getAllParameters(), Parameters::kRGBDOptimizeFromGraphEnd()))
+		{
+			bool optimizeFromGraphEnd = uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kRGBDOptimizeFromGraphEnd()));
+			// Align poses to ground truth
+			int rootId = 0;
+			if(!optimizeFromGraphEnd)
+			{
+				for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
+				{
+					std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
+					if(iter2!=poses.end())
+					{
+						rootId = iter->first;
+						break;
+					}
+				}
+			}
+			else
+			{
+				for(std::map<int, Transform>::const_reverse_iterator iter=groundTruth.rbegin(); iter!=groundTruth.rend(); ++iter)
+				{
+					std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
+					if(iter2!=poses.end())
+					{
+						rootId = iter->first;
+						break;
+					}
+				}
+			}
+			if(rootId>0)
+			{
+				t = groundTruth.at(rootId) * poses.at(rootId).inverse();
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					iter->second = t * iter->second;
+				}
+			}
+		}
+	}
+	return t;
+}
+
 void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 {
 	if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
@@ -2253,12 +2311,17 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 
 		int addedSignatures = 0;
 		std::map<int, int> mapIds;
+		std::map<int, Transform> groundTruth;
 		std::map<int, std::string> labels;
 		for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
 			iter!=event.getSignatures().end();
 			++iter)
 		{
 			mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
+			if(!iter->second.getGroundTruthPose().isNull())
+			{
+				groundTruth.insert(std::make_pair(iter->first, iter->second.getGroundTruthPose()));
+			}
 			if(!iter->second.getLabel().empty())
 			{
 				labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
@@ -2283,7 +2346,9 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			_initProgressDialog->appendText("Updating the 3D map cloud...");
 			_initProgressDialog->incrementStep();
 			QApplication::processEvents();
-			this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), mapIds, labels, true);
+			std::map<int, Transform> poses = event.getPoses();
+			alignPosesToGroundTruth(poses, groundTruth);
+			this->updateMapCloud(poses, Transform(), event.getConstraints(), mapIds, labels, groundTruth, true);
 			_initProgressDialog->appendText("Updating the 3D map cloud... done.");
 		}
 		else
@@ -2430,7 +2495,8 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 					Transform(),
 					std::multimap<int, Link>(_currentLinksMap),
 					std::map<int, int>(_currentMapIds),
-					std::map<int, std::string>(_curentLabels));
+					std::map<int, std::string>(_currentLabels),
+					std::map<int, Transform>(_currentGTPosesMap));
 		}
 	}
 
@@ -2935,6 +3001,7 @@ void MainWindow::editDatabase()
 
 void MainWindow::startDetection()
 {
+	UDEBUG("");
 	ParametersMap parameters = _preferencesDialog->getAllParameters();
 	// verify source with input rates
 	if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
@@ -3726,11 +3793,14 @@ void MainWindow::postProcessing()
 	}
 
 	_initProgressDialog->appendText(tr("Updating map..."));
+	alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap);
 	this->updateMapCloud(
-			optimizedPoses, Transform(),
+			optimizedPoses,
+			Transform(),
 			std::multimap<int, Link>(_currentLinksMap),
 			std::map<int, int>(_currentMapIds),
-			std::map<int, std::string>(_curentLabels),
+			std::map<int, std::string>(_currentLabels),
+			std::map<int, Transform>(_currentGTPosesMap),
 			false);
 	_initProgressDialog->appendText(tr("Updating map... done!"));
 
@@ -4092,7 +4162,8 @@ void MainWindow::anchorCloudsToGroundTruth()
 			Transform(),
 			std::multimap<int, Link>(_currentLinksMap),
 			std::map<int, int>(_currentMapIds),
-			std::map<int, std::string>(_curentLabels));
+			std::map<int, std::string>(_currentLabels),
+			std::map<int, Transform>(_currentGTPosesMap));
 }
 
 void MainWindow::clearTheCache()
@@ -4110,7 +4181,7 @@ void MainWindow::clearTheCache()
 	_currentGTPosesMap.clear();
 	_currentLinksMap.clear();
 	_currentMapIds.clear();
-	_curentLabels.clear();
+	_currentLabels.clear();
 	_odometryCorrection = Transform::getIdentity();
 	_lastOdomPose.setNull();
 	//disable save cloud action

guilib/src/PreferencesDialog.cpp

@@ -990,7 +990,7 @@ void PreferencesDialog::clicked(const QModelIndex & current, const QModelIndex &
 void PreferencesDialog::closeEvent(QCloseEvent *event)
 {
 	UDEBUG("");
-	_parameters.clear();
+	_modifiedParameters.clear();
 	_obsoletePanels = kPanelDummy;
 	this->readGuiSettings(getTmpIniFilePath());
 	this->readCameraSettings(getTmpIniFilePath());
@@ -1005,7 +1005,7 @@ void PreferencesDialog::closeDialog ( QAbstractButton * button )
 	switch(role)
 	{
 	case QDialogButtonBox::RejectRole:
-		_parameters.clear();
+		_modifiedParameters.clear();
 		_obsoletePanels = kPanelDummy;
 		this->readGuiSettings(getTmpIniFilePath());
 		this->readCameraSettings(getTmpIniFilePath());
@@ -1014,7 +1014,7 @@ void PreferencesDialog::closeDialog ( QAbstractButton * button )
 
 	case QDialogButtonBox::AcceptRole:
 		updateBasicParameter();// make that changes without editing finished signal are updated.
-		if((_obsoletePanels & kPanelAll) || _parameters.size())
+		if((_obsoletePanels & kPanelAll) || _modifiedParameters.size())
 		{
 			if(validateForm())
 			{
@@ -1327,6 +1327,7 @@ void PreferencesDialog::readSettings(const QString & filePath)
 	if(!readCoreSettings(filePath))
 	{
 		_parameters.clear();
+		_modifiedParameters.clear();
 		_obsoletePanels = kPanelDummy;
 
 		// only keep GUI settings
@@ -1690,11 +1691,11 @@ void PreferencesDialog::writeSettings(const QString & filePath)
 	writeCameraSettings(filePath);
 	writeCoreSettings(filePath);
 
-	UDEBUG("_obsoletePanels=%d parameters=%d", (int)_obsoletePanels, (int)_parameters.size());
+	UDEBUG("_obsoletePanels=%d modified parameters=%d", (int)_obsoletePanels, (int)_modifiedParameters.size());
 
-	if(_parameters.size())
+	if(_modifiedParameters.size())
 	{
-		emit settingsChanged(_parameters);
+		emit settingsChanged(_modifiedParameters);
 	}
 
 	if(_obsoletePanels)
@@ -1702,7 +1703,9 @@ void PreferencesDialog::writeSettings(const QString & filePath)
 		emit settingsChanged(_obsoletePanels);
 	}
 
-	_parameters.clear();
+	uInsert(_parameters, _modifiedParameters); // update cached parameters
+
+	_modifiedParameters.clear();
 	_obsoletePanels = kPanelDummy;
 }
 
@@ -2349,6 +2352,11 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri
 	return value;
 }
 
+const rtabmap::ParametersMap & PreferencesDialog::getAllParameters() const
+{
+	UASSERT(_parameters.size() == Parameters::getDefaultParameters().size());
+	return _parameters;
+}
 
 void PreferencesDialog::selectSourceDriver(Src src)
 {
@@ -2626,6 +2634,7 @@ void PreferencesDialog::selectSourceOni2Path()
 void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
 {
 	UDEBUG("%s=%s", key.c_str(), value.c_str());
+	uInsert(_parameters, ParametersPair(key, value));
 	QWidget * obj = _ui->stackedWidget->findChild<QWidget*>(key.c_str());
 	if(obj)
 	{
@@ -2795,13 +2804,6 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 {
 	if(object)
 	{
-		// Make sure the value is inserted, check if the same key already exists
-		rtabmap::ParametersMap::iterator iter = _parameters.find(object->objectName().toStdString());
-		if(iter != _parameters.end())
-		{
-			_parameters.erase(iter);
-		}
-
 		const QComboBox * comboBox = qobject_cast<const QComboBox*>(object);
 		const QSpinBox * spinbox = qobject_cast<const QSpinBox*>(object);
 		if(comboBox || spinbox)
@@ -2879,7 +2881,7 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 				}
 			}
 			// Add parameter
-			_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
+			uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
 		}
 		else
 		{
@@ -2897,20 +2899,13 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 {
 	if(object)
 	{
-		// Make sure the value is inserted, check if the same key already exists
-		rtabmap::ParametersMap::iterator iter = _parameters.find(object->objectName().toStdString());
-		if(iter != _parameters.end())
-		{
-			_parameters.erase(iter);
-		}
-
 		const QCheckBox * checkbox = qobject_cast<const QCheckBox*>(object);
 		const QRadioButton * radio = qobject_cast<const QRadioButton*>(object);
 		const QGroupBox * groupBox = qobject_cast<const QGroupBox*>(object);
 		if(checkbox || radio || groupBox)
 		{
 			// Add parameter
-			_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), uBool2Str(value)));
+			uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), uBool2Str(value)));
 
 			// RGBD panel
 			if(value && checkbox == _ui->general_checkBox_activateRGBD)
@@ -2955,14 +2950,7 @@ void PreferencesDialog::addParameter(const QObject * object, double value)
 {
 	if(object)
 	{
-		// Make sure the value is inserted, check if the same key already exists
-		rtabmap::ParametersMap::iterator iter = _parameters.find(object->objectName().toStdString());
-		if(iter != _parameters.end())
-		{
-			_parameters.erase(iter);
-		}
-		_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
-		//ULOGGER_DEBUG("PreferencesDialog::addParameter(object, double) Added [\"%s\",\"%s\"]", object->objectName().toStdString().c_str(), QString::number(value).toStdString().c_str());
+		uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
 	}
 	else
 	{
@@ -2975,12 +2963,7 @@ void PreferencesDialog::addParameter(const QObject * object, const QString & val
 	if(object)
 	{
 		// Make sure the value is inserted, check if the same key already exists
-		rtabmap::ParametersMap::iterator iter = _parameters.find(object->objectName().toStdString());
-		if(iter != _parameters.end())
-		{
-			_parameters.erase(iter);
-		}
-		_parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), value.toStdString()));
+		uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), value.toStdString()));
 		//ULOGGER_DEBUG("PreferencesDialog::addParameter(object, QString) Added [\"%s\",\"%s\"]", object->objectName().toStdString().c_str(), QString::number(value).toStdString().c_str());
 	}
 	else
@@ -3162,23 +3145,6 @@ void PreferencesDialog::makeObsoleteSourcePanel()
 	_obsoletePanels = _obsoletePanels | kPanelSource;
 }
 
-rtabmap::ParametersMap PreferencesDialog::getAllParameters()
-{
-	rtabmap::ParametersMap result;
-	rtabmap::ParametersMap tmpParameters = _parameters;
-	_parameters.clear();
-
-	QList<QGroupBox*> boxes = this->getGroupBoxes();
-	for(int i=0; i<boxes.size(); ++i)
-	{
-		this->addParameters(boxes.at(i));
-	}
-
-	result = _parameters;
-	_parameters = tmpParameters;
-	return result;
-}
-
 QList<QGroupBox*> PreferencesDialog::getGroupBoxes()
 {
 	QList<QGroupBox*> boxes;