Rtabmap: Added getPathNextPoses() and getPathNextNodes(). Local path is sent over statistics.

GUI: local path is shown in GraphViewer. Now handling stereo odometry data.

corelib/include/rtabmap/core/Rtabmap.h

@@ -120,6 +120,8 @@ public:
 	void clearPath();
 	std::list<std::pair<int, Transform> > computePath(int targetNode);
 	const std::vector<int> & getPath() const {return _path;}
+	std::list<std::pair<int, Transform> > getPathNextPoses() const;
+	std::vector<int> getPathNextNodes() const;
 	int getPathGoalId() const;
 
 	std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;

corelib/include/rtabmap/core/Statistics.h

@@ -140,6 +140,7 @@ public:
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
 	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
+	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -158,6 +159,7 @@ public:
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
 	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
+	const std::vector<int> & localPath() const {return _localPath;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -184,6 +186,8 @@ private:
 	std::map<int, float> _likelihood;
 	std::map<int, float> _rawLikelihood;
 
+	std::vector<int> _localPath;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/src/Rtabmap.cpp

@@ -1633,6 +1633,12 @@ bool Rtabmap::process(const SensorData & data)
 					statistics_.setRawLikelihood(rawLikelihood);
 				}
 			}
+
+			// Path
+			if(_path.size())
+			{
+				statistics_.setLocalPath(this->getPathNextNodes());
+			}
 		}
 
 		timeStatsCreation = timer.ticks();
@@ -2415,6 +2421,53 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
 	return pathPoses;
 }
 
+std::list<std::pair<int, Transform> > Rtabmap::getPathNextPoses() const
+{
+	std::list<std::pair<int, Transform> > poses;
+	if(_path.size())
+	{
+		UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
+		for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
+		{
+			std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i]);
+			if(iter != _optimizedPoses.end())
+			{
+				poses.push_back(*iter);
+			}
+			else
+			{
+				break;
+			}
+		}
+	}
+	return poses;
+}
+
+std::vector<int> Rtabmap::getPathNextNodes() const
+{
+	std::vector<int> ids;
+	if(_path.size())
+	{
+		UASSERT(_pathCurrentIndex < _path.size() && _pathGoalIndex < _path.size());
+		ids.resize(_pathGoalIndex-_pathCurrentIndex+1);
+		int oi = 0;
+		for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i)
+		{
+			std::map<int, Transform>::const_iterator iter = _optimizedPoses.find(_path[i]);
+			if(iter != _optimizedPoses.end())
+			{
+				ids[oi++] = iter->first;
+			}
+			else
+			{
+				break;
+			}
+		}
+		ids.resize(oi);
+	}
+	return ids;
+}
+
 int Rtabmap::getPathGoalId() const
 {
 	if(_path.size())

guilib/src/GraphViewer.cpp

@@ -157,6 +157,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_loopClosureLocalColor(Qt::yellow),
 		_loopClosureUserColor(Qt::red),
 		_loopClosureVirtualColor(Qt::magenta),
+		_pathColor(Qt::cyan),
 		_root(0),
 		_nodeRadius(0.01),
 		_linkWidth(0),
@@ -206,6 +207,7 @@ GraphViewer::~GraphViewer()
 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				 const std::multimap<int, Link> & constraints)
 {
+	bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
 	UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
 	//Hide nodes and links
 	for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
@@ -244,25 +246,50 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 
 	for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 	{
-		std::map<int, Transform>::const_iterator jterA = poses.find(iter->first);
+		// make the first id the smallest one
-		std::map<int, Transform>::const_iterator jterB = poses.find(iter->second.to());
+		int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
+		int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
+
+		std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
+		std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
 		if(jterA != poses.end() && jterB != poses.end() &&
-		   _nodeItems.contains(iter->first) && _nodeItems.contains(iter->second.to()))
+		   _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
 		{
 			const Transform & poseA = jterA->second;
 			const Transform & poseB = jterB->second;
 
 			bool added = false;
-			if(_linkItems.contains(iter->first))
+			if(_linkItems.contains(idFrom))
 			{
 				QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(iter->first);
-				while(itemIter.key() == iter->first && itemIter != _linkItems.end())
+				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
 				{
-					if(itemIter.value()->to() == iter->second.to())
+					if(itemIter.value()->to() == idTo)
 					{
 						itemIter.value()->setPoses(poseA, poseB);
 						itemIter.value()->show();
 						added = true;
+						// reset color
+						if(iter->second.type() == Link::kNeighbor)
+						{
+							itemIter.value()->setColor(_neighborColor);
+						}
+						else if(iter->second.type() == Link::kVirtualClosure)
+						{
+							itemIter.value()->setColor(_loopClosureVirtualColor);
+						}
+						else if(iter->second.type() == Link::kUserClosure)
+						{
+							itemIter.value()->setColor(_loopClosureUserColor);
+						}
+						else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
+						{
+							itemIter.value()->setColor(_loopClosureLocalColor);
+						}
+						else
+						{
+							itemIter.value()->setColor(_loopClosureColor);
+						}
 						break;
 					}
 					++itemIter;
@@ -271,7 +298,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			if(!added)
 			{
 				//create a link item
-				LinkItem * item = new LinkItem(iter->first, iter->second.to(), poseA, poseB, iter->second.type());
+				LinkItem * item = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type());
 				QPen p = item->pen();
 				p.setWidthF(_linkWidth);
 				item->setPen(p);
@@ -298,7 +325,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				this->scene()->addItem(item);
 				item->setZValue(1);
 				item->setParentItem(_root);
-				_linkItems.insert(iter->first, item);
+				_linkItems.insert(idFrom, item);
 			}
 		}
 	}
@@ -341,7 +368,15 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	}
 
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-	this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+
+	if(poses.size() == 0 || wasEmpty)
+	{
+		this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+	}
+	else
+	{
+		this->centerOn(_lastReferential);
+	}
 }
 
 void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
