From a6d9425bce9c3e971b9cd9341de9b6490d77c887 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 13 Mar 2024 22:42:47 -0700 Subject: [PATCH] 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 | 2 + guilib/include/rtabmap/gui/GraphViewer.h | 3 + guilib/include/rtabmap/gui/ImageView.h | 7 +- guilib/src/DatabaseViewer.cpp | 23 ++++++- guilib/src/GraphViewer.cpp | 28 +++++++- guilib/src/ImageView.cpp | 72 ++++++++++++++++++++- 6 files changed, 131 insertions(+), 4 deletions(-) diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index f4a2c675b8..ede9f1d187 100644 --- 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 index d42f19ede1..9e8996a582 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -166,6 +166,8 @@ 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(); @@ -173,6 +175,7 @@ public Q_SLOTS: 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 index 3ef4eee63f..2c09bacb63 100644 --- 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 #include #include "rtabmap/utilite/UCv2Qt.h" +#include class QAction; class QMenu; @@ -96,7 +97,7 @@ class RTABMAP_GUI_EXPORT ImageView : public QWidget { void setFeatures(const std::vector & 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 & models = std::vector(), 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 _models; + Transform _pose; }; } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index c4e466f8c8..90f4aec213 100644 --- 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 index 1fca51068c..bbd6618775 100644 --- 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(item); + LinkItem *linkItem = qgraphicsitem_cast(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 index 118954a8c4..e46c467b35 100644 --- 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 #include #include +#include #include "rtabmap/utilite/ULogger.h" #include "rtabmap/gui/KeypointItem.h" #include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d_transforms.h" #include #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(vd, ud); + } + else + { + depth = float(_imageDepthCv.at(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 & 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) {