Fixed PnP in OpenCV3: added parameter Vis/PnPRefineIterations (default 1)

CameraRGBDImages: Fixed calibration not loaded
Fixed loading ground truth from RGBD-SLAM format
MainWindow: added ground truth paths in CloudViewer and GraphViewer

guilib/include/rtabmap/gui/GraphViewer.h

@@ -57,6 +57,7 @@ public:
 	void updateGraph(const std::map<int, Transform> & poses,
 					 const std::multimap<int, Link> & constraints,
 					 const std::map<int, int> & mapIds);
+	void updateGTGraph(const std::map<int, Transform> & poses);
 	void updateReferentialPosition(const Transform & t);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void updatePosterior(const std::map<int, float> & posterior);
@@ -87,6 +88,7 @@ public:
 	const QColor & getRejectedLoopClosureColor() const {return _loopClosureRejectedColor;}
 	const QColor & getLocalPathColor() const {return _localPathColor;}
 	const QColor & getGlobalPathColor() const {return _globalPathColor;}
+	const QColor & getGTColor() const {return _gtPathColor;}
 	const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;}
 	const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;}
 	bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;}
@@ -112,6 +114,7 @@ public:
 	void setRejectedLoopClosureColor(const QColor & color);
 	void setLocalPathColor(const QColor & color);
 	void setGlobalPathColor(const QColor & color);
+	void setGTColor(const QColor & color);
 	void setIntraSessionLoopColor(const QColor & color);
 	void setInterSessionLoopColor(const QColor & color);
 	void setIntraInterSessionColorsEnabled(bool enabled);
@@ -145,12 +148,15 @@ private:
 	QColor _loopClosureRejectedColor;
 	QColor _localPathColor;
 	QColor _globalPathColor;
+	QColor _gtPathColor;
 	QColor _loopIntraSessionColor;
 	QColor _loopInterSessionColor;
 	bool _intraInterSessionColors;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
+	QMap<int, NodeItem*> _gtNodeItems;
+	QMultiMap<int, LinkItem*> _gtLinkItems;
 	QMultiMap<int, LinkItem*> _localPathLinkItems;
 	QMultiMap<int, LinkItem*> _globalPathLinkItems;
 	float _nodeRadius;

guilib/include/rtabmap/gui/MainWindow.h

@@ -292,6 +292,7 @@ private:
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
+	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>

guilib/src/GraphViewer.cpp

@@ -65,6 +65,7 @@ public:
 		this->setBrush(pen().color());
 		this->setAcceptHoverEvents(true);
 	}
