DBViewer: support doubleclick on node or link in graphview to update …

…correpsonding image view and constraints view, also added "Show pixel depth" menu option in ImageView to show pixel depth and pixel coordinate in map frame.

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -128,6 +128,8 @@ private Q_SLOTS:
 	void updateAllLandmarkCovariances();
 	void refineLinks();
 	void resetAllChanges();
+	void graphNodeSelected(int);
+	void graphLinkSelected(int, int);
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
 	void sliderAMoved(int);

guilib/include/rtabmap/gui/GraphViewer.h

@@ -166,13 +166,16 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView {
 Q_SIGNALS:
 	void configChanged();
 	void mapShownRequested();
+	void nodeSelected(int);
+	void linkSelected(int, int);
 
 public Q_SLOTS:
 	void restoreDefaults();
 
 protected:
 	virtual void wheelEvent ( QWheelEvent * event );
 	virtual void mouseMoveEvent(QMouseEvent * event);
+	virtual void mouseDoubleClickEvent(QMouseEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 
 private:

guilib/include/rtabmap/gui/ImageView.h

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/features2d/features2d.hpp>
 #include <map>
 #include "rtabmap/utilite/UCv2Qt.h"
+#include <rtabmap/core/CameraModel.h>
 
 class QAction;
 class QMenu;
@@ -96,7 +97,7 @@ class RTABMAP_GUI_EXPORT ImageView : public QWidget {
 	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
 	void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString());
-	void setImage(const QImage & image);
+	void setImage(const QImage & image, const std::vector<CameraModel> & models = std::vector<CameraModel>(), const Transform & pose = Transform());
 	void setImageDepth(const cv::Mat & imageDepth);
 	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, QColor color);
@@ -121,6 +122,7 @@ class RTABMAP_GUI_EXPORT ImageView : public QWidget {
 	virtual void paintEvent(QPaintEvent *event);
 	virtual void resizeEvent(QResizeEvent* event);
 	virtual void contextMenuEvent(QContextMenuEvent * e);
+	virtual void mouseMoveEvent(QMouseEvent * event);
 
 private Q_SLOTS:
 	void sceneRectChanged(const QRectF &rect);
@@ -164,6 +166,7 @@ private Q_SLOTS:
 	QAction * _colorMapBlueToRed;
 	QAction * _colorMapMinRange;
 	QAction * _colorMapMaxRange;
+	QAction * _mouseTracking;
 	QMenu * _featureMenu;
 	QMenu * _scaleMenu;
 
@@ -175,6 +178,8 @@ private Q_SLOTS:
 	QPixmap _image;
 	QPixmap _imageDepth;
 	cv::Mat _imageDepthCv;
+	std::vector<CameraModel> _models;
+	Transform _pose;
 };
 
 }

guilib/src/DatabaseViewer.cpp

@@ -268,6 +268,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateStatistics()));
 	connect(ui_->dockWidget_info->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateInfo()));
 
+	connect(ui_->graphViewer, SIGNAL(nodeSelected(int)), this , SLOT(graphNodeSelected(int)));
+	connect(ui_->graphViewer, SIGNAL(linkSelected(int,int)), this , SLOT(graphLinkSelected(int,int)));
 
 	connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
 
@@ -4547,6 +4549,20 @@ void DatabaseViewer::resetAllChanges()
 	}
 }
 