@@ -392,6 +427,31 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
 	}
 }
 
+void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
+{
+	if(localPath.size() > 1)
+	{
+		for(unsigned int i=0; i<localPath.size()-1; ++i)
+		{
+			if(_linkItems.contains(localPath[i]))
+			{
+				int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
+				int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
+				QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(idFrom);
+				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
+				{
+					if(itemIter.value()->to() == idTo)
+					{
+						itemIter.value()->setColor(_pathColor);
+						break;
+					}
+					++itemIter;
+				}
+			}
+		}
+	}
+}
+
 void GraphViewer::clearGraph()
 {
 	qDeleteAll(_nodeItems);
@@ -521,6 +581,10 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
 		}
 	}
 }
+void GraphViewer::setLocalPathColor(const QColor & color)
+{
+	_pathColor = color;
+}
 void GraphViewer::setGridMapVisible(bool visible)
 {
 	_gridMap->setVisible(visible);
@@ -576,16 +640,19 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
 	QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
 	QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
+	QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
 	aChangeNeighborColor->setIcon(createIcon(_neighborColor));
 	aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
 	aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
 	aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
 	aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
+	aChangeLocalPathColor->setIcon(createIcon(_pathColor));
 	aChangeNeighborColor->setIconVisibleInMenu(true);
 	aChangeGlobalLoopColor->setIconVisibleInMenu(true);
 	aChangeLocalLoopColor->setIconVisibleInMenu(true);
 	aChangeUserLoopColor->setIconVisibleInMenu(true);
 	aChangeVirtualLoopColor->setIconVisibleInMenu(true);
+	aChangeLocalPathColor->setIconVisibleInMenu(true);
 
 	menu.addSeparator();
 	QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
@@ -672,7 +739,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			r == aChangeGlobalLoopColor ||
 			r == aChangeLocalLoopColor ||
 			r == aChangeUserLoopColor ||
-			r == aChangeVirtualLoopColor)
+			r == aChangeVirtualLoopColor ||
+			r == aChangeLocalPathColor)
 	{
 		QColor color;
 		if(r == aChangeNodeColor)
@@ -695,6 +763,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		{
 			color = _loopClosureVirtualColor;
 		}
+		else if(r == aChangeLocalPathColor)
+		{
+			color = _pathColor;
+		}
 		else //if(r == aChangeNeighborColor)
 		{
 			color = _neighborColor;
@@ -723,6 +795,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			{
 				this->setVirtualLoopClosureColor(color);
 			}
+			else if(r == aChangeLocalPathColor)
+			{
+				this->setLocalPathColor(color);
+			}
 			else //if(r == aChangeNeighborColor)
 			{
 				this->setNeighborColor(color);

guilib/src/GraphViewer.h

@@ -55,6 +55,7 @@ public:
 					 const std::multimap<int, Link> & constraints);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void updatePosterior(const std::map<int, float> & posterior);
+	void updateLocalPath(const std::vector<int> & localPath);
 	void clearGraph();
 	void clearMap();
 	void clearPosterior();
@@ -70,6 +71,7 @@ public:
 	const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
 	const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
 	const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
+	const QColor & getLocalPathColor() const {return _pathColor;}
 	bool isGridMapVisible() const;
 
 	// setters
@@ -82,6 +84,7 @@ public:
 	void setLocalLoopClosureColor(const QColor & color);
 	void setUserLoopClosureColor(const QColor & color);
 	void setVirtualLoopClosureColor(const QColor & color);
+	void setLocalPathColor(const QColor & color);
 	void setGridMapVisible(bool visible);
 
 signals:
@@ -102,6 +105,7 @@ private:
 	QColor _loopClosureLocalColor;
 	QColor _loopClosureUserColor;
 	QColor _loopClosureVirtualColor;
+	QColor _pathColor;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;

guilib/src/MainWindow.cpp

@@ -713,19 +713,19 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			_odometryReceived = true;
 
 			// 3d cloud
-			if(data.depth().cols == data.image().cols &&
+			if(data.depthOrRightImage().cols == data.image().cols &&
-			   data.depth().rows == data.image().rows &&
+			   data.depthOrRightImage().rows == data.image().rows &&
-			   !data.depth().empty() &&
+			   !data.depthOrRightImage().empty() &&
 			   data.fx() > 0.0f &&
-			   data.fy() > 0.0f &&
+			   data.fyOrBaseline() > 0.0f &&
 			   _preferencesDialog->isCloudsShown(1))
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 				cloud = createCloud(0,
 						data.image(),
-						data.depth(),
+						data.depthOrRightImage(),
 						data.fx(),
-						data.fy(),
+						data.fyOrBaseline(),
 						data.cx(),
 						data.cy(),
 						data.localTransform(),
@@ -1133,6 +1133,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			_ui->graphicsView_graphView->updatePosterior(stat.posterior());
 		}
+		// update local path on the graph view
+		_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
 
 		UDEBUG("");
 	}
@@ -1246,7 +1248,8 @@ void MainWindow::updateMapCloud(
 			std::string cloudName = uFormat("cloud%d", iter->first);
 
 			// 3d point cloud
-			if(_preferencesDialog->isCloudsShown(0))
+			if((_ui->widget_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
+				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud()))
 			{
 				if(viewerClouds.contains(cloudName))
 				{
@@ -1281,7 +1284,8 @@ void MainWindow::updateMapCloud(
 
 			// 2d point cloud
 			std::string scanName = uFormat("scan%d", iter->first);
-			if(_preferencesDialog->isScansShown(0) || _ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())
+			if((_ui->widget_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
+				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
 			{
 				if(viewerClouds.contains(scanName))
 				{

guilib/src/PreferencesDialog.cpp

@@ -856,7 +856,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
 		_ui->checkBox_map_fillEmptySpace->setChecked(true);
 		_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
-		_ui->spinbox_map_fillEmptyRadius->setValue(0);
+		_ui->spinbox_map_fillEmptyRadius->setValue(1);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1795,6 +1795,7 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
 			settings.setValue("local_color", graphViewer->getLocalLoopClosureColor());
 			settings.setValue("user_color", graphViewer->getUserLoopClosureColor());
 			settings.setValue("virtual_color", graphViewer->getVirtualLoopClosureColor());
+			settings.setValue("local_path_color", graphViewer->getLocalPathColor());
 			settings.setValue("grid_visible", graphViewer->isGridMapVisible());
 		}
 		else
@@ -1892,6 +1893,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
 			graphViewer->setLocalLoopClosureColor(settings.value("local_color", graphViewer->getLocalLoopClosureColor()).value<QColor>());
 			graphViewer->setUserLoopClosureColor(settings.value("user_color", graphViewer->getUserLoopClosureColor()).value<QColor>());
 			graphViewer->setVirtualLoopClosureColor(settings.value("virtual_color", graphViewer->getVirtualLoopClosureColor()).value<QColor>());
+			graphViewer->setLocalPathColor(settings.value("local_path_color", graphViewer->getLocalPathColor()).value<QColor>());
 			graphViewer->setGridMapVisible(settings.value("grid_visible", graphViewer->isGridMapVisible()).toBool());
 		}
 		else

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-693</y>
+            <y>0</y>
             <width>744</width>
-            <height>1378</height>
+            <height>1074</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,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">
@@ -543,7 +543,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                             <number>10</number>
                            </property>
                            <property name="value">
-                            <number>0</number>
+                            <number>1</number>
                            </property>
                           </widget>
                          </item>