MainWindow: added "Edit->Anchor clouds to ground truth" action. Graph::importPoses() Fixed error when loading RGBD-SLAM poses.

corelib/src/CameraRGB.cpp

@@ -285,6 +285,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			}
 			else if(_groundTruthFormat == 1)
 			{
+				UDEBUG("");
 				//Match ground truth values with images
 				groundTruth_.clear();
 				std::map<double, int> stampsToIds;
@@ -339,6 +340,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			}
 			else
 			{
+				UDEBUG("");
 				groundTruth_ = uValuesList(poses);
 			}
 			UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());

corelib/src/Graph.cpp

@@ -192,7 +192,7 @@ bool importPoses(
 			std::string str;
 			std::getline(file, str);
 
-			if(str.at(0) == '#' || str.empty())
+			if(str.empty() || str.at(0) == '#')
 			{
 				continue;
 			}

guilib/include/rtabmap/gui/MainWindow.h

@@ -159,6 +159,7 @@ private slots:
 	void updateCacheFromDatabase();
 	void downloadAllClouds();
 	void downloadPoseGraph();
+	void anchorCloudsToGroundTruth();
 	void clearTheCache();
 	void openPreferences();
 	void openPreferencesSource();

guilib/src/GraphViewer.cpp

@@ -770,6 +770,10 @@ void GraphViewer::clearGraph()
 	_localPathLinkItems.clear();
 	qDeleteAll(_globalPathLinkItems);
 	_globalPathLinkItems.clear();
+	qDeleteAll(_gtNodeItems);
+	_gtNodeItems.clear();
+	qDeleteAll(_gtLinkItems);
+	_gtLinkItems.clear();
 
 	_referential->resetTransform();
 	_localRadius->resetTransform();

guilib/src/MainWindow.cpp

@@ -316,6 +316,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds()));
 	connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph()));
 	connect(_ui->actionUpdate_cache_from_database, SIGNAL(triggered()), this, SLOT(updateCacheFromDatabase()));
+	connect(_ui->actionAnchor_clouds_to_ground_truth, SIGNAL(triggered()), this, SLOT(anchorCloudsToGroundTruth()));
 	connect(_ui->menuEdit, SIGNAL(aboutToShow()), this, SLOT(updateEditMenu()));
 	connect(_ui->actionDefault_views, SIGNAL(triggered(bool)), this, SLOT(setDefaultViews()));
 	connect(_ui->actionAuto_screen_capture, SIGNAL(triggered(bool)), this, SLOT(selectScreenCaptureFormat(bool)));
@@ -345,6 +346,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
 	_ui->actionReset_Odometry->setEnabled(false);
 	_ui->actionPost_processing->setEnabled(false);
+	_ui->actionAnchor_clouds_to_ground_truth->setEnabled(false);
 
 	QToolButton* toolButton = new QToolButton(this);
 	toolButton->setMenu(_ui->menuSelect_source);
@@ -1103,6 +1105,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		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));
@@ -1489,6 +1492,26 @@ void MainWindow::updateMapCloud(
 	}
 	_ui->widget_mapVisibility->setMap(posesIn, posesMask);
 
+	if(_ui->actionAnchor_clouds_to_ground_truth->isChecked() && _currentGTPosesMap.size())
+	{
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
+			if(gtIter!=_currentGTPosesMap.end())
+			{
+				iter->second = gtIter->second;
+			}
+			else
+			{
+				UWARN("Not found ground truth pose for node %d", iter->first);
+			}
+		}
+	}
+	else if(_currentGTPosesMap.size() == 0)
+	{
+		_ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
+	}
+
 	// Map updated! regenerate the assembled cloud, last pose is the new one
 	UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
 	QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
@@ -4062,6 +4085,16 @@ void MainWindow::downloadPoseGraph()
 	}
 }
 
+void MainWindow::anchorCloudsToGroundTruth()
+{
+	this->updateMapCloud(
+			std::map<int, Transform>(_currentPosesMap),
+			Transform(),
+			std::multimap<int, Link>(_currentLinksMap),
+			std::map<int, int>(_currentMapIds),
+			std::map<int, std::string>(_curentLabels));
+}
+
 void MainWindow::clearTheCache()
 {
 	_cachedSignatures.clear();
@@ -5978,6 +6011,8 @@ void MainWindow::changeState(MainWindow::State newState)
 		UWARN("Menu File separators have not the same order.");
 	}
 
+	_ui->actionAnchor_clouds_to_ground_truth->setEnabled(!_currentGTPosesMap.empty());
+
 	switch (newState)
 	{
 	case kIdle: // RTAB-Map is not initialized yet

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -77,6 +77,7 @@
     <addaction name="actionDownload_all_clouds"/>
     <addaction name="actionDownload_graph"/>
     <addaction name="actionUpdate_cache_from_database"/>
+    <addaction name="actionAnchor_clouds_to_ground_truth"/>
     <addaction name="actionClear_cache"/>
     <addaction name="separator"/>
     <addaction name="actionView_high_res_point_cloud"/>
@@ -1289,6 +1290,14 @@
     <string>Export images (RGB=*.jpg Depth=*.png)...</string>
    </property>
   </action>
+  <action name="actionAnchor_clouds_to_ground_truth">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Anchor clouds to ground truth</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>