diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index e68145a265..2209675716 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -79,6 +79,7 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView { void setGlobalPath(const std::vector > & globalPath); void setCurrentGoalID(int id, const Transform & pose = Transform()); void setLocalRadius(float radius); + void highlightNode(int nodeId, int highlightIndex); void clearGraph(); void clearMap(); void clearPosterior(); @@ -132,8 +133,6 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView { void setNodeA(const int value); void setNodeB(const int value); void setNodeColor(const QColor & color); - void setNodeColorA(const QColor & color); - void setNodeColorB(const QColor & color); void setNodeOdomCacheColor(const QColor & color); void setCurrentGoalColor(const QColor & color); void setNeighborColor(const QColor & color); @@ -148,6 +147,7 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView { void setGlobalPathColor(const QColor & color); void setGTColor(const QColor & color); void setGPSColor(const QColor & color); + void setHighlightColor(const QColor & color, int index); void setIntraSessionLoopColor(const QColor & color); void setInterSessionLoopColor(const QColor & color); void setIntraInterSessionColorsEnabled(bool enabled); @@ -185,8 +185,6 @@ public Q_SLOTS: private: QString _workingDirectory; QColor _nodeColor; - QColor _nodeColorA; - QColor _nodeColorB; QColor _nodeOdomCacheColor; QColor _currentGoalColor; QColor _neighborColor; @@ -212,8 +210,6 @@ public Q_SLOTS: QGraphicsItem * _localPathRoot; QGraphicsItem * _gtGraphRoot; QGraphicsItem * _gpsGraphRoot; - NodeItem* _currentNodeA; - NodeItem* _currentNodeB; QMap _nodeItems; QMultiMap _linkItems; QMap _gtNodeItems; @@ -222,6 +218,7 @@ public Q_SLOTS: QMultiMap _gpsLinkItems; QMultiMap _localPathLinkItems; QMultiMap _globalPathLinkItems; + QVector > _highlightedNodes; bool _nodeVisible; float _nodeRadius; float _linkWidth; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index c426a6f0e8..c72342ebf6 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -587,6 +587,7 @@ void DatabaseViewer::readSettings() // GraphViewer settings ui_->graphViewer->loadSettings(settings, "GraphView"); + ui_->graphViewer->setReferentialVisible(false); settings.beginGroup("optimization"); ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble()); @@ -4613,8 +4614,6 @@ void DatabaseViewer::graphLinkSelected(int from, int to) void DatabaseViewer::sliderAValueChanged(int value) { - ui_->graphViewer->setNodeA(ids_.at(value)); - this->update(value, ui_->spinBox_indexA, ui_->label_parentsA, @@ -4642,8 +4641,6 @@ void DatabaseViewer::sliderAValueChanged(int value) void DatabaseViewer::sliderBValueChanged(int value) { - ui_->graphViewer->setNodeB(ids_.at(value)); - this->update(value, ui_->spinBox_indexB, ui_->label_parentsB, @@ -4725,6 +4722,10 @@ void DatabaseViewer::update(int value, labelId->setText(QString::number(id)); if(id>0) { + if(ui_->dockWidget_graphView->isVisible()) { + ui_->graphViewer->highlightNode(id, spinBoxIndex==ui_->spinBox_indexB?1:0); + } + //image QImage img; cv::Mat imgDepth; diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 251cf31fca..6e1d9bec40 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -284,8 +284,6 @@ class LinkItem: public QGraphicsLineItem GraphViewer::GraphViewer(QWidget * parent) : QGraphicsView(parent), _nodeColor(Qt::blue), - _nodeColorA(Qt::yellow), - _nodeColorB(Qt::magenta), _nodeOdomCacheColor(Qt::darkGreen), _currentGoalColor(Qt::darkMagenta), _neighborColor(Qt::blue), @@ -323,9 +321,6 @@ GraphViewer::GraphViewer(QWidget * parent) : _viewPlane(XY), _ensureFrameVisible(true) { - _currentNodeA = nullptr; - _currentNodeB = nullptr; - this->setScene(new QGraphicsScene(this)); this->setDragMode(QGraphicsView::ScrollHandDrag); _workingDirectory = QDir::homePath(); @@ -468,6 +463,9 @@ GraphViewer::GraphViewer(QWidget * parent) : _odomCacheOverlay->setBrush(QBrush(QColor(255, 255, 255, 150))); _odomCacheOverlay->setPen(QPen(Qt::NoPen)); + _highlightedNodes.push_back(QPair(Qt::magenta, nullptr)); + _highlightedNodes.push_back(QPair(Qt::yellow, nullptr)); + this->restoreDefaults(); this->fitInView(this->sceneRect(), Qt::KeepAspectRatio); @@ -707,6 +705,14 @@ void GraphViewer::updateGraph(const std::map & poses, { if(!iter.value()->isVisible()) { + for(int i=0; i<_highlightedNodes.size(); ++i) + { + if(_highlightedNodes[i].second && _highlightedNodes[i].second == iter.value()) + { + _highlightedNodes[i].second = nullptr; + } + } + delete iter.value(); iter = _nodeItems.erase(iter); } @@ -1213,6 +1219,33 @@ void GraphViewer::updateLocalPath(const std::vector & localPath) _localPathRoot->setVisible(wasVisible); } + +void GraphViewer::highlightNode(int nodeId, int highlightIndex) +{ + if(highlightIndex<0 || highlightIndex>_highlightedNodes.size()) + { + UERROR("Unsupported highlight color index %d", highlightIndex); + return; + } + for(int i=0; i<_highlightedNodes.size(); ++i) + { + if(_highlightedNodes[i].second && + (_highlightedNodes[i].first == nodeId || i == highlightIndex)) + { + // reset to normal color + _highlightedNodes[i].second->setColor(_nodeColor); + _highlightedNodes[i].second = nullptr; + } + } + + QMap::iterator iter = _nodeItems.find(nodeId); + if(iter != _nodeItems.end()) + { + iter.value()->setColor(_highlightedNodes[highlightIndex].first); + _highlightedNodes[highlightIndex].second = iter.value(); + } +} + void GraphViewer::clearGraph() { qDeleteAll(_nodeItems); @@ -1232,6 +1265,11 @@ void GraphViewer::clearGraph() qDeleteAll(_gpsLinkItems); _gpsLinkItems.clear(); + for(int i=0; i<_highlightedNodes.size(); ++i) + { + _highlightedNodes[i].second=nullptr; + } + _root->resetTransform(); _worldMapRotation = 0.0f; _referential->resetTransform(); @@ -1271,6 +1309,10 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("node_color", this->getNodeColor()); settings.setValue("node_odom_cache_color", this->getNodeOdomCacheColor()); settings.setValue("current_goal_color", this->getCurrentGoalColor()); + for(int i=0; i<_highlightedNodes.size(); ++i) + { + settings.setValue(QString("highlighting_color_%1").arg(i), _highlightedNodes[i].first); + } settings.setValue("neighbor_color", this->getNeighborColor()); settings.setValue("global_color", this->getGlobalLoopClosureColor()); settings.setValue("local_color", this->getLocalLoopClosureColor()); @@ -1317,6 +1359,10 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setNodeColor(settings.value("node_color", this->getNodeColor()).value()); this->setNodeOdomCacheColor(settings.value("node_odom_cache_color", this->getNodeOdomCacheColor()).value()); this->setCurrentGoalColor(settings.value("current_goal_color", this->getCurrentGoalColor()).value()); + for(int i=0; i<_highlightedNodes.size(); ++i) + { + this->setHighlightColor(settings.value(QString("highlighting_color_%1").arg(i), _highlightedNodes[i].first).value(), i); + } this->setNeighborColor(settings.value("neighbor_color", this->getNeighborColor()).value()); this->setGlobalLoopClosureColor(settings.value("global_color", this->getGlobalLoopClosureColor()).value()); this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value()); @@ -1464,38 +1510,6 @@ void GraphViewer::setNodeColor(const QColor & color) iter.value()->setColor(_nodeColor); } } -void GraphViewer::setNodeColorA(const QColor & color) -{ - _nodeColorA = color; - QPen border = QPen(_nodeColorA); - if(_currentNodeA != nullptr) - { - if(_currentNodeA->id() != 0) - { - if(_nodeItems.contains(_currentNodeA->id())) - { - border.setWidth(_linkWidth*100.0f); - _currentNodeA->setPen(border); - } - } - } -} -void GraphViewer::setNodeColorB(const QColor & color) -{ - _nodeColorB = color; - QPen border = QPen(_nodeColorB); - if(_currentNodeB != nullptr) - { - if(_currentNodeB->id() != 0) - { - if(_nodeItems.contains(_currentNodeB->id())) - { - border.setWidth(_linkWidth*100.0f); - _currentNodeB->setPen(border); - } - } - } -} void GraphViewer::setNodeOdomCacheColor(const QColor & color) { _nodeOdomCacheColor = color; @@ -1507,47 +1521,6 @@ void GraphViewer::setNodeOdomCacheColor(const QColor & color) } } } -void GraphViewer::setNodeA(int value){ - - QPen border = QPen(_nodeColorA); - QMap::iterator iter = _nodeItems.find(value); - if(iter != _nodeItems.end()){ - if(_currentNodeA != nullptr){ - if(_currentNodeA->id() != 0) - { - if(_nodeItems.contains(_currentNodeA->id())) - { - border.setWidth(0); - _currentNodeA->setPen(border); - } - } - } - _currentNodeA = iter.value(); - border.setWidth(_linkWidth*100.0f); - _currentNodeA->setPen(QPen(border)); - } -} -void GraphViewer::setNodeB(int value){ - - QPen border = QPen(_nodeColorB); - QMap::iterator iter = _nodeItems.find(value); - - if(iter != _nodeItems.end()){ - if(_currentNodeB != nullptr){ - if(_currentNodeB->id() != 0) - { - if(_nodeItems.contains(_currentNodeB->id())) - { - border.setWidth(0); - _currentNodeB->setPen(border); - } - } - } - _currentNodeB = iter.value(); - border.setWidth(_linkWidth*100.0f); - _currentNodeB->setPen(QPen(border)); - } -} void GraphViewer::setCurrentGoalColor(const QColor & color) { _currentGoalColor = color; @@ -1674,6 +1647,20 @@ void GraphViewer::setGPSColor(const QColor & color) iter.value()->setColor(_gpsPathColor); } } +void GraphViewer::setHighlightColor(const QColor & color, int index) +{ + if(index<0 || index > _highlightedNodes.size()) + { + UERROR("Unsupported highlight color index %d", index); + return; + } + _highlightedNodes[index].first = color; + NodeItem * node = _highlightedNodes[index].second; + if(node != nullptr) + { + node->setColor(color); + } +} void GraphViewer::setIntraSessionLoopColor(const QColor & color) { _loopIntraSessionColor = color; @@ -1843,13 +1830,25 @@ void GraphViewer::restoreDefaults() setNodeRadius(0.01f); setLinkWidth(0.0f); setNodeColor(Qt::blue); + setNodeOdomCacheColor(Qt::darkGreen); + setCurrentGoalColor(Qt::darkMagenta); + setHighlightColor(Qt::magenta, 0); + setHighlightColor(Qt::yellow, 1); setNeighborColor(Qt::blue); setGlobalLoopClosureColor(Qt::red); setLocalLoopClosureColor(Qt::yellow); setUserLoopClosureColor(Qt::red); setVirtualLoopClosureColor(Qt::magenta); setNeighborMergedColor(QColor(255,170,0)); + setRejectedLoopClosureColor(Qt::black); setLandmarkColor(Qt::darkGreen); + setLocalPathColor(Qt::cyan); + setGlobalPathColor(Qt::darkMagenta); + setGTColor(Qt::gray); + setGPSColor(Qt::darkCyan); + setIntraSessionLoopColor(Qt::red); + setInterSessionLoopColor(Qt::green); + setIntraInterSessionColorsEnabled(false); setGridMapVisible(true); setGraphVisible(true); setGlobalPathVisible(true); @@ -1931,6 +1930,12 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color...")); QAction * aChangeNodeOdomCacheColor = menu.addAction(createIcon(_nodeOdomCacheColor), tr("Set node odom cache color...")); QAction * aChangeCurrentGoalColor = menu.addAction(createIcon(_currentGoalColor), tr("Set current goal color...")); + QMenu * menuChangeHighlightingColors = menu.addMenu(tr("Set node highlighting colors...")); + QVector aChangeHighlightingColors(_highlightedNodes.size()); + for(int i=0; i<_highlightedNodes.size(); ++i) { + aChangeHighlightingColors[i] = menuChangeHighlightingColors->addAction(createIcon(_highlightedNodes[i].first), tr("Color %1...").arg(i+1)); + aChangeHighlightingColors[i]->setIconVisibleInMenu(true); + } aChangeNodeColor->setIconVisibleInMenu(true); aChangeNodeOdomCacheColor->setIconVisibleInMenu(true); aChangeCurrentGoalColor->setIconVisibleInMenu(true); @@ -2128,6 +2133,13 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults")); QAction * r = menu.exec(event->globalPos()); + int aChangeHighlightingColorIndex = 0; + for(int i=1; isetCurrentGoalColor(color); } + else if(r == aChangeHighlightingColors[aChangeHighlightingColorIndex]) + { + this->setHighlightColor(color, aChangeHighlightingColorIndex); + } else if(r == aChangeGlobalLoopColor) { this->setGlobalLoopClosureColor(color);