New feature: Show lines between corresponding visual words (right-click on image view)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@472 f169173b-cf89-36c8-b27e-44dbe73f0c83

guilib/src/ImageView.cpp

@@ -47,6 +47,9 @@ ImageView::ImageView(QWidget * parent) :
 	_showFeatures = _menu->addAction(tr("Show features"));
 	_showFeatures->setCheckable(true);
 	_showFeatures->setChecked(true);
+	_showLines = _menu->addAction(tr("Show lines"));
+	_showLines->setCheckable(true);
+	_showLines->setChecked(false);
 	_saveImage = _menu->addAction(tr("Save picture..."));
 }
 
@@ -70,6 +73,11 @@ bool ImageView::isFeaturesShown()
 	return _showFeatures->isChecked();
 }
 
+bool ImageView::isLinesShown()
+{
+	return _showLines->isChecked();
+}
+
 void ImageView::contextMenuEvent(QContextMenuEvent * e)
 {
 	QAction * action = _menu->exec(e->globalPos());
@@ -90,7 +98,7 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 			img.save(text);
 		}
 	}
-	else if(action == _showFeatures || action == _showImage)
+	else if(action == _showFeatures || action == _showImage || action == _showLines)
 	{
 		this->updateItemsShown();
 	}
@@ -105,6 +113,10 @@ void ImageView::updateItemsShown()
 		{
 			items.at(i)->setVisible(_showFeatures->isChecked());
 		}
+		else if( qgraphicsitem_cast<QGraphicsLineItem*>(items.at(i)))
+		{
+			items.at(i)->setVisible(_showLines->isChecked());
+		}
 		else if(qgraphicsitem_cast<QGraphicsPixmapItem*>(items.at(i)))
 		{
 			items.at(i)->setVisible(_showImage->isChecked());

guilib/src/ImageView.h

@@ -40,6 +40,7 @@ public:
 
 	bool isImageShown();
 	bool isFeaturesShown();
+	bool isLinesShown();
 
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * e);
@@ -59,6 +60,7 @@ private:
 	QMenu * _menu;
 	QAction * _showImage;
 	QAction * _showFeatures;
+	QAction * _showLines;
 	QAction * _saveImage;
 };
 

guilib/src/MainWindow.cpp

@@ -824,6 +824,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 	KeypointItem * item = 0;
 	int alpha = _preferencesDialog->getKeypointsOpacity()*255/100;
 	ULOGGER_DEBUG("refWords.size() = %d", refWords.size());
+	QMap<int, KeypointItem*> addedKeypoints;
 	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
 	{
 		const cv::KeyPoint & r = (*i).second;
@@ -865,12 +866,14 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 		item->setVisible(this->_ui->imageView_source->isFeaturesShown());
 		this->_ui->imageView_source->scene()->addItem(item);
 		item->setZValue(1);
+		addedKeypoints.insert(id, item);
 	}
 	ULOGGER_DEBUG("source time = %f s", timer.ticks());
 
 	timer.start();
 	item = 0;
 	ULOGGER_DEBUG("loopWords.size() = %d", loopWords.size());
+	QList<QPair<KeypointItem*, KeypointItem*> > uniqueCorrespondences;
 	for(std::multimap<int, cv::KeyPoint>::const_iterator i = loopWords.begin(); i != loopWords.end(); ++i )
 	{
 		const cv::KeyPoint & r = (*i).second;
@@ -888,6 +891,11 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 		{
 			// PINK = FOUND IN LOOP SIGNATURE
 			item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 255, alpha));
+			//To draw lines... get only unique correspondences
+			if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
+			{
+				uniqueCorrespondences.push_back(QPair<KeypointItem*, KeypointItem*>(addedKeypoints.value(id), item));
+			}
 		}
 		else if(id<=_lastId)
 		{
@@ -918,6 +926,38 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 		}
 		_lastIds = QSet<int>::fromList(QList<int>::fromStdList(uKeys(refWords)));
 	}
+
+	// Draw lines between corresponding features...
+	// TODO: support mirror view, switching between vertical to horizontal layout
+	int deltaX = this->_ui->imageView_source->sceneRect().width();
+	int deltaY = 0;
+	if(_preferencesDialog->isVerticalLayoutUsed())
+	{
+		deltaX = 0;
+		deltaY = this->_ui->imageView_source->sceneRect().height();
+	}
+	for(QList<QPair<KeypointItem*, KeypointItem*> >::iterator iter = uniqueCorrespondences.begin();
+		iter!=uniqueCorrespondences.end();
+		++iter)
+	{
+		QGraphicsLineItem * item = this->_ui->imageView_source->scene()->addLine(
+				iter->first->rect().x()+iter->first->rect().width()/2,
+				iter->first->rect().y()+iter->first->rect().height()/2,
+				iter->second->rect().x()+iter->second->rect().width()/2+deltaX,
+				iter->second->rect().y()+iter->second->rect().height()/2+deltaY,
+				QPen(QColor(255, 255, 255, alpha)));
+		item->setVisible(this->_ui->imageView_source->isLinesShown());
+		item->setZValue(1);
+
+		item = this->_ui->imageView_loopClosure->scene()->addLine(
+				iter->first->rect().x()+iter->first->rect().width()/2-deltaX,
+				iter->first->rect().y()+iter->first->rect().height()/2-deltaY,
+				iter->second->rect().x()+iter->second->rect().width()/2,
+				iter->second->rect().y()+iter->second->rect().height()/2,
+				QPen(QColor(255, 255, 255, alpha)));
+		item->setVisible(this->_ui->imageView_loopClosure->isLinesShown());
+		item->setZValue(1);
+	}
 }
 
 void MainWindow::resizeEvent(QResizeEvent* anEvent)