diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 1ea46d2f..ff40ced8 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -149,6 +149,7 @@ private: private: Ui_DatabaseViewer * ui_; QList ids_; + std::map mapIds_; QMap idToIndex_; QList neighborLinks_; QList loopLinks_; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index ce89ece1..8e93d733 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -476,6 +476,7 @@ bool DatabaseViewer::openDatabase(const QString & path) loopLinks_.clear(); graphes_.clear(); poses_.clear(); + mapIds_.clear(); links_.clear(); linksAdded_.clear(); linksRefined_.clear(); @@ -814,9 +815,19 @@ void DatabaseViewer::updateIds() std::set ids = memory_->getAllSignatureIds(); ids_ = QList::fromStdList(std::list(ids.begin(), ids.end())); idToIndex_.clear(); + mapIds_.clear(); for(int i=0; i d; + int mapId; + memory_->getNodeInfo(ids_[i], p, mapId, w, l, s, d, true); + mapIds_.insert(std::make_pair(ids_[i], mapId)); } poses_.clear(); @@ -2352,7 +2363,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } std::map & graph = uValueAt(graphes_, value); std::multimap links = updateLinksWithModifications(links_); - ui_->graphViewer->updateGraph(graph, links); + ui_->graphViewer->updateGraph(graph, links, mapIds_); if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible()) { float xMin, yMin; diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index eca96cef..64494aa8 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -55,9 +55,10 @@ class NodeItem: public QGraphicsEllipseItem { public: // in meter - NodeItem(int id, const Transform & pose, float radius) : + NodeItem(int id, int mapId, const Transform & pose, float radius) : QGraphicsEllipseItem(QRectF(-radius,-radius,radius*2.0f,radius*2.0f)), _id(id), + _mapId(mapId), _pose(pose) { this->setPos(-pose.y(),-pose.x()); @@ -81,7 +82,7 @@ public: protected: virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event ) { - this->setToolTip(QString("[%1] %2").arg(_id).arg(_pose.prettyPrint().c_str())); + this->setToolTip(QString("%1 [%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str())); this->setScale(2); QGraphicsEllipseItem::hoverEnterEvent(event); } @@ -94,6 +95,7 @@ protected: private: int _id; + int _mapId; Transform _pose; }; @@ -101,13 +103,14 @@ class LinkItem: public QGraphicsLineItem { public: // in meter - LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, Link::Type type) : + LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, Link::Type type, bool interSessionClosure) : QGraphicsLineItem(-poseA.y(), -poseA.x(), -poseB.y(), -poseB.x()), _from(from), _to(to), _poseA(poseA), _poseB(poseB), - _type(type) + _type(type), + _interSession(interSessionClosure) { this->setAcceptHoverEvents(true); } @@ -127,6 +130,7 @@ public: } Link::Type linkType() const {return _type;} + bool isInterSession() const {return _interSession;} int from() const {return _from;} int to() const {return _to;} @@ -154,6 +158,7 @@ private: Transform _poseA; Transform _poseB; Link::Type _type; + bool _interSession; }; GraphViewer::GraphViewer(QWidget * parent) : @@ -167,6 +172,9 @@ GraphViewer::GraphViewer(QWidget * parent) : _loopClosureVirtualColor(Qt::magenta), _localPathColor(Qt::cyan), _globalPathColor(Qt::darkMagenta), + _loopIntraSessionColor(Qt::red), + _loopInterSessionColor(Qt::green), + _intraInterSessionColors(false), _root(0), _nodeRadius(0.01), _linkWidth(0), @@ -226,7 +234,8 @@ GraphViewer::~GraphViewer() } void GraphViewer::updateGraph(const std::map & poses, - const std::multimap & constraints) + const std::multimap & constraints, + const std::map & mapIds) { bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0; UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size()); @@ -255,7 +264,7 @@ void GraphViewer::updateGraph(const std::map & poses, { // create node item const Transform & pose = iter->second; - NodeItem * item = new NodeItem(iter->first, pose, _nodeRadius); + NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius); this->scene()->addItem(item); item->setZValue(20); item->setColor(_nodeColor); @@ -319,10 +328,16 @@ void GraphViewer::updateGraph(const std::map & poses, if(!added) { //create a link item - LinkItem * item = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type()); + bool interSessionClosure = false; + if(uContains(mapIds, jterA->first) && uContains(mapIds, jterB->first)) + { + interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first); + } + LinkItem * item = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure); QPen p = item->pen(); p.setWidthF(_linkWidth); item->setPen(p); + item->setZValue(10); if(iter->second.type() == Link::kNeighbor) { item->setColor(_neighborColor); @@ -337,14 +352,30 @@ void GraphViewer::updateGraph(const std::map & poses, } else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure) { - item->setColor(_loopClosureLocalColor); + if(_intraInterSessionColors) + { + item->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); + item->setZValue(interSessionClosure?8:9); + } + else + { + item->setColor(_loopClosureLocalColor); + } } else { - item->setColor(_loopClosureColor); + if(_intraInterSessionColors) + { + item->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); + item->setZValue(interSessionClosure?8:9); + } + else + { + item->setColor(_loopClosureColor); + } } + this->scene()->addItem(item); - item->setZValue(10); item->setParentItem(_root); _linkItems.insert(idFrom, item); } @@ -465,7 +496,7 @@ void GraphViewer::setGlobalPath(const std::vector > & //create a link item int idFrom = globalPath[i].first; int idTo = globalPath[i+1].first; - LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link::kUndef); + LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link::kUndef, false); QPen p = item->pen(); p.setWidthF(_linkWidth); item->setPen(p); @@ -530,7 +561,7 @@ void GraphViewer::updateLocalPath(const std::vector & localPath) if(!updated) { //create a link item - LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link::kUndef); + LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link::kUndef, false); QPen p = item->pen(); p.setWidthF(_linkWidth); item->setPen(p); @@ -613,6 +644,9 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("virtual_color", this->getVirtualLoopClosureColor()); settings.setValue("local_path_color", this->getLocalPathColor()); settings.setValue("global_path_color", this->getGlobalPathColor()); + settings.setValue("intra_session_color", this->getIntraSessionLoopColor()); + settings.setValue("inter_session_color", this->getInterSessionLoopColor()); + settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()); settings.setValue("grid_visible", this->isGridMapVisible()); settings.setValue("origin_visible", this->isOriginVisible()); settings.setValue("referential_visible", this->isReferentialVisible()); @@ -640,10 +674,13 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value()); this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value()); this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value()); + this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value()); + this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value()); this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool()); this->setOriginVisible(settings.value("origin_visible", this->isOriginVisible()).toBool()); this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool()); this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool()); + this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool()); if(!group.isEmpty()) { settings.endGroup(); @@ -720,27 +757,31 @@ void GraphViewer::setNeighborColor(const QColor & color) void GraphViewer::setGlobalLoopClosureColor(const QColor & color) { _loopClosureColor = color; - for(QMultiMap::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter) + if(!_intraInterSessionColors) { - if(iter.value()->linkType() != Link::kNeighbor && - iter.value()->linkType() != Link::kLocalSpaceClosure && - iter.value()->linkType() != Link::kLocalTimeClosure && - iter.value()->linkType() != Link::kUserClosure && - iter.value()->linkType() != Link::kVirtualClosure) + for(QMultiMap::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter) { - iter.value()->setColor(_loopClosureColor); + if(iter.value()->linkType() == Link::kGlobalClosure) + { + iter.value()->setColor(_loopClosureColor); + iter.value()->setZValue(10); + } } } } void GraphViewer::setLocalLoopClosureColor(const QColor & color) { _loopClosureLocalColor = color; - for(QMultiMap::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter) + if(!_intraInterSessionColors) { - if(iter.value()->linkType() == Link::kLocalSpaceClosure || - iter.value()->linkType() == Link::kLocalTimeClosure) + for(QMultiMap::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter) { - iter.value()->setColor(_loopClosureLocalColor); + if(iter.value()->linkType() == Link::kLocalSpaceClosure || + iter.value()->linkType() == Link::kLocalTimeClosure) + { + iter.value()->setColor(_loopClosureLocalColor); + iter.value()->setZValue(10); + } } } } @@ -774,6 +815,58 @@ void GraphViewer::setGlobalPathColor(const QColor & color) { _globalPathColor = color; } +void GraphViewer::setIntraSessionLoopColor(const QColor & color) +{ + _loopIntraSessionColor = color; + if(_intraInterSessionColors) + { + for(QMultiMap::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter) + { + if((iter.value()->linkType() == Link::kGlobalClosure || + iter.value()->linkType() == Link::kLocalSpaceClosure || + iter.value()->linkType() == Link::kLocalTimeClosure) && + !iter.value()->isInterSession()) + { + iter.value()->setColor(_loopIntraSessionColor); + iter.value()->setZValue(9); + } + } + } +} +void GraphViewer::setInterSessionLoopColor(const QColor & color) +{ + _loopInterSessionColor = color; + if(_intraInterSessionColors) + { + for(QMultiMap::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter) + { + if((iter.value()->linkType() == Link::kGlobalClosure || + iter.value()->linkType() == Link::kLocalSpaceClosure || + iter.value()->linkType() == Link::kLocalTimeClosure) && + iter.value()->isInterSession()) + { + iter.value()->setColor(_loopInterSessionColor); + iter.value()->setZValue(8); + } + } + } +} + +void GraphViewer::setIntraInterSessionColorsEnabled(bool enabled) +{ + _intraInterSessionColors = enabled; + if(_intraInterSessionColors) + { + this->setIntraSessionLoopColor(_loopIntraSessionColor); + this->setInterSessionLoopColor(_loopInterSessionColor); + } + else + { + this->setGlobalLoopClosureColor(_loopClosureColor); + this->setLocalLoopClosureColor(_loopClosureLocalColor); + } +} + void GraphViewer::setGridMapVisible(bool visible) { _gridMap->setVisible(visible); @@ -845,6 +938,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure")); QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path")); QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path")); + menuLink->addSeparator(); + QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors")); + QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure")); + QAction * aChangeInterSessionLoopColor = menuLink->addAction(tr("Inter-session loop closure")); aChangeNeighborColor->setIcon(createIcon(_neighborColor)); aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor)); aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor)); @@ -852,6 +949,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor)); aChangeLocalPathColor->setIcon(createIcon(_localPathColor)); aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor)); + aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor)); + aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor)); aChangeNeighborColor->setIconVisibleInMenu(true); aChangeGlobalLoopColor->setIconVisibleInMenu(true); aChangeLocalLoopColor->setIconVisibleInMenu(true); @@ -859,6 +958,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aChangeVirtualLoopColor->setIconVisibleInMenu(true); aChangeLocalPathColor->setIconVisibleInMenu(true); aChangeGlobalPathColor->setIconVisibleInMenu(true); + aChangeIntraSessionLoopColor->setIconVisibleInMenu(true); + aChangeInterSessionLoopColor->setIconVisibleInMenu(true); + aSetIntraInterSessionColors->setCheckable(true); + aSetIntraInterSessionColors->setChecked(_intraInterSessionColors); menu.addSeparator(); QAction * aSetNodeSize = menu.addAction(tr("Set node radius...")); @@ -980,6 +1083,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) } return; // without emitting configChanged } + else if(r == aSetIntraInterSessionColors) + { + setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked()); + } else if(r == aChangeNodeColor || r == aChangeCurrentGoalColor || r == aChangeNeighborColor || @@ -988,7 +1095,9 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) r == aChangeUserLoopColor || r == aChangeVirtualLoopColor || r == aChangeLocalPathColor || - r == aChangeGlobalPathColor) + r == aChangeGlobalPathColor || + r == aChangeIntraSessionLoopColor || + r == aChangeInterSessionLoopColor) { QColor color; if(r == aChangeNodeColor) @@ -1023,6 +1132,14 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { color = _globalPathColor; } + else if(r == aChangeIntraSessionLoopColor) + { + color = _loopIntraSessionColor; + } + else if(r == aChangeInterSessionLoopColor) + { + color = _loopInterSessionColor; + } else //if(r == aChangeNeighborColor) { color = _neighborColor; @@ -1059,6 +1176,14 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { this->setLocalPathColor(color); } + else if(r == aChangeIntraSessionLoopColor) + { + this->setIntraSessionLoopColor(color); + } + else if(r == aChangeInterSessionLoopColor) + { + this->setInterSessionLoopColor(color); + } else //if(r == aChangeNeighborColor) { this->setNeighborColor(color); diff --git a/guilib/src/GraphViewer.h b/guilib/src/GraphViewer.h index c724b54a..5639dd53 100644 --- a/guilib/src/GraphViewer.h +++ b/guilib/src/GraphViewer.h @@ -53,7 +53,8 @@ public: virtual ~GraphViewer(); void updateGraph(const std::map & poses, - const std::multimap & constraints); + const std::multimap & constraints, + const std::map & mapIds); void updateReferentialPosition(const Transform & t); void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin); void updatePosterior(const std::map & posterior); @@ -82,6 +83,9 @@ public: const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;} const QColor & getLocalPathColor() const {return _localPathColor;} const QColor & getGlobalPathColor() const {return _globalPathColor;} + const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;} + const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;} + bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;} bool isGridMapVisible() const; bool isOriginVisible() const; bool isReferentialVisible() const; @@ -100,6 +104,9 @@ public: void setVirtualLoopClosureColor(const QColor & color); void setLocalPathColor(const QColor & color); void setGlobalPathColor(const QColor & color); + void setIntraSessionLoopColor(const QColor & color); + void setInterSessionLoopColor(const QColor & color); + void setIntraInterSessionColorsEnabled(bool enabled); void setGridMapVisible(bool visible); void setOriginVisible(bool visible); void setReferentialVisible(bool visible); @@ -126,6 +133,9 @@ private: QColor _loopClosureVirtualColor; QColor _localPathColor; QColor _globalPathColor; + QColor _loopIntraSessionColor; + QColor _loopInterSessionColor; + bool _intraInterSessionColors; QGraphicsItem * _root; QMap _nodeItems; QMultiMap _linkItems; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 5e95ebf4..c4178140 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1407,7 +1407,10 @@ void MainWindow::updateMapCloud( { _initProgressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size())); _initProgressDialog->incrementStep(); - QApplication::processEvents(); + if(poses.size() < 200 || i % 100 == 0) + { + QApplication::processEvents(); + } } } @@ -1468,7 +1471,7 @@ void MainWindow::updateMapCloud( // Update occupancy grid map in 3D map view and graph view if(_ui->graphicsView_graphView->isVisible()) { - _ui->graphicsView_graphView->updateGraph(posesIn, constraints); + _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn); if(!currentPose.isNull()) { _ui->graphicsView_graphView->updateReferentialPosition(currentPose);