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.

matlabbe · matlabbe · commit a6d9425bce9c · 2024-03-13T22:42:47.000-07:00
diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/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);
diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/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:
diff --git a/guilib/include/rtabmap/gui/ImageView.h b/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;
 };
 
 }
diff --git a/guilib/src/DatabaseViewer.cpp b/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
diff --git a/guilib/src/GraphViewer.cpp b/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);
diff --git a/guilib/src/ImageView.cpp b/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)
 	{