+	virtual ~NodeItem() {}
 
 	void setColor(const QColor & color)
 	{
@@ -114,6 +115,7 @@ public:
 	{
 		this->setAcceptHoverEvents(true);
 	}
+	virtual ~LinkItem() {}
 
 	void setColor(const QColor & color)
 	{
@@ -183,6 +185,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_loopClosureRejectedColor(Qt::black),
 		_localPathColor(Qt::cyan),
 		_globalPathColor(Qt::darkMagenta),
+		_gtPathColor(Qt::gray),
 		_loopIntraSessionColor(Qt::red),
 		_loopInterSessionColor(Qt::green),
 		_intraInterSessionColors(false),
@@ -459,6 +462,124 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	UDEBUG("_nodeItems=%d, _linkItems=%d", _nodeItems.size(), _linkItems.size());
 }
 
+void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
+{
+	bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
+	UDEBUG("poses=%d", (int)poses.size());
+	//Hide nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
+	{
+		iter.value()->hide();
+		iter.value()->setColor(_gtPathColor); // reset color
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(!iter->second.isNull())
+		{
+			QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
+			if(itemIter != _gtNodeItems.end())
+			{
+				itemIter.value()->setPose(iter->second);
+				itemIter.value()->show();
+			}
+			else
+			{
+				// create node item
+				const Transform & pose = iter->second;
+				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius);
+				this->scene()->addItem(item);
+				item->setZValue(20);
+				item->setColor(_gtPathColor);
+				item->setParentItem(_root);
+				_gtNodeItems.insert(iter->first, item);
+			}
+
+			if(iter!=poses.begin())
+			{
+				std::map<int, Transform>::const_iterator iterPrevious = iter;
+				--iterPrevious;
+				Transform previousPose = iterPrevious->second;
+				Transform currentPose = iter->second;
+
+				LinkItem * linkItem = 0;
+				QMultiMap<int, LinkItem*>::iterator linkIter = _gtLinkItems.end();
+				if(_gtLinkItems.contains(iterPrevious->first))
+				{
+					linkIter = _gtLinkItems.find(iter->first);
+					while(linkIter.key() == iterPrevious->first && linkIter != _gtLinkItems.end())
+					{
+						if(linkIter.value()->to() == iter->first)
+						{
+							linkIter.value()->setPoses(previousPose, currentPose);
+							linkIter.value()->show();
+							linkItem = linkIter.value();
+							break;
+						}
+						++linkIter;
+					}
+				}
+				if(linkItem == 0)
+				{
+					//create a link item
+					linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link::kUndef, 1);
+					QPen p = linkItem->pen();
+					p.setWidthF(_linkWidth);
+					linkItem->setPen(p);
+					linkItem->setZValue(10);
+					this->scene()->addItem(linkItem);
+					linkItem->setParentItem(_root);
+					_gtLinkItems.insert(iterPrevious->first, linkItem);
+				}
+				if(linkItem)
+				{
+					linkItem->setColor(_gtPathColor);
+				}
+			}
+		}
+	}
+
+	//remove not used nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _gtNodeItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _gtLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
+	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+
+	if(wasEmpty)
+	{
+		QRectF rect = this->scene()->itemsBoundingRect();
+		this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
+	}
+
+	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d", _gtNodeItems.size(), _gtLinkItems.size());
+}
+
 void GraphViewer::updateReferentialPosition(const Transform & t)
 {
 	QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
@@ -695,6 +816,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
 	settings.setValue("local_path_color", this->getLocalPathColor());
 	settings.setValue("global_path_color", this->getGlobalPathColor());
+	settings.setValue("gt_color", this->getGTColor());
 	settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
 	settings.setValue("inter_session_color", this->getInterSessionLoopColor());
 	settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
@@ -729,6 +851,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
 	this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
 	this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
+	this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
 	this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
 	this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
 	this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
@@ -772,6 +895,10 @@ void GraphViewer::setNodeRadius(float radius)
 	{
 		iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
 	}
+	for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
+	{
+		iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
+	}
 }
 void GraphViewer::setLinkWidth(float width)
 {
@@ -887,6 +1014,18 @@ void GraphViewer::setGlobalPathColor(const QColor & color)
 {
 	_globalPathColor = color;
 }
+void GraphViewer::setGTColor(const QColor & color)
+{
+	_gtPathColor = color;
+	for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
+	{
+		iter.value()->setColor(_gtPathColor);
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter=_gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
+	{
+		iter.value()->setColor(_gtPathColor);
+	}
+}
 void GraphViewer::setIntraSessionLoopColor(const QColor & color)
 {
 	_loopIntraSessionColor = color;
@@ -1022,6 +1161,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
 	QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
 	QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
+	QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
 	menuLink->addSeparator();
 	QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
 	QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
@@ -1035,6 +1175,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
 	aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
 	aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
+	aChangeGTColor->setIcon(createIcon(_gtPathColor));
 	aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
 	aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
 	aChangeNeighborColor->setIconVisibleInMenu(true);
@@ -1046,6 +1187,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aChangeRejectedLoopColor->setIconVisibleInMenu(true);
 	aChangeLocalPathColor->setIconVisibleInMenu(true);
 	aChangeGlobalPathColor->setIconVisibleInMenu(true);
+	aChangeGTColor->setIconVisibleInMenu(true);
 	aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
 	aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
 	aSetIntraInterSessionColors->setCheckable(true);
@@ -1205,6 +1347,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			r == aChangeRejectedLoopColor ||
 			r == aChangeLocalPathColor ||
 			r == aChangeGlobalPathColor ||
+			r == aChangeGTColor ||
 			r == aChangeIntraSessionLoopColor ||
 			r == aChangeInterSessionLoopColor)
 	{
@@ -1249,6 +1392,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		{
 			color = _globalPathColor;
 		}
+		else if(r == aChangeGTColor)
+		{
+			color = _gtPathColor;
+		}
 		else if(r == aChangeIntraSessionLoopColor)
 		{
 			color = _loopIntraSessionColor;
@@ -1305,6 +1452,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			{
 				this->setGlobalPathColor(color);
 			}
+			else if(r == aChangeGTColor)
+			{
+				this->setGTColor(color);
+			}
 			else if(r == aChangeIntraSessionLoopColor)
 			{
 				this->setIntraSessionLoopColor(color);

guilib/src/MainWindow.cpp

@@ -1099,6 +1099,10 @@ 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()));
+		}
 	}
 	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;
@@ -1633,6 +1637,20 @@ void MainWindow::updateMapCloud(
 			kter->second->push_back(pt);
 		}
 
+		//Ground truth graph?
+		for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
+		{
+			int mapId = -100;
+			//edges
+			std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
+			if(kter == graphs.end())
+			{
+				kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
+			}
+			pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
+			kter->second->push_back(pt);
+		}
+
 		// add graphs
 		for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
 		{
@@ -1677,6 +1695,8 @@ void MainWindow::updateMapCloud(
 		{
 			_ui->graphicsView_graphView->updateReferentialPosition(currentPose);
 		}
+
+		_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
 	}
 	cv::Mat map8U;
 	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
@@ -4078,6 +4098,7 @@ void MainWindow::clearTheCache()
 	_ui->widget_cloudViewer->clearTrajectory();
 	_ui->widget_mapVisibility->clear();
 	_currentPosesMap.clear();
+	_currentGTPosesMap.clear();
 	_currentLinksMap.clear();
 	_currentMapIds.clear();
 	_curentLabels.clear();

guilib/src/PreferencesDialog.cpp

@@ -208,11 +208,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
-#if CV_MAJOR_VERSION < 3
-	_ui->loopClosure_pnpOpenCV2->setVisible(false);
-	_ui->label_loopClosure_pnpOpenCV2->setVisible(false);
-#endif
-
 	// Default Driver
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
@@ -649,7 +644,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
-	_ui->loopClosure_pnpOpenCV2->setObjectName(Parameters::kVisPnPOpenCV2().c_str());
+	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
 
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
@@ -683,6 +678,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
+	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int)));
+	_ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
@@ -2522,7 +2519,7 @@ void PreferencesDialog::selectSourceRGBDImagesPathGt()
 		{
 			list.push_back(_ui->comboBox_cameraRGBDImages_gtFormat->itemText(i));
 		}
-		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list);
+		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
 		if(!item.isEmpty())
 		{
 			_ui->lineEdit_cameraRGBDImages_gt->setText(path);
@@ -3945,6 +3942,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 				dir = d.absolutePath();
 			}
 		}
+
 		if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
 		{
 			UWARN("init camera failed... ");