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

GraphViewer: added loop closure outlier color option

Browse Source
This commit is contained in:
matlabbe
2015-09-03 14:33:28 -04:00
parent 198899ba32
commit 1c327e7b67
2 changed files with 116 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
guilib/src/GraphViewer.cpp
View File

@@ -170,6 +170,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_loopClosureLocalColor(Qt::yellow),
_loopClosureUserColor(Qt::red),
_loopClosureVirtualColor(Qt::magenta),
_loopClosureRejectedColor(Qt::black),
_localPathColor(Qt::cyan),
_globalPathColor(Qt::darkMagenta),
_loopIntraSessionColor(Qt::red),
@@ -181,7 +182,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
_gridMap(0),
_referential(0),
_gridCellSize(0.0f),
_localRadius(0)
_localRadius(0),
_loopClosureOutlierThr(0)
{
this->setScene(new QGraphicsScene(this));
this->setDragMode(QGraphicsView::ScrollHandDrag);
@@ -282,13 +284,13 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
LinkItem * linkItem = 0;
if(jterA != poses.end() && jterB != poses.end() &&
_nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
{
const Transform & poseA = jterA->second;
const Transform & poseB = jterB->second;
bool added = false;
if(_linkItems.contains(idFrom))
{
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(iter->first);
@@ -298,86 +300,89 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
itemIter.value()->setPoses(poseA, poseB);
itemIter.value()->show();
added = true;
// reset color
if(iter->second.type() == Link::kNeighbor)
{
itemIter.value()->setColor(_neighborColor);
}
else if(iter->second.type() == Link::kVirtualClosure)
{
itemIter.value()->setColor(_loopClosureVirtualColor);
}
else if(iter->second.type() == Link::kUserClosure)
{
itemIter.value()->setColor(_loopClosureUserColor);
}
else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
{
itemIter.value()->setColor(_loopClosureLocalColor);
}
else
{
itemIter.value()->setColor(_loopClosureColor);
}
linkItem = itemIter.value();
break;
}
++itemIter;
}
}
if(!added)
bool interSessionClosure = false;
if(uContains(mapIds, jterA->first) && uContains(mapIds, jterB->first))
{
interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first);
}
if(linkItem == 0)
{
//create a link item
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();
linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure);
QPen p = linkItem->pen();
p.setWidthF(_linkWidth);
item->setPen(p);
item->setZValue(10);
if(iter->second.type() == Link::kNeighbor)
linkItem->setPen(p);
linkItem->setZValue(10);
this->scene()->addItem(linkItem);
linkItem->setParentItem(_root);
_linkItems.insert(idFrom, linkItem);
}
//update color
if(iter->second.type() == Link::kNeighbor)
{
linkItem->setColor(_neighborColor);
}
else if(iter->second.type() == Link::kVirtualClosure)
{
linkItem->setColor(_loopClosureVirtualColor);
}
else if(iter->second.type() == Link::kUserClosure)
{
linkItem->setColor(_loopClosureUserColor);
}
else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
{
if(_intraInterSessionColors)
{
item->setColor(_neighborColor);
}
else if(iter->second.type() == Link::kVirtualClosure)
{
item->setColor(_loopClosureVirtualColor);
}
else if(iter->second.type() == Link::kUserClosure)
{
item->setColor(_loopClosureUserColor);
}
else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
{
if(_intraInterSessionColors)
{
item->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
item->setZValue(interSessionClosure?8:9);
}
else
{
item->setColor(_loopClosureLocalColor);
}
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
linkItem->setZValue(interSessionClosure?8:9);
}
else
{
if(_intraInterSessionColors)
linkItem->setColor(_loopClosureLocalColor);
}
}
else
{
if(_intraInterSessionColors)
{
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
linkItem->setZValue(interSessionClosure?8:9);
}
else
{
linkItem->setColor(_loopClosureColor);
}
}
//rejected loop closures
if(_loopClosureOutlierThr > 0.0f)
{
Transform t = poseA.inverse()*poseB;
if(iter->second.to() != idTo)
{
t = t.inverse();
}
if(iter->second.type() != Link::kNeighbor)
{
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
if(linearError > _loopClosureOutlierThr)
{
item->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
item->setZValue(interSessionClosure?8:9);
}
else
{
item->setColor(_loopClosureColor);
linkItem->setColor(_loopClosureRejectedColor);
}
}
this->scene()->addItem(item);
item->setParentItem(_root);
_linkItems.insert(idFrom, item);
}
}
}
@@ -643,6 +648,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("local_color", this->getLocalLoopClosureColor());
settings.setValue("user_color", this->getUserLoopClosureColor());
settings.setValue("virtual_color", this->getVirtualLoopClosureColor());
settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
settings.setValue("local_path_color", this->getLocalPathColor());
settings.setValue("global_path_color", this->getGlobalPathColor());
settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
@@ -652,6 +658,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("origin_visible", this->isOriginVisible());
settings.setValue("referential_visible", this->isReferentialVisible());
settings.setValue("local_radius_visible", this->isLocalRadiusVisible());
settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr());
if(!group.isEmpty())
{
settings.endGroup();
@@ -673,6 +680,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value<QColor>());
this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value<QColor>());
this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value<QColor>());
this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
@@ -682,6 +690,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
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());
this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble());
if(!group.isEmpty())
{
settings.endGroup();
@@ -704,6 +713,10 @@ bool GraphViewer::isLocalRadiusVisible() const
{
return _localRadius->isVisible();
}
float GraphViewer::getLoopClosureOutlierThr() const
{
return _loopClosureOutlierThr;
}
void GraphViewer::setWorkingDirectory(const QString & path)
{
@@ -808,6 +821,10 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
}
}
}
void GraphViewer::setRejectedLoopClosureColor(const QColor & color)
{
_loopClosureRejectedColor = color;
}
void GraphViewer::setLocalPathColor(const QColor & color)
{
_localPathColor = color;
@@ -884,6 +901,10 @@ void GraphViewer::setLocalRadiusVisible(bool visible)
{
_localRadius->setVisible(visible);
}
void GraphViewer::setLoopClosureOutlierThr(float value)
{
_loopClosureOutlierThr = value;
}
void GraphViewer::restoreDefaults()
{
@@ -937,6 +958,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
QAction * aChangeRejectedLoopColor = menuLink->addAction(tr("Outlier loop closure"));
QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
menuLink->addSeparator();
@@ -948,6 +971,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
@@ -957,6 +981,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeLocalLoopColor->setIconVisibleInMenu(true);
aChangeUserLoopColor->setIconVisibleInMenu(true);
aChangeVirtualLoopColor->setIconVisibleInMenu(true);
aChangeRejectedLoopColor->setIconVisibleInMenu(true);
aChangeLocalPathColor->setIconVisibleInMenu(true);
aChangeGlobalPathColor->setIconVisibleInMenu(true);
aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
@@ -1088,6 +1113,15 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked());
}
else if(r == aChangeRejectedLoopThr)
{
bool ok;
double value = QInputDialog::getDouble(this, tr("Loop closure outlier threshold"), tr("Value (m)"), _loopClosureOutlierThr, 0.0, 1000.0, 2, &ok);
if(ok)
{
setLoopClosureOutlierThr(value);
}
}
else if(r == aChangeNodeColor ||
r == aChangeCurrentGoalColor ||
r == aChangeNeighborColor ||
@@ -1095,6 +1129,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
r == aChangeLocalLoopColor ||
r == aChangeUserLoopColor ||
r == aChangeVirtualLoopColor ||
r == aChangeRejectedLoopColor ||
r == aChangeLocalPathColor ||
r == aChangeGlobalPathColor ||
r == aChangeIntraSessionLoopColor ||
@@ -1125,6 +1160,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
color = _loopClosureVirtualColor;
}
else if(r == aChangeRejectedLoopColor)
{
color = _loopClosureRejectedColor;
}
else if(r == aChangeLocalPathColor)
{
color = _localPathColor;
@@ -1173,6 +1212,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setVirtualLoopClosureColor(color);
}
else if(r == aChangeRejectedLoopColor)
{
this->setRejectedLoopClosureColor(color);
}
else if(r == aChangeLocalPathColor)
{
this->setLocalPathColor(color);