i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

DbViewer: In graph view, nodes in latest working memory state are highlighted in red

Browse Source
This commit is contained in:
matlabbe
2018-12-13 13:24:57 -05:00
parent ccbc802fe6
commit 73004c643c
8 changed files with 99 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -193,6 +193,7 @@ private:
CloudViewer * stereoViewer_;
CloudViewer * occupancyGridViewer_;
QList<int> ids_;
std::set<int> lastWmIds_;
std::map<int, int> mapIds_;
std::map<int, int> weights_;
std::map<int, std::vector<int> > wmStates_;

5
guilib/include/rtabmap/gui/GraphViewer.h
View File

@@ -61,14 +61,15 @@ public:
void updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds);
const std::map<int, int> & mapIds,
const std::map<int, int> & weights = std::map<int, int>());
void updateGTGraph(const std::map<int, Transform> & poses);
void updateGPSGraph(
const std::map<int, Transform> & gpsMapPoses,
const std::map<int, GPS> & gpsValues);
void updateReferentialPosition(const Transform & t);
void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f);
void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
void updateLocalPath(const std::vector<int> & localPath);
void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
void setCurrentGoalID(int id, const Transform & pose = Transform());

25
guilib/src/DatabaseViewer.cpp
View File

@@ -945,6 +945,7 @@ bool DatabaseViewer::closeDatabase()
groundTruthPoses_.clear();
gpsPoses_.clear();
gpsValues_.clear();
lastWmIds_.clear();
mapIds_.clear();
weights_.clear();
wmStates_.clear();
@@ -1493,6 +1494,8 @@ void DatabaseViewer::updateIds()
std::set<int> ids;
dbDriver_->getAllNodeIds(ids);
ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
lastWmIds_.clear();
dbDriver_->getLastNodeIds(lastWmIds_);
idToIndex_.clear();
mapIds_.clear();
weights_.clear();
@@ -5342,7 +5345,27 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->graphViewer->updateGTGraph(groundTruthPoses_);
ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
if(!ui_->checkBox_wmState->isChecked())
{
bool allNodesAreInWM = true;
std::map<int, float> colors;
for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
{
if(lastWmIds_.find(iter->first) != lastWmIds_.end())
{
colors.insert(std::make_pair(iter->first, 1));
}
else
{
allNodesAreInWM = false;
}
}
if(!allNodesAreInWM)
{
ui_->graphViewer->updatePosterior(colors, 1, 1);
}
}
ui_->graphViewer->clearMap();
occupancyGridViewer_->clear();
if(graph.size() && localMaps.size() &&

24
guilib/src/GraphViewer.cpp
View File

@@ -60,10 +60,11 @@ class NodeItem: public QGraphicsEllipseItem
{
public:
// in meter
NodeItem(int id, int mapId, const Transform & pose, float radius) :
NodeItem(int id, int mapId, const Transform & pose, float radius, int weight=-1) :
QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
_id(id),
_mapId(mapId),
_weight(weight),
_pose(pose),
_line(0)
{
@@ -106,7 +107,14 @@ public:
protected:
virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
{
this->setToolTip(QString("%1 [%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
if(_weight>=0)
{
this->setToolTip(QString("%1 [map=%2, w=%3] %4").arg(_id).arg(_mapId).arg(_weight).arg(_pose.prettyPrint().c_str()));
}
else
{
this->setToolTip(QString("%1 [map=%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
}
this->setScale(2);
QGraphicsEllipseItem::hoverEnterEvent(event);
}
@@ -120,6 +128,7 @@ protected:
private:
int _id;
int _mapId;
int _weight;
Transform _pose;
QGraphicsLineItem * _line;
};
@@ -343,7 +352,8 @@ void GraphViewer::setWorldMapRotation(const float & theta)
void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds)
const std::map<int, int> & mapIds,
const std::map<int, int> & weights)
{
UTimer timer;
bool wasVisible = _graphRoot->isVisible();
@@ -356,6 +366,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
iter.value()->hide();
iter.value()->setColor(iter.key()<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor); // reset color
iter.value()->setZValue(iter.key()<0?21:20);
}
for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
{
@@ -376,7 +387,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
// create node item
const Transform & pose = iter->second;
NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius);
NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1);
this->scene()->addItem(item);
item->setZValue(iter->first<0?21:20);
item->setColor(iter->first<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor);
@@ -856,7 +867,7 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
}
}
void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max)
void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max, int zValueOffset)
{
//find max
if(max <= 0.0f)
@@ -878,8 +889,9 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float
{
float v = jter->second>max?max:jter->second;
iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
iter.value()->setZValue(iter.value()->zValue()+zValueOffset);
}
else
else if(iter.key() > 0)
{
iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1)); // blue
}