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)
  {