Skip to content

Commit

Permalink
Refactored: merged A-B functions into single one, fixed highlighted n…
Browse files Browse the repository at this point in the history 
…ode color not reset after selecting another node, highlighting also when changing constraint sliders, change node color instead of border, added menu options to change color (added to settings).
  • Loading branch information
@matlabbe
matlabbe committed Sep 28, 2024
1 parent 331b346 commit 395ceed
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 88 deletions.
9 changes: 3 additions & 6 deletions guilib/include/rtabmap/gui/GraphViewer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView {
void setGlobalPath(const std::vector<std::pair<int, Transform> > & 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();
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -185,8 +185,6 @@ public Q_SLOTS:
private:
QString _workingDirectory;
QColor _nodeColor;
QColor _nodeColorA;
QColor _nodeColorB;
QColor _nodeOdomCacheColor;
QColor _currentGoalColor;
QColor _neighborColor;
Expand All @@ -212,8 +210,6 @@ public Q_SLOTS:
QGraphicsItem * _localPathRoot;
QGraphicsItem * _gtGraphRoot;
QGraphicsItem * _gpsGraphRoot;
NodeItem* _currentNodeA;
NodeItem* _currentNodeB;
QMap<int, NodeItem*> _nodeItems;
QMultiMap<int, LinkItem*> _linkItems;
QMap<int, NodeItem*> _gtNodeItems;
Expand All @@ -222,6 +218,7 @@ public Q_SLOTS:
QMultiMap<int, LinkItem*> _gpsLinkItems;
QMultiMap<int, LinkItem*> _localPathLinkItems;
QMultiMap<int, LinkItem*> _globalPathLinkItems;
QVector<QPair<QColor, NodeItem*> > _highlightedNodes;
bool _nodeVisible;
float _nodeRadius;
float _linkWidth;
Expand Down
9 changes: 5 additions & 4 deletions guilib/src/DatabaseViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down
177 changes: 99 additions & 78 deletions guilib/src/GraphViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<QColor, NodeItem*>(Qt::magenta, nullptr));
_highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::yellow, nullptr));

this->restoreDefaults();

this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
Expand Down Expand Up @@ -707,6 +705,14 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & 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);
}
Expand Down Expand Up @@ -1213,6 +1219,33 @@ void GraphViewer::updateLocalPath(const std::vector<int> & 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<int, NodeItem*>::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);
Expand All @@ -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();
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -1317,6 +1359,10 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setNodeColor(settings.value("node_color", this->getNodeColor()).value<QColor>());
this->setNodeOdomCacheColor(settings.value("node_odom_cache_color", this->getNodeOdomCacheColor()).value<QColor>());
this->setCurrentGoalColor(settings.value("current_goal_color", this->getCurrentGoalColor()).value<QColor>());
for(int i=0; i<_highlightedNodes.size(); ++i)
{
this->setHighlightColor(settings.value(QString("highlighting_color_%1").arg(i), _highlightedNodes[i].first).value<QColor>(), i);
}
this->setNeighborColor(settings.value("neighbor_color", this->getNeighborColor()).value<QColor>());
this->setGlobalLoopClosureColor(settings.value("global_color", this->getGlobalLoopClosureColor()).value<QColor>());
this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value<QColor>());
Expand Down Expand Up @@ -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;
Expand All @@ -1507,47 +1521,6 @@ void GraphViewer::setNodeOdomCacheColor(const QColor & color)
}
}
}
void GraphViewer::setNodeA(int value){

QPen border = QPen(_nodeColorA);
QMap<int, NodeItem*>::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<int, NodeItem*>::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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<QAction*> 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);
Expand Down Expand Up @@ -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; i<aChangeHighlightingColors.size(); ++i)
{
if(r == aChangeHighlightingColors[i]) {
aChangeHighlightingColorIndex = i;
}
}
if(r == aScreenShot)
{
if(_root)
Expand Down Expand Up @@ -2295,6 +2307,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
else if(r == aChangeNodeColor ||
r == aChangeNodeOdomCacheColor ||
r == aChangeCurrentGoalColor ||
r == aChangeHighlightingColors[aChangeHighlightingColorIndex] ||
r == aChangeNeighborColor ||
r == aChangeGlobalLoopColor ||
r == aChangeLocalLoopColor ||
Expand Down Expand Up @@ -2322,6 +2335,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
color = _currentGoalColor;
}
else if(r == aChangeHighlightingColors[aChangeHighlightingColorIndex])
{
color = _highlightedNodes[aChangeHighlightingColorIndex].first;
}
else if(r == aChangeGlobalLoopColor)
{
color = _loopClosureColor;
Expand Down Expand Up @@ -2394,6 +2411,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setCurrentGoalColor(color);
}
else if(r == aChangeHighlightingColors[aChangeHighlightingColorIndex])
{
this->setHighlightColor(color, aChangeHighlightingColorIndex);
}
else if(r == aChangeGlobalLoopColor)
{
this->setGlobalLoopClosureColor(color);
Expand Down

0 comments on commit 395ceed

Please sign in to comment.