+void DatabaseViewer::graphNodeSelected(int id)
+{
+	if(id>0 && idToIndex_.contains(id))
+		ui_->horizontalSlider_A->setValue(idToIndex_.value(id));
+}
+
+void DatabaseViewer::graphLinkSelected(int from, int to)
+{
+	if(from>0 && idToIndex_.contains(from))
+		ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
+	if(to>0 && idToIndex_.contains(to))
+		ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
+}
+
 void DatabaseViewer::sliderAValueChanged(int value)
 {
 	this->update(value,
@@ -4685,7 +4701,12 @@ void DatabaseViewer::update(int value,
 				QRectF rect;
 				if(!img.isNull())
 				{
-					view->setImage(img);
+					Transform pose;
+					if(!graphes_.empty() && graphes_.back().find(data.id())!=graphes_.back().end())
+					{
+						pose = graphes_.back().at(data.id());
+					}
+					view->setImage(img, data.cameraModels(), pose);
 					rect = img.rect();
 				}
 				else

guilib/src/GraphViewer.cpp

@@ -1797,7 +1797,7 @@ void GraphViewer::mouseMoveEvent(QMouseEvent * event)
 	if(_mouseTracking && _viewPlane==XY && this->sceneRect().contains(scenePoint))
 	{
 #if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
-		QToolTip::showText(event->globalPosition().toPoint(), QString("%1,%2").arg(scenePoint.x()/100.0).arg(scenePoint.y()/100.0));
+		QToolTip::showText(event->globalPosition().toPoint(), QString("%1m %2m").arg(-scenePoint.y()/100.0).arg(-scenePoint.x()/100.0));
 #else
 		QToolTip::showText(event->globalPos(), QString("%1m %2m").arg(-scenePoint.y()/100.0).arg(-scenePoint.x()/100.0));
 #endif
@@ -1809,6 +1809,32 @@ void GraphViewer::mouseMoveEvent(QMouseEvent * event)
 	QGraphicsView::mouseMoveEvent(event);
 }
 
+void GraphViewer::mouseDoubleClickEvent(QMouseEvent * event)
+{
+	QGraphicsItem *item = this->scene()->itemAt(mapToScene(event->pos()), QTransform());
+	if(item)
+	{
+		NodeItem *nodeItem = qgraphicsitem_cast<NodeItem*>(item);
+		LinkItem *linkItem = qgraphicsitem_cast<LinkItem*>(item);
+		if(nodeItem && nodeItem->parentItem() == _graphRoot && nodeItem->id() != 0)
+		{
+			Q_EMIT nodeSelected(nodeItem->id());
+		}
+		else if(linkItem && linkItem->parentItem() == _graphRoot && linkItem->from() != 0 && linkItem->to() != 0)
+		{
+			Q_EMIT linkSelected(linkItem->from(), linkItem->to());
+		}
+		else
+		{
+			QGraphicsView::mouseDoubleClickEvent(event);
+		}
+	}
+	else
+	{
+		QGraphicsView::mouseDoubleClickEvent(event);
+	}
+}
+
 QIcon createIcon(const QColor & color)
 {
 	QPixmap pixmap(50, 50);

guilib/src/ImageView.cpp

@@ -40,9 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QColorDialog>
 #include <QPrinter>
 #include <QGraphicsRectItem>
+#include <QToolTip>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
 #include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d_transforms.h"
 
 #include <QtGlobal>
 #if QT_VERSION >= 0x050000
@@ -268,9 +270,14 @@ ImageView::ImageView(QWidget * parent) :
 	group->addAction(_colorMapRedToBlue);
 	group->addAction(_colorMapBlueToRed);
 	group->addAction(_colorMapMaxRange);
+	_mouseTracking = _menu->addAction(tr("Show pixel depth"));
+	_mouseTracking->setCheckable(true);
+	_mouseTracking->setChecked(false);
 	_saveImage = _menu->addAction(tr("Save picture..."));
 	_saveImage->setEnabled(false);
 
+	setMouseTracking(true);
+
 	connect(_graphicsView->scene(), SIGNAL(sceneRectChanged(const QRectF &)), this, SLOT(sceneRectChanged(const QRectF &)));
 }
 
@@ -1047,6 +1054,65 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	}
 }
 
+void ImageView::mouseMoveEvent(QMouseEvent * event)
+{
+	if(_mouseTracking->isChecked() &&
+		!_graphicsView->scene()->sceneRect().isNull() &&
+		!_image.isNull() &&
+		!_imageDepthCv.empty() &&
+		(_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1))
+	{
+		float scale, offsetX, offsetY;
+		computeScaleOffsets(this->rect(), scale, offsetX, offsetY);
+		float u = (event->pos().x() - offsetX) / scale;
+		float v = (event->pos().y() - offsetY) / scale;
+		float depthScale = 1;
+		if(_image.width() > _imageDepth.width())
+		{
+			depthScale = _imageDepth.width() / _image.width();
+		}
+		int ud = int(u*depthScale);
+		int vd = int(v*depthScale);
+		if( ud>=0 && vd>=0 &&
+			ud < _imageDepthCv.cols &&
+			vd < _imageDepthCv.rows)
+		{
+			float depth = 0;
+			if(_imageDepthCv.type() == CV_32FC1)
+			{
+				depth = _imageDepthCv.at<float>(vd, ud);
+			}
+			else
+			{
+				depth = float(_imageDepthCv.at<unsigned short>(vd, ud)) / 1000.0f;
+			}
+
+			cv::Point3f pt(0,0,0);
+			if(depth>0 && !_models.empty() && !_pose.isNull())
+			{
+				int subImageWidth = _imageDepthCv.cols / _models.size();
+				int subImageIndex = ud / subImageWidth;
+				UASSERT(subImageIndex < (int)_models.size());
+				float x,y,z;
+				_models[subImageIndex].project(u, v, depth, x, y, z);
+				pt = cv::Point3f(x,y,z);
+				pt = util3d::transformPoint(pt, _pose*_models[subImageIndex].localTransform());
+			}
+
+#if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
+			QToolTip::showText(event->globalPosition().toPoint(), QString("%1 %2").arg(scenePoint.x()).arg(scenePoint.y()));
+#else
+			QToolTip::showText(event->globalPos(), pt.x!=0?tr("Depth=%1m Map=(%2,%3,%4)").arg(depth).arg(pt.x).arg(pt.y).arg(pt.z):depth > 0?tr("Depth=%1m").arg(depth):tr("Depth=NA"));
+#endif
+		}
+		else
+		{
+			QToolTip::hideText();
+		}
+	}
+	QWidget::mouseMoveEvent(event);
+}
+
 void ImageView::updateOpacity()
 {
 	if(_imageItem && _imageDepthItem)
@@ -1166,9 +1232,11 @@ void ImageView::addLine(float x1, float y1, float x2, float y2, QColor color, co
 	}
 }
 
-void ImageView::setImage(const QImage & image)
+void ImageView::setImage(const QImage & image, const std::vector<CameraModel> & models, const Transform & pose)
 {
 	_image = QPixmap::fromImage(image);
+	_models = models;
+	_pose = pose;
 	if(_graphicsView->isVisible())
 	{
 		if(_imageItem)
@@ -1394,6 +1462,8 @@ void ImageView::clear()
 		_imageItem = 0;
 	}
 	_image = QPixmap();
+	_models.clear();
+	_pose.setNull();
 
 	if(_imageDepthItem)
 	{